自動駕駛（五十七）---------車身航跡推算

      自動駕駛中kalman濾波隨處可見，這中間不可避免的要知道預測模型，而預測模型很大程度上依賴於本車姿態變化的推算，俗稱航跡推算，尤其在控制中，pid調節也是以此爲基礎。

      之前的文章有零散的討論過這方面的內容總結：慣性導航原理   激光點雲配準   基於視覺的自定位  車輛運動學模型

      但是並沒有系統的總結如何融合車身現有傳感器，進行本車航跡推算。本文以常見傳感器數據（gps+imu+車速+高精地圖）爲基礎，提出一種計算航跡推算的方法。

1. 運動學模型

       首先我們獲取車身信號，採用運動學模型估算每一幀車身的變化，在運動學模型中，最關鍵的是角度變化，而車身並沒有角度測量儀，只有通過IMU積分計算車身角度變化。

       首先我們不斷獲取陀螺採集信號，△V=V-Vo，當前角速率f(x)=k*△V,k是陀螺的比例因子，對角速率求積分就可以得到角度值，常用積分公式爲梯度公式。

           不斷更新得到任意時刻機器人角度變化。

       知道角度變化後，採用運動學模型推算車身變化：

                      

        移動機器人前進速度等於左右輪速度的平均，現在來推導機器人航向角如何計算，以及如何計算角速度。如圖所示，把兩個時刻的機器人位置疊加在一起，可以清楚的看到移動機器人航向角變化量是。從圖中的幾何關係可以得到：  

        也就是說移動機器人航向角變化了多少角度，它就繞其運動軌跡的圓心旋轉了多少角度。這句話很好驗證，我們讓機器人做圓周運動，從起點出發繞圓心一圈回到起點處，在這過程中機器人累計的航向角爲360度，同時它也確實繞軌跡圓心運動了360度，說明機器人航向角變化多少度，就繞圓心旋轉了多少度。而這三個角度中，很容易計算出來，由於相鄰時刻時間很短，角度 變化量很小，有下面的近似公式： 

                

        所以可以得到機器人繞圓心運動的角速度，它也是機器人航向角變化的速度：

             

         線速度、角速度都有了，因此可以推出移動機器人圓弧運動的半徑：

                 

       可以發現當左輪速度等於右輪速度時，半徑無窮大，即直線運動。最後將三個公式綜合起來，可以得到左右輪速度和線速度角速度之間的關係如下：