机器人差分运动模型如上图：
假设我们知道左右轮的速度，则线速度为V=(Vl+Vr)/2 角速度为W=(Vr-Vl)/l 圆弧运动半径为r=V/W
公式①：机器人前进速度等于两轮速度之和的平均值
公式②：因为在采样周期非常短的时间内，两条虚线近似可以看成直线，角速度可以表示为（右轮速度-左轮速度）/轴长
公式③：根据以上2个公式整理得到的。
从公式③可以看出当左轮速度等于右轮时，半径无穷大，即机器人为直线运动。
言归正传，Vr与Vl怎么得到呢？采样时间内测得的编码值除以轮子一圈的总脉冲数就是速度,即将轮子分成无数份。
轴长怎么得到呢？测两电机直接摆放的距离。
就可以进行任意控制两轮机器人行走啦，还能通过计算获得里程计的信息。
里程计的计算：
已知以上的公式可以得到角速度和线速度，那么我们知道 采样时间内转过的角度=角速度*时间
那么对每一次采样得到的角度进行积分，就是转过的角度
得到角度值以后，
position_x= position_x +S*cos(θ)
position_y= position_y +S*sin(θ)