自动驾驶（五十七）---------车身航迹推算

      自动驾驶中kalman滤波随处可见，这中间不可避免的要知道预测模型，而预测模型很大程度上依赖于本车姿态变化的推算，俗称航迹推算，尤其在控制中，pid调节也是以此为基础。

      之前的文章有零散的讨论过这方面的内容总结：惯性导航原理   激光点云配准   基于视觉的自定位  车辆运动学模型

      但是并没有系统的总结如何融合车身现有传感器，进行本车航迹推算。本文以常见传感器数据（gps+imu+车速+高精地图）为基础，提出一种计算航迹推算的方法。

1. 运动学模型

       首先我们获取车身信号，采用运动学模型估算每一帧车身的变化，在运动学模型中，最关键的是角度变化，而车身并没有角度测量仪，只有通过IMU积分计算车身角度变化。

       首先我们不断获取陀螺采集信号，△V=V-Vo，当前角速率f(x)=k*△V,k是陀螺的比例因子，对角速率求积分就可以得到角度值，常用积分公式为梯度公式。

           不断更新得到任意时刻机器人角度变化。

       知道角度变化后，采用运动学模型推算车身变化：

                      

        移动机器人前进速度等于左右轮速度的平均，现在来推导机器人航向角如何计算，以及如何计算角速度。如图所示，把两个时刻的机器人位置叠加在一起，可以清楚的看到移动机器人航向角变化量是。从图中的几何关系可以得到：  

        也就是说移动机器人航向角变化了多少角度，它就绕其运动轨迹的圆心旋转了多少角度。这句话很好验证，我们让机器人做圆周运动，从起点出发绕圆心一圈回到起点处，在这过程中机器人累计的航向角为360度，同时它也确实绕轨迹圆心运动了360度，说明机器人航向角变化多少度，就绕圆心旋转了多少度。而这三个角度中，很容易计算出来，由于相邻时刻时间很短，角度 变化量很小，有下面的近似公式： 

                

        所以可以得到机器人绕圆心运动的角速度，它也是机器人航向角变化的速度：

             

         线速度、角速度都有了，因此可以推出移动机器人圆弧运动的半径：

                 

       可以发现当左轮速度等于右轮速度时，半径无穷大，即直线运动。最后将三个公式综合起来，可以得到左右轮速度和线速度角速度之间的关系如下：