方向余弦矩阵和姿态角提取算法(矩阵)

1、Lecture 10 - Algorithms for SINS1Strap-down Inertial Navigation System捷联惯导系统捷联惯导系统介绍及算法（介绍及算法（DCM）Introduction and AlgorithmsLecture 10 - Algorithms for SINS2Outlineq 捷联惯导系统概述捷联惯导系统概述q 姿态矩阵（姿态矩阵（DCM）微分方程的推导）微分方程的推导q 方向余弦矩阵的求解方向余弦矩阵的求解: 角增量算法角增量算法Lecture 10 - Algorithms for SINS31.1*捷联惯导系统捷联惯导系统: 特点特

2、点方位轴方位轴滚动轴滚动轴俯仰轴俯仰轴q 陀螺和加速度计被直接陀螺和加速度计被直接“捆绑捆绑(strapped)”到载体上到载体上q 没有物理的平台没有物理的平台q 对陀螺仪的角速度输出进行对陀螺仪的角速度输出进行积分，获取载体的姿态信息积分，获取载体的姿态信息.q 加速度计的输出需要变换到导航坐标系中加速度计的输出需要变换到导航坐标系中.EN对导航坐标系中的加速度分量进行补偿并积分，获取载体的速对导航坐标系中的加速度分量进行补偿并积分，获取载体的速度和位置信息度和位置信息.Lecture 10 - Algorithms for SINS4z, 方位轴方位轴x, 滚动轴滚动轴y, 俯仰轴俯仰轴

3、1.2 姿态变换姿态变换加速度需要变换加速度需要变换:eRNEbzbybxNEfffCfffq C 包含姿态信息包含姿态信息. CCq 也可用欧拉角、四元数等表示姿态也可用欧拉角、四元数等表示姿态Lecture 10 - Algorithms for SINS51.3 SINS的示意框图的示意框图计算载体的计算载体的姿态信息姿态信息 姿态信息姿态信息计算机计算机ZYXAAA沿载体轴的沿载体轴的 加速度输出加速度输出沿载体轴的沿载体轴的 角速率输出角速率输出惯性元件惯性元件ZYXGGG对加速度分量对加速度分量进行坐标变换进行坐标变换沿地理坐标沿地理坐标 系各轴的加系各轴的加速度速度导航计算导航计

4、算显示显示姿态姿态 位置位置速度速度对地理坐对地理坐标系进行标系进行 修正修正数学平台数学平台Lecture 10 - Algorithms for SINS6Outlineq 捷联惯导系统概述捷联惯导系统概述q 姿态矩阵（姿态矩阵（DCM）微分方程的推导）微分方程的推导q 方向余弦矩阵的求解方向余弦矩阵的求解: 角增量算法角增量算法Lecture 10 - Algorithms for SINS7z, 方位方位x, 滚动滚动y, 俯仰俯仰2.1 方向余弦矩阵方向余弦矩阵咋来的？咋来的？ CCRNES1设设 S1 为导航坐标系，其单为导航坐标系，其单位坐标矢量为位坐标矢量为 i, j 和和 k

5、S2S2 为载体坐标系为载体坐标系, 其单位坐标矢量其单位坐标矢量为为 i, j 和和 k Lecture 10 - Algorithms for SINS82.1 方向余弦矩阵方向余弦矩阵C 为从为从 S2 到到 S1 的方向余弦的方向余弦矩阵，即矩阵，即: 21SSCAAS2 j k iRjkiS1其中其中 333231232221131211CCCCCCCCCCkkjkikkjjjijkijiii载体的旋转导致载体的旋转导致 C 发生变化发生变化Lecture 10 - Algorithms for SINS92.2 方向余弦矩阵的导数方向余弦矩阵的导数kkjkikkjjjijkijii

