本論文由同濟大學智能車研究所夏新博士，熊璐教授，劉偉碩士和余卓平教授根據其車輛狀態估計方面研究成果撰寫完成，已收錄于2018 IEEE IV。本文獻提出了一種基于六自由度IMU和動力學的車身姿態和側向速度估計方法，文中用IMU結合動力學和運動學分別為估計方法設計了三個估計器，展現了作者對IMU、估計算法的研究深度和應用廣度，值得分享。

摘要

本論文利用六自由度慣性測量單元 (Inertial Measurment Unit) 結合車輛動力學提出了一種車身姿態和側向速度的估計方法。該估計方法僅依賴于IMU和車輛動力學，無需依賴其他外部信息源如GNSS(Global NavigationSatellite System)、視覺等。本文使用常用的卡爾曼濾波算法設計了三個估計器：基于IMU信息設計了姿態角估計器用于估計俯仰角和側傾角；基于IMU設計了側向速度估計器；基于車輛動力學模型設計了車輛側向速度估計器。在小側向激勵的工況下，基于車輛動力學的側向速度估計器的側向速度估計結果相比于基于IMU的估計結果更加可靠，該側向速度可用于修正兩個基于運動學模型估計器的累積誤差。在大側向激勵工況下，由于車輛動力學模型的模型精度較低，車輛動力學模型的側向速度估計結果精度也較低，此時基于IMU的姿態角估計器和側向車速估計器進行開環運算。最后進行了蛇行和雙移線工況的實車實驗，驗證了算法的有效性。

1前言

自動駕駛技術的發展衍生了大量前沿問題。其中，準確的車身姿態和車輛側向速度估計方法成為了研究熱點。從感知層面來看，如車載視覺模塊的圖像處理算法通常需要已知車身姿態信息，又如車身姿態和速度信息可輔助高精度定位；從車輛動力學控制角度看，側向車速是車輛轉向控制的基礎。

能夠直接測量車身姿態和側向速度的設備價格十分昂貴，如OxTS公司的RT3000或者是Kistler公司的S-Motion，無法用在量產車上。更加實際可行的實現方法是融合智能車上多源傳感器信息對其進行估計。然而，傳統車載級MEMS（Micro-Electro-Mechanical System）IMU的角速度和加速度傳感器測量值中通常伴隨有不穩定零偏，長時間對其直接積分來獲取車身姿態和車輛側向車速將面臨較大的累積誤差。盡管通過GNSS或者攝像頭模塊能夠對MEMS IMU提供有效的輔助信息，但是GNSS和攝像頭在使用時面臨信號遮擋和多路徑、光線變換等問題，通過GNSS和攝像頭輔助IMU方式有待進一步研究。

此外，可使用車輛標配傳感器如方向盤轉角和輪速傳感器的輸出，用于基于車輛動力學模型的側向車速估計器。使用該估計結果可計算車輛由于平動產生的側向加速度，去除該部分加速度后，加速度測量值中的余下部分即可用于車輛姿態估計。但在極限工況下，由于模型失準，基于動力學模型的側向車速估計器的估計精度會下降。

本文利用6自由度IMU（三軸陀螺儀和三軸加速度計）和車輛動力學對車身姿態和側向速度同時進行估計。設計了基于運動學模型的車輛側向速度估計器。在小側向激勵工況下，基于動力學模型的估計器能夠給出精確的側向速度。鑒于基于動力學模型的側向車速估計器和基于運動學模型的估計器能同時輸出側向速度，兩個結果的偏差能夠作為反饋，補償基于運動學模型的側向速度估計器在小側向激勵工況下的累積誤差。此外，由基于動力學模型的估計器輸出的側向速度，能夠抵除側向加速度中由運動產生的加速度，從而利用重力產生的加速度分量計算姿態角。在極限工況下，切斷反饋信息，利用開環積分直接計算姿態和側向速度。MEMS在劇烈工況下進行短時的積分不會產生大的累積誤差，文末的蛇行和雙移線實驗也驗證了這一點。

2姿態及側向速度估計

2.1.過程模型

a.傳感器模型

通過Allan方差分析IMU中的誤差源，陀螺儀和加速度計的輸出中包含真值，常值零偏和隨機游走項誤差以及寬帶噪聲項誤差。通過一階馬爾科夫模型來描述隨機游走誤差。τ是時間常數，Wb是寬帶噪聲。陀螺儀測量模型如下式給出：

加速度模型建立如下：

下標s表示傳感器的測量值，上標·表示變量的導數，Φ，θ和φ分別是側傾，俯仰和橫擺角，a下標x,y,z分別是縱向，側向，垂向加速度。

b.歐拉角和速度動力學

選擇歐拉角來表征車輛的姿態。旋轉順序是Z-Y-X。繞每根軸旋轉就得到了橫擺角、俯仰角和側傾角。歐拉角動力學由下式給出：

車身的速度動力學由下式給出，v下標x,y,z分別是縱向，側向和垂向速度。g是重力加速度。

c.輪胎模型

估計算法的精度取決于動力學模型的準確性，特別是輪胎模型。考慮到側向力存在非線性情況，本文中采用了刷子輪胎模型。假設不存在縱向力，刷子輪胎模型的公式如下：

