本發(fā)明主要涉及AUV水下導(dǎo)航技術(shù)領(lǐng)域，尤其涉及一種基于SINS/LBL緊組合的AUV水下導(dǎo)航定位方法，特別適用于水下自主航行器AUV的跟蹤定位。

背景技術(shù)：
AUV(AutonomousUnderwaterVehicle，自主式水下航行器)是一種可以完成水下探測、攻擊、運載、打撈等多種功能的水下工具，因其活動范圍廣、體積小、重量輕、隱蔽性高等特點，現(xiàn)已成為國內(nèi)外軍事海洋技術(shù)研究的一個重要方向。AUV水下高精度自主導(dǎo)航和定位跟蹤技術(shù)是完成其水下作業(yè)的前提和關(guān)鍵。在現(xiàn)有的定位技術(shù)中，SINS(StrapdownInertialNavigationSystems，捷聯(lián)式慣性導(dǎo)航系統(tǒng))因其具有隱蔽性強、自主性、抗干擾、數(shù)據(jù)更新頻率高、且在短時間內(nèi)具有較高精度等特點，因而成為AUV水下自主導(dǎo)航定位的首選定位方法。目前，盡管捷聯(lián)慣性導(dǎo)航技術(shù)的發(fā)展已日趨成熟，其導(dǎo)航定位誤差隨時間積累發(fā)散的這一動態(tài)特性卻未改變，在遠程、長期航行及武器發(fā)射等高精度導(dǎo)航時還不能完全滿足要求。組合導(dǎo)航技術(shù)的出現(xiàn)為這一問題的解決提供了一種有效途徑。LBL(LongBaseLine，長基線)水聲定位系統(tǒng)是由安裝在海底的基線長度為幾千米的應(yīng)答器基陣和安裝在載體上的問答機組成，其定位原理是利用載體上的問答機與海底應(yīng)答器陣之間的距離信息來求解AUV位置。LBL因其作用范圍廣、定位精度高已廣泛應(yīng)用于水下潛器。近年來，應(yīng)用于AUV的水下自主導(dǎo)航技術(shù)主要以SINS與DVL(DopplerVelocityLog，多普勒測速儀)的組合導(dǎo)航為主，輔以水面GPS(GlobalPositioningSystem，全球定位系統(tǒng))修正。在若干次實驗中取得了良好的導(dǎo)航精度，但航程相對較短，對于DVL，當(dāng)聲納傳感器遠離海底時測量速度精度很差，僅僅對AUV貼近海底時精度較好，而對于GPS，AUV需中斷潛行，浮到水面才能利用GPS信息，這在深海的情況下將浪費大量的時間與能源，嚴重影響AUV的水下作業(yè)效率。

