本發(fā)明屬于慣導(dǎo)對準(zhǔn)技術(shù)的方法，涉及一種基于逆向?qū)Ш剿惴ǖ慕萋?lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合對準(zhǔn)方法的研究。

背景技術(shù)：

自主水下航行器((Autonomous Underwater Vehicle，簡稱AUV)是執(zhí)行海洋軍事任務(wù)和進(jìn)行海洋開發(fā)的重要工具。當(dāng)AUV在進(jìn)行任務(wù)前，慣導(dǎo)需要對系統(tǒng)進(jìn)行初始對準(zhǔn)，其中快速性和精確性是對系統(tǒng)要求的重要指標(biāo)，一般來說二者的關(guān)系是互相矛盾、相互制約的。不同于靜基座對準(zhǔn)，行進(jìn)間對準(zhǔn)需要外部的輔助設(shè)備提供載體的運(yùn)動(dòng)信息，通過輔助設(shè)備量測的信息對慣導(dǎo)系統(tǒng)進(jìn)行修正補(bǔ)償。在非常時(shí)期航行器必須有快速機(jī)動(dòng)，并精確導(dǎo)航，兼具水下隱蔽性等要求。因此解決對準(zhǔn)的精確性和快速性問題成為AUV航行任務(wù)中的重要議題。然而傳統(tǒng)的對準(zhǔn)技術(shù)需要利用岸上平臺(tái)進(jìn)行傳遞對準(zhǔn)，或利用價(jià)值昂貴的輔助設(shè)備進(jìn)行組合對準(zhǔn)，這給AUV在快速投入作戰(zhàn)的需要和實(shí)際工程開發(fā)應(yīng)用中帶來巨大挑戰(zhàn)，因此，必須對其進(jìn)行適當(dāng)改善，在解決此類問題的同時(shí)，適應(yīng)時(shí)局的發(fā)展。

在水下領(lǐng)域，作為比較成熟的組合導(dǎo)航方式，SINS/DVL(Strap-down Inertial Navigation System,捷聯(lián)慣性導(dǎo)航系統(tǒng))組合導(dǎo)航系統(tǒng)利用DVL(Doppler Velocity Log,多普勒計(jì)程儀)提供的速度信息修正SINS的量測信息，以此抑制捷聯(lián)慣導(dǎo)系統(tǒng)的誤差積累，是目前應(yīng)用比較廣泛的水下組合導(dǎo)航技術(shù)。然而，由于DVL價(jià)值昂貴，并且會(huì)在工作時(shí)會(huì)向外發(fā)射聲波，暴露自身的位置，所以不能很好的滿足隱蔽性的要求。同時(shí)，DVL的有效測速度范圍為±10m/s，當(dāng)航行器處于相對速度較高的狀態(tài)下，DVL亦不能保證有效的測量精度，因此限制了該方式的應(yīng)用。

轉(zhuǎn)速計(jì)憑借其低廉的價(jià)格并且能夠測得在高速運(yùn)動(dòng)下航行器軸向速度的特點(diǎn)，可以有效解決上述的問題。通過選取轉(zhuǎn)速計(jì)作為捷聯(lián)慣導(dǎo)系統(tǒng)的輔助設(shè)備，測量載體在高速運(yùn)動(dòng)下的速度信息，利用量測的速度信息修正慣導(dǎo)系統(tǒng)，并借助正向和逆向結(jié)合的導(dǎo)航算法對獲取的數(shù)據(jù)信息反復(fù)處理，以加大對數(shù)據(jù)的利用，進(jìn)而滿足系統(tǒng)的對準(zhǔn)精度和快速性的要求。

因此，本文將逆向?qū)Ш剿惴ㄒ虢萋?lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合對準(zhǔn)方法。通過將捷聯(lián)慣導(dǎo)與轉(zhuǎn)速計(jì)的速度信息進(jìn)行信息融合，利用正逆向?qū)Ш剿惴ń惶嫣幚?，在增大對獲取的數(shù)據(jù)信息的同時(shí)，有效提高系統(tǒng)的對準(zhǔn)精度，縮短系統(tǒng)的對準(zhǔn)時(shí)間。

技術(shù)實(shí)現(xiàn)要素：

