李佳橦, 張 臣, 張宏欣

?

基于單信標(biāo)純方位測(cè)量的AUV水下定位方法

李佳橦, 張 臣, 張宏欣

(中國(guó)人民解放軍 91439部隊(duì), 遼寧 大連, 116041)

在信標(biāo)深度已知條件下, 提出了一種基于單信標(biāo)純方位信息測(cè)量的自主水下航行器(AUV)實(shí)時(shí)定位方法, 以解決單信標(biāo)AUV水下定位中遇到的問(wèn)題。不同于傳統(tǒng)基于單信標(biāo)測(cè)距信息方法, 文中從坐標(biāo)變換原理出發(fā), 構(gòu)建了2自由度(2-DOF)姿態(tài)的AUV運(yùn)動(dòng)模型, 推導(dǎo)了深度已知條件下純方位AUV定位的狀態(tài)空間模型, 在所建立模型的基礎(chǔ)上采用擴(kuò)展卡爾曼濾波器(EKF)進(jìn)行求解, 并針對(duì)EKF算法應(yīng)用給出了一種濾波器調(diào)參方法, 通過(guò)構(gòu)建仿真試驗(yàn)對(duì)所提出方法進(jìn)行了有效性驗(yàn)證, 并將其與現(xiàn)有方法進(jìn)行了對(duì)比分析。結(jié)果表明, 在該仿真條件下, 文中基于單信標(biāo)方位觀(guān)測(cè)模型的方法在濾波精度上要好于基于測(cè)距信息的方法, 且收斂性能優(yōu)于后者。

自主水下航行器; 定位方法; 單信標(biāo); 純方位測(cè)量; 擴(kuò)展卡爾曼濾波器

0 引言

自主水下航行器(autonomous undersea vehicle, AUV)已經(jīng)在軍民領(lǐng)域得到了廣泛應(yīng)用, 正向著高度自主化、長(zhǎng)航時(shí)化方向發(fā)展。精確自主定位是提高AUV自主能力的關(guān)鍵要素。受應(yīng)用成本的限制, 一般AUV難以搭載高精度的慣性導(dǎo)航系統(tǒng)(inertia navigation system, INS)。因此其自身航位推算能力較差, 誤差增長(zhǎng)過(guò)快, 一般需采用外部輔助傳感器獲得載體坐標(biāo)系相對(duì)于參考坐標(biāo)系的信息。由于GPS信息在水下不可用, 使得聲學(xué)定位方法成為事實(shí)上的水下導(dǎo)航輔助手段。

由于單聲信標(biāo)能夠有效降低系統(tǒng)部署和回收成本, 近年來(lái), 基于單信標(biāo)的水下定位方法得到廣泛的關(guān)注[1-3]。曹俊等[4-5]在AUV為直航的假設(shè)下, 基于單信標(biāo)測(cè)距信息, 采用離線(xiàn)迭代方法求解其初始位置進(jìn)行推算。張福斌等[6]基于單信標(biāo)測(cè)距信息, 利用Kalman濾波器將其與慣導(dǎo)/多普勒計(jì)程儀信息進(jìn)行融合, 提高了AUV的導(dǎo)航精度。曹方方等[7]及Vallicrosa等[8]分別基于單信標(biāo)測(cè)距信息, 在AUV自身偏航角度已知條件下, 利用濾波算法對(duì)AUV進(jìn)行實(shí)時(shí)定位。嚴(yán)衛(wèi)生等[9]進(jìn)一步就單信標(biāo)測(cè)距AUV定位可觀(guān)測(cè)性進(jìn)行了分析, 指出測(cè)距定位需要方位信息輔助才可滿(mǎn)足系統(tǒng)可觀(guān)測(cè)性條件。曹俊等[10]對(duì)單信標(biāo)測(cè)距誤差及其影響進(jìn)行了分析, 指出了測(cè)距誤差主要源于收發(fā)異步問(wèn)題。

