捷聯(lián)式慣性導(dǎo)航積分算法設(shè)計(jì)-上下完整篇

1、捷聯(lián)式慣性導(dǎo)航積分算法設(shè)計(jì)上篇：姿態(tài)算法PaulG.SavageStrapdownAssociates,Inc.,MaplePlain,Minnesota55359摘要：本論文分上下兩篇,用于給現(xiàn)代捷聯(lián)慣導(dǎo)系統(tǒng)的主要軟件算法設(shè)計(jì)提供一個(gè)嚴(yán)密的綜合方法：將角速率積分成姿態(tài)角,將加速度變換或積分成速度以及將速度積分成位置.該算法是用兩速修正法構(gòu)成的,而兩速修正法是具有一定創(chuàng)新程度的新奇算法,是為姿態(tài)修正而開發(fā)出來的,在姿態(tài)修正中,以中速運(yùn)用精密解析方程去校正積分參數(shù)姿態(tài)、速度或位置,其輸入是由在參數(shù)修正姿態(tài)錐化修正、速度搖櫓修正以及高分辨率位置螺旋修正時(shí)間間隔內(nèi)計(jì)算運(yùn)動角速度和加速度的高速算法提供

2、的.該設(shè)計(jì)方法考慮了通過捷聯(lián)系統(tǒng)慣性傳感器對角速度或比力加速度所進(jìn)行的測量以及用于姿態(tài)基準(zhǔn)和矢量速度積分的導(dǎo)航系旋轉(zhuǎn)問題.本論文上篇定義了捷聯(lián)慣導(dǎo)積分函數(shù)的總體設(shè)計(jì)要求,并開發(fā)出了用于姿態(tài)修正算法的方向余弦法和四元數(shù)法；下篇著重討論速度和位置積分算法的設(shè)計(jì).盡管上下兩篇討論中常常涉及到根本的慣性導(dǎo)航概念,然而,本論文是為那些已對根底慣導(dǎo)概念很熟悉的實(shí)際工作者而寫的.專門用語：A,A,4,A3=任意坐標(biāo)系CAA2=將矢量從A2坐標(biāo)系投影到A坐標(biāo)系的方向余弦矩陣I=單位矩陣qA1=從A2坐標(biāo)系投影到A坐標(biāo)系的旋轉(zhuǎn)矢量所構(gòu)成的姿態(tài)變化四元數(shù)qA2*=qA1的共軻四元數(shù),它的第1項(xiàng)與q2的首項(xiàng)相同,余

3、下的24項(xiàng)與qR的互為相反數(shù)q1=單位四元數(shù),它的第1項(xiàng)為1,其余3項(xiàng)為0V=無具體坐標(biāo)系定義的矢量VA=列向量,它的各項(xiàng)元素等于矢量V在坐標(biāo)系A(chǔ)的各軸上的投影AAV=向量V的反對稱或交叉積形式,代表如下矩陣：0VzaVyaVza0VxaVYAVXA0AAA其中：Vxa,Vya,Vza是V的分量,V與人系矢量的矩陣乘積等于V與該矢量的叉積V；A-0=與VA等量的四元數(shù)矢量,AVA=A2坐標(biāo)系相對于A坐標(biāo)系的角速率,當(dāng)Ai為慣性系I系時(shí),a2是由安裝在A2坐標(biāo)系上的角速率傳感器所測到的角速率1 .概論慣性導(dǎo)航是通過對速度積分得到位置并對總加速度積分得到速度的過程.總加速度是指由重力加速度和被施加

4、的非重力產(chǎn)生的加速度亦即比力加速度之和.慣性導(dǎo)航系統(tǒng)INS包括：用于積分的導(dǎo)航計(jì)算機(jī)；用于給積分運(yùn)算定時(shí)的精密時(shí)鐘,測量比力加速度用的加速度計(jì)組臺；用于作為所算位置的一個(gè)函數(shù)而進(jìn)行的重力加速度計(jì)算而留于導(dǎo)航計(jì)算機(jī)中的重力模型軟件,以及為了定義作為速度計(jì)算一局部的加速度計(jì)三元組的角度方向所用的姿態(tài)基準(zhǔn).在現(xiàn)代INS中,姿態(tài)基準(zhǔn)是由駐留于INS計(jì)算機(jī)中的軟件積分函數(shù)提供的,其輸入來自一個(gè)有三軸的慣性角速度傳感器.角速度傳感器和加速度計(jì)三元組安裝在一個(gè)公用的牢固構(gòu)架上,該構(gòu)架裝在INS的底盤上,以保證每個(gè)慣性傳感器之間的精確對準(zhǔn),這樣的一種布置稱之為捷聯(lián)INS.由于慣性傳感器牢固地固定在底盤內(nèi),所以

5、也就牢固地固定于安裝INS的飛行器上.INS計(jì)算機(jī)中的根本函數(shù)有將角速率變換為姿態(tài)的積分函數(shù)稱之為姿態(tài)積分.使用姿態(tài)數(shù)據(jù)將測得的加速度值轉(zhuǎn)換到適當(dāng)?shù)膶?dǎo)航坐標(biāo)系中,再將它積分成矢量速度的函數(shù)稱之為矢量速度積分,還有將導(dǎo)航系矢量速度積分成位置的函數(shù)稱之為位置積分.這樣就有了三個(gè)積分函數(shù),姿態(tài)函數(shù)、矢量速度函數(shù)及位置函數(shù),每個(gè)函數(shù)的精度要求很高,以保證函數(shù)誤差極小,符臺慣性傳感器精度的要求.回眸歷史,由于早在50年代,根底捷聯(lián)慣導(dǎo)概念就開始形成,所以多年來捷聯(lián)分析師將精力主要集中于姿態(tài)積分函數(shù)算法的設(shè)計(jì)上.而種種算法的設(shè)計(jì)方法總是受到當(dāng)時(shí)的飛行計(jì)算機(jī)技術(shù)的能力和局限性的影響.50年代后期和60年代,

6、各種研究機(jī)構(gòu)的捷聯(lián)工作者們采取兩種用于姿態(tài)積分函數(shù)運(yùn)算的方法,即用一階數(shù)字算法進(jìn)行高速姿態(tài)修正運(yùn)算,如1020kHz和用高階算法進(jìn)行的低速姿態(tài)修正運(yùn)算,如50100HZ.如果要精確考慮高頻角速率分量時(shí),就得考慮用高速算法,這樣可以調(diào)整為系統(tǒng)的三維姿態(tài)變化.然而,那個(gè)時(shí)期的計(jì)算機(jī)工藝技術(shù)只能對姿態(tài)修正算法進(jìn)行一些簡化的一階方程運(yùn)算,精度有限.相反,高階算法提倡者竭力吹捧較一階算法提升了解析精度的高階算法；但是,由于每個(gè)姿態(tài)修正循環(huán)可執(zhí)行運(yùn)算次數(shù)的連帶增加導(dǎo)致必須使姿態(tài)修正速率減緩以滿足當(dāng)時(shí)的計(jì)算機(jī)吞吐量的限制,從而使提升了的精度被降低.由于姿態(tài)四元數(shù)可以當(dāng)作對所算姿態(tài)參數(shù)的解析形,這一優(yōu)點(diǎn)使之成

