[發(fā)明專利]一種基于M估計(jì)的AUV組合導(dǎo)航方法及系統(tǒng)

在審

• 申請?zhí)枺?/td>: 202010513750.4
• 申請日: 2020-06-08
• 公開（公告）號: CN111829511A
• 公開（公告）日: 2020-10-27
• 發(fā)明（設(shè)計(jì)）人: 盧曼曼;張南南;李宇航;郭眀禹
• 申請（專利權(quán)）人: 中國航天空氣動力技術(shù)研究院
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 馬全亮

1.一種基于M估計(jì)的AUV組合導(dǎo)航方法，其特征在于，包括以下幾個步驟：

步驟一：建立SINS/USBL組合導(dǎo)航模型；

步驟二：對組合導(dǎo)航模型的量測方程進(jìn)行重構(gòu)；

步驟三：根據(jù)重構(gòu)后的量測方程，對組合導(dǎo)航模型中的狀態(tài)變量X求解，獲得X的最優(yōu)估計(jì)值；

步驟四：根據(jù)步驟三得到的X最優(yōu)估計(jì)值進(jìn)行校正，從而實(shí)現(xiàn)AUV組合導(dǎo)航定位。

2.根據(jù)權(quán)利要求1所述的基于M估計(jì)的AUV組合導(dǎo)航方法，其特征在于：建立SINS/USBL組合導(dǎo)航模型，具體為：

(1.1)選取東北天E-N-U地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，選取捷聯(lián)慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)的誤差量作為系統(tǒng)狀態(tài)量，即選取位置誤差[δL δλ δh]、速度誤差[δV_E δV_N δV_U]、姿態(tài)誤差角[φ_Eφ_N φ_U]、沿載體坐標(biāo)系X-Y-Z軸上的陀螺漂移[ε_x ε_y ε_z]和加速度計(jì)零偏作為狀態(tài)量，建立狀態(tài)向量如下

(1.2)SINS/USBL組合導(dǎo)航模型的狀態(tài)方程如下所示：

為X(t)的一階導(dǎo)數(shù)；

系統(tǒng)噪聲為：

η(t)＝[n_ax,n_ay,n_az,n_wx,n_wy,n_wz]^T；

其中，n_ax,n_ay,n_az,n_wx,n_wy,n_wz為三軸加速度計(jì)和三軸陀螺儀零均值高斯白噪聲；

G(t)為噪聲驅(qū)動矩陣：

其中，T為捷聯(lián)矩陣，o為零矩陣；

F(t)為狀態(tài)轉(zhuǎn)移矩陣，把F(t)變成分塊矩陣形式如下所示，

其中，F(xiàn)_INS,9×9代表SINS誤差方程；

(1.3)將狀態(tài)方程進(jìn)行一階離散化

X_k＝Φ_k/k-1X_k-1+w_k-1

式中：

X_k＝X(t_k)

T_s＝t_k-t_k-1

其中：I為單位陣，w_k為高斯白噪聲，即w_k～(0,Q)；Q是指高斯白噪聲方差；(1.4)SINS/USBL組合導(dǎo)模型的量測方程如下.

y_k＝H_kX_k+v_k

其中，y_k為量測量，H_k為量測矩陣，v_k為量測噪聲；

量測矩陣為：

式中，

其中：R為地球半徑，h為高度，L為緯度，λ為經(jīng)度，e為地球曲率半徑，為SINS計(jì)算的載體坐標(biāo)系下應(yīng)答器的位置信息；

為平臺坐標(biāo)系到載體坐標(biāo)系轉(zhuǎn)換矩陣，為地球坐標(biāo)系到平臺坐標(biāo)系轉(zhuǎn)換矩陣。