專利名稱：一種捷聯(lián)慣性導(dǎo)航系統(tǒng)初始姿態(tài)確定方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)初始姿態(tài)的確定方法，可用于各種中高精度的捷聯(lián)慣性導(dǎo)航系統(tǒng)的初始姿態(tài)確定，特別適合于機(jī)載SINS安裝時(shí)無轉(zhuǎn)位機(jī)構(gòu)，或轉(zhuǎn)位機(jī)構(gòu)精度不高、轉(zhuǎn)角有限的情況。
背景技術(shù)：
捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)的基本原理是根據(jù)牛頓提出的相對慣性空間的力學(xué)定律，利用陀螺儀、加速度計(jì)測量載體相對慣性空間的線運(yùn)動和角運(yùn)動參數(shù)，在給定的運(yùn)動初始條件下，由計(jì)算機(jī)進(jìn)行積分運(yùn)算，連續(xù)、實(shí)時(shí)地提供位置、速度和姿態(tài)信息。SINS完全依靠自身的慣性敏感元件，不依賴任何外界信息測量導(dǎo)航參數(shù)，因此，它具有隱蔽性好，不受氣候條件限制，無信號丟失，不受干擾等優(yōu)點(diǎn)，是一種完全自主式、全天候的導(dǎo)航系統(tǒng)，已廣泛應(yīng)用于航空、航天、航海等領(lǐng)域。根據(jù)SINS的基本原理，SINS在導(dǎo)航定位解算之前必須獲得初始信息，包括初始位置、速度和姿態(tài)。SINS的初始位置和速度信息較初始姿態(tài)容易獲得，初始姿態(tài)確定后的精度就是SINS進(jìn)入導(dǎo)航工作狀態(tài)時(shí)的初始精度。因此，SINS開始工作時(shí)進(jìn)行快速精確的初始姿態(tài)確定是十分重要的一步。
現(xiàn)有的捷聯(lián)慣導(dǎo)系統(tǒng)初始姿態(tài)確定可分為粗對準(zhǔn)和精對準(zhǔn)兩個(gè)階段。粗對準(zhǔn)階段就是在靜基座條件下，將單個(gè)位置或兩個(gè)位置上(固定兩位置或任意兩位置)的陀螺儀和加速度計(jì)輸出直接引入計(jì)算機(jī)，計(jì)算出載體的初始姿態(tài)。用此方法時(shí)，常常忽略陀螺儀與加速度計(jì)的誤差及外部干擾因素，然而這些因素會導(dǎo)致誤差，因此初始姿態(tài)計(jì)算精度不高。特別地，當(dāng)采用固定兩位置上的陀螺儀和加速度計(jì)的輸出進(jìn)行計(jì)算時(shí)，要求SINS繞Z軸旋轉(zhuǎn)180度或90度，這就需要將SINS安裝在一個(gè)轉(zhuǎn)位機(jī)構(gòu)上，利用轉(zhuǎn)位機(jī)構(gòu)實(shí)現(xiàn)180度或者90度的轉(zhuǎn)動，工程使用時(shí)極不方便，而且轉(zhuǎn)位機(jī)構(gòu)的精度不高，轉(zhuǎn)動角度也無法精確測量，降低了初始姿態(tài)確定的精度。由于具體工程中SINS安裝位置的限制，還存在轉(zhuǎn)動角度無法滿足180度或90度要求的情況，此時(shí)，現(xiàn)有的固定兩位置初始姿態(tài)確定方法將無法應(yīng)用。此外，當(dāng)利用任意兩位置上的陀螺儀和加速度計(jì)輸出時(shí)，需計(jì)算反正弦函數(shù)，陀螺儀和加速度計(jì)本身的誤差以及測量誤差等，容易造成計(jì)算結(jié)果不穩(wěn)定，出現(xiàn)虛數(shù)現(xiàn)象，因此常需要進(jìn)行多次任意兩位置上陀螺儀和加速度計(jì)輸出的采集和計(jì)算，對計(jì)算結(jié)果進(jìn)行挑選，取平均值后，作為載體的初始姿態(tài)。這樣一來，初始姿態(tài)確定的時(shí)間將成倍增加，而且初始姿態(tài)計(jì)算的精度也無法得到保證。
精對準(zhǔn)階段是在粗對準(zhǔn)的基礎(chǔ)上進(jìn)行，利用現(xiàn)代控制理論的狀態(tài)空間法，對陀螺儀和加速度計(jì)輸出的數(shù)據(jù)進(jìn)行處理。當(dāng)對單個(gè)位置的數(shù)據(jù)進(jìn)行處理時(shí)，方位失準(zhǔn)角的可觀度差，收斂速度較慢，所需時(shí)間較長，而且陀螺儀和加速度計(jì)誤差不可觀測，因此，無法進(jìn)行較好的估計(jì)，達(dá)不到提高初始姿態(tài)精度的目的，也不能在初始姿態(tài)計(jì)算的同時(shí)實(shí)現(xiàn)對陀螺儀和加速度計(jì)的標(biāo)定。當(dāng)對固定兩個(gè)位置的數(shù)據(jù)進(jìn)行處理時(shí)，如上已敘述，現(xiàn)有的固定兩位置初始姿態(tài)確定方法存在旋轉(zhuǎn)角度的限制，且對轉(zhuǎn)位機(jī)構(gòu)精度要求較高，具體工程中往往不能應(yīng)用。

