黃湘遠(yuǎn)，湯霞清，武 萌

(裝甲兵工程學(xué)院控制工程系，北京 100072)

2016160

基于CKF和R-T-S平滑的車載SINS/OD動(dòng)基座對(duì)準(zhǔn)方案*

黃湘遠(yuǎn)，湯霞清，武 萌

(裝甲兵工程學(xué)院控制工程系，北京 100072)

為提高車載捷聯(lián)慣導(dǎo)/里程計(jì)組合系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)精度、速度和位置導(dǎo)航精度，提出了“容積卡爾曼濾波(CKF)非線性對(duì)準(zhǔn)+非線性R-T-S平滑+再次Kalman濾波線性對(duì)準(zhǔn)”的方案，即首先應(yīng)用CKF進(jìn)行非線性對(duì)準(zhǔn)，對(duì)準(zhǔn)結(jié)果方位對(duì)準(zhǔn)精度一般、位置誤差較大；接著利用R-T-S平滑獲得初始時(shí)刻高精度對(duì)準(zhǔn)結(jié)果，此時(shí)非線性誤差模型退化為線性模型；最后再進(jìn)行Kalman濾波線性對(duì)準(zhǔn)而獲得高精度方位對(duì)準(zhǔn)精度和位置導(dǎo)航。該方案無需長時(shí)間的粗對(duì)準(zhǔn)，最終能獲得高精度的結(jié)果。實(shí)車試驗(yàn)驗(yàn)證了該方案的有效性。

捷聯(lián)慣導(dǎo)/里程計(jì)組合系統(tǒng)；容積卡爾曼濾波；非線性R-T-S平滑；卡爾曼濾波

前言

基于捷聯(lián)慣導(dǎo)(strapdown inertial navigation system, SINS)/里程計(jì)(odometer, OD)的組合導(dǎo)航系統(tǒng)已廣泛應(yīng)用于陸用導(dǎo)航[1-2]。系統(tǒng)正常工作之前必須進(jìn)行初始對(duì)準(zhǔn)，對(duì)準(zhǔn)分為粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)兩個(gè)過程。

由于里程計(jì)不能提供絕對(duì)的速度信息和位置信息，導(dǎo)致動(dòng)基座對(duì)準(zhǔn)中無法進(jìn)行高精度定位，對(duì)準(zhǔn)結(jié)束后載體位置誤差可能較大。文獻(xiàn)[3]中提出一種慣性系對(duì)準(zhǔn)方案，忽略對(duì)準(zhǔn)過程中的位置變化，長時(shí)間后位置誤差較大。文獻(xiàn)[4]和文獻(xiàn)[5]中以慣性系對(duì)準(zhǔn)方案完成粗對(duì)準(zhǔn)，基于小角度誤差模型利用線性濾波完成精對(duì)準(zhǔn)，獲得較高精度。上述文獻(xiàn)認(rèn)為60s內(nèi)能夠獲得小失準(zhǔn)角對(duì)準(zhǔn)結(jié)果，均沒有考慮粗對(duì)準(zhǔn)過程中的位置誤差。在履帶式裝甲車輛這一特殊載體中，經(jīng)過多次實(shí)驗(yàn)表明，運(yùn)動(dòng)過程中至少需5min左右才能獲得小失準(zhǔn)角結(jié)果，此時(shí)該方案帶來較大的位置誤差。CKF是一種優(yōu)秀的非線性濾波器[6-7]，在多個(gè)領(lǐng)域獲得了大量的應(yīng)用。本文中推導(dǎo)了采用大失準(zhǔn)角SINS/OD非線性誤差模型，利用CKF進(jìn)行非線性對(duì)準(zhǔn)，對(duì)準(zhǔn)結(jié)束后方位精度較高，位置精度一般，但高于慣性系對(duì)準(zhǔn)方案的位置精度。

