捷联惯性导航系统的解算方法ppt课件.ppt

惯性导航系统原理 3捷联式惯导系统程向红2010 03 19 1 2010 03 19 2 3捷联式惯导系统 3 1捷联式惯导算法概述 3 2姿态矩阵的计算 3 3姿态矩阵计算机执行算法 2 2010 03 19 3 3 1捷联式惯导算法概述 捷联式惯导算法 b ib fb ib P R H L VE VN 捷联式惯导航系统是一个信息处理系统 就是将载体上安装的惯性仪表所测量的载体运动信息 经过计算处理成所需要的导航信息 b 姿态矩阵计算 加速度计组 导航计算机 VE 初始条件 b SF VN n Cb n SF b in t HPR 3 捷联式惯性导航系统 信息处理系统 根据捷联式惯导的应用和功能要求不同 计算的内容和要求 有很大的差别 常有SINS StrapdownInertialNavigationSystemsSVRU StrapdownVerticalReferenceUintSAHRS StrapdownAttitudeandHeadingReferenceSystemsIMU InertialmeasurementUnit 捷联式惯导算法 b ibfib b P R H EN L V V 2010 03 19 4 接联式惯导的算法的基本内容 1 系统的启动和自检测 2 系统初始化 3 惯性仪表的误差补偿 4 姿态矩阵的计算 5 导航计算 6 制导和控制信息的提取 2010 03 19 5 1 系统的启动和自检测 系统启动后 各个部分的工作是否正常 要通过自检测程序加以检测 其中包括电源 惯性仪表 计算机以及计算机软件 通过自检测 发现有不正常 则发出告警信息 或故障码 系统的自检测是保证系统进入导航状态后能正常工作 提高系统可靠性的措施 2010 03 19 6 2 系统初始化 为何要初始化 给定载体 舰船 飞行器 车辆等 的初始位置 经度和纬度 和初始速度等初始信息 导航平台的初始对准 惯性仪表的校准Calibration 平台式 姿态矩阵的初始值 用物理的方法来实现 标度系数加速度计 捷联式 陀螺仪 进行测定 漂移偏置 2010 03 19 7 3 惯性仪表的误差补偿 对捷联式惯导系统来说 由于惯性仪表直接安装在载体上 因此 载体的线运动和角运动都引起较大的误差 为了保证系统的精度 必须对惯性仪表的误差进行补偿 最好的补偿方法是计算机补偿 在计算机中通过专用的软件来实现误差补偿 2010 03 19 8 4 姿态矩阵的计算 姿态矩阵的计算是捷联式惯导算法中最重要的一部分 也是捷联式系统所特有的 不管捷联式惯导应用和功能要求如何 姿态矩阵的计算却是不可少的 姿态矩阵算法是本章重点讨论的内容 2010 03 19 9 5 导航计算 导航计算就是把加速度计的输出信息变换到导航坐标系 然后 计算载体速度 位置等导航信息 2010 03 19 10 6 制导和控制信息的提取 制导和控制信息的提取 载体的姿态既可用来显示也是控制系统最基本的控制信息 此外 载体的角速度和线速度信息也都是控制载体所需要的信息 这些信息可以从姿态矩阵的元素和陀螺加速度计的输出中提取出来 2010 03 19 11 捷联式惯导系统算法流程图 启动自检测初始化姿态阵计算 迭代次数 控制信息提取返回9 2010 03 19 YES导航计算 NO 12 2010 03 19 13 3 2姿态矩阵的计算 捷联式惯导中 载体地理位置就是地理坐标系相对地球坐标系的方位 而载体的姿态和航向则是载体坐标系相对于地理坐标系的方位关系 确定两个坐标系的方位关系问题 是力学中的刚体定点转到理论 在刚体定点转动理论中 描述动坐标系相对参考坐标系方位关系的方法有多种 四参数法1843年发明的 首先在数学中引入四元数 以后用在刚体定位问题 凯里 克莱茵 Cayley Klein 参数法 是在1897年提出的 九参数法基于方向余弦的概念 也称方向余弦法 三参数法 欧拉角法 是欧拉在1776年提出的 四元数法 威廉 哈密顿 WilliamHamilton 在 等效转动矢量法 13 3 2姿态矩阵的计算 3 2 1欧拉角法 3 2 2方向余弦法 3 2 3四元数法 3 2 4等效转动矢量法 2010 03 19 14 3 2 1欧拉角法 Xb ENU作为参考坐标系 则航向角H 纵摇角 俯仰角 P和横摇角 横滚角 倾斜角 R 就是一组欧拉角 欧拉角没有严格的定义 根据需要 可以选用不同的欧拉角组 第一次转动 可以绕三个轴中的任一个转动 故有3种可能 第二次有2种可能 第三次也有2种可能 总共有12种可能 E Xb O U N H Zb Yb Xb Y Yb Zb b Zb P R H P R 一个动坐标系相对参考坐标系的方位 完全可以由动坐标系依次绕3个不同的轴转动的3个转角来确定 如把OXbYbZb作为动坐标系 2010 03 19 15 2 010 03 1916 用欧拉角表示的姿态矩阵 001 U 0 N 0 E Y sinH cosH b Xb sinHcosH Z v CH b 0 sinPcosP Z 00cosPsinP Yb b X v 0 1 Z b X Yb b CP b cosR Z Yb b 0 sinR X 01 v sinR0 Yb cosR b Z X 0 b CR b cosPcosR cosPsinR sinRcosH sinPcosRsinH cosRcosH sinPsinRsinH cosRsinH sinPsinRcosHcosPcosHsinRsinH sinPcosRcosH cosPsinH sinP bn C E X b O UZb bN H Y Xb X b Y Yb b Z b b Z P R H P R H P R 16 欧拉角微分方程 表示载体坐标 系相对地理坐标系的角速度矢量在载体坐标系轴向的分量构成的列矩阵 E Xb O UZb bN H Y Xb X b Y Yb b Z b b Z P R H P R b nb 0 0 R 0 0 P R 0 C H 0 RP CC nby nbz b b b nbx H P R 2010 03 19 17 欧拉角微分方程 cosPcosR H R 0 sinRcosP P 010 sinR nby nbz cosR nbx b b sinP b nby nbz b cosPcosR sinR R cosR 0 sinRcosP01sinP0 H P 1 b bnbx cosRsinP nby nbz b sinRcosP b R cosP sinPsinR 1 cosPcosR0cosP0 sinR H P b nbx cosR b Cn 求解微分方程 3个欧拉角 航向角 H 姿态角 P R 2010 03 19 18 2010 03 19 19 欧拉角法应用中的问题 求解方程可以直接得到航向和姿态信息 欧拉角法得到的姿态阵永远是正交阵 用这个矩阵将比力fb fn信息的坐标变换时 变换后的信息中不存在非正交误差 因此 用欧拉角法得到的姿态矩阵无需进行正交化处理 欧拉角微分方程中包含三角函数的运算 给实时计算带来困难 当P 90 时 方程式出现 奇点 使计算溢出 cosPcosR0cosP0 sinRcosP R 1 sinPsinR cosRsinP cosP sinR cosR bnbx b nby b nbz P H 返回3 2 垂 直 发 射 困 难 19 3 2 2方向余弦法 方向余弦表示的姿态矩阵方向余弦法 用矢量的方向余弦来表示姿态矩阵的方法 用in jn kn 表示沿地理坐标系轴向的单位矢量 ib jb kb 沿载体坐标系轴向的单位矢量 ib在地理坐标系内的方位完全可以由ib的三个方向余弦来确定 其表达式为ib ib in in ib jn jn ib kn kncos ib in jb jb in in jb jn jn jb kn knkb kb in in kb jn jn kb kn kn 2010 03 19 20 方向余弦法 kb kn kn n j jj k j ib kn in n kb in j j i ib in kb jn ib jn kb b ib b bn bn kb b b j ib kn n j n in b Cbn n kb kn bn j jj k ib kn kb in j i ib in kb jn ib jn bn bn Cb n 写成矩阵形式为 2010 03 19 21 矢量的坐标变换 旋转矢量的坐标变换 固定矢量的坐标变换 固定矢量的坐标变换是一个在空间大小和方向都不变的矢量在两个不同方位的坐标系轴向分量之间的变换关系 也即同一个矢量在两个不同的坐标系轴向投影之间的变换关系 是指一个矢量大小不变 但在方向上转动了一个位置 这个矢量转动前和转动后在同一个坐标系轴向分量之间的变换关系 2010 03 19 22 固定矢量的坐标变换 Zk rbTb r Xbib Ybjb bb b 载体坐标系n 地理坐标系 一个矢量r 写成载体坐标系轴向分量形式 Zk rnTn r Xnin Ynjn nn 同一个矢量r 如果写成地理坐标系轴向分量形式 rbTb rnTn Zb b b j Xb rb Y kb b ib Zn n Xn Y rn n jn kn in b Cbn n bTbTbnT n rb rCn rn rbTCb rnT n 由于r是同一个矢量 故 由于正交阵 故 nb bTb 1 n Cn C C 两边求转置 nTT bTbTT Cn r r Cnrb b rn Cbrn n rb 2010 03 19 23 旋转矢量的坐标变换 由于动坐标系随同矢量转动 故rbT rnT互逆 r 转动前的矢量 r 转动后的矢量假定有一个动坐标系和矢量固连 在矢量转动前 取动坐标系b和参考坐标系n重合 则 r rnTn b Cbnr rnTCbn nn r rbTb 如果用r n表示转动后的矢量在参考坐标系轴向的分量构成的矩阵 则 r r nTn r nT rnTCb n Cnrn b r n Cbrn n rb 由于坐标系不动而是矢量转动 它相应于矢量固定时坐标系方向转动 nTb n rCn 2010 03 19 24 2010 03 19 25 方向余弦矩阵微分方程 由矢量相对导数和绝对导数的关系式 r dr drdtdt nb nb 假定地理坐标系为参考坐标系 作为参考坐标系认为它在空间是不动的 即 0 n dt dr nb r drdt b b r bkrnbbnbb b r nbx nb nbz 0 nbz nby0 0 nbynbx bk b nb 载体坐标系相对地理坐标系的转动角速度在b系轴向分量的反对称矩阵 Skewsymmetricmatrix 25 2010 03 19 26 方向余弦矩阵微分方程 另外 从固定矢量的坐标变换关系式有 C brn Cbr nnn r b Cbrn n rb 两边求导 0r br b r n C brn C bCnrb nnb 考虑 bn CnCb bk nb b 两边同右乘Cn bkb nbCn bn C nb rb nbrb bbk nbk Cb nb n C b bk nb bk Tnb bk 1 nb 返回3 2 26 方向余弦矩阵微分方程的几种表示形式 bkb nbCn bnnb C nbk bnb C C 式中的角速度都是用载体坐标系内的分量表示的 如果角速度在地理坐标系轴向的分量表示时 则可用角速度反对称矩阵的相似变换来得到 bnkn nnbb bknb C C nbkb bnbn nknb C C 左式可以用展开的方式推导 bnk nnb bn C C nkn nbnbb C C 在捷联惯导系统中 由于陀螺是固联于载体上的 所以直接测量的角速度是载体坐标系轴向的分量 那么计算时哪个公式最方便 常用的姿态矩阵微分方程的4种形式 2010 03 19 27 方向余弦矩阵微分方程 陀螺仪测量的是载体相对于惯性空间的角速度 b nnbkbbnb C C ib 而式中需要的则是 bknb 两者的关系为 bkin bkib bknb bnkn nibb bkib C C bk n bkib in b nb C C nkninb nbkbib C C 包括载体的姿态和航向的变换角速度 数值较大 如飞机可达400 s 则是地球角速度和载体的位移运动相对地心形成的角速度 这个角速度比较小 一般为每小时几十度 在实时计算上式时 第一项需要用较高的速度计算 用迭代算法时 迭代频率要高 而第二项则可用较低迭代频率计算 可以看作是对第一项的修正 2010 03 19 28 2010 03 19 29 3 2 2 4矩阵微分方程的解下面是解方程的推导过程 C t C 0 C t dt nbk bnb b 0 C t C 0 C 0 C t dt dt t nbkbk nbnb t b n b 00 C t dt dt C 0 C 0 dt ttt bk nb bk nb n b bk nb 00 0 把等式右边的表达式逐次代入积分号内 dt dt C t C 0 C 0 dt C 0 ttt bk nb bk nb bk nb n b tt bk nb bk nb t bk nb n b C t dt dt dt 000 00 0 dt dt C t C 0 I dt ttt bk nb bk nb bk nb n b 00 0 C t dt dt dt ttt bk nb bk nb bk nb n b 000 第2次代入得 这样不断的进行代入 便得到 bk2 0 12 0 00 00 dt dt dt dt d dt t nb t bk nb tt bk nb tt bk nb bk nb bk3 0 16 000 dt C t dt dt dt t nb ttt bk nb bk nb bk nb n b C n Cn bkbbnb变系数的齐次微分方程 t n 可用毕卡 Peano Baker 逼近法求解 积分上式则有 第1次代入得 C t C 0 I dt dt dt 3 0 6 1 bk2 00 2 1 t bk nb t nb t bk bnb 故n Cb t C 0 e t bk nb dt n 0 29 2010 03 19 30 矩阵微分方程的解 Cb t C 0 e t bk nb dt n 0 C t t C t e tn 1 tn bk nbdt n b bk nbdt nb tn 1 tn bk bk Cn t t C t e nb b b b 0 b b 0 0b nbynbx bnbx nbz nby nbz bk nb bk2 12nb3 e KI K K nb bk bk nb I 单位阵 K1 K2 K3 系数 t tn 1 tn 下面来求三个系数 由矩阵的特征方程 如果知道了K1 K2 K3三个系数 则矩阵指数函数就可以表示成一个矩阵二次方程 bk nb 来求它的特征值 b b det I bk b b b b nbynbx nbx nbnbz nbznby 2 0 2 2 3b b nbz b nby nbx 20 2 2 2 b nbz b nby b nbx 3 2 0 01 0 2 3 士j 0 令 将矩阵的特征值代入方程 0 K1 1 KI K bk K bk 212nb3nb e nb bk 30 用201四0 03参 19数法 31 矩阵微分方程的解 j 0 j 0 bk2 nb3nb 20 1 e KI K K bk bk nb j 0 e e 0 K2 bk 2 2 00 sin 0 bk 1 cos 0 nb nb e I bk nb 0 K1 12 e K1 K2j 0 K3 j 0 e j 2 K1 K2j 0 K3 j 0 j 0 2 2K1 2K3 0 2K2 0 j 0 j 0 e e 2 0 1 cos 0 3 K j 0 sin 0 Cn t t C t I sin 0 bk 1 cos 0 bk 2 2 0 0 nb bnb 矩阵微分方程的精确解 这个精确解的前提条件是 nbkbbnb n C C bk nbdt nb tn 1 tn bk 这个式子只有在 t tn 1 tn内角速度矢量 nb方向不变的条件下才有意义 由于转动的不可交换性 当 nb方向随时间变化时 角速度的积分是无意义的 用方向余弦法求解姿态矩阵避免了欧拉角法方程退化的现象 可以全姿态工作 但是 由于方向余弦矩阵具有九个元素 所有 解算矩阵微分方程时 实际上是结算九个联立微分方程 一般说来 计算工作量比较大 为了减小计算工作量 可以采 31 3 2 3四元数法 四元数理论是数学中的一个古老的分支 1943年由威廉 哈密顿 WilliamHamilton 首先提出 目点是研究空间几何 一种类似平面问题中使用复数那样的方法 但是 这个理论建立以后 长期没有得到实际应用 直到空间技术出现以后 特别是捷联式制导技术出现以后 这一古老的数学分支 又重新受到人们的重视 得到了实际的应用 四元数的基本概念 四元数是由1个实数单位1和3个虚数单位i j k组成的含有4个元的数 其形式为Q q0 q1 q2 q3 q0 q1i q2j q3k q0 q标量矢量 2010 03 19 32 3 2 3四元数法 3 2 3 1四元数的基本概念 3 2 3 2四元数理论 3 2 3 3矢量坐标变换的四元数描述 3 2 3 4四元数和方向余弦矩阵的关系 3 2 3 5四元数微分方程 2010 03 19 33 ZRe实轴 Im虚轴 O zcos jzsin Z z1 jz2 zej j 1 u uxi uyj uzk u 1 Zsin k iuZsin j u uxZsin Z Zcos q3 z q2 y q1 q0 v v v v 2010 03 19 34 四元数的基本概念 Z cos uxsin i uysin j uzsin k Quaternions Q Z Z cos q0 Zuxsin q1 Zuysin q2 Zuzsin q3 Q cos usin Q q0 q1i q2j q3k Qeu 由于它具有和复数类似的形式 可看作是复数的推广 因此 也有 超复数 之称 四元数的3种表示形式 2010 03 19 35 坐标系的等效转动 E Xb O UZb N H Yb Xb Xb Y Yb Zb b Zb P R H P R Xb Xr u Yb Yr b Z Zr 2010 03 19 36 四元数的基本概念 如果用u表示欧拉轴向的单位矢量 则动坐标系的方位 完全可由u和 两个参数来确定 用u和 两个参数 可以构造一个四元数 1 如果把u写成分量的形式则 Q cos usin i usin j usin k2x2y2z2 q0 q1i q2j q3k 2 q0 cos 2 q1 uxsin 2 q2 uysin 12 q3 uzsin Q cos usin 22 四元数是张量为1的四元数 即 Q q2 q2 q2 q2 2 10123 这样的四元数称作 规范化 的四元数 而用来描述刚体定点转动的四元数就称作变换四元数 u e2 2010 03 19 37 3 2 3 2四元数理论 四元数相等如果两个四元数对应的元素相等 则两个四元数相等 四元数相加 0 1i 2j 3k m0 m1i m2j m3k 0 m0 1 m1 i 2 m2 j 3 m3 k 对应元素相加 则 四元数相加 服从一般加法的交换律和结合律 即 交换律 结合律 2010 03 19 38 四元数理论 四元数与标量相乘a a 0 a 1i a 2j a 3k 式中a 标量 各个元素分别乘以标量 ab ba a a a b a b a a a 分配律 交换律 结合律 2010 03 19 39 四元数理论 四元数与四元数相乘 0 1i 2j 3k m0 m1i m2j m3k 0 1i 2j 3k m0 m1i m2j m3k 0m0 1m1 2m2 3m3 0 m1i m2j m3k 乘积的矢量形式 2010 03 19 40 2010 03 19 3 41 四元数理论 0m0 1m1 2m2 3m3 i 0m1 1m0 2m3 3m2 j 0m2 2m0 3m1 1m3 k 0m3 3m0 1m2 2m1 乘积的四元数形式 n1 0m1 1m0 2m3 3m2 n2 0m2 2m0 3m1 1m3 n3 0m3 3m0 1m2 2m1 n0 0m0 1m1 2m2 3m3 nnn T Q n n 3 2 1 0 T Q 3 2 1 0 mmm T Q m m 123 0 00123 0 1 1 0 1 2 2 3 0 1 3 2 2 3 3 21 0 3 n m n m n m n m Q n M Q m n0 m0 m1 m2 m3 0 n1 m1 m0m3 m2 1 m3m0m2 m1 n2 m2 m1 2 m0 n3 m3 矩阵四元数 Q n M m Q 矩阵的 核 41 2010 03 19 42 四元数理论 M 和M m 除元素不同外 其核互为转置 这种四元数乘积的矩阵形式 也可推广到三个以上的四元数乘积 如 Q M Q M M P Q m M P Q M P M Q m M M P M P M 说明M 和M具有可交换性 而一般的矩阵相乘 则是不可交换的 Q M Q M M m Q P M Q P M M M m Q M P Q M P M m Q M Q M M P M m 顺序相乘 逆序相乘 类似正交阵的乘积的转置或方阵乘积的求逆 也是逆序 42 四元数理论 四元数的共轭如果一个四元数为八 则定义其共轭四元数为 结合律 分配律 推理1 八 M 八 M 四元数之和的共轭等于共轭之和推理2 八 M M 八 两个四元数之积的共轭等于共轭四元数等于两个四元数共轭之积取相反的顺序 四元数的范数 四元数的范数定义为 2222 3 2 1 0 2010 03 19 43 四元数理论 八 八 八 八 八 1的四元数称为规范化的四元数 八 M 八 M M 八 四元数的逆 则八 1 八 八 八 1 1 22 0123 2 2 2010 03 19 44 3 2 3 3矢量坐标变换的四元数描述 一个矢量r在参考坐标系 这里用地理坐标系作参考系 轴向的分量形式为r xnin ynjn znkn式中xn yn zn为r在地理坐标系轴向的分量 in jn kn为地理坐标系轴向的单位矢量 用xn yn zn把r写成四元数形式即 Rn 0 xni ynj znk 0 rRn就叫做矢量r在地理坐标系上的四元数影像 i j k是四元数的虚数单位 而r则是四元数的矢量部分 显然 如果认为i j k和in jn kn重合 则四元数的矢量部分就是三维空间的矢量r本身 2010 03 19 45 旋转矢量的坐标变换 定义 假设矢量r绕通过定点 O 的某一轴转动了一个角度 则和矢量固联的动坐标系和参考坐标系之间的变换四元数为 22 Q cos usin 式中u为转轴方向的单位矢量 这个四元数的范数为 Q q2 q2 q2 q2 10123 转动前的矢量用r表示 转动后的矢量用r 表示 则r和r 的关系可由四元数来描述 即 称作 规范化 的四元数 r Q r Q Q cos usin 四元数的共轭四元数22 黄式两边同时左乘Q 右乘Q得因为 Q r Q Q Q r Q QQ Q Q Q 1 2 2 cos usin cos usin cos sin 1 22 22 2 2 r Q r Q 2010 03 19 46 2010 03 19 47 证明 AO当矢量r绕OO 旋转时 矢端A在空间的轨迹是一个圆 这个圆平面和转轴垂直 圆心为O 在旋转轴上 在圆上取一点B 使 AO B 90 则按矢量关系有下列关系式 O r r A B u A O O B OO u O u r A OO r u u OO O A r 因为a b a b cos O A r OO r r u uO B u O A u r r u u u r r u u u u r u u 0 O B u r 47 2010 03 19 48 证明 A B O Acos A O O Bsin O O r r A B u A 如果Q q0 q R r0 r 则利用式可以写成矢量形式为 Q R q0r0 q0r qr0 q r q r利用上式将Q r Q 展开 v v q0q Q cos usin 22 r R r0 rr0 0 q 0O A r r u uO B u r q sin u r cos r u r sin v 2 2v 2 八 0m0 0m m0 m mO A O Acos O Bsin cos r r u u u rsin r OO O A r u u cos r cos r u u sin u r 1 cos r u u cos r sin u r Q r cosr usin r u r sin 222 48 2010 03 19 49 推导 q 0q0q 0q q q0 cos r u r sin usin cos r u r sin usin 2v 2 2 2v 2 2q q q q sin cos u r sin2 u r u cos cos r u r sin 2v 2 2v 22v 2 Q r Q sin u r cosr u r sin cos usin 2 222 2q sincos u r sin2 u r u cos2r sincos u r sincos u r 2222 Q R q0r0 q0r qr0 q r q r sin2 u r u sin cos r u sin2 u r u2222第一项与第五项相消 第六项为零第四项与第七项合并 sin2 u r u cos2 r sin u r sin2 u r u 222 22 22 q 0 q0 q 49 证明Q r Q sin2 u r u cos2 r sin u r sin2 u r u 222 利用Q r Q sin2 u r u cos2 r sin u r sin2 u u r u r u 222 cos2 r sin2 r sin u r 2sin2 u r u 222 r Q r Q cos r sin u r 1 cos u r ur OO O A r u u cos r cos r u u sin u r 1 cos r u u cos r sin u r u r u u u r u r u 2010 03 19 50 固定矢量的坐标变换 如果矢量固定不动 而动坐标系相对参考坐标系转动了一个角度 则以四元数描述的矢量在两个坐标系上的分量的变换关系为 r Q r Q 如果将四元数Q R M的四个元写成列矢量 即表示成 3 2 3 4四元数和方向余弦矩阵的关系 Q q q1 q2 q0 q3 Q r 1 Q m r2 r0 m0 r m r3 m3 m2 1 将固定矢量的坐标变换式 Rb Q Rr Q写成矩阵形式 并以地理坐标系为参考坐标系 则有 Q Rb M Q M Q Q Rn Rb Q Rr Q Rr Q Rb Q Rb Q Rr Q 2010 03 19 51 3 2 3 4四元数和方向余弦矩阵的关系 Q Rb M Q M Q Q Rn Q R 0 xyz T bb bb Q R 0 xyz T n q0 q1 q2 q3 qq n n n 2 qq q M Q q qqq2 q q3 q0q1 q1q0 1 0 3 2 3 10 q2q3 q 2 q q q2 M Q q3 0q3q0q1q2 q1q0 q0q1q2q3 qqq 3 1 yn n q x zn q1 q2 q3 0 qq 2 q2 q3 2 q q q0 03q3q0q1q2 q1q0 q2 b q3 03q3q0q1q2 q1q0 yb x zb 0 1 1 2010 03 19 52 四元数与姿态矩阵的关系 q2 q2q3 q0q1 yn 2 q1q3 q0q2 xn n 0 0123 q2 q2 q2 q2 q q q q 2 qq qq 2 qq qq q q qq q 02 q1q2 q0q3 q2 q223 q0 q1 z 2 qq 000 0 0 2222 2301 02 13 2 223 22 1 0 03 12 22 0123 2 q2q3 q0q1 yn n 2 q1q3 q0q2 xn 2 q1q3 q0q2 yb 2 q1q2 q0q3 q0 q1 q0 q1 q q q2 q2 q2 q201232 q2q3 q0q1 2 q1q2 q0q3 q2 q3 zb xb z 2222 23 2 222 q2 q2 q2 q2 2 q2q3 q0q1 3 2 q1q3 q0q2 2 qq qq Cn 2 q1q2 q0q3 q0 q1 2 qq qq q2 q223 q0 q1 2 q1q2 q0q3 q2 q3 012 2301 1302 22 2222 b 2010 03 19 53 由四元数计算姿态矩阵 由b Cbn可知上式可写为 n zb T31T33 zn 23 n y T13 xn y T T11T1222T32 b xb TT 21 T23 2 q2q3 q0q1 T31 2 q1q3 q0q2 T32 2 q2q3 q0q1 T q2 q2 q2 q2 T q2 q2 q2 q2 220123 T q2 q2 q2 q2110123T12 2 q1q2 q0q3 T13 2 q1q3 q0q2 T21 2 q1q2 q0q3 两个变换矩阵相等 即对应元素相等 如