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

摘要

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

1前言

自動(dòng)駕駛技術(shù)的發(fā)展衍生了大量前沿問題。其中，準(zhǔn)確的車身姿態(tài)和車輛側(cè)向速度估計(jì)方法成為了研究熱點(diǎn)。從感知層面來看，如車載視覺模塊的圖像處理算法通常需要已知車身姿態(tài)信息，又如車身姿態(tài)和速度信息可輔助高精度定位；從車輛動(dòng)力學(xué)控制角度看，側(cè)向車速是車輛轉(zhuǎn)向控制的基礎(chǔ)。

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

此外，可使用車輛標(biāo)配傳感器如方向盤轉(zhuǎn)角和輪速傳感器的輸出，用于基于車輛動(dòng)力學(xué)模型的側(cè)向車速估計(jì)器。使用該估計(jì)結(jié)果可計(jì)算車輛由于平動(dòng)產(chǎn)生的側(cè)向加速度，去除該部分加速度后，加速度測量值中的余下部分即可用于車輛姿態(tài)估計(jì)。但在極限工況下，由于模型失準(zhǔn)，基于動(dòng)力學(xué)模型的側(cè)向車速估計(jì)器的估計(jì)精度會下降。

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

2姿態(tài)及側(cè)向速度估計(jì)

2.1.過程模型

a.傳感器模型

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

加速度模型建立如下：

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

b.歐拉角和速度動(dòng)力學(xué)

選擇歐拉角來表征車輛的姿態(tài)。旋轉(zhuǎn)順序是Z-Y-X。繞每根軸旋轉(zhuǎn)就得到了橫擺角、俯仰角和側(cè)傾角。歐拉角動(dòng)力學(xué)由下式給出：

車身的速度動(dòng)力學(xué)由下式給出，v下標(biāo)x,y,z分別是縱向，側(cè)向和垂向速度。g是重力加速度。

c.輪胎模型

估計(jì)算法的精度取決于動(dòng)力學(xué)模型的準(zhǔn)確性，特別是輪胎模型?？紤]到側(cè)向力存在非線性情況，本文中采用了刷子輪胎模型。假設(shè)不存在縱向力，刷子輪胎模型的公式如下：

其中，μ是峰值輪胎路面附著系數(shù)，α是輪胎側(cè)偏角，F(xiàn)y是側(cè)向力，F(xiàn)z是輪胎垂向力，d是輪胎接地印記長度，c1是側(cè)向力側(cè)偏剛度。

d.車輛動(dòng)力學(xué)

基于單軌模型的車輛動(dòng)力學(xué)方程由下式給出，其中γ是橫擺角速度，F(xiàn)yf和Fyr分別是前后軸側(cè)向力，Nz是由車輪縱向力造成的橫擺力矩，m是總質(zhì)量，lf和lr分別是前后軸側(cè)向力作用點(diǎn)到質(zhì)心的距離，Iz是轉(zhuǎn)動(dòng)慣量，是狀態(tài)變量。

為了計(jì)算輪胎力，側(cè)偏角由下式獲得：

2.2測量模型

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

圖1 姿態(tài)估計(jì)反饋機(jī)制

圖2 速度估計(jì)反饋機(jī)制

之后我們將用EKF(Extended Kalman Filter)和KF(Kalman Filter)分別估計(jì)姿態(tài)和側(cè)向速度。從上面兩個(gè)邏輯圖可以看出，當(dāng)車輛處于小激勵(lì)情況下，新息可以用于時(shí)間更新過程，當(dāng)車輛處于大激勵(lì)運(yùn)動(dòng)時(shí)，反饋被切斷。

動(dòng)力學(xué)模型估計(jì)器一直處于運(yùn)行狀態(tài)，該估計(jì)器的測量值為橫擺角速度。

2.3.估計(jì)方法

本文使用常用的EKF和KF濾波算法進(jìn)行狀態(tài)估計(jì)。歐拉角動(dòng)力學(xué)是非線性的，我們使用擴(kuò)展卡爾曼濾波（EKF）進(jìn)行姿態(tài)估計(jì)?；谶\(yùn)動(dòng)學(xué)的估計(jì)器中模型是線性的，因此選用卡爾曼濾波（KF）進(jìn)行速度估計(jì)。對于估計(jì)側(cè)向速度的動(dòng)力學(xué)觀測器，使用擴(kuò)展卡爾曼濾波（EKF）處理輪胎模型的非線性特性。模塊整體結(jié)構(gòu)如圖3表示，綠色表示傳感器模塊，兩個(gè)黃色模塊表示圖1和圖2的反饋機(jī)制。

圖3 估計(jì)方法結(jié)構(gòu)示意圖

3實(shí)驗(yàn)驗(yàn)證

3.1.實(shí)驗(yàn)配置

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

圖4 硬件配置圖

圖5 實(shí)驗(yàn)硬件示意圖

3.2.實(shí)驗(yàn)結(jié)果

為了證明估計(jì)方法的有效性，在速度50~60Km/h的蛇行和雙移線工況下進(jìn)行了實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如下所示。

a.蛇行實(shí)驗(yàn)

(a)側(cè)向加速度

(b)縱向速度

(c)側(cè)傾角

(d)俯仰角

(e)側(cè)向速度

圖6 蛇行工況實(shí)驗(yàn)結(jié)果

b.雙移線實(shí)驗(yàn)

(a)側(cè)向加速度

(b)縱向速度

(c)側(cè)傾角

(d)俯仰角

(e)側(cè)向速度

圖7 雙移線工況實(shí)驗(yàn)結(jié)果

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

4結(jié)論

本文提出了智能車的姿態(tài)和側(cè)向速度估計(jì)方法。這種方法融合了6自由度IMU和車輛動(dòng)力學(xué)信息，不需要外界信息的輔助?？紤]到IMU和車輛動(dòng)力學(xué)的互補(bǔ)特性，我們建立了三個(gè)估計(jì)器：一個(gè)基于運(yùn)動(dòng)學(xué)模型的估計(jì)器用于估計(jì)姿態(tài)，一個(gè)基于運(yùn)動(dòng)學(xué)模型的估計(jì)器用于估計(jì)側(cè)向速度，一個(gè)車輛動(dòng)力學(xué)估計(jì)器用于估計(jì)側(cè)向速度。在小激勵(lì)情況下，車輛動(dòng)力學(xué)估計(jì)器獲得的側(cè)向速度能夠有效輔助另外兩個(gè)估計(jì)器，即作為反饋修正運(yùn)動(dòng)學(xué)模型的姿態(tài)和側(cè)向速度估計(jì)器的估計(jì)結(jié)果。在大側(cè)向激勵(lì)情況下，基于運(yùn)動(dòng)學(xué)模型的估計(jì)器獨(dú)立運(yùn)行。以上估計(jì)方法通過蛇行和雙移線等極限工況證明了有效性。