為了保證對(duì)準(zhǔn)結(jié)束后位置精度，文獻(xiàn)[8]中設(shè)計(jì)了逆向?qū)Ш剿惴ǎM(jìn)行再次導(dǎo)航；為了加快對(duì)準(zhǔn)速度，文獻(xiàn)[9]中引入了循環(huán)解算思路，文獻(xiàn)[10]中提出了嚴(yán)格逆向過程的羅經(jīng)回溯對(duì)準(zhǔn)方法；文獻(xiàn)[11]中提出基于狀態(tài)估計(jì)的卡爾曼濾波逆向?qū)Ш剿惴āＤ嫦蛩悸返膶?dǎo)航處理或狀態(tài)估計(jì)可解決對(duì)準(zhǔn)速度和位置導(dǎo)航的問題。非線性對(duì)準(zhǔn)的逆向處理可通過非線性R-T-S平滑來實(shí)現(xiàn)[12-13]，無需進(jìn)行逆向?qū)Ш浇馑恪?/p>

本文中利用CKF進(jìn)行非線性對(duì)準(zhǔn)，對(duì)準(zhǔn)結(jié)束時(shí)利用非線性R-T-S平滑迭代至初始時(shí)刻，得到載體初始姿態(tài)。利用存儲(chǔ)數(shù)據(jù)再次進(jìn)行導(dǎo)航解算，利用Kalman濾波進(jìn)行線性對(duì)準(zhǔn)至結(jié)束時(shí)刻。最后進(jìn)行了實(shí)車實(shí)驗(yàn)，結(jié)果表明經(jīng)過再次對(duì)準(zhǔn)之后，方位精度和位置精度都得到了較大的提高，該方案同時(shí)實(shí)現(xiàn)了高精度對(duì)準(zhǔn)和位置導(dǎo)航。

1 SINS/OD非線性對(duì)準(zhǔn)模型

1.1 SINS非線性誤差

記地心慣性坐標(biāo)系為i系；地球系為e系；導(dǎo)航系n系為東北天(o-ENU)坐標(biāo)系；計(jì)算平臺(tái)系為p系；載體系為b系；里程計(jì)坐標(biāo)系m系為右前上坐標(biāo)系。

φ=[φE,φN,φU]T

n系下載體速度vn=[vE,vN,vU]T

速度誤差δvn=[δvE,δvN,δvU]T

載體緯度L、經(jīng)度λ和高度h的位置誤差為

δp=[δL,δλ,δh]T

大失準(zhǔn)角下，基于歐拉角誤差的非線性姿態(tài)、速度和位置誤差的微分方程[14]為

(1)

該模型要求緯度誤差δL為小量。非線性對(duì)準(zhǔn)過程中，為了保證δL為小量，可通過位置、速度間歇性閉環(huán)反饋來抑制速度和位置誤差的快速發(fā)散。

1.2 OD誤差

(2)

(3)

式中ωD為里程計(jì)隨機(jī)量測(cè)噪聲。

當(dāng)失準(zhǔn)角φ=[φE,φN,φU]T為大角度時(shí)，里程計(jì)速度在計(jì)算平臺(tái)系p系的實(shí)際輸出為

(4)

1.3 SINS/OD動(dòng)基座非線性對(duì)準(zhǔn)系統(tǒng)

對(duì)準(zhǔn)過程中，需對(duì)陀螺、加速度計(jì)和里程計(jì)誤差進(jìn)行估計(jì)和補(bǔ)償。由于對(duì)準(zhǔn)時(shí)間較短，可將陀螺零偏εb、加速度計(jì)零偏b和里程計(jì)誤差ξD視作隨機(jī)常值，即

(5)

非線性對(duì)準(zhǔn)系統(tǒng)的狀態(tài)由姿態(tài)誤差φ、速度誤差δvn、位置誤差δp、陀螺零漂εb、加速度計(jì)零偏b和里程計(jì)誤差ξD構(gòu)成，即

(6)

(7)

由式(1)、式(5)和式(7)構(gòu)成SINS/OD的非線性對(duì)準(zhǔn)系統(tǒng)：

(8)

式中：f(x)和h(x)為非線性函數(shù)；w為系統(tǒng)噪聲；v為測(cè)量噪聲。w和v互相獨(dú)立，且w～N(w;0,Q)，v～N(v;0,R)，Q和R為噪聲方差矩陣。