為了避免現(xiàn)階段對準(zhǔn)技術(shù)的不足之處，本發(fā)明提出了捷聯(lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合對準(zhǔn)的方法，并引入逆向?qū)Ш剿惴?，通過獲取捷聯(lián)慣導(dǎo)與轉(zhuǎn)速計(jì)的采樣數(shù)據(jù)，并將二者加以濾波融合，然后利用正逆向?qū)Ш剿惴ǎ瑢Σ蓸与A段的信息進(jìn)行多次反復(fù)處理，得到更為精確的對準(zhǔn)信息。這種方法在提高系統(tǒng)對準(zhǔn)精確度的同時(shí)，還能有效縮短系統(tǒng)在對準(zhǔn)過程的時(shí)間，進(jìn)而提升系統(tǒng)的整體性能。

一種基于逆向?qū)Ш剿惴ǖ慕萋?lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合的對準(zhǔn)方法，其特征在于步驟如下：

步驟1：建立捷聯(lián)慣導(dǎo)與轉(zhuǎn)速計(jì)對準(zhǔn)模型：

轉(zhuǎn)態(tài)方程：

觀測方程：Z_ST＝H_STX_ST+v_ST

上式中

其中，F(xiàn)_ST為系統(tǒng)狀態(tài)矩陣，H_ST為系統(tǒng)量測矩陣，系統(tǒng)噪聲w_ST和量測噪聲μ_ST統(tǒng)為零均值高斯白噪聲。V_N、V_U、V_E為載體在北天東方向上的速度，λ、L和h分別表示載體的經(jīng)緯度與高度，且高度信息可以通過深度傳感器直接測量得到。

建立正向?qū)Ш剿惴ㄎ⒎址匠?，對其進(jìn)行離散化：

姿態(tài)方程：

速度方程：

位置方程：

R_M,R_N分別為載體所在位置地球子午圈和卯酉圈曲率半徑，其近似計(jì)算公式為：

R_M≈R_e(1-2e+3esin²L)

R_N≈R_e(1+esin²L)

步驟2：卡爾曼濾波初始化。

