利用MEMS-IMU 檢測車輛運動狀態(tài)的自適應(yīng)方法

胡昊杰，朱 鋒，張小紅,2

（1. 武漢大學(xué) 測繪學(xué)院，武漢 430079；2. 地球空間信息技術(shù)協(xié)同創(chuàng)新中心，武漢 430079）

0 引言

隨著自動駕駛、車聯(lián)網(wǎng)等技術(shù)的發(fā)展，人們對車輛定位的要求不斷提高。定位的精度和穩(wěn)定性直接影響著自動駕駛汽車的導(dǎo)航、決策以及高精度地圖的構(gòu)建[1]。精確、連續(xù)、穩(wěn)定的定位結(jié)果不僅可以為駕駛員自身提供豐富的行車信息，還可以為交管部門、其他駕駛員提供準(zhǔn)確的路況信息，提高道路交通的安全性和效率。

傳統(tǒng)的組合導(dǎo)航技術(shù)采用全球衛(wèi)星導(dǎo)航系統(tǒng) （global navigation satellite system， GNSS）與捷聯(lián)慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system， SINS）相結(jié)合，可以滿足上述領(lǐng)域?qū)Ω呔?、高可用性、高可靠性的要求。但受限于高級別慣性測量裝置（inertial measurement unit， IMU）昂貴的價格，高精度組合導(dǎo)航系統(tǒng)一般只能用于移動測量等專用領(lǐng)域，難以在民用市場普及。微機電系統(tǒng)（microelectro-mechanical system， MEMS）由于成本較低，可以滿足大眾的需求。

為了提高車輛的定位精度，在解算過程中常常采用零速修正、速度約束等方法來提高精度，而這些方法在特定的車輛運動條件下使用才能取得更好的效果。這意味著對車輛運動狀態(tài)的準(zhǔn)確檢測至關(guān)重要，比如判斷車輛停止、直行或轉(zhuǎn)彎?，F(xiàn)有研究主要通過閾值法進(jìn)行零速探測：文獻(xiàn)[2]通過加速度計X 軸、Y 軸在窗口內(nèi)的最大值與最小值之差來判斷零速；文獻(xiàn)[3]利用加速度計方差，并引入航向角進(jìn)行探測；文獻(xiàn)[4]通過加速度計X 軸、Y 軸數(shù)據(jù)的標(biāo)準(zhǔn)差配合磁力計磁強變化來判斷車輛是否靜止；文獻(xiàn)[5]通過滑動窗口內(nèi)陀螺和加速度計數(shù)據(jù)的標(biāo)準(zhǔn)差來判斷。但閾值法一般不具有通用性，不同的設(shè)備需要確定不同的閾值。文獻(xiàn)[6]通過處理樣本數(shù)據(jù)獲得標(biāo)準(zhǔn)差（standard deviation，STD）靜止概率曲線來實現(xiàn)對閾值的自適應(yīng)選??；文獻(xiàn)[7]通過時頻變化，從頻域上進(jìn)行探測。這些方法往往難以滿足當(dāng)前定位技術(shù)對實時性和靈活性的需求。在車輛轉(zhuǎn)彎檢測方面，文獻(xiàn)[8-9]直接將MEMS-IMU 安裝在方向盤上檢測轉(zhuǎn)向，但不同車型、不同路況條件下，方向盤轉(zhuǎn)動不能真實反映車輛轉(zhuǎn)向情況；文獻(xiàn)[4]定義了直線、擺動、曲線、轉(zhuǎn)彎4 種車輛運動狀態(tài)，并通過陀螺Z 軸數(shù)據(jù)的均值來檢測后2 種狀態(tài)；文獻(xiàn)[5]考慮了IMU 與車輛的安裝角問題，采用陀螺3 軸輸出判斷轉(zhuǎn)彎，但在測試時，主要針對大角度轉(zhuǎn)彎。