根據(jù)已公開(kāi)的文獻(xiàn)資料可知, 單信標(biāo)AUV水下定位研究多是基于距離觀(guān)測(cè)信息進(jìn)行的, 這一方面是由于近場(chǎng)水聲測(cè)距技術(shù)精度普遍比較高[4], 另一方面是由于基于距離的觀(guān)測(cè)模型復(fù)雜程度及非線(xiàn)性度均低于角度觀(guān)測(cè)模型, 實(shí)際算法設(shè)計(jì)時(shí)的調(diào)參較為容易。但對(duì)于測(cè)角信息利用方面則研究較少, 而測(cè)角相對(duì)測(cè)距具有成本低、易實(shí)現(xiàn)和方便實(shí)施的優(yōu)勢(shì)。Matsebe等[11]借助即時(shí)定位構(gòu)圖技術(shù)框架(simultaneous localization and mapp- ing, SLAM), 研究了基于角度-距離信息的AUV- 信標(biāo)同步定位方法[12]。Becker等[13]受測(cè)角SLAM技術(shù)啟發(fā), 提出了基于方位信息的AUV-信標(biāo)同步定位方法, 但假設(shè)AUV載體是水平運(yùn)動(dòng)的。在上述研究基礎(chǔ)上, 文中提出一種在信標(biāo)深度已知條件下, 基于信標(biāo)純方位信息的AUV定位方法, 構(gòu)建了考慮2自由度(degree of freedom, DOF)姿態(tài)的AUV定位模型, 采用擴(kuò)展卡爾曼濾波器(ex- tended Kalman filter, EKF)進(jìn)行求解, 并針對(duì)估值濾波器的選擇及調(diào)參方法進(jìn)行了分析, 并通過(guò)仿真分析對(duì)所提出方法進(jìn)行了驗(yàn)證。

1 模型構(gòu)建

由于文中僅針對(duì)單信標(biāo)定位問(wèn)題, 為簡(jiǎn)化問(wèn)題表述, 不考慮AUV當(dāng)?shù)貙?dǎo)航平面相對(duì)于地心地固坐標(biāo)系的轉(zhuǎn)動(dòng), 故參考坐標(biāo)系可選為當(dāng)?shù)貣|(east)-北(north)-天(up)導(dǎo)航坐標(biāo)系(下文稱(chēng)系), 其中東向?yàn)檩S, 北向?yàn)檩S, 天向?yàn)檩S, 且坐標(biāo)原點(diǎn)位于海底。AUV與信標(biāo)的定位計(jì)算結(jié)果均為ENU坐標(biāo)。對(duì)于進(jìn)一步的自主導(dǎo)航誤差修正應(yīng)用, ENU坐標(biāo)可方便的向其他參考系進(jìn)行轉(zhuǎn)換[14]。

通過(guò)式(1)可得系中的信標(biāo)方位單位矢量

對(duì)于AUV運(yùn)動(dòng)建模, 考慮其低動(dòng)態(tài)剛體特性, 可假設(shè)加速度方向始終沿系軸(前向), 且與速度方向一致, 無(wú)橫傾及平移運(yùn)動(dòng), 則其離散時(shí)間運(yùn)動(dòng)模型

利用式(3)可將以上寫(xiě)為矩陣形式

1.1 狀態(tài)模型

在上述基礎(chǔ)上, 綜合其他變量得到狀態(tài)向量

其中, 狀態(tài)轉(zhuǎn)移矩陣為

1.2 觀(guān)測(cè)模型

觀(guān)測(cè)信息為信標(biāo)相對(duì)于AUV的俯仰角和方位角,時(shí)刻AUV測(cè)得的角度

根據(jù)式(2)~(5), 可得

根據(jù)上述建模過(guò)程可知, 狀態(tài)空間模型(12)和(18)分別為線(xiàn)性狀態(tài)方程和非線(xiàn)性觀(guān)測(cè)方程, 而根據(jù)式(6)~(10)可知, 其可觀(guān)測(cè)性等價(jià)于如下的非線(xiàn)性狀態(tài)方程和線(xiàn)性觀(guān)測(cè)方程