一：首先，對捷聯(lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合系統(tǒng)的狀態(tài)變量初值進(jìn)行初始化，確定其在誤差干擾下的狀態(tài)初值x0＝[phi；dvn；dpos；ed；db]，其中平臺(tái)失準(zhǔn)角誤差phi＝[10；60；-10]*(pi/180/60)，位置誤差dpos＝[10/6378160；10/6378160]、速度誤差dvn＝[0.5；0.5；0.5]，陀螺隨機(jī)漂移誤差eb＝[0.02；0.02；0.02]*(pi/180/3600)與加速度計(jì)零位誤差db＝[100；100；100]*(0.000001*9.78)，估計(jì)陀螺測量白噪聲web＝[0.02；0.02；0.02]*(pi/180/3600)，加速度計(jì)的測量白噪聲wdb＝[50；50；50]*(0.000001*9.78)，

設(shè)webq＝[web(1,1)^2；web(2,1)^2；web(3,1)^2]，

wdbq＝[wdb(1,1)^2；wdb(2,1)^2；wdb(3,1)^2]。

可得系統(tǒng)方程中白噪聲均方差為w＝[web；wdb；webq；wdbq]，其中觀測方程中捷聯(lián)慣導(dǎo)SINS與轉(zhuǎn)速計(jì)的量測量之間的白噪聲均方差為v＝[0.01；0.01；0.01]。

然后，對狀態(tài)初值及相應(yīng)誤差函數(shù)分別賦值給相應(yīng)參數(shù)。phi＝x0(1:3)；dvn＝x0(4:6)；Qtd＝w(7:12)；Qt_d＝diag(Qtd)；Rk_d＝diag(v)^2；Pkd＝x0(1:14)；Pk_d＝10*diag(Pkd)^2；Xk_d＝zeros(14,1)；eb＝x0(9:11)；db＝x0(12:14)；web＝w(1:3)；wdb＝x0(4:6)。

其中，Qt_d、Rk_d和Pk_d分別為系統(tǒng)噪聲協(xié)方差驅(qū)動(dòng)矩陣、量測噪聲協(xié)方差陣和一步預(yù)測均方差陣。

二：對系統(tǒng)的姿態(tài)角、速度與位置進(jìn)行初始化。載體初始姿態(tài)角att＝[0；0；0]*(pi/180)；載體初始速度vb＝[8；0；0]；載體的初始位置pos＝[24*(pi/180)+35*(pi/180/60)；120*(pi/180)+58*(pi/180/60)；-10]。

然后對載體的軌跡進(jìn)行設(shè)置后保存到tr(1:15)函數(shù)，該函數(shù)保存的是不含誤差的真實(shí)值，包括載體的三個(gè)姿態(tài)角分量(橫滾角、航向角及俯仰角)、三個(gè)速度分量(北向、天向及東向)、三個(gè)位置分量(經(jīng)緯度及高度)、三個(gè)角速率分量個(gè)三個(gè)比力分量。

三：捷聯(lián)慣導(dǎo)解算參數(shù)初始化。將載體真實(shí)的姿態(tài)角矩陣轉(zhuǎn)換為四元數(shù)形式，然后加入平臺(tái)誤差角，即qnb＝tr(1:3)+phi，且系統(tǒng)的狀態(tài)變量初始值Xk_d＝zeros(14,0)；將載體真實(shí)的速度信息加入速度誤差，即vnm＝tr(4:6)+dvn；將載體真實(shí)的位置信息加入位置誤差，即posm＝tr(7:9)+[x0(7)；x0(8)；0.1]；最后，將載體輸出的陀螺儀與加速度計(jì)信息保存到wvm2函數(shù)，即wvm2＝tr(10:15)。

步驟3：步驟2完成后，對載體的姿態(tài)、速度與位置信息進(jìn)行組合濾波。首先對誤差估計(jì)函數(shù)進(jìn)行初始化置0，inse(1,:)＝zeros(14,1)；然后將載體真實(shí)的姿態(tài)信息tr(1:3)與加入誤差的實(shí)際值qnb中提取姿態(tài)誤差信息，具體步驟為：先將qnb轉(zhuǎn)化為共軛形式，再與姿態(tài)陣tr(1:3)的四元數(shù)形式做差乘得到姿態(tài)誤差信息；其次，將載體的實(shí)際速度值與真實(shí)值做差，求得速度誤差信息，即vnm-tr(4:6)；最后將載體實(shí)際的位置信息與真實(shí)值做差，求得位置誤差值，即posm(1)-tr(7),posm(2)-tr(8)。

步驟4：設(shè)置時(shí)間循環(huán)(for k＝2:2:s_time*100)讀取tr函數(shù)的角增量與比力增量，并對角增量與比力增量進(jìn)行更新，對更新的角增量與比力增量信息保存到擴(kuò)維后的tr函數(shù)中。即wvm0＝wvm2,wvm1＝tr(10:15),wvm2＝tr((15+10):(15+15))。其中，s_time為系統(tǒng)仿真時(shí)間。

獲取角增量與比力增量，加入誤差，并保存：

dwvm1＝wvm1-wvm0+([eb；db]+[web；wdb].randn(6,1))*Tm/2

dwvm2＝wvm2-wvm1+([eb；db]+[web；wdb].randn(6,1))*Tm/2

其中，Tm＝0.02為捷聯(lián)慣導(dǎo)解算周期。

步驟5：以獲取角增量與比力增量對陀螺儀增量與加速度計(jì)增量擴(kuò)維，進(jìn)行二子樣解算，即

wm＝[dwvm1(1:3,:),dwvm2(1:3,:)]

vm＝[dwvm1(4:6,:),dwvm2(4:6,:)]

對載體的深度用深度傳感器測量得到，加入相應(yīng)誤差，即posm(3,1)＝tr(9)+0.05*randn(1)

步驟6:將步驟5得到含有誤差的實(shí)際增量值進(jìn)行慣導(dǎo)解算，根據(jù)上一時(shí)刻的姿態(tài)四元素、速度、位置、陀螺儀與加速度計(jì)的測量值，計(jì)算當(dāng)前時(shí)刻的姿態(tài)四元數(shù)、速度與位置。

一：計(jì)算地球自轉(zhuǎn)角速度引起的陀螺感應(yīng)值

wnie＝wie*[cos(posm_1(1))；sin(posm_1(1))；0]，其中wie＝0.0000729215為地球自轉(zhuǎn)角速率；posm_1為前一時(shí)刻載體的位置信息。

二：計(jì)算載體在導(dǎo)航系下由北向與東向速度引起的陀螺感應(yīng)值

wnen＝[vnm_1(3)/(Re*(1+e*sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))；

vnm_1(3)*(sin(posm_1(1))/cos(posm_1(1)))/(Re*(1+e*sin(posm_1(1))*

sin(posm_1(1)))+posm_1(3))；-vnm_1(1)/(Re*(1-2*e+3*e*

sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))]

其中，posm_1(3)為前一時(shí)刻載體的高度信息；Re＝6378160為地球半徑；e為地球橢圓率；vnm_1(3)為載體東向航行速度。

三：由上述兩個(gè)步驟，可得由載體運(yùn)動(dòng)速度和地球自轉(zhuǎn)引起的導(dǎo)航系下的陀螺角速率wnin＝wnie+wnen。

四：采用二子樣方法，計(jì)算陀螺儀與加速度計(jì)的輸出增量

sumw＝wm(:,1)+wm(:,2)

sumv＝vm(:,1)+vm(:,2)

對sumw進(jìn)行保存，即phim＝sumw。

五：計(jì)算二子樣等效旋轉(zhuǎn)矢量，更新phim。即：

phim＝phim+2/3*cross(wm(:,1),wm(:,2))

六：計(jì)算劃槳效應(yīng)補(bǔ)償dvsculm與速度旋轉(zhuǎn)效應(yīng)補(bǔ)償dvrotm。

dvsculm＝2/3*(cross(wm(:,1),vm(:,2))+cross(vm(:,1),wm(:,2)))

dvrotm＝1/2*cross(sumw,sumv)

七：根據(jù)上式對速度進(jìn)行更新。

vnm＝vnm_1+Cnn*Cnb*(dvm+dvrotm+dvsculm)+gn-cross(2*wnie+wnen,vnm_1))*Tm

