基于狀態(tài)變換卡爾曼濾波的DVL/SINS組合導(dǎo)航算法

靳凱迪, 柴洪洲,*, 宿楚涵, 向民志, 李 明

(1. 戰(zhàn)略支援部隊信息工程大學(xué)地理空間信息學(xué)院, 河南 鄭州 450001;2. 鄭州航天宏圖北斗應(yīng)用技術(shù)研究院有限公司, 河南 鄭州 450001)

0 引 言

水下無人自主航行器(unmanned underwater vehicle, UUV)在海洋測繪、水下資源探索和水下目標(biāo)探測等任務(wù)中發(fā)揮著越來越重要的作用。獲取高精度的姿態(tài)、速度和位置等導(dǎo)航參數(shù)是UUV高效和安全完成任務(wù)的重要前提[1-3]。捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system, SINS)具有自主性、隱蔽性和主動性等優(yōu)勢,可為UUV提供豐富的導(dǎo)航參數(shù)[4-5]。然而,受慣性測量元件(inertial measurement unit, IMU)誤差的影響,SINS定位誤差會隨時間迅速累積,因此必須使用輔助傳感器修正SINS累積誤差。

多普勒計程儀(Doppler velocity logger,DVL)/SINS組合導(dǎo)航系統(tǒng)是應(yīng)用最為廣泛的UUV水下定位系統(tǒng)[6-7]。根據(jù)傳感器耦合方式的不同,DVL/SINS組合導(dǎo)航可分為松組合和緊組合[8],二者分別使用DVL三維速度和波束頻移作為Kalman濾波的量測信息[9-12]。然而,由于當(dāng)前大部分DVL僅能提供三維速度,在實際情況中應(yīng)用最為廣泛的仍為松組合方式。近年來,許多學(xué)者針對DVL/SINS組合導(dǎo)航Kalman濾波中的DVL誤差模型、非完整性約束和自適應(yīng)濾波等問題展開了廣泛研究,有效提升了DVL/SINS組合導(dǎo)航系統(tǒng)的定位性能[13-16]。

DVL/SINS組合導(dǎo)航卡爾曼濾波中的狀態(tài)方程由SINS誤差方程構(gòu)成。根據(jù)姿態(tài)誤差表達(dá)方式的不同,SINS誤差方程可分為歐拉角誤差模型和四元數(shù)誤差模型[17-18]。針對傳統(tǒng)SINS誤差模型不嚴(yán)密的問題,文獻(xiàn)[19]提出了一種基于矢量運算坐標(biāo)系一致的誤差構(gòu)建思想。在其基礎(chǔ)上,文獻(xiàn)[20]指出,傳統(tǒng)SINS速度誤差的定義只考慮了實際速度與理論速度大小的差異,而忽略了矢量坐標(biāo)系不一致的影響。進(jìn)一步,文獻(xiàn)[21]通過定義新的速度誤差提出了狀態(tài)變換卡爾曼濾波,并將其成功應(yīng)用于全球衛(wèi)星導(dǎo)航(global navigation satellite system, GNSS)系統(tǒng)/SINS組合導(dǎo)航、視覺/SINS組合導(dǎo)航和SINS初始對準(zhǔn)等陸地場景[22-23]。文獻(xiàn)[24]和文獻(xiàn)[25]將速度誤差定義在計算導(dǎo)航系中,分別推導(dǎo)了GNSS阻尼SINS非線性誤差模型的歐拉角形式和四元數(shù)形式,并將其應(yīng)用于GNSS/SINS大失準(zhǔn)角對準(zhǔn)中。

本文延續(xù)統(tǒng)一矢量運算中坐標(biāo)系的思想,將狀態(tài)變換卡爾曼濾波引入DVL/SINS組合導(dǎo)航。首先,指出傳統(tǒng)SINS誤差方程中姿態(tài)誤差方程存在的坐標(biāo)系不一致問題,進(jìn)而構(gòu)建了更為嚴(yán)密的SINS誤差方程;進(jìn)一步,基于新定義的速度誤差推導(dǎo)了改進(jìn)的DVL/SINS組合導(dǎo)航卡爾曼濾波狀態(tài)方程和量測方程;最后,利用仿真數(shù)據(jù)和船載實測數(shù)據(jù)驗證了本文所提算法的有效性。

1 坐標(biāo)系定義

本文坐標(biāo)系定義如下:

