技術(shù)特征：

1.一種SINSUSBL緊耦合算法，適用于多應(yīng)答器SINS/USBL組合導(dǎo)航，其特征在于，包括以下步驟：

(1)確定超短基線(xiàn)定位系統(tǒng)USBL原理

三應(yīng)答器USBL的基本工作原理是以三個(gè)位置已知的應(yīng)答器到基陣中心空間距離為半徑的球面交會(huì)；i＝1,2,3表示三個(gè)應(yīng)答器，其在地球直角坐標(biāo)系中的位置為USBL測(cè)得的三個(gè)斜距值為R_i以及基陣位置為(X^e,Y^e,Z^e)，根據(jù)空間球面交會(huì)原理，得到如下的參數(shù)方程組：

式(1)

利用事先標(biāo)定測(cè)量得到的應(yīng)答器位置和USBL實(shí)時(shí)測(cè)量的斜距值，求解該方程組得到基陣的絕對(duì)位置；

(2)構(gòu)建緊耦合導(dǎo)航算法濾波模型

選取慣導(dǎo)載體坐標(biāo)系b為前上右坐標(biāo)系，導(dǎo)航坐標(biāo)系n為北天東坐標(biāo)系；

選取傳統(tǒng)15維慣導(dǎo)系統(tǒng)誤差狀態(tài)量，速度誤差δV_n＝[δV_N δV_U δV_E]^T，姿態(tài)誤差φ_n＝[φ_N φ_U φ_E]^T，位置誤差陀螺漂移ε＝[ε_x ε_y ε_z]^T，加表零位▽?zhuān)絒▽_x ▽_y ▽_z]^T；

相應(yīng)捷聯(lián)慣導(dǎo)系統(tǒng)SINS誤差狀態(tài)量為：

式(2)

捷聯(lián)慣導(dǎo)系統(tǒng)誤差狀態(tài)方程為：

式(3)

將USBL測(cè)距精度誤差擴(kuò)展為系統(tǒng)狀態(tài)：

X_USBL3＝[δKu₁ δKu₂ δKu₃]^T 式(4)

USBL測(cè)距精度誤差δKu_i視為隨機(jī)常數(shù)，i＝1、2、3，USBL部分對(duì)應(yīng)的卡爾曼濾波狀態(tài)方程為：

式(5)

其中：

F_USBL3＝[0_3×3] 式(6)

18維的三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法誤差狀態(tài)量為：

X＝[X_SINS X_USBL3]^T 式(7)

SINS/USBL組合導(dǎo)航的卡爾曼濾波狀態(tài)方程為：

式(8)

其中，G為系統(tǒng)噪聲矩陣，W為系統(tǒng)噪聲，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣：

式(9)

公式(9)中，F(xiàn)_SINS為慣導(dǎo)部分系統(tǒng)矩陣；

(3)構(gòu)建緊耦合導(dǎo)航算法觀(guān)測(cè)模型

以SINS相對(duì)三個(gè)應(yīng)答器的距離信息和USBL直接測(cè)量得到的載體相對(duì)于三個(gè)應(yīng)答器的斜距信息之差為觀(guān)測(cè)量；

在地球直角坐標(biāo)系中，利用慣導(dǎo)位置(X_s,Y_s,Z_s)和三個(gè)應(yīng)答器位置構(gòu)造相對(duì)距離為：

式(10)

對(duì)式(10)求偏微分：

式(11)

式(12)

應(yīng)答器i的位置為慣導(dǎo)解算的位置為利用地球定位中球面坐標(biāo)系到直角坐標(biāo)系的轉(zhuǎn)換公式，二者在地球直角坐標(biāo)系的相對(duì)位置為

式(13)

其中，R_N表示地球卯酉圈曲率半徑，將式(13)化簡(jiǎn)得：

式(14)

聯(lián)立式(12)和式(14)得：

式(15)

其中：

USBL實(shí)際測(cè)距表示USBL測(cè)量得到的慣導(dǎo)位置和第i個(gè)答器之間的距離，真實(shí)距離R_i表示實(shí)際的慣導(dǎo)位置和第i個(gè)答器之間的距離；USBL實(shí)際測(cè)距真實(shí)距離R_i和USBL測(cè)距精度誤差δKu_i之間有如下關(guān)系：

式(16)

(4)確定三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法的觀(guān)測(cè)量為：

式(17)

三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法觀(guān)測(cè)方程為：

式(18)

三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法觀(guān)測(cè)矩陣為：

H＝[0_3×6 A_sB_s 0_3×6 diag[R₁ R₂ R₃]] 式(19)

三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法量測(cè)方程為：

Z＝HX+v 式(20)

(5)確定緊耦合導(dǎo)航算法濾波方程

對(duì)式(8)和式(20)進(jìn)行離散化處理，得到SINS/USBL組合導(dǎo)航的卡爾曼濾波模型為：

式(21)

其中k和k+1表示時(shí)刻。

2.如權(quán)利要求1所述的一種SINSUSBL緊耦合算法，其特征在于，用于多應(yīng)答器SINS/USBL組合導(dǎo)航，應(yīng)答器數(shù)量大于3個(gè)時(shí)，i＝1,2,3…n，n＞3，采用步驟(1)～(5)的方法確定SINSUSBL緊耦合算法。