其中，gn重加速度；Cnn為在導(dǎo)航系下載體的陀螺角速率由旋轉(zhuǎn)矢量轉(zhuǎn)化為矩陣形式；Cnb為載體的姿態(tài)矩陣。

步驟7:求解步驟1的系統(tǒng)的狀態(tài)矩陣Ft_d與觀測矩陣Hk_d，并進(jìn)行離散化，結(jié)果分別保存到Fk_d與Qk_d。

步驟8：模擬轉(zhuǎn)速計(jì)的測量值V_Tacho＝(Att2Mat(tr(1:3)))'*tr(4:6)+0.01*randn(3,1)，該測量值為載體坐標(biāo)系下的測量值，同時(shí)將該測量值轉(zhuǎn)化到導(dǎo)航系下，并將結(jié)果保存至V_nTacho。其中Att2Mat(tr(1:3))表示將載體真實(shí)的姿態(tài)角轉(zhuǎn)換成矩陣形式。

步驟9：由步驟6和步驟7的結(jié)果計(jì)算系統(tǒng)測量值Zk_d＝vnm-V_nTacho。

步驟10：采用卡爾曼濾波對狀態(tài)變量Xk_d進(jìn)行濾波更新。

卡爾曼增益：K_k＝P_{k_d}H_k_d'*inv(H_{k_d}P_{k_d}H_{k_d}'+R_{k_d})

誤差協(xié)方差陣：

P_{k_d}＝(eye(length(Xk_d))-Kk*Hk_d)*Pk_d*(eye(length(Xk_d))-Kk*Hk_d)'+Kk*Rk_d*Kk'

其中，Xk_d＝Fk_d*Xk_d；Pk_d＝Fk_d*Pk_d*Fk_d'+Qk_d。

狀態(tài)更新：Xk_d＝Xk_d+Kk*(Zk_d-Hk_d*Xk_d)

步驟11：反饋修正。

一：分別對捷聯(lián)慣導(dǎo)的姿態(tài)、速度與位置進(jìn)行修正。即

qnb＝qnb-Xk_d(1:3)

vnm＝vnm-Xk_d(4:6)

posm(1)＝posm(1)-Xk_d(7)

posm(2)＝posm(2)-Xk_d(8)

二：反饋校正后的狀態(tài)變量，得到Xk_d＝[zeros(8,1)；Xk_d(9:14)]。

步驟12：調(diào)用save函數(shù)，對系統(tǒng)狀態(tài)變量及導(dǎo)航信息進(jìn)行保存，包括Pk_d、Xk_d、Kk、qnb、vnm與posm。

步驟13：建立系統(tǒng)逆向?qū)Ш剿惴ā?/p>

一：首先調(diào)用load函數(shù)，加載步驟12保存的所有數(shù)據(jù)，并修改步驟4的時(shí)間循環(huán)順序?yàn)槟嫘?for k＝s_time*100:(-2):2)。

