慣性導(dǎo)航原理資料培訓(xùn)課件

3.1常用坐標(biāo)系

慣性導(dǎo)航中所采用的坐標(biāo)系可分為慣性坐標(biāo)系與非慣性坐標(biāo)系兩類，慣性導(dǎo)航區(qū)別于其它類型的導(dǎo)航方案（如無線電導(dǎo)航、天文導(dǎo)航等）的根本不同之處就在于其導(dǎo)航原理是建立在牛頓力學(xué)定律——又稱慣性定律——的基礎(chǔ)上的，“慣性導(dǎo)航”也因此而得名。而牛頓力學(xué)定律是在慣性空間內(nèi)成立的，這就有必要首先引入慣性坐標(biāo)系，作為討論慣導(dǎo)基本原理的坐標(biāo)基準(zhǔn)。對(duì)飛行器進(jìn)行導(dǎo)航的主要目的就是要確定其導(dǎo)航參數(shù)，飛行器的導(dǎo)航參數(shù)就是通過各個(gè)坐標(biāo)系之間的關(guān)系來確定的，這些坐標(biāo)系是區(qū)別于慣性坐標(biāo)系、并根據(jù)導(dǎo)航的需要來選取的。將它們統(tǒng)稱為非慣性坐標(biāo)系，如地球坐標(biāo)系、地理坐標(biāo)系、導(dǎo)航坐標(biāo)系、平臺(tái)坐標(biāo)系及機(jī)體坐標(biāo)系等。8/23/20241慣性導(dǎo)航原理資料在慣性導(dǎo)航中常用的坐標(biāo)系有1.地心慣性坐標(biāo)系——地心慣性坐標(biāo)系不考慮地球繞太陽的公轉(zhuǎn)運(yùn)動(dòng)，地心慣性坐標(biāo)系的原點(diǎn)選在地球的中心，它不參與地球的自轉(zhuǎn)。慣性坐標(biāo)系是慣性敏感元件測量的基準(zhǔn)，在導(dǎo)航計(jì)算時(shí)無需在這個(gè)坐標(biāo)系中分解任何向量，因此慣性坐標(biāo)系的坐標(biāo)軸的定向無關(guān)緊要，但習(xí)慣上將z軸選在沿地軸指向北極的方向上，而x、y軸則在地球的赤道平面內(nèi)，并指向空間的兩顆恒星。8/23/20242慣性導(dǎo)航原理資料2.地球坐標(biāo)系——地球坐標(biāo)系是固連在地球上的坐標(biāo)系，它相對(duì)慣性坐標(biāo)系以地球自轉(zhuǎn)角速率旋轉(zhuǎn)，地球坐標(biāo)系的原點(diǎn)在地球中心，軸與軸重合，在赤道平面內(nèi)，x軸指向格林威治經(jīng)線，y軸指向東經(jīng)90度方向。8/23/20243慣性導(dǎo)航原理資料3.地理坐標(biāo)系——地理坐標(biāo)系是在飛行器上用來表示飛行器所在位置的東向、北向和垂線方向的坐標(biāo)系。地理坐標(biāo)系的原點(diǎn)選在飛行器重心處，x指向東，y指向北，z沿垂線方向指向天（東北天）。8/23/20244慣性導(dǎo)航原理資料4.導(dǎo)航坐標(biāo)系——導(dǎo)航坐標(biāo)系是在導(dǎo)航時(shí)根據(jù)導(dǎo)航系統(tǒng)工作的需要而選取的作為導(dǎo)航基準(zhǔn)的坐標(biāo)系。指北方位系統(tǒng)：導(dǎo)航坐標(biāo)系與地理坐標(biāo)系重合；自由方位系統(tǒng)或游動(dòng)自由方位系統(tǒng)：軸與軸重合，而與及與之間相差一個(gè)自由方位角或游動(dòng)方位角。

