捷聯(lián)式慣性導(dǎo)航積分算法設(shè)計(jì)-速度位置計(jì)算

1、捷聯(lián)慣導(dǎo)積分算法設(shè)計(jì)下篇：速度和位置算法Paul G. SavageStrapdow n Associates, I nc., Maple Plai n, Minn esota 55359摘要：本論文分上下兩篇，用于給現(xiàn)代捷聯(lián)慣導(dǎo)系統(tǒng)的主要軟件算法設(shè)計(jì)提供一個(gè)嚴(yán)密 的綜合方法：將角速率積分成姿態(tài)角， 將加速度變換或積分成速度以及將速度積分成位置。 該算法是用兩速修正法構(gòu)成的，而兩速修正法是具有一定創(chuàng)新程度的新穎算法，是為姿態(tài)修正而開(kāi)發(fā)出來(lái)的，在姿態(tài)修正中，以中速運(yùn)用精密解析方程去校正積分參數(shù)（姿態(tài)、速度或位置），其輸入是由在參數(shù)修正（姿態(tài)錐化修正、速度劃槳修正以及高分辨率位置螺旋 修正）時(shí)間間

2、隔內(nèi)計(jì)算運(yùn)動(dòng)角速度和加速度的高速算法提供的。該設(shè)計(jì)方法考慮了通過(guò)捷 聯(lián)系統(tǒng)慣性傳感器對(duì)角速度或比力加速度所進(jìn)行的測(cè)量以及用于姿態(tài)基準(zhǔn)和矢量速度積 分的導(dǎo)航系旋轉(zhuǎn)問(wèn)題。本論文上篇定義了捷聯(lián)慣導(dǎo)積分函數(shù)的總體設(shè)計(jì)要求，并開(kāi)發(fā)出了用于姿態(tài)修正算法的方向余弦法和四元數(shù)法；下篇著重討論速度和位置積分算法的設(shè)計(jì)。盡管上下兩篇討論中常常涉及到基本的慣性導(dǎo)航概念，然而本論文提供的材料都假定是為那些熟悉慣性導(dǎo)航的人使用的。專(zhuān)門(mén)用語(yǔ)：a,a,A =任意坐標(biāo)系；asF=定義為由施加的非重力產(chǎn)生的相對(duì)于非旋轉(zhuǎn)慣性空間的加速度比力，用加速度計(jì)測(cè)得；CA2=將矢量從 A坐標(biāo)系投影到 A坐標(biāo)系的方向余弦矩陣；I=單位矩陣；

3、VA=列向量，它的各項(xiàng)元素等于矢量V在坐標(biāo)系A(chǔ)的各軸上的投影Aa（V ）=向量V的反對(duì)稱(chēng)（或交叉積）形式，代表如下矩陣：0-Vza VyaVZA0_VXAVxa0 j其中：Vxa，Vya，Vza是V的分量，（V ）與人系矢量的矩陣乘積等于 V與該矢量的叉積；灼珂2二A坐標(biāo)系相對(duì)于 A坐標(biāo)系的角速率，當(dāng) A為慣性系（I系）時(shí)，鉀2是由安裝在A坐標(biāo)系上的角速率傳感器所測(cè)到的角速率。1導(dǎo)論INS計(jì)算機(jī)提供數(shù)據(jù)的加速度計(jì)（捷聯(lián)）到INS底盤(pán)構(gòu)架上，這與原 INS技術(shù)形成對(duì)比。INS計(jì)算機(jī)中進(jìn)行的主要軟件函數(shù)運(yùn)算是將敏感的角速率積分成姿態(tài)，將加速度計(jì)敏感到的比力加速度變捷聯(lián)慣導(dǎo)系統(tǒng) （INS） 一般是由

4、慣性角速率傳感器和給 構(gòu)成的一個(gè)正交三軸系統(tǒng)組成的。慣性傳感器直接安裝 來(lái)采用主動(dòng)多軸方向架隔離安裝組合防止傳感器旋轉(zhuǎn)的 換成導(dǎo)航座標(biāo)系，將軟件模型重力與變換比力相加，計(jì)算出整個(gè)加速度，并將整個(gè)加速度 雙積分成矢量速度和位置。INS軟件設(shè)計(jì)過(guò)程中的關(guān)鍵因素是開(kāi)發(fā)出能在有動(dòng)態(tài)角速率, 比力加速度輸入的情況下完美無(wú)缺地進(jìn)行姿態(tài)、 矢量速度以及位置數(shù)字積分函數(shù)運(yùn)算的重 復(fù)數(shù)字算法。如上篇（參考文獻(xiàn)【1】）中討論的那樣，大多數(shù)現(xiàn)代捷聯(lián)INS采用一個(gè)基于雙速算法的姿態(tài)修正算法： 高階修正算法是用來(lái)自高速算法的輸入以中等重復(fù)速率進(jìn)行的。中速例行程序可以由精確的封閉型姿態(tài)修正運(yùn)算表示。 高速算法的設(shè)計(jì)目的在

5、于準(zhǔn)確計(jì)算出在能校 正成系統(tǒng)姿態(tài)變化 （ 傳統(tǒng)上稱(chēng)為錐化 ） 的各個(gè)中速算法修正之間的多軸高頻角運(yùn)動(dòng)。 原來(lái)設(shè) 想成為簡(jiǎn)單一階算法的現(xiàn)今高速姿態(tài)算法利用了現(xiàn)代計(jì)算機(jī)日益增加的吞吐能力， 已變成 以改善精度為目的的高階算法 （參見(jiàn)參考文獻(xiàn)【 1】第57頁(yè)和文獻(xiàn)【 8】的第 7節(jié)）。在姿 態(tài)修正函數(shù)演變成現(xiàn)有形式這期間， 雖然人們?cè)谟糜诒攘铀俣绒D(zhuǎn)換， 矢量速度積分以及 位置積分函數(shù)的配套捷聯(lián)INS算法方面同時(shí)進(jìn)行了開(kāi)發(fā)研究，但有關(guān)此類(lèi)研究成果的發(fā)表 的論文極為少見(jiàn)。本論文的論題恰是這一主題。比力變換算法的關(guān)鍵是處理初始傳感器數(shù)據(jù)， 進(jìn)而計(jì)算出整個(gè)矢量算法修正時(shí)間間隔內(nèi)導(dǎo)航座標(biāo)中的積分比力增量。

