基于逆向解算的領航AUV導航數據后處理方法

董權威, 岳才謙, 王奧博, 王亭亭

基于逆向解算的領航AUV導航數據后處理方法

董權威, 岳才謙, 王奧博, 王亭亭

(中國航天空氣動力技術研究院, 北京, 100074)

由于多自主水下航行器(AUV)協同導航系統在水下長時間工作時, 領航AUV無法接收外界信息, 其定位誤差會逐漸發散進而影響整個系統的定位性能。針對該問題, 文中提出一種基于捷聯慣導系統/多普勒計程儀(SINS/DVL)和SINS/DVL/全球定位系統(GPS)組合導航相結合的領航AUV導航數據后處理方法。領航AUV在水下采用SINS/DVL組合導航方式對導航數據進行正向解算, 并保存其在水下航行過程中產生的導航信息。領航AUV出水后接收GPS信號, 同時采用SINS/DVL與SINS/DVL/GPS 2種導航方式進行獨立濾波, 濾波結束后逆向解算出領航AUV在水下累計的定位誤差, 通過結合正向和逆向解算出的結果, 使領航AUV在水下累計的定位誤差得到有效修正, 進而提高多AUV系統的整體定位性能。最后通過仿真對該方法的可行性與有效性進行了驗證。該方法可用于海底地形掃描、海圖繪制與水下探測等領域, 可進一步提升多AUV系統采集的水下任務數據的位置精度, 提高數據的準確性與實用性。

自主水下航行器; 組合導航; 逆向解算; 后處理

0 引言

近年來, 在自主水下航行器(autonomous undersea vehicle, AUV)應用領域中, 多AUV協同導航系統已成為重點研究方向之一。多AUV協同作業能夠承擔單體AUV難以完成的復雜任務, 同時具有效率高、可靠性好的特點, 具有廣闊的應用前景。在多AUV協同系統中, 各AUV通過裝配水聲設備組成通信網絡, 使AUV間能夠進行數據信息交互, 從而實現AUV間導航數據的共享[1-2]。多AUV系統一般采用領航跟隨式協同導航方式, 且領航AUV中裝配有高精度導航設備與水聲通信設備, 跟隨AUV僅配有低精度的導航設備和水聲通信設備, 由于領航AUV配有高精度導航設備, 因此將其作為協同導航系統定位的基準, 即只需少量配有高精度導航設備的領航AUV幫助其他配有低精度導航設備的跟隨AUV完成高精度定位, 且跟隨AUV的數量理論上不受限制, 具有高精度、低成本的特點, 因此一直是協同導航研究的重點[3-5]。

主從式多AUV協同導航系統以領航AUV的定位信息為基準, 可在航行過程中時刻修正跟隨AUV累計的定位誤差[6-7]。但由于領航AUV在水下航行時不能接收外界信息, 經過長時間航行, 其定位精度勢必會發散, 增大導航數據的解算誤差, 從而會影響整個航行編隊的定位性能。文獻[3]針對單領航協同導航系統可觀測性弱的問題, 提出了一種基于雙領航AUV交替領航的多AUV協同導航方法, 通過在相鄰時刻, 跟隨AUV利用不同領航AUV的距離量測信息進行誤差的協同校正, 提高協同導航系統的可觀測性[8-9]。文獻[6]針對領航AUV無法借助外部有源信息修正自身定位誤差的問題, 提出一種基于距離量測的雙領航AUV間協同導航算法, 通過2個AUV進行相對機動, 以保證系統可觀測, 最終實現2個領航AUV定位誤差趨同。文獻[9]提出了一種基于導航數據反演的捷聯慣導系統(strap-down inertial navigation system, SINS)對準方法, 通過對導航數據執行逆向-正向解算, 降低了系統的對準時間。針對以上研究, 并參考文獻[10]和文獻[11]所采用的導航信息反演思想, 文中提出一種基于SINS/多普勒測速儀(Doppler velocity log, DVL)與SINS/DVL/全球定位系統(global positioning system, GPS)相結合的導航數據后處理方法, 該方法在水下采用SINS/DVL組合導航方式對導航數據進行正向濾波; 出水后, 領航AUV接收GPS定位信息, 采用SINS/DVL與SINS/DVL/GPS 2種組合導航方式并進行獨立濾波, 濾波結束后, 逆向解算得出系統的定位誤差, 通過結合正向和逆向解算的結果, 對領航AUV在水下累計的定位誤差進行修正。該方法應用于海圖繪制、海底勘探等領域, 可以有效修正領航AUV在水下累計的定位誤差, 提高其定位精度, 進而提升整個系統的定位性能。