本文提出的車輛運動狀態(tài)檢測自適應(yīng)方法，是利用滑動窗口內(nèi)的MEMS-IMU 數(shù)據(jù)提供零速探測閾值，閾值隨窗口滑動不斷變化，具有良好的靈活性和通用性；利用MEMS-IMU 數(shù)據(jù)計算航向角判斷轉(zhuǎn)彎，并通過速度變化實現(xiàn)閾值的自適應(yīng)，可檢測出小弧段變化及車輛變道的狀態(tài)。

1 車輛運動狀態(tài)約束分析

在組合導(dǎo)航系統(tǒng)中，除了GNSS 作為外部觀測更新手段外，零速修正、速度約束等方法常用來抑制SINS 誤差的快速累積。本節(jié)將介紹準(zhǔn)確檢測車輛運動狀態(tài)對于使用約束算法取得良好定位效果的重要性。

在城市環(huán)境下，停車等待是車輛運動中不可避免的1 種狀態(tài)。在理想條件下，在車輛停止期間，MEMS-IMU 輸出的部分?jǐn)?shù)據(jù)應(yīng)該是零，所以實際輸出可認(rèn)為是 MEMS-IMU 的零偏，從而為傳感器在線校正提供輔助。此外，零速探測也是零速修正的關(guān)鍵技術(shù)。零速修正可以通過卡爾曼（Kalman）濾波的系統(tǒng)模型建立速度和位置誤差之間的關(guān)系，進(jìn)而對SINS 遞推的位置誤差進(jìn)行校正[10]。零速修正的約束作用可以通過觀測方程體現(xiàn)。根據(jù)文獻(xiàn)[10]，線性形式的零速修正觀測方程可表示為

因此，零速探測對數(shù)據(jù)補償、算法校正等具有重要作用。

速度約束是指地面車輛在不發(fā)生跳動和側(cè)滑的情況下，側(cè)向速度和垂向速度為零，這可以為組合導(dǎo)航系統(tǒng)提供側(cè)向和垂向的虛擬觀測信息。對于MEMS-IMU，速度約束可以顯著改善 GNSS 失鎖階段的定位精度[11]。速度更新可表示為

速度約束中最關(guān)鍵的是速度約束噪聲的選取：約束過松，會降低約束的有效性；約束過緊，則反而會使結(jié)果變差。車輛在行駛中，發(fā)生側(cè)滑及垂向跳動是非常普遍的現(xiàn)象，尤其是在轉(zhuǎn)彎過程和速度較高的情況下[10]更是如此。對于約束噪聲，一般會給1 個定值，但這并不適用于所有運動狀態(tài)。文獻(xiàn)[12]通過神經(jīng)網(wǎng)絡(luò)訓(xùn)練，結(jié)果表明轉(zhuǎn)彎時的約束噪聲遠(yuǎn)大于直線時的約束噪聲。所以在車輛直線運動時，可以利用速度約束得到較好的定位結(jié)果，但在轉(zhuǎn)彎時，速度約束對結(jié)果的提升作用很小。

此外，在車輛穿過高架橋、天橋等衛(wèi)星信號短時遮擋的環(huán)境時，前后幾個歷元的定位結(jié)果可能出現(xiàn)偏差，此時若能保證車輛直線行駛，則可直接利用航向角信息和速度信息朝前進(jìn)方向遞推，獲得更為準(zhǔn)確的位置結(jié)果。

所以車輛轉(zhuǎn)彎的準(zhǔn)確檢測，可為提升車輛定位結(jié)果提供較大輔助。

2 車輛運動狀態(tài)自適應(yīng)檢測方法

本節(jié)將分析車輛運動狀態(tài)與MEMS-IMU 數(shù)據(jù)的相關(guān)性，說明MEMS-IMU 加速度計、陀螺數(shù)據(jù)變化分別與車輛零速、轉(zhuǎn)彎具有明顯對應(yīng)關(guān)系，并介紹自適應(yīng)方法的原理及實現(xiàn)。

2.1 零速探測

