非完整約束的OD/SINS自適應組合導航方法

劉萬科，農 旗，陶賢露，朱 鋒，胡 捷

1. 武漢大學測繪學院, 湖北 武漢 430079; 2. 武漢大學地球空間環(huán)境與大地測量教育部重點實驗室, 湖北 武漢 430079

在地面車輛組合導航中，基于微機電系統(tǒng)(micro-electro-mechanical system，MEMS)技術的低成本慣性測量單元(inertial measurement unit，IMU)因其體積小、成本低而受到廣泛應用，但受限于其誤差發(fā)散快而無法單獨使用[1-4]。全球導航衛(wèi)星系統(tǒng)(global navigation satellite system，GNSS)已經被廣泛應用于減少MEMS捷聯(lián)慣性導航系統(tǒng)(strap-down inertial navigation system，SINS)誤差累積，但當車輛在高樓之間、樹蔭和隧道內行駛時，GNSS會遇到頻繁的信號遮擋甚至中斷，導致組合導航結果發(fā)散[5-7]。

針對GNSS信號中斷導致車輛組合導航結果發(fā)散的問題，國內外學者開展了一系列的研究工作。根據運動學原理，車輛在地面上運動時，如果沒有側向滑動或上下跳躍波動，其橫向和垂直分量的速度將接近于零[1，8]，即車輛運動的非完整性約束(non-holonomic constraint，NHC)，因此可以在組合導航中構建虛擬的速度觀測量。相關研究已經表明，里程計和NHC可以顯著提高組合導航精度，增強SINS的性能，尤其是當GNSS信號不可用時，可以有效抑制組合導航系統(tǒng)的誤差發(fā)散[9-13]。此外，有學者指出在使用NHC時，前向桿臂對非完整性約束的影響最大，為保證非完整約束輔助的準確性和可靠性，桿臂誤差應控制在厘米級[14]。

NHC的虛擬觀測信息是在特定的車輛運動狀態(tài)中建立的，然而車輛在實際行駛時不可能完全沿直線，在轉彎過程和速度較高的情況下，發(fā)生側滑及垂向跳動是非常普遍的。因此，在使用NHC時應考慮車輛運動狀態(tài)信息[15-18]，實時調整約束噪聲。已有學者對車輛運動狀態(tài)進行了相關研究，通過車輛速度與IMU的原始數據可以檢測出車輛行駛過程中絕大部分轉彎、小弧段變化及變道情況[18-19]，該方法為NHC自適應調整噪聲提供了可能性，但相關學者沒有進一步將NHC與車輛運動狀態(tài)結合進行研究。

綜上所述，目前學者們廣泛將NHC用于抑制GNSS信號中斷期間組合導航的誤差發(fā)散，但大多數研究使用NHC時并未考慮車輛轉彎等特殊情況，也未詳細說明使用NHC時的噪聲設置。為解決NHC固定噪聲與車輛實際運動狀態(tài)不相符的問題，本文分析了NHC噪聲與車輛運動狀態(tài)的關系，構建了一種基于車輛速度與運動狀態(tài)的NHC噪聲自適應估計方法，該方法通過IMU測量值與車輛CAN總線提供的速度測量值判斷車輛運動狀態(tài)，實現(xiàn)了NHC噪聲的自適應估計。通過實測車載數據的分析表明，該方法能夠較好地抑制GNSS中斷期間組合導航系統(tǒng)的誤差發(fā)散。

1 NHC/OD/SINS組合模型

1.1 狀態(tài)方程

本文采用地心地固系(e系)松組合模型，系統(tǒng)狀態(tài)選取機械編排結果的3軸位置誤差δre、速度誤差δve、失準角誤差φ，IMU 3軸加速計零偏ab，3軸陀螺零偏εb，共15維

(1)

e系下機械編排誤差的時域微分方程可表示為[20-21]

(2)

(3)

式中，ηa和ηε為隨機白噪聲。

將式(2)的時域微分方程離散化，可得到狀態(tài)方程

Xk,k-1=Φk,k-1Xk-1+Wk-1

(4)

式中，Xk,k-1和Xk-1為當前時刻狀態(tài)預測值和上一時刻狀態(tài)值；Φk,k-1為狀態(tài)轉移矩陣；Wk-1為狀態(tài)噪聲。

