基于可觀測度反饋的SINS/DVL組合導(dǎo)航算法

李志煒, 岳 崴, 高勝峰, 蔡 鵬, 朱海榮

基于可觀測度反饋的SINS/DVL組合導(dǎo)航算法

李志煒1, 岳 崴2, 高勝峰2, 蔡 鵬2, 朱海榮2

(1. 中國人民解放軍91001部隊(duì), 北京, 100080; 2. 海軍潛艇學(xué)院, 山東 青島, 266199)

針對捷聯(lián)慣導(dǎo)/多普勒計(jì)程儀(SINS/DVL)組合導(dǎo)航中部分系統(tǒng)狀態(tài)對無人水下航行器(UUV)導(dǎo)航精度影響較大但可觀測性相對較弱的問題, 提出一種基于可觀測度反饋的SINS/DVL組合導(dǎo)航算法。該算法通過量化分析系統(tǒng)各狀態(tài)參數(shù)的可觀測性, 根據(jù)系統(tǒng)狀態(tài)參數(shù)在不同機(jī)動(dòng)狀態(tài)下可觀測度不同的特點(diǎn), 構(gòu)造基于可觀測度反饋的自適應(yīng)調(diào)節(jié)因子, 增加高可觀測度時(shí)的系統(tǒng)狀態(tài)參數(shù)的估計(jì)占比, 以此來提高組合導(dǎo)航濾波算法的適應(yīng)性和濾波估計(jì)精度。利用湖試數(shù)據(jù)對上述算法的性能進(jìn)行了驗(yàn)證。結(jié)果表明, 可觀測度反饋濾波算法累積誤差明顯小于常規(guī)卡爾曼濾波算法, 在相同的精度要求下, 可延長水下SINS/DVL組合導(dǎo)航自主航行的時(shí)間。

無人水下航行器; 組合導(dǎo)航; 捷聯(lián)慣導(dǎo); 多普勒計(jì)程儀; 可觀測度

0 引言

無人水下航行器(unmanned undersea vehicle, UUV)因其目標(biāo)特征小、隱蔽性好、工作范圍廣的優(yōu)點(diǎn)已逐漸成為探測海洋、利用海洋的重要工具, 而實(shí)時(shí)準(zhǔn)確的導(dǎo)航信息是UUV順利執(zhí)行任務(wù)的基礎(chǔ)[1]。受體積和成本的限制, 目前UUV搭載的導(dǎo)航系統(tǒng)通常為捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system, SINS)與多普勒計(jì)程儀(Doopler velocity log, DVL)等輔助導(dǎo)航設(shè)備組合的形式[2-3]。

目前提高SINS/DVL組合導(dǎo)航系統(tǒng)定位精度的方法主要有2種: 一是使用改進(jìn)的濾波算法, 文獻(xiàn)[4]利用濾波收斂判據(jù)對Sage-Husa自適應(yīng)濾波器進(jìn)行改進(jìn), 從而提高基于自適應(yīng)濾波算法的SINS/DVL組合導(dǎo)航系統(tǒng)穩(wěn)定性和導(dǎo)航精度; 文獻(xiàn)[5]采用了球面最簡相徑采樣規(guī)則改進(jìn)容積卡爾曼濾波, 并通過數(shù)學(xué)平臺仿真驗(yàn)證了該算法在S1N/DVL組合導(dǎo)航系統(tǒng)中的有效性。一是使用改進(jìn)SINS/DVL的組合形式, 文獻(xiàn)[6]推導(dǎo)了SINS/ DVL的緊組合導(dǎo)航形式, 并利用卡爾曼濾波信息進(jìn)行故障檢測與隔離, 有效提高了系統(tǒng)導(dǎo)航精度。但是這些方法都沒有解決SINS/DVL組合導(dǎo)航系統(tǒng)中部分狀態(tài)參數(shù)可觀測性弱的問題, 并且具有在不同機(jī)動(dòng)狀態(tài)下可觀測度不同的特點(diǎn)[7]。特別地, 慣性系統(tǒng)軸陀螺漂移的可觀測性相對較弱, 這也是在初始對準(zhǔn)中方位失準(zhǔn)角通常收斂更慢的原因。而且相對水平陀螺漂移,軸陀螺漂移對SINS/DVL組合導(dǎo)航精度的影響更大。

