基于可觀測度反饋的SINS/DVL組合導航算法

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

基于可觀測度反饋的SINS/DVL組合導航算法

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

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

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

無人水下航行器; 組合導航; 捷聯慣導; 多普勒計程儀; 可觀測度

0 引言

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

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

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

文中在文獻[15]的基礎上分析在卡爾曼濾波過程中誤差協方差陣對對應狀態分量的估計誤差或精度的影響, 提出了一種基于可觀測度反饋的SINS/DVL組合導航算法, 該方法可以有效解決部分狀態參數可觀測性弱導致的系統濾波精度較低的問題。

1 SINS/DVL組合導航模型

1.1 狀態方程

SINS/DVL組合導航系統各狀態變量分別選取為:

系統誤差狀態共22維, 如下

1.2 量測方程

根據上述誤差分析可知, DVL實際輸出速度

投影到導航坐標系, 可寫為

從而可得DVL速度誤差

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

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

2 基于可觀測度反饋的SINS/DVL組合導航算法

2.1 基于協方差陣的可觀測度分析

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

定義

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

推導可得

2.2 自適應調節引子計算步驟

在SINS/DVL組合導航系統中, 為了提高系統狀態的估計精度, 根據各狀態分量在不同時刻可觀測度的大小確定該時刻在狀態估計中占據的權重系數, 從而構造自適應調節因子, 以減小弱觀測度數據對整個濾波器的影響, 提高濾波算法估計精度。下面給出自適應調節因子的具體計算步驟。

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

5) 計算各狀態變量自適應調節因子

從而實現通過對不同時刻狀態變量的可觀測度信息來動態調整其增益的目的。

3 仿真結果與分析

為了驗證基于可觀測度反饋的SINS/DVL組合導航算法的效果, 進行仿真分析。仿真中對全球導航衛星系統定位(global navigation satellite system, GNSS)等傳感器參數進行了比較, 如表1所示。

表1 傳感器參數

圖1 UUV模擬航跡

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

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

圖5 DVL刻度系數誤差可觀測度及其估計值

Fig 6 Error of location

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

4 試驗數據分析

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

圖7 載體軌跡

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

圖8 定位誤差對比

綜上所述, 基于可觀測度反饋的SINS/DVL組合導航濾波算法, 通過引入含有系統狀態實時可觀測度信息的自適應調節因子, 實現了減小狀態變量的實時估計誤差, 進而提高SINS/DVL組合導航精度的目的。在相同的精度要求下, 可延長水下SINS/DVL組合導航自主航行的時間。

5 結束語

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

[1] 馮正平. 國外自治水下機器人發展現狀綜述[J]. 魚雷技術, 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] 徐曉蘇, 潘永飛, 鄒海軍. 基于自適應濾波的SINS/ DVL組合導航系統[J]. 華中科技大學學報(自然科學版), 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階球面最簡相徑的改進型容積卡爾曼濾波在SINS/DVL組合導航中的應用[J]. 中國慣性技術學報, 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緊組合導航算法[J]. 海軍工程大學學報, 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] 嚴濤, 王躍鋼, 楊波, 等. SINS/DR組合導航系統可觀測性研究[J]. 現代防御技術, 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組合導航系統狀態的可觀測度分析方法[J]. 宇航學報, 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] 楊曉霞, 黃一. 外場標定條件下捷聯慣導系統誤差狀態可觀測性分析[J]. 中國慣性技術學報, 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的慣導系統可觀測度分析方法[J]. 中國慣性技術學報, 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理論的可觀測度分析方法的幾個反例[J]. 中國慣性技術學報, 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] 楊曉霞, 陰玉梅. 可觀測度的探討及其在捷聯慣導系統可觀測性分析中的應用[J]. 中國慣性技術學報, 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]. 長沙: 國防科學技術大學, 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-), 男, 本科, 高工, 主要研究方向為導航時頻技術.

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

(責任編輯: 楊力軍)