二：然后對步驟6中的陀螺儀輸出取反，有wb_nb＝-(phim-Cnb'*wnin*Tm)。

三：最后對對步驟6中的速度取反，有

vnm＝vnm_1-Cnn*Cnb*(dvm+dvrotm+dvsculm)-(gn-cross(2*wnie+wnen,vnm_1))*Tm

四：數(shù)據(jù)保存。調(diào)用save函數(shù)，對步驟12中的信息進(jìn)行保存。

步驟1-步驟13為系統(tǒng)一次完整的正向和逆向處理過程，當(dāng)需要對系統(tǒng)進(jìn)行多次正逆向處理時(shí)，只需對導(dǎo)航信息進(jìn)行反復(fù)保存及加載，并修改系統(tǒng)仿真時(shí)間，參考上述方法，即可完成系統(tǒng)不同次數(shù)、不同時(shí)間的正逆向處理。

本發(fā)明提出的一種基于逆向?qū)Ш剿惴ǖ慕萋?lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合的對準(zhǔn)方法。對準(zhǔn)技術(shù)是自主水下航行器(AUV)編隊(duì)航行任務(wù)中的重要議題，同時(shí)快速性與精確性也是當(dāng)今作戰(zhàn)的主要任務(wù)需要。針對該問題，本發(fā)明提出了基于逆向?qū)Ш剿惴ǖ慕萋?lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合對準(zhǔn)的研究方法。該方法將轉(zhuǎn)速計(jì)測得的AUV的軸向速度與捷聯(lián)慣導(dǎo)測得的導(dǎo)航信息進(jìn)行有效結(jié)合。通過對轉(zhuǎn)速計(jì)和捷聯(lián)慣導(dǎo)系統(tǒng)的速度輸出進(jìn)行解算，減少二者的速度增量差；同時(shí)對對準(zhǔn)階段的采樣數(shù)據(jù)進(jìn)行反演解算，增大對信息量的利用，進(jìn)而快速獲得高精度的對準(zhǔn)結(jié)果。