6、矢量速度的修正是在以前的矢量速度值的基礎(chǔ)上增加導(dǎo)航系比力增量 （加上用于重力和座標(biāo)系旋轉(zhuǎn)影響的增量 ）來(lái)實(shí)現(xiàn)的。 變換算法的一個(gè)關(guān)鍵作用 是準(zhǔn)確計(jì)算出矢量速度更新時(shí)間周期內(nèi)的姿態(tài)旋轉(zhuǎn) （也就是捷聯(lián)加速度計(jì)的旋轉(zhuǎn) ）。在一些 應(yīng)用方面，這一目的已經(jīng)通過(guò)使用取中心算法得到實(shí)現(xiàn)。在取中心算法中，比力變換用的 姿態(tài)數(shù)據(jù)是在矢量速度修正時(shí)間間隔的中心點(diǎn)得到修正的 （因此，引入了交錯(cuò)姿態(tài)修正 / 矢量速度修正軟件結(jié)構(gòu) ）。變換運(yùn)算由對(duì)矢量速度修正間隔內(nèi)加速度計(jì)比力輸入進(jìn)行積分 運(yùn)算和用矢量修正時(shí)間間隔中心點(diǎn)的姿態(tài)數(shù)據(jù)將積分過(guò)的比力增量變換成導(dǎo)航系的運(yùn)算 這兩種運(yùn)算組成。后一種算法的變種是以?xún)杀妒噶啃拚俣?/p>

7、對(duì)姿態(tài)進(jìn)行修正，以便得出比力增量變化所需速度修正之間的姿態(tài)解。另一個(gè)變種是計(jì)算用于比力變換的姿態(tài)（作為速度修正時(shí)間間隔的起始和末尾姿態(tài)解算均值 ） 。兩速算法還可用于動(dòng)態(tài)環(huán)境下的比力 變換，速度積分 （ 與兩速姿態(tài)積分算法相類(lèi)似。參見(jiàn)參考文獻(xiàn)【5】和【 8】第 7.2節(jié)）。設(shè)計(jì)高速算法的目的是計(jì)算高頻角振蕩和線性振蕩（這些震蕩能糾正傳統(tǒng)上稱(chēng)之為劃槳的系統(tǒng)性速度生成 ） ；而中速算法的目的是進(jìn)行建立在高速算法輸入基礎(chǔ)上的比力變換。一般來(lái)講，比力變換 / 速度積分算法一直沒(méi)有姿態(tài)積分算法在分析上那么復(fù)雜，一般 只局限于在機(jī)動(dòng)條件下的初階精度。 實(shí)際上， 迄今為止尚未見(jiàn)到過(guò)有關(guān)慣性導(dǎo)航位置積分函數(shù)方

8、面的研究專(zhuān)著。據(jù)本文作者所知，現(xiàn)代捷聯(lián)INS通常產(chǎn)生的位置是一個(gè)簡(jiǎn)單的速度不等邊四角形 （梯形）積分， 其積分速率等于或小于速度修正頻率。 對(duì)于需要精確位置數(shù)據(jù) 動(dòng)態(tài)環(huán)境下的應(yīng)用而言，如此粗略的位置積分算法是遠(yuǎn)遠(yuǎn)不夠的。本文為捷聯(lián)慣性導(dǎo)航比力變換、 速度積分以及位置積分算法等的設(shè)計(jì)提供了一個(gè)綜合 方法。所提供的資料是由參考文獻(xiàn)【8】第7.2節(jié)和第 7.3節(jié)濃縮而成的 （參考文獻(xiàn)【 5】材料的擴(kuò)展 ），著重介紹一套嚴(yán)密的分析方程式和便于計(jì)算機(jī)文件處理和驗(yàn)證的準(zhǔn)確的封閉 形式的方程組。 論文中提供的速度和位置算法是采用兩速計(jì)算格式構(gòu)建起來(lái)的； 設(shè)計(jì)中速 算法如50-200HZ的目的是為了使在中速修

9、正時(shí)間間隔內(nèi)，在恒定角速率/比力加速度條件下的運(yùn)算更為精確；中速算法是由如1 4kHz這樣的高速運(yùn)算提供數(shù)據(jù)的，這類(lèi)高速運(yùn)算在恒定角速率 /比力加速度條件下會(huì)產(chǎn)生動(dòng)態(tài)偏差 速度算法產(chǎn)生劃槳和位置算法產(chǎn)生漩 渦（ 作者術(shù)語(yǔ) ） 。本文中還介紹了對(duì)積分修正時(shí)間間隔內(nèi)導(dǎo)航座標(biāo)系旋轉(zhuǎn)進(jìn)行的嚴(yán)密處理 的方法。本論文的層次結(jié)構(gòu)如下： 第2節(jié)定義所用座標(biāo)系。 第3節(jié)將上篇姿態(tài)算法推導(dǎo)式用作公 式化描述兩速比力加速度變換 /速度積分算法的模式。第 3節(jié)把第2節(jié)用作以下面兩種形式 推導(dǎo)位置修正算法的框架，兩種形式是建立在不等邊四角形基礎(chǔ)上的傳統(tǒng)形式和兩速高分 辨率形式。第5節(jié)給出可以參考的推導(dǎo)出的算法一覽表。第

10、6節(jié)先圍繞本文主題進(jìn)行一般性討論，隨后介紹如何選擇使用于特定用途的算法和如何建立這些算法的運(yùn)算速率。第7節(jié)是結(jié)束語(yǔ)。最后，認(rèn)識(shí)到下面這一點(diǎn)是十分重要的，即：兩速方法的初衷是克服早期計(jì)算機(jī)技術(shù) （1965 75）的吞吐量的局限性，這一局限性隨著現(xiàn)代高速計(jì)算機(jī)技術(shù)的不斷快速發(fā)展而迅 速消失。這就促使業(yè)內(nèi)人士最終回到簡(jiǎn)單的單速算法結(jié)構(gòu)問(wèn)題上，正由于單速算法結(jié)構(gòu)較為簡(jiǎn)單才使所有算法能以重復(fù)速率進(jìn)行運(yùn)算，而重復(fù)速率又十分之高，能精確求算出多 軸高頻角速率和比力加速度校正的效果。本論文所介紹的兩速結(jié)構(gòu)和上篇是兼容的，可以壓縮成一個(gè)單速格式，這在下面算法公式推導(dǎo)的一節(jié)中有專(zhuān)門(mén)介紹。2坐標(biāo)系座標(biāo)系是一個(gè)由連續(xù)

11、編號(hào)（或連續(xù)字母編序）的三個(gè)單元矢量定義的分析抽象陣，三個(gè) 矢量單元按右手法則彼此正交。該座標(biāo)系可以想象為一組三條垂直線（軸）通過(guò)一個(gè)公共點(diǎn) （原點(diǎn)），單元矢量由該原點(diǎn)沿各自軸線向外發(fā)散。本論文中的座標(biāo)系原點(diǎn)的實(shí)際位置是任意設(shè)定的。某一特定座標(biāo)系中某矢量的分量 （或投影）等于該矢量與座標(biāo)系各單元矢量的點(diǎn) 積。本文中采用的矢量歸類(lèi)為自由矢量，因而，在對(duì)它們進(jìn)行分析描述時(shí)座標(biāo)系中所取位 置上沒(méi)有孰優(yōu)孰劣的問(wèn)題。坐標(biāo)系定義如下：1. E系是用于位置定位因而定義的地球固聯(lián)座標(biāo)系。它的典型定義是它有一個(gè)軸與地球極軸平行，其他兩軸固連于地球并與赤道平面平行；2. N系是一個(gè)導(dǎo)航座標(biāo)系，Z軸與當(dāng)?shù)氐厍虮砻鎱?/p>