(1) 導(dǎo)航坐標(biāo)系(n系):定義“東-北-天(E-N-U)”當(dāng)?shù)厮阶鴺?biāo)系為導(dǎo)航坐標(biāo)系;

(2) 計算導(dǎo)航坐標(biāo)系(n′系):SINS實際計算時使用的導(dǎo)航坐標(biāo)系,n′系與n系間的歐拉角稱為失準(zhǔn)角φ;

(4) DVL坐標(biāo)系(d系):原點為DVL的幾何中心,x-y-z軸分別指向DVL框架的“右-前-上”方向,d系與b系間的歐拉角稱為DVL的安裝偏差角η,b系原點至d系原點的位移稱為DVL的桿臂誤差δl;

(5) 地球坐標(biāo)系(e系):原點位于地球質(zhì)心,x軸指向赤道與本初子午線的交點,z軸指向北極點,y軸與x軸和z軸構(gòu)成右手坐標(biāo)系;

(6) 地心慣性坐標(biāo)系(i系):原點位于地球質(zhì)心,x軸指向春分點,z軸指向北極點,y軸與x軸和z軸構(gòu)成右手坐標(biāo)系,i系不隨地球自轉(zhuǎn)而轉(zhuǎn)動。

2 狀態(tài)變換SINS誤差模型

2.1 傳統(tǒng)SINS誤差模型

SINS姿態(tài)可表示為歐拉角或四元數(shù)。在小失準(zhǔn)角假設(shè)下,SINS的歐拉角誤差模型和四元數(shù)誤差模型均為線性,但四元數(shù)各分量的物理意義不明確,而且增加了誤差模型的維度,因此本文選擇歐拉角(失準(zhǔn)角φ)描述SINS的姿態(tài)誤差。

傳統(tǒng)SINS姿態(tài)誤差模型為

(1)

速度誤差模型為

(2)

位置誤差模型為

(3)

式中:δp=[δL,δλ,δh]T為SINS的位置誤差;L、λ和h分別為SINS的緯度、經(jīng)度和高程;Mpv和Mpp的具體形式為

(4)

(5)

式中:RMh=RM+h;RMh=RN+h;RM和RN分別為SINS所在位置的子午圈曲率半徑與卯酉圈曲率半徑。

2.2 改進(jìn)的SINS誤差模型

(6)

(7)

其中,

(8)

(9)

根據(jù)式(6)定義的速度誤差,可將傳統(tǒng)速度誤差表示為

(10)

將式(10)代入式(7)可得

(11)

(12)

將式(11)代入式(1),可得

(13)

式中:

(14)

將式(14)代入式(13),得到新的姿態(tài)誤差方程:

(15)

式中:Map=M1+M2。

對式(6)兩側(cè)微分,可得

(16)

結(jié)合式(2)、式(13)和速度微分方程,式(16)可改寫為

(17)

將式(10)和式(11)代入式(17),有

(18)

由于

(19)

將式(14)和式(19)代入式(18)中并化簡,得到新的速度誤差方程:

(20)

將式(10)代入式(3),得到新的位置誤差方程:

(21)

3 DVL/SINS組合導(dǎo)航卡爾曼濾波

3.1 卡爾曼濾波狀態(tài)方程

DVL測速精度受水體溫度、鹽度和波束寬度等因素影響,在實際應(yīng)用中一般將DVL觀測值建模為

(22)

式中:δk是建模為常值的刻度因子;wd為高斯白噪聲。

考慮DVL和SINS的誤差參數(shù)以及d系與b系間的安裝偏差角和桿臂誤差,在DVL/SINS組合導(dǎo)航卡爾曼濾波中選擇22維狀態(tài)向量:

(23)

因此,Kalman濾波狀態(tài)方程為

(24)

式中:Ft是由SINS誤差方程構(gòu)成的狀態(tài)轉(zhuǎn)移矩陣;Gt為系統(tǒng)噪聲驅(qū)動矩陣;Wb為由陀螺和加速度計的白噪聲構(gòu)成的狀態(tài)噪聲。

根據(jù)式(15)、式(20)和式(21),狀態(tài)轉(zhuǎn)移矩陣和系統(tǒng)噪聲驅(qū)動矩陣的具體形式為

--------------------

(25)

--------------------

3.2 卡爾曼濾波量測方程

DVL/SINS松組合中以三維速度作為卡爾曼濾波的量測信息?？紤]DVL的安裝偏差角與桿臂誤差,由式(22)可得

(26)

由式(24),得到DVL/SINS組合導(dǎo)航量測方程為

(27)