7、為一種更可取的算法與傳統(tǒng)方向余弦矩陣姿態(tài)表示法相比所以它的出現(xiàn)使上述兩種算法的優(yōu)點(diǎn)黯然失色.就那一時(shí)期研究過的算法而言,四元數(shù)法在高頻角速率環(huán)境下表現(xiàn)出的運(yùn)算精度最優(yōu).1966年,作者提出一個(gè)新的兩速姿態(tài)積分函數(shù)運(yùn)算方法,在該方法中,姿態(tài)修正運(yùn)算被分成兩部分,即把簡單高速一階算法局部與更復(fù)雜的中速高階算法局部結(jié)合起來使用,后一局部的輸入由高速算法提供.簡化了的高速局部用于考慮姿態(tài)修正循環(huán)內(nèi)的高頻角振蕩,這能調(diào)整系統(tǒng)的姿態(tài)建立傳統(tǒng)上稱之為錐化.合在一起,兩速方法的組合精度等于以高速速率進(jìn)行的高階運(yùn)算為了提升精度；然而由于高速算法簡化之故,組合后對計(jì)算機(jī)的吞吐量的要求絲毫也不比原來的高速一階姿態(tài)修

8、正算法的吞吐量大.參考文獻(xiàn)【6】采用的兩速算法設(shè)計(jì)受到其根本解析公式的限制,該公式是一個(gè)皮卡德類型Picard-type的連續(xù)型的姿態(tài)速率微分方程的遞歸積分,在該方程中,同時(shí)產(chǎn)生中速和高速算法.解析遞歸積分設(shè)計(jì)過程的復(fù)雜性限制了高階中速算法的擴(kuò)展在參考文獻(xiàn)【6】中僅擴(kuò)展到2階,這在當(dāng)時(shí)被認(rèn)為是可以接受的o196孫,焦?fàn)柖豃ordan在一個(gè)不相關(guān)的設(shè)計(jì)活動中提出一個(gè)用于捷聯(lián)姿態(tài)修正函數(shù)的兩速法,在此方法中,解析公式的開頭建立在兩個(gè)單獨(dú)定義的計(jì)算上：一個(gè)建立在輸入姿態(tài)變化根底上的中速、經(jīng)典封閉型、準(zhǔn)確高階姿態(tài)修正算法和一個(gè)測量中速算法姿態(tài)變化輸入的簡化高速二階積分算法.1971年,鮑爾茲Bortz

9、將焦?fàn)柖氐母拍顢U(kuò)展到基于微分方程的高速計(jì)算,積分時(shí)將測量的準(zhǔn)確姿態(tài)變化輸入給準(zhǔn)確的姿態(tài)修正算法.準(zhǔn)確的中速姿態(tài)算法可以通過兩個(gè)三角系數(shù)舍位構(gòu)建成任何精度要求的特定階次.實(shí)際上,鮑爾茲的姿態(tài)變化微分力程的簡化型式現(xiàn)被用于高速函數(shù)計(jì)算.因此,參考文獻(xiàn)【7】和【8】提供了一個(gè)更一般的兩速姿態(tài)修正算法,在此算法中,中速高階算法和高速簡化算法可以獨(dú)立地進(jìn)行改編以滿足特定的應(yīng)用要求.讓人感興趣的是,參考文獻(xiàn)【8】提出了一個(gè)簡化型高速算法的模擬執(zhí)行程序.參考文獻(xiàn)【7】和【8】提出的兩速法主張?jiān)跍?zhǔn)確中速姿態(tài)修正運(yùn)算中采用方向余弦法的第二個(gè)好處是中速局部也可以用一個(gè)解析的精確的封閉形式的四元數(shù)修正算法來作為公式

10、,該算法采用同用于方向余弦修正算法一樣的高速輸入.這樣一來,新的兩速法無論對于方向余弦法還是對四元數(shù)修正法都有同樣的精度,而這兩種方法都是由解析的精確封閉式方程推導(dǎo)出來的這里假定對泰勒級數(shù)的三角系數(shù)的展開進(jìn)行到比擬準(zhǔn)確的量級.大多數(shù)飛機(jī)使用的現(xiàn)代捷聯(lián)INS采用以兩速法為根底的姿態(tài)修正算法.中速算法局部的重復(fù)速率一般是基于最大角速率來設(shè)計(jì)如50200Hz,以盡量減少中、高速算法中募級數(shù)舍位誤差的影響.高速算法的重復(fù)速率是在預(yù)計(jì)的捷聯(lián)慣性傳感器組合振動環(huán)境的根底上精確考慮了引起振蕩的錐化效應(yīng)而進(jìn)行設(shè)計(jì)的.如用于1nmile/h有50%的徑向位置誤差速率的飛機(jī)的INS的重復(fù)速率為14kHz.連續(xù)進(jìn)行

11、的兩速姿態(tài)算法的開發(fā)工作一直集中于高速積分函數(shù)的變化上.原來設(shè)想的簡單一階的種種現(xiàn)有高速姿態(tài)算法利用現(xiàn)代計(jì)算機(jī)日益增加的吞吐量,演變?yōu)楦鞣N高階算法,而且經(jīng)度得到提升,參見參考文獻(xiàn)【9】【11】和【12】的第7-1節(jié).對于姿態(tài)修正函數(shù)的演化,截止目前,有關(guān)用于加速度變換或矢量速度積分和位置積分的捷聯(lián)INS姊妹算法的開發(fā)方面的其他并駕齊驅(qū)式著述很少見到發(fā)表.本論文上篇定義了捷聯(lián)慣導(dǎo)積分函數(shù)的總體設(shè)計(jì)要求并描述了開發(fā)基于兩速法的姿態(tài)積分算法的綜合設(shè)計(jì)過程.文章中所提供的材料是參考文獻(xiàn)【12】第7-1節(jié)它是參考文獻(xiàn)【9】中材料的擴(kuò)展的濃縮型.這些材料著重于提供更嚴(yán)密的解析公式,同時(shí),在可能的情況下,考

12、慮到便于生成計(jì)算機(jī)軟件的文件及有效性這與現(xiàn)代飛行計(jì)算機(jī)技術(shù)也是一致的,這些材料使用準(zhǔn)確的封閉式方程.在姿態(tài)算法沒計(jì)過程中包括了考慮到姿態(tài)修正時(shí)間周期內(nèi)導(dǎo)航坐標(biāo)系旋轉(zhuǎn)的種種嚴(yán)密處理方法.本論文的第二節(jié)提供了有關(guān)坐標(biāo)系和所采用參數(shù)的背景材料.第三節(jié)以連續(xù)微分方程形式提供了一整套典型的捷聯(lián)慣導(dǎo)姿態(tài)、矢量速度和位置方程,這構(gòu)成了等量算法設(shè)計(jì)過程的框架.第四節(jié)探討以高速局部的普通形式進(jìn)行兩速姿態(tài)積分運(yùn)算的算法適用于在導(dǎo)航系旋轉(zhuǎn)影響下的方向余弦法和四元數(shù)法的公式,并描述了一個(gè)特殊的形式,說明在古典高速二階錐化計(jì)算算法中一種算法的設(shè)計(jì),第五節(jié)給出有關(guān)姿態(tài)積分算法的表格參考數(shù)據(jù),第六節(jié)就選擇特殊應(yīng)用算法和建立