12、考位置的垂直向上方向平行，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)行討論。3速度修正算法本節(jié)著重推導(dǎo)用參考文獻(xiàn) 【1】的方程（16）和（18）積分求解參考文獻(xiàn) 【1】方程（20）的算法, 比力變換項(xiàng)的算法以及將參考文獻(xiàn) 1方程（14）和（15）得出的角速率用于互補(bǔ)（

13、復(fù)合向心） 加速度項(xiàng)（角速率與速度的乘積）：vN -CfcBaBFgN -（ En 2 iE） vN（1）暉=（cN）3E（2）En =Fc（uZNn VN）=uZn式中v是相對(duì)于大地的速度分析定義為E座標(biāo)系中從大地中心到INS的位置矢量的時(shí)間導(dǎo)數(shù)；gp是鉛垂重力（或重力），對(duì)于固定的INS而言,該重力沿鉛垂線沿線。Fc是一個(gè)曲線矩陣(3 x 3)，是一個(gè)其元素(3, i )和(i,3)等于零且其余元素繞對(duì)角線對(duì)稱(chēng)的位置函數(shù)。 對(duì)于球形大地模型而言，F(xiàn)C的其余元素是偏離對(duì)角線的零.且等于對(duì)角線從大地到INS徑向距離的倒數(shù)。對(duì)于扁球狀大地模型而言，其余的FC項(xiàng)代表投影到INS高度的大地表面 的本

14、地曲面(參見(jiàn)參考文獻(xiàn)【8】中5.3節(jié)有關(guān)閉環(huán)形的表述)。/ZN是EN的垂直分量。?ZN 的值的大小取決于所采用的N系的類(lèi)型，如漂移方位或自由方位。設(shè)計(jì)來(lái)確保對(duì)于所有大地定位來(lái)說(shuō)醫(yī)是非奇異的(見(jiàn)參考文獻(xiàn)【8】4.6節(jié)和文獻(xiàn)【10】第88 89頁(yè))。UZN是一 個(gè)向上沿地垂線(N系的Z軸)發(fā)散的單元矢量。方程 采用的是方向余弦矩陣變換比力，而不是它的替代形式一參考文獻(xiàn)1方程(17)，四元數(shù)變換法，如適用于B系高度以高度四元數(shù)法形式計(jì)算那種情況下的四元數(shù)變換法。基于四元數(shù)比力變換的速度積分算法可以通過(guò)擴(kuò)展這里提供的結(jié)果推導(dǎo)出來(lái)。數(shù)字速度積分算法可直接的由方程(1)導(dǎo)出：N NN . L. N/、Vm

15、 - VmCL ”VSFm * =vg/corm( 4)A N*m - N / N 丄 c N、” 卞丄/ l、也VG/cmr= f g P ® En2 齊 EV 丨 d t( 5)Tm LL,mL BVsfm = tC Ba s Fd t(6)fm 1式中n是數(shù)字速度積分算法修正速率計(jì)算機(jī)周期指數(shù)。如果要把垂直信道重力/離散穩(wěn)定性結(jié)合在一起，那就須給方程(4)增加修正運(yùn)算.方程 代表垂直速度控制函數(shù)(見(jiàn)參考文獻(xiàn)【8 4.4.1節(jié)和【10】第102103頁(yè))。方程(5)用公式表示用于求算重力/互補(bǔ)速度增量-vg/corm的數(shù)字算法，而方程(6)用 公式表示用于求算積分變換比力增量.-

16、：vSF的數(shù)字算法。m3.1重力/互補(bǔ)速度增量方程(5) gN項(xiàng)是位置定位函數(shù)，其水平分量很小。因?yàn)樵谡麄€(gè)數(shù)字算法n周期內(nèi)位置變化很平緩，幅度變化有限(尤其在高度上)，所以，方程(5)的gN可以由n周期內(nèi)的均值近似表示。因?yàn)榉?程(5)的互補(bǔ)項(xiàng)小(由于其角速率小的緣故)且因?yàn)樵趎周期內(nèi)速度變化平緩，所以，互補(bǔ)量大小也可 近似為m周期各互補(bǔ)量的均值。后者順理成章地構(gòu)成下述算法的基礎(chǔ)下述算法是用求算 Nn的方程去求算方程(5)的“N/c% :上vG/corm " 9,2一 |2'lEm寶2"zNm1i2UZN'FCm 1/2UZNVm4/ J 'V1/&

17、#39;Tm(7)式中m-1/2是指代tm4和tm之間的參數(shù)值，而Tm是速度積分算法修正時(shí)間段垢-二。方程 的N項(xiàng)是用方程 估算的。而gN是由參考文獻(xiàn)【1】方程(19)計(jì)算出來(lái)的。因?yàn)锳vG/corm在方程 中被用來(lái)去修正VN，使之從m一 1周期的值修正到m周期值，所以VN蟲(chóng)2未在方程（7）中清晰顯現(xiàn)，必須在從過(guò)去的值出發(fā)進(jìn)行推斷的基礎(chǔ)上近似估算。舉一個(gè)線性外推算法的例子如下式：Nn1 廠 Nn3n1nvm J./2 ：" vm J 'vm 二vm_2Vm_J 小 vm_2（8）2 - - 2 2方程（7）中g(shù)PN，-IE，Gn和Fc參數(shù)是位置的函數(shù)，這些函數(shù)隨速度修正而得到

18、修正，其修正速率可能是較慢的n倍周期重復(fù)速率，如比原速率慢五倍。因此，為這些參數(shù)規(guī)定的m-1/2未清晰顯現(xiàn)出來(lái)，必須在從以往值進(jìn)行推算的基礎(chǔ)上近似估算。例如，線性推算 可表示如下：（r -1/2） rt（）m12（）n（）2-0（9）j式中：n=位置修正計(jì)算機(jī)周期指數(shù)；j=每個(gè)n周內(nèi)m周期數(shù)；r=自從上個(gè)n周期（即從tn J以來(lái)m周期個(gè)數(shù)。3.2積分變換比力增量積分變換比力增量方程（6）的數(shù)字算法必須考慮在tm4到tm計(jì)算機(jī)循環(huán)周期內(nèi)當(dāng)?shù)厮絃系的旋轉(zhuǎn)和捷聯(lián)慣傳感器機(jī)體 B系的旋轉(zhuǎn)。采用參考文獻(xiàn)【1】第4.1節(jié)描述L系和B系在 計(jì)算機(jī)修正時(shí)刻相對(duì)于慣性空間 I的離散方向時(shí)，方程（6）可以通過(guò)使