1.2 觀測方程

在車輛導航中，通常假設側向與高程方向的運動速度近似為0，且NHC一般適用于非轉向輪(即車輛后輪)。此外，從車輛的CAN總線中可以獲取到前進方向的運動速度，即里程計(OD)速度vod。結合NHC適用于非轉向輪，本文選取的b系的方向為右前上，即y軸朝車輛前進方向，x軸垂直于y軸朝車輛前進方向的右向，z軸垂直朝上。將NHC/OD組合，考慮IMU安裝失準角為可忽略的小角度，可得b系下車輛的速度為

(5)

由慣性導航機械編排獲得速度在b系下可表示為

(6)

其中，上標“～”表示該量帶有誤差(由機械編排直接獲得)；“φ×”表示φ的反對稱矩陣。

(7)

考慮NHC/OD測量值帶有誤差σ，則有

(8)

對式(8)進行擾動分析，即將機械編排的速度與理論真實速度相減，有

(9)

將式(7)代入式(9)，并整理得

(10)

式(10)即NHC/OD/SINS測量方程，可表示為

Zk=HkXk+σ

(11)

其觀測噪聲表示為

(12)

式中，σod為里程計測速噪聲項；σNHC,x、σNHC,z為NHC噪聲項，其值的大小反映了速度約束的松緊。

1.3 誤差校正

濾波更新完成以后，立即進行全閉環(huán)校正操作，分為3步。

(1) 用公共誤差狀態(tài)修正機械編排結果，輸出最終的導航參數，并且用該導航參數初始化下一歷元機械編排中的位置、速度和姿態(tài)

(13)

式中，下標“mech”表示慣導機械編排結果。

(2) 用IMU誤差狀態(tài)修正下一歷元的慣導輸出

(14)

式中，Δvk+1、Δθk+1分別表示下一歷元的加速度計與陀螺測量值；上標“^”表示該量進行了修正。

(3) 將所有誤差狀態(tài)置零

Xk=0

(15)

1.4 車輛轉彎運動分析

對車輛轉彎時進行運動學分析，如圖1所示，A、B兩點分別為前軸中心與后軸中心。根據阿克曼理論轉向特性，車輛在轉彎時所有車輪應繞著同一點O′做瞬時圓周轉動，且點O′位于兩后輪軸線延長線上，此時根據圓周運動原理，車輛后軸速度的方向應垂直于O′B(即沿vB1方向)。然而，阿克曼理論轉向特性,是以汽車車輛前輪定位角都等于零、行走系統(tǒng)為剛性、車輛行駛過程中無側向力為假設條件的，實際車輛的垂直載荷，輪胎結構、材料和充氣壓力，路面的形狀及潮濕程度，側向風，車輪外傾角，轉彎半徑和轉彎速度等均會影響輪胎側偏角，導致理論上瞬時轉動點(O′)與實際瞬時轉動點(O)無法交匯于一點[22-24]，即實際車輛轉彎時是以O點為圓心做瞬時圓周轉動。此時B點處速度vB的方向如圖1所示，垂直于OB。對vB進行速度分解可得vB1與vB2，vB2為由于車輛轉彎運動時產生橫向速度分量。如上所述，其方向與車輛轉彎幅值大小等因素相關。

圖1 車輛轉彎Fig.1 Vehicle turning

由以上分析可知，由于車輛輪胎的側偏角等受轉彎速度、轉彎幅值等因素的影響，車輛在轉彎時會產生一定的橫向速度噪聲，因此在設置NHC噪聲時，不能設置為固定值，應當考慮車輛的運動狀態(tài)進行自適應調整。

2 NHC噪聲自適應模型

為研究NHC橫向速度噪聲與車輛運動狀態(tài)的關系，選取了實際路測數據進行分析：數據于2020年8月27日下午在武漢市卓刀泉立交采集，軌跡如圖2所示，標記點為主要的轉彎的路段。試驗使用加拿大NovAtel公司的PwrPak7測量型GNSS接收機與SPAN-FSAS高精度組合導航系統(tǒng)，通過Inertial Explorer 8.70軟件后處理計算得到b系下車輛的行駛速度(vy)、右向速度(vx)及歷元間航向角差(da)。