13、運(yùn)算運(yùn)行速率之后的過程進(jìn)行了一般化討論,第七節(jié)給出結(jié)論性評述.最后,熟悉到下述觀點(diǎn)很重要：盡管兩速法的初衷是克服早期l9651975年計(jì)算機(jī)技術(shù)的吞吐量的局限性,然而隨著現(xiàn)代高速計(jì)算機(jī)的快速開展進(jìn)步,這一局限性正在變得無足輕重.這就促使工程技術(shù)人員想回到一個(gè)更簡單的單速算法結(jié)構(gòu)上來,采用這種結(jié)構(gòu)時(shí),所有計(jì)算以一個(gè)重復(fù)速率進(jìn)行,速率很高,足以準(zhǔn)確地將多軸高頻角速率和加速度調(diào)整影響考慮進(jìn)去.本論文上,下兩篇提供的兩速結(jié)構(gòu)是和壓縮成在對算法進(jìn)行公式化的專門章節(jié)里解釋的那種單速型是相兼容的.2 .坐標(biāo)系和姿態(tài)方向的關(guān)系本節(jié)定義了本篇論文中使用的坐標(biāo)系,并一般性地說明方向余弦矩陣、姿態(tài)四元數(shù)和用于代表兩

14、坐標(biāo)系之間的角度關(guān)系的旋轉(zhuǎn)矢量姿態(tài)參數(shù)等等的性質(zhì).坐標(biāo)系是根據(jù)右手法那么對彼此互相垂直的三個(gè)連續(xù)編了號或標(biāo)了字符的單位矢量加以定義的解析抽象.它可以想像為三個(gè)垂直線軸穿過一個(gè)共同的點(diǎn)原點(diǎn),各單位矢量從原點(diǎn)沿三條垂直線向外發(fā)散.在本論文中,坐標(biāo)系原點(diǎn)的實(shí)際位置是任意的.在特定坐標(biāo)系中,一個(gè)矢量的分量或投影等于矢量與該坐標(biāo)系各個(gè)單位矢量的點(diǎn)乘.本論文中采用的矢量為自由矢量,因此,在對它們進(jìn)行解析描述的坐標(biāo)系中,其位置不存在優(yōu)劣性.坐標(biāo)系定義如下：1 .E系是用于位置定位因而定義的地球固聯(lián)坐標(biāo)系.它的典型定義是它有一個(gè)軸與地球極軸平行,其他兩軸固連于地球并與赤道平面平行；2 .N系是一個(gè)導(dǎo)航坐標(biāo)系,

15、Z軸與當(dāng)?shù)氐厍蛲獗韰⒖嘉恢冒状怪毕蛏戏较蚱叫?N系用于將加速度積分為速度并定義E系內(nèi)當(dāng)?shù)卮怪苯欠较颍? .L系是當(dāng)?shù)厮阶鴺?biāo)系,與N1平行,但Z軸垂直向下,而期口Y軸沿N系的Y和期由.L系用于描述捷聯(lián)傳感器坐標(biāo)系方向的基準(zhǔn)；4 .B系是捷聯(lián)慣性傳感器坐標(biāo)系機(jī)體系,其軸與標(biāo)準(zhǔn)右手正交傳感器輸入軸相平行；5.I系是一個(gè)非旋轉(zhuǎn)慣性坐標(biāo)系,用作角旋轉(zhuǎn)測量基準(zhǔn).為I系選擇的特殊方向在其方向與解2.2姿態(tài)參數(shù)定義方向余弦矩陣定義為一個(gè)方陣,其各列是單位矢量的正交組,每列等于沿A2系的一個(gè)軸方向上的單位矢量在A系各軸上的投影CA2uiA2u2a2u3A2式中uA；是沿A2系軸i方向上的單位矢量在坐標(biāo)系A(chǔ)1各

16、軸上的投影.從這一根本定義出發(fā),可以驗(yàn)證出：C：的i行、j列的元素等于A系軸i和A2系軸j之間的夾角的余弦值,CA2的轉(zhuǎn)置矩陣等于其逆矩陣,CA2轉(zhuǎn)置矩陣的各列等于系A(chǔ)軸中單位矢量在A系各軸上的投影,且CA2與一個(gè)投影于A2系各軸上的向量的積等于此向量在Ai系各軸上的投影分量而對CA1的轉(zhuǎn)置矩陣那么相反.va2(cA2)tvAi(2)2式可以用于導(dǎo)出方向余弦矩陣鏈?zhǔn)揭?guī)律:cA3A2A3A2CA2cA旋轉(zhuǎn)矢量定義旋轉(zhuǎn)軸和繞軸的旋轉(zhuǎn)幅度.可以想像A系通過繞此旋轉(zhuǎn)矢量旋轉(zhuǎn)而從它的起始姿態(tài)轉(zhuǎn)到一個(gè)新的姿態(tài),繞旋轉(zhuǎn)矢量旋轉(zhuǎn)的角度等于旋轉(zhuǎn)矢量的大小.現(xiàn)在,稱a2系為A系的新姿態(tài),通過給4系的這一定義,一個(gè)

17、任意定義的旋轉(zhuǎn)矢量就可唯一地確定A2系姿態(tài)與原始Ai系姿態(tài)二者的關(guān)系.反之,如果給定Ai系和A2系的姿態(tài)關(guān)系,就可以定義一個(gè)與此姿態(tài)一致的旋轉(zhuǎn)矢量.這樣來,旋轉(zhuǎn)矢量可以用來定義A2系與A系相關(guān)的姿態(tài).從解析式上看見參考文獻(xiàn)【4】,【9】和【12】的第3.2.2.1節(jié)旋轉(zhuǎn)矢量和方向余弦矩陣之間的關(guān)系可以由下式給出:CfA2sin(1cos)2()sin-2一-()(4)式中是旋轉(zhuǎn)矢量及其幅度.旋轉(zhuǎn)矢量的一個(gè)獨(dú)特特性是在A系和A2系中有相同的分量見參考文獻(xiàn):【12】的第3.2.2.1節(jié)；因此4式中的代表A1或A2姿態(tài)四元數(shù)是一個(gè)四矢量,即有四個(gè)分量,定義為旋轉(zhuǎn)矢量的一個(gè)函數(shù)和【12】的第3.2.4

18、節(jié)和【14】的第7376頁見參考文獻(xiàn)【4】,【9】cos0.5JqAsin0.50.50.5(5)由5式可以看出,qA1各元素的平方和是單位1.坐標(biāo)變換方程可以用四元數(shù)代數(shù)方程表示見參考文獻(xiàn)【4】、【9】和【12】的第3.2.4.1節(jié).VqAqA：VqA2qA：*,VqA2qA；VqAq2qA2VqAqA2*(6)6式可以用來推導(dǎo)出姿態(tài)四元數(shù)的鏈?zhǔn)揭?guī)律:AA3A2qAqjq%2.3姿態(tài)參數(shù)速率方程第2.2節(jié)姿態(tài)參數(shù)的變化速率見參考文獻(xiàn)【4】、【8】、【9】和【12】的第3.3節(jié)由下式給出:CA1A2cA1(A2、A21A2)(I)CA1(8)qA1A21A2q21qA1(9)AA1A2sin2

19、(1cos)(10)3連續(xù)型捷聯(lián)慣導(dǎo)方程定義捷聯(lián)慣導(dǎo)系統(tǒng)中進(jìn)行的典型根本運(yùn)算的微分方程的第77103頁以及156177頁給出如下：姿態(tài)速率：見參考文獻(xiàn)【9】、H2的第4章和【15】CBCb(BIBLIL(11)或代之以:LILqLqB(12)當(dāng)?shù)厮较敌D(zhuǎn)速率:加速度變換:LasF或代之以:NasFLILNIENENEN)(13)(Cn)TNFc(UZNLBCbHsfLBL*qBas后qBcLNLasFEIEN、N)ZNUZN(14)(15)(16)(17)(18)矢量速度速率Ngp(iE)(NIE)RN(19)NasFNNgp(encN、N2ie)v(20)位置速率:CncE(NN)(21)