綜上,DVL/SINS組合導(dǎo)航卡爾曼濾波數(shù)據(jù)融合過程如圖1所示。

圖1 DVL/SINS組合導(dǎo)航數(shù)據(jù)融合過程

4 實驗結(jié)果與分析

4.1 仿真實驗

為驗證DVL/SINS狀態(tài)變換Kalman濾波算法的有效性,首先設(shè)計如圖2所示的仿真軌跡進(jìn)行實驗驗證。仿真軌跡總時長為800 s,包含如表1所示的加速、減速、勻速、橫滾和轉(zhuǎn)向等典型UUV運動狀態(tài)。軌跡初始位置為北緯30°、東經(jīng)120°、高程0 m,初始速度為0 m/s,初始姿態(tài)角為[0°,0°,0°]。軌跡的初始水平失準(zhǔn)角設(shè)為5′,初始天向失準(zhǔn)角為30′,E-N-U3個方向的初始速度誤差均為1 m/s。

表1 仿真軌跡運動狀態(tài)

圖2 UUV仿真軌跡

為說明本文算法的有效性,使用常規(guī)DVL/SINS組合導(dǎo)航Kalman濾波(以“常規(guī)算法”表示)作為對比[28-29]。在相同條件下執(zhí)行兩種算法,分別進(jìn)行50次蒙特卡羅仿真實驗。

在姿態(tài)誤差上,由于兩種算法的水平姿態(tài)角估計精度均較高,在此僅對比天向失準(zhǔn)角的估計效果。圖3給出了50次蒙特卡羅實驗中兩種算法的天向失準(zhǔn)角平均值曲線。從圖3可見,本文算法的收斂速度更快。在0.5°初始天向失準(zhǔn)角下,本文算法的平均天向失準(zhǔn)角在280 s左右收斂到0.1°以下,而常規(guī)方式在560 s左右收斂到0.1°以下。導(dǎo)航結(jié)束時,常規(guī)算法的天向失準(zhǔn)角為0.045°,本文算法的天向失準(zhǔn)角為0.024°。因此,本文算法在一定程度上提高了DVL/SINS組合導(dǎo)航的姿態(tài)精度。

圖3 天向失準(zhǔn)角平均值

DVL/SINS松組合直接以三維速度作為量測值,組合導(dǎo)航系統(tǒng)中速度誤差具有較強的可觀測性。圖4給出了兩種方案的E-N-U 3個方向速度誤差的平均誤差曲線。從圖4可見,兩種算法的速度誤差在DVL的約束下迅速收斂,且本文算法的水平速度估計效果優(yōu)于常規(guī)算法,在測速精度和穩(wěn)定性上均有一定程度的提升。經(jīng)過初始收斂階段,在E-N方向上,常規(guī)算法的最大速度誤差分別為0.214 m/s和-0.069 m/s,而本文算法分別為0.142 m/s和-0.031 m/s,本文算法在導(dǎo)航前期有較高的速度精度。隨著導(dǎo)航進(jìn)行,二者差異逐漸減小,在對準(zhǔn)結(jié)束時,兩種算法在E-N方向上的速度誤差均為0.003 m/s和0.009 m/s。對比式(2)和式(20)可見,由于在新的速度誤差方程中由更穩(wěn)定的重力矢量代替了式(2)中的比力信息,因此Kalman濾波狀態(tài)轉(zhuǎn)移矩陣更加穩(wěn)定。

圖4 速度平均誤差

圖5給出了兩種方案E-N-U 3個方向的平均位置誤差曲線。由于DVL/SINS系統(tǒng)中位置誤差不可觀,因此解算的位置誤差是速度誤差累積的結(jié)果。從圖5可見,常規(guī)算法的E-N-U 3個方向的最大位置誤差分別為13.1 m、-8.9 m和-2.4 m,本文算法3個方向的最大位置誤差分別為8.1 m,-5.3 m和-2.6 m。在導(dǎo)航初始階段,本文算法的位置誤差發(fā)散得到了一定程度的改善。在導(dǎo)航結(jié)束時,兩種算法的定位誤差的差異在毫米級。

圖5 位置平均誤差

圖6～圖8給出了兩種方案的DVL的刻度系數(shù)、安裝偏差角和桿臂誤差的50次實驗平均值曲線。從圖中可見,對于可觀性較強的狀態(tài)參數(shù),如x軸-z軸安裝偏差角、刻度系數(shù)和桿臂誤差,兩種算法的估計效果較為接近,均可估計出添加的誤差參數(shù)。對于可觀性較差的狀態(tài)參數(shù),兩種算法有一定差異,且均無法準(zhǔn)確估計。

