关于二轮差速小车轮速计算和里程计计算

这是我参与8月更文挑战的第17天，活动详情查看：8月更文挑战”

里程计是衡量我们从初始位姿到终点位姿的一个标准，通俗的说，我们要实现机器人的定位与导航，就需要知道机器人行进了多少距离，是往哪个方向行进的
里程计的计算是指以机器人上电时刻为世界坐标系的起点（机器人的航向角是世界坐标系X正方向）开始累积计算任意时刻机器人在世界坐标系下的位姿。通常计算里程计方法是速度积分推算：通过左右电机的编码器测得机器人的左右轮的速度VL和VR，在一个短的时刻△t内，认为机器人是匀速运动，并且根据上一时刻机器人的航向角计算得出机器人在该时刻内世界坐标系上X和Y轴的增量，然后将增量进行累加处理，关于航向角θ采用的IMU的yaw值。然后根据以上描述即可得到机器人的里程计。

1 逆解

通过设定的v和角度，计算向左右轮发的速度
ROBOT_RADIUS指的是机器人半径，也就是机器人两轮间距的一半
   // 计算左右轮期望速度
    if(RobotV == 0)
    {
        leftdata.d = -YawRate * ROBOT_RADIUS;
        rightdata.d = YawRate * ROBOT_RADIUS;
    } 
    else if(YawRate == 0)
    {
        leftdata.d = RobotV;
        rightdata.d = RobotV;
    }
    else
    {
        leftdata.d  = YawRate * (r – ROBOT_RADIUS);
        rightdata.d = YawRate * (r + ROBOT_RADIUS);
    }

2 里程计正解

根据左右轮速和角度来计算里程计
 //===========================速度计算和Angle获取
  // x方向速度，以及角速度
 vx       = (rightVelNow.d + leftVelNow.d) / 2.0 / 1000.0;        //m/s
vth      = (rightVelNow.d – leftVelNow.d) / ROBOT_LENGTH ;       //rad/s
th       = angleNow.d*0.01745;//实时角度信息(rad)

double dt = (curr_time – last_time_).toSec(); //间隔时间
double delta_x = (vx_ * cos(th_)) * dt;         //th_弧度
double delta_y = (vx_ * sin(th_)) * dt;
double delta_th = vth_ * dt;

//打印时间间隔调试信息，不用的时候可以关闭
//ROS_INFO(“dt:%f\n”,dt);                       //s
     //里程计累加
        x_ += delta_x;
        y_ += delta_y;
        //实时角度信息,如果这里不使用IMU，也可以通过这种方式计算得出
        th_ += delta_th;                                //里程计解算积分后角度