20、NNUznv(22)式中：R=從地球中央到ins的位置矢量；V=相對于地球的速度矢量位置變化速率,解析式中定義為E系中R的時(shí)間導(dǎo)數(shù)；h=地面高度,定義為從INs到地球外表的距離,沿從INs到地球參考重力平面上的切線平面的垂線方向測量；Fc=曲率矩陣3X3是位置CE,h的一個(gè)函數(shù),其元素3、i和i、3等于零,而其余元素繞對角線成對稱形.對于球形地球模型而言,除對角線上外其余元素是零,且對角線上數(shù)值是從地球中央到INS的徑向距離的倒數(shù).對于扁地球模型而言,其余的項(xiàng)代表投影到線上的地球外表上的本地曲率見參考文獻(xiàn)【12】第5,3節(jié)有關(guān)封閉型的表述局部ins高度);UZN=沿重力平面垂直方向N系的Z軸上

21、的單位矢量;ZN=EN的垂直分量；ZN選取的值取決于所采用的N系的類型.如：游動方位系統(tǒng)或自由方位系統(tǒng)的設(shè)計(jì)保證NN對所有地球定位而言是非奇異的見參考文獻(xiàn)【12】的第4,6節(jié)和【15】的88-99頁;asF=比力加速度,定義為由施加的非重力力產(chǎn)生的相對于非旋轉(zhuǎn)慣性空間的加速度,它由加速度計(jì)測得;g=質(zhì)量吸引重力加速度或重力R的一個(gè)函數(shù)；gp=鉛垂重力或重力,對于穩(wěn)定的ins而言,它沿鉛垂線方向；g的解析模型可以在參考文獻(xiàn)【16】；【17的第4.4節(jié)和【18】的6.3節(jié)找到.關(guān)于g在NK的分量,請參見參考文獻(xiàn)【12】第5.4.1節(jié).在運(yùn)行捷聯(lián)慣導(dǎo)函數(shù)過程中,捷聯(lián)INS計(jì)算機(jī)用適當(dāng)?shù)姆e分算法對后面

22、的姿態(tài)速率、矢量速度速率以及位置速率方程進(jìn)行積分運(yùn)算.關(guān)于后面的導(dǎo)航方程形式,下面幾點(diǎn)值得注意.方向余弦和四元數(shù)姿態(tài)都是以機(jī)體姿態(tài)速率或加速度變換運(yùn)算的形式表示.無論哪一個(gè)部可以用于實(shí)踐,而其結(jié)果實(shí)質(zhì)上是一致的.矢量速度是相對于地球E系定義的,矢量速度速率方程是以當(dāng)?shù)厮蕉x的N系寫出來的用于將它積分成矢量速度.對于像飛機(jī)的INS之類的許多地形導(dǎo)航用途而言,這很典型.也可以選擇其它坐標(biāo)系用于矢量速度定義和矢量速度速率方程,如用于戰(zhàn)術(shù)和戰(zhàn)略導(dǎo)彈制導(dǎo).位置速率方程將位置定義為高度和NK相對于E系的角度方向.從中可以提取出經(jīng)度和緯度,并可以計(jì)算出R直見參考文獻(xiàn)【12】第4.5.1和4.5.3以及【1

23、5】的88,89頁.對于位置速率方程而言,位置也可以定義為簡單的R從中可以計(jì)算出CNE和h,見參考文獻(xiàn)【12】的第4.5.4節(jié).高度速率方程22顯得價(jià)值不大,但當(dāng)你考慮旋轉(zhuǎn)扁地球模型、地球上的一個(gè)旋轉(zhuǎn)NK和固定的高度定義時(shí),這就不一定了.參考文獻(xiàn)【12】的4.4節(jié)和5.5節(jié)說明對于旋轉(zhuǎn)的扁地球模型而言,方程22是準(zhǔn)確的.如果斷定引入垂直通道重力或發(fā)散限制來防止不穩(wěn)定垂直通道誤差呈指數(shù)增加,那么方程20和22務(wù)必包括一個(gè)額外增加的垂直限制項(xiàng)參考文獻(xiàn)【12】第4.4.1節(jié),【15】第102-103頁以及【18】的10.3節(jié).4姿態(tài)即時(shí)修正算法本節(jié)探討適合用數(shù)字計(jì)算機(jī)進(jìn)行積分的方向余弦矩陣速率方程1

24、1和姿態(tài)四元數(shù)速率方程12的算法形式,這些方程將用現(xiàn)在的傳統(tǒng)兩速法來建立,傳統(tǒng)兩速法中的解析的精確的封閉形式的方程可以用于根本姿態(tài)修正函數(shù)的運(yùn)算.該函數(shù)的輸入是由一個(gè)為了在根本姿態(tài)修正周期中測出姿態(tài)變化而設(shè)計(jì)的高速算法所提供的.4.1姿態(tài)方向余弦矩陣CL方向余弦矩陣的修正算法是為實(shí)現(xiàn)下述目的而設(shè)計(jì)的：在姿態(tài)即時(shí)修正時(shí)間內(nèi)所得的數(shù)字結(jié)果跟在相同時(shí)間間隔對方程11中CL表達(dá)式的正規(guī)連續(xù)積分得出的結(jié)果相同.該算法的思路是以理想的機(jī)體B系和當(dāng)?shù)厮絃系在數(shù)字即時(shí)修正區(qū)間內(nèi)確定的變化方程11中由?和；引的,作為每一即時(shí)修正時(shí)刻相對非旋轉(zhuǎn)慣性空間I系的連續(xù)離散點(diǎn)上的當(dāng)前構(gòu)造值.考慮到完全一般化的情形,我們

25、還允許由于L系角運(yùn)動的CL修正運(yùn)算不一定是在當(dāng)B系運(yùn)動而對CL修正的同一時(shí)刻瞬時(shí)運(yùn)行,例如在一個(gè)多速率數(shù)字計(jì)算回路結(jié)構(gòu)中,CL以高速率進(jìn)行修正是由于B系旋轉(zhuǎn)而不是由于L系旋轉(zhuǎn).如果對計(jì)算機(jī)吞吐量的最小化感興趣的話,從軟件結(jié)構(gòu)上說,可以讓L系修正比B系修正慢510倍.我們用來描述坐標(biāo)系方向隨時(shí)間變化的專門術(shù)語如下：B|m=當(dāng)計(jì)算機(jī)修正時(shí)間為tm時(shí),機(jī)體B系在非旋轉(zhuǎn)慣性空間I中的離散方向；m=因8系角運(yùn)動對CB進(jìn)行即時(shí)修正的計(jì)算機(jī)循環(huán)拍次；L(當(dāng)?shù)厮絃系在計(jì)算機(jī)修正時(shí)間tn在非旋轉(zhuǎn)慣性空間1中的離散方向;n=因1系角運(yùn)動對CtL進(jìn)行即時(shí)修正的計(jì)算機(jī)循環(huán)拍次;有了這些定義,采用(3)式方向余弦矩陣