圖2 路測軌跡(卓刀泉立交)Fig.2 Road test track (Zhuodaoquan Interchange)

圖2中軌跡部分為選取的驗證路段，標記點為主要的轉彎路段，圖3為對應的橫向速度(vx)與車輛行駛速度(vy)、歷元間航向角差(da)關系。根據NHC假設，車輛橫向的速度vx應為0，而由圖3可知，vx不為0，即存在橫向速度偏差，車輛實際運動時并不能完全滿足NHC假設：圖3(a)為vx與vy的關系圖，可以發(fā)現(xiàn)兩者在數值上有明顯的正相關性，在車速vy增大/減小時，vx數值也相應地增大/減小(圖3(a)中標記部分尤其明顯)，即NHC橫向速度噪聲受車輛前進速度影響；圖3(b)為vx與da的關系圖，標記處為圖2對應標記的主要轉彎路段，可以發(fā)現(xiàn)在轉彎路段，vx數值的增大/減少主要由da的增大/減小引起，其中b處進行大幅度轉彎運動時，vx變化尤其明顯，即NHC橫向速度噪聲受車輛轉彎影響。綜上可知，NHC橫向速度噪聲項σNHC,x受車輛速度與車輛轉彎的共同影響。

圖3 橫向速度與車輛行駛速度、歷元間航向角差Fig.3 Lateral speed, vehicle speed, heading angle difference between epochs

本文根據文獻[18]，提出了基于車輛運動狀態(tài)的NHC噪聲自適應模型。如前所述，σNHC,x受車輛速度與車輛轉彎的共同影響，車輛的速度可直接從車輛CAN總線獲取(vod)，而車輛轉彎的判斷條件可表示為[18]

(16)

式中，|δY|為航向角變化值；λi(i=1,2,3)為角度變化率閾值；vi(i=1,2,3)為速度閾值；詳細可參考文獻[18]。

根據式(16)可判斷車輛是否轉彎，當車輛存在轉彎運動時，根據車輛行駛速度和轉彎角度的大小，動態(tài)調整NHC噪聲項σNHC,x

σNHC,x=vod·|δY|·SY

(17)

式中，SY為設定的自適應比例因子。

為減少過松或者過緊的噪聲約束導致濾波結果異常，在進行濾波更新時，首先根據式(17)計算噪聲項σNHC,x，再進行自適應調整

(18)

3 實測分析

本文采用兩組實測車載數據進行驗證與分析，采用的MEMS IMU為Bosch Sensortec公司生產的IMU BMI160，其部分參數見表1。里程計數據直接從車輛CAN總線中獲取，參考值為加拿大NovAtel公司的SPAN-CPT組合導航結果。

表1 BMI160參數Tab.1 Parameters of BMI160

3.1 測試結果1

車載數據1于2020年9月10日在武漢市洪山區(qū)采集獲得，IMU數據采樣率為100 Hz，GNSS數據采樣率為1 Hz，從車輛CAN總線中獲得的里程計數據采樣率為20 Hz。為驗證本文算法，選取了車輛行駛110 s總長約1.3 km的轉彎路段模擬GNSS中斷。軌跡圖和GNSS中斷區(qū)間如圖4所示，圖中箭頭指示車輛的行駛方向。可以發(fā)現(xiàn)，車輛在GNSS中斷后共經過了兩段360°轉彎路段，能夠充分驗證車輛轉向時對NHC約束性能的影響。

圖4 實測軌跡1與模擬GNSS中斷區(qū)間(白線軌跡)Fig.4 Measured trajectory 1 and simulated GNSS interruption interval (White line trajectory)

在GNSS中斷期間，根據式(10)，使用里程計速度與NHC結合進行測量更新。本文設計了不同數據處理方案進行對比見表2，其中方案3、方案4用于說明NHC對于OD/SINS組合系統(tǒng)的有效性，方案1、方案2用于驗證本文所構建算法的有效性。由于σNHC,x主要影響水平位置精度，因此，本文主要從水平軌跡、水平誤差序列及其統(tǒng)計值對比分析不同方案的定位性能。

表2 數據處理方案Tab.2 Data processing strategy

