慣性技術(shù)課件9姿態(tài)算法(哈工大版,116全)

1、Lecture 10 - Algorithms for SINS1Strap-down Inertial Navigation System捷聯(lián)慣導(dǎo)系統(tǒng)捷聯(lián)慣導(dǎo)系統(tǒng)介紹及算法（介紹及算法（DCM）Introduction and AlgorithmsLecture 10 - Algorithms for SINS2Outline捷聯(lián)慣導(dǎo)系統(tǒng)概述捷聯(lián)慣導(dǎo)系統(tǒng)概述方向余弦矩陣微分方程的推導(dǎo)方向余弦矩陣微分方程的推導(dǎo)方向余弦矩陣的求解方向余弦矩陣的求解: 角增量算法角增量算法Lecture 10 - Algorithms for SINS31.1*捷聯(lián)慣導(dǎo)系統(tǒng)捷聯(lián)慣導(dǎo)系統(tǒng): 特點(diǎn)特點(diǎn)方位軸方位軸滾動(dòng)

2、軸滾動(dòng)軸俯仰軸俯仰軸q 沒(méi)有物理的平臺(tái)沒(méi)有物理的平臺(tái) q 陀螺和加速度和載體固聯(lián)陀螺和加速度和載體固聯(lián)q 對(duì)陀螺儀的角速度輸出進(jìn)行對(duì)陀螺儀的角速度輸出進(jìn)行積分，獲取載體的姿態(tài)信息積分，獲取載體的姿態(tài)信息.q 加速度計(jì)的輸出需要投影到導(dǎo)航坐標(biāo)系中加速度計(jì)的輸出需要投影到導(dǎo)航坐標(biāo)系中.q 對(duì)導(dǎo)航坐標(biāo)系中的加速度分量進(jìn)行補(bǔ)償并積分，獲取載體對(duì)導(dǎo)航坐標(biāo)系中的加速度分量進(jìn)行補(bǔ)償并積分，獲取載體的速度和位置信息的速度和位置信息.Lecture 10 - Algorithms for SINS4方位軸方位軸滾動(dòng)軸滾動(dòng)軸俯仰軸俯仰軸1.2 姿態(tài)變換姿態(tài)變換S1加速度需要變換加速度需要變換:bzbybxNEAA

3、ACAAA CCq C 包含姿態(tài)信息包含姿態(tài)信息. q 也可利用歐拉角、四元數(shù)等表示也可利用歐拉角、四元數(shù)等表示.Lecture 10 - Algorithms for SINS51.3 SINS的示意框圖的示意框圖計(jì)算載體的計(jì)算載體的姿態(tài)姿態(tài) 方向余弦矩陣方向余弦矩陣的元素的元素計(jì)算機(jī)計(jì)算機(jī)ZYXAAA沿載體軸的沿載體軸的 加速度輸出加速度輸出沿載體軸的沿載體軸的 角速率輸出角速率輸出慣性元件慣性元件ZYXGGG利用方向余弦利用方向余弦矩陣進(jìn)行坐標(biāo)矩陣進(jìn)行坐標(biāo)變換變換沿地理坐標(biāo)沿地理坐標(biāo) 系各軸的加系各軸的加速度速度導(dǎo)航計(jì)算機(jī)導(dǎo)航計(jì)算機(jī)顯示顯示姿態(tài)姿態(tài) 位置位置速度速度對(duì)地理坐對(duì)地理坐標(biāo)系進(jìn)

4、行標(biāo)系進(jìn)行 修正修正數(shù)學(xué)平臺(tái)數(shù)學(xué)平臺(tái)Lecture 10 - Algorithms for SINS6Outline捷聯(lián)慣導(dǎo)系統(tǒng)概述捷聯(lián)慣導(dǎo)系統(tǒng)概述方向余弦矩陣微分方程的推導(dǎo)方向余弦矩陣微分方程的推導(dǎo)方向余弦矩陣的求解方向余弦矩陣的求解: 角增量算法角增量算法Lecture 10 - Algorithms for SINS7方位方位滾動(dòng)滾動(dòng)俯仰俯仰2.1 方向余弦矩陣方向余弦矩陣推導(dǎo)方向余弦矩陣推導(dǎo)方向余弦矩陣(DCM)微分方程微分方程:設(shè)設(shè) S1 為導(dǎo)航坐標(biāo)系，其單為導(dǎo)航坐標(biāo)系，其單位坐標(biāo)矢量為位坐標(biāo)矢量為 i, j 和和 kS1S2 為載體坐標(biāo)系為載體坐標(biāo)系, 其單位坐標(biāo)矢量其單位坐標(biāo)矢量

5、為為 i, j 和和 k S2Lecture 10 - Algorithms for SINS8S1方位方位滾動(dòng)滾動(dòng)俯仰俯仰S22.1 方向余弦矩陣方向余弦矩陣假設(shè)假設(shè) 為載體坐標(biāo)系為載體坐標(biāo)系 S2 相對(duì)于導(dǎo)航坐相對(duì)于導(dǎo)航坐標(biāo)系標(biāo)系 S1 的角速度的角速度, 表示在表示在 S2 中中TZYXC 為為 S2 和和 S1 之間的方向余之間的方向余弦矩陣，即弦矩陣，即: 21SSCAA333231232221131211CCCCCCCCCC其中其中 kkjkikkjjjijkijiiiLecture 10 - Algorithms for SINS92.2 方向余弦矩陣的導(dǎo)數(shù)方向余弦矩陣的導(dǎo)數(shù)kk

