[發(fā)明專利]基于長(zhǎng)基線和信標(biāo)在線標(biāo)定的水下機(jī)器人組合導(dǎo)航方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201610944484.4
• 申請(qǐng)日: 2016-10-26
• 公開(kāi)（公告）號(hào): CN107990891B
• 公開(kāi)（公告）日: 2021-05-28
• 發(fā)明（設(shè)計(jì)）人: 王軼群;李波;劉健;趙宏宇
• 申請(qǐng)（專利權(quán)）人: 中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01S5/22
• 代理公司: 沈陽(yáng)科苑專利商標(biāo)代理有限公司 21002
• 代理人: 王倩

1.基于長(zhǎng)基線和信標(biāo)在線標(biāo)定的水下機(jī)器人組合導(dǎo)航方法，其特征在于，包括以下步驟：

利用AUV不同時(shí)刻的單信標(biāo)斜距測(cè)量值，在線標(biāo)定各個(gè)信標(biāo)位置得到各個(gè)信標(biāo)的標(biāo)定誤差；

標(biāo)定誤差通過(guò)下式得到

[dx_i,dy_i]^T＝A^-1B

其中，

t時(shí)刻、t+1時(shí)刻和t+2時(shí)刻的AUV位置分別為(x_t,y_t,z_t),(x_t+1,y_t+1,z_t+1)和(x_t+2,y_t+2,z_t+2)，t時(shí)刻、t+1時(shí)刻和t+2時(shí)刻AUV到信標(biāo)i的斜距分別為d_{i_t},d_{i_t+1},d_{i_t+2}；

t時(shí)刻、t+1時(shí)刻和t+2時(shí)刻AUV到信標(biāo)i的水平距離分別為r_{i_t},r_{i_t+1},r_{i_t+2}；t時(shí)刻、t+1時(shí)刻和t+2時(shí)刻AUV到信標(biāo)i的水平距離估計(jì)分別為信標(biāo)i的母船初始標(biāo)定位置為(x_i,y_i,z_i)；

根據(jù)標(biāo)定誤差、通過(guò)基于長(zhǎng)基線聲學(xué)定位的量測(cè)方程和慣性導(dǎo)航的預(yù)測(cè)方程估計(jì)組合導(dǎo)航的AUV位置；

所述根據(jù)標(biāo)定誤差、通過(guò)基于長(zhǎng)基線聲學(xué)定位的量測(cè)方程和慣性導(dǎo)航的預(yù)測(cè)方程估計(jì)組合導(dǎo)航的位置包括以下步驟：

1)根據(jù)標(biāo)定誤差計(jì)算t時(shí)刻AUV的長(zhǎng)基線定位位置Z_t(x_t,y_t)；包括以下步驟：[x_t,y_t]^T＝C^-1(R-D)

其中，

t時(shí)刻AUV到信標(biāo)1、2、3的水平距離為r_{1_t},r_{2_t},r_{3_t}；信標(biāo)i的母船初始標(biāo)定位置為(x_i,y_i,z_i)；信標(biāo)1、2、3的位置估計(jì)為(x₁+dx₁,y₁+dy₁)，(x₂+dx₂,y₂+dy₂)和(x₃+dx₃,y₃+dy₃)，dx_i,dy_i為標(biāo)定誤差；

上式得到的[x_t,y_t]^T作為t時(shí)刻AUV的長(zhǎng)基線定位位置Z_t(x_t,y_t)的列向量表示；

2)根據(jù)AUV長(zhǎng)基線定位位置建立量測(cè)方程；

所述量測(cè)方程為

其中，V_t是t時(shí)刻信標(biāo)的測(cè)量噪聲；表示t時(shí)刻的量測(cè)表達(dá)式；是t時(shí)刻測(cè)量時(shí)AUV的真實(shí)經(jīng)度和真實(shí)緯度；H_t＝[1,1]^T是測(cè)量方程的雅各比矩陣；

3)建立組合導(dǎo)航的預(yù)測(cè)方程得到AUV的預(yù)測(cè)位置X_t(x_t,y_t)；

所述預(yù)測(cè)方程為

其中，u_t,ψ,θ,φ是t時(shí)刻AUV的速度、航向角、縱傾角和橫滾角，U_t＝[u_t,ψ]是系統(tǒng)輸入矩陣；ΔT是從t-1時(shí)刻到t時(shí)刻的時(shí)間間隔；H_t＝[1,1]^T是測(cè)量方程的雅各比矩陣；系統(tǒng)輸入噪聲的方差矩陣為Q_t；x_t-1和y_t-1分別表示t-1時(shí)刻AUV在經(jīng)度和緯度位置；P_t,t-1表示t時(shí)刻預(yù)測(cè)AUV位置的預(yù)測(cè)方差；P_t-1表示t-1時(shí)刻估計(jì)AUV位置的估計(jì)方差；是歐拉轉(zhuǎn)換矩陣；R_e是地球的短軸半徑；e是地球旋轉(zhuǎn)橢球扁率；

4)根據(jù)AUV的長(zhǎng)基線定位位置Z_t(x_t,y_t)和預(yù)測(cè)位置X_t(x_t,y_t)得到組合導(dǎo)航的AUV位置，通過(guò)下式得到

其中，t時(shí)刻AUV估計(jì)位置和分別表示t時(shí)刻AUV的估計(jì)位置經(jīng)度和緯度；R_t表示t時(shí)刻量測(cè)噪聲的方差矩陣，h(X_t)＝H_tX_t+V_t。

2.根據(jù)權(quán)利要求1所述的基于長(zhǎng)基線和信標(biāo)在線標(biāo)定的水下機(jī)器人組合導(dǎo)航方法，其特征在于所述估計(jì)組合導(dǎo)航的位置后，通過(guò)計(jì)算組合導(dǎo)航的位置估計(jì)方差來(lái)估計(jì)導(dǎo)航精度：其中，I是單位矩陣，P_t表示t時(shí)刻AUV組合導(dǎo)航的位置估計(jì)方差。