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

1、6.6 捷聯(lián)慣導(dǎo)系統(tǒng)的初始對(duì)準(zhǔn)捷聯(lián)慣導(dǎo)系統(tǒng)的初始對(duì)準(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)中的慣性敏感元件測(cè)量的比力和角速率通過計(jì)算機(jī)實(shí)性敏感元件測(cè)量的比力和角速率通過計(jì)算機(jī)實(shí)時(shí)地計(jì)算出各種導(dǎo)航參數(shù)。時(shí)地計(jì)算出各種導(dǎo)航參數(shù)。 對(duì)于捷聯(lián)慣導(dǎo)系統(tǒng)，由于捷聯(lián)矩陣對(duì)于捷聯(lián)慣導(dǎo)系統(tǒng)，由于捷聯(lián)矩陣T起到起到了平臺(tái)的作用，因此導(dǎo)航工作一開始就需要獲了平臺(tái)的作用，因此導(dǎo)航工作一開始就需要獲得捷聯(lián)矩陣得捷聯(lián)矩陣T的初始值，以便完

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

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

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

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

6、eieieieieieCTTTLLTTTTTTTLTLTLTLTLTL 由于由于 已經(jīng)計(jì)算出來了，所以根據(jù)已經(jīng)計(jì)算出來了，所以根據(jù)上式可以計(jì)算出上式可以計(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， 可用加速度計(jì)的輸出可用加速度計(jì)的輸出 來近似代替，來近似代替， 可由陀螺的可由陀螺的輸出輸出 來近似代替。公式中對(duì)準(zhǔn)點(diǎn)來近似代替。公式中對(duì)準(zhǔn)點(diǎn)的緯度的緯度L和重力加速度和重力加速度g的精確值可作為已知的精確值可作為已知量，量， 也是已知量。也是已知量。由慣導(dǎo)基本方程和靜基座條件由慣導(dǎo)基本方程和靜基座條件,可得可得 f=-g因此

8、有因此有bbbxyzgggbbbxyzfffbbbiexieyiezbbbibxibyibziebzbzbybybxbxfgfgfg通過上面的計(jì)算，可以計(jì)算出初始捷聯(lián)矩陣的通過上面的計(jì)算，可以計(jì)算出初始捷聯(lián)矩陣的近似值，也就完成了解析式粗對(duì)準(zhǔn)。近似的原近似值，也就完成了解析式粗對(duì)準(zhǔn)。近似的原因主要是陀螺和加速度計(jì)有測(cè)量誤差和漂移、因主要是陀螺和加速度計(jì)有測(cè)量誤差和漂移、載體有擾動(dòng)運(yùn)動(dòng)。載體有擾動(dòng)運(yùn)動(dòng)。經(jīng)過解析式初對(duì)準(zhǔn)，實(shí)際上我們得到的矩陣是經(jīng)過解析式初對(duì)準(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)初始對(duì)準(zhǔn)誤差傳播v與平臺(tái)式慣導(dǎo)系統(tǒng)的分析方法類似，列出誤與平臺(tái)式慣導(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é)模型 通過對(duì)比歐拉角法、四元數(shù)法和方向余通過對(duì)比

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

11、計(jì)算捷聯(lián)矩陣根據(jù)下式計(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ù)為為將四元數(shù)將四元數(shù)除以它的

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

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

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

15、 1()bbbbpppbibipibieepT 十一、姿態(tài)角的計(jì)算十一、姿態(tài)角的計(jì)算捷聯(lián)矩陣又叫做姿態(tài)矩陣，它是姿態(tài)角捷聯(lián)矩陣又叫做姿態(tài)矩陣，它是姿態(tài)角 的函數(shù)，所以根據(jù)捷聯(lián)矩陣的元的函數(shù)，所以根據(jù)捷聯(lián)矩陣的元素，可以求出姿態(tài)角。計(jì)算過程如下素，可以求出姿態(tài)角。計(jì)算過程如下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)時(shí)當(dāng)時(shí)當(dāng)時(shí)220220220360001800GGTTTG主G主G主主G主 當(dāng)且0時(shí)當(dāng)且時(shí)當(dāng)時(shí)G當(dāng)確定當(dāng)確定 后，可以確定飛行器的航向角后，可以確定飛行器的航向角G00360360360當(dāng)當(dāng)十二、位置計(jì)算十二、位置計(jì)算 位置矩陣位置矩陣 可以表示為經(jīng)緯度和可以表示為經(jīng)緯度和游移方位角的函數(shù)，即游移方位角的函數(shù)，即peCLLLLLLLLLCpesinsincoscoscoscoscossinsincoscossin

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

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

19、，另一類是通過計(jì)算時(shí)給定的初始條件，另一類是通過計(jì)算獲得的初始數(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，初始速度，對(duì)于從靜止?fàn)顟B(tài)開始，初始速度，對(duì)于從靜止?fàn)顟B(tài)開始起飛的情況可取起飛的情況可取3，初始游移方位角，初始游移方位角通常將初始游移方位角選為通常將初始游移方位角選為 。000Lh、 、0000 xyzVVV00二、初始數(shù)據(jù)的計(jì)算二、初

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

21、23222222012333222201231qqqqTqqqqTqqqqTqqqq求解上述方程可得如下的絕對(duì)值方程求解上述方程可得如下的絕對(duì)值方程1112233112qTTT2112233112qTTT3112233112qTTT2220123112qqqq為了確定為了確定 的符號(hào)，可先選的符號(hào)，可先選 為正，為正，根據(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ù)時(shí)，可以看出為負(fù)時(shí)，可以看出 均改變符號(hào)。均改變符號(hào)。 0q123qqq、 、而當(dāng)而當(dāng) 全改變符號(hào)時(shí)，矩陣全改變符號(hào)時(shí)，矩陣T保持保持不變不變(對(duì)角線上的元素，非對(duì)角線上的元素對(duì)角線上的元素，非對(duì)角線上的元素中各項(xiàng)的乘積都不