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

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

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

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

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

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

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

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

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

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

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

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

12、考位置的垂直向上方向平行，N系用于將加速度積分為速度并定義 E系內(nèi)當?shù)卮怪苯欠较颍?. L系是當?shù)厮阶鶚讼?，與 N系平行，但Z軸垂直向下，而X和Y軸沿N系的Y和X軸。L系用 于描述捷聯(lián)傳感器座標系方向的基準；4. B系是捷聯(lián)慣性傳感器座標系 （機體系），其軸與標準右手正交傳感器輸入軸相平行；5.I系是一個非旋轉(zhuǎn)慣性座標系，用作角旋轉(zhuǎn)測量基準。為I系選擇的特殊方向在其方向與 解析運算有關(guān)的章節(jié)中進行討論。3速度修正算法本節(jié)著重推導(dǎo)用參考文獻 【1】的方程（16）和（18）積分求解參考文獻 【1】方程（20）的算法, 比力變換項的算法以及將參考文獻 1方程（14）和（15）得出的角速率用于互補（

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

14、地曲面(參見參考文獻【8】中5.3節(jié)有關(guān)閉環(huán)形的表述)。/ZN是EN的垂直分量。?ZN 的值的大小取決于所采用的N系的類型，如漂移方位或自由方位。設(shè)計來確保對于所有大地定位來說醫(yī)是非奇異的(見參考文獻【8】4.6節(jié)和文獻【10】第88 89頁)。UZN是一 個向上沿地垂線(N系的Z軸)發(fā)散的單元矢量。方程 采用的是方向余弦矩陣變換比力，而不是它的替代形式一參考文獻1方程(17)，四元數(shù)變換法，如適用于B系高度以高度四元數(shù)法形式計算那種情況下的四元數(shù)變換法?；谒脑獢?shù)比力變換的速度積分算法可以通過擴展這里提供的結(jié)果推導(dǎo)出來。數(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ù)字速度積分算法修正速率計算機周期指數(shù)。如果要把垂直信道重力/離散穩(wěn)定性結(jié)合在一起，那就須給方程(4)增加修正運算.方程 代表垂直速度控制函數(shù)(見參考文獻【8 4.4.1節(jié)和【10】第102103頁)。方程(5)用公式表示用于求算重力/互補速度增量-vg/corm的數(shù)字算法，而方程(6)用 公式表示用于求算積分變換比力增量.-

16、：vSF的數(shù)字算法。m3.1重力/互補速度增量方程(5) gN項是位置定位函數(shù)，其水平分量很小。因為在整個數(shù)字算法n周期內(nèi)位置變化很平緩，幅度變化有限(尤其在高度上)，所以，方程(5)的gN可以由n周期內(nèi)的均值近似表示。因為方 程(5)的互補項小(由于其角速率小的緣故)且因為在n周期內(nèi)速度變化平緩，所以，互補量大小也可 近似為m周期各互補量的均值。后者順理成章地構(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是速度積分算法修正時間段垢-二。方程 的N項是用方程 估算的。而gN是由參考文獻【1】方程(19)計算出來的。因為AvG/corm在方程 中被用來去修正VN，使之從m一 1周期的值修正到m周期值，所以VN蟲2未在方程（7）中清晰顯現(xiàn)，必須在從過去的值出發(fā)進行推斷的基礎(chǔ)上近似估算。舉一個線性外推算法的例子如下式：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)出來，必須在從以往值進行推算的基礎(chǔ)上近似估算。例如，線性推算 可表示如下：（r -1/2） rt（）m12（）n（）2-0（9）j式中：n=位置修正計算機周期指數(shù)；j=每個n周內(nèi)m周期數(shù)；r=自從上個n周期（即從tn J以來m周期個數(shù)。3.2積分變換比力增量積分變換比力增量方程（6）的數(shù)字算法必須考慮在tm4到tm計算機循環(huán)周期內(nèi)當?shù)厮絃系的旋轉(zhuǎn)和捷聯(lián)慣傳感器機體 B系的旋轉(zhuǎn)。采用參考文獻【1】第4.1節(jié)描述L系和B系在 計算機修正時刻相對于慣性空間 I的離散方向時，方程（6）可以通過使

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

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

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