可觀測性分析的方法有很多, 應(yīng)用于慣性導(dǎo)航可觀測性研究中主要有分段線性定常系統(tǒng)(piecewise constant system, PWCS)方法[8]和非線性全局可觀測性分析方法[9]。可觀測性分析方法一般只能給出系統(tǒng)可觀與不可觀的結(jié)論, 而實(shí)際中往往更希望得到定量的分析結(jié)果, 文獻(xiàn)[10]和文獻(xiàn)[11]給出了基于可觀測矩陣的奇異值或特征值來求解狀態(tài)可觀測度的方法; 文獻(xiàn)[12]基于PWCS提出利用可觀測階數(shù)和相對可觀測度定量分析狀態(tài)的可觀測性, 但是上述方法均有一定的局限性[13-14]。

文中在文獻(xiàn)[15]的基礎(chǔ)上分析在卡爾曼濾波過程中誤差協(xié)方差陣對對應(yīng)狀態(tài)分量的估計(jì)誤差或精度的影響, 提出了一種基于可觀測度反饋的SINS/DVL組合導(dǎo)航算法, 該方法可以有效解決部分狀態(tài)參數(shù)可觀測性弱導(dǎo)致的系統(tǒng)濾波精度較低的問題。

1 SINS/DVL組合導(dǎo)航模型

1.1 狀態(tài)方程

SINS/DVL組合導(dǎo)航系統(tǒng)各狀態(tài)變量分別選取為:

系統(tǒng)誤差狀態(tài)共22維, 如下

1.2 量測方程

根據(jù)上述誤差分析可知, DVL實(shí)際輸出速度

投影到導(dǎo)航坐標(biāo)系, 可寫為

從而可得DVL速度誤差

以SINS和DVL輸出的速度差值為量測量, 則

整理可得如下形式的非線性量測方程

2 基于可觀測度反饋的SINS/DVL組合導(dǎo)航算法

2.1 基于協(xié)方差陣的可觀測度分析

可觀測度的概念是在可觀測性概念的基礎(chǔ)上發(fā)展而來的, 所以有必要首先明確可觀測性的概念。對于有界輸入輸出線性/非線性系統(tǒng), 其可觀測性定義如下。

定義

將式(11)代入式(12)得

推導(dǎo)可得

2.2 自適應(yīng)調(diào)節(jié)引子計(jì)算步驟

在SINS/DVL組合導(dǎo)航系統(tǒng)中, 為了提高系統(tǒng)狀態(tài)的估計(jì)精度, 根據(jù)各狀態(tài)分量在不同時(shí)刻可觀測度的大小確定該時(shí)刻在狀態(tài)估計(jì)中占據(jù)的權(quán)重系數(shù), 從而構(gòu)造自適應(yīng)調(diào)節(jié)因子, 以減小弱觀測度數(shù)據(jù)對整個(gè)濾波器的影響, 提高濾波算法估計(jì)精度。下面給出自適應(yīng)調(diào)節(jié)因子的具體計(jì)算步驟。

1) 通過一致變換使估計(jì)誤差向量無量綱化

5) 計(jì)算各狀態(tài)變量自適應(yīng)調(diào)節(jié)因子

從而實(shí)現(xiàn)通過對不同時(shí)刻狀態(tài)變量的可觀測度信息來動(dòng)態(tài)調(diào)整其增益的目的。

3 仿真結(jié)果與分析

為了驗(yàn)證基于可觀測度反饋的SINS/DVL組合導(dǎo)航算法的效果, 進(jìn)行仿真分析。仿真中對全球?qū)Ш叫l(wèi)星系統(tǒng)定位(global navigation satellite system, GNSS)等傳感器參數(shù)進(jìn)行了比較, 如表1所示。

表1 傳感器參數(shù)

圖1 UUV模擬航跡

分別對基于常規(guī)卡爾曼濾波和可觀測度反饋濾波的SINS/DVL組合導(dǎo)航算法進(jìn)行仿真, 并實(shí)時(shí)計(jì)算陀螺漂移、加表零偏、SINS的安裝誤差角和DVL刻度系數(shù)誤差的可觀測度及狀態(tài)估計(jì)值, 結(jié)果如圖2~圖5所示, 2種算法的定位誤差見圖6。