式(21)~(23)表明, 在給定觀(guān)測(cè)信息, 即深度和空間方位(俯仰角和偏航角)條件下, 可以唯一的確定AUV的位置, 因此上述建立的狀態(tài)空間模型是可觀(guān)測(cè)的。

1.3 濾波求解

可見(jiàn)上式是一個(gè)線(xiàn)性模型, 可直接利用卡爾曼濾波進(jìn)行求解, 其中線(xiàn)型新的觀(guān)測(cè)值

狀態(tài)預(yù)測(cè)

觀(guān)測(cè)更新

雖然已有無(wú)跡卡爾曼濾波(unscented Kalman filter, UKF)、容積卡爾曼濾波(cubature Kalman filter, CKF)及粒子濾波(particle filter, PF)等其他典型估值濾波器可選, 但考慮到如下情況:

1) 文中模型的非線(xiàn)性?xún)H反映在觀(guān)測(cè)模型上, 且AUV運(yùn)動(dòng)特性決定了其過(guò)程噪聲并不大, 在這種條件下, UKF與CKF相對(duì)于EKF在處理大過(guò)程噪聲及非線(xiàn)性模型時(shí)的優(yōu)越性不明顯;

2) EKF作為一種應(yīng)用已久的算法[16], 已經(jīng)有大量實(shí)際工程化算法[17-19], 更適合作為衡量文中建立算法模型性能的參考方法;

3) EKF是目前非線(xiàn)性濾波中相對(duì)復(fù)雜度最低的一種方法, 更適合實(shí)時(shí)應(yīng)用[19]。

因此文中仍采用傳統(tǒng)的EKF進(jìn)行求解, 關(guān)于進(jìn)一步的算法對(duì)比分析將在仿真中結(jié)合算例具體給出。

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

圖1 仿真場(chǎng)景與定位結(jié)果

表1 自主水下航行器航跡初始狀態(tài)及仿真參數(shù)

此處要說(shuō)明的是, 由于實(shí)際中AUV自身推算誤差累積導(dǎo)致在位置上容易出現(xiàn)較大誤差, 而速度信息一般可利用AUV搭載的測(cè)速設(shè)備(多普勒計(jì)程儀等)獲得, 故上述初值中根據(jù)這一事實(shí)進(jìn)行了設(shè)置。

同時(shí)可以注意到, 速度誤差呈現(xiàn)上升趨勢(shì), 這是由于文中不考慮導(dǎo)航問(wèn)題, 為便于仿真驗(yàn)證, 載體姿態(tài)是依賴(lài)于速度解算出來(lái)的, 而仿真過(guò)程中, 速度除了在初始時(shí)刻都是未知的, 且根據(jù)前述可觀(guān)測(cè)性分析可知, 目標(biāo)位置本質(zhì)上是通過(guò)方位和深度確定的, 從而速度的不確定性影響了方位的解算和目標(biāo)位置估計(jì), 因此速度實(shí)際上是不可觀(guān)測(cè)的。而在實(shí)際應(yīng)用中, 姿態(tài)可由AUV慣導(dǎo)設(shè)備獲得, 此時(shí)速度誤差和空間方位不是耦合的, 即能夠?qū)ξ恢煤退俣冗M(jìn)行修正。

圖2 AUV運(yùn)動(dòng)狀態(tài)估計(jì)誤差(sd=10 m)

圖3 AUV運(yùn)動(dòng)狀態(tài)估計(jì)誤差(sd=5 m)

圖4 AUV運(yùn)動(dòng)狀態(tài)估計(jì)誤差(sd=1 m)

3 結(jié)束語(yǔ)

