捷聯(lián)式慣性導(dǎo)航積分算法設(shè)計(jì)-姿態(tài)算法

1、捷聯(lián)式慣性導(dǎo)航積分算法設(shè)計(jì)上篇：姿態(tài)算法Paul G. Savage Strapdown Associates, Inc., Maple Plain, Minnesota 55359摘要：本論文分上下兩篇，用于給現(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)動(dòng)角

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

3、 =單位四元數(shù)，它的第1項(xiàng)為1，其余3項(xiàng)為0 =無(wú)具體坐標(biāo)系定義的矢量 =列向量，它的各項(xiàng)元素等于矢量在坐標(biāo)系A(chǔ)的各軸上的投影 =向量的反對(duì)稱（或交叉積）形式，代表如下矩陣： 其中：，是的分量，與A系矢量的矩陣乘積等于與該矢量的叉積 =與等量的四元數(shù)矢量， =坐標(biāo)系相對(duì)于坐標(biāo)系的角速率，當(dāng)為慣性系（I系）時(shí)，是由安裝在坐標(biāo)系上的角速率傳感器所測(cè)到的角速率1.概論 慣性導(dǎo)航是通過對(duì)速度積分得到位置并對(duì)總加速度積分得到速度的過程?？偧铀俣仁侵赣芍亓铀俣群捅皇┘拥姆侵亓Ξa(chǎn)生的加速度（亦即比力加速度）之和。慣性導(dǎo)航系統(tǒng)( INS)包括：用于積分的導(dǎo)航計(jì)算機(jī)；用于給積分運(yùn)算定時(shí)的精密時(shí)鐘，測(cè)量比力加速

4、度用的加速度計(jì)組臺(tái)；用于作為所算位置的一個(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è)慣性傳感器之間的精確對(duì)準(zhǔn)，這樣的一種布置稱之為捷聯(lián)INS。因?yàn)閼T性傳感器牢固地固定在底盤內(nèi)，所以也就牢固地固定于安裝INS的飛行器上。 INS計(jì)算機(jī)中的基本函數(shù)有將角速率變換為姿態(tài)的積分函數(shù)(稱之為姿態(tài)積分)使用姿態(tài)數(shù)據(jù)將測(cè)

5、得的加速度值轉(zhuǎn)換到適當(dāng)?shù)膶?dǎo)航坐標(biāo)系中，再將它積分成矢量速度的函數(shù)(稱之為矢量速度積分)，還有將導(dǎo)航系矢量速度積分成位置的函數(shù)(稱之為位置積分)。這樣就有了三個(gè)積分函數(shù)，姿態(tài)函數(shù)、矢量速度函數(shù)及位置函數(shù)，每個(gè)函數(shù)的精度要求很高，以確保函數(shù)誤差極小，符臺(tái)慣性傳感器精度的要求?；仨鴼v史，因?yàn)樵缭?0年代，基礎(chǔ)捷聯(lián)慣導(dǎo)概念就開始形成，所以多年來捷聯(lián)分析師將精力主要集中于姿態(tài)積分函數(shù)算法的設(shè)計(jì)上。而種種算法的設(shè)計(jì)方法總是受到當(dāng)時(shí)的飛行計(jì)算機(jī)技術(shù)的能力和局限性的影響。50年代后期和60年代，各種研究機(jī)構(gòu)的捷聯(lián)工作者們采取兩種用于姿態(tài)積分函數(shù)運(yùn)算的方法，即用一階數(shù)字算法進(jìn)行高速姿態(tài)修正運(yùn)算，如1020kHz

6、和用高階算法進(jìn)行的低速姿態(tài)修正運(yùn)算，如50100HZ。如果要精確考慮高頻角速率分量時(shí)，就得考慮用高速算法，這樣可以調(diào)整為系統(tǒng)的三維姿態(tài)變化。然而，那個(gè)時(shí)期的計(jì)算機(jī)工藝技術(shù)只能對(duì)姿態(tài)修正算法進(jìn)行一些簡(jiǎ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)作對(duì)所算姿態(tài)參數(shù)的解析形，這一優(yōu)點(diǎn)使之成為一種更可取的算法(與傳統(tǒng)方向余弦矩陣姿態(tài)表示法相比)所以它的出現(xiàn)使上述兩種算法的優(yōu)點(diǎn)黯然失色。就那一時(shí)期研究過的算

7、法而言，四元數(shù)法在高頻角速率環(huán)境下表現(xiàn)出的運(yùn)算精度最優(yōu)。1966年，作者提出一個(gè)新的兩速姿態(tài)積分函數(shù)運(yùn)算方法，在該方法中，姿態(tài)修正運(yùn)算被分成兩部分，即把簡(jiǎn)單高速一階算法部分與更復(fù)雜的中速高階算法部分結(jié)合起來使用，后一部分的輸入由高速算法提供。簡(jiǎn)化了的高速部分用于考慮姿態(tài)修正循環(huán)內(nèi)的高頻角振蕩，這能調(diào)整系統(tǒng)的姿態(tài)建立(傳統(tǒng)上稱之為錐化)。合在一起，兩速方法的組合精度等于以高速速率進(jìn)行的高階運(yùn)算(為了提高精度)；然而 由于高速算法簡(jiǎn)化之故，組合后對(duì)計(jì)算機(jī)的吞吐量的要求絲毫也不比原來的高速一階姿態(tài)修正算法的吞吐量大。參考文獻(xiàn)【6】采用的兩速算法設(shè)計(jì)受到其基本解析公式的限制，該公式是一個(gè)皮卡德類型（P

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

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

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