當(dāng)姿態(tài)誤差φ為小角度時(shí)，非線性系統(tǒng)退化為線性系統(tǒng)，可以用Kalman濾波完成對(duì)準(zhǔn)。

2 CKF及R-T-S平滑

2.1 CKF算法

為了提高CKF的濾波精度，文獻(xiàn)[7]中提出了5階CKF，濾波精度達(dá)到5階。為了避免濾波過程中由于協(xié)方差矩陣Pk失去正定性導(dǎo)致濾波精度下降，提高算法的數(shù)值穩(wěn)定性，使用協(xié)方差矩陣奇異值分解(SVD)代替Cholesky分解，算法實(shí)現(xiàn)如下。

(1) 時(shí)間更新

(9)

b.初始采樣

(10)

[0,0,…,1]T}T

c.狀態(tài)傳播

(11)

(12)

(2) 量測(cè)更新

a.SVD分解Pk|k-1

(13)

b.重采樣

(14)

c.觀測(cè)傳播

η0=h(y0)；ηi=h(yi)；ηj=h(yj)

(15)

(16)

e.利用高斯濾波框架完成非線性濾波

(17)

2.2 基于CKF的非線性對(duì)準(zhǔn)仿真

假設(shè)陀螺常值漂移0.02°/h，隨機(jī)漂移為0.01°/h，加速度計(jì)常值零偏為1×10-4g，隨機(jī)零偏為5×10-5g。里程計(jì)安裝失準(zhǔn)角δαψ=0.5°，δαγ=0.5°，標(biāo)定系數(shù)誤差δKD=0.02。車輛初始位置緯度L=39.84°，經(jīng)度λ=116.12°。設(shè)置車輛運(yùn)動(dòng)過程如下：車輛靜止；北向1m/s2勻加速10s；勻速運(yùn)動(dòng)200s；2°/s左轉(zhuǎn)彎90°；勻速運(yùn)動(dòng)200s；2°/s右轉(zhuǎn)彎90°；勻速運(yùn)動(dòng)200s。

分別設(shè)置初始失準(zhǔn)角1[0.1°,0.1°,0.5°]、失準(zhǔn)角2[1°,1°,10°]和失準(zhǔn)角3[2°,2°,30°]，進(jìn)行非線性對(duì)準(zhǔn)仿真，時(shí)間700s。由于水平對(duì)準(zhǔn)速度快，精度高，因此只分析方位對(duì)準(zhǔn)情況。圖1給出了方位對(duì)準(zhǔn)結(jié)果，圖2給出了對(duì)準(zhǔn)過程中的水平位置誤差。

圖1表明失準(zhǔn)角1和2方位對(duì)準(zhǔn)精度較高，失準(zhǔn)角3對(duì)準(zhǔn)精度相對(duì)較差。圖2表明小失準(zhǔn)角下位置誤差較小，大失準(zhǔn)角下位置誤差較大。因?yàn)槔锍逃?jì)測(cè)量的是m系下車輛前向速度，由m系到n系的轉(zhuǎn)換過程中耦合了姿態(tài)誤差，造成n系下速度參考信息vn誤差較大，導(dǎo)致對(duì)準(zhǔn)開始時(shí)刻的位置誤差迅速增大，對(duì)準(zhǔn)一段時(shí)間后各項(xiàng)誤差逐步得到估計(jì)，位置誤差逐步減小。由于SINS/OD組合系統(tǒng)的位置誤差是發(fā)散的，隨著時(shí)間的繼續(xù)增長，位置誤差將緩慢增大。相對(duì)于慣性系對(duì)準(zhǔn)過程中忽略位置變化的處理方法而言，此位置誤差依然相對(duì)較小。由于多種誤差的互相耦合導(dǎo)致大失準(zhǔn)角下陀螺零漂εb、加速度計(jì)零偏b和里程計(jì)誤差ξD估計(jì)精度相對(duì)較差。