26、遞推計(jì)算規(guī)律便能建立起對cB即時(shí)修正算法如下n1)CL(n1)C(m1)(m)BI(m1)BI(m)CL(n)(n1)(23)(24)其中：CL(n1)=tm1時(shí)刻B系與tn1時(shí)刻L系的轉(zhuǎn)換關(guān)系矩陣Cb；(m1)C：(n)=tm時(shí)刻B系與tn時(shí)刻L系的轉(zhuǎn)換關(guān)系矩陣Cb；I(m)BCB1(m1)=考慮到B系相對慣T空間從tm1時(shí)刻點(diǎn)到tm時(shí)刻點(diǎn)的旋轉(zhuǎn)的方向余弦矩陣；1(m)Li,.、CL1(n)=考慮到L系相對慣性空間從tn1時(shí)刻點(diǎn)到tn時(shí)刻點(diǎn)的旋轉(zhuǎn)的方向余弦矩陣；I(n1)在(23)和(24)式描述的算法把各個(gè)時(shí)間的機(jī)體晾和當(dāng)?shù)厮絃系的方向聯(lián)系起來,并為B系與L系提供了以不同修正速率對CL進(jìn)

27、行的慣性角運(yùn)動修正.與B系(它能以200-300度/秒的速度進(jìn)行動態(tài)旋轉(zhuǎn)、相比,當(dāng)?shù)厮絃系的慣性角速率一般來說較小,它等于地球旋轉(zhuǎn)速率加上L系相對地球的角速率(傳遞速率,它一般不會大過地球速率的幾倍).因此,L系即時(shí)修正一般可以以低于晾修正的速率完成,而且有相當(dāng)?shù)木?應(yīng)當(dāng)注意,對晾和L系運(yùn)動修正速率的要求,在某種程度上,是通過和測量姿態(tài)變化的近似高速算法作比擬,將誤差降至最小的根底上確立的(見本文4.1.1和4.1.2節(jié)).對CL的B系和L系的運(yùn)動修正是由(23)和BL,(24)式的CBI(m1)項(xiàng)和C?n)項(xiàng)完成的,其算法分別推導(dǎo)如下:(m)1(n1)4.1.1機(jī)體系旋轉(zhuǎn)(23)式用CB

28、(m1)來考慮捷聯(lián)傳感器(機(jī)體汨系相對于非旋轉(zhuǎn)空間的角速率B來修正姿態(tài)1(m)、.、.一IB|.、方向余弦矩陣CB,Cb(、的正式定義為:(m)BI(m1)BI(m)mCB(m1)tm1cBdt(25)式中B(t)是tm1到tm時(shí)間間隔內(nèi)任意時(shí)間的機(jī)體B系姿態(tài)BiCb矩陣還可以用定義的BI系姿態(tài)相對于B1系的旋轉(zhuǎn)矢量來表示.對(4)式進(jìn)行泰勒級(m)I(m)I(m1)數(shù)展開,可得系數(shù)項(xiàng)：CB(m1)(m)sn(m)sinm1m2m4m3!5!1cosm12m4m2m2!4!6!(26)式中,m定義為在tm時(shí)刻B1系姿態(tài)相對B1系的旋轉(zhuǎn)矢量.m旋轉(zhuǎn)矢量可以通過把當(dāng)作定義為大于tm1時(shí)刻的普通的B

29、系姿態(tài)相對于R系普通旋轉(zhuǎn)矢量來對待的辦I(m1)法計(jì)算出來.然后,是通過對普通的方程從tm1時(shí)刻進(jìn)行積分得來,而(26)式中用到的就是在tm時(shí)刻的積分結(jié)果.把用于定義的BI系當(dāng)作非旋轉(zhuǎn)慣性基準(zhǔn)系(m1)I來處理,般可以得到以下表達(dá)式,其方法是將(10)式中的坐標(biāo)系慶2由機(jī)體系B取代,而坐標(biāo)系A(chǔ)由慣性系I取代,得到用角速率描述的下式:B1IB2B2?)(iB)(27)式中定義為在大于tm1時(shí)刻的B系姿態(tài)相對于BI系的旋轉(zhuǎn)矢量.通常稱之為鮑爾1(m1)(m)(m1)B茲萬程的(27)式使機(jī)體系B的姿態(tài)的變化與B系相對于慣性系I的角速度舊產(chǎn)生聯(lián)系,而BIB可以通過捷聯(lián)角速率傳感器測得.于是(26)式

30、的姿態(tài)旋轉(zhuǎn)矢量m便可以通過從tm1時(shí)刻到tm時(shí)刻對(27)式進(jìn)行積分求得：t-(t)()dt,(m)(tm)(28)tm1式中是運(yùn)行中的積分時(shí)間變量.要減少(27)式中求算值的計(jì)算次數(shù),應(yīng)參加簡化設(shè)計(jì).B、例如,通過帚級數(shù)擴(kuò)展,可以將(27)式中的(ib)項(xiàng)的放大系數(shù)近似為：1Msin1T(1T7；)二(12(1cos)1260112因此,(27)式的可以由下式給出：B1B工(B)IB2IB12(IB)通過仿真和分析(在假定的解析定義角運(yùn)動條件下進(jìn)行解析擴(kuò)展階精度的表達(dá)式：(29)(30),可以得到保證值的二1 B1,B、1B2 IB12IB2IB式中：tBttiBdtm131式很重要,由于它

31、能使27式簡化到二階精度,即(31)(32)的誤差為三階,只保持-階項(xiàng).這樣一來,27式就變成二階精度:(33)B1BIB2IB代入方程33,28式那么變?yōu)橄率?tm1tmBIB(t)b)dt(34)(35)(36)最后,用32式可得:mmm其中：,、t(t)tIBd,m(tm)tm11tmBm1t(IB)dtBIB2tm1式中m是從31到tm的圓錐姿態(tài)運(yùn)動.m項(xiàng)被錐化成錐化工程,由于它受到測量B的圓錐運(yùn)動分量的影響.圓錐運(yùn)動定義為角速度矢量本身在旋轉(zhuǎn)的狀態(tài).由于舊表現(xiàn)為純B圓錐運(yùn)動舊的幅度雖然恒定不變,但其矢量在旋轉(zhuǎn),那么B系的一個(gè)固定軸近似垂直于因此用術(shù)語圓錐運(yùn)動B.IB矢量的旋轉(zhuǎn)平面,該

32、固定軸將隨著角速度的運(yùn)動而形成一個(gè)錐面來描述這個(gè)運(yùn)動.在圓錐角運(yùn)動條件下,與iB垂直的B系軸表現(xiàn)為振蕩這與非圓錐運(yùn)動或自轉(zhuǎn)角運(yùn)動相反,在后一種情況下,于垂直的軸繞上旋轉(zhuǎn).,BB對舊不發(fā)生旋轉(zhuǎn)的情況而言,從36式可以容易看出,t將與舊平行,因此m中B的積分函數(shù)的叉乘將是零,m也將是零.這種情況下,即當(dāng)舊不發(fā)生旋轉(zhuǎn)時(shí),34式將變成如下簡化形式：tmBmtIBdt37tm1B注意：37式也適用于當(dāng)舊不旋轉(zhuǎn)時(shí),27和28式中的m精確求解,即不存在近似問題.這點(diǎn)可以通過觀察27式很容易地得到驗(yàn)證,由于在27式中,t在積分開始B.BB時(shí)就與舊對齊,并將與舊保持平行,這是由于在(t)表達(dá)式中,(t)與舊叉乘

33、將保持為零.在這種條件下,(27)式和(28)式也將化簡為(37)式.下面討論積分角速率和圓錐增量算法.關(guān)于(36)式中m的積分速率和m的錐化表述的離散化數(shù)字算法可以通過這樣的方法開發(fā)出來,即考慮m是一般函數(shù)(t)在t=tm時(shí)刻的值,由(36)式得：1 tB(t)1t()iB)d(38)2 tm1現(xiàn)在考慮將(38)式的積分分成在tm1到口時(shí)間間隔內(nèi)到達(dá)t時(shí)刻的時(shí)間和11時(shí)刻之后的時(shí)間兩局部,這樣一來,(38)式等于：(t)|1(t),m(tm)1tlBi1t(t)iB)dt(39)2t1式中：11是ttl1時(shí)(t)值,而l是ttl時(shí)的計(jì)算機(jī)的循環(huán)拍次.需注意,根據(jù)上述定義,l循環(huán)拍次快于m循環(huán)