22、豹；的近似值：灼I； =CN®IN +En)MN 叫二 +卩乙心詁第 +Fcns(U：N WN)(式中下標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) 啤項是由方程(2)估算的。如第三節(jié)A所述，方程(17)()n#m須在以往值推斷的基礎(chǔ)上近似求岀，例如：()n4,m、()n2(r/j)")n4-()nJ(19)(25)因為方程(17)被用于修正方程 、(13)和

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

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

25、一階近似值忽略(：(t) )2并將正弦 項Sn (t)近似表示為1(假設(shè)所選的周期速度十分之快)，足以使' (t)維持在一 一個小得合情合理(t)的值上，如小于0.05度。借用方程(22)，可將方程(21)簡化為：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)定義了計算方程(11)也 VBl(m 衛(wèi)SFm _的一種方法。建議在恒定B系角速率和比力aSF情況下分析這些方程，對于它們而言:：=(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)與用于恒定角速率/比力計算的方程(29)進行比較，便可以發(fā):m Vm取代了整數(shù)項。2對于在tm4到tm時間間隔內(nèi)恒定角速率/比力是一個合乎情理的近似值的那種情況，方1程(29)更為適用，因為整數(shù)項(及其伴生高速算法)被 m vm所取代，該值在每一個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)的形式，這被認為是可取的。此算法可以通過下述方法合并而成，首先注意到：一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)等式右邊一個項可得：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：丄表達式中被積式的替換式。用方程(35)代替方程(26)求被積函數(shù)可得到下述等量式：BIr 4、11 tmbbVsf：二vm =(：m vm) ：(： (t) 8sf ' v(t) B)dt( 36

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

31、動處于與另一軸同頻同相的狀態(tài)(然后沿第三個軸方向產(chǎn)生得到校準的恒定比力)。這跟水平用單槳通過起伏搖動劃船前進的原 理是一樣的(誠如前面定義的“劃槳”這一原始術(shù)語所表述的)。必須指出，即使方程(26)的積分項在恒定角速率/比力亦即非劃槳狀態(tài)下含有大基值，該積分項亦被稱之為劃槳。1方程(36)的 Cm vm)項在這里被當作速度旋轉(zhuǎn)補償項。速度旋轉(zhuǎn)被用來表示將一旋轉(zhuǎn)2補償項反饋給速度速率方程(這與第四節(jié)所討論的反饋給位置速率方程的位置旋轉(zhuǎn)補償項 相反)。根據(jù)這些說明，通過對方程(26)和(36)進行比較，識別出方程(26)的積分項是代表劃槳效應(yīng)補償和速度旋轉(zhuǎn)補償這二者的合成，用后一術(shù)語可將方程(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)補償項，而Avsculm是劃槳效應(yī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來自方程(38)的劃槳效應(yīng)項，而式中心Vrot/scul是復(fù)合劃槳效應(yīng)和速度旋轉(zhuǎn) m補償項。方程(37-39)與方程

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

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

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

36、(u., a$F)si n：(t)dt 叫 u. (u., aSF)(Vcos (t)dt(44)第3.2.2節(jié)的專門術(shù)語現(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àn)采用恒定角速率條件， 在此條件下.方程(42).為恒定值。然后：(t) = (t tm4), 二co n st(47)用求算：-m的方程(45),將方程(47)代入(46),可以得到在恒定B系角速率情況下的積分項估算為:/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的精確解。現(xiàn)在我們可以在同樣的條件下對方程(49)與(37)進行比較，以識別精確速度旋轉(zhuǎn)補償項。在恒定角速率/比力條件B下，方程(37)的劃槳項變?yōu)榱?見第222節(jié)討論)，且ZsF：1可由下式給出：BI( m 1)丄VsFmVm - Wrotm( 50)如果對方程(49)和(50)進行比較，根據(jù)其定義，務(wù)必明白這一點，即精確速度旋轉(zhuǎn)補償項Avr°tm可以表示為如下式：也Vetm(1 -COS： m) ,、1 - sin m、,、(冷叫)+ (1_)%工(F Vm)«m%2 ：m(

39、51)方程(51)的三角系數(shù)可以從泰勒系列公式中計算出來: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)補償項厶vrot求解的一個替代算法，它在恒定rot m7B速率/比力條件下為(37)式的AVsF：衛(wèi)產(chǎn)生一個精確解。反之，(39)式的也Vrotm算法只準確到一階。須注意，當 «m采用一階形式時，求解AVrotm時，用(52)式和(51)式求得的會降到(39)式也Vrotm的形式(即像它理應(yīng)呈現(xiàn)的形式那樣 )。積分比力和劃槳增量。這一部

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