文中提出了一種在信標(biāo)深度已知條件下, 基于信標(biāo)純方位信息的AUV定位方法, 構(gòu)建了2-DOF姿態(tài)的AUV定位問(wèn)題模型, 采用EKF進(jìn)行求解, 并針對(duì)估值濾波器的選擇及調(diào)參方法進(jìn)行了分析, 通過(guò)仿真分析對(duì)所提出方法進(jìn)行了驗(yàn)證, 并將其與現(xiàn)有基于測(cè)距信息觀(guān)測(cè)模型的方法進(jìn)行對(duì)比分析, 結(jié)果表明, 文中方法在濾波精度和收斂性上具有優(yōu)勢(shì)。需要指出, 由于文中重點(diǎn)考查提出方法的性能, 因此AUV的姿態(tài)是在無(wú)橫滾平移假設(shè)下根據(jù)速度求得, 而非實(shí)際中通過(guò)航位推算過(guò)程得到, 下一步工作應(yīng)結(jié)合AUV自身導(dǎo)航過(guò)程, 融合角度信息進(jìn)行修正, 并采用實(shí)測(cè)數(shù)據(jù)進(jìn)行驗(yàn)證。

[1] 孫大軍, 鄭翠娥. 水聲導(dǎo)航、定位技術(shù)發(fā)展趨勢(shì)探討[J]. 海洋技術(shù)學(xué)報(bào), 2015, 34(3): 64-65.Sun Da-jun, Zheng Cui-e. Discussion on Development Trend of Underwater Acoustic Navigation and Positioning Technology[J]. Journal of Marine Technology, 2015, 34(3): 64-65.

[2] Alexander P S. The AUV Positioning Using Ranges from One Transponder LBL[C]//Proceedings of the 1995 MTS/ IEEE Oceans Conference. San Diego. CA, USA: IEEE, 1995.

[3] Vaganay J, Baccou P, Jouvencel B. Homing by Acoustic Ranging to a Single Beacon[C]//Proceedings of the 2000 MTS/IEEE Oceans Conference. Providence, RI, USA: IEEE, 2000.

[4] 曹俊, 鄭翠娥, 孫大軍, 等. 系數(shù)矩陣正定的單信標(biāo)測(cè)距定位算法[J]. 電子與信息學(xué)報(bào), 2017, 39(4): 981-988.Cao Jun, Zheng Cui-e, Sun Da-jun, et al. A Single Beacon Location Algorithm with Positive Definite Coefficient Matrix[J]. Journal of Electronics & Information Techno- logy, 2017, 39(4): 981-988.

[5] 曹俊, 鄭翠娥, 孫大軍, 等. AUV直線(xiàn)航跡下的單信標(biāo)測(cè)距定位算法[J]. 哈爾濱工程大學(xué)學(xué)報(bào), 2017, 38(5): 733-738.Cao Jun, Zheng Cui-e, Sun Da-jun, et al. AUV Positioning Based on Single-beacon Ranging in Straight-line Trajec- tory[J]. Journal of Harbin Engineering University, 2017, 38(5): 733-738.

[6] 張福斌, 張永清. 一種利用單信標(biāo)修正AUV定位誤差的方法[J]. 魚(yú)雷技術(shù), 2012, 20(1): 38-41.Zhang Fu-bin, Zhang Yong-qing. Correcting Localization Error with a Single Beacon for AUV[J]. Torpedo Techno- logy, 2012, 20(1): 38-41.

[7] 曹方方, 劉衛(wèi)東, 李娟麗. 基于EKF的UUV單信標(biāo)水聲導(dǎo)航定位方法研究[J]. 計(jì)算機(jī)測(cè)量與控制, 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.

[8] Vallicrosa G, Ridao P, Ribas D. AUV Single Beacon Range-Only SLAM with a SOG Filter[J]. IFAC-Papers- Online, 2015, 48(2): 26-31.

[9] 嚴(yán)衛(wèi)生, 房新鵬, 崔榮鑫. 單信標(biāo)測(cè)距AUV水下定位系統(tǒng)觀(guān)測(cè)性分析[J]. 中南大學(xué)學(xué)報(bào)(自然科學(xué)版), 2012, 43(8): 202-207.Yan Wei-sheng, Fang Xin-peng, Cui Rong-xin. Observability Analysis of Single-beacon Underwater Localization of Autonomous Underwater Vehicle[J]. Journal of Central South Unive- rsity(Science and Technology), 2012, 43(8): 202-207.

