里程计是衡量我们从初始位姿到终点位姿的一个标准，通俗的说，我们要实现机器人的定位与导航，就需要知道机器人行进了多少距离，是往哪个方向行进的 里程计的计算是指以机器人上电时刻为世界坐标系的起点（机器人的航向角是世界坐标系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 *