本發(fā)明屬于緊耦合導(dǎo)航算法技術(shù)領(lǐng)域，具體涉及一種多應(yīng)答器SINS/USBL組合導(dǎo)航算法。
背景技術(shù)：
捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)能夠為水下載體提供完備導(dǎo)航信息，但其誤差隨時間積累；超短基線(USBL)定位系統(tǒng)操作簡單、便攜性強、成本較低，能夠測量應(yīng)答器相對于基陣的相對位置信息(包括角度信息和斜距信息)，可作為輔助導(dǎo)航系統(tǒng)，用來抑制慣導(dǎo)系統(tǒng)誤差的發(fā)散。SINS/USBL組合導(dǎo)航能夠集成二者的優(yōu)點，實現(xiàn)互補。
現(xiàn)有USBL設(shè)備的普遍特點是測距精度高而測角精度相對較低。此外，常規(guī)的單應(yīng)答器SINS/USBL組合導(dǎo)航系統(tǒng)使用前都要預(yù)先標定USBL到SINS的安裝角，準備工作繁瑣。國內(nèi)外現(xiàn)有的單應(yīng)答器SINS/USBL組合導(dǎo)航方案，其定位精度很大程度上受到USBL測角誤差以及二者安裝角誤差的限制，不能滿足部分水下高精度導(dǎo)航的需求。
因此，亟需研制一種SIN/SUSBL緊耦合算法，以解決SINS/USBL組合導(dǎo)航時存在的不足。
技術(shù)實現(xiàn)要素：
本發(fā)明要解決的技術(shù)問題是提供一種SINS/USBL緊耦合算法，以獲得比現(xiàn)有單應(yīng)答器SINS/USBL組合導(dǎo)航算法更高的定位精度。
為了實現(xiàn)這一目的，本發(fā)明采取的技術(shù)方案是：
一種SINSUSBL緊耦合算法，適用于多應(yīng)答器SINS/USBL組合導(dǎo)航，包括 以下步驟：
(1)確定超短基線定位系統(tǒng)USBL原理
三應(yīng)答器USBL的基本工作原理是以三個位置已知的應(yīng)答器到基陣中心空間距離為半徑的球面交會；i＝1,2,3表示三個應(yīng)答器，其在地球直角坐標系中的位置為USBL測得的三個斜距值為Ri以及基陣位置為(Xe,Ye,Ze)，根據(jù)空間球面交會原理，得到如下的參數(shù)方程組：
式1
利用事先標定測量得到的應(yīng)答器位置和USBL實時測量的斜距值，求解該方程組得到基陣的絕對位置；
(2)構(gòu)建緊耦合導(dǎo)航算法濾波模型
選取慣導(dǎo)載體坐標系b為前上右坐標系，導(dǎo)航坐標系n為北天東坐標系；
選取傳統(tǒng)15維慣導(dǎo)系統(tǒng)誤差狀態(tài)量，速度誤差δVn＝[δVN δVU δVE]T，姿態(tài)誤差φn＝[φN φU φE]T，位置誤差陀螺漂移ε＝[ε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測距精度誤差擴展為系統(tǒng)狀態(tài)：
XUSBL3＝[δKu1 δKu2 δKu3]T 式4
USBL測距精度誤差δKui視為隨機常數(shù)，i＝1、2、3，USBL部分對應(yīng)的卡爾曼 濾波狀態(tài)方程為：
式5
其中：
FUSBL3＝[03×3] 式6
18維的三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法誤差狀態(tài)量為：
X＝[XSINS XUSBL3]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)航算法觀測模型
以SINS相對三個應(yīng)答器的距離信息和USBL直接測量得到的載體相對于三個應(yīng)答器的斜距信息之差為觀測量；
在地球直角坐標系中，利用慣導(dǎo)位置(Xs,Ys,Zs)和三個應(yīng)答器位置構(gòu)造相對距離為：
式10
對式10求偏微分：
式11
式12
應(yīng)答器i的位置為慣導(dǎo)解算的位置為利用地球定位中球面坐標系到直角坐標系的轉(zhuǎn)換公式，二者在地球直角坐標系的相對位置為
式13
其中，RN表示地球卯酉圈曲率半徑，將式(13)化簡得：
式14
聯(lián)立式12和式14得：
式15
其中：
USBL實際測距表示USBL測量得到的慣導(dǎo)位置和第i個答器之間的距離，真實距離Ri表示實際的慣導(dǎo)位置和第i個答器之間的距離；USBL實際測距真實距離Ri和USBL測距精度誤差δKui之間有如下關(guān)系：
式16
(4)確定三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法的觀測量為：
式17
三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法觀測方程為：
式18
三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法觀測矩陣為：
H＝[03×6 AsBs 03×6 diag[R1 R2 R3]] 式19
三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法量測方程為：
Z＝HX+v 式20
(5)確定緊耦合導(dǎo)航算法濾波方程
對式(8)和式(20)進行離散化處理，得到SINS/USBL組合導(dǎo)航的卡爾曼濾波模型為：
其中k和k+1表示時刻。
進一步的，如上所述的一種SINSUSBL緊耦合算法，用于多應(yīng)答器SINS/USBL組合導(dǎo)航，應(yīng)答器數(shù)量大于3個時，i＝1,2,3…n，n＞3，采用步驟(1)～(5)的方法確定SINSUSBL緊耦合算法。
本發(fā)明技術(shù)方案的有益效果在于：通過采用標準卡爾曼濾波算法基本方程，選擇合適的濾波初值(狀態(tài)初始值，初始估計均方誤差陣，系統(tǒng)噪聲初始方差陣以及量測噪聲方差陣)，進行濾波估計，并實時修正速度、姿態(tài)和位置信息，；設(shè)計了一種新型的多應(yīng)答器SINS/USBL組合導(dǎo)航方案；并對多應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法進行誤差建模與仿真分析。本發(fā)明能夠避免USBL測角誤差和USBL到SINS安裝角誤差等對定位精度帶來的影響，且無需對USBL到SINS的安裝角進行標定，使用更加方便。仿真結(jié)果表明，本發(fā)明能夠獲得比現(xiàn)有單應(yīng)答器SINS/USBL組合導(dǎo)航算法更高的定位精度。
具體實施方式
下面結(jié)合具體實施例對本發(fā)明技術(shù)方案進行詳細說明。
本發(fā)明一種SINSUSBL緊耦合算法，適用于多應(yīng)答器SINS/USBL組合導(dǎo)航，包括以下步驟：
(1)確定超短基線定位系統(tǒng)USBL原理
三應(yīng)答器USBL的基本工作原理是以三個位置已知的應(yīng)答器到基陣中心空間距離為半徑的球面交會；i＝1,2,3表示三個應(yīng)答器，其在地球直角坐標系中的位置為USBL測得的三個斜距值為Ri以及基陣位置為(Xe,Ye,Ze)，根據(jù)空間球面交會原理，得到如下的參數(shù)方程組：
式(1)
利用事先標定測量得到的應(yīng)答器位置和USBL實時測量的斜距值，求解該方程組得到基陣的絕對位置；
(2)構(gòu)建緊耦合導(dǎo)航算法濾波模型
選取慣導(dǎo)載體坐標系b為前上右坐標系，導(dǎo)航坐標系n為北天東坐標系；
選取傳統(tǒng)15維慣導(dǎo)系統(tǒng)誤差狀態(tài)量，速度誤差δVn＝[δVN δVU δVE]T，姿態(tài)誤差φn＝[φN φU φE]T，位置誤差陀螺漂移ε＝[ε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測距精度誤差擴展為系統(tǒng)狀態(tài)：
XUSBL3＝[δKu1 δKu2 δKu3]T 式(4)
USBL測距精度誤差δKui視為隨機常數(shù)，i＝1、2、3，USBL部分對應(yīng)的卡爾曼濾波狀態(tài)方程為：
式(5)
其中：
FUSBL3＝[03×3] 式(6)
18維的三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法誤差狀態(tài)量為：
X＝[XSINS XUSBL3]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)航算法觀測模型
以SINS相對三個應(yīng)答器的距離信息和USBL直接測量得到的載體相對于三個應(yīng)答器的斜距信息之差為觀測量；
在地球直角坐標系中，利用慣導(dǎo)位置(Xs,Ys,Zs)和三個應(yīng)答器位置構(gòu)造相對距離為：
式(10)
對式(10)求偏微分：
式(11)
式(12)
應(yīng)答器i的位置為慣導(dǎo)解算的位置為利用地球定位中球面坐標系到直角坐標系的轉(zhuǎn)換公式，二者在地球直角坐標系的相對位置為
式(13)
其中，RN表示地球卯酉圈曲率半徑，將式(13)化簡得：
式(14)
聯(lián)立式(12)和式(14)得：
式(15)
其中：
USBL實際測距表示USBL測量得到的慣導(dǎo)位置和第i個答器之間的距離，真實距離Ri表示實際的慣導(dǎo)位置和第i個答器之間的距離；USBL實際測距真實距離Ri和USBL測距精度誤差δKui之間有如下關(guān)系：
式(16)
(4)確定三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法的觀測量為：
式(17)
三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法觀測方程為：
式(18)
三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法觀測矩陣為：
H＝[03×6 AsBs 03×6 diag[R1 R2 R3]] 式(19)
三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法量測方程為：
Z＝HX+v 式(20)
(5)確定緊耦合導(dǎo)航算法濾波方程
對式(8)和式(20)進行離散化處理，得到SINS/USBL組合導(dǎo)航的卡爾曼濾波模型為：
其中k和k+1表示時刻。
本實施例中所述的一種SINSUSBL緊耦合算法，應(yīng)答器數(shù)量為3個時，當應(yīng)答器數(shù)量大于3個時，i＝1,2,3…n，n＞3，采用步驟(1)～(5)的方法確定SINSUSBL緊耦合算法同樣適用。