圖4 SINS z軸安裝角誤差可觀測度及其估計(jì)值

圖5 DVL刻度系數(shù)誤差可觀測度及其估計(jì)值

Fig 6 Error of location

從圖2和圖3可以看出, 與常規(guī)卡爾曼濾波算法相比, 采用可觀測度反饋濾波算法時(shí), 不僅使陀螺漂移和加表零偏等系統(tǒng)狀態(tài)估計(jì)精度更高,而且收斂速度更快, 特別是在系統(tǒng)狀態(tài)的可觀測度非常小的情況下, 其狀態(tài)估計(jì)值具有更高的穩(wěn)定性和估計(jì)精度。從圖4和圖5可以看出, 在可觀測度反饋濾波算法中, SINS的軸安裝角誤差和DVL刻度系數(shù)誤差的可觀測度在100 min左右迅速降到非常小的情況, 而其狀態(tài)估計(jì)值也迅速穩(wěn)定, 具有更好的估計(jì)效果。從圖6中可以看出, 在定位誤差上, 采用可觀測度反饋濾波算法的SINS/DVL組合導(dǎo)航定位精度明顯優(yōu)于使用常規(guī)卡爾曼濾波算法。

4 試驗(yàn)數(shù)據(jù)分析

為了驗(yàn)證基于可觀測度反饋濾波的SINS/ DVL組合導(dǎo)航算法在湖中試驗(yàn)的效果, 進(jìn)行湖試數(shù)據(jù)分析。其中使用的各傳感器參數(shù)如表1所示。采用的航跡數(shù)據(jù)為某次湖上試驗(yàn)航行數(shù)據(jù), 試驗(yàn)船主要進(jìn)行1個(gè)旋回機(jī)動(dòng)和1個(gè)S形機(jī)動(dòng), 載體速度在3~5 kn之間, 全程航行37.78 km, 總航時(shí)4.94 h, 載體軌跡見圖7。

圖7 載體軌跡

分別對基于常規(guī)卡爾曼濾波和可觀測度反饋濾波的SINS/DVL組合導(dǎo)航算法進(jìn)行仿真, 結(jié)果如圖8所示。由圖8可以看出, 在定位誤差上, 采用常規(guī)卡爾曼濾波算法, 全程累積誤差為333 m, 相對誤差為8.8‰, 而采用可觀測度反饋濾波算法, 全程累積誤差為57 m, 相對誤差為1.5‰, 可見可觀測度反饋濾波算法累積誤差明顯小于常規(guī)卡爾曼濾波算法。

圖8 定位誤差對比

綜上所述, 基于可觀測度反饋的SINS/DVL組合導(dǎo)航濾波算法, 通過引入含有系統(tǒng)狀態(tài)實(shí)時(shí)可觀測度信息的自適應(yīng)調(diào)節(jié)因子, 實(shí)現(xiàn)了減小狀態(tài)變量的實(shí)時(shí)估計(jì)誤差, 進(jìn)而提高SINS/DVL組合導(dǎo)航精度的目的。在相同的精度要求下, 可延長水下SINS/DVL組合導(dǎo)航自主航行的時(shí)間。

5 結(jié)束語

SINS/DVL組合導(dǎo)航是UUV自主航行的主要導(dǎo)航方式。文中針對部分系統(tǒng)狀態(tài)可觀測性卻相對較弱的問題, 提出1種基于可觀測度反饋濾波算法的SINS/DVL組合導(dǎo)航算法, 根據(jù)各狀態(tài)分量在不同時(shí)刻可觀測度的大小確定該時(shí)刻在狀態(tài)估計(jì)中占據(jù)的權(quán)重系數(shù), 從而構(gòu)造自適應(yīng)調(diào)節(jié)因子以減小弱觀測度數(shù)據(jù)對整個(gè)濾波器的影響, 提高濾波算法估計(jì)精度。利用湖試數(shù)據(jù)對可觀測度反饋濾波算法與常規(guī)卡爾曼濾波算法進(jìn)行了對比分析, 從狀態(tài)變量估計(jì)精度和定位誤差的角度驗(yàn)證了可觀測度反饋濾波算法的效果。在對試驗(yàn)數(shù)據(jù)進(jìn)行整理分析過程中發(fā)現(xiàn), 除了系統(tǒng)中可觀測度弱的狀態(tài)參數(shù)會影響系統(tǒng)的濾波效果外, 當(dāng)DVL量測信息中存在較大誤差時(shí), 也會降低系統(tǒng)狀態(tài)參數(shù)的估計(jì)精度, 因此需要進(jìn)一步優(yōu)化濾波算法, 規(guī)避大誤差量測信息的影響。

