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

2、軸滾動軸俯仰軸俯仰軸q 沒有物理的平臺沒有物理的平臺 q 陀螺和加速度和載體固聯陀螺和加速度和載體固聯q 對陀螺儀的角速度輸出進行對陀螺儀的角速度輸出進行積分，獲取載體的姿態信息積分，獲取載體的姿態信息.q 加速度計的輸出需要投影到導航坐標系中加速度計的輸出需要投影到導航坐標系中.q 對導航坐標系中的加速度分量進行補償并積分，獲取載體對導航坐標系中的加速度分量進行補償并積分，獲取載體的速度和位置信息的速度和位置信息.Lecture 10 - Algorithms for SINS4方位軸方位軸滾動軸滾動軸俯仰軸俯仰軸1.2 姿態變換姿態變換S1加速度需要變換加速度需要變換:bzbybxNEAA

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

4、行標系進行 修正修正數學平臺數學平臺Lecture 10 - Algorithms for SINS6Outline捷聯慣導系統概述捷聯慣導系統概述方向余弦矩陣微分方程的推導方向余弦矩陣微分方程的推導方向余弦矩陣的求解方向余弦矩陣的求解: 角增量算法角增量算法Lecture 10 - Algorithms for SINS7方位方位滾動滾動俯仰俯仰2.1 方向余弦矩陣方向余弦矩陣推導方向余弦矩陣推導方向余弦矩陣(DCM)微分方程微分方程:設設 S1 為導航坐標系，其單為導航坐標系，其單位坐標矢量為位坐標矢量為 i, j 和和 kS1S2 為載體坐標系為載體坐標系, 其單位坐標矢量其單位坐標矢量

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

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

7、31312000333231232221131211xyxzyzCCCCCCCCC C - 斜斜對稱矩陣對稱矩陣 CC- 關于方向余弦矩陣的微分方程關于方向余弦矩陣的微分方程Lecture 10 - Algorithms for SINS11Outline捷聯慣導系統概述捷聯慣導系統概述方向余弦矩陣微分方程的推導方向余弦矩陣微分方程的推導方向余弦矩陣的求解方向余弦矩陣的求解: 角增量算法角增量算法Lecture 10 - Algorithms for SINS123.1 方向余弦矩陣微分方程方向余弦矩陣微分方程方向余弦矩陣微分方程方向余弦矩陣微分方程 CCbEbEbEbCC其中其中000 xy

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

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

10、5畢畢-卡解可以改寫為卡解可以改寫為 :2)()(CSItCttCEbEb3.4 C-S 參數泰勒近似參數泰勒近似近似計算近似計算:orderCS1234021212421201161206120解的結果解的結果:CtCttCEbEb)()(或或CnCnCEbEb)() 1(對對 C-S 參數不同程度的近似可以得到不同階次的算法參數不同程度的近似可以得到不同階次的算法. 其中其中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 算例算例某捷聯慣導系統在某捷聯慣導系統在 n 時刻時刻, 其載體坐標系和慣性坐標系重合其載體坐標系和慣性坐標系重合. 然然后從時刻后從時刻 n 到時刻到時刻 n+1, 沿著載體三個軸的三個陀螺儀沿著載體三個軸的三個陀螺儀 X, Y, Z 的角增量輸出分別為的角增量輸出分別為 0.002, 0.

12、004 和和 0.006 (rad); 請利用基于請利用基于Peano-Paker 解的一階角增量算法計算時刻解的一階角增量算法計算時刻 n+1 載體和慣性坐載體和慣性坐標系之間的方向余弦矩陣標系之間的方向余弦矩陣.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