使用里程计odometry计算转角

我们一般使用里程计odometry获取轮式机器人左右轮子的转速，从而计算出机器人前进的距离，进一步估算出机器人相对于上一次的运动位置。里程计除了可以计算距离也可以计算机器人的转角。

 

 

如上图所示，机器人运动到红色位置。通过里程计，我们可以计算出左轮和右轮行进的距离S1和S2。在进行位置估计时，机器人前后位置相差是比较小的，因此弧段S1和S2可以看做是直线段。右轮行进的距离减去左轮行进的距离得到距离差S2'，再除以轴距l，即可得到机器人转过的角度theta。

即：theta= (rpmR-rpmL)*dt*wheel_diameter*π/(60*l)，其中rpmR是右轮的转速，rpmL是左轮的转速，wheel_diameter是轮子的直径，l是轴距。