圖5為使用不同方案得到的GNSS中斷期間的水平軌跡對比，可以發(fā)現(xiàn)在GNSS信號中斷后，車輛隨即進入轉彎路段。可以發(fā)現(xiàn)采用NHC輔助OD/SINS導航的方案1、方案2的軌跡與實際運動軌跡基本吻合；采用純SINS解算的方案3在GNSS信號中斷后誤差迅速發(fā)散，軌跡已經完全偏離實際軌跡；僅采用OD輔助SINS導航的方案4在GNSS信號中斷后在第一個轉彎路段整體維持在車道線附近，在車輛駛出第一個轉彎路段進入直線后，軌跡出現(xiàn)了明顯發(fā)散的現(xiàn)象。由此可知，GNSS中斷后誤差的發(fā)散是由于SINS帶入的。相比于單OD/SINS組合導航，使用NHC引入了右向與垂向的速度觀測值，從而提高了卡爾曼濾波估計器的冗余度，有利于提升系統(tǒng)的抗差性和穩(wěn)定性。由此可見，使用NHC輔助可以顯著提升OD/SINS組合系統(tǒng)的可用性(由表3的統(tǒng)計結果可以進一步佐證)。因此，下文主要針對本文所提出的自適應噪聲方案與傳統(tǒng)固定噪聲的方案進行對比分析。

圖5 不同方案的水平軌跡——數據1Fig.5 Horizontal trajectory of different strategies—data 1

圖6為GNSS中斷期間分別使用方案1、方案2得到的水平軌跡對比。可以發(fā)現(xiàn)，在第1個轉彎路段中，兩種方案解算的軌跡都與參考軌跡基本重合，整體無明顯發(fā)散的現(xiàn)象。在車輛駛出第一個轉彎路段進入直線時，使用方案1，車輛未能準確進入直線路段，軌跡開始出現(xiàn)發(fā)散現(xiàn)象，明顯偏離車道；而方案2軌跡雖然也有一定的發(fā)散，但整體還是可以維持在車道附近，沒有明顯發(fā)散。在第2個彎道部分，方案1軌跡出現(xiàn)了明顯偏移的現(xiàn)象，而方案2軌跡與參考軌跡符合程度較好。在車輛駛出彎道進入直線路段后，方案1、方案2都可以維持直線行駛，但方案1由于之前的誤差累積，軌跡迅速發(fā)散，而方案2軌跡發(fā)散程度明顯更小，能與實際軌跡維持較高的符合度。

圖6 不同方案的水平軌跡(方案1與方案2)——數據1Fig.6 Horizontal trajectory of different strategies(strategy 1 & strategy 2)—data 1

圖7為水平誤差序列圖，可以發(fā)現(xiàn)在前35 s部分(第1個轉彎路段)， 由于使用與直線行駛時一樣的固定NHC噪聲項， 組合導航濾波結果的誤差累積加快，方案1的誤差曲線已經出現(xiàn)波動，而方案2由于可以正確探測到車輛的轉彎，對NHC噪聲項進行了自適應調整，誤差曲線基本維持在0附近。在第60 s左右時(第2個轉彎路段中段)，方案1的水平誤差已經達到了30 m，而方案2誤差在10 m左右。第80 s(車輛駛出彎道進入直線)后，由于誤差累積，方案1的右向誤差相比于方案2迅速發(fā)散，進而導致水平誤差的迅速發(fā)散。

圖7 誤差序列圖——數據1Fig.7 Error sequence diagram—data 1

圖8為兩種方案給出的NHC噪聲對比序列，方案1使用固定的σNHC,x值(設置為0.01)，方案2根據式(18)自適應調整σNHC,x值，可以發(fā)現(xiàn)方案1在轉彎與直線路段都使用了固定的σNHC,x值，在轉彎路段不能完全符合車輛實際運動狀態(tài)，因此最終定位誤差較大。而由方案2給出的σNHC,x在實時調整，σNHC,x在轉彎路段明顯增大，更加符合車輛實際運動狀態(tài)，因此最終定位誤差相對較小。由此可見，本文構建的基于車輛運動狀態(tài)的NHC噪聲自適應模型，相比于固定噪聲的NHC模型，通過自適應調整噪聲，限制了NHC方法在轉彎狀態(tài)下的對觀測結果的影響權重。

