[發明專利]一種深水機器人組合導航裝置和方法

有效

• 申請號: 201310282048.1
• 申請日: 2013-07-05
• 公開（公告）號: CN104280024B
• 公開（公告）日: 2017-04-19
• 發明（設計）人: 冀大雄;劉健;劉鐵軍;徐春暉;石凱
• 申請（專利權）人: 中國科學院沈陽自動化研究所
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01S15/06
• 代理公司: 沈陽科苑專利商標代理有限公司21002
• 代理人: 周秀梅,許宗富

1.一種深水機器人組合導航方法，其特征在于，包括以下步驟：

深水機器人下水后，導航計算機對測距器、航向傳感器、航速傳感器和深度傳感器進行數據采集；

當導航計算機采集到測距器的距離數據時，計算深水機器人的初始位置；

判斷初始位置是否有效；

如果初始位置有效，則判斷距離數據是否有效，如果距離數據無效，則舍棄該距離數據，返回計算深水機器人的初始位置步驟；

如果距離數據有效，則根據距離數據對深水機器人的位置進行在線修正。

2.根據權利要求1所述的一種深水機器人組合導航方法，其特征在于，所述計算深水機器人的初始位置包括以下步驟：

1)當測距周期內的測距數據數量大于3時，將初始值N置0，對深水機器人水平面坐標進行解算，解算公式如下：

Ax＝v(1)

式中，

(x_i,y_i,z_i)(i＝1,2,3)為第i號信標的三維坐標，(x,y,z)為深水機器人水平面坐標和深度，為第i號信標到坐標系原點的水平間距，為第i號信標到深水機器人的水平距離，其中R_i表示第i號信標的單程傳播距離；

2)根據解算的深水機器人水平面坐標推算航位：

式中，(x₀,y₀)為初始點，Δt為采集周期，(x_t,y_t)為推算航位，v_e＝vsinθ，v_n＝vcosθ，θ為當前航向，v為當前航速。

3.根據權利要求2所述的一種深水機器人組合導航方法，其特征在于，所述判 斷初始位置是否有效具體為：

在下一個測距周期到來時，重新解算公式(1)中的深水機器人水平面坐標，將其與所述推算航位求差后求取均方根誤差，即：

如果e＜＝Bias

則N＝N+1，返回2)，直到N＞＝Window時，判定當前測距周期內解算的初始位置 有效，

否則N＝0，返回1)；

上式中，Bias范圍為1米～100米，Window范圍為2～30。

4.根據權利要求1所述的一種深水機器人組合導航方法，其特征在于，所述判斷距離數據是否有效的方法為：判斷公式

S(k)＝H_kP_k|k-1H'_k|k-1+R_k

是否成立，如果成立，則使用當前測量值z_i；否則，舍棄；

式中，γ范圍為6～500，H_k為觀測矩陣，P_k|k-1為預測協方差矩陣，H'_k|k-1為預測觀測矩陣，R_k為觀測噪聲，為測量值的估計矩陣。

5.根據權利要求1所述的一種深水機器人組合導航方法，其特征在于，所述根據距離數據對深水機器人的位置進行在線修正包括以下步驟：

設任意信標的坐標為(x_i,y_i)(i＝1,2,...,)，則離散系統狀態方程為

其中，

s_k＝[x_k y_k]^T，x_k，y_k分別表示當前時刻k的東向坐標、北向坐標；

z_k＝c·τ_i，表示雙程聲傳播距離，其中c表示聲速，τ_i表示第i個信標的雙程聲傳播時間，v_e為深水機器人東向速度，x_i為第i個信標的東向坐標，v_n為深水機器人北向速 度，y_i為第i個信標的北向坐標，z_i為第i個信標的深度,z_k為深水機器人當前時刻k的深度；

w_k是一個二維向量，為過程驅動噪聲，w_k(1)和w_k(2)為正值且不大于10；

ν_k是一個一維向量，為觀測噪聲，ν_k為正值且不大于50；

1)初始化

2)預測

P_k|k-1＝AP_k|k-1A'+BQ_kB'

3)修正

K_k＝P_k|k-1H'_k(H_kP_k|k-1H'_k|k-1+R_k)^-1

P_k|k＝(I-K_kH_k)P_k|k-1

其中

Q_k＝diag([w_k(1))² (w_k(2))²])，