19、用參考文獻(xiàn)【1】方程（3）鏈?zhǔn)揭?guī)則加以擴(kuò)展，見(jiàn)下式：tIIb.、，LmL|(m)L|(n_T)B|(m亠vSFC,CB CBm'tmiL|(n1)B1 (m DB(t)aBFdt(10)或進(jìn)行下一步擴(kuò)展：LlBMsFT 乂壯心(11)Wm瞌5帥tm丄(12)vSf =C：I(m)3；F j 丄心v；Fn一mI (n _1 )mm-(I)曲-1 ) m(13)L萬(wàn)程（11）-（13）容許用于下述一般情況，即由于L系旋轉(zhuǎn)需要對(duì)矩陣CB進(jìn)行修正，修正的循環(huán)速率（系數(shù)n）可以不同于（慢于）由于B系旋轉(zhuǎn)引起的C；修正速率（系數(shù)m）,例如：為了盡量減少計(jì)算機(jī)吞吐量的要求，軟件結(jié)構(gòu)中，n周期L系修正

20、速率的設(shè)定比m周期B 系修正速率慢四倍。然而，如果我們選擇以用于B和L系運(yùn)動(dòng)的恒等速率即：n=m寸C；進(jìn)行修正，方程(11) - (13)仍然有效。必須指出，當(dāng)nm寸，方程(13)仍需要在B系 m循環(huán)修正時(shí)間上對(duì)L系的方向進(jìn)行估算(求算矩陣C："m)的LI )。還必須指出方程(11)是建立于在上述BLl(n 衛(wèi)I(m)系m循環(huán)中采用Cb的基礎(chǔ)上，即C：I(n衛(wèi)矩陣中的BI 。這就意味著當(dāng)B系緊隨方程(11)(m 1)(m 1)變換運(yùn)算之后進(jìn)行旋轉(zhuǎn)，cB將得到修正。該值用于定義求解方程(13)的c：I(m)項(xiàng)的算法，B從而求解比力變換過(guò)程中的本地水平系旋轉(zhuǎn)以及求解方程(12)的機(jī)體系積

21、分比力增量項(xiàng)。3.2.1比力變換過(guò)程中當(dāng)?shù)厮较敌D(zhuǎn)校正因?yàn)橄鄬?duì)于慣性空間來(lái)說(shuō)L系的角速率慢，所以，方程(13)的C,Ll(m)很接近于單位矩陣I。Ll(n_H因此，對(duì)于許多應(yīng)用場(chǎng)合來(lái)說(shuō)，方程(13)的(c'I(m) - I)完全可被忽略，因?yàn)榕c其他加速M(fèi)(n_D.度誤差源相比較，它可以忽略不計(jì)。對(duì)于包含有(C,(m) -I)的高精度應(yīng)用場(chǎng)合來(lái)說(shuō)，參I(nD考文獻(xiàn)【1】方程(49)和(50)的一階形式通常就夠了，由此可以得出:(14)c：I(m)(n Hn -4,mtmILdttn(15)然后用參考文獻(xiàn)【1】中的方程(3)、方程(13)以及第3節(jié)A段慢速改變互補(bǔ)項(xiàng)的假設(shè)，求方程(15)

22、豹；的近似值：灼I； =CN®IN +En)MN 叫二 +卩乙心詁第 +Fcns(U：N WN)(式中下標(biāo)n1、m表示從tn4到tm中途的參數(shù)值。將方程(16)代入方程(15)得：-n4,m 丸 CNrTm * °ZNn 出 UzN rTm + Fcn丄m (Uzn "人尺 4,m )( 17)URm 三;m VNdt( 18)n丄方程(17) 啤項(xiàng)是由方程(2)估算的。如第三節(jié)A所述，方程(17)()n#m須在以往值推斷的基礎(chǔ)上近似求岀，例如：()n4,m、()n2(r/j)")n4-()nJ(19)(25)因?yàn)榉匠?17)被用于修正方程 、(13)和

23、(14)的vN，所以VN的現(xiàn)值尚不能用于估算方程(18) AR/m。因此，必須采用以往值推斷，如象第三節(jié)A所述：曲m二導(dǎo)閔二-vmR當(dāng)r =1此=¥門(mén)二一 vN尅(v牛 vN)1 當(dāng) r1( 20)3.2.2機(jī)體系積分比力增量B方程(11)和(12)的積分項(xiàng).WsF是用類(lèi)似于參考文獻(xiàn) 【1】方程(35)和(36)姿態(tài)修正求解Bi所采用的那種形式計(jì)算出來(lái)的。該算法的推導(dǎo)開(kāi)始是建立在對(duì) Cb®衛(wèi)進(jìn)行一階近似求算的基礎(chǔ)上的一階解被分成兩部分，應(yīng)用于兩速算法：其中：一部分可以以0循環(huán)速率計(jì)算出來(lái)(循環(huán)速率測(cè)量恒定b系角速度和比力的影響)；而高速部分在mi環(huán)范圍內(nèi)，它可測(cè)量b 系角速