8/23/20245慣性導(dǎo)航原理資料5.平臺(tái)坐標(biāo)系——平臺(tái)坐標(biāo)系是用慣導(dǎo)系統(tǒng)來復(fù)現(xiàn)導(dǎo)航坐標(biāo)系時(shí)所獲得的坐標(biāo)系，平臺(tái)坐標(biāo)系的坐標(biāo)原點(diǎn)位于飛行器的重心處。對(duì)于平臺(tái)慣導(dǎo)系統(tǒng)，平臺(tái)坐標(biāo)系是通過平臺(tái)臺(tái)體來實(shí)現(xiàn)的；對(duì)于捷聯(lián)慣導(dǎo)系統(tǒng)，平臺(tái)坐標(biāo)系是通過存儲(chǔ)在計(jì)算機(jī)中的方向余弦矩陣來實(shí)現(xiàn)的。8/23/20246慣性導(dǎo)航原理資料6.機(jī)體坐標(biāo)系——機(jī)體坐標(biāo)系是固連在機(jī)體上的坐標(biāo)系。機(jī)體坐標(biāo)系的坐標(biāo)原點(diǎn)o位于飛行器的重心處，x沿機(jī)體橫軸指向右，y沿機(jī)體縱軸指向前，z垂直于oxy，并沿飛行器的豎軸指向上。8/23/20247慣性導(dǎo)航原理資料3.2四元數(shù)理論8/23/20248慣性導(dǎo)航原理資料四元數(shù)表示四元數(shù)：描述剛體角運(yùn)動(dòng)的數(shù)學(xué)工具(quaternions)針對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)，可彌補(bǔ)歐拉參數(shù)在描述和解算方面的不足。

四元數(shù)的表示由一個(gè)實(shí)單位和三個(gè)虛數(shù)單位i,j,k組成的數(shù)

或者省略1，寫成i,j,k服從如下運(yùn)算公式：

8/23/20249慣性導(dǎo)航原理資料四元數(shù)組成部分i,j,k服從如下運(yùn)算公式

λ稱作標(biāo)量部分，

稱作矢量部分

四元數(shù)的另一種表示法

P泛指矢量部分提示：四元數(shù)與剛體轉(zhuǎn)動(dòng)的關(guān)系8/23/202410慣性導(dǎo)航原理資料四元數(shù)基本性質(zhì)加減法1．四元數(shù)加減法

或簡單表示為

8/23/202411慣性導(dǎo)航原理資料四元數(shù)基本性質(zhì)乘法2．四元數(shù)乘法

或簡單表示為

※關(guān)于相乘符號(hào)※關(guān)于交換律和結(jié)合律8/23/202412慣性導(dǎo)航原理資料四元數(shù)基本性質(zhì)共軛范數(shù)3．共軛四元數(shù)

僅向量部分符號(hào)相反的兩個(gè)四元數(shù)

和

互為共軛

可證明：

4．四元數(shù)的范數(shù)

定義

則稱為規(guī)范化四元數(shù)

8/23/202413慣性導(dǎo)航原理資料四元數(shù)基本性質(zhì)逆除法5．逆四元數(shù)

當(dāng)

時(shí)

6．四元數(shù)的除法

若

則

若

則

不能表示為

（含義不確切）8/23/202414慣性導(dǎo)航原理資料四元數(shù)表示轉(zhuǎn)動(dòng)約定一個(gè)坐標(biāo)系或矢量相對(duì)參考坐標(biāo)系旋轉(zhuǎn)，

轉(zhuǎn)角為θ，

轉(zhuǎn)軸n與參考系各軸間的方向余弦值為cosα、cosβ、cosγ。

則表示該旋轉(zhuǎn)的四元數(shù)可以寫為

為特征四元數(shù)（范數(shù)為1）四元數(shù)既表示了轉(zhuǎn)軸方向，又表示了轉(zhuǎn)角大?。ㄞD(zhuǎn)動(dòng)四元數(shù)）8/23/202415慣性導(dǎo)航原理資料四元數(shù)表示轉(zhuǎn)動(dòng)矢量旋轉(zhuǎn)如果矢量R相對(duì)固定坐標(biāo)系旋轉(zhuǎn)，旋轉(zhuǎn)四元數(shù)為q，轉(zhuǎn)動(dòng)后的矢量為R’，

