基于CKF和R-T-S平滑的車載SINS/OD動基座對準方案*

黃湘遠，湯霞清，武 萌

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

2016160

基于CKF和R-T-S平滑的車載SINS/OD動基座對準方案*

黃湘遠，湯霞清，武 萌

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

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

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

前言

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

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

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

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

1 SINS/OD非線性對準模型

1.1 SINS非線性誤差

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

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

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

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

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

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

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

(1)

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

1.2 OD誤差

(2)

(3)

式中ωD為里程計隨機量測噪聲。

當失準角φ=[φE,φN,φU]T為大角度時，里程計速度在計算平臺系p系的實際輸出為

(4)

1.3 SINS/OD動基座非線性對準系統(tǒng)

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

(5)

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

(6)

(7)

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

(8)

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

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

2 CKF及R-T-S平滑

2.1 CKF算法

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

(1) 時間更新

(9)

b.初始采樣

(10)

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

c.狀態(tài)傳播

(11)

(12)

(2) 量測更新

a.SVD分解Pk|k-1

(13)

b.重采樣

(14)

c.觀測傳播

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

(15)

(16)

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

(17)

2.2 基于CKF的非線性對準仿真

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

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

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

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

2.3 非線性R-T-S平滑

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

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

(18)

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

3 動基座對準實驗

3.1 對準算法設置

(1) 粗對準

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

(2) 非線性對準

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

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

將存儲的前向濾波數(shù)據(jù)非線性R-T-S平滑至初始時刻，進行姿態(tài)修正，獲得初始時刻的慣導姿態(tài)。

(4) 再次對準

基于存儲的慣導數(shù)據(jù)和里程計數(shù)據(jù)，進行SINS/OD線性對準至對準結(jié)束時刻。

3.2 實驗驗證

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

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

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

表1 方位對準結(jié)果

表2 對準結(jié)束時的經(jīng)度誤差

表3 對準結(jié)束時的緯度誤差

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

4 結(jié)論

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

該對準方案為離線方案，實際應用中需使用實時方案，實時方案還需進一步研究。

[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] 朱立彬,王瑋.車輛導航系統(tǒng)中里程計標度因數(shù)的自標定[J].汽車工程,2013,35(5):472-476.

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

[4] 趙小明,趙帥,郭永剛,等.基于強跟蹤濾波的車載行進間對準[J].中國慣性技術(shù)學報,2015,23(2):141-144.

[5] 肖煊,王清哲,付夢印,等.里程計輔助陸用慣導行進間對準方法[J].中國慣性技術(shù)學報,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] 嚴恭敏,嚴衛(wèi)生,徐德民.逆向?qū)Ш剿惴捌湓诮萋?lián)羅經(jīng)動基座初始對準中的應用[C].第27屆中國控制會議論文集,北京:北京航空航天大學出版社,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] 李京書,許江寧,何泓洋,等.嚴格逆向過程的羅經(jīng)回溯對準方法[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] 嚴恭敏,嚴衛(wèi)生,徐德民.基于歐拉平臺誤差角的SINS非線性誤差模型研究[J].西北工業(yè)大學學報,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

*軍隊計劃項目資助。

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