34、拍次.現(xiàn)在,我們再定義一個(gè)在tm1到tm的時(shí)間間隔內(nèi)l循環(huán)時(shí)間點(diǎn)tl,于是在tl時(shí)間點(diǎn),包擴(kuò)初始條件在內(nèi)的(39)式就變?yōu)?ml(tltm)當(dāng)ttm1,l0(40)1 tlbi1t(t)iB)dt2 ll1通過類似過程,(40)式的(t)表達(dá)式可以通過(36)式(t)的計(jì)算求得：八、tB.(t)i1(t),(t)t1tBd(41)(42)tl1l(tl),ll1l當(dāng)ttm1,l0根據(jù)(41)式和(42)式得：iB)dt11tli2(i1J2tn(t)ml(tltm)(41)和(42)式構(gòu)成數(shù)字遞歸算法,該算法以l作為計(jì)算機(jī)循環(huán)速率計(jì)算m和錐化項(xiàng)m,作為從ti1到ti的整個(gè)時(shí)間間隔內(nèi)和的變化之和

35、,(42)式中1積分項(xiàng)的數(shù)字當(dāng)量仍尚待確定.姿態(tài)算法開發(fā)方面的后續(xù)工作集中在估算錐化方程(42)中積分項(xiàng)的數(shù)字算法設(shè)計(jì)上,一一B般而百,所米用的萬法都是對從tl1到tl時(shí)間間隔內(nèi)的角速率模型舊假定一個(gè)一般的解析式,如一個(gè)在時(shí)間上截尾的一般多項(xiàng)式.于是(42)式的積分項(xiàng)就可以以解析的形式作為一般速率模型系數(shù)的一個(gè)函數(shù),如多項(xiàng)式系數(shù)來表示.最后,角速率模型系數(shù)是通過匹配連續(xù)積分計(jì)算出的角速率增量的測量值來計(jì)算出來的.就隨后的舉例而言,角速率模型被近似為一個(gè)常數(shù)加一個(gè)實(shí)時(shí)線性累積值,其常數(shù)和梯度是由現(xiàn)有的和以前的l值計(jì)算出來的.此算法的一個(gè)更為復(fù)雜的形式可能是在假設(shè)的角速率模型中包含有一個(gè)時(shí)變平方項(xiàng)

36、,用現(xiàn)有的、過去的以及過去之過去的l值判定系數(shù).近來在這一領(lǐng)域進(jìn)行的工作是通過利用tl1到ti時(shí)間間隔內(nèi)角速率傳感器的測量值來計(jì)算角速率模型系數(shù)(參見文獻(xiàn)【19】提出的一個(gè)用于強(qiáng)化單速算法的技術(shù)擴(kuò)展),從而將第三個(gè)循環(huán)速率加到整個(gè)姿態(tài)修正過程的結(jié)構(gòu)中：姿態(tài)(C(L)修正和錐化(i)修正(正如至此所作的討論)以及錐化修正的傳感器采樣.后一種技術(shù)在改良時(shí)采用一個(gè)一般的角速率模型,該角速率模型是依據(jù)其對(42)式中積分項(xiàng)的影響直接定義為ti1到ti時(shí)間間隔內(nèi)連續(xù)積分角速率增量傳感器采樣值之間的加權(quán)叉乘之和(類似于文獻(xiàn)【19】介紹的tm1到口時(shí)間間隔內(nèi)的方法).在后一種情況下的加權(quán)系數(shù)于是得到R優(yōu)化并

37、可以獲取純錐化環(huán)境下的最正確平均性能,亦即舊在幅度上為常數(shù),但在旋轉(zhuǎn).之后提到的種種設(shè)計(jì)方法中的每一種都是以根據(jù)假定的角速率模型形狀使用曲面擬合技術(shù)為基礎(chǔ)的.每一種的最終算法在不是為某個(gè)速率環(huán)境而設(shè)計(jì)時(shí)和存在有角速率傳感器量化噪聲的情況下會表現(xiàn)不同.更為可取的算法的選擇,務(wù)必包括仿真分析以確認(rèn)運(yùn)行速率環(huán)境下和傳感器噪聲特性狀態(tài)下是否可以接受.現(xiàn)通過算法舉倒對這一局部進(jìn)行總結(jié).這兒舉出的是(42)式積分項(xiàng)算法的例子,它的機(jī)B理是通過對塞級數(shù)進(jìn)行截尾展開,使機(jī)體速率舊項(xiàng)被近似到一階：Bib(tt1),和二吊數(shù)(43)參考文獻(xiàn)9-【11】和【12】的第7.1.1.1.1說明對于時(shí)間段ti2到L的(4

38、3)式運(yùn)動有:titi1B1(iB)dt而(i1i)(44)將(44)式代入(42)式,可得：1z1、i(i1i1)i26(45)式從類別上可歸為求m的二階算法,這是由于它在l方程中包含有現(xiàn)在的和過去的.一.一1,l循環(huán)乘積.從導(dǎo)出(44)式的分析可以看出,l中的l和l1乘積項(xiàng)即一項(xiàng)為哪一項(xiàng)由tl6到tl2時(shí)間間隔內(nèi)線性斜坡角速率的近似化得來的.如果角速率近似化成時(shí)間的二次曲線變化函數(shù),那么,勢必導(dǎo)致包含l,l1和l2的乘積的三階算法,如果角速率被近似成.1t到tl整個(gè)時(shí)間間隔內(nèi)的一個(gè)常數(shù),那么,(45)式中l(wèi)的一項(xiàng)勢必導(dǎo)致求m的一階算法.6最后,如果角速率正在慢慢變化,那么,我們可以將m近似

39、為等于零.換言之(或更準(zhǔn)確地說),我們可以讓l循環(huán)速率等于m循環(huán)速率.這使m等于用(45)式在時(shí)刻計(jì)算出來的注意根據(jù)(41)式初始狀態(tài)定義l1為零,后一種算法在參考文獻(xiàn)【4】得到了改良.須注意.要設(shè)定l和m速率相等,還可以通過增加m速率使之與l速率相匹配的方法實(shí)現(xiàn).結(jié)果得到一個(gè)單一的高速高階算法,該算法附帶有一個(gè)比兩速算法更為簡單的軟件結(jié)構(gòu),但是,需要更大的吞吐量,現(xiàn)代計(jì)算機(jī)速度的不斷提升使這一算法在未來成為更為可取的算法.(35)式m和m的全部數(shù)字算法是由(41)、(42)和(45)式合并后得出的結(jié)果所決定的:tldtl1(46)ml(tl露),當(dāng)ttm1時(shí)l01)(47)l(tltm),當(dāng)

40、ttm1時(shí)l0式中：d微積分角速率增量,即：捷聯(lián)角速率傳感器給出的脈沖輸出的解析表達(dá)式,即iBdt=由角速率傳感器給出的積分角速率輸出增量之和.4.1.2當(dāng)?shù)厮较档男D(zhuǎn)(24)式采用C：I(n)式來修正姿態(tài)方向余弦矩陣C：,以考慮本地水平坐標(biāo)系L系相對于Li,、,cLi(n)的正式i(n1)L|(n1)非旋轉(zhuǎn)空間的角速率11cLi的推導(dǎo)與第4.1.1節(jié)C：I(m1)的推導(dǎo)非常類似.1(n1)km)定義是：c：i(n)Itncjdt|(n1)tn1i(n1)式中,L(t)是tn1到tn時(shí)間間隔內(nèi)任意時(shí)刻的L系姿態(tài).CLI(n)矩陣也可以用定義的L,系的姿態(tài)相對于L系的旋轉(zhuǎn)矢量來表示.對(4)式