圖8 不同方案NHC噪聲對比序列圖——數據1Fig.8 NHC noise comparison sequence diagram of different strategies—data 1

表3給出了不同數據處理方案得到的最大位置誤差結果。在GNSS信號中斷的110 s期間，采用純SINS解算的方案3誤差最大，使用OD/SINS組合的方案4相比于方案3的誤差明顯減小，但明顯大于使用了NHC的方案1、方案2，進一步說明了使用NHC輔助可以顯著提升OD/SINS組合系統(tǒng)的可用性。與方案1使用固定的NHC噪聲項相比，方案2使用自適應的NHC噪聲項的最大右向誤差從84.60 m減小到26.66 m，最大前向誤差從27.52 m減小到11.08 m，最大水平誤差從87.40 m減小到了27.61 m，結果表明本文構建的NHC噪聲自適應算法是有效的。

表3 不同方案統(tǒng)計指標對比——數據1Tab.3 Comparison of statistical indicators of different schemes—data 1 m

3.2 測試結果2

為進一步驗證本文所構建自適應噪聲方案相比于固定噪聲方案的有效性，選取一段實測直線路段車載數據進行分析。車載數據2于2020年9月10日在武漢市洪山區(qū)采集獲得，所采用的設備與數據記錄、處理方案均與車載數據1一致。

圖9為軌跡圖和GNSS中斷區(qū)間示意圖，圖中箭頭指示車輛的行駛方向。可見GNSS中斷路段為直線路段，GNSS中斷時長為74 s，路線長度約為700 m，車輛進行變速運動。

圖9 實測軌跡2與模擬GNSS中斷區(qū)間Fig.9 Measured trajectory 2 and simulated GNSS interruption interval

圖10為兩種方案給出的NHC噪聲對比序列，方案1使用固定的σNHC,x值(設置為0.01)，方案2根據式(18)自適應調整σNHC,x值，可見在變速運動下，方案2會自適應調整σNHC,x值，因此兩種方案的結果也會存在差別。

圖10 不同方案NHC噪聲對比序列圖——數據2Fig.10 NHC noise comparison sequence diagram of different strategies—data 2

圖11為水平軌跡對比圖，表4為不同數據處理方案得到的最大位置誤差結果。由結果可見，在GNSS中斷74 s的直線路段中，采用本文所構建的自適應噪聲方案2，最大水平誤差不超過10 m，而采用固定噪聲的方案1時，最大水平誤差超過了60 m，結果表明本文構建的NHC噪聲自適應算法是有效的。

圖11 不同方案的水平軌跡(方案1與方案2)——數據2Fig.11 Horizontal trajectory of different strategies(strategy 1& strategy 2)—data 2

表4 不同方案統(tǒng)計指標對比——數據2Tab.4 Comparison of statistical indicators of different schemes—data 2 m

綜合以上分析可知，根據車輛的運動狀態(tài)，使用NHC噪聲自適應的方法能更加有效地利用NHC信息，可有效地提高車輛組合導航定位的精度。所選場景的實測結果表明，相比于固定噪聲的NHC/OD/SINS組合算法，采用自適應噪聲的NHC/OD/SINS組合算法，在110 s的GNSS信號中斷、車輛連續(xù)轉彎的情況下，最大水平位置誤差減小了68.4%；在74 s的GNSS信號中斷、車輛直線行駛的情況下，最大水平位置誤差減小了87.3%。

4 結 語

在地面車載組合導航中，當GNSS信號中斷時采用NHC/OD/SINS組合可以抑制組合導航結果的誤差發(fā)散。本文構建的基于車輛運動狀態(tài)的NHC噪聲自適應方法，通過IMU測量值與車輛CAN總線提供的速度測量值判斷車輛的運動狀態(tài)。所選場景的實測結果表明，該自適應方法能夠較好地抑制GNSS中斷期間組合導航系統(tǒng)的誤差發(fā)散，采用自適應噪聲的NHC/OD/SINS組合算法的最大水平位置相比于固定噪聲的NHC/OD/SINS組合算法，在110 s的GNSS中斷的連續(xù)轉彎路段，最大水平位置誤差減小了68.4%；在74 s的GNSS中斷的直線路段，最大水平位置誤差減小了87.4%。