[1] 馮正平. 國外自治水下機(jī)器人發(fā)展現(xiàn)狀綜述[J]. 魚雷技術(shù), 2005, 13(1): 5-9.Feng Zheng-ping. A Review of the Development of Autonomous Underwater Vehicles(AUVs) in Western Countries[J]. Torpedo Technology, 2005, 13(1): 5-9.

[2] Hayato K, Tamaki U. Navigation of an AUV for Inves- tigation of Underwater Structures[J]. Control Engineer- ing Practice, 2004, 12(12): 1551-1559.

[3] Yaruui G, Martins R, Sousa J. Accuracy Analysis of DVL/ IMUI Magnetometer Integrated Navigation System using Different IMUs in AUV[C]//Proceedings of the 2010 8th IEEE International Conference on Control and Automation, Xiamen, China: ICCA, 2010: 519-521.

[4] 徐曉蘇, 潘永飛, 鄒海軍. 基于自適應(yīng)濾波的SINS/ DVL組合導(dǎo)航系統(tǒng)[J]. 華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版), 2015, 43(2): 48-53.Xu Xiao-su, Pan Yong-fei, Zou Hai-jun. SINS/DVL Integrated Navigation System Based on Adaptive Filtering[J]. Journal of Huazhong University of Science and Technology(Nature Science Edition), 2015, 43(2): 48-53.

[5] 徐曉蘇, 董亞, 童金武, 等. 基于5階球面最簡相徑的改進(jìn)型容積卡爾曼濾波在SINS/DVL組合導(dǎo)航中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報(bào), 2017, 25(3): 343-348.Xu Xiao-su, Dong Ya, Tong Jin-wu, et al. Improved Fifth- degree Spherical Simplex Sadial Cubature Kalman Filter in SINS/DVL Integrated Navigation[J]. Journal of Chinese Inertial Technology, 2017, 25(3): 343-348.

[6] 陳建華, 朱海, 王超, 等. 水下SINS/DVL緊組合導(dǎo)航算法[J]. 海軍工程大學(xué)學(xué)報(bào), 2017, 29(2): 108-112.Chen Jian-hua, Zhu Hai, Wang Chao, et al. Underwater SINS/DVL Tightly Integrated Navigation Algorithm[J]. Journal of Naval University of Engineering, 2017, 29(2): 108-112.

[7] 嚴(yán)濤, 王躍鋼, 楊波, 等. SINS/DR組合導(dǎo)航系統(tǒng)可觀測性研究[J]. 現(xiàn)代防御技術(shù), 2012, 40(3): 83-87.Yan Tao, Wang Yue-gang, Yang Bo, et al. Observability of SINS/DR Integrated Navigation System[J]. Modern Defe- nce Technology, 2012, 40(3): 83-87.

[8] Goshen-Meskin D, Bar-Itzhack I Y. Observability Analysis of Piece-wise Constant Systems. II. Application to Inertial Navigation In-flight Alignment[J]. IEEE Transac- tions on Aerospace and Electronic Systems, 1992, 28(4): 1068-1075.

[9] Wu Y X, Wu M P, Hu X P. Self-calibration for Land Navigation Using Inertial Sensors and Odometer: Observability Analysis[C]//AIAA Guidance, Navigation and Control Conference, Chicago, Illinois, USA: AIAA, 2010: 964- 969.

[10] 帥平, 陳定昌, 江涌. GPS/SINS組合導(dǎo)航系統(tǒng)狀態(tài)的可觀測度分析方法[J]. 宇航學(xué)報(bào), 2004, 12(2): 219-224．Shuai Ping, Chen Ding-chang, Jiang Yong. Observable Degree Analysis Method of Integrated GPS/SINS Navigation System[J]. Journal of Astronautics, 2004, 12(2): 219- 224.