大失準(zhǔn)角對(duì)準(zhǔn)結(jié)束時(shí)，若在此對(duì)準(zhǔn)結(jié)果的基礎(chǔ)上進(jìn)行SINS/OD組合導(dǎo)航，帶來較大的初始位置誤差，該誤差在后續(xù)組合導(dǎo)航中無法進(jìn)行修正。應(yīng)急情況或位置精度相對(duì)較低的情況下，可直接使用此對(duì)準(zhǔn)結(jié)果，組合導(dǎo)航過程中一旦有位置參考信息時(shí)可進(jìn)行校正。當(dāng)計(jì)算速度較快或有多個(gè)處理器時(shí)，為了減小組合導(dǎo)航的位置誤差，可借用數(shù)據(jù)存儲(chǔ)和循環(huán)解算的思想，利用逆向?qū)Ш交驙顟B(tài)平滑對(duì)導(dǎo)航信息進(jìn)行再處理。

2.3 非線性R-T-S平滑

非線性平滑分為R-T-S平滑[15]和TF平滑[16]兩種，前者計(jì)算量較小，原理簡單，應(yīng)用較為廣泛。非線性R-T-S平滑在前向非線性濾波的基礎(chǔ)上進(jìn)行后向遞推，獲得此前任意時(shí)刻狀態(tài)估計(jì)，精度高于濾波。估計(jì)過程分為前向?yàn)V波和后向遞推兩部分。

第二部分：后向遞推，按照k=N,N-1,…,1的順序進(jìn)行逆向遞推，即

(18)

經(jīng)過后向遞推后，可獲得各時(shí)刻的狀態(tài)平滑估計(jì)。圖3給出了失準(zhǔn)角3方位對(duì)準(zhǔn)的平滑估計(jì)結(jié)果，可以看出整個(gè)平滑過程中方位失準(zhǔn)角為小角度。

3 動(dòng)基座對(duì)準(zhǔn)實(shí)驗(yàn)

3.1 對(duì)準(zhǔn)算法設(shè)置

(1) 粗對(duì)準(zhǔn)

非線性對(duì)準(zhǔn)能夠完成大失準(zhǔn)角下初始對(duì)準(zhǔn)。為了在較短的時(shí)間里獲得較好的估計(jì)效果，需對(duì)初始方位失準(zhǔn)角進(jìn)行一定的限制。車輛起動(dòng)或緩慢直線行駛的情況下，采集10s左右的數(shù)據(jù)，進(jìn)行慣性系粗對(duì)準(zhǔn)，獲得慣導(dǎo)初始姿態(tài)。

(2) 非線性對(duì)準(zhǔn)

車輛開始行駛，利用CKF進(jìn)行SINS/OD非線性對(duì)準(zhǔn)，對(duì)準(zhǔn)過程中存儲(chǔ)相關(guān)數(shù)據(jù)。車輛行駛一段時(shí)間后完成非線性對(duì)準(zhǔn)。

(3) 非線性R-T-S平滑

將存儲(chǔ)的前向?yàn)V波數(shù)據(jù)非線性R-T-S平滑至初始時(shí)刻，進(jìn)行姿態(tài)修正，獲得初始時(shí)刻的慣導(dǎo)姿態(tài)。

(4) 再次對(duì)準(zhǔn)

基于存儲(chǔ)的慣導(dǎo)數(shù)據(jù)和里程計(jì)數(shù)據(jù)，進(jìn)行SINS/OD線性對(duì)準(zhǔn)至對(duì)準(zhǔn)結(jié)束時(shí)刻。

3.2 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)室將某型光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)安裝在某車輛上，使用車輛自身的里程計(jì)信號(hào)。光纖陀螺的零偏穩(wěn)定性不超過0.02°/h，加速度計(jì)的偏值重復(fù)性小于5×10-5g。首先對(duì)SINS和OD的安裝偏差角、里程計(jì)標(biāo)定系數(shù)進(jìn)行初始標(biāo)定。

