[發明專利]一種接近信標軌跡下的水下機器人定位方法

有效

• 申請號: 201911064151.2
• 申請日: 2019-11-04
• 公開（公告）號: CN110888104B
• 公開（公告）日: 2022-03-22
• 發明（設計）人: 冀大雄;方文巍
• 申請（專利權）人: 浙江大學
• 主分類號: G01S1/72
• 分類號: G01S1/72;G01S11/14
• 代理公司: 杭州中成專利事務所有限公司 33212
• 代理人: 周世駿

本發明涉及水下機器人定位技術，旨在提供一種接近信標軌跡下的水下機器人定位方法。包括：在保持水下機器人勻速率運動且速率大小已知的情況下，按照設定測量周期計算得到當前位移方向與當前觀測距離方向之間的夾角；然后通過強化學習和訓練，獲得艏向角的調整策略并用于調整艏向角，使水下機器人朝向接近信標的方向運動；在接近信標的過程中，利用擴展卡爾曼濾波位置估計方程計算水下機器人的位置以實現定位。本發明在初始估計誤差較大的情況下，相比于圓形軌跡，收斂速度更快。用強化學習的方法來調整艏向角，使水下機器人做接近信標的運動。避免了復雜的調整角度規則的制定。不需要準確的初始位置估計值，運算量小，計算簡單，定位穩定可靠。

技術領域

本發明涉及水下機器人定位技術，特別涉及一種接近信標軌跡下的水下機器人定位方法。

背景技術

水下機器人在水下執行一段時間任務后，累積誤差會隨著時間增大。這時候，一般需要浮出水面，接收GPS信號重新定位。對于深水作業的水下機器人，浮出水面需要消耗大量能量，且隱蔽性不好。

單信標測距定位技術由于安裝簡單、成本低，成為近幾年水下定位技術新的發展方向。但是單個測距信息不能完全確定水下機器人的位置，因此需要設計一定的軌跡和相應的算法來實現定位。圓形軌跡和擴展卡爾曼濾波算法是常用的機動軌跡和濾波算法。其中，擴展卡爾曼濾波是一個非線性的濾波器，對初始估計值要求較高，當濾波初始估計值誤差較大時，往往會導致收斂速度慢，甚至發散。在初始估計誤差較大時，采用圓形軌跡，收斂速度慢。

發明內容

本發明要解決的技術問題是，克服現有技術中的不足，提供一種接近信標軌跡下的水下機器人定位方法。該方法是在單信標測距系統下，通過調整艏向角，使水下機器人向信標運動，同時在該軌跡中用擴展卡爾曼濾波進行位置估計，實現定位。

為解決技術問題，本發明的解決方案是：

提供一種接近信標軌跡下的水下機器人定位方法，包括以下步驟：

(1)在保持水下機器人勻速率運動且速率大小已知的情況下，按照設定測量周期，以單個聲信標測量水下機器人與信標之間的距離值，以水下機器人自帶的羅盤測量艏向角，由前一個時刻的位移距離和前一時刻的觀測距離，和當前時刻的觀測距離，通過已知三邊長度的余弦定理計算得到當前位移方向與當前觀測距離方向之間的夾角；然后通過強化學習和訓練，獲得艏向角的調整策略；

(物理定義中勻速運動是指速度大小和方向不變，本發明所述勻速率是指速度大小不變，但通過調整艏向角的角度改變方向。)

(觀測距離方向并不可測量。但通過三邊長度已知的余弦定理，可以求出當前位移方向與觀測距離方向的夾角。觀測距離即由信標測量得到的水下機器人與信標之間的距離值，觀測距離也就是量測距離，由于水聲測距帶有噪聲，會與實際距離存在誤差。)

(2)利用強化學習訓練得到的策略來調整艏向角，使水下機器人朝向接近信標的方向運動；

(3)在接近信標的過程中，利用擴展卡爾曼濾波位置估計方程計算水下機器人的位置以實現定位。

本發明中，所述步驟(1)具體包括：

首先，建立8行×16列的R表格，表格內容是對應于各情況下的平均獎勵值，表示選擇相應動作時對接近信標的有利程度；其中，