發(fā)明內(nèi)容
本發(fā)明的技術(shù)解決問題是克服現(xiàn)有技術(shù)的不足，提供一種精確、方便的捷聯(lián)慣性導(dǎo)航系統(tǒng)初始姿態(tài)確定方法。
本發(fā)明的技術(shù)解決方案為一種捷聯(lián)慣性導(dǎo)航系統(tǒng)初始姿態(tài)確定方法，具體步驟如下(1)SINS預(yù)熱準(zhǔn)備，具體準(zhǔn)備時(shí)間根據(jù)不同的系統(tǒng)而不同；(2)SINS準(zhǔn)備完畢后，保持SINS在初始位置(稱為第一位置)靜止不動，采集陀螺儀輸出和加速度計(jì)輸出；
(3)利用加速度計(jì)輸出與重力加速度的關(guān)系以及陀螺儀輸出與地球自轉(zhuǎn)角速率的關(guān)系，計(jì)算出第一位置的航向角1、俯仰角θ1和橫滾角γ1；(4)無需轉(zhuǎn)位機(jī)構(gòu)，通過任意方法將SINS從第一位置繞空間三維任意軸旋轉(zhuǎn)到任意一個(gè)位置(稱為第二位置)，保持SINS在第二位置靜止不動，采集陀螺儀輸出和加速度計(jì)輸出；(5)采用卡爾曼濾波技術(shù)，將1、θ1和γ1作為初始參數(shù)，對兩個(gè)位置上陀螺儀和加速度計(jì)輸出的數(shù)據(jù)進(jìn)行處理。由于SINS位置的改變，變化了SINS誤差模型中的系統(tǒng)矩陣，從而SINS系統(tǒng)的可觀測性得到改善，濾波效果得到提高，更好的估計(jì)出第二位置時(shí)計(jì)算地理坐標(biāo)系n′與真實(shí)地理坐標(biāo)系n之間的誤差角(簡稱為失準(zhǔn)角φx、φy和φz)及陀螺儀常值漂移、加速度計(jì)常值偏置；(6)利用估計(jì)出的φx、φy和φz計(jì)算真實(shí)地理坐標(biāo)系n與計(jì)算地理坐標(biāo)系n′之間的轉(zhuǎn)換矩陣Cn′n。根據(jù)陀螺儀輸出的角增量或角速度信息，采用四元數(shù)方法，計(jì)算載體坐標(biāo)系b與計(jì)算地理坐標(biāo)系n′之間的轉(zhuǎn)換矩陣Cbn′。由Cn′n和Cbn′，計(jì)算出載體坐標(biāo)系b與真實(shí)地理坐標(biāo)系n之間的轉(zhuǎn)換矩陣Cbn，再由Cbn計(jì)算第二位置的航向角2、俯仰角θ2和橫滾角γ2，將其作為SINS的初始姿態(tài)。
本發(fā)明的原理是SINS在某一個(gè)位置保持靜止不動時(shí)，加速度計(jì)輸出與重力加速度以及陀螺儀輸出與地球自轉(zhuǎn)角速率有如下關(guān)系fx1fy1fz1=Cnb00g...(1)]]>ωx1ωy1ωz1=Cnb0ωiecosLωiesinL...(2)]]>其中，fx1、fy1和fz1以及ωx1、ωy1和ωz1分別為此位置上SINS的X軸、Y軸和Z軸輸出的比力和角速率；g為重力加速度；ωie為地球自轉(zhuǎn)角速率，其在東、北、天方向上的投影為Ω＝
；L為當(dāng)?shù)鼐暥?；Cnb為導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的轉(zhuǎn)換矩陣，Cnb=(Cbn)T=C11C13C13C21C12C23C31C32C33.]]>Cnb可以表達(dá)成此位置處航向角1、俯仰角θ1和橫滾角γ1的表達(dá)式，利用(1)式和(2)式可以計(jì)算出此位置處的1、β1和γ1。計(jì)算公式如下由(1)、(2)式可得C13=fx1g]]>C23=fy1g]]>C33=fz1g]]>C12=ωx1ωiecosL-fx1tanLg]]>C22=ωy1ωiecosL-fy1tanLg...(3)]]>C32=ωz1ωiecosL-fz1tanLg]]>C11＝C22C33-C23C32C21＝-C12C33+C13C32C31＝C12C23-C13C22航向角1、俯仰角θ1和橫滾角γ1的主值計(jì)算公式為 若航向角1、俯仰角θ1和橫滾角γ1的取值范圍定義為
、 [-π，+π]，那么1、θ1和γ1的真值可按如下方法確定。
θ1＝θ1主(5) 由(5)式確定的1、θ1和γ1即為SINS在此位置上的航向角、俯仰角和橫滾角。
由于(1)式和(2)式中未考慮加速度計(jì)偏置、陀螺儀漂移以及加速度計(jì)和陀螺儀輸出信息的測量誤差，求得的航向角1、俯仰角θ1和橫滾角γ1不能準(zhǔn)確描述載體坐標(biāo)系與當(dāng)?shù)氐乩碜鴺?biāo)系之間的真實(shí)角度關(guān)系。因此，應(yīng)當(dāng)在此基礎(chǔ)上，進(jìn)一步利用現(xiàn)代估計(jì)理論將初始姿態(tài)失準(zhǔn)角從隨機(jī)誤差和隨機(jī)干擾中估計(jì)出來。
利用卡爾曼濾波技術(shù)，將1、θ1和γ1作為初始參數(shù)，可以估計(jì)出計(jì)算地理坐標(biāo)系n′與真實(shí)地理坐標(biāo)系n之間的誤差角，校正Cbn′后，可獲得更為準(zhǔn)確的初始姿態(tài)。但是，當(dāng)卡爾曼濾波對單個(gè)位置的加速度計(jì)和陀螺儀輸出進(jìn)行處理時(shí)，系統(tǒng)不完全可觀測，其中，兩個(gè)加速度計(jì)和一個(gè)陀螺儀的誤差不可觀測，因此估計(jì)效果差，達(dá)不到提高初始姿態(tài)精度的目的。本發(fā)明通過將SINS從第一位置繞空間三維任意軸旋轉(zhuǎn)到任意一個(gè)位置，即改變SINS的位置，來變化SINS誤差模型中的系統(tǒng)矩陣，從而改善SINS系統(tǒng)的可觀測性，提高卡爾曼濾波效果，更好地估計(jì)出失準(zhǔn)角及陀螺儀、加速度計(jì)的誤差。利用估計(jì)出的失準(zhǔn)角對轉(zhuǎn)換矩陣Cbn′進(jìn)行校正，得到更為準(zhǔn)確的航向角、俯仰角和橫滾角。
本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于(1)本發(fā)明打破了傳統(tǒng)固定兩位置初始姿態(tài)計(jì)算需要通過轉(zhuǎn)位機(jī)構(gòu)將SINS繞Z軸旋轉(zhuǎn)180度或90度的約束，而是無需通過轉(zhuǎn)位機(jī)構(gòu)，將SINS繞任意軸旋轉(zhuǎn)到任意位置，是一種空間三維的任意雙位置初始姿態(tài)計(jì)算方法。避免了因轉(zhuǎn)位機(jī)構(gòu)精度不高，造成的固定兩位置初始姿態(tài)計(jì)算精度的降低，即提高了初始姿態(tài)計(jì)算的精度。此外，將SINS繞任意軸旋轉(zhuǎn)到任意位置也極大的方便了在實(shí)際工程中的應(yīng)用。
(2)本發(fā)明利用兩個(gè)位置上陀螺儀和加速度計(jì)輸出的數(shù)據(jù)，改善SINS系統(tǒng)的可觀測性，提高卡爾曼濾波器的濾波效果，更好地估計(jì)出失準(zhǔn)角及陀螺儀常值漂移、加速度計(jì)常值偏置，利用估計(jì)出的失準(zhǔn)角對姿態(tài)矩陣進(jìn)行校正后，得到更為準(zhǔn)確的航向角、俯仰角和橫滾角。
(3)本發(fā)明能夠在初始姿態(tài)計(jì)算的同時(shí)完成測漂和定標(biāo)任務(wù)，不但提高了系統(tǒng)初始姿態(tài)計(jì)算的精度，而且定標(biāo)精度也得到了提高，在SINS導(dǎo)航狀態(tài)給予補(bǔ)償，可有效地提高導(dǎo)航定位精度。