車輛行駛過程中使用高精度BD2導(dǎo)航芯片進(jìn)行實(shí)時(shí)速度、位置測(cè)量。由于車輛行駛過程中無法獲得準(zhǔn)確姿態(tài)，姿態(tài)參考值由靜基座對(duì)準(zhǔn)后轉(zhuǎn)SINS/BD2組合導(dǎo)航提供。

為了驗(yàn)證非線性對(duì)準(zhǔn)的有效性，對(duì)準(zhǔn)初始時(shí)刻在參考姿態(tài)的姿態(tài)上設(shè)置失準(zhǔn)角φ=[2°,2°,30°]，對(duì)準(zhǔn)時(shí)間900s。共進(jìn)行了5組試驗(yàn)，方位對(duì)準(zhǔn)結(jié)果及位置誤差如表1～表3所示。表中：ψ1，λ1和L1為首次非線性對(duì)準(zhǔn)的方位角、經(jīng)度和緯度；ψ2，λ2和L2為再次線性對(duì)準(zhǔn)的結(jié)果；ψ，λ和L為參考值。

表1 方位對(duì)準(zhǔn)結(jié)果

表2 對(duì)準(zhǔn)結(jié)束時(shí)的經(jīng)度誤差

表3 對(duì)準(zhǔn)結(jié)束時(shí)的緯度誤差

非線性對(duì)準(zhǔn)結(jié)束后，方位精度0.200°，經(jīng)度誤差121.7m，緯度誤差68.3m，方位角和位置誤差較大，其中位置誤差對(duì)后續(xù)組合導(dǎo)航影響更大。盡管如此，對(duì)準(zhǔn)過程中能夠提供相當(dāng)精度的位置導(dǎo)航，這是慣性系粗對(duì)準(zhǔn)中較難做到的。經(jīng)過R-T-S平滑和再次線性對(duì)準(zhǔn)后，方位精度提高到0.117°，經(jīng)度誤差8.5m，緯度誤差11.3m，對(duì)準(zhǔn)精度和位置精度大幅提高。再次對(duì)準(zhǔn)大大減小了后續(xù)SINS/OD組合導(dǎo)航的初始位置誤差和初始姿態(tài)誤差，從而提高組合導(dǎo)航的精度。

4 結(jié)論

為了同時(shí)實(shí)現(xiàn)SINS/OD動(dòng)基座對(duì)準(zhǔn)和位置導(dǎo)航，本文中設(shè)計(jì)了“非線性對(duì)準(zhǔn)+非線性后向平滑+再次線性對(duì)準(zhǔn)”的動(dòng)基座對(duì)準(zhǔn)方案。該方案非線性對(duì)準(zhǔn)在對(duì)準(zhǔn)結(jié)束時(shí)能使方位誤差為小角度，對(duì)準(zhǔn)過程中能夠提供一定精度的位置導(dǎo)航，經(jīng)過“平滑+再次對(duì)準(zhǔn)”的離線操作能夠同時(shí)提高對(duì)準(zhǔn)結(jié)束時(shí)刻的姿態(tài)和位置精度。

該對(duì)準(zhǔn)方案為離線方案，實(shí)際應(yīng)用中需使用實(shí)時(shí)方案，實(shí)時(shí)方案還需進(jìn)一步研究。

[1] WU Yuanxin, WU Meiping, HU Xiaoping, et al. Self-calibration for land navigation using inertial sensors and odometer: observability analysis[C]. AIAA Conference of Guidance, Navigation and Control,2009:1-10.

[2] 朱立彬,王瑋.車輛導(dǎo)航系統(tǒng)中里程計(jì)標(biāo)度因數(shù)的自標(biāo)定[J].汽車工程,2013,35(5):472-476.

[3] 嚴(yán)恭敏,秦永元,衛(wèi)育新,等.一種適用于SINS動(dòng)基座初始對(duì)準(zhǔn)的新算法[J].系統(tǒng)工程與電子技術(shù),2009,31(3):634-637.

[4] 趙小明,趙帥,郭永剛,等.基于強(qiáng)跟蹤濾波的車載行進(jìn)間對(duì)準(zhǔn)[J].中國慣性技術(shù)學(xué)報(bào),2015,23(2):141-144.