則這種轉(zhuǎn)動(dòng)關(guān)系可通過四元數(shù)旋轉(zhuǎn)運(yùn)算來實(shí)現(xiàn)含義：矢量R相對(duì)固定坐標(biāo)系產(chǎn)生旋轉(zhuǎn)，轉(zhuǎn)角和轉(zhuǎn)軸由q決定8/23/202416慣性導(dǎo)航原理資料四元數(shù)表示轉(zhuǎn)動(dòng)坐標(biāo)系旋轉(zhuǎn)如果坐標(biāo)系OXYZ發(fā)生q旋轉(zhuǎn)，得到新坐標(biāo)系OX’Y’Z’

一個(gè)相對(duì)原始坐標(biāo)系OXYZ不發(fā)生旋轉(zhuǎn)變換的矢量V

矢量V在新坐標(biāo)系上OX’Y’Z’的投影為

則不變矢量V在兩個(gè)坐標(biāo)系上的投影之間存在如下關(guān)系：

式中

分別稱為矢量V在坐標(biāo)系OXYZ和OX’Y’Z’上的映像8/23/202417慣性導(dǎo)航原理資料四元數(shù)映象圖解8/23/202418慣性導(dǎo)航原理資料四元數(shù)表示轉(zhuǎn)動(dòng)方向余弦將該投影變換式展開，也就是把代入上述投影變換式進(jìn)行四元數(shù)乘法運(yùn)算，整理運(yùn)算結(jié)果可得8/23/202419慣性導(dǎo)航原理資料四元數(shù)表示轉(zhuǎn)動(dòng)方向余弦其中方向余弦矩陣

8/23/202420慣性導(dǎo)航原理資料四元數(shù)表示轉(zhuǎn)動(dòng)旋轉(zhuǎn)合成多次旋轉(zhuǎn)的合成對(duì)于一個(gè)坐標(biāo)系經(jīng)過多次旋轉(zhuǎn)后，新坐標(biāo)系和原始坐標(biāo)系之間的關(guān)系等效于一個(gè)一次轉(zhuǎn)動(dòng)的效果，

相應(yīng)地有合成轉(zhuǎn)動(dòng)四元數(shù)

假定q1、q2

分別是第一次轉(zhuǎn)動(dòng)、第二次轉(zhuǎn)動(dòng)的四元數(shù)

q是合成轉(zhuǎn)動(dòng)的四元數(shù)，那么有如下關(guān)系成立：

上式中q1和q2的轉(zhuǎn)軸方向必須以映象的形式給出。

如果q1和q2的轉(zhuǎn)軸方向都以原始坐標(biāo)系的分量表示，則有

8/23/202421慣性導(dǎo)航原理資料求方向余弦非映象方式1用四元數(shù)旋轉(zhuǎn)變換的方法求取兩個(gè)坐標(biāo)系之間的方向余弦表。

坐標(biāo)系OX’Y’Z’相對(duì)OXYZ三次旋轉(zhuǎn)，以歐拉角ψ、θ、φ的形式給出。

第一轉(zhuǎn)，繞Z軸轉(zhuǎn)ψ角，瞬時(shí)轉(zhuǎn)軸n和k軸重合，則轉(zhuǎn)動(dòng)四元數(shù)為

8/23/202422慣性導(dǎo)航原理資料第二轉(zhuǎn)，繞OX1軸轉(zhuǎn)θ角，瞬時(shí)轉(zhuǎn)軸n的方向表示式為

其轉(zhuǎn)動(dòng)四元數(shù)為

求方向余弦非映象方式28/23/202423慣性導(dǎo)航原理資料求方向余弦非映象方式合成由于q1和q2的瞬時(shí)轉(zhuǎn)軸都是以同一個(gè)坐標(biāo)系的方向余弦來表示，則合成轉(zhuǎn)動(dòng)四元數(shù)q的計(jì)算采用：8/23/202424慣性導(dǎo)航原理資料求方向余弦映象方式1以瞬時(shí)轉(zhuǎn)軸映象形式給出轉(zhuǎn)動(dòng)四元數(shù)的表達(dá)式并求出合成轉(zhuǎn)動(dòng)四元數(shù)

第一次轉(zhuǎn)時(shí)，映象形式的q1和非映象形式的q1是一致的：