本文采用加速度計輸出作為零速探測的依據(jù)。首先選取了1 段車輛動靜態(tài)交替且區(qū)分較為明顯的數(shù)據(jù)段1（Data1），加速度計3 軸輸出與車輛是否運動如圖1 所示。

圖1 Data1 時加速度計3 軸輸出變化

從圖1 中可以看出，當(dāng)車輛靜止時，加速度計3 軸輸出趨于平穩(wěn)，而在運動時，則波動較大。根據(jù)這個特性，本文采用加速度計數(shù)據(jù)的STD 來判斷車輛是否靜止。若在窗口時間內(nèi)有N 組數(shù)據(jù)，則這N 組數(shù)據(jù)的某1 軸的STD 可表示為

式中：iA 為第i 個歷元的加速度計單軸輸出；μ 為對應(yīng)軸N 組數(shù)據(jù)的平均值。

對同一時段內(nèi)加速度計3 軸在動態(tài)和靜態(tài)情況下，每秒計算1 個STD，可得到圖2 至圖4 的時序?qū)Ρ葓D。

圖2 Data1 時加速度計X 軸動靜態(tài)STD 對比

圖3 Data1 時加速度計Y 軸動靜態(tài)STD 對比

圖4 Data1 時加速度計Z 軸動靜態(tài)STD 對比

MEMS-IMU 在車上的安裝方式為：X 軸朝向車輛右方；Y 軸朝向車輛前方；Z 軸朝向車輛上方。從圖2～圖4 可以看出，X 軸和Z 軸的STD 有相似的特征：在每1 個獨立的靜止時段內(nèi)，STD 整體呈現(xiàn)逐漸增大的趨勢，這可能是實驗車輛在停車怠速情況下，抖動會逐漸增大導(dǎo)致的；對于不同的靜止時段，STD 的均值具有一定差異。這說明臨近時間的STD 對閾值確定具有更大的參考價值。Y 軸指向為車輛前進(jìn)方向，在啟動和停止的臨界時間段，STD 較不穩(wěn)定。基于這樣的特性，本文提出的零速探測方法，是根據(jù)固定長度的滑動窗口內(nèi)得到的X 軸和Z 軸STD 來計算零速閾值的，閾值可表示為

式中：λS為閾值；n 為窗口長度，這里的窗口指的是用于計算閾值的固定窗口，而式（4）中的窗口N 是用于計算STD 的，本文取N=1，所以在計算閾值的窗口內(nèi)，每個軸會有n 個STD 值； ai為窗口內(nèi)的時間順序排列，每秒計算的STD 值； pi為權(quán)重，且 p1≤ p2≤ … ≤pn，以此保證越接近當(dāng)前歷元的STD，對閾值設(shè)定的影響越大；k 為放大系數(shù)，考慮到圖2、圖4 中X 軸和Z 軸每秒計算的STD在獨立的靜止時段內(nèi)呈增大趨勢，k 應(yīng)大于1，但又不宜過大，根據(jù)測試數(shù)據(jù)的趨勢，本文取k=1.3。由于窗口長度是固定的，所以窗口內(nèi)數(shù)據(jù)會不斷更新。若當(dāng)前時段內(nèi)計算的X 軸和Z 軸STD 分別小于2 軸閾值時，則認(rèn)為當(dāng)前時段車輛靜止。本文的窗口長度n 設(shè)計為30 s，故窗口內(nèi)X 軸和Z 軸的STD 值都有30 個。窗口長度根據(jù)測試數(shù)據(jù)獨立靜止時段的長度分布及不同長度窗口的實驗結(jié)果進(jìn)行設(shè)計，結(jié)果表明10～50 s 的結(jié)果差異很小，但窗口過長會導(dǎo)致結(jié)果變差，故將窗口長度固定在30 s。權(quán)重可表示為

式中i 為的排序號。

本文以STD 值在窗口內(nèi)的排序號的平方為依據(jù)來設(shè)計權(quán)重：相比于1 次方，可以增加相近歷元的影響；相比于高次方，不會過于突出相近歷元，以抑制粗差對閾值計算的影響。

