利用MEMS-IMU 檢測車輛運動狀態的自適應方法

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

（1. 武漢大學 測繪學院，武漢 430079；2. 地球空間信息技術協同創新中心，武漢 430079）

0 引言

隨著自動駕駛、車聯網等技術的發展，人們對車輛定位的要求不斷提高。定位的精度和穩定性直接影響著自動駕駛汽車的導航、決策以及高精度地圖的構建[1]。精確、連續、穩定的定位結果不僅可以為駕駛員自身提供豐富的行車信息，還可以為交管部門、其他駕駛員提供準確的路況信息，提高道路交通的安全性和效率。

傳統的組合導航技術采用全球衛星導航系統 （global navigation satellite system， GNSS）與捷聯慣性導航系統（strapdown inertial navigation system， SINS）相結合，可以滿足上述領域對高精度、高可用性、高可靠性的要求。但受限于高級別慣性測量裝置（inertial measurement unit， IMU）昂貴的價格，高精度組合導航系統一般只能用于移動測量等專用領域，難以在民用市場普及。微機電系統（microelectro-mechanical system， MEMS）由于成本較低，可以滿足大眾的需求。

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

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

1 車輛運動狀態約束分析

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

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

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

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

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

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

所以車輛轉彎的準確檢測，可為提升車輛定位結果提供較大輔助。

2 車輛運動狀態自適應檢測方法

本節將分析車輛運動狀態與MEMS-IMU 數據的相關性，說明MEMS-IMU 加速度計、陀螺數據變化分別與車輛零速、轉彎具有明顯對應關系，并介紹自適應方法的原理及實現。

2.1 零速探測

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

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

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

式中：iA 為第i 個歷元的加速度計單軸輸出；μ 為對應軸N 組數據的平均值。

對同一時段內加速度計3 軸在動態和靜態情況下，每秒計算1 個STD，可得到圖2 至圖4 的時序對比圖。

圖2 Data1 時加速度計X 軸動靜態STD 對比

圖3 Data1 時加速度計Y 軸動靜態STD 對比

圖4 Data1 時加速度計Z 軸動靜態STD 對比

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

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

式中i 為的排序號。

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

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

2.2 轉彎檢測

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

圖5 車輛行駛的5 種狀態

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

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

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

這里選取1 段包含幾個轉彎路段的數據段2（Data2），角度變化率與車輛是否轉彎的關系如圖6 所示。

圖6 Data2 航向角變化率

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

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

圖7 車速對變道的影響

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

式中： λ1、 λ2、 λ3、 λ4為角度變化率 閾 值，λ1＞ λ2＞ λ3＞λ4；v1、v2、v3為速度閾值，v1＜ v2＜v3。由于本文測試路段為城市道路，一般限速最高為60 km ? s-1，故本文最高速度閾值設置為13 m ? s-1，即46.8 km ? h-1。通過分析測試數據，根據車輛航向角變化率的變化情況與是否在變道或在小角度弧段中行駛的關系，設計閾值為： λ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 實驗與結果分析

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

圖8 實驗路線

3.1 零速探測

利用高精度組合系統數據計算的速度結果，認為水平速度小于0.05 m ? s-1的歷元是靜態，以此得到全時段車輛動靜態情況，如圖9 所示。

圖9 全時段車輛動靜態情況

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

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

圖10 X 軸靜態STD 與閾值對比

圖11 Z 軸靜態STD 與閾值對比

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

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

表1 零速探測統計結果

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

3.2 轉彎檢測

首先給出采用固定閾值進行轉彎檢測的結果，如圖12 所示。

圖12 固定閾值的轉彎檢測結果

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

根據式（7）添加速度條件后，依速度自適應閾值進行彎道檢測的結果如圖13 所示。

圖13 根據速度自適應閾值的轉彎檢測結果

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

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

圖14 變道示意圖

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

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

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

圖16 不同速度閾值對轉彎檢測的影響

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

4 結束語

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