1 基于SINS/DVL/GPS組合的領航AUV協同導航系統

1.1 協同導航系統濾波方法

一般情況下, SINS/DVL組合系統所采用的是間接濾波方式, 獲得SINS誤差狀態最優估計后, 通過反饋校正, 將速度誤差、位置誤差、失準角誤差與陀螺漂移反饋給系統, 利用慣導解算對反饋的信息進行誤差補償, 得到更加準確的導航信息。

通過結合SINS/DVL與SINS/DVL/GPS 2種組合方式, 可為領航AUV提供更高精度的導航信息。領航AUV在水下航行時, 采用SINS/DVL方式進行導航, 當其完成水下任務出水后接收GPS信息, 并采用SINS/DVL/GPS方式進行導航, 對自身的定位信息進行修正, 同時繼續采用SINS/DVL進行水面導航, 對速度信息進行更新與修正。通過接收的GPS信息, 對領航AUV出水時刻的定位誤差進行修正, 并進行逆向推導, 反演出領航AUV在水下累計的定位誤差, 進而對其在水下航行過程累計的誤差進行修正。該組合導航系統濾波原理框圖如圖1所示。

1.2 DVL誤差模型

DVL所采用的原理是多普勒效應, 多普勒效應是指沿發射源的發射點與接收源的接收點間的直線方向產生相對速度時, 發射與接收頻率不相同, 產生的頻率差為多普勒頻移。產生的相對速度與多普勒頻移成正比, 根據發射頻率和多普勒頻移就可求出這一相對速度[6-7], DVL的工作原理圖如圖2所示。

圖1 SINS/DVL/GPS組合導航系統濾波原理框圖

首先, 在不考慮DVL與INS安裝偏差的條件下, 在載體系上DVL測速誤差方程的矢量方程形式可以表示為

式中: 為載體系下DVL的測速誤差; 為DVL的速度; 為量測噪聲; 為刻度系數誤差, 一般情況下可以看作常值。

若采用SINS/DVL組合導航方式, 由于DVL的實際坐標系與載體系不完全重合, 所以需要對轉換矩陣進行估計, DVL下的坐標系與AUV下的坐標系間的速度關系可表示為

其中

2 領航AUV導航數據后處理算法

2.1 導航數據后處理算法設計思路

領航AUV導航數據的后處理采用逆向解算的方式進行處理, 通過對正向解算后的結果反向遞推, 得到逆向推導后的誤差, 最后結合正向與逆向解算后的結果, 消除正向解算過程發散的定位誤差, 使之趨于收斂。

多AUV協同導航系統在水下航行過程中, 采用SINS/DVL組合導航方式進行定位, 將導航設備與傳感器采集的信息進行保存, 并用于事后的分析和處理。通常情況下, 對于導航系統的狀態參數采用卡爾曼濾波進行估計。但是, 由于水下GPS信號缺失, 采樣的導航數據不包含GPS信息, 當AUV在水下長期運行后, 必然會引起導航系統定位誤差的發散, 此時如果將其用于逆向解算, 會導致整個多AUV系統的定位性能受到影響[8]。

圖3 領航AUV正向-逆向解算過程原理圖

水下航行時, 系統采用SINS/DVL組合方式導航; 出水后系統接收GPS信息, 由于此時的GPS信息含有噪聲干擾, 若用其對系統進行位置修正會帶來誤差。通過設計SINS/DVL與SINS/ DVL/GPS 2種濾波方式對導航數據信息進行后處理, 可使系統的定位精度得到進一步提高。對導航數據進行處理時, 2種濾波器獨立運行, 濾波結束后, 利用濾波結果對系統的位置進行修正。領航AUV的導航數據后處理算法如圖4所示。

圖4 導航數據后處理示意圖

將SINS與DVL的速度差和SINS與GPS的位置差作為量測輸入, 利用卡爾曼濾波器對導航參數進行估計, 并對估計出的導航參數進行校正后反饋給SINS, 最后利用逆向解算算法對系統在航行過程中保存的數據進行逆向解算, 得到逆向解算后的定位誤差。結合系統出水前后正向和逆向解算的定位誤差, 對系統出水前的位置誤差進行校正。利用這種后處理算法可以有效對系統在水下航行過程中產生的定位誤差進行修正, 從而提高系統整體的定位性能。

2.2 逆向導航算法

1) 姿態更新微分方程

式中

2) 速度更新微分方程

3) 位置更新微分方程

其中

通過上式的推導, 對獲取的數據進行記錄和逆向處理, 實現了從點到點的逆向解算。在正逆向解算過程中, 航行器的位置坐標、姿態矩陣和速度大小在同一時刻相同, 而速度方向相反。