24、率/比力的動(dòng)態(tài)變化。之后在恒定的角速率/比力條件下，一階m循環(huán)部分被擴(kuò)展開(kāi)來(lái)進(jìn)行準(zhǔn)確分析。Bb緊隨參考文獻(xiàn)【1】第4.1.1節(jié)的推導(dǎo)，方程(12) AVs；：血被推函數(shù)CB(：mJi項(xiàng)被表示為 下式：BiCI(m 1)BB(t)sn (t)(t)3(t)1 -cos (t)(t)23(t)2(21)式中叮(t)是一個(gè)旋轉(zhuǎn)矢量，定義t大于tmJ時(shí)刻相對(duì)于B(m亠系的B系的一般指向。參考文 獻(xiàn)【1】方程(32)和(33)表明方程(21)的:(t)可以近似地表示為:(t) " : (t)(22)tB：(t)JiBdtm_L(23)式中是一個(gè)積分時(shí)間參數(shù)。與方程(22)相一致的方程(21)的

25、一階近似值忽略(：(t) )2并將正弦 項(xiàng)Sn (t)近似表示為1(假設(shè)所選的周期速度十分之快)，足以使' (t)維持在一 一個(gè)小得合情合理(t)的值上，如小于0.05度。借用方程(22)，可將方程(21)簡(jiǎn)化為：Bi /Cb°T G (t) )(24) 將方程(24)代入方程(12),然后得一階:就身亠:| | . (-. (t) ) Sf dtLm 1Lm 1或者，包括方程(23 )：BI( m 1 )tm - - - a -一tmbaF dtt ： ( ( )t) saF dttm 1i m二Vm t (： (t) asF)dt%m 1t v(t)二 t asFd .，

26、'tm 1Vm =V(tm)(26)方程(26)定義了計(jì)算方程(11)也 VBl(m 衛(wèi)SFm _的一種方法。建議在恒定B系角速率和比力aSF情況下分析這些方程，對(duì)于它們而言:：=(t 一垢 J |B， V(t) =(t - 歸初B B丄ib , aSF = const(27)將方程(27)B:-(t)代入方程(26)V 表示式可得到恒定B系角速率和比力:B|沁FTt mBBt (t-tm)，iBa：F)dttm 1C'iB a：F)： (t-tmJdttm 11 bb-(t -tm)心覽垢)2或用方程(26)和(27)求恒定B系角速率和比力：.B|(m 1)1：VsFm=Vm

27、 2>mvm(28)tB：(t) Jid，tm 1_v(t) = tasFd -，*m J_將用于一般求算的方程1現(xiàn)二者的差別是用1Vm =V(tm)(29)(26)與用于恒定角速率/比力計(jì)算的方程(29)進(jìn)行比較，便可以發(fā):m Vm取代了整數(shù)項(xiàng)。2對(duì)于在tm4到tm時(shí)間間隔內(nèi)恒定角速率/比力是一個(gè)合乎情理的近似值的那種情況，方1程(29)更為適用，因?yàn)檎麛?shù)項(xiàng)(及其伴生高速算法)被 m vm所取代，該值在每一個(gè)m2周期估算一次。方程(26)或(29)的帶根本性的局限性在于其一階是近似值，這阻礙了它們的發(fā)展應(yīng)用，DDB(t)B1 (m _y即方程(24)用于求算曾用于方程(12).沁$/4

28、表示的CB'(m。方程(24)的估算值僅僅應(yīng) 用于求解CB；亠的高頻內(nèi)容，而低頻內(nèi)容仍保持滿足方程 (21)的形式，這被認(rèn)為是可取的。此算法可以通過(guò)下述方法合并而成，首先注意到：一C (t) v(t)=： (t) v(t)心 v(t) - ： (t) v(t)-v(t) :- (t)(30)dt式中:(t)和v(t)的定義與方程(26)中的定義相同。一旦重新組合，方程(30)即變成：. d_：i(t) v(t) (_：i(t) v(t) v(t) : (t)(31)dt略作變形得：. 1 1 :(t) v(t) (t) v(t) (t) v(t)(32)2 2現(xiàn)在用方程(31)代替方程

29、(32)等式右邊一個(gè)項(xiàng)可得：1 d1-:(t) v(t)C (t) v(t) C (t) v(t) v(tp < (t)( 33)2 dt2從方程(26)可知：(t) z：iB， v(t)二 aBF(34)由方程(33)式推導(dǎo)出下述形式：B1 d1BB:(t) asF(： (t) v(t) G (t) asF v(t) ，ib)(35)2 dt2B|方程(35)是方程(26) Msf：丄表達(dá)式中被積式的替換式。用方程(35)代替方程(26)求被積函數(shù)可得到下述等量式：BIr 4、11 tmbbVsf：二vm =(：m vm) ：(： (t) 8sf ' v(t) B)dt( 36

30、)22 tm 1-.B如果現(xiàn)在在恒定角速率/比力條件下對(duì)方程(36)和(29)的項(xiàng)進(jìn)行比較，我們會(huì)發(fā)現(xiàn)，除了方程(36)的積分項(xiàng)外，它們是等量值。通過(guò)置換方程 (27)，會(huì)很容易地驗(yàn)證這一 點(diǎn)，即對(duì)于恒定B系角速率/比力而言，方程(36)的積分項(xiàng)變?yōu)榱??？梢缘贸鼋Y(jié)論：方 程(36)的積分項(xiàng)代表了方程(12) AvsFmmJ1被積函數(shù)中高頻內(nèi)容的積分基值。剩余項(xiàng)1vmG m vm)項(xiàng)則代表了低頻內(nèi)容。2前面定名為劃槳的方程(36)的積分項(xiàng)，測(cè)量并入純恒基值的動(dòng)態(tài)角速率/比力對(duì)vsFm的校正。該校正在經(jīng)典劃槳運(yùn)動(dòng)條件下是最大值，經(jīng)典劃槳運(yùn)動(dòng)定義為角速率/比力為正弦項(xiàng)，在此狀態(tài)下，繞B系某一軸的角運(yùn)

31、動(dòng)處于與另一軸同頻同相的狀態(tài)(然后沿第三個(gè)軸方向產(chǎn)生得到校準(zhǔn)的恒定比力)。這跟水平用單槳通過(guò)起伏搖動(dòng)劃船前進(jìn)的原 理是一樣的(誠(chéng)如前面定義的“劃槳”這一原始術(shù)語(yǔ)所表述的)。必須指出，即使方程(26)的積分項(xiàng)在恒定角速率/比力亦即非劃槳狀態(tài)下含有大基值，該積分項(xiàng)亦被稱(chēng)之為劃槳。1方程(36)的 Cm vm)項(xiàng)在這里被當(dāng)作速度旋轉(zhuǎn)補(bǔ)償項(xiàng)。速度旋轉(zhuǎn)被用來(lái)表示將一旋轉(zhuǎn)2補(bǔ)償項(xiàng)反饋給速度速率方程(這與第四節(jié)所討論的反饋給位置速率方程的位置旋轉(zhuǎn)補(bǔ)償項(xiàng) 相反)。根據(jù)這些說(shuō)明，通過(guò)對(duì)方程(26)和(36)進(jìn)行比較，識(shí)別出方程(26)的積分項(xiàng)是代表劃槳效應(yīng)補(bǔ)償和速度旋轉(zhuǎn)補(bǔ)償這二者的合成，用后一術(shù)語(yǔ)可將方程(36

32、)重新表示為：丸 Bl(m 1)WsFm 二 VmBbasF v(.) - -iB)d.：()=<t Bd.,vG)=aBF ,m 1:m(tm)Vm =V(tm)1%m =2(m Vm)式中也vrotm是速度旋轉(zhuǎn)補(bǔ)償項(xiàng)，而Avsculm是劃槳效應(yīng)補(bǔ)償項(xiàng)。換一種方式，從方程發(fā):.Bl(m Q=vm'一 vrot/sculm(37)(38)(39)(26)出(40)BWot/scul(t) J C ( )asF)d吐m 1rot /scul(tm)式中a (l)和Vm來(lái)自方程(38)的劃槳效應(yīng)項(xiàng)，而式中心Vrot/scul是復(fù)合劃槳效應(yīng)和速度旋轉(zhuǎn) m補(bǔ)償項(xiàng)。方程(37-39)與方程

33、(40)和(41)完全等價(jià)；兩個(gè)方程組只展示岀一階精度。然而，現(xiàn)在方程(32)處于這樣一種形式，它能使我們用一個(gè)擴(kuò)展表示式代替方程(39)速度旋轉(zhuǎn)補(bǔ)償項(xiàng)(該項(xiàng)能使方程(37)正好處于恒定速率/比力狀態(tài)下)。這是一個(gè)重要的擴(kuò)展，因?yàn)橐话愕倪\(yùn)動(dòng)通常都受到低頻角速率和比力補(bǔ)償韻支配(角速率和比力補(bǔ)償在極端機(jī)動(dòng)條件下亦即二階算法誤差不得忽略的條件下幅度很大)。要將方程(40)和(41)擴(kuò)展到精度很高是不可能 的，因?yàn)樾D(zhuǎn)補(bǔ)償效應(yīng)內(nèi)嵌有積 分項(xiàng)，該積分項(xiàng)包含一階劃槳項(xiàng)。下面的各個(gè)代入法除了能為方程(38)推導(dǎo)岀數(shù)字積分算法外，還可以為方程(37)推導(dǎo)岀精確的.-：vro速度補(bǔ)償算法。用同樣的過(guò)程，還能為

