一種面向即插即用車載GNSS/INS/ODO系統的時空在線標定方法

岑益挺，賴際舟，呂 品，白師宇，李金鑫

(1. 南京航空航天大學自動化學院，南京 211106;2. 中國兵器裝備集團兵器裝備研究所，北京 102202)

0 引言

全球衛星導航系統(Global Navigation Satellite System，GNSS)與慣性導航系統(Inertial Navigation System，INS)在陸地車輛組合導航中可以提供位置、速度和姿態等完備導航信息，在民用和軍用領域得到了廣泛的研究與應用。在組合導航系統中，GNSS通過接收衛星信號解算得到準確的三維位置和速度信息，但是由于信號容易受隧道峽谷、高樓樹木等遮擋以及電磁干擾，導致GNSS無法有效地進行定位和測速；INS憑借全自主的優勢，能夠在短時間內提供精確的導航信息，但是其誤差會隨時間快速累積，需要GNSS在車輛運動過程中進行抑制。而當GNSS不可用時，需要再引入輔助信息，修正INS單獨導航時的誤差，如零速修正、地圖匹配、激光雷達、相機、輪速里程計等。

輪速里程計(Odometer，ODO)作為另一個自主且經濟、方便搭載的車載傳感器，可以提供車輛的前向速度，與車輛非完整約束(Non-Holonomic Constraints，NHCs)結合，可以有效提高GNSS失效時INS的導航精度，而精度提升的關鍵在于慣性測量單元(Inertial Measurement Unit，IMU)與ODO之間參數的準確標定。通常兩者之間的桿臂可以通過手動測量的方式進行補償，但操作較為繁瑣且測量精度較低，另外安裝偏差角難以測量，因此研究在GNSS有效時利用其輔助IMU/ODO參數在線估計，對提升GNSS失效時INS/ODO組合導航精度具有積極意義。文獻[11]和文獻[12]分別通過卡爾曼濾波與非線性優化實現了GNSS輔助IMU/ODO的在線標定，過程中考慮了GNSS天線與IMU之間的桿臂，但是均未考慮GNSS時延與ODO時延的影響。在系統集成化較高的車載導航應用中，通常采用硬件同步的方式統一各傳感器的時鐘，但是對于即插即用的系統而言，每當傳感器需要更新或者改變時，硬件同步會帶來巨大的成本。目前，許多研究者已經對傳感器時延以及軟件時間同步方法開展了不少研究。文獻[13]中指出，GNSS接收機的信號處理過程給GNSS輸出帶來的延時達到百毫秒級別，對GNSS實時應用帶來的影響不可忽視。文獻[14]研究了GNSS/INS系統的時延估計與補償算法。但是針對GNSS/INS/ODO系統，目前尚未有文獻同時考慮GNSS與ODO的時延，且兩種時延對GNSS輔助IMU/ODO參數在線標定帶來的影響也尚未有文獻進行研究。

針對上述問題，本文提出了一種面向即插即用車載GNSS/INS/ODO系統的時空在線標定方法。該方法分析了GNSS時延與ODO時延在車輛運動過程中對IMU/ODO在線標定的影響，在傳統GNSS輔助IMU/ODO參數估計模型的基礎上，增廣了GNSS與ODO的時延誤差，并且構建了完整的GNSS與ODO觀測模型，采用卡爾曼濾波器對GNSS時延、ODO時延以及IMU與ODO之間的參數進行估計。實際測試結果表明，該方法可以有效提高IMU/ODO在線標定的精度。

1 坐標系定義

本文采用的坐標系定義如圖1所示。

圖1 坐標系定義

1)車體坐標系(系)：以車體后軸中心作為坐標原點，軸沿車體向右，軸沿車體向前，軸與軸、軸形成右手坐標系；

2)IMU坐標系(系)：以IMU的中心作為坐標原點，與系同為右前上坐標系；

3)導航坐標系(系)：為東北天(ENU)坐標系。

