ROS机器人底盘(2)-运动解算

这里以zeus为例子讲述运动正解与逆解

1. 正解

从整体速度转换为各个轮子速度即为正解,关系着如何根据既定速度控制机器人正确运行

控制结构

我们知道ROS里面驱动小车最终下发的为线速度和角速度,通过rosmsg show可以看到


对于我们的机器底盘只涉及到linear.x linear.yangular.z,分别表示 xy方向线速度和一个转动的角速度z

可以看到ROS坐标系规定


下面的事就是怎么转换这个三个值到轮子的转速。前为x方向,右为y方向,逆时针为角速度的z

解算

对于每个轮子:
前轮,左轮及后轮速度分别设为Vf, Vl,Vr(假设轮子使得底盘逆时针时的线速度为正);轮子所在圆直接为L

  • 前轮只受线速度y和角速度控制影响,可得到 Vf = y+z*L/2
  • 左轮收到x、y和角速度控制影响
    Left Wheel

    Vl = -x*cos(30°)-y*sin(30°)+z*L/2
  • 右轮同样ru左轮可以得到:
    Vr = x*cos(30°)-y*sin(30°)+z*L/2

2.逆解

从各个轮子速度(行径距离)转换为整体机器人的速度(坐标、姿态)

解算

只需从正解

Vf = y + z*L/2
Vl = -x*cos(30°) - y*sin(30°) + z*L/2
Vr = x*cos(30°) - y*sin(30°) + z*L/2 

反推即可得到

x = (-Vl + Vr) / (2 * cos(30°))
y = (2Vf - Vl - Vr) / 3
z = (Vf + Vl + Vr) * 2 / (3L)

3. 用途

通过正解可以转换对机器人的速度控制为对各个轮子的速度控制;通过逆解,通过积分可以根据每个轮子的行径距离求得机器人的坐标和姿态

4.代码

model.h 模型接口类

#ifndef PIBOT_MODLE_H_
#define PIBOT_MODLE_H_


struct Odom{
    float x;
    float y;
    float z;
    float vel_x;
    float vel_y;
    float vel_z;
};

struct Model{
    Model(){}
    Model(float _wheel_radius, float _body_radius): wheel_radius(_wheel_radius), body_radius(_body_radius){}

    void set(float _wheel_radius, float _body_radius){
        wheel_radius = _wheel_radius;
        body_radius = _body_radius;
    }

    ~Model(){}

    //robot speed ------------> motor speed
    virtual void motion_solver(float* robot_speed, float* motor_speed) = 0;

    //motor speed-------------> robot speed
    //interval  ms
    virtual void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval) = 0;

protected:
    float wheel_radius;
    float body_radius;
};

#endif

differential.h 差分轮解算实现

#ifndef PIBOT_DIFFERENTIAL_H_
#define PIBOT_DIFFERENTIAL_H_

#include "model.h"
#include "math.h"

#define MOTOR_COUNT 2

class Differential :    public Model{
public:
    Differential() {}
    Differential(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
    
    void motion_solver(float* robot_speed, float* motor_speed){
        //  robot_speed[0] x    robot_speed[1] y    robot_speed[2] z
        motor_speed[0] = (-robot_speed[0] + robot_speed[2] * body_radius)/ wheel_radius;
        motor_speed[1] = (robot_speed[0] + robot_speed[2] * body_radius) / wheel_radius;
    }
        
    void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval)
    {
        float dxy_ave = (-motor_dis[0] + motor_dis[1]) / 2.0;
        float dth = (motor_dis[0] + motor_dis[1]) / (2* body_radius);
        float vxy = 1000 * dxy_ave / interval;
        float vth = 1000 * dth / interval;

        odom->vel_x = vxy;
        odom->vel_y = 0;
        odom->vel_z = vth;
        float dx = 0, dy = 0;
        if (motor_dis[0] != motor_dis[1])
        {
            dx = cos(dth) * dxy_ave;
            dy = -sin(dth) * dxy_ave;
            odom->x += (cos(odom->z) * dx - sin(odom->z) * dy);
            odom->y += (sin(odom->z) * dx + cos(odom->z) * dy);;
        }

        if (motor_dis[0] + motor_dis[1] != 0)
            odom->z += dth;
    }
};

#endif

omni3.h三轮全向轮解算

#ifndef PIBOT_OMNI3_H_
#define PIBOT_OMNI3_H_

#include "model.h"
#include "math.h"

#define MOTOR_COUNT 3


#define _sqrt_of_3 1.73205f

class Omni3:public Model{
public:
    Omni3() {}
    Omni3(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
    
    void motion_solver(float* robot_speed, float* motor_speed){
        //  robot_speed[0] x    robot_speed[1] y    robot_speed[2] z
        motor_speed[0] = (robot_speed[1] + robot_speed[2] * body_radius)/ wheel_radius;
        motor_speed[1] = (-robot_speed[0]*_sqrt_of_3 - robot_speed[1]*0.5 + robot_speed[2] * body_radius) / wheel_radius;
        motor_speed[2] = (robot_speed[0]*_sqrt_of_3 - robot_speed[1]*0.5 + robot_speed[2] * body_radius) / wheel_radius;
    }
        
    void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval){
        if (motor_dis[0]!=0 || motor_dis[1]!=0 || motor_dis[2]!=0){
            //speed
            double dvx = (-motor_dis[1]+motor_dis[2])*_sqrt_of_3/3.0f;
            double dvy = (motor_dis[0]*2-motor_dis[1]-motor_dis[2])/3.0f;
            double dvth = (motor_dis[0]+motor_dis[1]+motor_dis[2])/ (3 * body_radius);
            odom->vel_x = dvx / dt; 
            odom->vel_y = dvy / dt; 
            odom->vel_z = dvth / dt; 

            //odometry
            double dx = (-sin(th)*motor_dis[0]*2+(-_sqrt_of_3*cos(th)+sin(th))*motor_dis[1]+(_sqrt_of_3*cos(th)+sin(th))*motor_dis[2])/3.0f;
            double dy = (cos(th)*motor_dis[0]*2+(-_sqrt_of_3*sin(th)-cos(th))*motor_dis[1]+(_sqrt_of_3*sin(th)-cos(th))*motor_dis[2])/3.0f;
            double dth = (motor_dis[0]+motor_dis[1]+motor_dis[2])/ (3 * body_radius);
            
            odom->x += dx;
            odom->y += dy;
            odom->z += dth;
        }
    }
};

#endif

4.实现

    static Differential diff(wheel_diameter*0.5, wheel_track*0.5);
    static Omni3 omni3(wheel_diameter*0.5, wheel_track*0.5);
    Model* model = &omni3;

根据配置选择加载对应模型接口解耦模型的正反解算