8/23/202425慣性導(dǎo)航原理資料求方向余弦映象方式2第二轉(zhuǎn)繞OX1軸轉(zhuǎn)θ角瞬時(shí)轉(zhuǎn)軸n是由OX經(jīng)過第一轉(zhuǎn)轉(zhuǎn)換來的OX軸對(duì)應(yīng)單位矢量i，所以定義n的映象為i則q2的映象表示式為

8/23/202426慣性導(dǎo)航原理資料求方向余弦映象方式3第三轉(zhuǎn)，繞OZ’軸轉(zhuǎn)動(dòng)φ角瞬時(shí)轉(zhuǎn)軸n是由OZ經(jīng)過第一轉(zhuǎn)和第二轉(zhuǎn)轉(zhuǎn)換來的OZ’軸對(duì)應(yīng)單位矢量k，所以定義n的映象為k則q3的映象表示式為8/23/202427慣性導(dǎo)航原理資料求方向余弦映象合成由于q1、q2和q3都是映象形式，所以三次轉(zhuǎn)動(dòng)的合成轉(zhuǎn)動(dòng)四元數(shù)q為據(jù)此可算出對(duì)應(yīng)的方向余弦表

8/23/202428慣性導(dǎo)航原理資料四元數(shù)補(bǔ)充兩種轉(zhuǎn)動(dòng)公式坐標(biāo)系旋轉(zhuǎn)時(shí)，不變矢量V在兩個(gè)坐標(biāo)系上的投影之間存在如下關(guān)系：

在一些資料中，四元數(shù)的轉(zhuǎn)動(dòng)公式也經(jīng)常寫成如下的形式

這個(gè)公式的意義是說，在一個(gè)超復(fù)數(shù)空間中，或者在一個(gè)固定坐標(biāo)系中，矢量VE按著四元數(shù)q所表示的方向和大小轉(zhuǎn)動(dòng)了一個(gè)角度，得到一個(gè)新的矢量VE’8/23/202429慣性導(dǎo)航原理資料四元數(shù)補(bǔ)充計(jì)算上的優(yōu)點(diǎn)四元數(shù)法能得到迅速發(fā)展，是由于飛行器控制與導(dǎo)航的發(fā)展，要求更合理地描述剛體空間運(yùn)動(dòng)，以及便于計(jì)算機(jī)的應(yīng)用。采用方向余弦矩陣描述飛行器運(yùn)動(dòng)時(shí)，要積分矩陣微分方程式：

式中C為動(dòng)坐標(biāo)系轉(zhuǎn)置到定坐標(biāo)系的方向余弦矩陣，Ω為動(dòng)坐標(biāo)系相對(duì)定坐標(biāo)系旋轉(zhuǎn)角速度ω的反對(duì)稱矩陣：

包含9個(gè)一階微分方程式，計(jì)算量比較大

8/23/202430慣性導(dǎo)航原理資料四元數(shù)補(bǔ)充計(jì)算上的優(yōu)點(diǎn)如果采用四元數(shù)法，則是要求解四元數(shù)方程式

q為動(dòng)坐標(biāo)系的轉(zhuǎn)動(dòng)四元數(shù)，ω為動(dòng)坐標(biāo)系相對(duì)定坐標(biāo)系的旋轉(zhuǎn)角速度，也表示為四元數(shù)

按四元數(shù)乘積展開

只要解四個(gè)一階微分方程式組即可8/23/202431慣性導(dǎo)航原理資料3.3視加速度和比力8/23/202432慣性導(dǎo)航原理資料根據(jù)質(zhì)心運(yùn)動(dòng)定理和相對(duì)運(yùn)動(dòng)學(xué)原理，飛行體質(zhì)心運(yùn)動(dòng)的微分方程（在慣性坐標(biāo)系下）為：

式中，---飛行體的質(zhì)量；----推力； ---空氣阻力；---慣性空間飛行時(shí)，導(dǎo)彈質(zhì)心加速度；，---由推力產(chǎn)生的加速度；---由阻力引起的阻力加速度。8/23/202433慣性導(dǎo)航原理資料由上式可得出