41、scul (t)t C ( ) a；Fv( )rB)d(53)2勺丄現(xiàn)在，我們定義tmj到tm時間間隔內(nèi)的下一個I循環(huá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)當V =°時，E =tm厶Vscuh 二 “Vscuii J、訓1 t B BVscui (t)t c (t) aSF V(t) B)dt2勺丄'Vscuii 八

42、 Vscui (t|)( 56)ulm = Zscu、(ti 二 tm)，當 t = tm_,也制=0式中i是高速計算機循環(huán)次數(shù)。(54)-(56)式構(gòu)成以丨為計算機循環(huán)速率計算.-：Vscu劃槳效 應(yīng)項和Vm (作為整個tm到tm時間間隔內(nèi)Vscui和V的變化累加求和)的數(shù)字遞歸算法的 結(jié)構(gòu)框架。它仍然起決定(56)式：Vscui積分項的數(shù)字等量的作用。我們開始通過把:-(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)式積分項的數(shù)字算法是在對到tl時間間隔內(nèi)B系角速率/比力曲線關(guān)系變化過程進行假設(shè)的基礎(chǔ)上推導(dǎo)出來的。目前已發(fā)表的關(guān)于選擇角速率/比力時間曲線關(guān)系應(yīng)用于劃槳算法設(shè)計方面的論文很少，不像錐化算法方面的論文那么多。從原則上講，用于錐化算法的種種方法也可以用于劃槳，包括劃槳式運動的優(yōu)化法(參見本文上篇第4.1.1節(jié))。就此論文而言，我們提供了一個基于ty到tl時間間隔內(nèi)一般線性變化角速率/比力的例子:罷：A B(t tj,aSBF :-C D(t -tid)(58)式中：A, B, C, D是常數(shù)矢量。(57)式積分項算法可以通過下述方法推導(dǎo)出來，即先用(58)式取代(57)式

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

45、器增量進行取樣，測量須在t|和t|時間點處進行，與t|跨接，t|與tl跨接(或t|工與tl整體相接)。另一種可替代的方法是，傳感器取樣可以在ti到tl時間間隔內(nèi)進行，在每個I循環(huán)為(58)式線性斜坡模型取兩個樣，為拋物線模型取三個樣等等。對于以l循環(huán)速率采的傳感器的樣而言，后一過程(詳見參考文獻【8】第72222節(jié))的結(jié)果表明，對于(58)式線 性斜坡模型而言，與(54 57)式等量的算法可以由下式給出：.,_：» =本文上篇(46)式的積分角速率傳感器輸岀(59)t|vldv|-t| 丄V=V4+V| , Vm =V| 魚=垢),當 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 )，當 t tm -1 Vscu |_ 0 ( 61)式中：厶V二來自加速度儀的積分比力輸出增量累加求和；dv =微分積分比力增量，即來自捷聯(lián)慣加速度計的脈沖輸出的分析表述，aSBFdt表示。求解'vs叫的(61)式被劃歸為二階算法，這是因為它包括現(xiàn)有的和過去的l循環(huán)'，'v1的乘積。、：vscul|中的| , | -1循環(huán)匯,V乘積項，亦即一項源于tl到屯時間間隔內(nèi)線| 6性斜波角速率和比力的近似值。如

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

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

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

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

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

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

53、其結(jié)果是（70）式n可以被求出來，作為本文上篇（10）式旋轉(zhuǎn)矢量速率表達式的積分簡化形式，該式中各叉積項被忽略不計：<.tn Nn、t ENdt（ 71）tn 1_（71）式n積分的離散數(shù)字算法可以這樣建立起來，即先將 （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是計算機n循環(huán)修正周期爲-爲。（71）式（人/2項是所有的位置函數(shù)，這些位 置函數(shù)尚未得到修正。因此，要算出各（）nj/2項的值，必須使用一個基于前述參數(shù)計算值的近似推算公式。例如，使用線性推算公式算出的（）最后兩個值將會是：1 31（）n/2 （）n 4 匚（）n 4 - （）n _2（） n匚（）n_2（ 74）2 22計算源自（67）式積分的（66）式和（73）式的厶只補項的方法取決于在位置修正中是否采用典 型的梯形積分法還是采用更為精確的高解積分法