[11] 楊曉霞, 黃一. 外場標(biāo)定條件下捷聯(lián)慣導(dǎo)系統(tǒng)誤差狀態(tài)可觀測性分析[J]. 中國慣性技術(shù)學(xué)報(bào), 2008, 16(6): 1-8.Yang Xiao-xia, Huang Yi. Observability Analysis for the Error States of SINS Under the Outer Field Conditions[J]. Journal of Chinese Inertial Technology, 2008, 16(6): 1-8.

[12] 孔星煒, 董景新, 吉慶昌, 等. 一種基于PWCS的慣導(dǎo)系統(tǒng)可觀測度分析方法[J]. 中國慣性技術(shù)學(xué)報(bào), 2011, 19(6): 631-636.Kong Xing-wei, Dong Jing-xin, Ji Qing-chang, et a1. INS Observability Degree Analysis Method Based on PWCS [J]. Journal of Chinese Inertial Technology, 2011, 19(6): 631-636.

[13] 馬艷紅, 胡軍. 基于SVD理論的可觀測度分析方法的幾個(gè)反例[J]. 中國慣性技術(shù)學(xué)報(bào), 2008, 16(4): 448-457.Ma Yan-hong, Hu Jun. Counterexamples for Degree of Observability Analysis Method Based on SVD Theory[J]. Journal of Chinese Inertial Technology, 2008, 16(4): 448- 457.

[14] 楊曉霞, 陰玉梅. 可觀測度的探討及其在捷聯(lián)慣導(dǎo)系統(tǒng)可觀測性分析中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報(bào), 2012, 20(4): 405-409.Yang Xiao-xia,Yin Yu-mei. Discussions on Observability and Its Applications in SINS[J]. Journal of Chinese Iner- tial Technology, 2012, 20(4): 405-409.

[15] Ham F M, Brown T G. Observability, Eigenvalues, and Kalman filtering[J]. IEEE Transactions on Aerospace and Electronic Systems, 1983, 19(2): 269-273.

[16] 章大勇. 激光雷達(dá)-慣性組合導(dǎo)航系統(tǒng)的一致性與最優(yōu)估計(jì)問題研究[D]. 長沙: 國防科學(xué)技術(shù)大學(xué), 2010.

The SINS/DVL Integrated Navigation Algorithm Based on Observable Degree Feedback

LI Zhi-wei1, YUE Wei2, GAO Sheng-feng2, CAI Peng2, ZHU Hai-rong2

(1. 91001thUnit, the People’s Liberation Army of China, Beijing 100080, China; 2. Navy Submarine Academy, Qingdao 266199, China)

Aiming at the problem that some system states in strap-down inertial navigation system(SINS)/Doppler velocity log(DVL) integrated navigation have significant influence on navigation accuracy but have relatively weak observability, a SINS/DVL integrated navigation algorithm based on observable degree feedback is proposed. The algorithm quantitatively analyzes the observability of each state parameter of the system, and constructs an adaptive factor based on observability feedback according to the fact that the observability of the system state parameters depends on maneuver states in order to increase the estimated proportion of state parameters when they have high observable degree, and so as to improve the adaptability and accuracy of the algorithm. The performance of the proposed algorithm is verified through lake trial, and the results show that the accumulated error of this observable degree feedback filtering algorithm is obviously smaller than that of the conventional Kalman filter algorithm, and the autonomous navigation time of the SINS/DVL integrated navigation can be prolonged under the same accuracy requirement.

unmanned undersea vehicle(UUV); integrated navigation; strap-down inertial navigation system(SINS); Doppler velocity log(DVL); observable degree

TJ630; U666.1

A

2096-3920(2020)02-0155-07

10.11993/j.issn.2096-3920.2020.02.006

2019-4-17;

2019-07-28.

李志煒(1979-), 男, 本科, 高工, 主要研究方向?yàn)閷?dǎo)航時(shí)頻技術(shù).

李志煒, 岳崴, 高勝峰, 等. 基于可觀測度反饋的SINS/DVL組合導(dǎo)航算法[J]. 水下無人系統(tǒng)學(xué)報(bào), 2020, 28(2): 155-161.

(責(zé)任編輯: 楊力軍)