其中，μ是峰值輪胎路面附著系數，α是輪胎側偏角，Fy是側向力，Fz是輪胎垂向力，d是輪胎接地印記長度，c1是側向力側偏剛度。

d.車輛動力學

基于單軌模型的車輛動力學方程由下式給出，其中γ是橫擺角速度，Fyf和Fyr分別是前后軸側向力，Nz是由車輪縱向力造成的橫擺力矩，m是總質量，lf和lr分別是前后軸側向力作用點到質心的距離，Iz是轉動慣量，是狀態變量。

為了計算輪胎力，側偏角由下式獲得：

2.2測量模型

如前文所述，本文建立了三個估計器，在小側向激勵工況下，可通過基于動力學模型的估計器估計出側向車速，進一步使用該側向車速估計出由于平動產生的側向加速度，加速度傳感器測量值剔除該平動加速度后，余下的部分即為由于姿態角導致的重力在加速度傳感器測量值中的分量。由于本文關注的是姿態和側向速度估計，因此假設在小縱向加速度情況下，縱向速度可直接從輪速傳感器獲取。然后，我們給出圖1和圖2來判斷基于動力學模型估計器輸出的側向速度能否用于側傾角反饋以及側向速度的運動學估計，并判斷輪速傳感器輸出的縱向速度能否用于俯仰角及縱向速度反饋。

圖1 姿態估計反饋機制

圖2 速度估計反饋機制

之后我們將用EKF(Extended Kalman Filter)和KF(Kalman Filter)分別估計姿態和側向速度。從上面兩個邏輯圖可以看出，當車輛處于小激勵情況下，新息可以用于時間更新過程，當車輛處于大激勵運動時，反饋被切斷。

動力學模型估計器一直處于運行狀態，該估計器的測量值為橫擺角速度。

2.3.估計方法

本文使用常用的EKF和KF濾波算法進行狀態估計。歐拉角動力學是非線性的，我們使用擴展卡爾曼濾波（EKF）進行姿態估計。基于運動學的估計器中模型是線性的，因此選用卡爾曼濾波（KF）進行速度估計。對于估計側向速度的動力學觀測器，使用擴展卡爾曼濾波（EKF）處理輪胎模型的非線性特性。模塊整體結構如圖3表示，綠色表示傳感器模塊，兩個黃色模塊表示圖1和圖2的反饋機制。

圖3 估計方法結構示意圖

3實驗驗證

3.1.實驗配置

硬件配置如圖4所示，ADIS16445是一款包含三軸陀螺儀和三軸加速度計的IMU。XC2287是一款英飛凌生產的微控制單元，用于從ADIS16445讀取數據，使用SPI通訊協議將信號從ADIS16445中讀取出來，然后以500Kbps發送到CAN總線上。S-motion用于提供側傾，俯仰角和側向車速參考值。NI的CompactRIO用于數據采集。所有的硬件采集設備如圖5所示。

圖4 硬件配置圖

圖5 實驗硬件示意圖

3.2.實驗結果

為了證明估計方法的有效性，在速度50~60Km/h的蛇行和雙移線工況下進行了實驗，實驗結果如下所示。

a.蛇行實驗

(a)側向加速度

(b)縱向速度

(c)側傾角

(d)俯仰角

(e)側向速度

圖6 蛇行工況實驗結果

b.雙移線實驗

(a)側向加速度

(b)縱向速度

(c)側傾角

(d)俯仰角

(e)側向速度

圖7 雙移線工況實驗結果

兩種工況下縱向速度保持在50~60Km/h，而最大側向加速度達到了10m/s^2，表明車輛達到了極限工況狀態，本文的估計方法依然可以較好地估計姿態和側向速度。對于側傾角，雖然開環積分時會存在一定的累積誤差，但是由于側傾角反饋機制，后面側傾角的估計值接近于S-motion的測量值。由于俯仰角絕對值很小，即使輪速傳感器存在很小的縱向速度誤差，對結果也可能造成較大的俯仰角估計誤差。在反饋邏輯中，我們把a0取為較小值，導致俯仰角的反饋持續時間小于側傾角的反饋時間，這也解釋了為什么俯仰角估計誤差大于側傾角估計誤差。從圖6(e)我們得知，藍線跟蹤紅線的效果比綠線要好，表明使用基于車輛動力學模型估計出的側向速度，能夠對運動學模型的側向速度和姿態估計提供有效的反饋。最大側向速度估計誤差小于0.3 m/s顯示了較好的估計性能。從雙移線工況也看出來相同的估計結果。

4結論

本文提出了智能車的姿態和側向速度估計方法。這種方法融合了6自由度IMU和車輛動力學信息，不需要外界信息的輔助。考慮到IMU和車輛動力學的互補特性，我們建立了三個估計器：一個基于運動學模型的估計器用于估計姿態，一個基于運動學模型的估計器用于估計側向速度，一個車輛動力學估計器用于估計側向速度。在小激勵情況下，車輛動力學估計器獲得的側向速度能夠有效輔助另外兩個估計器，即作為反饋修正運動學模型的姿態和側向速度估計器的估計結果。在大側向激勵情況下，基于運動學模型的估計器獨立運行。以上估計方法通過蛇行和雙移線等極限工況證明了有效性。