圖6 DVL刻度系數(shù)平均值

圖7 DVL安裝偏差角平均值

圖8 DVL桿臂誤差平均值

4.2 船載實驗

圖9 船載平臺及設(shè)備

實驗使用高精度后處理軟件Novatel Inertial Explorer(IE)的GNSS-RTK/SINS前后向平滑緊組合的輸出作為參考基準(zhǔn)。由IE結(jié)果獲得軌跡的初始位置為北緯30.577°,東經(jīng)114.472°,高程19 m。船載軌跡如圖10所示。

圖10 武漢湖試軌跡

為了避免DVL桿臂誤差、安裝誤差角和刻度系數(shù)對導(dǎo)航參數(shù)的影響,使用文獻(xiàn)[30]算法對實測數(shù)據(jù)中的DVL誤差參數(shù)進(jìn)行了預(yù)標(biāo)定。實驗中,在IE參考值的基礎(chǔ)上,設(shè)置軌跡的初始水平失準(zhǔn)角為0.5°,初始天向失準(zhǔn)角為1°,E-N-U 3個方向速度誤差均為1 m/s。

使用常規(guī)算法與本文算法分別對該軌跡進(jìn)行處理。圖11給出了兩種算法估計的天向失準(zhǔn)角對比。從圖11可見,在導(dǎo)航前期二者的差異較為顯著,本文算法的最大天向失準(zhǔn)角誤差為-2.390°,常規(guī)算法為-4.199°。隨著導(dǎo)航進(jìn)行,二者差異逐漸減小,在導(dǎo)航結(jié)束時常規(guī)算法和本文算法的天向失準(zhǔn)角分別為-0.040°和0.017°。因此,在補償了坐標(biāo)系不一致誤差后,組合導(dǎo)航系統(tǒng)估計的姿態(tài)精度更高。

圖11 湖試中的天向失準(zhǔn)角

圖12給出了兩種算法的速度誤差估計曲線。從圖12可見,兩種算法的天向速度有基本一致的估計精度。在E-N方向上,常規(guī)算法的最大速度誤差分別為-0.214 m/s和0.172 m/s,而本文算法的最大速度誤差分別為-0.105 m/s和0.098 m/s。因此,本文算法在導(dǎo)航前期的測速精度較高,隨著導(dǎo)航進(jìn)行,兩種算法速度誤差的差異逐漸減小。

圖12 湖試中的速度誤差

圖13給出了兩種算法的位置誤差估計曲線。從圖13可見,常規(guī)算法和本文算法的水平最大位置誤差分別為18.700 m和14.239 m。當(dāng)導(dǎo)航結(jié)束時,常規(guī)算法和本文算法的水平位置誤差分別為4.937 m和3.109 m。這是由于本文算法的速度誤差較小,而位置誤差受速度誤差累積的影響。統(tǒng)計兩種算法估計的姿態(tài)、速度和位置均方根差(root mean squared error, RMSE)如表2所示。從表2可見,本文算法的姿態(tài)、速度和位置的精度較常規(guī)算法均有一定程度的提升。其中,天向失準(zhǔn)角RMSE降低了43%,東向速度RMSE降低了34%,北向速度RMSE降低了31%。本文算法的東向位置誤差和北向位置誤差的RMSE分別降低了28%和10%。

表2 湖試中導(dǎo)航參數(shù)RMSE統(tǒng)計結(jié)果

圖13 湖試中的位置誤差

5 結(jié) 論

為了提高DVL/SINS組合導(dǎo)航系統(tǒng)的定位精度,考慮SINS速度誤差中坐標(biāo)系不一致的問題,提出了一種基于狀態(tài)變換Kalman濾波的DVL/SINS組合導(dǎo)航算法。在分析傳統(tǒng)SINS誤差方程中存在坐標(biāo)系不一致誤差的基礎(chǔ)上,詳細(xì)推導(dǎo)了改進(jìn)的狀態(tài)變換SINS誤差方程;進(jìn)一步地,構(gòu)建了DVL/SINS組合導(dǎo)航Kalman濾波方程。實驗結(jié)果表明,該算法可以有效處理傳統(tǒng)速度誤差中坐標(biāo)系不一致的影響,在一定程度上提高了DVL/SINS組合導(dǎo)航系統(tǒng)的精度。