2.3 卡爾曼濾波器設計

根據圖3所示, 領航AUV從點航行至點時, SINS/DVL組合濾波方式一直存在; 而在時刻系統出水后引入GPS信息, 系統增加了SINS/DVL/GPS組合導航方式, 2種濾波方式同時存在且獨立運行, 結合領航AUV的正向和逆向解算的數據信息, 對其在水下航行過程中的定位誤差進行修正。

2.3.1 SINS/DVL組合導航系統的濾波模型

其中

取系統狀態向量為

系統的狀態方程

已知捷聯慣導測得的速度為

將SINS測得的速度與DVL速度儀測得的速度在地理坐標系下的投影作差可得

2.3.2 SINS/DVL/GPS組合導航系統濾波模型

采用SINS/DVL/GPS組合導航方式, 系統的狀態方程與SINS/DVL組合導航狀態方程相同, 因此只研究系統的觀測方程。

系統的觀測方程為

根據離散系統的卡爾曼濾波方程

3 仿真驗證與分析

為驗證SINS/DVL/GPS組合導航對協同導航系統定位誤差精度的影響, 并驗證該方法的可行性及有效性, 文中通過對領航AUV的導航數據進行后處理, 并通過仿真對比分析了該方法對領航AUV定位精度的影響。

表1 領航AUV航行軌跡參數設置

圖5 領航AUV運動軌跡

SINS/DVL/GPS組合導航系統濾波過程中所需的初始條件如下:

圖6為領航AUV正向解算得到的經緯度誤差曲線, 利用SINS/DVL組合導航方式進行正向導航解算, 隨著時間的增長, 領航AUV的定位誤差也隨之增大。圖7為對領航AUV的導航信息進行逆向解算得到的經緯度誤差曲線, 系統出水后引入GPS信息, 修正系統的位置誤差, 同時利用SINS/DVL與SINS/DVL/GPS 2種導航方式進行獨立濾波, 對領航AUV的導航信息進行逆向解算處理, 可將正向解算過程結束時刻的定位誤差修正到近似為0, 然后定位誤差反向遞增。圖8為正向解算與逆向解算相結合的領航AUV的定位誤差曲線, 結合上述2種解算方式, 最終可將領航AUV在水下航行過程中積累的定位誤差進行修正, 使領航AUV的定位誤差得到有效收斂, 進而提升多AUV協同導航系統整體的定位性能。

圖6 領航AUV正向解算后的定位誤差

圖7 領航AUV逆向解算后的定位誤差

圖8 領航AUV正逆向結合解算后的定位誤差

4 結論

針對多AUV協同導航系統中領航AUV隨水下長時航行引起定位誤差發散的問題, 提出了一種基于SINS/DVL和SINS/DVL/GPS組合導航相結合的領航AUV導航數據后處理方法。通過對領航AUV在水上和水下航行過程建立不同的濾波模式, 水下采用SINS/DVL組合導航方式對導航數據進行正向解算, 出水后采用SINS/DVL與SINS/DVL/GPS結合的方式進行獨立濾波, 然后進行逆向遞推, 逆向解算出領航AUV在水下的定位誤差。最后結合正向與逆向解算結果, 對正向解算的誤差結果進行修正, 以提高領航AUV的定位誤差, 使得多AUV系統的定位性能得到提升, 并最終通過仿真驗證了該方法的可行性與有效性。該方法雖然對領航AUV的定位誤差有較為明顯的改善, 但仍存在如圖8所示的正向與逆向結合時的最佳交點選擇, 以及領航AUV出水后需要一定的GPS搜星時間, 無法立即獲取GPS信息等問題。因此對所涉及的領航AUV導航數據的后處理方法需要開展更多的研究。

[1] Bahr A, Leonard J J. Cooperative Localization for Auto- nomous Underwater Vehicle[J]. The International Journal of Robotics Research, 2009, 28(6): 714-728.

[2] Maczka D K, Gadre A S, Stilwell D J. Implementation of A Cooperative Navigation Algorithm on A Platoon of Autonomous Underwater Vehicles[C]//Oceans 2007. [S.l.]: IEEE, 2007.

[3] 高偉, 劉亞龍, 徐博, 等. 基于雙主交替領航的多AUV協同導航方法[J]. 哈爾濱工程大學學報, 2014, 35(6): 735-740.

Gao Wei, Liu Ya-long, Xu Bo, et al. Multiple AUV Cooperative Navigation Based on Two Leaders Alternated Navigation[J]. Journal of Harbin Engineering University, 2014, 35(6): 735-740.

[4] 黃玉龍, 張勇剛, 趙玉新. 自主水下航行器導航方法綜述[J]. 水下無人系統學報, 2019, 27(3): 232-253.

