[發(fā)明專利]基于虛擬速度量測的水下無人潛航器測流DVL/SINS組合導(dǎo)航方法

在審

• 申請?zhí)枺?/td>: 202210232800.0
• 申請日: 2022-03-09
• 公開（公告）號: CN114459476A
• 公開（公告）日: 2022-05-10
• 發(fā)明（設(shè)計(jì)）人: 劉錫祥;王子璇;吳小強(qiáng);盛廣潤;黃永江
• 申請（專利權(quán)）人: 東南大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 南京眾聯(lián)專利代理有限公司 32206
• 代理人: 蔣昱

1.基于虛擬速度量測的中層水域SINS/測流DVL組合導(dǎo)航方法，其特征在于，具體方法如下：

(1)獲取安裝在水下無人潛航器上的傳感器實(shí)時數(shù)據(jù)，具體包括多普勒測速儀的速度量測數(shù)據(jù)以及慣性測量元件所輸出的陀螺儀和加速度計(jì)的輸出數(shù)據(jù)，并利用傳感器數(shù)據(jù)執(zhí)行姿態(tài)解算算法，獲得SINS系統(tǒng)以及DVL系統(tǒng)的導(dǎo)航系下速度，具體步驟如下：

S1.1：選取當(dāng)?shù)氐乩碜鴺?biāo)系作為導(dǎo)航坐標(biāo)系，分別指向東、北、天，記導(dǎo)航坐標(biāo)系為n；載體坐標(biāo)系原點(diǎn)位于載體重心，三個軸分別指向載體的右、前、上，記載體坐標(biāo)系為b；記慣性坐標(biāo)系為i、地球坐標(biāo)系為e，則導(dǎo)航坐標(biāo)系下導(dǎo)航解算的微分方程如式(1)～(4)：

其中，q表示載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的旋轉(zhuǎn)四元數(shù)；ωnbb表示載體系b和導(dǎo)航系n之間的相對角速度在載體系b下的投影；ωien表示地球系e與慣性系i的相對角速度在導(dǎo)航系n下的投影；ωenn表示導(dǎo)航系n與地球系e之間的相對角速度在導(dǎo)航系n下的投影；Vn＝vEvNvUT是對地速度在導(dǎo)航系n下的投影；fb表示加速度計(jì)輸出的比力信息；gn表示重力加速度在導(dǎo)航系n下的投影；L、λ和h分別表示緯度、經(jīng)度和高度；R_n、R_e分別表示地球子午圈、卯酉圈曲率半徑；

S1.2：已知初始時刻的姿態(tài)q(0)、速度v(0)和位置P(0)，通過對導(dǎo)航解算的微分方程(2)-(4)進(jìn)行積分，實(shí)時遞歸計(jì)算得到當(dāng)前時刻的姿態(tài)、速度和位置。

S1.3：DVL傳感器安裝在載體的底部，通過向下發(fā)送超聲波以完成對載體速度的測量。高精度的DVL傳感器分別在載體的前后左右四個方向發(fā)送超聲波束，可獲得AUV在載體坐標(biāo)系下的三維速度Vdvlb＝[vdvlrightvdvlfrontvdvlup]。然后，利用S1.2中SINS系統(tǒng)解算獲得的AUV姿態(tài)，可獲取到導(dǎo)航坐標(biāo)系下的三維速度：

(2)利用步驟(1)中獲得的IMU輸出、SINS系統(tǒng)以及DVL系統(tǒng)的導(dǎo)航系下速度，構(gòu)建一組AUV的虛擬對地速度量測；

步驟(2)中t時刻虛擬速度量測的構(gòu)建方程具體為：

式中，代表構(gòu)建的t時刻不含洋流干擾的由虛擬東向速度量測和虛擬北向速度量測組成的虛擬速度量測向量，為由SINS在t-1時刻經(jīng)過捷聯(lián)解算算法獲得的東向速度和北向速度組成的向量，為從t-1時刻到t時刻構(gòu)建的速度變化量，其計(jì)算方式如下：

式中，為DVL測量誤差協(xié)方差矩陣，而為SINS系統(tǒng)的速度估計(jì)誤差協(xié)方差矩陣；

(3)將步驟(2)中構(gòu)建的水下無人潛航器虛擬對地速度量測與步驟(1)中獲得SINS系統(tǒng)、DVL系統(tǒng)速度輸出一同輸入至卡爾曼濾波器中，進(jìn)行信息融合，完成對水下無人潛航器的定位與導(dǎo)航，具體步驟如下；

S3.1：選取SINS系統(tǒng)姿態(tài)失準(zhǔn)角、SINS系統(tǒng)導(dǎo)航系速度誤差、SINS系統(tǒng)位置誤差、陀螺常值誤差和、速度計(jì)常值誤差以及水流對地速度作為17維狀態(tài)變量，選取DVL系統(tǒng)與SINS系統(tǒng)導(dǎo)航系速度誤差、SINS系統(tǒng)與高度計(jì)高度測量誤差以及SINS系統(tǒng)與虛擬速度量測導(dǎo)航系速度誤差作為5維量測變量，獲得離散化的kalman濾波狀態(tài)方程和量測方程；

S3.2：給定狀態(tài)估計(jì)和估計(jì)誤差方差的初始值X0和P0，根據(jù)k時刻的觀測值Z(k)，實(shí)時遞推計(jì)算得到k時刻的狀態(tài)估計(jì)Xkk；

S3.3：利用步驟3.2中得到的狀態(tài)估計(jì)對SINS輸出的導(dǎo)航參數(shù)進(jìn)行修正。

2.根據(jù)權(quán)利要求1所述的基于虛擬速度量測的水下無人潛航器測流DVL/SINS組合導(dǎo)航方法，其特征在于，

ΔV_est的計(jì)算方式如下：

式中，f^b(t)為加速度計(jì)在t時刻的比力輸出值，為SINS系統(tǒng)t時刻解算獲得的姿態(tài)矩陣，φ為SINS系統(tǒng)計(jì)算導(dǎo)航系與實(shí)際的地理導(dǎo)航系之間的失準(zhǔn)角，b_a為SINS中加速度計(jì)的常值零偏估計(jì)值。