34、方程(40)和(41)的也Vrot/sculm推導(dǎo)岀數(shù)字積分算法，如參考文獻(xiàn)【8】第72222節(jié)所示。精確的速度補(bǔ)償 精確速度旋轉(zhuǎn)補(bǔ)償算法定義為這樣一種算法，即當(dāng)該算法計(jì)算方B程(37)Avrot丿寸，該算法在恒定B系角速率/比力條件下，可以為方程(12)的也VsFm提供一個(gè)精確的解。精確速度補(bǔ)償算法則是用在恒定角速率/比力條件下求算(21)式的CB；)m4，并從方程(12)中推導(dǎo)出來(lái)。我們首先考慮更為一般的條件，在此條件下，只有角速度矢量 的方向是恒定的，亦即非錐化環(huán)境，在此環(huán)境下角速率矢量不發(fā)生旋轉(zhuǎn)。從方程(23)可以看出，對(duì)于非錐化角速率而言：ta (t)： (t)(t)u.,，： (t

35、)二 d，u.,(42)tm_L(t)Bb式中是-'ib的模值，而u 是沿著'|B方向的單位矢量，它在 B系中被當(dāng)作是恒定的。B如象參考文獻(xiàn)【1】第4.1.1節(jié)中所討論的，對(duì)于-.IB不發(fā)生旋轉(zhuǎn)的情況而言，(t)等于(t) (雹的積分)。在這樣的約束條件下，用于求解方程(12)的:(t)項(xiàng)的方程(21)與方程(42)給出適合于非錐化角速度條件下的解算式：- n- K -AVtmsin： (t)(u., ) (1 -cos： (t)(u , )2) a；Fdt(43)(49)(49)對(duì)于非錐化角速率和恒定 B系比力而言，方程(43)可以擴(kuò)展成:Bitmtmtm二 tmfsFdt

36、(u., a$F)si n：(t)dt 叫 u. (u., aSF)(Vcos (t)dt(44)第3.2.2節(jié)的專(zhuān)門(mén)術(shù)語(yǔ)現(xiàn)在可用于非錐化速率/恒定比力推導(dǎo)與適用的方程(42)的關(guān)系：f aSFdt = asF (tm -tm=aSFTm ,tm 1:(t):'(t)(49)(49)而：m = ： (tm)是(tm)的模值。將方程(45)代入(44)式可以得到非錐化角速率和恒定比力解如下式：%寶SFm.：mvm：'mTmtm、；m 二(、；m 二 Vm) tmsin : (t)dt m 尹 m(1 cosi (t)dttmA：'m Tmtm1(46 )要估算(46)式的

37、積分項(xiàng)，現(xiàn)采用恒定角速率條件， 在此條件下.方程(42).為恒定值。然后：(t) = (t tm4), 二co n st(47)用求算：-m的方程(45),將方程(47)代入(46),可以得到在恒定B系角速率情況下的積分項(xiàng)估算為:/m sin a(t)dttm丄：'m(1 -COS： m)tmt (1-COS： (t)dt =Tm(1m j_sin : m)am(48)B代入方程(46)得到恒定B系角速率和比力條件下精確表述AvsFja的理想形式:BiA I(m 1)_'VSF=Vmm(1 一 cos-：im)1 sin ： m皿 v ) (1 ，: v ) 2Im 5 丿2

38、(I丿 m I m vm7、二 、二mm：m(49)方程(49)構(gòu)成恒定角速率/比力條件下n_m的精確解?，F(xiàn)在我們可以在同樣的條件下對(duì)方程(49)與(37)進(jìn)行比較，以識(shí)別精確速度旋轉(zhuǎn)補(bǔ)償項(xiàng)。在恒定角速率/比力條件B下，方程(37)的劃槳項(xiàng)變?yōu)榱?見(jiàn)第222節(jié)討論)，且ZsF：1可由下式給出：BI( m 1)丄VsFmVm - Wrotm( 50)如果對(duì)方程(49)和(50)進(jìn)行比較，根據(jù)其定義，務(wù)必明白這一點(diǎn)，即精確速度旋轉(zhuǎn)補(bǔ)償項(xiàng)Avr°tm可以表示為如下式：也Vetm(1 -COS： m) ,、1 - sin m、,、(冷叫)+ (1_)%工(F Vm)«m%2 ：m(

39、51)方程(51)的三角系數(shù)可以從泰勒系列公式中計(jì)算出來(lái):242 ：m(1-C0S： m)1m .m"6-4(52)B系角1Sin : m、1: m .： m2 (1 _ )二:m2: m3!5!7方程(51)與(52)構(gòu)成(39)式速度旋轉(zhuǎn)補(bǔ)償項(xiàng)厶vrot求解的一個(gè)替代算法，它在恒定rot m7B速率/比力條件下為(37)式的AVsF：衛(wèi)產(chǎn)生一個(gè)精確解。反之，(39)式的也Vrotm算法只準(zhǔn)確到一階。須注意，當(dāng) «m采用一階形式時(shí)，求解AVrotm時(shí)，用(52)式和(51)式求得的會(huì)降到(39)式也Vrotm的形式(即像它理應(yīng)呈現(xiàn)的形式那樣 )。積分比力和劃槳增量。這一部

40、分我們著重推導(dǎo)(37)和(38)式中用于計(jì)算Vm和厶VSCuim積分的數(shù)字算法(37)和(38)式的：m項(xiàng)由本文上篇(41)式姿態(tài)算法求得。亦可以用類(lèi)似的過(guò)程推導(dǎo)出(40)和(41)式也Vrot/scul的算法。依照本文上篇第 4.1.1節(jié)用于推導(dǎo)錐 m化算法的相同過(guò)程，我們可以通過(guò)假定'Vsculm是在一般函數(shù)Vscul(t)在t = tm時(shí)刻的值的辦法，推導(dǎo)出AVsculm劃槳算法(如像(38)式那樣)。試設(shè)(38)式也Vscul (t)積分在tm一到tm時(shí)間間隔內(nèi)被分成多個(gè)部分，直到時(shí)刻ty或其后，于是得出：*scul(t)- 'Vscu、'%ul(t)1 t-V

41、scul (t)t C ( ) a；Fv( )rB)d(53)2勺丄現(xiàn)在，我們定義tmj到tm時(shí)間間隔內(nèi)的下一個(gè)I循環(huán)時(shí)間點(diǎn)，以便使(53)式中源自(38) 式的：(.)和v()，包括初始條件在內(nèi)，變成下式：()iBdt，(54)吒I 1：l 二 >11 : m »l(t| 二悅)v( ) = V|LV().v()二 t aBFdt，V 二 V(t JV =V|V，Vm =VI (tI =%)(55)當(dāng)V =°時(shí)，E =tm厶Vscuh 二 “Vscuii J、訓(xùn)1 t B BVscui (t)t c (t) aSF V(t) B)dt2勺丄'Vscuii 八

