二轮差动模型，机器人底盘chassis里程计计算。

二轮差动模型的机器人底盘，是最常见的机器人底盘，像扫地机器人，循迹小车等。作为机器人的移动部件，它需要实现输入输出两大功能。

输入：接收控制指令速度V和角速度W（v,w）--单位m/s rad/s

输出：机器人相对位姿x、y轴坐标，角度th（x,y,th）--单位 m m rad 

            必要时反馈当前机器人速度CV和角速度CW

以上定义把机器人抽象成一个移动的质点，中心为机器人原地旋转时的旋转中心（非机械或物理上的中心），坐标系为右手系。

定义好了坐标系，我们用一组数据来表示机器人在移动过程中的位姿（位置 x，y 姿态角 th） （x , y , th）坐标系的原点（ 0 , 0 , 0）一般是机器人上电时候的位姿。

通常我们把机器人的旋转中心的坐标系叫做机器人坐标系，机器人相对于上电时刻（0 , 0 , 0）原点的位姿叫做里程计坐标系。

机器人接受控制指令（V , W ）V是指机器人正前方的速度 ， W是指机器人原地旋转时角速度（逆时针为正）。

我们的主要问题是如何获得机器人的里程计数据，即机器人相对运动的轨迹（x , y , th）。

首先我们看角度th，这个比较好理解。就是 机器人当前的所有角度增量的和；角度增量= 角速度*时间  ∆th = CW * ∆t ;   th = ∑ ∆th ;

然后我们看坐标(x,y)，这个要抽象点，分三步：

第一步：计算 在机器人坐标系下（这个坐标系相对机器人中心而言的） 延x轴的增量  ∆x = V *  ∆t；

第二步：将此增量转换成里程计坐标系下的  （∆x ,  ∆y )，这里要用到上周期机器人的角度值 th；  ∆x` = cos(th)    ∆y` = sin(th)

第三步，将 ∆x·  ∆y·累加到到上里程计  x = x + ∆x`   y = y +  ∆y` ;

讲讲麻烦，我们直接举个例子来说明，其实很好理解。

我们假设几个机器人有这样三个时刻 t0 t1 t2，分别对应：(为方便计算角度单位为度 deg deg/s)

 t0：时刻机器人上电，机器人坐标系原点与里程计坐标系原点重叠 ，位于里程计坐标系 （0 , 0 , 0） 点，此时机器人以0.2m/s的速度向前移动1秒。

 t1：时刻机器人停止运动，由于以0.2的速度向前移动1秒，所以里程计下的位姿是（ 0.2 ,  0 , 0 ）。

        此时机器人以30 deg/s 的角速度旋转1秒，后停止转动此时里程计下的位姿是（0.2 , 0 , 30）。

 t2：时刻机器人再以0.2m/s的速度运行1秒。时刻机器人停止运动，此时里程计下坐标是（0.373, 0.1 ,  30） 

上图说明

先写这么多，示例代码整理后再贴出来。

最后打个广告tgv1.taobao.com科派斯机器人，这是我做的重载机器人底盘，请指教，谢谢！

这个博主写的比较漂亮规范，大家可以看看。http://blog.csdn.net/heyijia0327/article/details/47021861