6、jkikkjjjijkijiiiCsodtdkkdtdjkdtdikdtdkjdtdjjdtdijdtdkidtdjidtdiiC)(yzzyxkjkjiidtdi001其中其中)(zxikdtdj)(xyjidtdkLecture 10 - Algorithms for SINS10則則2.3 方向余弦矩陣微分方程方向余弦矩陣微分方程)()()()()()()()()(xyzxyzxyzxyzxyzxyzjikikkkjkjijikjkjjjiiikikjiCxyzxyzxyzxyzxyzxyzCCCCCCCCCCCCCCCCCC3231313333322221212323221211111

7、31312000333231232221131211xyxzyzCCCCCCCCC C - 斜斜對(duì)稱(chēng)矩陣對(duì)稱(chēng)矩陣 CC- 關(guān)于方向余弦矩陣的微分方程關(guān)于方向余弦矩陣的微分方程Lecture 10 - Algorithms for SINS11Outline捷聯(lián)慣導(dǎo)系統(tǒng)概述捷聯(lián)慣導(dǎo)系統(tǒng)概述方向余弦矩陣微分方程的推導(dǎo)方向余弦矩陣微分方程的推導(dǎo)方向余弦矩陣的求解方向余弦矩陣的求解: 角增量算法角增量算法Lecture 10 - Algorithms for SINS123.1 方向余弦矩陣微分方程方向余弦矩陣微分方程方向余弦矩陣微分方程方向余弦矩陣微分方程 CCbEbEbEbCC其中其中000 xy

8、xzyzbEb陀螺儀敏感到的是載體的絕陀螺儀敏感到的是載體的絕對(duì)角速度對(duì)角速度 , 因此因此bibbiEbibbEb導(dǎo)航計(jì)算可以得到導(dǎo)航計(jì)算可以得到EiE并且并且EbEiEbEbiECC 因此因此EbEiEbEbibbEbCC 所以所以EbEiEbibEbEbCCC第二項(xiàng)可相對(duì)較慢更新第二項(xiàng)可相對(duì)較慢更新Lecture 10 - Algorithms for SINS13bibEbEbCC解方程：解方程：3.2*畢畢-卡解卡解Tzyxbib記記Tzyxtttbibdt22220zyx000 xyxzyz220000cos1sin)()(ItCttCEbEb則畢則畢-卡解：卡解：bib如果如果

9、的方向不變，則上述解是精確的的方向不變，則上述解是精確的需要解需要解9個(gè)微分方程個(gè)微分方程, 計(jì)算量較大計(jì)算量較大Lecture 10 - Algorithms for SINS143.3*角增量算法角增量算法激光陀螺一般是脈沖輸出激光陀螺一般是脈沖輸出, 每個(gè)脈沖代表每個(gè)脈沖代表一個(gè)單位的角增量一個(gè)單位的角增量.在每個(gè)采樣時(shí)間間隔在每個(gè)采樣時(shí)間間隔t 內(nèi)內(nèi), 角增量輸出可以近似表示成角增量輸出可以近似表示成:220000cos1sin)()(ItCttCEbEbtttbibbibdtTzyx000 xyxzyz22220zyxLecture 10 - Algorithms for SINS1

10、5畢畢-卡解可以改寫(xiě)為卡解可以改寫(xiě)為 :2)()(CSItCttCEbEb3.4 C-S 參數(shù)泰勒近似參數(shù)泰勒近似近似計(jì)算近似計(jì)算:orderCS1234021212421201161206120解的結(jié)果解的結(jié)果:CtCttCEbEb)()(或或CnCnCEbEb)() 1(對(duì)對(duì) C-S 參數(shù)不同程度的近似可以得到不同階次的算法參數(shù)不同程度的近似可以得到不同階次的算法. 其中其中200cos1C00sinSLecture 10 - Algorithms for SINS163.5 1階階 4階階 角增量算法角增量算法nnCnS12340212124212011612061202)() 1(CS

11、InCnCEbEb1階階 :InCnCEbEb)() 1(2階階:221)() 1(InCnCEbEb3階階:22021)61 ()() 1(InCnCEbEb4階階:22020)2421()61 ()() 1(InCnCEbEbLecture 10 - Algorithms for SINS173.7 算例算例某捷聯(lián)慣導(dǎo)系統(tǒng)在某捷聯(lián)慣導(dǎo)系統(tǒng)在 n 時(shí)刻時(shí)刻, 其載體坐標(biāo)系和慣性坐標(biāo)系重合其載體坐標(biāo)系和慣性坐標(biāo)系重合. 然然后從時(shí)刻后從時(shí)刻 n 到時(shí)刻到時(shí)刻 n+1, 沿著載體三個(gè)軸的三個(gè)陀螺儀沿著載體三個(gè)軸的三個(gè)陀螺儀 X, Y, Z 的角增量輸出分別為的角增量輸出分別為 0.002, 0.

12、004 和和 0.006 (rad); 請(qǐng)利用基于請(qǐng)利用基于Peano-Paker 解的一階角增量算法計(jì)算時(shí)刻解的一階角增量算法計(jì)算時(shí)刻 n+1 載體和慣性坐載體和慣性坐標(biāo)系之間的方向余弦矩陣標(biāo)系之間的方向余弦矩陣.3)(InC000 xyxzyz0002. 0004. 0002. 00006. 0004. 0006. 003)() 1(InCnCLecture 10 - Algorithms for SINS183.7 算例算例3)() 1(InCnC0002. 0004. 0002. 00006. 0004. 0006. 001000100013I1002. 0004. 0002. 01006. 0004. 0006. 0110001000