技術(shù)實現(xiàn)要素：
針對現(xiàn)有AUV水下導(dǎo)航精度的問題，本發(fā)明提供了一種基于SINS/LBL緊組合的AUV水下導(dǎo)航定位方法。本發(fā)明的目的可以通過以下技術(shù)方案實現(xiàn)，具體為：(1)捷聯(lián)慣性導(dǎo)航系統(tǒng)SINS(1)通過捷聯(lián)解算得到相應(yīng)的包括AUV的位置信息的導(dǎo)航信息，解算的位置信息用地球大地坐標PSINS(LS,λS,hS)表示，并將PSINS(LS,λS,hS)轉(zhuǎn)化為用地球直角坐標PSINS(xS,yS,zS)表示；(2)SINS兩兩基元與目標斜距差推算模塊(3)根據(jù)SINS提供的AUV位置信息PSINS(xS,yS,zS)和水聽器基陣位置Pi(xi,yi,zi)推算SINS斜距差ρSINS；(3)SINS/LBL緊組合模塊(4)根據(jù)長基線水聲定位系統(tǒng)LBL(2)的定位特點建立LBL斜距差模型，將SINS斜距差ρSINS和LBL水聽器i(i＝1,2,3)與AUV之間的斜距與水聽器0與AUV之間的斜距之差ρLBL的差值作為外部觀測信息輸入到卡爾曼濾波器進行濾波；(4)校正模塊(5)根據(jù)SINS/LBL緊組合模塊(4)的卡爾曼濾波結(jié)果對SINS(1)進行校正，最終得到精確的AUV位置信息PAUV。所述的SINS兩兩基元與目標斜距差推算模塊(3)計算SINS斜距差的方法如下：(1)根據(jù)長基線水聲定位系統(tǒng)LBL中水聽器位置Pi(xi,yi,zi)和SINS解算AUV位置PSINS(xs,ys,zs)計算得到水聽器i(i＝1,2,3)與AUV之間的斜距與水聽器0與AUV之間的斜距之差(2)將ρSINSi利用泰勒級數(shù)線性化。設(shè)AUV真實位置為PAUV(x,y,z),(δx,δy,δz)為SINS解算AUV位置的誤差，則xS＝x+δx,yS＝y(tǒng)+δy,zS＝z+δz。將ρSINSi泰勒級數(shù)展開取前兩項得：設(shè)同理其中，Gij(i＝0,1,2,3；j＝x,y,z)為已知量，可由SINS解算的概略位置PSINS(xS,yS,zS)和水底應(yīng)答器陣基元的位置Pi(xi,yi,zi)計算得到，由于SINS解算的概略位置PSINS(xS,yS,zS)可能有較大誤差，這樣在進行方程線性化時略去高階項會引起線性誤差，可以利用迭代法解算，即在第一次解之后，用它作為近似值再重新計算。設(shè)：eix＝Gix-G0x，eiy＝Giy-G0y，eiz＝Giz-G0z，i＝1,2,3于是：ρSINSi＝Ri-R0+(Gix-G0x)δx+(Giy-G0y)δy+(Giz-G0z)δz＝Ri-R0+eixδx+eiyδy+eizδz所述的SINS/LBL緊組合模塊(4)的具體實現(xiàn)步驟如下：(1)建立LBL斜距差模型由于時延差測量、聲傳播的多途徑效應(yīng)等將引起斜距差測量有誤差，為簡化模型，可認為斜距差誤差是由常值偏置和隨機噪聲組成，則LBL水聽器i(i＝1,2,3)與AUV的斜距與水聽器0與AUV的斜距之差可表示為：式中，ΔRmeas為LBL水聽器i(i＝1,2,3)與AUV的斜距與水聽器0與AUV的斜距之差，ΔR為斜距差真值，δR＝[δR1δR2δR3]T為隨機常值，νδR(t)～N(0,QΔR)為高斯白噪聲。(2)建立SINS/LBL緊組合狀態(tài)方程SINS/LBL緊組合狀態(tài)方程描述為：其中：XSINS為SINS的狀態(tài)向量，XLBL為LBL的狀態(tài)向量，F(xiàn)SINS為SINS的轉(zhuǎn)移矩陣，F(xiàn)LBL為LBL的轉(zhuǎn)移矩陣，WSINS為SINS的系統(tǒng)噪聲向量，WLBL為LBL的系統(tǒng)噪聲向量，F(xiàn)為緊組合系統(tǒng)轉(zhuǎn)移矩陣，X為緊組合系統(tǒng)狀態(tài)向量，W為緊組合系統(tǒng)噪聲向量。根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)長期工作時的誤差特點，選擇位置誤差、速度誤差、姿態(tài)誤差、陀螺漂移和加速度計零偏作為狀態(tài)量：XSINS＝[δVEδVNδVUφEφNφUδLδLδh▽bx▽by▽bzεbxεbyεbz]T式中，δVE、δVN、δVU分別是捷聯(lián)東向、北向、天向的速度誤差，分別是捷聯(lián)東向、北向、天向的失準角，δL、δλ、δh分別是捷聯(lián)緯度、經(jīng)度、高度誤差，三個位置誤差由地球坐標系描述，▽bx、▽by、▽bz是捷聯(lián)加表三個軸向的偏置誤差，εbx、εby、εbz是捷聯(lián)陀螺的三個軸向漂移。XLBL＝[δR1δR2δR3]T式中，δR1、δR2、δR3分別為LBL水聽器i(i＝1,2,3)與AUV的斜距與水聽器0與AUV的斜距之差的隨機常值漂移。系統(tǒng)噪聲陣WLBL＝[000]T系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣式中，其中：Fij為F9×9的元素RN為參考橢球體子午面內(nèi)的曲率半徑,RN＝Re(1-2e+3esin2L)RE為垂直子午面內(nèi)的曲率半徑,RE＝Re(1+esin2L)其中：Re為參考橢球體的長軸半徑；e為橢球的橢圓度。F37＝-2ωiecosLVEF57＝-ωiesinLCij為姿態(tài)轉(zhuǎn)移矩陣的元素FLBL＝03×3(3)建立SINS/LBL緊組合量測方程。緊組合系統(tǒng)采用SINS推算的水聽器與AUV的斜距差與LBL測量得到的斜距差之差作為觀測量。在緊組合系統(tǒng)中，設(shè)LBL測得的斜距差為ρLBLi,水底應(yīng)答器陣基元的位置為P(xi,yi,zi),SINS測得的AUV位置為PSINS(xS,yS,zS),由SINS測得的AUV位置PSINS(xS,yS,zS)和水底應(yīng)答器陣基元的位置為Pi(xi,yi,zi)所確定的斜距差為ρSINSi。SINS斜距差：ρSINSi＝Ri-R0+(Gix-G0x)δx+(Giy-G0y)δy+(Giz-G0z)δz＝Ri-R0+eixδx+eiyδy+eizδzLBL斜距差則量測可寫成則有：當(dāng)系統(tǒng)采用地球直角坐標系(Oxeyeze)作為導(dǎo)航坐標系時，可用上式構(gòu)造系統(tǒng)量測方程。實際應(yīng)用中是以經(jīng)緯度和高度定位的，因此要把dx,dy,dz用dl,dλ,dh表示。由量測方程為Z3×1＝H3×18X18×1+VΔR(3×1)式中，設(shè)其中aij(i＝1,2,3；j＝1,2,3)為矩陣H1的元素H1非零元素如下:ai1＝-(RN+h)sinLcosλei1-(RN+h)sinLsinλei2+[RN(1-e2)+h]ei3ai2＝-(RN+h)cosLsinλei1-(RN+h)cosLcosλei2ai3＝cosLcosλei1+cosLsinλei2+sinLei3(i＝1,2,3)所述的校正模塊(5)根據(jù)SINS/LBL緊組合模塊(4)的卡爾曼濾波結(jié)果對SINS(1)進行校正，最終得到精確的AUV位置信息PAUV。與現(xiàn)有技術(shù)相比，本發(fā)明具有如下優(yōu)點：(1)解決了SINS系統(tǒng)誤差隨時間積累的問題，保證了AUV在水下長期自主導(dǎo)航定位的精度，同時避免了GPS及其他無線電定位系統(tǒng)的使用，為水下作業(yè)節(jié)約時間和能耗，提高了AUV水下作業(yè)效率。(2)本發(fā)明重點介紹了SINS與LBL緊組合，對慣性導(dǎo)航系統(tǒng)與聲學(xué)系統(tǒng)組合應(yīng)用的研究有一定的意義。附圖說明圖1為SINS/LBL緊組合定位系統(tǒng)原理框圖；圖2為長基線水聲定位系統(tǒng)LBL示意圖；圖3為水聽器節(jié)點定位示意圖。具體實施方式下面結(jié)合附圖，進一步闡明本發(fā)明。如圖1所示，本發(fā)明由安裝在AUV上的捷聯(lián)慣性導(dǎo)航系統(tǒng)SINS(1)、布放在海底的長基線水聲定位系統(tǒng)LBL(2)和數(shù)據(jù)處理單元三大部分組成。數(shù)據(jù)處理單元包括SINS兩兩基元與AUV斜距差計算模塊(3)、SINS/LBL緊組合模塊(4)和校正模塊(5)。通過采用SINS與LBL緊組合的方法完成AUV水下自主導(dǎo)航，具體實現(xiàn)步驟如下：(1)對慣性測量元件(IMU)輸出數(shù)據(jù)通過捷聯(lián)解算獲得AUV位置信息，用地球大地坐標PSINS(LS,λS,hS)表示，并將PSINS(LS,λS,hS)轉(zhuǎn)化為用地球直角坐標PSINS(xS,yS,zS)表示。所述的SINS(1)系統(tǒng)包括IMU(InertialMeasurementUnit，慣性測量單元)元件及捷聯(lián)解算模塊，IMU元件用于得到慣性數(shù)據(jù)，捷聯(lián)解算模塊用于通過捷聯(lián)解算，得到導(dǎo)航信息，其中包括位置信息PSINS。1)SINS姿態(tài)矩陣及姿態(tài)角計算采用四元數(shù)法計算姿態(tài)矩陣，根據(jù)歐拉定理,動坐標系相對參考坐標系的方位等效于動坐標系繞某個等效轉(zhuǎn)軸轉(zhuǎn)動一個角度θ，如果用u表示等效轉(zhuǎn)軸方向的單位矢量，則動坐標系的方位完全由u和θ兩個參數(shù)來確定。用u和θ可構(gòu)造一個四元數(shù)：對上式求導(dǎo)并化簡可得四元數(shù)微分方程：式中根據(jù)畢卡逼近法求解四元數(shù)微分方程得：式中式中令地球坐標系相對慣性坐標系的自轉(zhuǎn)角速度為ωie，(其值為15.04088°/h)，L表示當(dāng)?shù)鼐暥?λ表示當(dāng)?shù)亟?jīng)度，則ωien：地球坐標系相對慣性坐標系的自轉(zhuǎn)角速度在地理坐標系中的矢量，為：ωieb：地球坐標系相對慣性坐標系的自轉(zhuǎn)角速度在載體坐標系中的矢量，為：式中的姿態(tài)矩陣在載體靜止時，由初始角度決定；當(dāng)載體相對地理坐標系轉(zhuǎn)動時，姿態(tài)矩陣跟著變化，由四元數(shù)即時修正后求得(下同)。ωenn:地理坐標相對地球坐標系轉(zhuǎn)動角速度在地理坐標系中的矢量，為：VE、VN分別為載體運動的東向和北向速度；RN為參考橢球體子午面內(nèi)的曲率半徑，RN＝Re(1-2e+3esin2L)；RE為垂直子午面的法線平面內(nèi)的曲率半徑，RE＝Re(1+esin2L)；其中Re為參考橢球體的長軸半徑；e為橢球的橢圓度。又因為,則ωenb:地理坐標相對地球坐標系轉(zhuǎn)動角速度在載體坐標系中的矢量，為：ωibb：陀螺輸出角速度，記為ωnbb：載體坐標系相對地理坐標系的轉(zhuǎn)動角速度在載體坐標系中的矢量，記為則可得ωnbb＝ωibb-ωieb-ωenb四元數(shù)即時修正后，根據(jù)下式可由四元數(shù)的元實時更新姿態(tài)矩陣從姿態(tài)陣中即可提取實時姿態(tài)角2)SINS速度計算載體坐標系中的比力矢量為fb，則地理坐標系中有：式中的方向余弦矩陣在載體靜止時，由初始角度決定；當(dāng)載體相對地理坐標系轉(zhuǎn)動時，方向余弦矩陣跟著變化，由四元數(shù)即時修正后求得。載體在慣導(dǎo)系內(nèi)的比力方程為：寫成分量形式有：式中：fn為載體加速度在導(dǎo)航坐標系上的投影，fn＝[fEfNfU]T；Vn表示船體在導(dǎo)航坐標系中的速度矢量，Vn＝[VEVNVU]T；gn為重力加速度矢量，gn＝[00-g]T。積分上式，即可求得運載體在導(dǎo)航坐標系上的各個速度分量VE、VN、VU。3)位置計算得到經(jīng)緯度高度的微分方程可表示如下：式中，h為高度。積分上式的經(jīng)緯度高度的更新公式即可得到經(jīng)緯度和高度：則得到位置P(λ,L,h)。4)將3)得到的AUV在地球直角坐標系的坐標PSINS(LS,λS,hS)轉(zhuǎn)化為其在地球大地坐標系的坐標PSINS(xS,yS,zS)?？捎晒将@得PSINS(xS,yS,zS)。式中:RN為參考橢球體子午面內(nèi)的曲率半徑，RN＝Re(1-2e+3esin2L)RE為垂直子午面內(nèi)的曲率半徑，RE＝Re(1+esin2L)其中：Re為參考橢球體的長軸半徑；e為橢球的橢圓度。(2)SINS兩兩基元與目標斜距差推算1)根據(jù)SINS解算的AUV位置PSINS(xs,ys,zs)和長基線水聲定位系統(tǒng)LBL中水聽器基元位置Pi(xi,yi,zi)計算得到水聽器i(i＝1,2,3)與AUV之間的斜距與水聽器0與AUV之間的斜距之差所述的長基線水聲定位系統(tǒng)LBL(2)由布放在海底的四個位置已知的水聽器組成，如圖2所示，各水聽器之間的距離為4km。如圖3所示，利用母船，采用超短基線系統(tǒng)對水聽器進行精確定位，計算精確坐標值。母船上安裝有GPS、IMU和羅經(jīng)，母船底安裝有換能器基陣。根據(jù)超短基線系統(tǒng)計算出每個水聽器在換能器基陣坐標下的相對位置，結(jié)合母船GPS位置、母船姿態(tài)以及各安裝誤差等因素可以計算出各水聽器節(jié)點在地球坐標下的絕對位置。2)將ρSINSi利用泰勒級數(shù)線性化。設(shè)AUV真實位置為PAUV(x,y,z),(δx,δy,δz)為SINS解算AUV位置的誤差，則xS＝x+δx,yS＝y(tǒng)+δy,zS＝z+δz。將ρSINSi泰勒級數(shù)展開取前兩項得：設(shè)同理其中，Gij(i＝0,1,2,3；j＝x,y,z)為已知量，可由SINS解算的概略位置PSINS(xS,yS,zS)和水底應(yīng)答器陣基元的位置Pi(xi,yi,zi)計算得到，由于SINS解算的概略位置PSINS(xS,yS,zS)可能有較大誤差，這樣在進行方程線性化時略去高階項會引起線性誤差，可以利用迭代法解算，即在第一次解之后，用它作為近似值再重新計算。設(shè)：eix＝Gix-G0x，eiy＝Giy-G0y，eiz＝Giz-G0z，i＝1,2,3于是：ρSINSi＝Ri-R0+(Gix-G0x)δx+(Giy-G0y)δy+(Giz-G0z)δz＝Ri-R0+eixδx+eiyδy+eizδz(3)SINS/LBL緊組合1)建立LBL斜距差模型由于時延差測量、聲傳播的多途徑效應(yīng)等將引起斜距差測量有誤差，為簡化模型，可認為斜距差誤差是由常值偏置和隨機噪聲組成，則LBL水聽器i(i＝1,2,3)與AUV的斜距與水聽器0與AUV的斜距之差可表示為：式中，ΔRmeas為LBL水聽器i(i＝1,2,3)與AUV的斜距與水聽器0與AUV的斜距之差，ΔR為斜距差真值，δR＝[δR1δR2δR3]T為隨機常值，νδR(t)～N(0,QΔR)為高斯白噪聲。2)建立SINS/LBL緊組合狀態(tài)方程SINS/LBL緊組合狀態(tài)方程描述為：其中：XSINS為SINS的狀態(tài)向量，XLBL為LBL的狀態(tài)向量，F(xiàn)SINS為SINS的轉(zhuǎn)移矩陣，F(xiàn)LBL為LBL的轉(zhuǎn)移矩陣，WSINS為SINS的系統(tǒng)噪聲向量，WLBL為LBL的系統(tǒng)噪聲向量，F(xiàn)為緊組合系統(tǒng)轉(zhuǎn)移矩陣，X為緊組合系統(tǒng)狀態(tài)向量，W為緊組合系統(tǒng)噪聲向量。根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)長期工作時的誤差特點，選擇位置誤差、速度誤差、姿態(tài)誤差、陀螺漂移和加速度計零偏作為狀態(tài)量：XSINS＝[δVEδVNδVUφEφNφUδLδLδh▽bx▽by▽bzεbxεbyεbz]T式中，δVE、δVN、δVU分別是捷聯(lián)東向、北向、天向的速度誤差，分別是捷聯(lián)東向、北向、天向的失準角，δL、δλ、δh分別是捷聯(lián)緯度、經(jīng)度、高度誤差，三個位置誤差由地球坐標系描述，▽bx、▽by、▽bz是捷聯(lián)加表三個軸向的偏置誤差，εbx、εby、εbz是捷聯(lián)陀螺的三個軸向漂移。XLBL＝[δR1δR2δR3]T式中，δR1、δR2、δR3分別為LBL水聽器i(i＝1,2,3)與AUV的斜距與水聽器0與AUV的斜距之差的隨機常值誤差。系統(tǒng)噪聲陣WLBL＝[000]T系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣式中，其中：Fij為F9×9的元素,RN為參考橢球體子午面內(nèi)的曲率半徑，RN＝Re(1-2e+3esin2L)RE為垂直子午面內(nèi)的曲率半徑，RE＝Re(1+esin2L)其中：Re為參考橢球體的長軸半徑；e為橢球的橢圓度。F37＝-2ωiecosLVEF57＝-ωiesinLCij為姿態(tài)轉(zhuǎn)移矩陣的元素FLBL＝03×33)建立SINS/LBL緊組合量測方程緊組合系統(tǒng)采用SINS推算的水聽器與AUV的斜距差與LBL測量得到的斜距差之差作為觀測量。在緊組合系統(tǒng)中，設(shè)LBL測得的斜距差為ρLBLi,水底應(yīng)答器陣基元的位置為P(xi,yi,zi),SINS測得的AUV位置為PSINS(xS,yS,zS),由SINS測得的AUV位置PSINS(xS,yS,zS)和水底應(yīng)答器陣基元的位置為Pi(xi,yi,zi)所確定的斜距差為ρSINSi。SINS斜距差：LBL斜距差則量測可寫成則有：當(dāng)系統(tǒng)采用地球直角坐標系(Oxeyeze)作為導(dǎo)航坐標系時，可用上式構(gòu)造系統(tǒng)量測方程。實際應(yīng)用中是以經(jīng)緯度和高度定位的，因此要把dx,dy,dz用dl,dλ,dh表示。由量測方程為Z3×1＝H3×18X18×1+VΔR(3×1)式中，其中aij(i＝1,2,3；j＝1,2,3)為矩陣H1的元素H1非零元素如下:ai1＝-(RN+h)sinLcosλei1-(RN+h)sinLsinλei2+[RN(1-e2)+h]ei3ai2＝-(RN+h)cosLsinλei1-(RN+h)cosLcosλei2ai3＝cosLcosλei1+cosLsinλei2+sinLei3(i＝1,2,3)4)系統(tǒng)狀態(tài)方程及量測方程的離散化Xk＝φk,k-1Xk-1+Γk-1Wk-1Zk＝HkXk+Vk式中，Xk為k時刻的狀態(tài)向量，也就是被估計矢量；Zk為k時刻的測量序列；Wk-1為k-1時刻的系統(tǒng)噪聲；Vk為k時刻的測量噪聲序列；Φk,k-1為k-1時刻到k時刻的一步狀態(tài)轉(zhuǎn)移矩陣；Γk-1是系統(tǒng)噪聲輸入矩陣，Hk為k時刻的測量矩陣，利用標準卡爾曼濾波方程計算狀態(tài)的最優(yōu)估計：狀態(tài)一步預(yù)測向量Xk/k-1＝φk,k-1Xk-1狀態(tài)估值計算Xk＝Xk/k-1+Kk(Zk-HkXk/k-1)濾波增益Kk＝Pk/k-1HkT(HkPk/k-1HkT+Rk)-1一步預(yù)測均方誤差矩陣估計均方誤差方程(4)校正根據(jù)濾波得到的狀態(tài)估計，通過下述方法進行校正。1)速度和位置校正下次濾波前，每次捷聯(lián)解算得到的速度和位置均通過下式進行校正：2)慣性儀表輸出校正下次濾波前，每次捷聯(lián)解算時所需的慣性儀表輸出在使用前通過下式進行校正：3)姿態(tài)矩陣、四元數(shù)校正姿態(tài)校正：下次濾波前，按下式對每次捷聯(lián)解算得到的進行校正。四元數(shù)校正：因為捷聯(lián)解算采用的是四元數(shù)算法，算法中是是采用四元數(shù)進行迭代更新的，所有還需對四元數(shù)進行校正。四元數(shù)可由更新的姿態(tài)矩陣轉(zhuǎn)換得到。