两轮差速底盘运动解算

• 运动学方程

• 干货

class MotionModel
{
public:
    MotionModel(double wheel_radius, double body_radius);
    virtual ~MotionModel();

    // robot speed -> motor speed
    virtual void MotionSolver(double *robot_speed, double *motor_speed) = 0;
    // motor speed -> robot speed
    virtual void GetOdom(odom_t odom, double * motor_speed, uint32_t interval = 20) = 0;

protected:
    double wheel_radius_; // 车轮半径
    double body_radius_; // 机器人半径
};

class DiffMotionModel : public MotionModel
{
public:
    DiffMotionModel(double wheel_radius, double body_radius);
    virtual ~DiffMotionModel();

    // 通过 MotionModel 继承
    virtual void MotionSolver(double* robot_speed, double* motor_speed) override;
    virtual void GetOdom(odom_t odom, double* motor_speed, uint32_t interval = 20) override;

};

// 运动解算
void DiffMotionModel::MotionSolver(double* robot_speed, double* motor_speed)
{
    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_;
    SYS_DEBUG("robot_speed[%f, %f, %f], motor_speed[%f, %f]", robot_speed[0], robot_speed[1], robot_speed[2], motor_speed[1], motor_speed[0]);
}

// 速度推算里程计
void odom_interface::calc_vel_odom()
{
    double dt = (curr_time_ - last_time_).toSec();
    double delta_x = 0.0;
    double delta_y = 0.0;
    double delta_t = 0.0;
    if (!IS_FLOAT_ZERO(curr_vel_.vx) || !IS_FLOAT_ZERO(curr_vel_.vy) || !IS_FLOAT_ZERO(curr_vel_.vt))
    {
        delta_x = (curr_vel_.vx * cos(curr_vel_.vt) - curr_vel_.vy * sin(curr_vel_.vt)) * dt;
        delta_y = (curr_vel_.vx * sin(curr_vel_.vt) + curr_vel_.vy * cos(curr_vel_.vt)) * dt;
        delta_t = curr_vel_.vt * dt;
        odom_changed_ = true;
    }
    if (odom_changed_)
    {
        ROS_DEBUG_THROTTLE(LOG_CTRL_SPEED, "calc diff odom. diff_time(%f) diff_odom(%f %f %f=%f).", dt, delta_x, delta_y, delta_t, RADIAN_TO_ANGLE(delta_t));

        odom_x_ += delta_x;
        odom_y_ += delta_y;
        odom_t_ += delta_t;
        odom_t_ = fmod(odom_t_, 2 * PI);

        ROS_DEBUG_THROTTLE(LOG_CTRL_SPEED, "calc total odom. vel(%f %f %f) odom(%f %f %f=%f).", curr_vel_.vx, curr_vel_.vy, curr_vel_.vt, odom_x_, odom_y_, odom_t_, RADIAN_TO_ANGLE(odom_t_));
    }
}

参考：

两轮差速运动解算

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

两轮差速底盘的运动模型分析

原理篇：机器人常见底盘运动学分析