導(dǎo)航原理_捷聯(lián)式導(dǎo)航

1、6.6 捷聯(lián)慣導(dǎo)系統(tǒng)的初始對準(zhǔn)捷聯(lián)慣導(dǎo)系統(tǒng)的初始對準(zhǔn) 慣導(dǎo)系統(tǒng)是一種自主式導(dǎo)航系統(tǒng)。它不需慣導(dǎo)系統(tǒng)是一種自主式導(dǎo)航系統(tǒng)。它不需要任何外部信息，只要給定導(dǎo)航的初始條件要任何外部信息，只要給定導(dǎo)航的初始條件（初始速度、位置等），便可根據(jù)系統(tǒng)中的慣（初始速度、位置等），便可根據(jù)系統(tǒng)中的慣性敏感元件測量的比力和角速率通過計算機(jī)實(shí)性敏感元件測量的比力和角速率通過計算機(jī)實(shí)時地計算出各種導(dǎo)航參數(shù)。時地計算出各種導(dǎo)航參數(shù)。 對于捷聯(lián)慣導(dǎo)系統(tǒng)，由于捷聯(lián)矩陣對于捷聯(lián)慣導(dǎo)系統(tǒng)，由于捷聯(lián)矩陣T起到起到了平臺的作用，因此導(dǎo)航工作一開始就需要獲了平臺的作用，因此導(dǎo)航工作一開始就需要獲得捷聯(lián)矩陣得捷聯(lián)矩陣T的初始值，以便完

2、成導(dǎo)航任務(wù)。的初始值，以便完成導(dǎo)航任務(wù)。捷聯(lián)慣導(dǎo)系統(tǒng)的初始對準(zhǔn)就是確定捷聯(lián)矩陣的捷聯(lián)慣導(dǎo)系統(tǒng)的初始對準(zhǔn)就是確定捷聯(lián)矩陣的初始值。初始值。在靜基座條件下，捷聯(lián)慣導(dǎo)系統(tǒng)的加速度計的在靜基座條件下，捷聯(lián)慣導(dǎo)系統(tǒng)的加速度計的輸入量為輸入量為 ，陀螺的輸入量為地球自轉(zhuǎn)角速，陀螺的輸入量為地球自轉(zhuǎn)角速度度 。因此。因此 和和 就成為初始對準(zhǔn)的基準(zhǔn)。就成為初始對準(zhǔn)的基準(zhǔn)。將陀螺和加速度計的輸出采集到計算機(jī)中，通將陀螺和加速度計的輸出采集到計算機(jī)中，通過計算可以求出捷聯(lián)矩陣過計算可以求出捷聯(lián)矩陣T的初始值。的初始值。bgbie bgbie 靜基座條件下解析式初始對準(zhǔn)的過程靜基座條件下解析式初始對準(zhǔn)的過程初始對

3、準(zhǔn)的已知條件：對準(zhǔn)地點(diǎn)的經(jīng)、緯初始對準(zhǔn)的已知條件：對準(zhǔn)地點(diǎn)的經(jīng)、緯度度矩陣矩陣 可以通過加速度計與陀螺的測量值可以通過加速度計與陀螺的測量值來計算。在進(jìn)行初始對準(zhǔn)時，當(dāng)?shù)氐慕?jīng)度來計算。在進(jìn)行初始對準(zhǔn)時，當(dāng)?shù)氐慕?jīng)度 和緯度和緯度L是已知的，因此重力加速度是已知的，因此重力加速度 和地和地球自轉(zhuǎn)角速度球自轉(zhuǎn)角速度 在地理坐標(biāo)系的分量都是確在地理坐標(biāo)系的分量都是確定的。定的。 tbCbgbie 00txttytzggggg0cossintiexttieieyietiezieLL 由由 和和 構(gòu)成一個新向量構(gòu)成一個新向量 ，即，即gie ieg 根據(jù)地理坐標(biāo)系與機(jī)體坐標(biāo)系之間的轉(zhuǎn)換矩根據(jù)地理坐標(biāo)系與機(jī)