(1)

其中，三角函數sin和cos分別用s和c表示。

由于安裝偏差角通常為小角度，則式(1)可以近似為

(2)

則系下速度與系下速度之間的關系為

(3)

根據式(3)可知，系下的速度在系下的分量與安裝偏差角無關，在標定時可以不作考慮。

2 改進GNSS輔助IMU/ODO參數估計濾波器設計

2.1 GNSS時延對IMU/ODO標定的影響分析

對于GNSS/INS組合導航系統，由于GNSS接收機與IMU的采樣時間不一致，且信號處理時間也不一致，導致兩者存在時延且時延相對穩定。假設GNSS位置觀測的時延為一個固定值，則時刻GNSS位置觀測的實際值可以建模為

(4)

由于時刻GNSS的實際位置觀測為-時刻的值，因此在用INS預測GNSS位置時，需要預測-時刻的值

≈(-)+δ(-)-

=(-)+δ(-)-

(5)

將GNSS位置的預測量與觀測量作差，得到位置量測方程為

=δ(-)-(-)δ+

(6)

根據文獻[15]對IMU/ODO參數標定的可觀測性分析可知，輪速里程計標度因數誤差以及IMU與ODO之間安裝偏差角的估計需要車輛的加減速機動激勵，而IMU與ODO之間的桿臂估計需要至少繞2個軸向的角速度機動激勵。而在上述機動下，GNSS時延對IMU/ODO標定的影響具體如下：

1)根據式(6)可知，GNSS時延將與車輛速度耦合引起GNSS位置觀測誤差，從而降低濾波器對INS位置誤差的估計精度；

2)在IMU/ODO標定所需的加減速或角速度機動下，速度矢量的大小或方向發生變化，使得GNSS時延帶來的INS位置誤差同步改變,從而引起INS速度誤差，影響IMU/ODO的參數標定。

2.2 ODO時延對IMU/ODO標定的影響分析

同樣假設ODO與IMU之間的時延為一個固定值,則時刻ODO速度觀測的實際值可以建模為

=(1+δ)(--δ)+

=(1+δ)((-)-(-

)δ)+

≈(1+δ)(-)-(-

)δ+

(7)

其中，δ為輪速里程計的標度因數誤差；δ為ODO與IMU之間殘余待估計的時延值；(-)為-時刻車輛的前向加速度；為速度觀測噪聲。

根據式(7)可知，ODO時延與車輛前向加速度耦合引起輪速里程計的速度觀測誤差，從而直接影響其標度因數誤差的估計。

2.3 基于GNSS與ODO時延估計的IMU/ODO濾波模型

為降低GNSS與ODO時延對IMU與ODO之間參數估計的影響，將GNSS與ODO時延增廣到狀態量中。基于GNSS與ODO時延估計的IMU/ODO參數估計濾波結構如圖2所示。

圖2 基于GNSS與ODO時延估計的IMU/ODO參數估計濾波結構

此時，狀態方程為

(8)

其中

=[(),(δ),(δ),(),()，

式(8)中，和為INS的狀態轉移矩陣和系統噪聲矩陣，具體形式可參見文獻[16];為INS的系統噪聲，即陀螺和加速度計的白噪聲。

輪速里程計與非完整約束一起構成系下的三維速度觀測

(9)

根據式(9)可以得到系下的速度觀測

≈(-)+[(-)×](-)+

)(-)δ+

(10)

而根據INS得到的時刻的速度為

(11)

將式(10)與式(11)作差，得到速度量測方程為

=-[(-)×](-)+δ(-)-

)(-)δ

=(-)-

(12)

其中，為速度量測矩陣。

根據式(6)可得位置量測方程為

=(-)-

(13)

其中，為位置量測矩陣。

3 試驗驗證