41、(n1)I(n)(n1)進(jìn)行泰勒級數(shù)展開可得系數(shù)項(xiàng)：CL(n)(n1)sinn(1cosn)(n)(n)sinn2n3!4n5!(49)(1cosn)2n12!2n4!6!注意：在(49)式中,式中n是定義tn時(shí)刻L1系的姿態(tài)相對于tn1時(shí)刻L1系的旋轉(zhuǎn)矢量.(n)1(n1)空二(n)項(xiàng)的符號與(26)式C：；1)表達(dá)式的同類項(xiàng)的符號相反,是負(fù)的.這是由于nL,、BL,、Bi,八“與Cb(或(4)式C：)的指向相反,t的矢量變換是從L、到L,而CbI(n1)I(m)1I(n1)(n1)'(n)I(m),一一、一一一、-.一,Ll、,.一一.,BI的矢量變換是從BI到BI.這樣,(49)

42、式中的CL形式便是(26)式中CB表達(dá)式的I(m)I(m1)I(n1)(m)轉(zhuǎn)置.由于tn1到tn的修正周期相對較短,n在數(shù)值上將會很小.又由于；小且在整個(gè)tn1到tn的典型修正周期內(nèi)變化緩慢(由于該時(shí)間周期內(nèi)的矢量速度和位置變化小),所以L系速率矢量；可以近似為非旋轉(zhuǎn).結(jié)果可以像旋轉(zhuǎn)矢量方程(1O)的簡化形式一樣計(jì)算(49),這里的叉乘項(xiàng)被忽略不計(jì)：tnLntILdt(50)tn1順便指出,鑒于n是個(gè)小量,如已討論過的那樣,用于CL的(49)式亦可簡化.例如,nI(n1)可從(49)式導(dǎo)出一個(gè)二階形式：Jig1CliI(n)1(n)(n)I(n1)2L,對于現(xiàn)行的現(xiàn)代計(jì)算機(jī)技術(shù)而言,采用解算

43、Cl的(49)式簡化形式如(51)式的計(jì)算(n1)機(jī)在內(nèi)存或吞吐量方面的優(yōu)點(diǎn)與增加軟件有效性或文件復(fù)雜性以及精確度上損失相比擬,是微缺乏道的.精確度損失在導(dǎo)航過程中一般來說是很小的；然而,在初始對準(zhǔn)運(yùn)算過程中(慣性導(dǎo)航啟動之前)這種精度損失不可無視,初始對準(zhǔn)運(yùn)算時(shí)C：矩陣常被用于對CeL進(jìn)行傾(n1)斜修正(參考文獻(xiàn)【12】第6,1.2節(jié)和第15節(jié)120121頁).對Cb的初始傾斜對準(zhǔn)修正量可能會很大,如0.1-1度,如果對49式采用過于簡化的形式,在初始對準(zhǔn)過程中CL就可能產(chǎn)生不希望的誤差.初始對準(zhǔn)運(yùn)算的閉環(huán)伺服運(yùn)動最終會修正由CEL產(chǎn)生的結(jié)果性姿態(tài)誤差；然而,它可能會將冗余正交誤差留在C.

44、行和列里面.其結(jié)果一定是需要包括一個(gè)正交或歸一化修正算法見第4.1.3節(jié)作為C：修正過程的外層循環(huán).50式n積分的離散數(shù)字算法可以通過下述方法建立起來,即第一步先將13式和15式合并,求出被積函數(shù)；,然后進(jìn)行如下近似計(jì)算：(52)LLNNNN、ILCn耳1/27*1/2mFCn1/2(UzNV)一一1一式中,下標(biāo)n一是tn1到tn的中途的值.將52式代入5O式,然后可得:2CLNTnCN|En1/2'nZNn1/2UZNTnFCn1/2(UZNRm)(53)用14式估算ie且(54)NtmN,RmtVdttm1式中,是計(jì)算機(jī)循環(huán)修正周期tn31,而j是在tn1到tn的計(jì)算機(jī)修正n循環(huán)內(nèi)

45、m循環(huán)的循環(huán)次數(shù).153式下標(biāo)為n3的項(xiàng)都是位置的函數(shù),它見下篇參考文獻(xiàn)【13】在以n循環(huán)速率進(jìn)行的姿態(tài)修正之后得到更新.因此,要計(jì)算52式中的這些項(xiàng),必須在以前計(jì)算的參數(shù)的根底上,采用一個(gè)近似外推公式.例如,采用最后兩次的計(jì)算值的線性外推公式,將變?yōu)椋?55)1n1/2n12n1n2在下篇參考文獻(xiàn)【13中,我們發(fā)現(xiàn)速度vN更新緊跟著位置更新.因此,現(xiàn)在的和過去的m循環(huán)的數(shù)值可用于估算54式中RN的積分.對54式進(jìn)行梯形積分可得:(56)式中Tm是計(jì)算機(jī)m循環(huán)的更新周期tmt下篇參考文獻(xiàn)【13】還開發(fā)了一種具有高分辯水平的RN算法,用于精密位置修正,它考慮了從m1到m循環(huán)修正時(shí)間間隔內(nèi)的動態(tài)角

46、速率和加速度變化.4.1.3歸一化和正交化根據(jù)第2.2節(jié)的根本定義C：的列(和行)代表正交單位矢量,因此,它們在模值上等于1(歸一化條件)且彼此相互正交(正交化條件).C：更新算法中,除了已描述過的根本CL修正算法外,也常常包括一個(gè)歸一化和正交化算法以保證C：的各行和各列正交.導(dǎo)致C：正交誤差的因素包括CfL正交初始化誤差、軟件編程誤差、由于計(jì)算機(jī)字長小于用于CfL算法更新周期所希望的全部位數(shù)而引起的舍入誤差、以及(26)和(49)式泰勒級數(shù)展開中所選項(xiàng)數(shù)太少而引起的舍位誤差.重要的一點(diǎn)是要注意到(參考文獻(xiàn)【12】第3.4.1節(jié))：歸一化和正交化誤差只是由于(23)、(24)、(26)及(49

47、)式的軟件運(yùn)行過程中出現(xiàn)的誤差所引起的,而不是由于供這些方程的算法誤差或是由慣性傳感器輸入誤差所引起.Cb積分算法軟件的總體設(shè)計(jì)或校正過程必須保證：在預(yù)想的導(dǎo)航時(shí)間周期內(nèi)和角速率環(huán)境條件下,編程無誤差并且舍入誤差和舍位誤差在可以接受的范圍內(nèi).這是一個(gè)用現(xiàn)代計(jì)算機(jī)技術(shù)或軟件開發(fā)技術(shù)可輕易實(shí)現(xiàn)的目標(biāo).不過,將C.正交化或歸一化修正算法包括進(jìn)去是許多捷聯(lián)慣導(dǎo)軟件包一直采用的傳統(tǒng)作法,目的在于提升精度,并放寬不許運(yùn)算中出現(xiàn)任何正交誤差這一嚴(yán)格要求.用于歸一化正交的算法是以下面這一特性為根底的；即方向余弦矩陣的轉(zhuǎn)置矩陣等于其逆矩陣(見2.2節(jié))；因此,CB與其轉(zhuǎn)置矩陣之乘積為單位矩陣.這一狀態(tài)的改變,可

