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

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

0 引言

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

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

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

1 坐標(biāo)系定義

本文采用的坐標(biāo)系定義如圖1所示。

圖1 坐標(biāo)系定義

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

2)IMU坐標(biāo)系(系)：以IMU的中心作為坐標(biāo)原點(diǎn)，與系同為右前上坐標(biāo)系；

3)導(dǎo)航坐標(biāo)系(系)：為東北天(ENU)坐標(biāo)系。

(1)

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

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

(2)

則系下速度與系下速度之間的關(guān)系為

(3)

根據(jù)式(3)可知，系下的速度在系下的分量與安裝偏差角無關(guān)，在標(biāo)定時(shí)可以不作考慮。

2 改進(jìn)GNSS輔助IMU/ODO參數(shù)估計(jì)濾波器設(shè)計(jì)

2.1 GNSS時(shí)延對IMU/ODO標(biāo)定的影響分析

對于GNSS/INS組合導(dǎo)航系統(tǒng)，由于GNSS接收機(jī)與IMU的采樣時(shí)間不一致，且信號處理時(shí)間也不一致，導(dǎo)致兩者存在時(shí)延且時(shí)延相對穩(wěn)定。假設(shè)GNSS位置觀測的時(shí)延為一個(gè)固定值，則時(shí)刻GNSS位置觀測的實(shí)際值可以建模為

(4)

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

≈(-)+δ(-)-

=(-)+δ(-)-

(5)

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

=δ(-)-(-)δ+

(6)

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

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

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

2.2 ODO時(shí)延對IMU/ODO標(biāo)定的影響分析

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

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

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

)δ)+

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

)δ+

(7)

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

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

2.3 基于GNSS與ODO時(shí)延估計(jì)的IMU/ODO濾波模型

為降低GNSS與ODO時(shí)延對IMU與ODO之間參數(shù)估計(jì)的影響，將GNSS與ODO時(shí)延增廣到狀態(tài)量中。基于GNSS與ODO時(shí)延估計(jì)的IMU/ODO參數(shù)估計(jì)濾波結(jié)構(gòu)如圖2所示。

圖2 基于GNSS與ODO時(shí)延估計(jì)的IMU/ODO參數(shù)估計(jì)濾波結(jié)構(gòu)

此時(shí)，狀態(tài)方程為

(8)

其中

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

式(8)中，和為INS的狀態(tài)轉(zhuǎn)移矩陣和系統(tǒng)噪聲矩陣，具體形式可參見文獻(xiàn)[16];為INS的系統(tǒng)噪聲，即陀螺和加速度計(jì)的白噪聲。

輪速里程計(jì)與非完整約束一起構(gòu)成系下的三維速度觀測

(9)

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

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

)(-)δ+

(10)

而根據(jù)INS得到的時(shí)刻的速度為

(11)

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

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

)(-)δ

=(-)-

(12)

其中，為速度量測矩陣。

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

=(-)-

(13)

其中，為位置量測矩陣。

3 試驗(yàn)驗(yàn)證

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

圖3 車輛試驗(yàn)平臺

圖4 試驗(yàn)軌跡

表1 IMU性能參數(shù)

1)方式1：后15min進(jìn)行GNSS/INS組合導(dǎo)航，以IMU/ODO在線標(biāo)定的結(jié)果作為已知量，檢驗(yàn)車輛非完整約束的滿足情況。

2)方式2：后15min以IMU/ODO在線標(biāo)定的結(jié)果作為已知量，進(jìn)行INS/ODO組合導(dǎo)航，以差分GNSS信息為位置基準(zhǔn)，檢驗(yàn)INS/ODO組合導(dǎo)航的定位精度。

前20min在線標(biāo)定試驗(yàn)過程中,位置、速度、姿態(tài)如圖5所示。

(a)

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

表2 參數(shù)標(biāo)定結(jié)果對比

圖6 ODO標(biāo)度因數(shù)誤差、IMU與ODO之間的安裝偏差角估計(jì)對比

圖7 IMU與ODO之間的桿臂估計(jì)對比

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

圖9 GNSS時(shí)延與ODO時(shí)延估計(jì)

根據(jù)圖6～圖9以及表2數(shù)據(jù)可以看出：

3)GNSS時(shí)延對IMU與ODO之間的標(biāo)度因數(shù)誤差δ以及安裝偏差角、的標(biāo)定影響較大。從圖6可以看出，在470s、1120s車輛加減速時(shí)，δ出現(xiàn)明顯的跳變，在130s時(shí)由于車輛的減速再加速，導(dǎo)致M1與M2對、的估計(jì)結(jié)果出現(xiàn)較大差異，這都是因?yàn)樵囼?yàn)中加減速機(jī)動較為明顯，GNSS時(shí)延引起的INS速度誤差較大所致。

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

圖10 b系下的速度

圖11 經(jīng)M1標(biāo)定結(jié)果補(bǔ)償后的v系下速度

圖12 經(jīng)M3標(biāo)定結(jié)果補(bǔ)償后的v系下速度

為了進(jìn)一步體現(xiàn)使用本文方法對IMU/ODO標(biāo)定補(bǔ)償后INS/ODO組合導(dǎo)航系統(tǒng)在GNSS失效時(shí)的定位性能優(yōu)勢，開展了前文方式(2)所述的驗(yàn)證工作。圖13所示為經(jīng)過M1、M2與M3三種方法標(biāo)定結(jié)果補(bǔ)償后的INS/ODO組合導(dǎo)航系統(tǒng)水平位置誤差與高度誤差對比圖，最大位置誤差對比如表3所示。

圖13 INS/ODO組合導(dǎo)航系統(tǒng)水平位置誤差與高度誤差對比

表3 INS/ODO組合導(dǎo)航系統(tǒng)最大位置誤差對比

4 結(jié)論

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