4、體坐標(biāo)系之間的轉(zhuǎn)換矩陣陣 可得下列三個公式：可得下列三個公式：btCbbttbbtietiebbttCCCgg（6.6-1） 根據(jù)以上三個向量等式可以寫出根據(jù)以上三個向量等式可以寫出9個方程，可個方程，可以求出以求出 的的9個元素。其中個元素。其中 、 分別由加速分別由加速度計和陀螺儀測量得到，而度計和陀螺儀測量得到，而 均可通均可通過計算得到。過計算得到。btCbgbie tttbie、 、g 具體計算過程具體計算過程將式將式(6.6-1)兩邊轉(zhuǎn)置，可以得到如下公式：兩邊轉(zhuǎn)置，可以得到如下公式：()()()()b TtTtbbTtTtieiebb TtTtbCCCgg （6.6-2）1112

5、13212223313233tbTTTTCTTTTTT設(shè)設(shè) 根據(jù)公式（根據(jù)公式（6.6-2），可以寫出如下計算公），可以寫出如下計算公式：式：111213212223313233313233()()00b TtTtbbbbxyzCTTTggggTTTTTTgTgTgT gg通過上式，可以求出通過上式，可以求出 。31T3233、TT同樣，通過同樣，通過 ，可以求出，可以求出 ，)()bTtTtieiebC 21T2223、TT111213212223313233213122322333)()0cossincossincossincossinbTtTtieiebbbbiexieyiezieiei

6、eieieieieieCTTTLLTTTTTTTLTLTLTLTLTL 由于由于 已經(jīng)計算出來了，所以根據(jù)已經(jīng)計算出來了，所以根據(jù)上式可以計算出上式可以計算出 31T3233、TT21T2223、TT通過通過 ，可以求出，可以求出)()bTtTtbC 11T1213、TT先求先求 和和b t 由于由于 ，所以，所以ieg 000bbbzyiexbbbbbbiezxieybbbyxiezbbbbiezyieyzbbbbiexziezxbbbbieyxiexyggggggggggggg同理同理00000cos000sincos00tttieieieieggLLgL g根據(jù)根據(jù) ，可以寫出，可以寫出

7、 111213212223313233111213cos00coscoscosbbbbbbbbbbbbiezyieyziexziezxieyxiexyieieieieggggggTTTgLTTTTTTT gLT gLT gL)()bTtTtbC 注意事項(xiàng)：注意事項(xiàng)：1， 可用加速度計的輸出可用加速度計的輸出 來近似代替，來近似代替， 可由陀螺的可由陀螺的輸出輸出 來近似代替。公式中對準(zhǔn)點(diǎn)來近似代替。公式中對準(zhǔn)點(diǎn)的緯度的緯度L和重力加速度和重力加速度g的精確值可作為已知的精確值可作為已知量，量， 也是已知量。也是已知量。由慣導(dǎo)基本方程和靜基座條件由慣導(dǎo)基本方程和靜基座條件,可得可得 f=-g因此

8、有因此有bbbxyzgggbbbxyzfffbbbiexieyiezbbbibxibyibziebzbzbybybxbxfgfgfg通過上面的計算，可以計算出初始捷聯(lián)矩陣的通過上面的計算，可以計算出初始捷聯(lián)矩陣的近似值，也就完成了解析式粗對準(zhǔn)。近似的原近似值，也就完成了解析式粗對準(zhǔn)。近似的原因主要是陀螺和加速度計有測量誤差和漂移、因主要是陀螺和加速度計有測量誤差和漂移、載體有擾動運(yùn)動。載體有擾動運(yùn)動。經(jīng)過解析式初對準(zhǔn)，實(shí)際上我們得到的矩陣是經(jīng)過解析式初對準(zhǔn)，實(shí)際上我們得到的矩陣是坐標(biāo)系坐標(biāo)系 與理想的坐標(biāo)系與理想的坐標(biāo)系 存在著小的誤差存在著小的誤差角。如果能夠求得誤差角，那么就可以通過一角。

