基于單信標(biāo)測距輔助SINS的中層水域?qū)Ш剿惴?/h1>
2021-01-16 05:17:56劉湘衡王亞波劉賢俊
水下無人系統(tǒng)學(xué)報(bào) 2020年6期
劉湘衡, 王亞波, 劉賢俊
基于單信標(biāo)測距輔助SINS的中層水域?qū)Ш剿惴?/p>
劉湘衡1, 王亞波2, 劉賢俊3
(1. 海軍裝備部駐武漢地區(qū)第六軍事代表室, 湖北 武漢, 430000; 2. 武漢第二船舶設(shè)計(jì)研究所, 湖北 武漢, 430205;3. 東南大學(xué) 儀器科學(xué)與工程學(xué)院, 江蘇 南京, 210096)
針對(duì)全球?qū)Ш叫l(wèi)星系統(tǒng)和多普勒測速儀無法用于位于中層水域的深海潛水器的高精度導(dǎo)航定位問題, 文中提出一種經(jīng)濟(jì)可行的中層水域?qū)Ш剿惴?。該算法基于單程到達(dá)時(shí)間機(jī)制測量已知位置的信標(biāo)到深海潛水器的距離, 并基于捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)誤差傳播模型和單信標(biāo)測距模型設(shè)計(jì)擴(kuò)展卡爾曼濾波器進(jìn)行信息融合, 從而實(shí)現(xiàn)利用單信標(biāo)測距信息輔助SINS以抑制其誤差發(fā)散。仿真結(jié)果表明, 基于距離輔助SINS的中層水域?qū)Ш剿惴軌颢@取誤差不隨時(shí)間累積的定位結(jié)果。此外, 文中從解析可觀測性的角度, 對(duì)深海潛水器的靜止和圓周運(yùn)動(dòng)軌跡進(jìn)行了精度分析, 且理論分析與仿真結(jié)果吻合。
深海潛水器; 中層水域?qū)Ш? 單信標(biāo)測距; 捷聯(lián)慣性導(dǎo)航系統(tǒng); 擴(kuò)展卡爾曼濾波器
0 引言
現(xiàn)有中層水域?qū)Ш椒椒ㄖ饕☉T性導(dǎo)航系統(tǒng)和聲學(xué)定位[4,8]。SINS是一種基于積分工作模式的導(dǎo)航系統(tǒng), 具有可靠性高、完全自主、數(shù)據(jù)輸出率高和導(dǎo)航信息豐富等優(yōu)點(diǎn), 目前已廣泛應(yīng)用于水下潛水器的導(dǎo)航、制導(dǎo)與控制。但其誤差隨時(shí)間累積, 在海洋環(huán)境中通常需要水下聲學(xué)定位系統(tǒng)來修正SINS的位置誤差累積[8]。
根據(jù)可用輔助導(dǎo)航信息源的不同, 現(xiàn)有中層水域慣性組合導(dǎo)航系統(tǒng)主要包括聲學(xué)輔助SINS和速度輔助SINS。速度輔助SINS主要包括模型輔助、聲學(xué)多普勒流速剖面儀(acoustic Doppler current profiler, ADCP)輔助和流輔助等[4, 6-7]。文獻(xiàn)[4]利用搭載的海流估計(jì)模型和ADCP測量構(gòu)建一套海流輔助慣性導(dǎo)航框架。文獻(xiàn)[6]基于下潛過程中海流速度短期保持不變的假設(shè), 提出一種基于ADCP的定位方法, 然而ADCP與SINS的融合有待進(jìn)一步考察。文獻(xiàn)[7]提出一種模型輔助慣性導(dǎo)航方案, 該方案的定位精度與洋流速度估計(jì)和水下載體的運(yùn)動(dòng)學(xué)/動(dòng)力學(xué)模型精度有關(guān), 且需要事先試驗(yàn)精確獲取相關(guān)模型參數(shù)。這些技術(shù)在一定程度上能夠有效地提高中層水域?qū)Ш叫阅? 卻存在一定的使用先決條件且僅僅只能抑制SINS誤差發(fā)散的速度。
對(duì)于聲學(xué)輔助SINS, 主要包括超短基線(ult- ra short base line, USBL)[9-10]、長基線(long base line, LBL)[11]和單信標(biāo)導(dǎo)航[12], 其中SINS/USBL已經(jīng)廣泛應(yīng)用于深海潛水器的導(dǎo)航定位。盡管基于聲傳播的LBL系統(tǒng)或USBL系統(tǒng)都可以提供絕對(duì)的位置信息, 仍不可避免地存在難以部署或者價(jià)格昂貴等問題[6]。與USBL和LBL相比, 單信標(biāo)導(dǎo)航由于具有減少部署復(fù)雜性和硬件復(fù)雜性的特點(diǎn), 近年來得到越來越多的關(guān)注[13-15]。現(xiàn)有的單信標(biāo)導(dǎo)航技術(shù)主要是通過設(shè)計(jì)直接卡爾曼濾波來融合姿態(tài)傳感器、DVL測速信息和單個(gè)測距信息來實(shí)現(xiàn)[13-14], 并未考慮地球曲率和地球自轉(zhuǎn), 且仍然存在DVL在中層水域不可用以及洋流速度無法獲取或估計(jì)的問題。文中針對(duì)深海潛水器下潛/上浮和中層水域巡航過程中的導(dǎo)航定位問題, 提出了一種基于單信標(biāo)距離輔助SINS的中層水域?qū)Ш椒桨? 可提供不隨時(shí)間累積的定位結(jié)果。該方案利用單程到達(dá)時(shí)間計(jì)算問答機(jī)到應(yīng)答器的距離, 結(jié)合測得的深度信息計(jì)算出信標(biāo)(位置已知)與潛水器之間的水平距離, 通過擴(kuò)展卡爾曼濾波器(extended Kalman filter, EKF)來融合水平距離信息與SINS, 仿真驗(yàn)證了其有效性。
1 SINS機(jī)械編排與誤差傳播方程
作為一種自主導(dǎo)航系統(tǒng), SINS利用慣性測量單元(inertial measurement unit, IMU)測量載體的六自由度運(yùn)動(dòng), 對(duì)IMU測量的角速率信息和比力信息積分計(jì)算得到載體的姿態(tài)、速度和位置信息。
其中
已知初始時(shí)刻的姿態(tài)、速度和位置, 通過對(duì)導(dǎo)航解算的微分方程(1)~(3)進(jìn)行數(shù)值積分, 實(shí)時(shí)遞歸計(jì)算所得當(dāng)前時(shí)刻的姿態(tài)、速度和位置。由于初始對(duì)準(zhǔn)誤差(例如失準(zhǔn)角)、傳感器誤差(例如陀螺儀和加速度計(jì)偏置)以及導(dǎo)航算法誤差(例如圓錐誤差)的存在, 方程(1)~(3)中姿態(tài)、速度和位置的計(jì)算值跟真實(shí)值之間存在誤差, 采用擾動(dòng)法推導(dǎo)出SINS誤差傳播方程
2 單信標(biāo)測距
水下信標(biāo)可分為移動(dòng)信標(biāo)和固定信標(biāo)2類。聲學(xué)定位系統(tǒng)的定位精度通常與載體及信標(biāo)之間的距離有關(guān)-, 且信標(biāo)的作用距離是有限的。移動(dòng)信標(biāo)相較于固定信標(biāo)而言, 大大拓寬了單信標(biāo)測距的作用范圍和應(yīng)用場景, 兩者在原理機(jī)制上并無差別。對(duì)于固定信標(biāo)而言, 需要事先布放在調(diào)查水域并精確測量其大地坐標(biāo)。文中描述的單信標(biāo)測距是利用安裝在水面支持母船底部的問答機(jī)-, 即移動(dòng)信標(biāo)來實(shí)現(xiàn)的。水面支持母船和深海潛水器上安裝同步時(shí)間系統(tǒng), 問答機(jī)以固定頻率發(fā)出信號(hào), 通過計(jì)算應(yīng)答器接受到問詢信號(hào)的單程到達(dá)時(shí)間, 來計(jì)算水面支持母船與深海潛水器之間的距離, 如圖1所示。
圖1 單信標(biāo)測距示意圖
式中: 深度值z(或高度值)通過壓力傳感器精確獲得; 距離, c為聲波在水下的傳播速度, 為傳播時(shí)間差, 故水平距離為
整理式(14)和式(15), 得
3 距離輔助SINS組合導(dǎo)航
選取失準(zhǔn)角、速度誤差、位置誤差、陀螺常值零偏和加速度計(jì)常值零偏為狀態(tài)變量(15維), 水平距離和高度差為量測量(2維)
則系統(tǒng)連續(xù)時(shí)間狀態(tài)方程和量測方程為
將式(18)轉(zhuǎn)換成離散形式
基于式(19)~(21)可構(gòu)成距離輔助SINS的中層水域?qū)Ш剿惴ā?/p>
4 仿真結(jié)果及分析
4.1 仿真條件設(shè)置
圖3 深海潛水器仿真軌跡
4.2 仿真結(jié)果及分析
圖4 靜止時(shí)不同方案輸出水平位置對(duì)比曲線
圖5 圓周運(yùn)動(dòng)時(shí)不同方案輸出水平位置對(duì)比曲線
為進(jìn)一步直觀評(píng)價(jià)定位精度, 將定位誤差轉(zhuǎn)換為以m為單位的位置誤差
圖6 靜止時(shí)水平位置誤差曲線
圖7 圓周運(yùn)動(dòng)時(shí)水平位置誤差曲線
距離約束使得定位精度
此外, 有
式(23)和式(24)代表的理論分析結(jié)果與圖6中的仿真結(jié)果吻合。
圖8 靜止水平位置誤差分析示意圖
5 結(jié)束語
文中針對(duì)中層水域可用導(dǎo)航傳感器有限的問題, 提出一種基于距離輔助SINS的中層水域?qū)Ш椒椒?。該方法不需要使用昂貴的USBL水聲定位系統(tǒng)(需要精確校準(zhǔn))和LBL水聲定位系統(tǒng)(校準(zhǔn)復(fù)雜、布放船時(shí)長), 通過測量信標(biāo)到深海潛水器之間的距離, 并設(shè)計(jì)EKF來實(shí)現(xiàn)距離輔助SINS的組合導(dǎo)航方案, 具有作用范圍遠(yuǎn)、實(shí)時(shí)性強(qiáng)、信息融合算法簡單等優(yōu)點(diǎn)。仿真結(jié)果表明,該方法可有效修正SINS的經(jīng)緯度誤差。解析可觀測性分析可以幫助設(shè)計(jì)合適的機(jī)動(dòng)條件以獲取更高的定位精度。在后續(xù)研究中, 會(huì)進(jìn)一步考察深海潛水器與移動(dòng)信標(biāo)同時(shí)機(jī)動(dòng)以及不同機(jī)動(dòng)組合條件下, 基于距離輔助SINS組合導(dǎo)航系統(tǒng)的可觀測性。
[1] 張同偉, 唐嘉陵, 楊繼超, 等. 4500 m以深作業(yè)型載人航行器[J]. 船舶工程, 2017, 39(6): 77-83.
Zhang Tong-wei, Tang Jia-ling, Yang Ji-chao, et al. Deep Operational Manned Submersibles＞4 500 m[J]. Ship Engineering, 2017, 39(6): 77-83.
[2] Hekinian R. Sea Floor Exploration Scientific Adventures Diving into the Abyss[M]. New York: Springer, 2014.
[3] Vedachalam N, Ramadass G A, Atmanand M A. Reiability Centered Modeling for Development of Deep Water Human Occupied Vehicles[J]. Applied Ocean Research, 2014, 46: 131-143.
[4] Song Z, Mohseni K. Long-Term Inertial Navigation Aided by Dynamics of Flow Field Features[J]. IEEE Journal of Oceanic Engineering, 2018, 43(4): 940-954.
[5] Paull L, Saeedi S, Seto M , et al. AUV Navigation and Localization: A Review[J]. IEEE Journal of Oceanic Engineering, 2014, 39(1): 131-149.
[6] Medagoda L, Williams S B, Pizarro O, et al. Mid-water Current Aided Localization for Autonomous Underwater Vehicles[J]. Autonomous Robots, 2016, 40(7): 1207-1227.
[7] Hegrenaes O, Hallingstad O. Model-Aided INS With Sea Current Estimation for Robust Underwater Navigation[J]. IEEE Journal of Oceanic Engineering, 2011, 36(2): 316-337.
[8] Lee P M, Jun B H. Pseudo Long Base Line Navigation Algorithm for Underwater Vehicles with Inertial Sensors and Two Acoustic Range Measurements[J]. Ocean Engineering, 2007, 34(3-4): 416-425.
[9] Morgado M, Oliveira P, Silvestre C. Tightly Coupled Ultrashort Baseline and Inertial Navigation System for Underwater Vehicles: An Experimental Validation[J]. Journal of Field Robotics, 2013, 30(1): 142-170.
[10] Wang J, Zhang T, Jin B, et al. Student’s t-Based Robust Kalman Filter for a SINS/USBL Integration Navigation Strategy[J]. IEEE Sensors Journal, 2020, 20(10): 5540- 5553.
[11] Zhan T, Shi H, Chen L, et al. AUV Positioning Method Based on Tightly Coupled SINS/LBL for Underwater Acoustic Multipath Propagation[J]. Sensors, 2016, 16(3): 357.
[12] Lee P M, Jun B H, Kim K, et al. Simulation of an Inertial Acoustic Navigation System With Range Aiding for an Autonomous Underwater Vehicle[J]. IEEE Journal of Oceanic Engineering, 2007, 32(2): 327-345.
[13] 劉健, 冀大雄. 用固定單信標(biāo)修正水下機(jī)器人導(dǎo)航誤差[J]. 控制與決策, 2010, 25(9): 1354-1358.
Liu Jian, Ji Da-xiong. Navigation Error Amendment for Underwater Robot Using Single Beacon[J]. Control and Decision, 2010, 25(9): 1354-1358
[14] 曹方方, 劉衛(wèi)東, 李娟麗. 基于EKF的UUV單信標(biāo)水聲導(dǎo)航定位方法研究[J]. 計(jì)算機(jī)測量與控制, 2011, 19(9): 2234-2236.
Cao Fang-fang, Liu Wei-dong, Li Juan-li. Navigating and Positioning Based on EKF for Unmanned Underwater Vehicle from A Single Beacon[J]. Computer Measurement & Control, 2011, 19(9): 2234-2236.
[15] Mcphail S D, Pebody M. Range-Only Positioning of a Deep-Diving Autonomous Underwater Vehicle From a Surface Ship[J]. IEEE Journal of Oceanic Engineering, 2009, 34(4): 669-677.
[16] 劉錫祥, 程向紅.捷聯(lián)式慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)理論與方法[M].北京: 科學(xué)出版社, 2020.
Midwater NavigationAlgorithm Based on Single-Beacon Ranging-Aided SINS
LIU Xiang-heng1, WANG Ya-bo2, LIU Xian-jun3
(1. The Sixth Military Representative Office of the Naval Equipment Department in Wuhan, Wuhan 430000, China; 2. Wuhan Second Ship Design and Research Institute, Wuhan 430205, China; 3. School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China)
Global navigation satellite system and Doppler velocity log cannot be used for deep-diving undersea vehicles in midwater. Therefore, this study proposes an economically feasible midwater navigation algorithm to solve this problem. The algorithm utilizes the one-way arrival time mechanism to measure the distance from a beacon of a known position to a deep-diving undersea vehicle. To suppress error divergence of the strapdown inertial navigation system(SINS), an extended Kalman filter based on SINS error propagation and a single-beacon ranging model is designed to execute information fusion for single-beacon ranging-aided SINS. Simulation results show that the midwater navigation algorithm based on ranging-aided SINS can obtain localization results whose errors do not accumulate over time. In addition, for analytical observability, this study analyzes the accuracy of static and circular motion trajectories, where the theoretical analysis is shown to be consistent with the simulation results.
deep-diving undersea vehicle; midwater navigation; single-beacon ranging; strapdown inertial navigation system(SINS); extended Kalman filter
劉湘衡, 王亞波, 劉賢俊. 基于單信標(biāo)測距輔助SINS的中層水域?qū)Ш剿惴╗J]. 水下無人系統(tǒng)學(xué)報(bào), 2020, 28(6): 670-676.
U674.941; TN967.2;
A
2096-3920(2020)06-0670-007
10.11993/j.issn.2096-3920.2020.06.012
2020-07-22;
2020-10-21.
劉湘衡(1987-), 男, 碩士, 主要研究方向?yàn)榇氨Ｕ?
(責(zé)任編輯: 楊力軍)
劉湘衡, 王亞波, 劉賢俊
基于單信標(biāo)測距輔助SINS的中層水域?qū)Ш剿惴?/p>
劉湘衡1, 王亞波2, 劉賢俊3
(1. 海軍裝備部駐武漢地區(qū)第六軍事代表室, 湖北 武漢, 430000; 2. 武漢第二船舶設(shè)計(jì)研究所, 湖北 武漢, 430205;3. 東南大學(xué) 儀器科學(xué)與工程學(xué)院, 江蘇 南京, 210096)
針對(duì)全球?qū)Ш叫l(wèi)星系統(tǒng)和多普勒測速儀無法用于位于中層水域的深海潛水器的高精度導(dǎo)航定位問題, 文中提出一種經(jīng)濟(jì)可行的中層水域?qū)Ш剿惴?。該算法基于單程到達(dá)時(shí)間機(jī)制測量已知位置的信標(biāo)到深海潛水器的距離, 并基于捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)誤差傳播模型和單信標(biāo)測距模型設(shè)計(jì)擴(kuò)展卡爾曼濾波器進(jìn)行信息融合, 從而實(shí)現(xiàn)利用單信標(biāo)測距信息輔助SINS以抑制其誤差發(fā)散。仿真結(jié)果表明, 基于距離輔助SINS的中層水域?qū)Ш剿惴軌颢@取誤差不隨時(shí)間累積的定位結(jié)果。此外, 文中從解析可觀測性的角度, 對(duì)深海潛水器的靜止和圓周運(yùn)動(dòng)軌跡進(jìn)行了精度分析, 且理論分析與仿真結(jié)果吻合。
深海潛水器; 中層水域?qū)Ш? 單信標(biāo)測距; 捷聯(lián)慣性導(dǎo)航系統(tǒng); 擴(kuò)展卡爾曼濾波器
0 引言
現(xiàn)有中層水域?qū)Ш椒椒ㄖ饕☉T性導(dǎo)航系統(tǒng)和聲學(xué)定位[4,8]。SINS是一種基于積分工作模式的導(dǎo)航系統(tǒng), 具有可靠性高、完全自主、數(shù)據(jù)輸出率高和導(dǎo)航信息豐富等優(yōu)點(diǎn), 目前已廣泛應(yīng)用于水下潛水器的導(dǎo)航、制導(dǎo)與控制。但其誤差隨時(shí)間累積, 在海洋環(huán)境中通常需要水下聲學(xué)定位系統(tǒng)來修正SINS的位置誤差累積[8]。
根據(jù)可用輔助導(dǎo)航信息源的不同, 現(xiàn)有中層水域慣性組合導(dǎo)航系統(tǒng)主要包括聲學(xué)輔助SINS和速度輔助SINS。速度輔助SINS主要包括模型輔助、聲學(xué)多普勒流速剖面儀(acoustic Doppler current profiler, ADCP)輔助和流輔助等[4, 6-7]。文獻(xiàn)[4]利用搭載的海流估計(jì)模型和ADCP測量構(gòu)建一套海流輔助慣性導(dǎo)航框架。文獻(xiàn)[6]基于下潛過程中海流速度短期保持不變的假設(shè), 提出一種基于ADCP的定位方法, 然而ADCP與SINS的融合有待進(jìn)一步考察。文獻(xiàn)[7]提出一種模型輔助慣性導(dǎo)航方案, 該方案的定位精度與洋流速度估計(jì)和水下載體的運(yùn)動(dòng)學(xué)/動(dòng)力學(xué)模型精度有關(guān), 且需要事先試驗(yàn)精確獲取相關(guān)模型參數(shù)。這些技術(shù)在一定程度上能夠有效地提高中層水域?qū)Ш叫阅? 卻存在一定的使用先決條件且僅僅只能抑制SINS誤差發(fā)散的速度。
對(duì)于聲學(xué)輔助SINS, 主要包括超短基線(ult- ra short base line, USBL)[9-10]、長基線(long base line, LBL)[11]和單信標(biāo)導(dǎo)航[12], 其中SINS/USBL已經(jīng)廣泛應(yīng)用于深海潛水器的導(dǎo)航定位。盡管基于聲傳播的LBL系統(tǒng)或USBL系統(tǒng)都可以提供絕對(duì)的位置信息, 仍不可避免地存在難以部署或者價(jià)格昂貴等問題[6]。與USBL和LBL相比, 單信標(biāo)導(dǎo)航由于具有減少部署復(fù)雜性和硬件復(fù)雜性的特點(diǎn), 近年來得到越來越多的關(guān)注[13-15]。現(xiàn)有的單信標(biāo)導(dǎo)航技術(shù)主要是通過設(shè)計(jì)直接卡爾曼濾波來融合姿態(tài)傳感器、DVL測速信息和單個(gè)測距信息來實(shí)現(xiàn)[13-14], 并未考慮地球曲率和地球自轉(zhuǎn), 且仍然存在DVL在中層水域不可用以及洋流速度無法獲取或估計(jì)的問題。文中針對(duì)深海潛水器下潛/上浮和中層水域巡航過程中的導(dǎo)航定位問題, 提出了一種基于單信標(biāo)距離輔助SINS的中層水域?qū)Ш椒桨? 可提供不隨時(shí)間累積的定位結(jié)果。該方案利用單程到達(dá)時(shí)間計(jì)算問答機(jī)到應(yīng)答器的距離, 結(jié)合測得的深度信息計(jì)算出信標(biāo)(位置已知)與潛水器之間的水平距離, 通過擴(kuò)展卡爾曼濾波器(extended Kalman filter, EKF)來融合水平距離信息與SINS, 仿真驗(yàn)證了其有效性。
1 SINS機(jī)械編排與誤差傳播方程
作為一種自主導(dǎo)航系統(tǒng), SINS利用慣性測量單元(inertial measurement unit, IMU)測量載體的六自由度運(yùn)動(dòng), 對(duì)IMU測量的角速率信息和比力信息積分計(jì)算得到載體的姿態(tài)、速度和位置信息。
其中
已知初始時(shí)刻的姿態(tài)、速度和位置, 通過對(duì)導(dǎo)航解算的微分方程(1)~(3)進(jìn)行數(shù)值積分, 實(shí)時(shí)遞歸計(jì)算所得當(dāng)前時(shí)刻的姿態(tài)、速度和位置。由于初始對(duì)準(zhǔn)誤差(例如失準(zhǔn)角)、傳感器誤差(例如陀螺儀和加速度計(jì)偏置)以及導(dǎo)航算法誤差(例如圓錐誤差)的存在, 方程(1)~(3)中姿態(tài)、速度和位置的計(jì)算值跟真實(shí)值之間存在誤差, 采用擾動(dòng)法推導(dǎo)出SINS誤差傳播方程
2 單信標(biāo)測距
水下信標(biāo)可分為移動(dòng)信標(biāo)和固定信標(biāo)2類。聲學(xué)定位系統(tǒng)的定位精度通常與載體及信標(biāo)之間的距離有關(guān)-, 且信標(biāo)的作用距離是有限的。移動(dòng)信標(biāo)相較于固定信標(biāo)而言, 大大拓寬了單信標(biāo)測距的作用范圍和應(yīng)用場景, 兩者在原理機(jī)制上并無差別。對(duì)于固定信標(biāo)而言, 需要事先布放在調(diào)查水域并精確測量其大地坐標(biāo)。文中描述的單信標(biāo)測距是利用安裝在水面支持母船底部的問答機(jī)-, 即移動(dòng)信標(biāo)來實(shí)現(xiàn)的。水面支持母船和深海潛水器上安裝同步時(shí)間系統(tǒng), 問答機(jī)以固定頻率發(fā)出信號(hào), 通過計(jì)算應(yīng)答器接受到問詢信號(hào)的單程到達(dá)時(shí)間, 來計(jì)算水面支持母船與深海潛水器之間的距離, 如圖1所示。
圖1 單信標(biāo)測距示意圖
式中: 深度值z(或高度值)通過壓力傳感器精確獲得; 距離, c為聲波在水下的傳播速度, 為傳播時(shí)間差, 故水平距離為
整理式(14)和式(15), 得
3 距離輔助SINS組合導(dǎo)航
選取失準(zhǔn)角、速度誤差、位置誤差、陀螺常值零偏和加速度計(jì)常值零偏為狀態(tài)變量(15維), 水平距離和高度差為量測量(2維)
則系統(tǒng)連續(xù)時(shí)間狀態(tài)方程和量測方程為
將式(18)轉(zhuǎn)換成離散形式
基于式(19)~(21)可構(gòu)成距離輔助SINS的中層水域?qū)Ш剿惴ā?/p>
4 仿真結(jié)果及分析
4.1 仿真條件設(shè)置
圖3 深海潛水器仿真軌跡
4.2 仿真結(jié)果及分析
圖4 靜止時(shí)不同方案輸出水平位置對(duì)比曲線
圖5 圓周運(yùn)動(dòng)時(shí)不同方案輸出水平位置對(duì)比曲線
為進(jìn)一步直觀評(píng)價(jià)定位精度, 將定位誤差轉(zhuǎn)換為以m為單位的位置誤差
圖6 靜止時(shí)水平位置誤差曲線
圖7 圓周運(yùn)動(dòng)時(shí)水平位置誤差曲線
距離約束使得定位精度
此外, 有
式(23)和式(24)代表的理論分析結(jié)果與圖6中的仿真結(jié)果吻合。
圖8 靜止水平位置誤差分析示意圖
5 結(jié)束語
文中針對(duì)中層水域可用導(dǎo)航傳感器有限的問題, 提出一種基于距離輔助SINS的中層水域?qū)Ш椒椒?。該方法不需要使用昂貴的USBL水聲定位系統(tǒng)(需要精確校準(zhǔn))和LBL水聲定位系統(tǒng)(校準(zhǔn)復(fù)雜、布放船時(shí)長), 通過測量信標(biāo)到深海潛水器之間的距離, 并設(shè)計(jì)EKF來實(shí)現(xiàn)距離輔助SINS的組合導(dǎo)航方案, 具有作用范圍遠(yuǎn)、實(shí)時(shí)性強(qiáng)、信息融合算法簡單等優(yōu)點(diǎn)。仿真結(jié)果表明,該方法可有效修正SINS的經(jīng)緯度誤差。解析可觀測性分析可以幫助設(shè)計(jì)合適的機(jī)動(dòng)條件以獲取更高的定位精度。在后續(xù)研究中, 會(huì)進(jìn)一步考察深海潛水器與移動(dòng)信標(biāo)同時(shí)機(jī)動(dòng)以及不同機(jī)動(dòng)組合條件下, 基于距離輔助SINS組合導(dǎo)航系統(tǒng)的可觀測性。
[1] 張同偉, 唐嘉陵, 楊繼超, 等. 4500 m以深作業(yè)型載人航行器[J]. 船舶工程, 2017, 39(6): 77-83.
Zhang Tong-wei, Tang Jia-ling, Yang Ji-chao, et al. Deep Operational Manned Submersibles＞4 500 m[J]. Ship Engineering, 2017, 39(6): 77-83.
[2] Hekinian R. Sea Floor Exploration Scientific Adventures Diving into the Abyss[M]. New York: Springer, 2014.
[3] Vedachalam N, Ramadass G A, Atmanand M A. Reiability Centered Modeling for Development of Deep Water Human Occupied Vehicles[J]. Applied Ocean Research, 2014, 46: 131-143.
[4] Song Z, Mohseni K. Long-Term Inertial Navigation Aided by Dynamics of Flow Field Features[J]. IEEE Journal of Oceanic Engineering, 2018, 43(4): 940-954.
[5] Paull L, Saeedi S, Seto M , et al. AUV Navigation and Localization: A Review[J]. IEEE Journal of Oceanic Engineering, 2014, 39(1): 131-149.
[6] Medagoda L, Williams S B, Pizarro O, et al. Mid-water Current Aided Localization for Autonomous Underwater Vehicles[J]. Autonomous Robots, 2016, 40(7): 1207-1227.
[7] Hegrenaes O, Hallingstad O. Model-Aided INS With Sea Current Estimation for Robust Underwater Navigation[J]. IEEE Journal of Oceanic Engineering, 2011, 36(2): 316-337.
[8] Lee P M, Jun B H. Pseudo Long Base Line Navigation Algorithm for Underwater Vehicles with Inertial Sensors and Two Acoustic Range Measurements[J]. Ocean Engineering, 2007, 34(3-4): 416-425.
[9] Morgado M, Oliveira P, Silvestre C. Tightly Coupled Ultrashort Baseline and Inertial Navigation System for Underwater Vehicles: An Experimental Validation[J]. Journal of Field Robotics, 2013, 30(1): 142-170.
[10] Wang J, Zhang T, Jin B, et al. Student’s t-Based Robust Kalman Filter for a SINS/USBL Integration Navigation Strategy[J]. IEEE Sensors Journal, 2020, 20(10): 5540- 5553.
[11] Zhan T, Shi H, Chen L, et al. AUV Positioning Method Based on Tightly Coupled SINS/LBL for Underwater Acoustic Multipath Propagation[J]. Sensors, 2016, 16(3): 357.
[12] Lee P M, Jun B H, Kim K, et al. Simulation of an Inertial Acoustic Navigation System With Range Aiding for an Autonomous Underwater Vehicle[J]. IEEE Journal of Oceanic Engineering, 2007, 32(2): 327-345.
[13] 劉健, 冀大雄. 用固定單信標(biāo)修正水下機(jī)器人導(dǎo)航誤差[J]. 控制與決策, 2010, 25(9): 1354-1358.
Liu Jian, Ji Da-xiong. Navigation Error Amendment for Underwater Robot Using Single Beacon[J]. Control and Decision, 2010, 25(9): 1354-1358
[14] 曹方方, 劉衛(wèi)東, 李娟麗. 基于EKF的UUV單信標(biāo)水聲導(dǎo)航定位方法研究[J]. 計(jì)算機(jī)測量與控制, 2011, 19(9): 2234-2236.
Cao Fang-fang, Liu Wei-dong, Li Juan-li. Navigating and Positioning Based on EKF for Unmanned Underwater Vehicle from A Single Beacon[J]. Computer Measurement & Control, 2011, 19(9): 2234-2236.
[15] Mcphail S D, Pebody M. Range-Only Positioning of a Deep-Diving Autonomous Underwater Vehicle From a Surface Ship[J]. IEEE Journal of Oceanic Engineering, 2009, 34(4): 669-677.
[16] 劉錫祥, 程向紅.捷聯(lián)式慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)理論與方法[M].北京: 科學(xué)出版社, 2020.
Midwater NavigationAlgorithm Based on Single-Beacon Ranging-Aided SINS
LIU Xiang-heng1, WANG Ya-bo2, LIU Xian-jun3
(1. The Sixth Military Representative Office of the Naval Equipment Department in Wuhan, Wuhan 430000, China; 2. Wuhan Second Ship Design and Research Institute, Wuhan 430205, China; 3. School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China)
Global navigation satellite system and Doppler velocity log cannot be used for deep-diving undersea vehicles in midwater. Therefore, this study proposes an economically feasible midwater navigation algorithm to solve this problem. The algorithm utilizes the one-way arrival time mechanism to measure the distance from a beacon of a known position to a deep-diving undersea vehicle. To suppress error divergence of the strapdown inertial navigation system(SINS), an extended Kalman filter based on SINS error propagation and a single-beacon ranging model is designed to execute information fusion for single-beacon ranging-aided SINS. Simulation results show that the midwater navigation algorithm based on ranging-aided SINS can obtain localization results whose errors do not accumulate over time. In addition, for analytical observability, this study analyzes the accuracy of static and circular motion trajectories, where the theoretical analysis is shown to be consistent with the simulation results.
deep-diving undersea vehicle; midwater navigation; single-beacon ranging; strapdown inertial navigation system(SINS); extended Kalman filter
劉湘衡, 王亞波, 劉賢俊. 基于單信標(biāo)測距輔助SINS的中層水域?qū)Ш剿惴╗J]. 水下無人系統(tǒng)學(xué)報(bào), 2020, 28(6): 670-676.
U674.941; TN967.2;
A
2096-3920(2020)06-0670-007
10.11993/j.issn.2096-3920.2020.06.012
2020-07-22;
2020-10-21.
劉湘衡(1987-), 男, 碩士, 主要研究方向?yàn)榇氨Ｕ?
(責(zé)任編輯: 楊力軍)