11、原來設(shè)想的簡(jiǎn)單一階的種種現(xiàn)有高速姿態(tài)算法利用現(xiàn)代計(jì)算機(jī)日益增加的吞吐量，演變?yōu)楦鞣N高階算法，而且經(jīng)度得到提高，(參見參考文獻(xiàn)【9】【11】和【12】的第7-1節(jié))。對(duì)于姿態(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í)，在可能的情況下，考慮到便于生成計(jì)算機(jī)軟件的文件及有效性(這與現(xiàn)代飛

12、行計(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)用算法和建立運(yùn)算運(yùn)行速率之后的過程進(jìn)行了一般化討論，

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

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

15、地球表面參考位置的垂直向上方向平行，N系用于將加速度積分為速度并定義E系內(nèi)當(dāng)?shù)卮怪苯欠较颍?.L系是當(dāng)?shù)厮阶鴺?biāo)系，與N系平行，但Z軸垂直向下，而X和Y軸沿N系的Y和X軸。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)測(cè)量基準(zhǔn)。為I系選擇的特殊方向在其方向與解析運(yùn)算有關(guān)的章節(jié)中進(jìn)行討論。 2.2姿態(tài)參數(shù)定義 方向余弦矩陣定義為一個(gè)方陣，其各列是單位矢量的正交組，每列等于沿系的一個(gè)軸方向上的單位矢量在系各軸上的投影 ： (1)式中是沿系軸i方向上的單位矢量在坐標(biāo)系各軸上的投影。從這

16、一基本定義出發(fā)，可以驗(yàn)證出：的i行、j列的元素等于系軸i和系軸j之間的夾角的余弦值，的轉(zhuǎn)置矩陣等于其逆矩陣，轉(zhuǎn)置矩陣的各列等于系軸中單位矢量在系各軸上的投影，且與一個(gè)投影于系各軸上的向量的積等于此向量在系各軸上的投影分量(而對(duì)的轉(zhuǎn)置矩陣則相反)。 (2)(2)式可以用于導(dǎo)出方向余弦矩陣鏈?zhǔn)揭?guī)律： (3)旋轉(zhuǎn)矢量定義旋轉(zhuǎn)軸和繞軸的旋轉(zhuǎn)幅度?？梢韵胂裣低ㄟ^繞此旋轉(zhuǎn)矢量旋轉(zhuǎn)而從它的起始姿態(tài)轉(zhuǎn)到一個(gè)新的姿態(tài)，繞旋轉(zhuǎn)矢量旋轉(zhuǎn)的角度等于旋轉(zhuǎn)矢量的大小?，F(xiàn)在，稱系為系的新姿態(tài)，通過給系的這一定義，一個(gè)任意定義的旋轉(zhuǎn)矢量就可唯一地確定系姿態(tài)與原始系姿態(tài)二者的關(guān)系。反之，如果給定系和系的姿態(tài)關(guān)系，就可以定義一個(gè)

17、與此姿態(tài)一致的旋轉(zhuǎn)矢量。這樣一來，旋轉(zhuǎn)矢量可以用來定義系與系相關(guān)的姿態(tài)。從解析式上看(見參考文獻(xiàn)【4】，【9】和【12】的第3.2.2.1節(jié))旋轉(zhuǎn)矢量和方向余弦矩陣之間的關(guān)系可以由下式給出： (4)式中和是旋轉(zhuǎn)矢量及其幅度。旋轉(zhuǎn)矢量的一個(gè)獨(dú)特特性是在系和系中有相同的分量(見參考文獻(xiàn)：【12】的第3.2.2.1節(jié))；因此 (4)式中的代表或.姿態(tài)四元數(shù)是一個(gè)四矢量，即有四個(gè)分量，定義為旋轉(zhuǎn)矢量的一個(gè)函數(shù)(見參考文獻(xiàn)【4】，【9】和【12】的第3.2.4節(jié)和【14】的第7376頁(yè)) （5）由(5)式可以看出，各元素的平方和是單位1。坐標(biāo)變換方程可以用四元數(shù)代數(shù)方程表示(見參考文獻(xiàn)【4】、【9】和【

18、12】的第3.2.4.1節(jié))。 ， （6）(6)式可以用來推導(dǎo)出姿態(tài)四元數(shù)的鏈?zhǔn)揭?guī)律： （7）2.3 姿態(tài)參數(shù)速率方程 第22節(jié)姿態(tài)參數(shù)的變化速率(見參考文獻(xiàn)【4】、【8】、【9】和【12】的第3.3節(jié))由下式給出： （8） （9） （10） 3 連續(xù)型捷聯(lián)慣導(dǎo)方程定義捷聯(lián)慣導(dǎo)系統(tǒng)中進(jìn)行的典型基本運(yùn)算的微分方程(見參考文獻(xiàn) 【9】、【l2】的第4章和【15】的第77103頁(yè)以及156177頁(yè))給出如下：姿態(tài)速率： (11) 或代之以： (12)當(dāng)?shù)厮较敌D(zhuǎn)速率： (13) (14) (15)加速度變換: (16)或代之以： (17) (18)矢量速度速率: (19) (20)位置速率： (2

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

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

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

22、5.3以及【15】的88，89頁(yè))。對(duì)于位置速率方程而言，位置也可以定義為簡(jiǎn)單的R(從中可以計(jì)算出和h,見參考文獻(xiàn)【12】的第4.5.4節(jié))。高度速率方程(22)顯得價(jià)值不大，但當(dāng)你考慮旋轉(zhuǎn)扁地球模型、地球上的一個(gè)旋轉(zhuǎn)N系和固定的高度定義時(shí)，這就不一定了。參考文獻(xiàn)【12】的4.4節(jié)和5.5節(jié)表明對(duì)于旋轉(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頁(yè)以及【18】的10.3節(jié))。4 姿態(tài)即時(shí)修正算法 本節(jié)探討適合用數(shù)

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