42、 Vscui (t|)( 56)ulm = Zscu、(ti 二 tm)，當(dāng) t = tm_,也制=0式中i是高速計(jì)算機(jī)循環(huán)次數(shù)。(54)-(56)式構(gòu)成以丨為計(jì)算機(jī)循環(huán)速率計(jì)算.-：Vscu劃槳效 應(yīng)項(xiàng)和Vm (作為整個(gè)tm到tm時(shí)間間隔內(nèi)Vscui和V的變化累加求和)的數(shù)字遞歸算法的 結(jié)構(gòu)框架。它仍然起決定(56)式：Vscui積分項(xiàng)的數(shù)字等量的作用。我們開(kāi)始通過(guò)把:-(t)和(54)式AW和AVi的定義代入§Vscui丨可得到下式：1 1 tiBB"Vscui丨Ci4'Vi - Vi j -i) t (匯(t) asF'V(t)IB )dt( 57)2

43、 2勺丄劃槳方程(57)式積分項(xiàng)的數(shù)字算法是在對(duì)到tl時(shí)間間隔內(nèi)B系角速率/比力曲線關(guān)系變化過(guò)程進(jìn)行假設(shè)的基礎(chǔ)上推導(dǎo)出來(lái)的。目前已發(fā)表的關(guān)于選擇角速率/比力時(shí)間曲線關(guān)系應(yīng)用于劃槳算法設(shè)計(jì)方面的論文很少，不像錐化算法方面的論文那么多。從原則上講，用于錐化算法的種種方法也可以用于劃槳，包括劃槳式運(yùn)動(dòng)的優(yōu)化法(參見(jiàn)本文上篇第4.1.1節(jié))。就此論文而言，我們提供了一個(gè)基于ty到tl時(shí)間間隔內(nèi)一般線性變化角速率/比力的例子:罷：A B(t tj,aSBF :-C D(t -tid)(58)式中：A, B, C, D是常數(shù)矢量。(57)式積分項(xiàng)算法可以通過(guò)下述方法推導(dǎo)出來(lái)，即先用(58)式取代(57)式

44、和a；F ,然后分析計(jì)算整個(gè)t|到t|時(shí)間間隔內(nèi)(57)式的積分項(xiàng)。中間結(jié)果是作為A、B、C和D各常數(shù)矢量的一個(gè)函數(shù)的(57)式積分項(xiàng)公式。然后通過(guò)對(duì)來(lái)自各慣性傳感器的積分角速率和比 力增量進(jìn)行連續(xù)測(cè)量，計(jì)算出每個(gè)tl j到tl時(shí)間間隔的A B、C和D常數(shù)矢量。這步務(wù)必需要進(jìn)行兩次連續(xù)測(cè)量，方能極為不易地為(58)式線性斜坡式模型測(cè)定出四個(gè)常數(shù)矢量A、BC和D(拋物線式模型的特點(diǎn)是它有六個(gè)矢量，而且需要進(jìn)行三次連續(xù)的傳感器測(cè)量才能確定，等等)。其結(jié)果然后代替前面已定義過(guò)的中間結(jié)果中的A、B、C和D,導(dǎo)出與在整個(gè)tl d到t|時(shí)間間隔內(nèi)與(57)式積分項(xiàng)等量的積分算法。如果以I循環(huán)速率對(duì)連續(xù)傳感

45、器增量進(jìn)行取樣，測(cè)量須在t|和t|時(shí)間點(diǎn)處進(jìn)行，與t|跨接，t|與tl跨接(或t|工與tl整體相接)。另一種可替代的方法是，傳感器取樣可以在ti到tl時(shí)間間隔內(nèi)進(jìn)行，在每個(gè)I循環(huán)為(58)式線性斜坡模型取兩個(gè)樣，為拋物線模型取三個(gè)樣等等。對(duì)于以l循環(huán)速率采的傳感器的樣而言，后一過(guò)程(詳見(jiàn)參考文獻(xiàn)【8】第72222節(jié))的結(jié)果表明，對(duì)于(58)式線 性斜坡模型而言，與(54 57)式等量的算法可以由下式給出：.,_：» =本文上篇(46)式的積分角速率傳感器輸岀(59)t|vldv|-t| 丄V=V4+V| , Vm =V| 魚(yú)=垢),當(dāng) t=tm_LV=O(60),A 1 _ 1 A

46、1 A A 1fcullCl4l4)W (V| JV|)2 6 6 Vscu l lVscul 4"Vscul,Vsculm Vscu l l(tl -tm )，當(dāng) t tm -1 Vscu |_ 0 ( 61)式中：厶V二來(lái)自加速度儀的積分比力輸出增量累加求和；dv =微分積分比力增量，即來(lái)自捷聯(lián)慣加速度計(jì)的脈沖輸出的分析表述，aSBFdt表示。求解'vs叫的(61)式被劃歸為二階算法，這是因?yàn)樗ìF(xiàn)有的和過(guò)去的l循環(huán)'，'v1的乘積。、：vscul|中的| , | -1循環(huán)匯,V乘積項(xiàng)，亦即一項(xiàng)源于tl到屯時(shí)間間隔內(nèi)線| 6性斜波角速率和比力的近似值。如

47、果角速率和比力項(xiàng)被近似為拋物線變化的時(shí)間函數(shù)，定會(huì)得出一個(gè)含丨，丨-1和丨-2循環(huán).-：:，-:v乘積的三階算法。如果角速率和比力被近似1為t|到t|區(qū)間的一個(gè)常數(shù)，貝U (61)式 項(xiàng)定會(huì)消失，導(dǎo)致一個(gè)求.也嘰的一階算法。如6 m果角速率和比力緩慢地變化，我們可以將.: vscuL近似為等于零。另一個(gè)可供選擇(且更為精確)的方法是我們可以設(shè)l循環(huán)速率等于m循環(huán)速率，這使與曾在tm時(shí)間點(diǎn)計(jì)算出 的(61)式j(luò).Vscuh相等從(60)式初始條件定義和本文上篇(46)式可以看出:-| J和v必定是零。須指出，通過(guò)增加 m速率使它與l速率相匹配的辦法也能達(dá)到使l和m速率相等的目的。其結(jié)果勢(shì)必是一個(gè)