[10] 曹俊, 鄭翠娥, 孫大軍, 等. 水下載體的聲學(xué)定位系統(tǒng)測(cè)距誤差分析[J]. 南京大學(xué)學(xué)報(bào)(自然科學(xué)), 2016, 52 (5): 816-824.Cao Jun, Zheng Cui-e, Sun Da-jun. Ranging Error Anal- ysis of Acoustic Positioning System for Underwater Vehicle[J]. Journal of Nanjing University(Natural Science), 2016, 52(5): 816-824.

[11] Civera J, Davison A J M M. Inverse Depth Parameterization for Monocular SLAM[J]. IEEE Transactions on Robotics, 2008, 24(5): 932-945.

[12] Matsebe O, Holtzhausen S, Kumile C M, et al. Modeling the Kinematics of an Autonomous Underwater Vehicle for Range-Bearing Simultaneous Localization and Mapping [C]//International Conference on Mechatronics and Mach- ine Vision in Practice. Auckland, New Zealand: IEEE, 2009.

[13] Becker C, Ribas D, Ridao P. Simultaneous Sonar Beacon Localization & AUV Navigation[J]. IFAC Proceedings Volumes, 2012, 45(27): 200-205.

[14] Chaffield B. Fundamentals of High Accuracy Inertial Na- vigation[M]. Reston: AIAA Inc, 1997.

[15] PereaL, How J, Breger L. Nonlinearity in Sensor Fusion: Divergence Issues in EKF, Modified Truncated SOF, and UKF[C]//Proceedings of AIAA Conference of Guidance, Navigation, and Control. South Carolina, USA: IEEE, 2007: 3489-3499.

[16] Auger F, Hilairet M, Guerrero J M, et al. Industrial App- lications of the Kalman Filter: A Review[J]. IEEE Trans- actions on Industrial Electronics, 2013, 60(12): 5458- 5471.

[17] Gibbs, Bruce P. Advanced Kalman Filtering, Leastsqu- ares and Modeling: a Practical Handbook[M]. Pennsyl- vania. US: John Wiley & Sons, Inc., 2011.

[18] Hidalgo F, Br?unl T. Review of underwater SLAM techni- ques[C]//International Conference on Automation, Robot- ics and Applications. Queenstown, New Zealand , IEEE, 2015: 306-311.

[19] Paz L M, Tardós J D, Neira J. Divide and Conquer: EKF SLAM in O(n)[J]. IEEE Transactions on Robotics, 2008, 24(5): 1107-1120.

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

Underwater Localization Method of AUV Based on Single Beacon Bearing-Only Measurement

LI Jia-tong, ZHANG Chen, ZHANG Hong-xin

(91439thUnit, The People’s Liberation Army of China, Dalian 116041, China)

Under the condition that the depth of a beacon is known, a real-time positioning method of an autonomous undersea vehicle(AUV) is presented based on single beacon bearing-only information measurement to solve the problem of single beacon AUV underwater localization. Different from the traditional method based on single beacon ranging information, the coordinate transformation principle is adopted to construct a 2-degree of freedom(2-DOF) attitude AUV motion model, and a state space model of bearing-only AUV localization problem is derived under the condition of knowing depth. Based on the established models, the extended Kalman filter(EKF) is used to solve the problem, and a filter parameter adjusting method is presented for the application of EKF algorithm. The validity of the proposed method is verified by a simulation experiment. Comparison between the proposed method and the existing method shows that the proposed method based on single beacon bearing observation model is better in filtering accuracy than that based on ranging information, and its convergence performance is superior to that of the latter.

autonomous undersea vehicle(AUV); localization method; single beacon; bearing-only measurement; extended Kalman filter(EKF)

TJ630; TN953

A

2096-3920(2018)04-0310-06

10.11993/j.issn.2096-3920.2018.04.006

李佳橦, 張臣, 張宏欣. 基于單信標(biāo)純方位測(cè)量的AUV水下定位方法[J].水下無(wú)人系統(tǒng)學(xué)報(bào), 2018, 26(4): 310-315.

2018-04-19;

2018-06-25.

李佳橦(1991-), 女, 碩士, 主要研究方向?yàn)樗聼o(wú)人系統(tǒng)定位技術(shù).