24、離散點(diǎn)上的當(dāng)前構(gòu)造值?？紤]到完全一般化的情形，我們還允許由于L系角運(yùn)動(dòng)的修正運(yùn)算不一定是在當(dāng)B系運(yùn)動(dòng)而對(duì)修正的同一時(shí)刻瞬時(shí)運(yùn)行，例如在一個(gè)多速率數(shù)字計(jì)算回路結(jié)構(gòu)中，以高速率進(jìn)行修正是由于B系旋轉(zhuǎn)而不是由于L系旋轉(zhuǎn)。如果對(duì)計(jì)算機(jī)吞吐量的最小化感興趣的話，從軟件結(jié)構(gòu)上說，可以讓L系修正比B系修正慢510倍。我們用來描述坐標(biāo)系方向隨時(shí)間變化的專門術(shù)語(yǔ)如下： =當(dāng)計(jì)算機(jī)修正時(shí)間為時(shí)，機(jī)體B系在非旋轉(zhuǎn)慣性空間I中的離散方向； =因B系角運(yùn)動(dòng)對(duì)進(jìn)行即時(shí)修正的計(jì)算機(jī)循環(huán)拍次； =當(dāng)?shù)厮絃系在計(jì)算機(jī)修正時(shí)間在非旋轉(zhuǎn)慣性空間I中的離散方向； =因L系角運(yùn)動(dòng)對(duì)進(jìn)行即時(shí)修正的計(jì)算機(jī)循環(huán)拍次；有了這些定義，采用(3

25、)式方向余弦矩陣遞推計(jì)算規(guī)律便能建立起對(duì)即時(shí)修正算法如下： （23） （24）其中：=時(shí)刻B系與時(shí)刻L系的轉(zhuǎn)換關(guān)系矩陣；=時(shí)刻B系與時(shí)刻L系的轉(zhuǎn)換關(guān)系矩陣；=考慮到B系相對(duì)慣性空間從時(shí)刻點(diǎn)到時(shí)刻點(diǎn)的旋轉(zhuǎn)的方向余弦矩陣；=考慮到L系相對(duì)慣性空間從時(shí)刻點(diǎn)到時(shí)刻點(diǎn)的旋轉(zhuǎn)的方向余弦矩陣； 在(23)和(24)式描述的算法把各個(gè)時(shí)間的機(jī)體B系和當(dāng)?shù)厮絃系的方向聯(lián)系起來，并為B系與L系提供了以不同修正速率對(duì)進(jìn)行的慣性角運(yùn)動(dòng)修正。與B系(它能以200-300度/秒的速度進(jìn)行動(dòng)態(tài)旋轉(zhuǎn))相比，當(dāng)?shù)厮絃系的慣性角速率一般來說較小，它等于地球旋轉(zhuǎn)速率加上L系相對(duì)地球的角速率(傳遞速率，它一般不會(huì)大過地球速率的幾

26、倍)。因此，L系即時(shí)修正一般可以以低于B系修正的速率完成，而且有相當(dāng)?shù)木取?yīng)當(dāng)注意，對(duì)B系和L系運(yùn)動(dòng)修正速率的要求，在某種程度上，是通過和測(cè)量姿態(tài)變化的近似高速算法作比較，將誤差降至最小的基礎(chǔ)上確立的(見本文4.1.1和4.1.2節(jié))。對(duì)的B系和L系的運(yùn)動(dòng)修正是由(23)和(24)式的項(xiàng)和項(xiàng)完成的，其算法分別推導(dǎo)如下：4.1.1 機(jī)體系旋轉(zhuǎn) (23)式用來考慮捷聯(lián)傳感器(機(jī)體)B系相對(duì)于非旋轉(zhuǎn)空間的角速率來修正姿態(tài)方向余弦矩陣，的正式定義為： （25）式中B(t)是到時(shí)間間隔內(nèi)任意時(shí)間的機(jī)體B系姿態(tài)。 矩陣還可以用定義的系姿態(tài)相對(duì)于系的旋轉(zhuǎn)矢量來表示。對(duì)(4)式進(jìn)行泰勒級(jí)數(shù)展開，可得系數(shù)項(xiàng)：

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