仿真實(shí)驗(yàn)結(jié)果表明，在捷聯(lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合的方式下，該方法可以實(shí)現(xiàn)快速對準(zhǔn)，同時(shí)滿足對準(zhǔn)精度的需要，故所提方法在理論和工程實(shí)踐中具有較好的指導(dǎo)價(jià)值和應(yīng)用價(jià)值。

附圖說明

圖1捷聯(lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合對準(zhǔn)方法流程圖；

圖2前30秒采樣數(shù)據(jù)的常規(guī)導(dǎo)航解算過程；

圖3前200秒采樣數(shù)據(jù)的常規(guī)導(dǎo)航解算過程；

圖4前300秒采樣數(shù)據(jù)的常規(guī)導(dǎo)航解算過程；

圖5前200秒采樣數(shù)據(jù)正逆向交替解算5次；

圖6前300秒采樣數(shù)據(jù)正逆向交替解算5次。

圖7前200秒采樣數(shù)據(jù)正逆向交替解算10次。

圖8前300秒采樣數(shù)據(jù)正逆向交替解算10次。

具體實(shí)施方式

現(xiàn)結(jié)合實(shí)施例、附圖對本發(fā)明作進(jìn)一步描述：

步驟1：建立捷聯(lián)慣導(dǎo)與轉(zhuǎn)速計(jì)對準(zhǔn)模型：

轉(zhuǎn)態(tài)方程：

觀測方程：Z_ST＝H_STX_ST+v_ST

上式中

其中，F(xiàn)_ST為系統(tǒng)狀態(tài)矩陣，H_ST為系統(tǒng)量測矩陣，系統(tǒng)噪聲w_ST和量測噪聲μ_ST統(tǒng)為零均值高斯白噪聲。V_N、V_U、V_E為載體在北天東方向上的速度，λ、L和h分別表示載體的經(jīng)緯度與高度，且高度信息可以通過深度傳感器直接測量得到。

建立正向?qū)Ш剿惴ㄎ⒎址匠蹋瑢ζ溥M(jìn)行離散化：

姿態(tài)方程：

速度方程：

位置方程：

R_M,R_N分別為載體所在位置地球子午圈和卯酉圈曲率半徑，其近似計(jì)算公式為：

R_M≈R_e(1-2e+3esin²L)

R_N≈R_e(1+esin²L)

步驟2：卡爾曼濾波初始化。

一：首先，對捷聯(lián)慣導(dǎo)與轉(zhuǎn)速計(jì)組合系統(tǒng)的狀態(tài)變量初值進(jìn)行初始化，確定其在誤差干擾下的狀態(tài)初值x0＝[phi；dvn；dpos；ed；db]，其中平臺(tái)失準(zhǔn)角誤差phi＝[10；60；-10]*(pi/180/60)，位置誤差dpos＝[10/6378160；10/6378160]、速度誤差dvn＝[0.5；0.5；0.5]，陀螺隨機(jī)漂移誤差eb＝[0.02；0.02；0.02]*(pi/180/3600)與加速度計(jì)零位誤差db＝[100；100；100]*(0.000001*9.78)，估計(jì)陀螺測量白噪聲web＝[0.02；0.02；0.02]*(pi/180/3600)，加速度計(jì)的測量白噪聲wdb＝[50；50；50]*(0.000001*9.78)，

設(shè)webq＝[web(1,1)^2；web(2,1)^2；web(3,1)^2]，

wdbq＝[wdb(1,1)^2；wdb(2,1)^2；wdb(3,1)^2]。

可得系統(tǒng)方程中白噪聲均方差為w＝[web；wdb；webq；wdbq]，其中觀測方程中捷聯(lián)慣導(dǎo)SINS與轉(zhuǎn)速計(jì)的量測量之間的白噪聲均方差為v＝[0.01；0.01；0.01]。

然后，對狀態(tài)初值及相應(yīng)誤差函數(shù)分別賦值給相應(yīng)參數(shù)。phi＝x0(1:3)；dvn＝x0(4:6)；Qtd＝w(7:12)；Qt_d＝diag(Qtd)；Rk_d＝diag(v)^2；Pkd＝x0(1:14)；Pk_d＝10*diag(Pkd)^2；Xk_d＝zeros(14,1)；eb＝x0(9:11)；db＝x0(12:14)；web＝w(1:3)；wdb＝x0(4:6)。

其中，Qt_d、Rk_d和Pk_d分別為系統(tǒng)噪聲協(xié)方差驅(qū)動(dòng)矩陣、量測噪聲協(xié)方差陣和一步預(yù)測均方差陣。

二：對系統(tǒng)的姿態(tài)角、速度與位置進(jìn)行初始化。載體初始姿態(tài)角att＝[0；0；0]*(pi/180)；載體初始速度vb＝[8；0；0]；載體的初始位置

pos＝[24*(pi/180)+35*(pi/180/60)；120*(pi/180)+58*(pi/180/60)；-10]。

然后對載體的軌跡進(jìn)行設(shè)置后保存到tr(1:15)函數(shù)，該函數(shù)保存的是不含誤差的真實(shí)值，包括載體的三個(gè)姿態(tài)角分量(橫滾角、航向角及俯仰角)、三個(gè)速度分量(北向、天向及東向)、三個(gè)位置分量(經(jīng)緯度及高度)、三個(gè)角速率分量個(gè)三個(gè)比力分量。

三：捷聯(lián)慣導(dǎo)解算參數(shù)初始化。將載體真實(shí)的姿態(tài)角矩陣轉(zhuǎn)換為四元數(shù)形式，然后加入平臺(tái)誤差角，即qnb＝tr(1:3)+phi，且系統(tǒng)的狀態(tài)變量初始值Xk_d＝zeros(14,0)；將載體真實(shí)的速度信息加入速度誤差，即vnm＝tr(4:6)+dvn；將載體真實(shí)的位置信息加入位置誤差，即posm＝tr(7:9)+[x0(7)；x0(8)；0.1]；最后，將載體輸出的陀螺儀與加速度計(jì)信息保存到wvm2函數(shù)，即wvm2＝tr(10:15)。

步驟3：步驟2完成后，對載體的姿態(tài)、速度與位置信息進(jìn)行組合濾波。首先對誤差估計(jì)函數(shù)進(jìn)行初始化置0，inse(1,:)＝zeros(14,1)；然后將載體真實(shí)的姿態(tài)信息tr(1:3)與加入誤差的實(shí)際值qnb中提取姿態(tài)誤差信息，具體步驟為：先將qnb轉(zhuǎn)化為共軛形式，再與姿態(tài)陣tr(1:3)的四元數(shù)形式做差乘得到姿態(tài)誤差信息；其次，將載體的實(shí)際速度值與真實(shí)值做差，求得速度誤差信息，即vnm-tr(4:6)；最后將載體實(shí)際的位置信息與真實(shí)值做差，求得位置誤差值，即posm(1)-tr(7),posm(2)-tr(8)。

步驟4：設(shè)置時(shí)間循環(huán)(for k＝2:2:s_time*100)讀取tr函數(shù)的角增量與比力增量，并對角增量與比力增量進(jìn)行更新，對更新的角增量與比力增量信息保存到擴(kuò)維后的tr函數(shù)中。即wvm0＝wvm2,wvm1＝tr(10:15),wvm2＝tr((15+10):(15+15))。其中，s_time為系統(tǒng)仿真時(shí)間。

獲取角增量與比力增量，加入誤差，并保存：

dwvm1＝wvm1-wvm0+([eb；db]+[web；wdb].randn(6,1))*Tm/2

dwvm2＝wvm2-wvm1+([eb；db]+[web；wdb].randn(6,1))*Tm/2

其中，Tm＝0.02為捷聯(lián)慣導(dǎo)解算周期。

步驟5：以獲取角增量與比力增量對陀螺儀增量與加速度計(jì)增量擴(kuò)維，進(jìn)行二子樣解算，即

wm＝[dwvm1(1:3,:),dwvm2(1:3,:)]

vm＝[dwvm1(4:6,:),dwvm2(4:6,:)]

對載體的深度用深度傳感器測量得到，加入相應(yīng)誤差，即posm(3,1)＝tr(9)+0.05*randn(1)

步驟6:將步驟5得到含有誤差的實(shí)際增量值進(jìn)行慣導(dǎo)解算，根據(jù)上一時(shí)刻的姿態(tài)四元素、速度、位置、陀螺儀與加速度計(jì)的測量值，計(jì)算當(dāng)前時(shí)刻的姿態(tài)四元數(shù)、速度與位置。

一：計(jì)算地球自轉(zhuǎn)角速度引起的陀螺感應(yīng)值

wnie＝wie*[cos(posm_1(1))；sin(posm_1(1))；0]，其中wie＝0.0000729215為地球自轉(zhuǎn)角速率；posm_1為前一時(shí)刻載體的位置信息。

二：計(jì)算載體在導(dǎo)航系下由北向與東向速度引起的陀螺感應(yīng)值

wnen＝[vnm_1(3)/(Re*(1+e*sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))；

vnm_1(3)*(sin(posm_1(1))/cos(posm_1(1)))/(Re*(1+e*sin(posm_1(1))*

sin(posm_1(1)))+posm_1(3))；-vnm_1(1)/(Re*(1-2*e+3*e*

sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))]

其中，posm_1(3)為前一時(shí)刻載體的高度信息；Re＝6378160為地球半徑；e為地球橢圓率；vnm_1(3)為載體東向航行速度。

三：由上述兩個(gè)步驟，可得由載體運(yùn)動(dòng)速度和地球自轉(zhuǎn)引起的導(dǎo)航系下的陀螺角速率wnin＝wnie+wnen。

四：由于采用二子樣方法，計(jì)算陀螺儀與加速度計(jì)的輸出增量

sumw＝wm(:,1)+wm(:,2)

sumv＝vm(:,1)+vm(:,2)

對sumw進(jìn)行保存，即phim＝sumw。

五：計(jì)算二子樣等效旋轉(zhuǎn)矢量，更新phim。即：

phim＝phim+2/3*cross(wm(:,1),wm(:,2))

六：計(jì)算劃槳效應(yīng)補(bǔ)償dvsculm與速度旋轉(zhuǎn)效應(yīng)補(bǔ)償dvrotm。

dvsculm＝2/3*(cross(wm(:,1),vm(:,2))+cross(vm(:,1),wm(:,2)))

dvrotm＝1/2*cross(sumw,sumv)

七：根據(jù)上式對速度進(jìn)行更新。

vnm＝vnm_1+Cnn*Cnb*(dvm+dvrotm+dvsculm)+gn-cross(2*wnie+wnen,vnm_1))*Tm其中，gn重加速度；Cnn為在導(dǎo)航系下載體的陀螺角速率由旋轉(zhuǎn)矢量轉(zhuǎn)化為矩陣形式；Cnb為載體的姿態(tài)矩陣。

