[發(fā)明專利]基于自主水下機(jī)器人慣導(dǎo)與超短基線定位傳感器的軌跡解算與配準(zhǔn)方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202210548232.5
• 申請(qǐng)日: 2022-05-18
• 公開（公告）號(hào): CN114993313A
• 公開（公告）日: 2022-09-02
• 發(fā)明（設(shè)計(jì)）人: 潘漢;袁煜;敬忠良;彭湃
• 申請(qǐng)（專利權(quán)）人: 上海交通大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G01C21/16
• 代理公司: 上海旭誠知識(shí)產(chǎn)權(quán)代理有限公司 31220
• 代理人: 鄭立

1.一種基于自主水下機(jī)器人慣導(dǎo)與超短基線定位傳感器的軌跡解算與配準(zhǔn)方法，其特征在于，包括以下步驟：

步驟1、通過包含慣性導(dǎo)航設(shè)備與超短基線傳感器的自主水下機(jī)器人，獲取IMU數(shù)據(jù)和USBL數(shù)據(jù)，篩選并處理USBL數(shù)據(jù)；

步驟2、將IMU數(shù)據(jù)點(diǎn)坐標(biāo)由WGS84坐標(biāo)系轉(zhuǎn)化為ENU坐標(biāo)系；

步驟3、繪制處理后的USBL數(shù)據(jù)點(diǎn)與IMU數(shù)據(jù)點(diǎn)在ENU坐標(biāo)系下的軌跡；

步驟4、求解ENU坐標(biāo)系與USBL坐標(biāo)系之間的角度與原點(diǎn)位置偏差；

步驟5、求解ENU坐標(biāo)系與USBL坐標(biāo)系之間的變換矩陣，并將USBL數(shù)據(jù)點(diǎn)與IMU數(shù)據(jù)點(diǎn)進(jìn)行配準(zhǔn)；

步驟6，繪制配準(zhǔn)后的IMU和USBL數(shù)據(jù)在ENU坐標(biāo)系下的軌跡，利用衛(wèi)星照片與相關(guān)GPS坐標(biāo)值，將繪制的軌跡與實(shí)際現(xiàn)場(chǎng)衛(wèi)星照片進(jìn)行疊加。

2.根據(jù)權(quán)利要求1所述的基于自主水下機(jī)器人慣導(dǎo)與超短基線定位傳感器的軌跡解算與配準(zhǔn)方法，其特征在于，在所述步驟1中，根據(jù)AUV航行器在實(shí)際實(shí)驗(yàn)中的先驗(yàn)信息，處理USBL數(shù)據(jù)的異常數(shù)據(jù)，確保余下USBL數(shù)據(jù)的可靠性。

3.根據(jù)權(quán)利要求2所述的基于自主水下機(jī)器人慣導(dǎo)與超短基線定位傳感器的軌跡解算與配準(zhǔn)方法，其特征在于，在所述步驟1中具體運(yùn)算過程為：

步驟1.1、在X方向上，對(duì)數(shù)據(jù)通過以下方式處理：

其中，x(i)為第i個(gè)USBL數(shù)據(jù)點(diǎn)的X軸坐標(biāo)，即深度坐標(biāo)；d₀為AUV在實(shí)驗(yàn)過程中的最大下潛深度，根據(jù)d₀將深度異常數(shù)據(jù)點(diǎn)標(biāo)記為error；

步驟1.2、在Y方向上，對(duì)數(shù)據(jù)通過以下方式處理：

其中，y(i)為第i個(gè)USBL數(shù)據(jù)點(diǎn)的Y軸坐標(biāo)；AUV在實(shí)驗(yàn)過程中，Y方向上運(yùn)動(dòng)范圍為實(shí)驗(yàn)水域?qū)挾葁₀/2之內(nèi)，根據(jù)w₀將Y方向上異常數(shù)據(jù)點(diǎn)標(biāo)記為error；

步驟1.3、在Z方向上，對(duì)數(shù)據(jù)通過以下方式處理：

其中，z(i)為第i個(gè)USBL數(shù)據(jù)點(diǎn)的Z軸坐標(biāo)；AUV在實(shí)驗(yàn)過程中，Z方向上運(yùn)動(dòng)范圍為實(shí)驗(yàn)水域長度l₀之內(nèi)，根據(jù)l₀將Z方向上異常數(shù)據(jù)點(diǎn)標(biāo)記為error；

步驟1.4、在相鄰時(shí)間戳數(shù)據(jù)點(diǎn)間，對(duì)數(shù)據(jù)通過以下方式處理：

其中，v_max為AUV在實(shí)驗(yàn)過程中的最大運(yùn)動(dòng)速度，根據(jù)v_max將速度異常數(shù)據(jù)點(diǎn)標(biāo)記為error；

步驟1.5、根據(jù)以上步驟1.1至1.4，剔除標(biāo)記為error的USBL數(shù)據(jù)點(diǎn)。

4.根據(jù)權(quán)利要求3所述的基于自主水下機(jī)器人慣導(dǎo)與超短基線定位傳感器的軌跡解算與配準(zhǔn)方法，其特征在于，所述步驟1還包括：刪去數(shù)據(jù)點(diǎn)中X、Y、Z坐標(biāo)均為0值的數(shù)據(jù)，此數(shù)據(jù)代表在此期間超短基線傳感器未正常發(fā)送或接收有效數(shù)據(jù)。

5.根據(jù)權(quán)利要求1所述的基于自主水下機(jī)器人慣導(dǎo)與超短基線定位傳感器的軌跡解算與配準(zhǔn)方法，其特征在于，所述步驟2具體包括：

步驟2.1、將IMU數(shù)據(jù)點(diǎn)坐標(biāo)從WGS84坐標(biāo)系轉(zhuǎn)ECEF坐標(biāo)系；

步驟2.2、將ECEF坐標(biāo)系下的IMU數(shù)據(jù)點(diǎn)轉(zhuǎn)ENU坐標(biāo)系。

6.根據(jù)權(quán)利要求5所述的基于自主水下機(jī)器人慣導(dǎo)與超短基線定位傳感器的軌跡解算與配準(zhǔn)方法，其特征在于，所述步驟2.1具體包括：ECEF坐標(biāo)系原點(diǎn)為地球的質(zhì)心，X軸延伸通過本初子午線(0度經(jīng)線)和赤道(0度緯線)的交點(diǎn)，Z軸延伸通過的北極點(diǎn)，Y軸遵從右手坐標(biāo)系，穿過赤道和90度經(jīng)度；將WGS84坐標(biāo)系下的(lon,lat,alt)轉(zhuǎn)換為ECEF坐標(biāo)系下點(diǎn)(X,Y,Z)的計(jì)算步驟為：

其中e為橢球偏心率，τ為基準(zhǔn)橢球體的曲率半徑；

其中a為赤道半徑，b為極軸半徑。