零速探測的結(jié)果可用探測率和誤判率來評估。探測率表示探測出的零速歷元在真實零速歷元中所占的比例，誤判率表示探測出的零速歷元在實際為動態(tài)的歷元所占的比例。探測率越高，誤判率越低，則零速探測的結(jié)果越好。

2.2 轉(zhuǎn)彎檢測

車輛在行駛過程中包含多種狀態(tài)，本文將這些狀態(tài)歸為5 種，如圖5 所示。

圖5 車輛行駛的5 種狀態(tài)

實際情況是，車輛不可能完全沿直線行駛，難免會有微小擺動，本文把圖5（a）和圖5（b）這2 種情況都認(rèn)定為直線行駛?，F(xiàn)有研究一般不考慮變道，但針對自動駕駛、車聯(lián)網(wǎng)等技術(shù)，車道級的定位精度是必要的，所以本文把變道與大弧段、轉(zhuǎn)彎一并認(rèn)定為車輛非直線行駛。

本文采用窗口時間內(nèi)陀螺輸出算得的航向角變化（即航向角變化率）作為判斷車輛是否轉(zhuǎn)彎的依據(jù)，車輛轉(zhuǎn)彎需滿足的條件可表示為

式中： δY為航向角變化率；N 為窗口內(nèi)的數(shù)據(jù)總量；gz( i )為陀螺Z 軸方向的輸出；dt 為采樣間隔；T 為窗口時長；Yδλ 為閾值。

這里選取1 段包含幾個轉(zhuǎn)彎路段的數(shù)據(jù)段2（Data2），角度變化率與車輛是否轉(zhuǎn)彎的關(guān)系如圖6 所示。

圖6 Data2 航向角變化率

從圖6 可以看出：航向角變化率快的歷元與車輛轉(zhuǎn)彎的歷元存在明顯對應(yīng)關(guān)系。圖6 中2 條橫線對應(yīng)的橫坐標(biāo)為±2 (° ) ?s-1，基本區(qū)分了不同的角度變化率與是否轉(zhuǎn)彎的關(guān)系，所以2 (° ) ?s-1可以作為轉(zhuǎn)彎閾值。

然而，對于變道和小角度弧段等情況，不得不考慮車速對轉(zhuǎn)彎角度的影響。變道的目的在于快速地從1 條車道切換至另1 條車道，除堵車外，大多數(shù)變道都應(yīng)在短時間內(nèi)完成，即不論車速快慢，變道的時間應(yīng)該是基本相同的。這種不同車速對變道的影響可以通過圖7 體現(xiàn)。

圖7 車速對變道的影響

假設(shè)在相同時間內(nèi)完成變道，圖7（a）車速快于圖7（b），α1小于α2，說明較快的車速只需較小的航向角變化即可完成變道。此外，曲線道路往往由于地形原因而把路修得帶有一定弧度，一般沒有岔路，彎道較緩，車速較快。因此，本文提出的方法針對不同車速設(shè)計不同閾值，判斷車輛轉(zhuǎn)彎應(yīng)滿足的條件可表示為

式中： λ1、 λ2、 λ3、 λ4為角度變化率 閾 值，λ1＞ λ2＞ λ3＞λ4；v1、v2、v3為速度閾值，v1＜ v2＜v3。由于本文測試路段為城市道路，一般限速最高為60 km ? s-1，故本文最高速度閾值設(shè)置為13 m ? s-1，即46.8 km ? h-1。通過分析測試數(shù)據(jù)，根據(jù)車輛航向角變化率的變化情況與是否在變道或在小角度弧段中行駛的關(guān)系，設(shè)計閾值為： λ1= 2. 0 ，λ2=1 . 5 ，λ3=1 . 0 ，λ4= 0. 7，單 位 為 (° ) ? s-1； v1=5 ，v2=1 0，v3=13，單位為 m ? s-1。

3 實驗與結(jié)果分析

