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

有效

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

1.一種接近信標軌跡下的水下機器人定位方法，其特征在于，包括以下步驟：

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

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

(2.1)按設定時間間隔，測量當前時刻的觀測距離r(t+1)、上一時刻的觀測距離r(t)、上一時刻的位移距離d(t)和用已知三邊長度下的余弦定理，計算位移方向與當前觀測距離方向之間的夾角α2；如果r(t+1)＜r(t)且夾角α2小于45°，保持直行；否則將水下機器人的行進方向逆時針旋轉5°后直行，并測量獲得下一個時刻的觀測距離、位移和夾角數據；

(2.2)調整艏向角后，繼續直行，并重新進行測量和計算；重復以上過程，直到水下機器人到信標的距離小于設定的距離值D；然后圍繞信標方向隨機地運動一段設定時間T，定位結束；

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

2.根據權利要求1所述的方法，其特征在于，所述步驟(1)具體包括：

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

行名包括下述八種情況：情況1：觀測距離變大，變化量大于一半的位移距離且位移方向與觀測距離方向的夾角變大；情況2：觀測距離變大，變化量小于等于一半的位移距離，且位移方向與觀測距離方向的夾角變大；情況3：觀測距離變小，變化量小于一半的位移距離，且位移距離與觀測距離方向的夾角變大；情況4：觀測距離變小，變化量大于等于一半的位移距離，且位移距離與觀測距離方向的夾角變大；情況5：觀測距離變大，變化量大于一半的位移距離，且位移方向與觀測距離方向的夾角變小；情況6：觀測距離變大，變化量小于等于一半的位移距離，且位移方向與觀測距離方向的夾角變小；情況7：觀測距離變小，變化量小于一半的位移距離，且位移距離與觀測距離方向的夾角變小；情況8：觀測距離變小，變化量大于等于一半的位移距離，且位移距離與觀測距離方向的夾角變小；

列名包括下述十六個調整動作：順時針旋轉15°、30°、45°、60°、75°、90°、105°、120°和逆時針旋轉15°、30°、45°、60°、75°、90°、105°、120°；

其次，以經過強化學習訓練的R表格作為艏向角的調整策略，具體訓練的過程包括：

(1.1)初始化R表格，確定訓練次數；

(1.2)設置水下機器人的起始位置后，使其保持勻速率運動；在第一個時刻里，水下機器人朝任意方向移動；

(1.3)按設定時間間隔，測量當前時刻的觀測距離r(t)、上一時刻的觀測距離r(t-1)、上一時刻的位移距離d(t-1)和用已知三邊長度下的余弦定理，計算位移方向與當前觀測距離方向之間的夾角α；如果r(t)＜r(t-1)且夾角α小于45°，保持直行；否則將水下機器人的行進方向逆時針旋轉5°后直行，并測量獲得下一個時刻的觀測距離、位移和夾角數據；

(1.4)由連續三個時刻的觀測距離和兩次位移距離的信息，計算出后兩次觀測距離的差值r(t+1)-r(t)和兩次夾角的差值α2-α1，并在細分后進行歸類；

(1.5)以ε-greedy的探索策略選取設計好的十六個動作；執行動作后，根據觀測距離變化情況獲得對應獎勵值Re；當觀測距離減小時Re＝+1，當觀測距離不變或變大時Re＝-1；并更新R表格；更新公式為：

R(s，a)＝(R(s，a)×(N(s，a)-1)+Re)/N(s，a)

s為情況s1～s8中的一個，a為動作a1～a16，具體為得到獎勵值Re前，所歸類的情況s，和所執行的動作a；

N(s，a)為訓練過程中N(S，A)表格中的一項，表示在情況s下執行了動作a的次數；N(S，A)是一個8×16的表格；更新規則為N(s，a)＝N(s，a)+1；

R(s，a)為訓練過程中R(S，A)表格中的一項，表示在情況s狀態下執行動作a獲得獎勵值的平均值，R(S，A)是一個8×16的表格；

(1.6)當觀測距離小于設定的距離值D；本輪訓練結束，返回步驟(1.3)；當訓練達到設定次數后結束訓練，保留訓練結果R表格。

3.根據權利要求1所述位方法，其特征在于，所述步驟(3)包括：

(1)設定擴展卡爾曼濾波位置估計方程為：

K(k+1)＝P(k+1|k)·H^T(k+1)·[H(k+1)·P(k+1|k)·H^T(k+1)+R]^-1

P(k+1|k+1)＝[I-K(k+1)·H(k+1)]·P(k+1|k)

上式中，k表示k時刻，X(k)＝[x_k y_k]^T上標“T”表示向量或者矩陣的轉置；x_k，y_k分別是水下機器人在k時刻以信標為原點的東向和北向坐標；字母上帶“^”符號的表示狀態量的預測值或估計值；

方程式中，是k時刻的狀態估計值，是k+1時刻的狀態預測值，是根據觀測值校正后的k+1時刻的狀態估計值；

U(k)＝[v·sinθ_k v·cosθ_k]^T，θ_k為k時刻的水下機器人的艏向角，v為水下機器人的航速；

I為單位矩陣；P(k|k)是k時刻的誤差的協方差矩陣；P(k+1|k)是k+1時刻預測的誤差協方差矩陣；P(k+1|k+1)是量測更新后的協方差矩陣；

V(k+1)為k+1時刻過程噪聲，W(k+1)為k+1時刻觀測噪聲，滿足零均值高斯分布，Q是過程噪聲的方差，R是觀測噪聲的方差；

Z(k+1)＝h(X(k+1))+W(k+1)，是即k+1時刻觀測到的帶有噪聲的水下機器與信標之間的距離值；

即k+1時刻水下機器人到信標的距離；

即k+1時刻，通過狀態預測量計算的距離；

H(k+1)是將通過泰勒展開，保留一階項的方式，線性化后得到的雅克比矩陣，