步驟7:求解步驟1的系統(tǒng)的狀態(tài)矩陣Ft_d與觀測矩陣Hk_d，并進(jìn)行離散化，結(jié)果分別保存到Fk_d與Qk_d。

步驟8：模擬轉(zhuǎn)速計(jì)的測量值V_Tacho＝(Att2Mat(tr(1:3)))'*tr(4:6)+0.01*randn(3,1)，該測量值為載體坐標(biāo)系下的測量值，同時(shí)將該測量值轉(zhuǎn)化到導(dǎo)航系下，并將結(jié)果保存至V_nTacho。其中Att2Mat(tr(1:3))表示將載體真實(shí)的姿態(tài)角轉(zhuǎn)換成矩陣形式。

步驟9：由步驟6和步驟7的結(jié)果計(jì)算系統(tǒng)測量值Zk_d＝vnm-V_nTacho。

步驟10：采用卡爾曼濾波對狀態(tài)變量Xk_d進(jìn)行濾波更新。

卡爾曼增益：K_k＝P_{k_d}H_k_d'*inv(H_{k_d}P_{k_d}H_{k_d}'+R_{k_d})

誤差協(xié)方差陣：

P_{k_d}＝(eye(length(Xk_d))-Kk*Hk_d)*Pk_d*(eye(length(Xk_d))-Kk*Hk_d)'+Kk*Rk_d*Kk'