9、如果能夠求得誤差角，那么就可以通過一次修正得到精確的初始捷聯(lián)矩陣。次修正得到精確的初始捷聯(lián)矩陣。tbCttv設(shè)坐標(biāo)系設(shè)坐標(biāo)系 偏離坐標(biāo)系偏離坐標(biāo)系t 的角度為的角度為 ，則，則v修正：修正：tTzyxTtxtytxtztytzttIC111tbTtbTtttbtttbCICCCCC)()(捷聯(lián)慣導(dǎo)系統(tǒng)初始對準(zhǔn)誤差傳播v與平臺式慣導(dǎo)系統(tǒng)的分析方法類似，列出誤與平臺式慣導(dǎo)系統(tǒng)的分析方法類似，列出誤差狀態(tài)方程，求解特征根，根據(jù)特征根分析差狀態(tài)方程，求解特征根，根據(jù)特征根分析誤差傳播的規(guī)律。誤差傳播的規(guī)律。6.7 捷聯(lián)慣導(dǎo)系統(tǒng)的數(shù)學(xué)模型捷聯(lián)慣導(dǎo)系統(tǒng)的數(shù)學(xué)模型 通過對比歐拉角法、四元數(shù)法和方向余通過對比

10、歐拉角法、四元數(shù)法和方向余弦法，選擇四元數(shù)法作為捷聯(lián)矩陣即時修正弦法，選擇四元數(shù)法作為捷聯(lián)矩陣即時修正算法。算法。一、四元數(shù)一、四元數(shù)Q的即時修正的即時修正0123bbbQqqiq jq k機(jī)體坐標(biāo)系相對平臺坐標(biāo)系的轉(zhuǎn)動四元數(shù)為機(jī)體坐標(biāo)系相對平臺坐標(biāo)系的轉(zhuǎn)動四元數(shù)為 Q的即時修正可通過解下面的四元數(shù)微分方程的即時修正可通過解下面的四元數(shù)微分方程來實(shí)現(xiàn)：來實(shí)現(xiàn)： 00112233001020bbbpbxpbypbzbbbpbxpbzpbybbbpbypbzpbxbbbpbzpbypbxqqqqqqqq二、捷聯(lián)矩陣的計算二、捷聯(lián)矩陣的計算根據(jù)四元數(shù)即時修正求出的根據(jù)四元數(shù)即時修正求出的 ，根據(jù)下式

11、計算捷聯(lián)矩陣根據(jù)下式計算捷聯(lián)矩陣T：013qqqq、 、 、2222012312031302222212030123230122221302230101232()2()2()2()2()2()qqqqq qq qq qq qTq qq qqqqqq qq qq qq qq qq qqqqq三、四元數(shù)三、四元數(shù)Q的最佳歸一化的最佳歸一化Q0123bbbQqqiq jq kQ22220123qqqq012301232222222201230123ooooobbbbbbqq iq jq kQQqq iq jq kqqqqqqqq設(shè)即時修正獲得的四元數(shù)設(shè)即時修正獲得的四元數(shù)為為將四元數(shù)將四元數(shù)除以它的

12、模除以它的模 得到以漂移誤差最小為指標(biāo)的最佳歸一化得到以漂移誤差最小為指標(biāo)的最佳歸一化四元數(shù)：四元數(shù)：四、比力的坐標(biāo)轉(zhuǎn)換四、比力的坐標(biāo)轉(zhuǎn)換bfpf 加速度計測量的比力加速度計測量的比力 通過捷聯(lián)矩通過捷聯(lián)矩陣陣T可轉(zhuǎn)換為可轉(zhuǎn)換為 ，即，即111213212223313233pbxxpbyypbzzfTTTffTTTffTTTf五、速度五、速度V（即（即 ）的即時修正）的即時修正pV根據(jù)慣導(dǎo)基本方程來進(jìn)行速度的即時修正根據(jù)慣導(dǎo)基本方程來進(jìn)行速度的即時修正00(2)2020(2)(2)20ppppppxxiezepzieyepyppppppyyiezepziexepxppppppzzieyepyi

13、exepxVfVfVfg對于游移方位系統(tǒng)，對于游移方位系統(tǒng)， ，上式可以簡化。，上式可以簡化。0pepzpzpypxVVV六、地速計算六、地速計算 通常將飛行器相對地球的運(yùn)動速度在通常將飛行器相對地球的運(yùn)動速度在水平面的投影稱為地速。可以通過下式計水平面的投影稱為地速?？梢酝ㄟ^下式計算：算：22()()ppxyVVV七、位置矩陣的即時修正七、位置矩陣的即時修正位置矩陣位置矩陣 可以通過求解下列矩可以通過求解下列矩陣微分方程而得：陣微分方程而得：peCC11121311121321222321222331323331323300000pepypepxppepyepxCCCCCCCCCCCCCCC

14、CCC八、位置速率的計算八、位置速率的計算對于游動方位系統(tǒng)，由于對于游動方位系統(tǒng)，由于 ，因此，因此0pepz1111pPypepxxppepyyxpRVVR式中：式中： )21 (11213233eCeCRRexp)21 (11223233eCeCRReyp132312eeC CR九、地球速率的計算九、地球速率的計算地球速率地球速率 經(jīng)矩陣經(jīng)矩陣 轉(zhuǎn)換為轉(zhuǎn)換為 eie peCpie 11121313212223233132333300piexiepieyiepiezieieCCCCCCCCCCCC十、姿態(tài)速率的計算十、姿態(tài)速率的計算姿態(tài)速率姿態(tài)速率 可以通過下式計算：可以通過下式計算：bpb

15、 1()bbbbpppbibipibieepT 十一、姿態(tài)角的計算十一、姿態(tài)角的計算捷聯(lián)矩陣又叫做姿態(tài)矩陣，它是姿態(tài)角捷聯(lián)矩陣又叫做姿態(tài)矩陣，它是姿態(tài)角 的函數(shù)，所以根據(jù)捷聯(lián)矩陣的元的函數(shù)，所以根據(jù)捷聯(lián)矩陣的元素，可以求出姿態(tài)角。計算過程如下素，可以求出姿態(tài)角。計算過程如下G、 、cos cossinsinsincos sinsincoscos sinsincos sinsinsincoscoscossinsincos sincossincossincos cosGGGGGGGGGGT根據(jù)式捷聯(lián)矩陣的元素，可以確定根據(jù)式捷聯(lián)矩陣的元素，可以確定 的主值。的主值。G、 、132131331122

16、2sintantanTTTTT主主G主G、 、求出主值后，可用如下公式求出求出主值后，可用如下公式求出 的真值：的真值：主3303303301800,01800,0TTT主主主主主 當(dāng)時當(dāng)時當(dāng)時220220220360001800GGTTTG主G主G主主G主 當(dāng)且0時當(dāng)且時當(dāng)時G當(dāng)確定當(dāng)確定 后，可以確定飛行器的航向角后，可以確定飛行器的航向角G00360360360當(dāng)當(dāng)十二、位置計算十二、位置計算 位置矩陣位置矩陣 可以表示為經(jīng)緯度和可以表示為經(jīng)緯度和游移方位角的函數(shù)，即游移方位角的函數(shù)，即peCLLLLLLLLLCpesinsincoscoscoscoscossinsincoscossin

17、cossincossinsincossinsinsinsincoscoscossinsinsincos2313313233arctanarctanarcsinCCCCCLf主主31C主0180主0180主主23C主主0360主0180主的真值確定的真值確定 -+-+的真值確定的真值確定+-+-緯度的真值就等緯度的真值就等于緯度的主值。于緯度的主值。十三、高度計算十三、高度計算 純慣性系統(tǒng)的高度通道是不穩(wěn)定的，純慣性系統(tǒng)的高度通道是不穩(wěn)定的，通過引入外部高度信息設(shè)計反饋回路使高通過引入外部高度信息設(shè)計反饋回路使高度通道穩(wěn)定。捷聯(lián)系統(tǒng)的高度通道也是由度通道穩(wěn)定。捷聯(lián)系統(tǒng)的高度通道也是由計算機(jī)計算出

18、來的。計算機(jī)計算出來的。十四、重力加速度十四、重力加速度g的計算的計算 重力加速度不是常數(shù)，隨高度的變化而變重力加速度不是常數(shù)，隨高度的變化而變化。計算公式為化。計算公式為02(1)hggR當(dāng)考慮到地球的橢球度時，重力加速度還與當(dāng)考慮到地球的橢球度時，重力加速度還與緯度有關(guān)。計算公式為緯度有關(guān)。計算公式為2629.78030.051799sin0.94114 10(/)gLh m s6.8 初始條件的給定與初始數(shù)據(jù)的計算初始條件的給定與初始數(shù)據(jù)的計算 為了進(jìn)行捷聯(lián)慣性導(dǎo)航的計算，需為了進(jìn)行捷聯(lián)慣性導(dǎo)航的計算，需要事先知道兩類數(shù)據(jù)：一類是開始計算要事先知道兩類數(shù)據(jù)：一類是開始計算時給定的初始條件

19、，另一類是通過計算時給定的初始條件，另一類是通過計算獲得的初始數(shù)據(jù)。獲得的初始數(shù)據(jù)。 一、初始條件的給定一、初始條件的給定飛行器在從靜止?fàn)顟B(tài)進(jìn)入飛行狀態(tài)飛行器在從靜止?fàn)顟B(tài)進(jìn)入飛行狀態(tài)前，需要給定起飛前的初始條件。初始條前，需要給定起飛前的初始條件。初始條件包括：件包括：1，初始位置，初始位置 分別是起飛地點(diǎn)分別是起飛地點(diǎn)的經(jīng)度和緯度和高度的經(jīng)度和緯度和高度2，初始速度，對于從靜止?fàn)顟B(tài)開始，初始速度，對于從靜止?fàn)顟B(tài)開始起飛的情況可取起飛的情況可取3，初始游移方位角，初始游移方位角通常將初始游移方位角選為通常將初始游移方位角選為 。000Lh、 、0000 xyzVVV00二、初始數(shù)據(jù)的計算二、初