實驗數(shù)據(jù)采集使用的MEMS-IMU為MPU9250，采用Novatel SPAN-FSAS 高精度組合導(dǎo)航系統(tǒng)采集的數(shù)據(jù)經(jīng)IE（inertial explorer）軟件計算的緊組合結(jié)果作為參考真值。MEMS-IMU 與高精度IMU均固連于架設(shè)在車頂行李架的鋼板上，各自的載體坐標(biāo)系軸向一致，運動狀態(tài)基本相同。數(shù)據(jù)采集地點為武漢市洪山區(qū)，行車軌跡如圖8 所示。

圖8 實驗路線

3.1 零速探測

利用高精度組合系統(tǒng)數(shù)據(jù)計算的速度結(jié)果，認(rèn)為水平速度小于0.05 m ? s-1的歷元是靜態(tài)，以此得到全時段車輛動靜態(tài)情況，如圖9 所示。

圖9 全時段車輛動靜態(tài)情況

從圖9 可以看出，除實驗開始和結(jié)束時2 段有較長時間的靜置外，中間時段頻繁出現(xiàn)運動和靜止交替出現(xiàn)的情況，真實反映了城市環(huán)境中的行車情況。

圖10、圖11 分別反映了X 軸和Z 軸全時段靜態(tài)STD 值與按照本文提出的方法不斷計算更新的閾值的對比情況。

圖10 X 軸靜態(tài)STD 與閾值對比

圖11 Z 軸靜態(tài)STD 與閾值對比

從圖10（a）、圖11（a）可以看出，在900 s和1 200 s 左右有2 段時間的STD 很大，X 軸達(dá)到0.5 m ? s-1左右，Z 軸達(dá)到0.7 m ? s-1左右，這里為了更好地體現(xiàn)細(xì)節(jié)而把圖中的Y 軸范圍適當(dāng)縮小。并且前面靜置段的STD 也并不是很平穩(wěn)。這是由于實驗起始段停車靜置時，實驗人員不在車上， 但調(diào)試設(shè)備、取放東西等對車輛造成了輕微影響，導(dǎo)致STD 值波動；靜置完成后，人員先后上車，開關(guān)車門與上車會對X 軸、Z 軸數(shù)據(jù)產(chǎn)生影響，導(dǎo)致STD 大幅上升。從圖10（b）、圖11（b）的閾值來看，這樣的數(shù)據(jù)抖動基本被平滑了。這是可以接受的，因為即使這些歷元車速的確為零，但考慮到零速狀態(tài)可能用于數(shù)據(jù)補償，使用這些歷元的數(shù)據(jù)并不是好的選擇。而當(dāng)車輛在道路上正常行駛，動靜交替時，可以看到閾值變化趨勢與靜態(tài)STD 基本相符，且一般情況下閾值略大于STD。

本文零速探測的結(jié)果首先統(tǒng)計了實驗全時段，得到91.91%的探測率和9.92%的誤判率；考慮到駕駛員實際開車時一般不會有起始段長時間的靜止，本文又統(tǒng)計了圖7 中從T 時刻至實驗結(jié)束的結(jié)果，得到94.25%的探測率和10.32%的誤判率。統(tǒng)計如表1 所示。

表1 零速探測統(tǒng)計結(jié)果

表1 中：探測成功歷元數(shù)A 為探測出的歷元中實際為靜止的歷元數(shù)；實際靜止歷元數(shù)B 為實際中靜止的歷元數(shù)；探測靜止歷元數(shù)C 為探測出靜止的歷元數(shù)。探測率D 和誤判率E 可表示為

3.2 轉(zhuǎn)彎檢測

首先給出采用固定閾值進(jìn)行轉(zhuǎn)彎檢測的結(jié)果，如圖12 所示。

圖12 固定閾值的轉(zhuǎn)彎檢測結(jié)果

從圖12 中可以看出，對于直角彎、掉頭、環(huán)島以及部分弧段，固定閾值算法可以檢測到，但對于圖12 中圈出的包含角度緩變曲線的路段，固定閾值算法并不能檢測成功。