本文開展了實際試驗，以驗證所提出的算法。試驗所用IMU的性能參數如表1所示，輪速里程計的測速精度為0.02m/s，GNSS位置精度為0.05m。IMU、輪速里程計以及GNSS天線的安裝如圖3所示，其中GNSS后天線與IMU之間桿臂的粗略測量值為[0.2,-1.15,0.5]m。試驗軌跡如圖4所示，試驗總時長35min，其中試驗過程前20min用于GNSS/INS/ODO系統的參數在線標定，后15min用于驗證基于GNSS與ODO時延估計的IMU/ODO參數在線標定方法的準確性，后15min行駛路程約1380m。由于IMU與ODO之間參數的真值未知，本文采用如下兩種方式間接驗證：

圖3 車輛試驗平臺

圖4 試驗軌跡

表1 IMU性能參數

1)方式1：后15min進行GNSS/INS組合導航，以IMU/ODO在線標定的結果作為已知量，檢驗車輛非完整約束的滿足情況。

2)方式2：后15min以IMU/ODO在線標定的結果作為已知量，進行INS/ODO組合導航，以差分GNSS信息為位置基準，檢驗INS/ODO組合導航的定位精度。

前20min在線標定試驗過程中,位置、速度、姿態如圖5所示。

(a)

為了具體對比GNSS時延與ODO時延對IMU/ODO參數在線標定的影響，將不考慮GNSS時延與ODO時延(記為M1)、僅考慮GNSS時延(記為M2)以及本文同時考慮GNSS時延與ODO時延(記為M3)的參數標定方法進行對比，標定結果如表2所示，圖6～圖8所示為對應的標定曲線。圖9所示為本文方法的GNSS時延與ODO時延估計曲線。圖6～圖8中，除δ之外，M2與M3的其余參數標定曲線幾乎重合。

表2 參數標定結果對比

圖6 ODO標度因數誤差、IMU與ODO之間的安裝偏差角估計對比

圖7 IMU與ODO之間的桿臂估計對比

圖8 GNSS天線與IMU之間的桿臂估計對比

圖9 GNSS時延與ODO時延估計

根據圖6～圖9以及表2數據可以看出：

3)GNSS時延對IMU與ODO之間的標度因數誤差δ以及安裝偏差角、的標定影響較大。從圖6可以看出，在470s、1120s車輛加減速時，δ出現明顯的跳變，在130s時由于車輛的減速再加速，導致M1與M2對、的估計結果出現較大差異，這都是因為試驗中加減速機動較為明顯，GNSS時延引起的INS速度誤差較大所致。

4)本文試驗中使用的輪速里程計存在15ms左右的時延，其主要在470s、1120s等車輛加減速時引起輪速里程計標度因數誤差估計的跳變，不過由于ODO時延量較小，因此跳變幅度較小。

圖10 b系下的速度

圖11 經M1標定結果補償后的v系下速度

圖12 經M3標定結果補償后的v系下速度

為了進一步體現使用本文方法對IMU/ODO標定補償后INS/ODO組合導航系統在GNSS失效時的定位性能優勢，開展了前文方式(2)所述的驗證工作。圖13所示為經過M1、M2與M3三種方法標定結果補償后的INS/ODO組合導航系統水平位置誤差與高度誤差對比圖，最大位置誤差對比如表3所示。

圖13 INS/ODO組合導航系統水平位置誤差與高度誤差對比

表3 INS/ODO組合導航系統最大位置誤差對比

4 結論

本文提出了一種面向即插即用車載GNSS/INS/ODO系統的時空在線標定方法。該方法在傳統GNSS輔助IMU/ODO參數估計模型的基礎上,增廣了GNSS與ODO的時延誤差，分析了兩種時延對IMU/ODO參數在線標定的影響，推導并構建了完整的GNSS與ODO觀測模型，采用卡爾曼濾波器對GNSS時延、ODO時延以及IMU與ODO之間的參數進行估計。實際測試結果表明，該方法可以有效提高IMU/ODO在線標定的精度。