48、用來測量正交誤差,而該誤差在迭代運(yùn)算中可被限制算法修正(參考文獻(xiàn)【9】,【12】第7.1.1.3節(jié),【15】216-218頁)4.2姿態(tài)四元數(shù)設(shè)計(jì)qB姿態(tài)四元數(shù)的修正算法的目的是為了使在各姿態(tài)修正時(shí)刻得到的數(shù)字結(jié)果跟(12)LL式qB表達(dá)式的正規(guī)連續(xù)積分在同一時(shí)刻瞬間得到的數(shù)字結(jié)果相同.qB姿態(tài)四兀數(shù)修正算法式(57)(58)是嚴(yán)格根據(jù)本論文第4.1節(jié)用CB修正算法推導(dǎo)的同樣過程開發(fā)出來的.因此,采用姿態(tài)四元數(shù)鏈?zhǔn)揭?guī)律可以寫出下式：LL,B,1(n1)1(n1)I(m1)qBgqB'm)L|(n)L|(n)L|(n1)QB.1"1)靠15)式中：q：(n1)=tm1時(shí)刻的B系

49、到tn1時(shí)刻的L系的四元數(shù)qB；(m1)qB?*=tm時(shí)刻的B系到tn時(shí)刻的L系的四元數(shù)qB;I(m)qB；/)=考慮了B系相對慣性空間旋轉(zhuǎn)的從幻1時(shí)刻的方向到tm時(shí)刻的方向的姿態(tài)四元數(shù)；qL?)=考慮了L系相對慣性空間旋轉(zhuǎn)的從tn1時(shí)刻的方向到tn時(shí)刻的方向的姿態(tài)四元I(n1)qB的修正是由(57)式和(58)式中q：I(m1)和q：I(n)完成的,兩個(gè)算法分別推導(dǎo)如下:I(m)I(n1)Bi,八(57)式用qB；的旋轉(zhuǎn)角速率4.2.1機(jī)體系旋轉(zhuǎn)修正姿態(tài)四元數(shù)q：以考慮捷聯(lián)傳感器(機(jī)體)B系相對于非旋轉(zhuǎn)空間Bl“、qBI(m1)的正式定義為:I(m)BI(m1)qBI(m)tBtm"

50、;m1)J/dtLm1(59)式中B(t)是加1到幻時(shí)間間隔中任意時(shí)刻B系的姿態(tài).B一qB：；姿態(tài)四元數(shù)也可以用一個(gè)旋轉(zhuǎn)矢量的萬式來表達(dá).該旋轉(zhuǎn)矢量定義B,、系與(m)BII(m1)系之間的關(guān)系.對(5)式進(jìn)行泰勒級數(shù)展開可得如下各系數(shù)項(xiàng):cos0.5mBI(m1)朝0.50.5msin0.50.5m1.5m)23!(0.5m)45!(60)cos0.512(0.5m)2!4(0.5m)4!(60)式用姿態(tài)四元數(shù)修正旋轉(zhuǎn)矢量m與在4.1.1節(jié)用方向余弦矩陣CB修正m是的,而且是用由(35)、(41)和(42)或(35)、(46)和(47)式提供的相同算法計(jì)算出來的.4.2.2當(dāng)?shù)厮阶鴺?biāo)系(L

51、系)旋轉(zhuǎn)(58)式修正姿態(tài)四元數(shù)qB的方法是利用qL.考慮當(dāng)?shù)厮阶鴺?biāo)系L系相對非旋轉(zhuǎn)空I(n1)間的角速率LL與qLI(n)二者的關(guān)系來實(shí)現(xiàn)的.qLI(n)的正式定義是:1I(n1)1I(n1)(61)L(n)tnL(t)qL(n1)q1tn1qL(n1)dt(61)式中的L(t)代表tni到tn時(shí)間間隔內(nèi)任一時(shí)間的L系姿態(tài).姿態(tài)四元數(shù)qLI(n)也可以用旋轉(zhuǎn)矢量的方式來表達(dá),該旋轉(zhuǎn)矢量定義為L系的姿態(tài)相I(n1)In)對于LI系的關(guān)系.對(5)式進(jìn)行泰勒級數(shù)展開可得如下積分項(xiàng):I(n1)cos0.5nqLI(n)(n1)sin0-5n0.50.5n一一,一一、2,一一、4sin0.5n1(

52、0.5n)(0.5n)0.5n3!5!(62)(0.5n)2(0.5n)4COS0.5n12!4!.L,式中n的負(fù)號考慮了qL的反相指向,它描述了(n1)Li系姿態(tài)相對Li系的關(guān)系,這與旋(n1)(n),轉(zhuǎn)矢量n的指向不同,描述了Li.系姿態(tài)相對L|(m)系的關(guān)系.(62)式n旋轉(zhuǎn)矢量與用于方向余弦矩陣C：修正的n是一致的,且是用第4.1.2節(jié)中描述的且由(53)、(55)和(56)式提供的相同的方法計(jì)算出來的.在精度上可與(51)式方向余弦修正相媲美的(62)式的近似形式可以很輕易地通過代入和舍位求得：(n1)210.5(0.5n)20.5n(63)第4.1.2節(jié)中關(guān)于使用簡化的(51)式方

53、向余弦當(dāng)?shù)厮较敌拚惴ǖ暮侠硇缘囊庖娨策m用于(63)式姿態(tài)四元數(shù)修正算法,但不太適用于完整的(62)式那種形式.4.2.3歸一化為了保持第2.2節(jié)中所討論過的根本姿態(tài)四元數(shù)歸一化的特性,在姿態(tài)四元數(shù)q：修正過程中往往包含一個(gè)歸一化算法,作為該修正過程中的一個(gè)外環(huán)函數(shù).在第4.1.3節(jié)中有關(guān)方向余弦矩陣需要?dú)w一化或正交化的討論同樣適用于姿態(tài)四元數(shù),唯一的例外是正交化在定義四元數(shù)上毫無意義(如同定義姿態(tài)方向余弦矩陣那樣)；因此.在第4.1.3節(jié)中的正交化討論在此不適用.如果斷定要采用四元數(shù)正交化算法,那么該算法是基于使用一個(gè)限制算法將qB的模值與1相比擬,而且該模值與1的差值可利用迭代的限制算法進(jìn)行修正(參考文獻(xiàn)9,【12】第7.1.1.3節(jié),【15】216-218頁).5姿態(tài)積分算法總結(jié)表I匯總了介紹過的捷聯(lián)慣導(dǎo)姿態(tài)積分函數(shù)的種種算法,它是根據(jù)導(dǎo)航計(jì)算機(jī)中運(yùn)算執(zhí)行次序列出的.表1列出算法函數(shù)、輸入?yún)?shù)、輸出參數(shù)及方程序號數(shù).6算法及運(yùn)行速率選擇面對可供選擇的大量捷