Huang Yu-long, Zhang Yong-gang, Zhao Yu-xin. Review of Autonomous Undersea Vehicle Navigation Methods[J]. Jou- rnal of Unmanned Undersea Systems, 2019, 27(3): 232-253.

[5] 張立川, 許少峰, 劉明雍, 等. 多無人水下航行器協同導航定位研究進展[J]. 高技術通訊, 2016, 26(5): 475-482.

Zhang Li-chuan, Xu Shao-feng, Liu Ming-yong, et al. Research Process of Multi-Unmanned Underwater Vehicle Cooperative and Positioning[J]. High Technology Letters, 2016, 26(5): 475-482.

[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] 曹忠義, 鄭翠娥, 張殿倫. 聲學多普勒速度儀安裝誤差校準方法[J]. 哈爾濱工程大學學報, 2013, 34(4): 434-439.Cao Zhong-yi, Zheng Cui-e, Zhang Dian-lun. Study on the Calibration of Installation Errors in An Acoustic Doppler Velocity Log[J]. Journal of Harbin Engineering University, 2013, 34(4): 434-439.

[8] 張福斌, 馬朋, 劉書強. 基于距離量測的雙領航AUV見協同導航算法[J]. 系統工程理論與實踐, 2016, 36(7): 1898-1904.Zhang Fu-bin, Ma Peng, Liu Shu-qiang. A Cooperative Na- vigation Algorithm for Two Leader AUVs Based on Range Measurements[J]. System Engineering-Theory & Practice, 2016, 36(7): 1898-1904.

[9] 馬平, 呂峰, 杜海蓮, 等. 多傳感器信息融合基本原理及應用[J]. 控制工程, 2006, 13(1): 48-53.Ma Ping, Lü Feng, Du Hai-lian, et al. Theory and Applica- tion of Multi-sensor Information Fusion[J]. Control Engineering of China, 2006, 13(1): 48-53.

[10] 孫進, 孫曉蘇, 劉義亭, 等. 基于逆向導航解算和數據融合的SINS傳遞對準方法[J]. 中國慣性技術學報, 2015, 12(6): 727-732.Sun Jin, Xu Xiao-su, Liu Yi-ting, et al. Transfer Alignment Method for SINS Based on Reverse Navigation Solution and Data Fusion[J]. Journal of China Inertial Technology, 2015, 12(6): 727-732.

[11] 嚴恭敏, 嚴衛生, 徐德民. 逆向導航算法及其在捷聯羅經動基座初始對準中的應用[C]//中國自動化學會控制理論專業委員會第二十七屆中國控制會議論文集. 昆明: 中國自動化學會控制理論專業委員會, 2008.

A Post-Processing Method of Navigation Data for Leader AUV Based on Backward Calculating

DONG Quan-wei, YUE Cai-qian, WANG Ao-bo, WANG Ting-ting

(China Academy of Aerospace and Aerodynamics, Beijing 100074, China)

Since the leader autonomous undersea vehicle(AUV) cannot receive the external information if the multi-AUV cooperative navigation system works under water for a long time and its localization error will gradually diverge and affect the localization performance of the overall system, a post-processing method based on integrated navigation of strap-down inertial navigation system(SINS)/Doppler velocity log(DVL) and SINS/DVL/global positioning system(GPS) is proposed. Adopting SINS/DVL integrated navigation, the leader AUV processes the navigation information forwardly and stores the navigation information sampled during the process when AUV maneuvers, and then it receives the GPS information after coming out of water and uses the two navigation methods of SINS/DVL and SINS/DVL/GPS for independent filtering. After filtering, the leader AUV’s localization error accumulated under water is processed inversely to correct the localization error of the leader AUV through forward and backward calculating, so as to improve the localization performance of the overall multi-AUV system. Moreover, the feasibility and validity of the proposed post-processing method is verified through simulation. This method can be applied to underwater terrain scanning, sea chart drawing and underwater exploration to further improve localization accuracy of the underwater data sampled by multi-AUV and enhance veracity and usefulness of the data.

autonomous undersea vehicle(AUV); integrated navigation; backward calculating; post-processing

TJ6; TN967.2

A

2096-3920(2020)04-0420-08

10.11993/j.issn.2096-3920.2020.04.010

2019-12-11;

2020-01-05.

董權威(1991-), 男, 碩士, 工程師, 主要從事水下航行器導航、控制技術等研究.

董權威, 岳才謙, 王奧博, 等. 基于逆向解算的領航AUV導航數據后處理方法[J]. 水下無人系統學報, 2020, 28(4): 420-427.

(責任編輯: 楊力軍)