6、iCsodtdkkdtdjkdtdikdtdkjdtdjjdtdijdtdkidtdjidtdiiC设载体坐标系相对于导航坐标系的角速度为设载体坐标系相对于导航坐标系的角速度为表示在载体坐标系中表示在载体坐标系中TZYXidtdi则则)(001yzzyxkjkji)(zxikdtdj类似的类似的)(xyjidtdkLecture 10 - Algorithms for SINS10则则2.3 方向余弦矩阵微分方程方向余弦矩阵微分方程)()()()()()()()()(xyzxyzxyzxyzxyzxyzjikikkkjkjijikjkjjjiiikikjiCxyzxyzxyzxyzxyzxyz

7、CCCCCCCCCCCCCCCCCC323131333332222121232322121111131312000333231232221131211xyxzyzCCCCCCCCC C - 斜斜对称矩阵对称矩阵 CC- 其解取决于其解取决于 C(0) 和载体的转动角速度的变化规律和载体的转动角速度的变化规律Lecture 10 - Algorithms for SINS11Outlineq 捷联惯导系统概述捷联惯导系统概述q 姿态矩阵（姿态矩阵（DCM）微分方程的推导）微分方程的推导q 方向余弦矩阵的求解方向余弦矩阵的求解: 角增量算法角增量算法Lecture 10 - Algorithms

8、for SINS123.1 方向余弦矩阵微分方程方向余弦矩阵微分方程BGBGBGBCC记初始时刻的地理坐标系为记初始时刻的地理坐标系为 G0 , 则有则有BIBGBGBCC00000 xyxzyzBIB其中其中TBIBdttGBGBeCTC000)()0()(则则记记dtTBIB0000 xyxzyzzyxi,Tiidt0where(角增量角增量)2220zyxand denote(向量的模向量的模)Lecture 10 - Algorithms for SINS133.2*毕毕-卡解卡解TBIBdttGBGBeCTC000)()0()(eCGB)0(0220000cos1sin)0(0ICG

9、B- 毕毕-卡形式的解卡形式的解可记为可记为:2)0()(00CSICTCGBGB近似计算近似计算:orderCS1234021212421201161206120Lecture 10 - Algorithms for SINS143.3*角增量算法角增量算法orderCS1234021212421201161206120方程的解方程的解)0()(200CSICTCGBGBCCGB)0(0各阶近似各阶近似:1st :IC2nd :221IC.许多类型的陀螺仪可用内部许多类型的陀螺仪可用内部硬件完成对角速率的积分，硬件完成对角速率的积分，从而直接输出角增量从而直接输出角增量. Lecture 1

10、0 - Algorithms for SINS153.6 地理坐标系的修正地理坐标系的修正CCTCGBGB)0()(00EN00B0GTTBENTG00GBC0BBTC0000BBGBGBTTCCCTTGBC0GGTC00001)(BBGBGGGBTTTTCCCCsintancoseEeENGIGRVRVRVGIGGGGGCC00CCCGGGGT000C IGIGTILecture 10 - Algorithms for SINS163.7 算例算例某捷联惯导系统在某捷联惯导系统在 n 时刻时刻, 其载体坐标系和惯性坐标系重合其载体坐标系和惯性坐标系重合. 然然后从时刻后从时刻 n 到时刻到时

11、刻 n+1, 沿着载体三个轴的三个陀螺仪沿着载体三个轴的三个陀螺仪 X, Y, Z 的角增量输出分别为的角增量输出分别为 0.002, 0.004 和和 0.006 (rad); 请利用基于请利用基于Peano-Paker 解的一阶角增量算法计算时刻解的一阶角增量算法计算时刻 n+1 载体和惯性坐载体和惯性坐标系之间的方向余弦矩阵标系之间的方向余弦矩阵.3)(InC000 xyxzyz0002. 0004. 0002. 00006. 0004. 0006. 003)() 1(InCnCLecture 10 - Algorithms for SINS173.7 算例算例3)() 1(InCnC0002. 0004. 0002. 00006. 0004. 0006. 001000100013I1002. 0004. 0002. 01006. 0004. 0006. 011000100011002. 0004