[5] 肖煊,王清哲,付夢(mèng)印,等.里程計(jì)輔助陸用慣導(dǎo)行進(jìn)間對(duì)準(zhǔn)方法[J].中國慣性技術(shù)學(xué)報(bào),2012,20(2):140-145.

[6] ARASARATNAM I, HAYKIN S. Cubature kalman filters[J]. IEEE Transactions on Automatic Control,2009,54(6):1254-1269.

[7] JIA B, XIN M, CHENG Y. High-degree cubature Kalman filter[J]. Automatica,2013,49:510-518.

[8] 嚴(yán)恭敏,嚴(yán)衛(wèi)生,徐德民.逆向?qū)Ш剿惴捌湓诮萋?lián)羅經(jīng)動(dòng)基座初始對(duì)準(zhǔn)中的應(yīng)用[C].第27屆中國控制會(huì)議論文集,北京:北京航空航天大學(xué)出版社,2008:724-729.

[9] LI Wanli, WU Wenqi, WANG Jinling, et al. A novel backtracking navigation scheme for autonomous underwater vehicles[J]. Measurement,2014,47:496-504.

[10] 李京書,許江寧,何泓洋,等.嚴(yán)格逆向過程的羅經(jīng)回溯對(duì)準(zhǔn)方法[J].系統(tǒng)工程與電子技術(shù),2014,36(2):348-353.

[11] GAO Wei, BEN Yueyang, ZHANG Xin, et al. Rapid fine strapdown INS alignment method under marine mooring condition[J]. IEEE Transactions on Aerospace and Electronic System,2011,47(4):2887-2896.

[12] GADSDEN S A, AL-SHABI M, ARASARATNAM I, et al. Combined cubature Kalman and smooth variable structure filtering: a robust nonlinear estimation strategy[J]. Signal Process,2014,96:290-299.

[13] JIA B, XIN M. Rauch-Tung-Striebel high-degree cubature Kalman smoother[C]. American Control Conference,2013:2472-2477.

[14] 嚴(yán)恭敏,嚴(yán)衛(wèi)生,徐德民.基于歐拉平臺(tái)誤差角的SINS非線性誤差模型研究[J].西北工業(yè)大學(xué)學(xué)報(bào),2009,27(4):511-516.

[15] WANG X X, PAN Q, LIANG Y, et al. Application of unscented transformation for nonlinear state smoothing[J]. Acta Automatica Sinica,2012,38(7):1107-1112.

[16] MA Yanhai, FANG Jiancheng, LI Jianli. Accurate estimation of lever arm in SINS/GPS integration by smoothing methods[J]. Measurement,2014,48:119-127.

Moving Base Alignment Scheme for On-board SINS/OD CombinationBased on CKF and R-T-S Smoothing

Huang Xiangyuan, Tang Xiaqing & Wu Meng

DepartmentofControlEngineering,AcademyofArmoredForceEngineering,Beijing100072

In order to improve the alignment accuracy and speed and the accuracy of position navigation of moving base in a strapdown inertial navigation system/odometer (SINS/OD) combination, a scheme of “nonlinear alignment by cubature Kalman filter (CKF) + nonlinear Rauch-Tung-Striebel (R-T-S) smoothing + linear realignment by Kalman filter” is proposed. Specifically, a nonlinear alignment is conducted by CKF first with a result of ordinary azimuth alignment accuracy and a relatively significant position error, then a nonlinear R-T-S smoothing is performed to obtain a high accuracy alignment result at initial moment with the nonlinear error model degenerated to linear model, and finally a linear realignment is carried out by Kalmann filter to obtain high accuracy azimuth alignment and position navigation. The scheme can obtain a high accuracy result without time-consuming coarse alignment, and the real vehicle test verifies the effectiveness of the scheme proposed.

strapdown inertial navigation system/odometer combination; cubature Kalman filter; nonlinear Rauch-Tung-Striebel smoothing; Kalman filter

*軍隊(duì)計(jì)劃項(xiàng)目資助。

原稿收到日期為2015年7月3日。