飛行體質(zhì)心運(yùn)動(dòng)的微分方程（在彈體坐標(biāo)系下）為：或，式中是動(dòng)點(diǎn)的相對(duì)加速度，將（*）代入上式得

8/23/202434慣性導(dǎo)航原理資料由上式可知，測得的是推力加速度和阻力加速度的矢量和，稱為視加速度，在實(shí)際的測試中由加速度傳感器得到的值是在敏感軸上的分量，實(shí)際的慣性坐標(biāo)系下的加速度可通過上式變換得到，在彈體坐標(biāo)系上動(dòng)點(diǎn)的力為稱為比力，加速度計(jì)實(shí)際是通過比力來測量加速度的。8/23/202435慣性導(dǎo)航原理資料由慣性測量組合測得的視加速度是相對(duì)慣性空間的加速度，在以上的分析計(jì)算中，假設(shè)了地球的曲率半徑很大，自轉(zhuǎn)速度為零，在實(shí)際的導(dǎo)航中，飛行體是在曲率半徑不為零且具有引力場的地球表面上，因此，需要對(duì)慣性空間加速度相對(duì)地球加速度之差，即有害加速度進(jìn)行補(bǔ)償。即飛行體速度和地球自轉(zhuǎn)角速度引起的哥氏加速度，飛行體沿地球表面飛行而產(chǎn)生的向心加速度。8/23/202436慣性導(dǎo)航原理資料3.4捷聯(lián)慣導(dǎo)系統(tǒng)的算法實(shí)現(xiàn)8/23/202437慣性導(dǎo)航原理資料捷聯(lián)慣導(dǎo)基本算法與誤差捷聯(lián)慣導(dǎo)系統(tǒng)算法概述算法：從慣性儀表輸出到導(dǎo)航與控制信息捷聯(lián)慣導(dǎo)算法的基本內(nèi)容：一、系統(tǒng)初始化(Initialization)：1、給定飛行器初始位置、速度等2、數(shù)學(xué)平臺(tái)的初始對(duì)準(zhǔn)3、慣性儀表的校準(zhǔn)二、慣性儀表誤差補(bǔ)償(Compensation)三、姿態(tài)矩陣的計(jì)算四、導(dǎo)航計(jì)算五、導(dǎo)航控制信息的提取8/23/202438慣性導(dǎo)航原理資料姿態(tài)計(jì)算歐拉角微分方程1姿態(tài)矩陣的計(jì)算假設(shè)數(shù)學(xué)坐標(biāo)系模擬地理坐標(biāo)系飛行器姿態(tài)的描述：航向角ψ、俯仰角θ、滾動(dòng)角γ一、歐拉微分方程從地理坐標(biāo)系到載體坐標(biāo)系的旋轉(zhuǎn)順序：Ψ→θ→γ方向余弦矩陣：8/23/202439慣性導(dǎo)航原理資料姿態(tài)計(jì)算歐拉角微分方程2飛行器相對(duì)地理坐標(biāo)系的角速度：8/23/202440慣性導(dǎo)航原理資料姿態(tài)計(jì)算歐拉角微分方程3求解歐拉角速率得注意事項(xiàng)：當(dāng)θ=90度時(shí)，方程出現(xiàn)奇點(diǎn)8/23/202441慣性導(dǎo)航原理資料姿態(tài)計(jì)算矩陣方程精確解1二、方向余弦矩陣微分方程及其解其中由于陀螺儀直接測得的是載體相對(duì)慣性空間的角速度，所以：導(dǎo)航計(jì)算可以得到有因此得8/23/202442慣性導(dǎo)航原理資料姿態(tài)計(jì)算矩陣方程精確解2的精確解（畢卡逼近）：其中方向不變時(shí)的精確解九個(gè)微分方程求解，計(jì)算量大8/23/202443慣性導(dǎo)航原理資料姿態(tài)計(jì)算四元數(shù)精確解1三、四元數(shù)微分方程式及其解由第一章，四元數(shù)微分方程式：對(duì)的處理類似上一節(jié)精確解：其中：8/23/202444慣性導(dǎo)航原理資料姿態(tài)計(jì)算四元數(shù)精確解2其中：8/23/202445慣性導(dǎo)航原理資料姿態(tài)計(jì)算姿態(tài)航向角計(jì)算1四、姿態(tài)和航向角的計(jì)算根據(jù)載體和地理坐標(biāo)系之間的方向余弦矩陣可確定姿態(tài)、航向角姿態(tài)、航向角真值的判斷8/23/202446慣性導(dǎo)航原理資料姿態(tài)計(jì)算姿態(tài)航向角計(jì)算2如利用四元數(shù)微分方程求解，則先利用四元數(shù)求解結(jié)果計(jì)算方向余弦矩陣的元素(1-58)：8/23/202447慣性導(dǎo)航原理資料姿態(tài)實(shí)時(shí)計(jì)算概述姿態(tài)矩陣的實(shí)時(shí)計(jì)算因假定“數(shù)學(xué)平臺(tái)”跟蹤地理坐標(biāo)系，因此所以可得相應(yīng)的姿態(tài)矩陣微分方程（6-12）：或四元數(shù)微分方程：注意事項(xiàng)：1、上述兩個(gè)方程中的角速度表達(dá)式不一樣2、方程第二項(xiàng)較小，計(jì)算時(shí)速度可以低一些8/23/202448慣性導(dǎo)航原理資料增量算法矩陣方程精確解一、角增量算法(AngularIncrementAlgorithm)角增量：陀螺儀數(shù)字脈沖輸出，每個(gè)脈沖代表一個(gè)角增量一個(gè)采樣周期內(nèi)，陀螺輸出脈沖數(shù)對(duì)應(yīng)的角增量為：1、矩陣微分方程(MatrixDifferentialEquation)計(jì)算根據(jù)矩陣微分方程的精確解（6-20），有：（解的第一項(xiàng)）8/23/202449慣性導(dǎo)航原理資料增量算法矩陣方程CS參數(shù)展開合并上式，得其中8/23/202450慣性導(dǎo)航原理資料增量算法矩陣方程1階將前式簡寫為：或離散形式：ΔC按Cn、Sn取不同的近似值，形成相應(yīng)的一階~四階算法一階算法：令可將上述算法解寫成矩陣元素的形式：8/23/202451慣性導(dǎo)航原理資料增量算法矩陣方程1階——一階增量算法8/23/202452慣性導(dǎo)航原理資料增量算法矩陣方程2-4階當(dāng)Cn、Sn取n=2,3,4時(shí)：二階增量算法：三階增量算法：四階增量算法：8/23/202453慣性導(dǎo)航原理資料增量算法四元數(shù)2、四元數(shù)微分方程的計(jì)算：其中，I為單位四元數(shù)，[Δθ]如（6-24）所示：寫成迭代形式：8/23/202454慣性導(dǎo)航原理資料增量算法四元數(shù)設(shè)一階算法：8/23/202455慣性導(dǎo)航原理資料增量算法四元數(shù)或展開為元素形式：8/23/202456慣性導(dǎo)航原理資料增量算法四元數(shù)同理，可得二階算法：三階算法：四階算法：8/23/202457慣性導(dǎo)航原理資料數(shù)值積分1階用一階~四階龍格-庫塔積分矩陣和四元數(shù)微分方程1、一階龍格-庫塔法(Runge-Kutta)一個(gè)矩陣微分方程當(dāng)初始條件已知，其一階龍格-庫塔的解為:方程的解為初始值加上以初始點(diǎn)斜率為斜率的一個(gè)增量斜率K的準(zhǔn)確度不同，解的精確度也不同8/23/202458慣性導(dǎo)航原理資料數(shù)值積分1階矩陣（1）姿態(tài)矩陣微分方程簡化為其一階龍格-庫塔解：展開為元素形式：與一階增量算法一致8/23/202459慣性導(dǎo)航原理資料數(shù)值積分1階四元數(shù)（2）四元數(shù)微分方程或一階龍格-庫塔解8/23/202460慣性導(dǎo)航原理資料數(shù)值積分2階矩陣2、二階龍格-庫塔法對(duì)一階算法適當(dāng)改進(jìn)，使平均斜率更準(zhǔn)確一些二階龍格-庫塔算法的解：（1）矩陣微分方程