28、便可以通過從時(shí)刻到時(shí)刻對(duì)(27)式進(jìn)行積分求得： ， （28）式中是運(yùn)行中的積分時(shí)間變量。要減少(27)式中求算值的計(jì)算次數(shù)，應(yīng)加入簡(jiǎn)化設(shè)計(jì)。例如，通過冪級(jí)數(shù)擴(kuò)展，可以將(27)式中的項(xiàng)的放大系數(shù)近似為： （29）因此，(27)式的可以由下式給出： （30）通過仿真和分析(在假定的解析定義角運(yùn)動(dòng)條件下進(jìn)行解析擴(kuò)展)，可以得到保證值的二階精度的表達(dá)式： （31）式中： （32）(31)式很重要，因?yàn)樗苁?27)式簡(jiǎn)化到二階精度，即的誤差為三階，只保持-階項(xiàng)。這樣一來，(27)式就變成二階精度： （33）代入方程(33)，(28)式則變?yōu)橄率剑?（34）最后，用(32)式可得： （35）其中：

29、， （36）式中是從到的圓錐姿態(tài)運(yùn)動(dòng)。項(xiàng)被錐化成錐化項(xiàng)目，因?yàn)樗艿綔y(cè)量的圓錐運(yùn)動(dòng)分量的影響。圓錐運(yùn)動(dòng)定義為角速度矢量本身在旋轉(zhuǎn)的狀態(tài)。由于表現(xiàn)為純圓錐運(yùn)動(dòng)(的幅度雖然恒定不變，但其矢量在旋轉(zhuǎn))，則系的一個(gè)固定軸近似垂直于矢量的旋轉(zhuǎn)平面，該固定軸將隨著角速度的運(yùn)動(dòng)而形成一個(gè)錐面(因此用術(shù)語(yǔ)圓錐運(yùn)動(dòng)來描述這個(gè)運(yùn)動(dòng))。在圓錐角運(yùn)動(dòng)條件下，與垂直的系軸表現(xiàn)為振蕩(這與非圓錐運(yùn)動(dòng)或自轉(zhuǎn)角運(yùn)動(dòng)相反，在后一種情況下，于垂直的軸繞旋轉(zhuǎn))。對(duì)不發(fā)生旋轉(zhuǎn)的情況而言，從(36)式可以容易看出，將與平行，因此中的積分函數(shù)的叉乘將是零，也將是零。這種情況下，即當(dāng)不發(fā)生旋轉(zhuǎn)時(shí)，(34)式將變成如下簡(jiǎn)化形式： （37）注

30、意：（37）式也適用于當(dāng)不旋轉(zhuǎn)時(shí)，（27）和(28)式中的精確求解，即不存在近似問題。這點(diǎn)可以通過觀察(27)式很容易地得到驗(yàn)證，因?yàn)樵?27)式中，在積分開始時(shí)就與對(duì)齊，并將與保持平行，這是由于在表達(dá)式中，與叉乘將保持為零。在這種條件下，(27)式和(28)式也將化簡(jiǎn)為(37)式。 下面討論積分角速率和圓錐增量算法。關(guān)于(36)式中的積分速率和的錐化表述的離散化數(shù)字算法可以通過這樣的方法開發(fā)出來，即考慮是一般函數(shù)在=時(shí)刻的值，由（36）式得： （38）現(xiàn)在考慮將(38)式的積分分成在到時(shí)間間隔內(nèi)到達(dá)時(shí)刻的時(shí)間和時(shí)刻之后的時(shí)間兩部分，這樣一來，(38)式等于： ， （39）式中：是時(shí)值，而是時(shí)

31、的計(jì)算機(jī)的循環(huán)拍次。需注意，根據(jù)上述定義，循環(huán)拍次快于循環(huán)拍次。現(xiàn)在，我們?cè)俣x一個(gè)在到的時(shí)間間隔內(nèi)循環(huán)時(shí)間點(diǎn)，于是在時(shí)間點(diǎn)，包擴(kuò)初始條件在內(nèi)的（39）式就變?yōu)椋?， 當(dāng)， （40） 通過類似過程，（40）式的表達(dá)式可以通過（36）式的計(jì)算求得： ， ， （41） 當(dāng)，根據(jù)(41)式和(42)式得： ， （42） 當(dāng)， (41)和(42)式構(gòu)成數(shù)字遞歸算法，該算法以作為計(jì)算機(jī)循環(huán)速率計(jì)算和錐化項(xiàng)，作為從到的整個(gè)時(shí)間間隔內(nèi)和的變化之和，(42)式中積分項(xiàng)的數(shù)字當(dāng)量仍尚待確定。姿態(tài)算法開發(fā)方面的后續(xù)工作集中在估算錐化方程(42)中積分項(xiàng)的數(shù)字算法設(shè)計(jì)上，一一般而言，所采用的方法都是對(duì)從到時(shí)間間隔內(nèi)

32、的角速率模型假定一個(gè)一般的解析式，如一個(gè)在時(shí)間上截尾的一般多項(xiàng)式。于是(42)式的積分項(xiàng)就可以以解析的形式作為一般速率模型系數(shù)的一個(gè)函數(shù)，如多項(xiàng)式系數(shù)來表示。最后，角速率模型系數(shù)是通過匹配連續(xù)積分計(jì)算出的角速率增量的測(cè)量值來計(jì)算出來的。就隨后的舉例而言，角速率模型被近似為一個(gè)常數(shù)加一個(gè)實(shí)時(shí)線性累積值，其常數(shù)和梯度是由現(xiàn)有的和以前的值計(jì)算出來的。此算法的一個(gè)更為復(fù)雜的形式可能是在假設(shè)的角速率模型中包含有一個(gè)時(shí)變平方項(xiàng)，用現(xiàn)有的、過去的以及過去之過去的值判定系數(shù)。近來在這一領(lǐng)域進(jìn)行的工作是通過利用到時(shí)間間隔內(nèi)角速率傳感器的測(cè)量值來計(jì)算角速率模型系數(shù)（參見文獻(xiàn)【19】提出的一個(gè)用于強(qiáng)化單速算法的技術(shù)

