[發(fā)明專利]一種引入徑向速度的SINS/USBL緊組合導(dǎo)航定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010144245.7
• 申請(qǐng)日: 2020-03-04
• 公開（公告）號(hào): CN111380518B
• 公開（公告）日: 2021-10-29
• 發(fā)明（設(shè)計(jì)）人: 孫大軍;鄭翠娥;張居成;韓云峰;崔宏宇;張殿倫
• 申請(qǐng)（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/18;G01C21/00;G01S5/22;G01S19/42
• 代理公司: 哈爾濱市松花江專利商標(biāo)事務(wù)所 23109
• 代理人: 時(shí)起磊

1.一種引入徑向速度的SINS/USBL緊組合導(dǎo)航定位方法，其特征在于：所述方法具體過程為：

步驟一、將USBL聲學(xué)基陣倒置安裝在水下航行器上，SINS的陀螺組件和加速度計(jì)組件固聯(lián)在水下航行器上；

所述USBL為超短基線定位系統(tǒng)；SINS為捷聯(lián)慣性導(dǎo)航系統(tǒng)；

水面布放一個(gè)同步信標(biāo)，信標(biāo)在導(dǎo)航坐標(biāo)系n下的位置通過GPS獲得；

所述聲學(xué)基陣是由多個(gè)基元均勻分布的一個(gè)圓柱形設(shè)備；

所述多個(gè)為大于等于3個(gè)；

所述導(dǎo)航坐標(biāo)系是指“東北天”地理坐標(biāo)系，該地理坐標(biāo)系以水下航行器的質(zhì)心為原點(diǎn)o，x軸指向地理東向，y軸指向地理北向，z軸垂直于xoy平面指向天向，x軸、y軸和z軸構(gòu)成右手坐標(biāo)系；

步驟二、建立聲學(xué)基陣坐標(biāo)系a，并確定基元在聲學(xué)基陣坐標(biāo)系下的位置；

步驟三、USBL測(cè)量的聲信號(hào)在各基元和信標(biāo)間的傳播時(shí)延為傳播時(shí)延差為下標(biāo)表示第j個(gè)基元測(cè)量的傳播時(shí)延與1號(hào)基元測(cè)量的傳播時(shí)延之差，其中j＝2,...,I′，各基元在徑向方向相對(duì)信標(biāo)的運(yùn)動(dòng)速度為

步驟四、根據(jù)SINS計(jì)算水下航行器的姿態(tài)、速度和位置；

結(jié)合聲學(xué)基陣坐標(biāo)系與水下航行器坐標(biāo)系的安裝偏差校準(zhǔn)結(jié)果，得到基元在計(jì)算導(dǎo)航坐標(biāo)系下的位置；

步驟五、結(jié)合聲信號(hào)在水下航行器與信標(biāo)之間傳播的有效聲速，根據(jù)SINS轉(zhuǎn)換的基元在計(jì)算導(dǎo)航坐標(biāo)系中的位置信息，計(jì)算聲信號(hào)在各基元與信標(biāo)間的傳播時(shí)延和傳播時(shí)延差；

步驟六、將SINS解算的水下航行器運(yùn)動(dòng)速度沿徑向分解；

步驟七、建立基于傳播時(shí)延、傳播時(shí)延差和徑向速度信息融合的擴(kuò)展卡爾曼濾波器狀態(tài)方程和觀測(cè)方程；

步驟八、計(jì)算SINS誤差，修正SINS輸出，重置擴(kuò)展卡爾曼濾波器狀態(tài)為零，重新執(zhí)行步驟三～步驟八，直至導(dǎo)航結(jié)束。

2.根據(jù)權(quán)利要求1所述一種引入徑向速度的SINS/USBL緊組合導(dǎo)航定位方法，其特征在于：所述步驟二中建立聲學(xué)基陣坐標(biāo)系a，并確定基元在聲學(xué)基陣坐標(biāo)系下的位置；具體過程為：

聲學(xué)基陣坐標(biāo)系是指以基陣中心為原點(diǎn)，y軸沿基陣平面指向水下航行器的艏向方向，z軸垂直于基陣平面向上，x軸與y軸、z軸構(gòu)成右手坐標(biāo)系；

基元在聲學(xué)基陣坐標(biāo)系下的坐標(biāo)為：

式(1)中，i為基元序號(hào)，I′為基元總數(shù)，為基元i在聲學(xué)基陣坐標(biāo)系x軸下的坐標(biāo)，r為基線長度，為基元i在聲學(xué)基陣坐標(biāo)系y軸下的坐標(biāo)，為基元i在聲學(xué)基陣坐標(biāo)系z(mì)軸下的坐標(biāo)。

3.根據(jù)權(quán)利要求1或2所述一種引入徑向速度的SINS/USBL緊組合導(dǎo)航定位方法，其特征在于：所述步驟三中USBL測(cè)量的聲信號(hào)在各基元和信標(biāo)間的傳播時(shí)延為傳播時(shí)延差為下標(biāo)表示第j個(gè)基元測(cè)量的傳播時(shí)延與1號(hào)基元測(cè)量的傳播時(shí)延之差，其中j＝2,...,I′，各基元在徑向方向相對(duì)信標(biāo)的運(yùn)動(dòng)速度為具體過程為：

將和表示為：

式中，τ_i為第i個(gè)基元測(cè)量的傳播時(shí)延真值，n_i為第i個(gè)基元的測(cè)量噪聲，τ_1j為第j個(gè)基元測(cè)量的傳播時(shí)延與1號(hào)基元測(cè)量的傳播時(shí)延差真值，n_1j為時(shí)延差測(cè)量噪聲，v_ri為第i個(gè)基元測(cè)量的徑向速度真值，n_ri為第i個(gè)基元的徑向速度測(cè)量噪聲。