其中，Xk_d＝Fk_d*Xk_d；Pk_d＝Fk_d*Pk_d*Fk_d'+Qk_d。

狀態(tài)更新：Xk_d＝Xk_d+Kk*(Zk_d-Hk_d*Xk_d)

步驟11：反饋修正。

一：分別對捷聯(lián)慣導(dǎo)的姿態(tài)、速度與位置進(jìn)行修正。即

qnb＝qnb-Xk_d(1:3)

vnm＝vnm-Xk_d(4:6)

posm(1)＝posm(1)-Xk_d(7)

posm(2)＝posm(2)-Xk_d(8)

二：反饋校正后的狀態(tài)變量，得到Xk_d＝[zeros(8,1)；Xk_d(9:14)]。

步驟12：調(diào)用save函數(shù)，對系統(tǒng)狀態(tài)變量及導(dǎo)航信息進(jìn)行保存，包括Pk_d、Xk_d、Kk、qnb、vnm與posm。

步驟13：建立系統(tǒng)逆向?qū)Ш剿惴ā?/p>

一：首先調(diào)用load函數(shù)，加載步驟12保存的所有數(shù)據(jù)，并修改步驟4的時(shí)間循環(huán)順序?yàn)槟嫘?for k＝s_time*100:(-2):2)。

二：然后對步驟6中的陀螺儀輸出取反，有wb_nb＝-(phim-Cnb'*wnin*Tm)。

三：最后對對步驟6中的速度取反，有

vnm＝vnm_1-Cnn*Cnb*(dvm+dvrotm+dvsculm)-(gn-cross(2*wnie+wnen,vnm_1))*Tm

四：數(shù)據(jù)保存。調(diào)用save函數(shù)，對步驟12中的信息進(jìn)行保存。

步驟1-步驟13為系統(tǒng)一次完整的正向和逆向處理過程，當(dāng)需要對系統(tǒng)進(jìn)行多次正逆向處理時(shí)，只需對導(dǎo)航信息進(jìn)行反復(fù)保存及加載，并修改系統(tǒng)仿真時(shí)間，參考上述方法，即可完成系統(tǒng)不同次數(shù)、不同時(shí)間的正逆向處理。該算法流程圖如圖1所示。

在實(shí)驗(yàn)分析部分，首先對常規(guī)的正向?qū)Ш剿惴ㄅc正逆向結(jié)合的導(dǎo)航算法性能進(jìn)行對比，通過對不同時(shí)間、不同處理次數(shù)的對準(zhǔn)過程進(jìn)行對比，驗(yàn)證該方法的有效性及優(yōu)越性。

圖2是對系統(tǒng)前30秒采樣數(shù)據(jù)的常規(guī)導(dǎo)航解算過程?？梢钥闯?，在30s內(nèi)對系統(tǒng)進(jìn)行常規(guī)正向?qū)Ш浇馑銜r(shí)，對準(zhǔn)發(fā)散；在200s和300s內(nèi)對系統(tǒng)進(jìn)行常規(guī)正向?qū)Ш浇馑銜r(shí)，對準(zhǔn)角誤差收斂，并且300s的對準(zhǔn)效果優(yōu)于200s，見圖3和圖4；分別對系統(tǒng)前200秒和前300s的時(shí)間內(nèi)采樣數(shù)據(jù)正逆向交替解算5次，可以看出，此時(shí)對準(zhǔn)精度明顯優(yōu)于常規(guī)的正向?qū)Ш浇馑惴绞剑瑫r(shí)前300s的對準(zhǔn)效果優(yōu)于200s，見圖5和圖6；分別對系統(tǒng)前200秒和前300s的時(shí)間內(nèi)采樣數(shù)據(jù)正逆向交替解算10次，此時(shí)與系統(tǒng)通過正逆向交替解算5次后的結(jié)果相比，失準(zhǔn)角誤差更小更平穩(wěn)，見圖7和圖8。實(shí)驗(yàn)結(jié)果表明，利用此種方法能夠獲得較高的對準(zhǔn)精度以及更短的對準(zhǔn)時(shí)間，進(jìn)而提高系統(tǒng)的整體性能，故所使用的方法是正確有效的。