33、擴(kuò)展），從而將第三個(gè)循環(huán)速率加到整個(gè)姿態(tài)修正過程的結(jié)構(gòu)中：姿態(tài)()修正和錐化()修正(正如至此所作的討論)以及錐化修正的傳感器采樣。后一種技術(shù)在改進(jìn)時(shí)采用一個(gè)一般的角速率模型，該角速率模型是依據(jù)其對(duì)(42)式中積分項(xiàng)的影響直接定義為到時(shí)間間隔內(nèi)連續(xù)積分角速率增量傳感器采樣值之間的加權(quán)叉乘之和(類似于文獻(xiàn)【19】介紹的到時(shí)間間隔內(nèi)的方法)。 在后一種情況下的加權(quán)系數(shù)于是得到優(yōu)化并可以獲取純錐化環(huán)境下的最佳平均性能，亦即在幅度上為常數(shù)，但在旋轉(zhuǎn)。之后提到的種種設(shè)計(jì)方法中的每一種都是以根據(jù)假定的角速率模型形狀使用曲面擬合技術(shù)為基礎(chǔ)的。每一種的最終算法在不是為某個(gè)速率環(huán)境而設(shè)計(jì)時(shí)和存在有角速率傳感器量

34、化噪聲的情況下會(huì)表現(xiàn)不同。更為可取的算法的選擇，務(wù)必包括仿真分析以確認(rèn)運(yùn)行速率環(huán)境下和傳感器噪聲特性狀態(tài)下是否可以接受。現(xiàn)通過算法舉倒對(duì)這一部分進(jìn)行總結(jié)。這兒舉出的是(42)式積分項(xiàng)算法的例子，它的機(jī)理是通過對(duì)冪級(jí)數(shù)進(jìn)行截尾展開，使機(jī)體速率項(xiàng)被近似到一階：， 和=常數(shù) （43）參考文獻(xiàn)【9】-【11】和【12】的第7.1.1.1.1表明對(duì)于時(shí)間段到的(43)式運(yùn)動(dòng)有： （44）將(44)式代入(42)式，可得： （45）(45)式從類別上可歸為求的二階算法，這是因?yàn)樗诜匠讨邪鞋F(xiàn)在的和過去的循環(huán)乘積。從導(dǎo)出(44)式的分析可以看出，中的和乘積項(xiàng)即項(xiàng)是由到時(shí)間間隔內(nèi)線性斜坡角速率的近似化得來

35、的。如果角速率近似化成時(shí)間的二次曲線變化函數(shù)，那么，勢(shì)必導(dǎo)致包含，和的乘積的三階算法，如果角速率被近似成到整個(gè)時(shí)間間隔內(nèi)的一個(gè)常數(shù)，那么，(45)式中的項(xiàng)勢(shì)必導(dǎo)致求的一階算法。最后，如果角速率正在慢慢變化，那么，我們可以將近似為等于零。換言之(或更準(zhǔn)確地說)，我們可以讓循環(huán)速率等于循環(huán)速率這使等于用(45)式在時(shí)刻計(jì)算出來的 注意根據(jù)(41)式初始狀態(tài)定義為零，后一種算法在參考文獻(xiàn)【4】得到了改進(jìn)。須注意要設(shè)定和速率相等，還可以通過增加速率使之與速率相匹配的辦法實(shí)現(xiàn)。結(jié)果得到一個(gè)單一的高速高階算法，該算法附帶有一個(gè)比兩速算法更為簡(jiǎn)單的軟件結(jié)構(gòu)，但是，需要更大的吞吐量，現(xiàn)代計(jì)算機(jī)速度的不斷提高使

36、這一算法在未來成為更為可取的算法。 (35)式和的全部數(shù)字算法是由(41)、(42)和(45)式合并后得出的結(jié)果所決定的： ， ，當(dāng)時(shí) （46） ， ， 當(dāng)時(shí) （47）式中：微積分角速率增量，即：捷聯(lián)角速率傳感器給出的脈沖輸出的解析表達(dá)式，即=由角速率傳感器給出的積分角速率輸出增量之和。4.1.2 當(dāng)?shù)厮较档男D(zhuǎn) （24）式采用式來修正姿態(tài)方向余弦矩陣，以考慮本地水平坐標(biāo)系系相對(duì)于非旋轉(zhuǎn)空間的角速率.的推導(dǎo)與第4.1.1節(jié)的推導(dǎo)非常類似。的正式定義是： （48）式中，是到時(shí)間間隔內(nèi)任意時(shí)刻的系姿態(tài)。矩陣也可以用定義的系的姿態(tài)相對(duì)于系的旋轉(zhuǎn)矢量來表示。對(duì)（4）式進(jìn)行泰勒級(jí)數(shù)展開可得系數(shù)項(xiàng)： （