圖1為本發(fā)明的初始姿態(tài)確定方法的流程圖；圖2為航向角、俯仰角θ和橫滾角γ的示意圖，圖中Oxnynzn為導(dǎo)航坐標(biāo)系，即東北天地理坐標(biāo)系，Oxbybzb為載體坐標(biāo)系。其中，圖2a表示從導(dǎo)航坐標(biāo)系Oxnynzn繞zn軸逆時(shí)針旋轉(zhuǎn)與載體坐標(biāo)系Oxbybzb重合，即為航向角；圖2b表示從導(dǎo)航坐標(biāo)系Oxnynzn繞xn軸逆時(shí)針旋轉(zhuǎn)θ與載體坐標(biāo)系Oxbybzb重合，θ即為俯仰角；圖2c表示從導(dǎo)航坐標(biāo)系Oxnynzn繞yn軸逆時(shí)針旋轉(zhuǎn)γ與載體坐標(biāo)系Oxbybzb重合，γ即為橫滾角。
具體實(shí)施例方式
如圖1所示，本發(fā)明的具體實(shí)施方法如下1、捷聯(lián)慣性導(dǎo)航系統(tǒng)的準(zhǔn)備SINS開機(jī)后，進(jìn)入準(zhǔn)備狀態(tài)。
2、第一位置數(shù)據(jù)采集SINS準(zhǔn)備完畢，保持SINS在初始位置(稱為第一位置)靜止不動，采集5分鐘陀螺儀輸出和加速度計(jì)輸出，如果SINS的精度較低可適當(dāng)延長采數(shù)時(shí)間。
3、第一位置姿態(tài)計(jì)算導(dǎo)航坐標(biāo)系取為東北天地理坐標(biāo)系，第一位置處航向角1、俯仰角θ1和橫滾角γ1的定義如圖2a、圖2b和圖2c所示。
加速度計(jì)輸出與重力加速度以及陀螺儀輸出與地球自轉(zhuǎn)角速率有如下關(guān)系fx1fy1fz1=Cnb00g...(6)]]>ωx1ωy1ωz1=Cnb0ωiecosLωiesinL...(7)]]>式中，fx1、fy1和fz1以及ωx1、ωy1和ωz1分別為第一個(gè)位置上SINS的X軸、Y軸和Z軸輸出的比力和角速率；g為重力加速度；ωie為地球自轉(zhuǎn)角速率，其在東、北、天方向上的投影為Ω＝
，L表示當(dāng)?shù)鼐暥?；Cnb為導(dǎo)航系到載體系的轉(zhuǎn)換矩陣，可寫為Cnb=(Cbn)T=C11C13C13C21C12C23C31C32C33.]]>由(6)、(7)式可得C13=fx1g]]>C23=fy1g]]>C33=fz1g]]>C12=ωx1ωiecosL-fx1tanLg]]>C22=ωy1ωiecosL-fy1tanLg...(8)]]>
C32=ωz1ωiecosL-fz1tanLg]]>C11＝C22C33-C23C32C21＝-C12C33+C13C32C31＝C12C23-C13C22航向角1、俯仰角θ1和橫滾角γ1的主值計(jì)算公式為 θ1主＝arcsin(C23) (9) 若航向角1、俯仰角θ1和橫滾角γ1的取值范圍定義為
、 [-π，+π]，那么1、θ1和γ1的真值可按如下方法確定。
θ1＝θ1主(10) 由(10)式確定的1、θ1和γ1即為SINS在第一位置上的航向角、俯仰角和橫滾角。
4、第二位置數(shù)據(jù)采集通過任意方法將SINS從初始位置繞空間三維任意軸旋轉(zhuǎn)到任意一個(gè)位置(稱為第二位置)，保持SINS在第二位置靜止不動，采集5分鐘陀螺儀輸出和加速度計(jì)輸出。
5、對兩個(gè)位置的數(shù)據(jù)進(jìn)行濾波處理將1、θ1和γ1作為初始參數(shù)，利用卡爾曼濾波技術(shù)，對兩個(gè)位置上陀螺儀和加速度計(jì)輸出的數(shù)據(jù)進(jìn)行處理，精確估計(jì)出失準(zhǔn)角φx、φy和φz(東北天地理坐標(biāo)系下是φE、φN和φU)及陀螺儀常值漂移、加速度計(jì)常值偏置。
(1)SINS初始姿態(tài)確定誤差模型的建立導(dǎo)航坐標(biāo)系取為東北天地理坐標(biāo)系，位置和垂直速度誤差省略，加速度計(jì)和陀螺儀的誤差視為隨機(jī)偏置加白噪聲過程，此時(shí)SINS的誤差模型為δV·EδV·Nφ·Eφ·Nφ·U▿·x▿·yϵ·xϵ·yϵ·z=02ΩU0-g0T11T12000-2ΩU0g00T21T22000000ΩU-ΩN00T11T12T1300-ΩU0000T21T22T2300-ΩN0000T31T32T3300000000000000000000000000000000000000000000000000δVEδVNφEφNφU▿x▿yϵxϵyϵz...(11)]]>式中，下標(biāo)E、N、U分別表示東、北、天。地球自轉(zhuǎn)角速率ωie在東、北、天方向上的投影為Ω＝
＝
，L表示當(dāng)?shù)鼐暥?；下?biāo)x、y、z為載體坐標(biāo)系；Tij(i＝1，2，3；j＝1，2，3)為姿態(tài)矩陣Cbn中的元素，Cbn={Tij}i=1,2,3;j=1,2,3.]]>(2)SINS初始姿態(tài)確定卡爾曼濾波模型的建立對于捷聯(lián)慣導(dǎo)系統(tǒng)，考慮到陀螺儀隨機(jī)漂移誤差和加速度計(jì)的隨機(jī)偏差，將方程(11)修正為如下形式[X·a(t)X·b(t)]=FTi05×505×5[Xa(t)Xb(t)]+[W′(t)05×1]=AiX(t)+W(t)...(12)]]>式中，W(t)為N(O，Q)的高斯白噪聲；狀態(tài)變量Xa＝[δVEδVNφEφNφU]T，Xb＝[xyεxεyεz]T；隨機(jī)噪聲狀態(tài)矢量W′(t)=wδVEwδVNwφEwφNwφUT,]]>其中，δVE、δVN分別為東向和北向速度誤差，φE、φN為水平失準(zhǔn)角；φU為方位失準(zhǔn)角；x、y為加速度計(jì)隨機(jī)常值偏置，εx、εy、εz是陀螺儀隨機(jī)常值漂移；05×5和05×1均為指定維數(shù)的零矩陣；F和Ti代表的內(nèi)容如下Ti=T11T12000T21T2200000T11T12T1300T21T22T2300T31T32T33,]]>F=02ΩU0-g0-2ΩU0g00000ΩU-ΩN00-ΩU0000ΩN00]]>(12)式為捷聯(lián)慣導(dǎo)系統(tǒng)初始姿態(tài)確定卡爾曼濾波模型的系統(tǒng)方程。為了應(yīng)用卡爾曼濾波器進(jìn)行狀態(tài)向量的最優(yōu)估計(jì)，還需建立系統(tǒng)觀測方程。選取兩個(gè)水平速度誤差為觀測量，建立的系統(tǒng)觀測方程為Z(t)=10000000000100000000XaXb+ηEηN=HX(t)+η(t)...(13)]]>其中，η(t)是系統(tǒng)觀測噪聲向量，為N(O，R)的高斯白噪聲過程。
(3)離散卡爾曼濾波模型的建立根據(jù)上述系統(tǒng)方程和觀測方程，可建立離散卡爾曼濾波方程如下。
濾波方程 增益方程Kk=Pk,k-1HkT[HkPk,k-1HkT+Rk]-1...(15)]]>預(yù)報(bào)誤差方差方程Pk,k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1...(16)]]>濾波誤差方差方程Pk＝[I-KkHk]Pk，k-1(17)式中，Φk，k-1為離散化的狀態(tài)轉(zhuǎn)移矩陣(系統(tǒng)矩陣)，Q、R分別為系統(tǒng)噪聲和觀測噪聲的協(xié)方差矩陣。
(4)濾波初始條件X(0)＝010×1；P(0)、Q和R對應(yīng)中等精度SINS的取值如下
P(0)＝diag{(0.3m/s)2，(0.3m/s)2，(30′)2，(10″)2，(10″)2，(100μg)2，(100μg)2，(0.1°)2，(0.1°)2，(0.1°)2}；Q＝diag{(100μg)2，(100μg)2，(0.1°)2，(0.1°)2，(0.1°)2，0，0，0，0，0}；R＝diag{(0.1m/s)2，(0.1m/s)2}。
6、計(jì)算陀螺儀隨機(jī)常值漂移和加速度計(jì)隨機(jī)常值偏置濾波器估計(jì)出的x、y即為加速度計(jì)隨機(jī)常值偏置，εx、εy、εz為陀螺儀隨機(jī)常值漂移。
7、計(jì)算載體坐標(biāo)系b與計(jì)算地理坐標(biāo)系n′之間的轉(zhuǎn)換矩陣Cbn′可利用陀螺儀輸出的角增量或角速度信息，采用四元數(shù)方法計(jì)算Cbn′，計(jì)算步驟如下(1)利用航向角1、俯仰角θ1和橫滾角γ1初始化第一個(gè)位置時(shí)的姿態(tài)，計(jì)算初始轉(zhuǎn)換矩陣Cbn′和四元數(shù)q，計(jì)算公式如下 令Cbn′=T11T12T13T21T22T23T31T32T33]]>則有q0=±121+T11+T22-T33]]>q1=±121+T11+T22-T33]]>q2=±121+T11+T22-T33]]>q3=±121+T11+T22-T33]]>(2)更新四元數(shù)q0、q1、q2、q3和轉(zhuǎn)換矩陣Cbn′
q(n+1)={(1-(Δθ0)28+(Δθ0)4384)I+(12-(Δθ0)248)(Δθ)}q(n)...(19)]]>其中，Δθ=0-Δθx-Δθy-ΔθzΔθx0Δθz-ΔθyΔθy-Δθz0ΔθxΔθzΔθy-Δθx0]]>Δθ0=Δθx2+Δθy2+Δθz2]]>轉(zhuǎn)換矩陣Cbn′的更新公式如下Cbn′=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32...(20)]]>8、計(jì)算航向角2、俯仰角θ2和橫滾角γ2利用卡爾曼濾波估計(jì)出的φE、φN、φU計(jì)算真實(shí)地理坐標(biāo)系n與計(jì)算地理坐標(biāo)系n′之間的轉(zhuǎn)換矩陣Cn′n。根據(jù)Cn′n和(20)式計(jì)算出的Cbn′，計(jì)算載體坐標(biāo)系b與真實(shí)地理坐標(biāo)系n之間的轉(zhuǎn)換矩陣Cbn，再由Cbn計(jì)算第二位置的航向角2、俯仰角θ2和橫滾角γ2，將其作為SINS的初始姿態(tài)。具體計(jì)算步驟如下(1)計(jì)算真實(shí)地理坐標(biāo)系n與計(jì)算地理坐標(biāo)系n′之間的轉(zhuǎn)換矩陣Cnn′Cn′n=1-φUφNφU1-φE-φNφE1...(21)]]>(2)計(jì)算載體坐標(biāo)系b與真實(shí)地理坐標(biāo)系n之間的轉(zhuǎn)換矩陣CbnCbn=Cn′nCbn′...(22)]]>(3)計(jì)算航向角2、俯仰角θ2和橫滾角γ2航向角2、俯仰角θ2和橫滾角γ2的定義如圖2a、圖2b和圖2c所示。
將(22)式計(jì)算出的Cbn記為
Cbn=T11T12T13T21T22T23T31T32T33...(23)]]>又因?yàn)?因此，由(23)式和(24)式，可以確定航向角2、俯仰角θ2和橫滾角γ2的主值，即 若航向角2、俯仰角θ2和橫滾角γ2的取值范圍分別定義為
、 [-π，+π]。那么，2、θ2和γ2的真值可由下式確定 θ2＝θ2主(26) 由(26)式確定的2、θ2和γ2即為SINS在第二位置上的航向角、俯仰角和橫滾角，將其作為SINS進(jìn)入導(dǎo)航工作狀態(tài)的初始姿態(tài)。
權(quán)利要求
1.一種捷聯(lián)慣性導(dǎo)航系統(tǒng)初始姿態(tài)確定方法，其特征在于包括以下步驟(1)SINS預(yù)熱準(zhǔn)備完畢，保持SINS在初始位置，稱為第一位置，靜止不動，采集陀螺儀輸出和加速度計(jì)輸出；(2)根據(jù)加速度計(jì)輸出與重力加速度的關(guān)系以及陀螺儀輸出與地球自轉(zhuǎn)角速率的關(guān)系，計(jì)算出第一位置的航向角1、俯仰角θ1和橫滾角γ1；(3)將SINS從第一位置繞空間三維任意軸旋轉(zhuǎn)到任意一個(gè)位置，稱為第二位置，保持SINS在第二位置靜止不動，采集陀螺儀輸出和加速度計(jì)輸出；(4)采用卡爾曼濾波技術(shù)，將1、θ1和γ1作為初始參數(shù)，對兩個(gè)位置上陀螺儀和加速度計(jì)輸出的數(shù)據(jù)進(jìn)行處理，估計(jì)出第二位置時(shí)計(jì)算地理坐標(biāo)系n′與真實(shí)地理坐標(biāo)系n之間的誤差角，簡稱為失準(zhǔn)角φx、φy和φz及陀螺儀常值漂移、加速度計(jì)常值偏置；(5)利用卡爾曼濾波估計(jì)出的φx、φy和φz計(jì)算真實(shí)地理坐標(biāo)系n與計(jì)算地理坐標(biāo)系n′之間的轉(zhuǎn)換矩陣Cn＇n，根據(jù)陀螺儀輸出的角增量或角速度信息，采用四元數(shù)方法，計(jì)算載體坐標(biāo)系b與計(jì)算地理坐標(biāo)系n′之間的轉(zhuǎn)換矩陣Cbn′，再由Cbn計(jì)算第二位置的航向角2、俯仰角θ2和橫滾角γ2，將其作為SINS的初始姿態(tài)。
全文摘要
一種捷聯(lián)慣性導(dǎo)航系統(tǒng)初始姿態(tài)確定方法，通過將SINS從初始位置繞空間三維任意軸旋轉(zhuǎn)到任意一個(gè)位置，根據(jù)位置一上SINS的輸出與地球自轉(zhuǎn)角速度和重力加速度的關(guān)系，初步確定出SINS的初始姿態(tài)。由于SINS位置的改變，提高了系統(tǒng)的可觀測性，進(jìn)一步利用卡爾曼濾波技術(shù)，對姿態(tài)失準(zhǔn)角及陀螺儀常值漂移、加速度計(jì)常值進(jìn)行精確估計(jì)，得到SINS更加準(zhǔn)確的初始姿態(tài)。本發(fā)明具有自主、精度高、靈活簡便的特點(diǎn)，適用于各種中高精度的捷聯(lián)慣性導(dǎo)航系統(tǒng)。
文檔編號G01C21/16GK1908584SQ20061011252
公開日2007年2月7日 申請日期2006年8月23日 優(yōu)先權(quán)日2006年8月23日
發(fā)明者房建成, 宮曉琳, 盛蔚, 楊勝, 徐帆, 劉百奇 申請人:北京航空航天大學(xué)