48、簡(jiǎn)單、高速、高階算法，其軟件結(jié)構(gòu)比兩速算法更簡(jiǎn)便，但要求的 吞吐量更大?，F(xiàn)代計(jì)算機(jī)不斷增強(qiáng)的運(yùn)算速率會(huì)使這一算法更有前途。4位置修正算法在這一節(jié)中，著重就以地球表面上方高度h的形式求算相對(duì)于地球的位置和定義本地水平N系與地球E系之間角方向的CN方向余弦矩陣(從中可以提取出經(jīng)度與緯度)的數(shù)字積 分算法。文中寫(xiě)出兩種算法形式：一個(gè)是基于梯形速率積分的典型算法，另一個(gè)是描述位 置修正期間動(dòng)態(tài)姿態(tài)和速度變化情況的高解算法。高解算法的數(shù)學(xué)模型建立在第3節(jié)兩速矢量速度修正算法后面。上述兩種算法即典型算法和高解算法都可以用本文上篇(21)和(22)式連續(xù)微分方程形式表示，重抄如下：h=u；N vN(62)C

49、N = CN C 'EN )(63)式中：h表示地球表面上方高度。典型算法和高解算法是從h和 CN的一般修正公式中推導(dǎo)出來(lái)的。下面幾節(jié)推導(dǎo)出一般位置修正過(guò)程公式，然后推導(dǎo)出典型的、高解的位置修正計(jì) 算方法。4.1 一般位置修正一般的高度h的修正算法與(62)式積分一樣，是在位置修正 n循環(huán)內(nèi)被公式化的：hn嘰(64)/nNN* =uzn v dttn 1(65)考慮到更高速的數(shù)字算法回路即高度和速度積分的m回路，(65)式可以寫(xiě)成：j嘰=u；n、八 Rmm d(66)三 fm vNdttm J_(67)如果要將垂直信道重力/離散穩(wěn)定也包括進(jìn)去，則(64)式須包括表示高度控制函數(shù)的附加運(yùn)

50、算(見(jiàn)參考文獻(xiàn)【8】第4. 4. 1節(jié)和參考文獻(xiàn)【10】第102103頁(yè))。求解方向余弦矩陣cN的一般修正算法是為了在修正時(shí)間里獲得與(63)式cN表示式在同樣的時(shí)間間隔內(nèi)得到的正規(guī)連續(xù)積分一樣的數(shù)字結(jié)果而設(shè)計(jì)的。該算法是這樣開(kāi)發(fā)岀來(lái) 的，即把數(shù)字修正領(lǐng)域的本地水平導(dǎo)航N系方向關(guān)系函數(shù)(63)式乘以，二想像為它是由每個(gè)修正時(shí)刻相對(duì)于地球(E系)的連續(xù)獨(dú)立方向構(gòu)成的。然后，用本文上篇(3)式方向余弦矩陣積鏈 式法則建立求解 cN的一般性修正算法，如下式：NE(n)= CNE(nJ)cN：J(68)式中:Ne()=在計(jì)算機(jī)修正時(shí)刻tn , N系在旋轉(zhuǎn)地球座標(biāo)系空間(E)中的離散方向；(n)cNEe

51、=在tn二時(shí)刻，n系相對(duì)e系的方向余弦矩陣 cNE ；E(n 1)cNE(n)=在tn時(shí)刻，N系相對(duì)E系的方向余弦矩陣 cn ；cN：(5=方向余弦矩陣，描述 N系相對(duì)于地球(E)，在tn時(shí)刻的指向朝tn時(shí)刻的指 E( n)向發(fā)生的旋轉(zhuǎn)。(68)式矩陣cN® 的正式定義如下：E(n)NetnNeCne：- I t cN(Jdt(69)n 1(69 )式中的N(t)代表在tn 4到tn時(shí)間間隔內(nèi)的任意時(shí)刻，N系的姿態(tài)。Bi按照本文上篇第4.1.1節(jié)關(guān)于cB(mJD求解算法的同樣開(kāi)發(fā)過(guò)程，也可以利用定義的(m)Ne系的姿態(tài)相對(duì)于 Ne系的旋轉(zhuǎn)矢量形式表示E(n)匚(nl.NCnE(3矩陣

52、。利用本文上篇 式，并 E(n)進(jìn)行泰勒展開(kāi)可得:cNET 二sin nFn1 (1 COS 打)F 彈n2nn 丿nsin2 4 _- - n_3!5!(70)(1 -COS n)2!4!6!412412式中n是定義的tn時(shí)刻N(yùn)E(n)系姿態(tài)相對(duì)于tn/時(shí)刻N(yùn)E(g系姿態(tài)的旋轉(zhuǎn)矢量。N系相對(duì)于地球的角速率 EN是比較小的，在典型情況下絕不會(huì)大于一個(gè)或兩個(gè)地412球速率。之所以如此，是因?yàn)閺?tnj到tn的修正循環(huán)相對(duì)而言較短，n的幅度很小。因?yàn)?、En小，而且在典型的tn j到tn修正循環(huán)范圍內(nèi)在緩慢慢改變（由于在這一時(shí)間段內(nèi)矢量 速度和位置發(fā)生的變化?。?，所以N系速率矢量值可以近似為非旋轉(zhuǎn)。

53、其結(jié)果是（70）式n可以被求出來(lái)，作為本文上篇（10）式旋轉(zhuǎn)矢量速率表達(dá)式的積分簡(jiǎn)化形式，該式中各叉積項(xiàng)被忽略不計(jì)：<.tn Nn、t ENdt（ 71）tn 1_（71）式n積分的離散數(shù)字算法可以這樣建立起來(lái)，即先將 （3）式求解的EN近似成表示成 下式：N、NN N xEN，： ' ZNn 1/2 UZN FCnl/2 （UZN V ）（ 72）式中（）n/2是（）在匕和山中間的值。將（72）式代入（71）式并應(yīng)用（67）式定義，然后得出 下式：j1 %衛(wèi)2必人+Fc譏2（uZn V心螢）（73）m呂式中，Tn是計(jì)算機(jī)n循環(huán)修正周期爲(wèi)-爲(wèi)。（71）式（人/2項(xiàng)是所有的位置函數(shù)，這些位 置函數(shù)尚未得到修正。因此，要算出各（）nj/2項(xiàng)的值，必須使用一個(gè)基于前述參數(shù)計(jì)算值的近似推算公式。例如，使用線性推算公式算出的（）最后兩個(gè)值將會(huì)是：1 31（）n/2 （）n 4 匚（）n 4 - （）n _2（） n匚（）n_2（ 74）2 22計(jì)算源自（67）式積分的（66）式和（73）式的厶只補(bǔ)項(xiàng)的方法取決于在位置修正中是否采用典 型的梯形積分法還是采用更為精確的高解積分法