37、49） 式中是定義時(shí)刻系的姿態(tài)相對(duì)于時(shí)刻系的旋轉(zhuǎn)矢量。注意：在(49)式中，項(xiàng)的符號(hào)與(26)式表達(dá)式的同類項(xiàng)的符號(hào)相反，是負(fù)的。這是因?yàn)榕c（或(4)式）的指向相反，的矢量變換是從到，而的矢量變換是從到。這樣，(49)式中的形式便是(26)式中表達(dá)式的轉(zhuǎn)置。因?yàn)榈降男拚芷谙鄬?duì)較短，在數(shù)值上將會(huì)很小。又因?yàn)樾∏以谡麄€(gè)到的典型修正周期內(nèi)變化緩慢(由于該時(shí)間周期內(nèi)的矢量速度和位置變化小)，所以系速率矢量可以近似為非旋轉(zhuǎn)。結(jié)果可以像旋轉(zhuǎn)矢量方程(1O)的簡(jiǎn)化形式一樣計(jì)算(49)，這里的叉乘項(xiàng)被忽略不計(jì)： （50）順便指出，鑒于是個(gè)小量，如已討論過的那樣，用于的(49)式亦可簡(jiǎn)化。例如，可從（49）式

38、導(dǎo)出一個(gè)二階形式： （51） 對(duì)于現(xiàn)行的現(xiàn)代計(jì)算機(jī)技術(shù)而言，采用解算的(49)式簡(jiǎn)化形式如(51)式的計(jì)算機(jī)在內(nèi)存或吞吐量方面的優(yōu)點(diǎn)與增加軟件有效性或文件復(fù)雜性以及精確度上損失相比較，是微不足道的。精確度損失在導(dǎo)航過程中一般來說是很小的；然而，在初始對(duì)準(zhǔn)運(yùn)算過程中(慣性導(dǎo)航啟動(dòng)之前)這種精度損失不可忽視，初始對(duì)準(zhǔn)運(yùn)算時(shí)矩陣常被用于對(duì)進(jìn)行傾斜修正(參考文獻(xiàn)【12】第6.1.2節(jié)和第15節(jié)l20121頁(yè))。對(duì)的初始傾斜對(duì)準(zhǔn)修正量可能會(huì)很大，如0.1-1度，如果對(duì)(49)式采用過于簡(jiǎn)化的形式，在初始對(duì)準(zhǔn)過程中就可能產(chǎn)生不希望的誤差。初始對(duì)準(zhǔn)運(yùn)算的閉環(huán)伺服運(yùn)動(dòng)最終會(huì)修正由產(chǎn)生的結(jié)果性姿態(tài)誤差；然而，它

39、可能會(huì)將冗余正交誤差留在行(和列)里面。其結(jié)果一定是需要包括一個(gè)正交或歸一化修正算法(見第4.1.3節(jié))作為修正過程的外層循環(huán)。(50)式積分的離散數(shù)字算法可以通過下述方法建立起來，即第一步先將(13)式和(15)式合并，求出被積函數(shù)，然后進(jìn)行如下近似計(jì)算： (52)式中，下標(biāo)是到的中途()的值。將(52)式代入(5O)式，然后可得： （53）用(14)式估算且 （54）式中，是計(jì)算機(jī)循環(huán)修正周期，而是在到的計(jì)算機(jī)修正循環(huán)內(nèi)循環(huán)的循環(huán)次數(shù)。 (53)式下標(biāo)為的項(xiàng)都是位置的函數(shù)，它(見下篇參考文獻(xiàn)【13】)在以循環(huán)速率進(jìn)行的姿態(tài)修正之后得到更新。因此，要計(jì)算(52)式中的這些項(xiàng)，必須在以前計(jì)算的

40、（）參數(shù)的基礎(chǔ)上，采用一個(gè)近似外推公式。例如，采用最后兩次（）的計(jì)算值的線性外推公式，將變?yōu)椋?（55）在下篇(參考文獻(xiàn)【13】)中，我們發(fā)現(xiàn)速度更新緊跟著位置更新。因此，現(xiàn)在的和過去的循環(huán)的數(shù)值可用于估算(54)式中的積分。對(duì)(54)式進(jìn)行梯形積分可得： （56）式中是計(jì)算機(jī)循環(huán)的更新周期。下篇(參考文獻(xiàn)【13】 )還開發(fā)了一種具有高分辯能力的算法，用于精密位置修正，它考慮了從到循環(huán)修正時(shí)間間隔內(nèi)的動(dòng)態(tài)角速率和加速度變化。4.1.3 歸一化和正交化 根據(jù)第2.2節(jié)的基本定義的列(和行)代表正交單位矢量，因此，它們?cè)谀Ｖ瞪系扔?(歸一化條件)且彼此相互正交(正交化條件)。更新算法中，除了已描述