20、始數(shù)據(jù)的計算1，捷聯(lián)矩陣初始值，捷聯(lián)矩陣初始值 的確定的確定0pbTC當(dāng)初始游移方位角當(dāng)初始游移方位角 時，時， 。 可在初始對準(zhǔn)中確定?？稍诔跏紝?zhǔn)中確定。 00ptbbCCtbC2，初始姿態(tài)角的計算，初始姿態(tài)角的計算飛行器的初始姿態(tài)角可根據(jù)初始對準(zhǔn)時飛行器的初始姿態(tài)角可根據(jù)初始對準(zhǔn)時獲得的獲得的 的矩陣元素來進(jìn)行計算。的矩陣元素來進(jìn)行計算。由于初始游移方位角選為由于初始游移方位角選為 ，所以，所以 ptbbTCC00G3，初始四元數(shù)，初始四元數(shù)Q的計算的計算 根據(jù)捷聯(lián)矩陣的初值的對角線元素和四元數(shù)根據(jù)捷聯(lián)矩陣的初值的對角線元素和四元數(shù)約束方程可得約束方程可得2222012311222201

21、23222222012333222201231qqqqTqqqqTqqqqTqqqq求解上述方程可得如下的絕對值方程求解上述方程可得如下的絕對值方程1112233112qTTT2112233112qTTT3112233112qTTT2220123112qqqq為了確定為了確定 的符號，可先選的符號，可先選 為正，為正，根據(jù)捷聯(lián)矩陣與四元數(shù)的關(guān)系，可得根據(jù)捷聯(lián)矩陣與四元數(shù)的關(guān)系，可得0123qqqq、 、 、0q2222012312031302222212030123230122221302230101232()2()2()2()2()2()qqqqq qq qq qq qTq qq qqqqq

22、q qq qq qq qq qq qqqqq0signq 13223()signqsign TT23012301012()2()4q qq qq qq qq q 21331()signqsign T)2()4q qq qq qq qq q 32112()signqsign TT12031203032()2()4q qq qq qq qq q 當(dāng)當(dāng) 為負(fù)時，可以看出為負(fù)時，可以看出 均改變符號。均改變符號。 0q123qqq、 、而當(dāng)而當(dāng) 全改變符號時，矩陣全改變符號時，矩陣T保持保持不變不變(對角線上的元素，非對角線上的元素對角線上的元素，非對角線上的元素中各項(xiàng)的乘積都不