[發明專利]一種基于超短基線和航位推算的水下導航定位方法

有效

• 申請號: 201910836803.3
• 申請日: 2019-09-05
• 公開（公告）號: CN110568407B
• 公開（公告）日: 2023-06-27
• 發明（設計）人: 馬杰;張煜;亓強強;彭智浩;李文楷
• 申請（專利權）人: 武漢理工大學
• 主分類號: G01S5/22
• 分類號: G01S5/22;G01S15/74;H03H17/02
• 代理公司: 湖北武漢永嘉專利代理有限公司 42102
• 代理人: 張惠玲

本發明涉及一種基于超短基線和航位推算的水下導航定位方法，包括以下步驟：S1、確定超短基線系統的定位原理；S2、確定超短基線系統的誤差模型；S3、建立狀態模型和觀測模型；S4、建立考慮方向角量測偏差變化的擴展卡爾曼濾波算法。本發明能有效地適應方向角量測噪聲變化的情形，在小方向角下也能保持較高的定位精度。

技術領域

本發明涉及水下定位與導航技術領域，更具體地說，涉及一種基于超短基線和航位推算的水下導航定位方法。

背景技術

在水下航行器如ROV(Remote?Operated?Vehicle)、AUV(Autonomous?UnderwaterVehicle)作業過程中，對其進行高精度地導航定位是一項十分重要的操作。

作為一種多源信息融合算法，卡爾曼濾波(Kalman?Filtering)已被廣泛應用于水下航行器的導航定位中。基于超短基線(Ultrashort?Base?Line，簡稱USBL)和航位推算(Dead?reckoning，簡稱DR)的擴展卡爾曼濾波可以融合超短基線系統的觀測數據和航位推算信息，得到水下目標位置的最優估計。精確的濾波模型和誤差統計特性是卡爾曼濾波最優估計的基礎。對于平面超短基線系統，其方向角量測偏差與方向角自身相關。而傳統基于超短基線和航位推算的擴展卡爾曼濾波算法中，超短基線系統的方向角量測偏差常看作定值，這導致在小方向角下，其濾波定位精度較低。考慮到船舶搖晃或水下目標自主移動等導致方向角量測偏差發生變化的因素，本發明提出一種考慮方向角量測偏差變化、基于超短基線和航位推算的水下導航定位方法。

發明內容

(一)要解決的技術問題

為了解決傳統基于超短基線和航位推算的擴展卡爾曼濾波算法在小方向角下定位精度低的問題，本發明提供一種考慮方向角量測偏差變化、基于超短基線和航位推算的水下導航定位方法。

(二)技術方案

為了達到上述目的，本發明采用的主要技術方案包括：

設計一種基于超短基線和航位推算的水下導航定位方法，包括以下步驟：

S1、確定超短基線系統的定位原理；

S2、確定超短基線系統的誤差模型；

S3、建立狀態模型和觀測模型；

S4、建立考慮方向角量測偏差變化的擴展卡爾曼濾波算法。

在上述方案中，在所述步驟S4中，建立考慮方向角量測偏差變化的卡爾曼濾波算法，包括以下步驟：

S4-1、根據上一時刻后驗估計位置以及其后驗協方差，通過狀態模型確定此時刻水下目標的先驗估計位置以及其先驗協方差；

S4-2、根據上一時刻的后驗估計位置，通過超短基線系統的誤差模型確定此時刻的觀測噪聲矩陣；

S4-3、根據此時刻觀測噪聲矩陣，確定增益；

S4-4、根據量測方程確定新息；

S4-5、根據增益及新息確定此時刻水下目標的后驗估計位置以及其后驗協方差；

S4-6、重復上述步驟，獲得水下航行器的實時位置。

在上述方案中，在所述步驟S1中，換能器O布置在船體空間坐標系O-XYZ的原點位置，水聽器S1、S2、S3、S4在船體空間坐標系XOY平面中心對稱布置，其中，S1、S3陣元位于指向船艏的X軸上，S2、S4陣元位于垂直于船艏的Y軸上；陣元間距為d，即S1與S3間距、S2與S4間距；搭載聲學應答器的水下目標位于T點，其在船體空間坐標系下坐標為(x，y，z)；超短基線水聲定位系統利用水聽器接收信號之間的時延差T和相位差Δφ_x，Δφ_y解算水下目標的距離r：