41、過的基本修正算法外，也常常包括一個(gè)歸一化和正交化算法以保證的各行和各列正交。導(dǎo)致正交誤差的因素包括正交初始化誤差、軟件編程誤差、由于計(jì)算機(jī)字長(zhǎng)小于用于算法更新周期所希望的全部位數(shù)而引起的舍入誤差、以及(26)和(49)式泰勒級(jí)數(shù)展開中所選項(xiàng)數(shù)太少而引起的舍位誤差。重要的一點(diǎn)是要注意到(參考文獻(xiàn)【12】第3.4.1節(jié))：歸一化和正交化誤差只是由于(23)、(24)、(26)及(49)式的軟件運(yùn)行過程中出現(xiàn)的誤差所引起的，而不是由于供這些方程的算法誤差或是由慣性傳感器輸入誤差所引起。積分算法軟件的總體設(shè)計(jì)或校正過程必須保證：在預(yù)想的導(dǎo)航時(shí)間周期內(nèi)和角速率環(huán)境條件下，編程無(wú)誤差并且舍入誤差和舍位誤差

42、在可以接受的范圍內(nèi)。這是一個(gè)用現(xiàn)代計(jì)算機(jī)技術(shù)或軟件開發(fā)技術(shù)可輕易實(shí)現(xiàn)的目標(biāo)。不過，將正交化或歸一化修正算法包括進(jìn)去是許多捷聯(lián)慣導(dǎo)軟件包一直采用的傳統(tǒng)作法，目的在于提高精度，并放寬不許運(yùn)算中出現(xiàn)任何正交誤差這一嚴(yán)格要求。用于歸一化正交的算法是以下面這一特性為基礎(chǔ)的；即方向余弦矩陣的轉(zhuǎn)置矩陣等于其逆矩陣(見2.2節(jié))；因此，與其轉(zhuǎn)置矩陣之乘積為單位矩陣。這一狀態(tài)的改變，可用來測(cè)量正交誤差，而該誤差在迭代運(yùn)算中可被控制算法修正(參考文獻(xiàn)【9】，【12】第7.1.1.3節(jié)，【15】216-218頁(yè))4.2 姿態(tài)四元數(shù)設(shè)計(jì)姿態(tài)四元數(shù)的修正算法的目的是為了使在各姿態(tài)修正時(shí)刻得到的數(shù)字結(jié)果跟(12)式表達(dá)式

43、的正規(guī)連續(xù)積分在同一時(shí)刻瞬間得到的數(shù)字結(jié)果相同。姿態(tài)四元數(shù)修正算法是嚴(yán)格按照本論文第41節(jié)用修正算法推導(dǎo)的同樣過程開發(fā)出來的。因此，采用(7)式姿態(tài)四元數(shù)鏈?zhǔn)揭?guī)律可以寫出下式： （57） （58）式中： =、時(shí)刻的系到時(shí)刻的系的四元數(shù)；=時(shí)刻的系到時(shí)刻的系的四元數(shù)；=考慮了系相對(duì)慣性空間旋轉(zhuǎn)的從時(shí)刻的方向到時(shí)刻的方向的姿態(tài)四元數(shù)；=考慮了系相對(duì)慣性空間旋轉(zhuǎn)的從時(shí)刻的方向到時(shí)刻的方向的姿態(tài)四元數(shù)。的修正是由(57)式和(58)式中和完成的，兩個(gè)算法分別推導(dǎo)如下：4.2.1 機(jī)體系旋轉(zhuǎn)(57)式用修正姿態(tài)四元數(shù)以考慮捷聯(lián)傳感器(機(jī)體) 系相對(duì)于非旋轉(zhuǎn)空間的旋轉(zhuǎn)角速率，的正式定義為： （59）式中是

44、到時(shí)間間隔中任意時(shí)刻系的姿態(tài)。姿態(tài)四元數(shù)也可以用一個(gè)旋轉(zhuǎn)矢量的方式來表達(dá)。該旋轉(zhuǎn)矢量定義系與系之間的關(guān)系。對(duì)(5)式進(jìn)行泰勒級(jí)數(shù)展開可得如下各系數(shù)項(xiàng)： （60） （60）式用姿態(tài)四元數(shù)修正旋轉(zhuǎn)矢量與在4.1.1節(jié)用方向余弦矩陣修正是一致的，而且是用由(35)、(41)和(42)或(35)、(46)和(47)式提供的相同算法計(jì)算出來的。4.2.2 當(dāng)?shù)厮阶鴺?biāo)系（系）旋轉(zhuǎn) （58）式修正姿態(tài)四元數(shù)的方法是利用。考慮當(dāng)?shù)厮阶鴺?biāo)系L系相對(duì)非旋轉(zhuǎn)空間的角速率與二者的關(guān)系來實(shí)現(xiàn)的。的正式定義是： （61）(61)式中的代表到時(shí)間間隔內(nèi)任一時(shí)間的系姿態(tài)。姿態(tài)四元數(shù)也可以用旋轉(zhuǎn)矢量的方式來表達(dá)，該旋轉(zhuǎn)矢量

45、定義為系的姿態(tài)相對(duì)于系的關(guān)系。對(duì)(5)式進(jìn)行泰勒級(jí)數(shù)展開可得如下積分項(xiàng)： （62） 式中的負(fù)號(hào)考慮了的反相指向，它描述了系姿態(tài)相對(duì)系的關(guān)系，這與旋轉(zhuǎn)矢量的指向不同，描述了系姿態(tài)相對(duì)系的關(guān)系。(62)式旋轉(zhuǎn)矢量與用于方向余弦矩陣修正的是一致的，且是用第4.1.2節(jié)中描述的且由(53)、(55)和(56)式提供的相同的方法計(jì)算出來的。在精度上可與(51)式方向余弦修正相媲美的(62)式的近似形式可以很輕易地通過代入和舍位求得： （63）第4.1.2節(jié)中關(guān)于使用簡(jiǎn)化的(51)式方向余弦當(dāng)?shù)厮较敌拚惴ǖ暮侠硇缘囊庖娨策m用于(63)式姿態(tài)四元數(shù)修正算法，但不太適用于完整的(62)式那種形式。4.2.