根據(jù)式（7）添加速度條件后，依速度自適應(yīng)閾值進(jìn)行彎道檢測的結(jié)果如圖13 所示。

圖13 根據(jù)速度自適應(yīng)閾值的轉(zhuǎn)彎檢測結(jié)果

從圖13 中可以看出，整個行車路線中較為明顯的彎道及弧段已全部檢測出。同時部分直道上也新增了被檢測出的軌跡點。實際上這些軌跡可能包含著變道信息，這也是固定閾值算法難以檢測出的。以圖13 中的圓圈標(biāo)記為例，放大后的軌跡投影在谷歌地球上如圖14 所示。

車輛在這1 段道路上先后行駛過2 次，中間路段2 條軌跡分離，從投影的軌跡來看，車輛行駛在2 條車道上時，可以檢驗出軌跡分離與合并處的坐標(biāo)點，這證明本文的算法對變道的檢測是可行的。

圖14 變道示意圖

此外，本文以圖12 中上方標(biāo)記處的路段為例，說明根據(jù)不同速度設(shè)置閾值是如何影響轉(zhuǎn)彎檢測的。此路段包含了速度閾值劃分的所有速度區(qū)間，整體上車速較快。首先給出此路段的速度變化情況，如圖15 所示，圖例速度的單位為 m ? s-1。

圖15 示例路段速度變化情況

添加不同個數(shù)的速度分段閾值后，檢測結(jié)果對比如圖16 所示。

圖16 不同速度閾值對轉(zhuǎn)彎檢測的影響

圖16（a）為不加速度限制的固定閾值算法，無法檢測出這個帶弧線路段的任何彎曲部分，說明初始閾值對小角度弧段的檢測能力很弱。圖16（b）相較于圖16（a）在速度大于5 m ? s-1時添加1 個閾值，檢測出了2 個明顯弧段，結(jié)合圖15 可以看到，這2 個弧段的速度都在13 m ? s-1以上，說明最初的閾值設(shè)置對于這2 個弧段略高，在非低速情況下可適當(dāng)降低閾值。圖16（c）相較于圖16（b），在速度大于10 m ? s-1時添加1 個閾值，多檢測出了1 個弧段和1 個變道。這個弧段與之前已經(jīng)檢測出的2 個相比，角度更小，所以需要更小的閾值；而檢測出的變道路段的速度處在新增閾值的這個速度段內(nèi)，說明這個速度段和閾值的設(shè)計是合理的。圖16（d）相較于圖16（c），在速度大于13 m ? s-1時添加1 個閾值，由于此路段的轉(zhuǎn)彎已經(jīng)全被檢測出，所以圖16（d）相較于圖16（c）無明顯差異。由此看來，根據(jù)速度改變閾值對于檢測小角度弧段、變道是很有必要的。

4 結(jié)束語

本文提出了根據(jù)滑動窗口內(nèi)的加速度計數(shù)據(jù)在線確定閾值的零速探測方法，以及根據(jù)速度自適應(yīng)閾值進(jìn)行轉(zhuǎn)彎檢測的方法。首先說明了車輛運動狀態(tài)的準(zhǔn)確檢測對提升定位算法結(jié)果的重要性，然后分析了IMU 數(shù)據(jù)與車輛運動狀態(tài)之間的關(guān)系，表明加速度計輸出與車輛是否靜止、陀螺航 向軸數(shù)據(jù)與車輛轉(zhuǎn)彎具有明顯聯(lián)系，并介紹了算法的設(shè)計。測試結(jié)果表明零速探測方法具有較高成功率，探測率為94.25%，誤判率為10.32%；轉(zhuǎn) 彎檢測算法可檢測出車輛行駛中絕大部分轉(zhuǎn)彎、小弧段變化及變道情況，并分析了不同速度及對應(yīng)閾值對轉(zhuǎn)彎檢測的影響。