46、3 歸一化 為了保持第2.2節(jié)中所討論過的基本姿態(tài)四元數(shù)歸一化的特性，在姿態(tài)四元數(shù)修正過程中往往包含一個(gè)歸一化算法，作為該修正過程中的一個(gè)外環(huán)函數(shù)。在第4.1.3節(jié)中有關(guān)方向余弦矩陣需要?dú)w一化或正交化的討論同樣適用于姿態(tài)四元數(shù)，唯一的例外是正交化在定義四元數(shù)上毫無(wú)意義（如同定義姿態(tài)方向余弦矩陣那樣)；因此在第4.1.3節(jié)中的正交化討論在此不適用。如果決定要采用四元數(shù)正交化算法，那么該算法是基于使用一個(gè)控制算法將的模值與1相比較，而且該模值與1的差值可利用迭代的控制算法進(jìn)行修正(參考文獻(xiàn)【9】，【12】第7.1.1.3節(jié)，【15】216-218頁(yè))。 5 姿態(tài)積分算法總結(jié)表I匯總了介紹過的捷聯(lián)慣

47、導(dǎo)姿態(tài)積分函數(shù)的種種算法，它是按照導(dǎo)航計(jì)算機(jī)中運(yùn)算執(zhí)行次序列出的。表1列出算法函數(shù)、輸入?yún)?shù)、輸出參數(shù)及方程序號(hào)數(shù)。6 算法及運(yùn)行速率選擇 面對(duì)可供選擇的大量捷聯(lián)慣導(dǎo)算法，軟件設(shè)計(jì)人員終歸要選擇一套適于自己使用的算法，把它掌握在手。本論文上篇和續(xù)篇中介紹的算法只是多年來好幾位作者發(fā)表了的很多類似算法中的一種版本。在選擇適合于特定用途的算法的過程中務(wù)必要考慮到：在預(yù)定角速率、角加速度或振動(dòng)的條件下算法的容許誤差，還需考慮按指標(biāo)設(shè)計(jì)的導(dǎo)航計(jì)算機(jī)對(duì)所選算法的運(yùn)行速度的能力要求，還要對(duì)電腦軟件有效的程序設(shè)計(jì)的復(fù)雜性以及包含所選算法的文件中所提供的設(shè)計(jì)過程的復(fù)雜性有所了解。對(duì)擬選算法誤差特性的評(píng)估一般通

48、過微機(jī)化的時(shí)域仿真機(jī)來完成，仿真機(jī)以所選重復(fù)速率并以特定的運(yùn)算組進(jìn)行算法運(yùn)算。仿真機(jī)產(chǎn)生仿真的捷聯(lián)慣導(dǎo)傳感器的角速率模型或加速度模型，并將此用于算法的測(cè)試輸入，也可用于算法輸出，來比較已知的導(dǎo)航參數(shù)解，如參考文獻(xiàn)【12】第11.2節(jié)。對(duì)于討論過的姿態(tài)算法而言，簡(jiǎn)化解析誤差模型也可以作為算法重復(fù)速率的一個(gè)函數(shù)，并利用它在特定錐化速率或幅度情況下預(yù)估高速錐化算法的誤差（見參考文獻(xiàn) 【9】-【11】和【12】的第10節(jié))。錐化速率或幅度必須能從實(shí)驗(yàn)數(shù)據(jù)中推導(dǎo)出來，或是更普遍地從傳感器組臺(tái)基座的失衡解析模型以及它對(duì)特殊頻率進(jìn)行的外部輸入振動(dòng)的響應(yīng)中推導(dǎo)出來(參考文獻(xiàn)【12】第lO節(jié))。頻域仿真機(jī)可以在

49、特定輸入振動(dòng)功率頻譜密度和傳感器組合基座失衡情況下作為算法重復(fù)速率的一個(gè)函數(shù)(見參考文獻(xiàn)【12】第10節(jié))，并以此來評(píng)估高速錐化算法的誤差。例如，(46)和(47)式描述的錐化算法可以由此類仿真機(jī)演示出來，在承受7.6均方根寬帶隨機(jī)線性輸入振動(dòng)(從201000Hz，保持0.04/Hz的密度，然后在為2000Hz以對(duì)數(shù)方式減小到0.01/Hz的密度)的情況下以2kHZ重復(fù)速率運(yùn)行時(shí)，仿真機(jī)的誤差速率為0.00037度/小時(shí)(degh)。由于選擇了下述典型傳感器組合安裝特性作為仿真機(jī)輸入?yún)?shù)的緣故，線性振動(dòng)使傳感器組臺(tái)產(chǎn)生0.0003弧度(rad)多軸角振蕩，相應(yīng)的錐化速率為9.9度/小時(shí)(degh)：50HZ線性振動(dòng)模型無(wú)阻尼自然頻率、0.125線性震動(dòng)模型