四元数与欧拉角刚体动力学数值积分算法及其比较
刚体运动的四元素表示
刚体运动的四元素表示
杨天标
【期刊名称】《德州学院学报》
【年(卷),期】2010(026)002
【摘要】刚体运动是旋转与平移的合成,文中主要讨论旋转,仅在应用实例中讨论平移的影响.旋转的表达形式很多,例如标准正交矩阵、Euler角、向量表达式、四元数等等.用四元数表达三维的旋转与使用矩阵相比具有计算简单和几何意义明确两大优点,四元数旋转可以避免Euler角旋转在某些情况下产生的自由度丧失.四元数方法在计算机仿真、图形图像、飞行力学、航空航天等领域应用广泛.讨论旋转的四元数形式及其与其它形式,主要是变换矩阵形式,之间的相互转化,并试图给出比较简单的转化算法.
【总页数】8页(P5-12)
【作者】杨天标
【作者单位】长江师范学院数学与计算机学院,重庆,408100
【正文语种】中文
【中图分类】O1
【相关文献】
1.强化职业正能量的"三课堂四元素六路径"药品经管类课程教学模式改革与探索[J], 丁静;丁洁琼;吕金丽
2.把握"四元素"书写新闻故事r——浅谈现场新闻的几点写作体会 [J], 吕卫锋;曹
菡
3.基于极化特征参数和极化干涉最优参数的改进四元素分解方法 [J], 王宇; 禹卫东; 刘秀清
4.镁将成为继氮磷钾之后作物需求的第四元素 [J], 吴江
5.线变换刚体运动矩阵的群表示方法 [J], 杨朔飞;孙涛;黄田;戴建生
因版权原因，仅展示原文概要，查看原文内容请购买。
四元数
二.四元数与姿态阵之间的关系
3.由于 || Q || q0 2 q12 q2 2 q3 2 =1，所以：
q0 2 q12 q2 2 q3 2 R Cb 2(q1q2 q0 q3 ) 2(q q q q ) 1 3 0 2
2(q1q2 q0 q3 ) q0 q1 q2 q3 2(q2 q3 q0 q1 )
构造四元数：
q0 cos
2
2 q2 m sin 2 q3 n sin 2
q1 l sin
Q q0 q1i0 q2 j0 q3 k0 cos cos
2
(li0 mj0 bk0 ) sin
2
2
u R sin
2
二.四元数与姿态阵之间的关系
记：
rx ' r 'R r ' y rz '
rx rR r y rz
l uR m n
二.四元数与姿态阵之间的关系
0 n m rx r (u r ) R n 0 l y 0 m l rz
q0 2 q12 q2 2 q3 2 CbR 2(q1q2 q0 q3 ) 2(q q q q ) 1 3 0 2 2(q1q2 q0 q3 ) q0 q1 q2 q3 2(q2 q3 q0 q1 )
2 2 2 2
2(q2 q3 q0 q1 ) 2 2 2 2 q0 q1 q2 q3 2(q1q3 q0 q2 )
二.四元数与姿态阵之间的关系
四元数与欧拉角刚体动力学数值积分算法及其比较
四元数与欧拉角刚体动力学数值积分算法及其比较徐小明;钟万勰【摘要】为推广四元数保辛积分在工程中的应用,对欧拉角表示的状态方程数值积分与四元数的保辛积分进行比较.重陀螺的数值仿真结果表明四元数保辛积分的数值结果明显优于欧拉角状态方程积分.与欧拉角状态方程积分相比,四元数保辛积分在刚体动力学的数值仿真中更具优势.【期刊名称】《计算机辅助工程》【年(卷),期】2014(023)001【总页数】6页(P59-63,75)【关键词】四元数;欧拉角;刚体动力学;保辛;重陀螺【作者】徐小明;钟万勰【作者单位】大连理工大学工程力学系,辽宁大连116024;大连理工大学工业装备结构分析国家重点实验室,辽宁大连116024;大连理工大学工程力学系,辽宁大连116024;大连理工大学工业装备结构分析国家重点实验室,辽宁大连116024【正文语种】中文【中图分类】TP391.9;O313.30 引言四元数、欧拉角和方向余弦［1］是描述刚体旋转最主要的3 种坐标形式.方向余弦法需要9 个参量，应用较少;而另外2 种则应用广泛，如航天器姿态控制和捷联式惯性导航系统［1］等，对于两者的研究也卷帙浩繁，但对两者优劣的评价却褒贬不一.描述刚体在三维空间中的运动姿态可采用2 类12 种欧拉角系统，分别对应于不同的旋转轴先后次序.目前公认的用欧拉角描述旋转的固有缺陷是奇异性问题［2］，即:无论采用哪种欧拉角系统，都不可避免地会含有奇异点，使得在该点附近区域进行的数值积分精度不高.对于小角度旋转，只要采用适当的欧拉角系统便可避开奇异点;而在大角度旋转时，若想避开奇异点，必须提供2 套欧拉角系统交替进行计算.据此，黄雪樵［3］提出一种“双欧法”的计算方法;近几年仍有学者［4］在继续研究该方法.双欧法虽然解决奇异性问题，但是计算过程较复杂.四元数用于描述刚体旋转，没有奇异性问题，可很好地描述刚体的全角度旋转.然而，四元数需要满足长度等于1 的单位约束，这成为制约其应用的限制.在实际应用中，经常采用的正交化修正等方法只能缓解长度的偏移，无法从根本上解决问题;黄雪樵［3］在其双欧法中也有所讨论.目前，对于单位约束最佳的解决方案是将四元数表示的刚体动力学方程导入微分代数方程范畴，近年来逐渐有学者［5-7］展开相关问题的研究.徐小明等［8］提出一种基于四元数理论描述刚体旋转的保辛数值积分方法.该方法先将问题导入微分-代数方程系统，然后利用分析结构力学理论［9］进行逐步积分，该积分保辛.该方法在积分点上严格满足四元数长度等于1 的约束条件，而在区段内部则由插值近似，属于祖冲之类方法［10］.数值算例表明仿真效果优异.本文简要介绍四元数和欧拉角的基本理论，以重陀螺为例对2 种表示形式的数值积分进行比较.对于欧拉角表述，应用比较普遍的状态方程表述.研究表明，以欧拉角和角速度为状态变量形成的1 阶微分方程在使用差分近似积分时，精度损失很大，能量不守恒;该现象被周江华等［11］称为“睡陀螺”.这是一个值得注意的问题，却未得到广泛关注;而采用文献［8］给出的保辛格式，四元数单位长度约束条件得到满足，仿真结果优异，能量也达到守恒.1 刚体旋转及其运动学表示刚体运动由质心平动和绕质心转动组成.如果刚体上有一固定点，则称为刚体定点转动问题.假设Oxyz 为系统的惯性坐标系，O 为原点，亦为固定点.Ox'y'z'为随体坐标系，固定于刚体上.若将刚体的随体坐标轴取为与固定点有关的主轴，则刚体定点转动可由式(1)描述.式中:I1，I2和I3为将定点选为参考点的主转动惯量;ω1，ω2和ω3为绕3 个主轴的角速度分量;N1，N2和N3为外力矩沿3 个主轴的分量.式(1)中的3 个方程称为刚体定点转动的欧拉方程.要求解式(1)的微分方程，还需将描述旋转的坐标与角速度联系起来.如果采用欧拉角描述刚体运动，可设φ，θ 和Ψ 分别代表绕z 轴、转动后的x 轴及二次转动后的z 轴的三次旋转，即采用12 种欧拉角系统中的z-x-z 模式，则角速度分量与欧拉角之间的微分关系可以表示为式(2)称为刚体定点转动的运动方程.将式(2)与式(1)联立，将(φ，θ，Ψ)作为广义位移，将(ω1，ω2，ω3)作为广义速度，构成系统的状态方程.对于此状态方程，应用欧拉差分格式或者龙格库塔格式等可进行数值积分.本文将在第2 节对其应用辛-欧拉格式［12］进行研究.另一方面，若采用单位四元数描述旋转，则有四元数的运动学方程可以定义四元数向量式(3)隐含着四元数单位长度的约束条件将式(3)与式(1)联立可得状态方程，对其应用数值算法求解，约束条件无法很好地满足，往往导致结果失真.反对称群是正交矩阵群的李代数［8］，据此在群上定义微商，可将式(3)等价成式中:通过式(6)便可得到刚体的动能，进而得到系统的Lagrange 函数，然后通过作用量的变分原理进行数值离散求解，具体算法见第2 节.2 数值积分算法首先给出2 种表示形式的积分格式.对于欧拉角，采用辛-欧拉差分格式［12］.为此，可将式(2)和(1)写为式中:则欧拉差分格式为对式(11)进行迭代便可逐步积分求解.根据文献［12］，将式(11)应用于Hamilton 正则方程可达到保辛效果.显然式(8)仅是状态方程表述，不能达到保辛效果.然而，式(8)应用十分广泛，形式也较为简单，对其数值积分进行比较研究具有一定的现实意义.对于四元数的数值积分，首先需要刚体系统的Lagrange 函数式中:U(q)为系统的势能为系统的动能.根据式(6)，式(12)可以写为式中:q 参见式(4).要进行数值积分，首先要对系统进行有限元离散.具体来说，取离散时间区段η，t0=0，t1=η，t2=2η，…，tk=kη，….可假设tk-1时的位移与速度是已知，并满足的约束.现在的问题是通过tk-1步的已知量计算下一个时间步的qk和为此，首先在［tk-1，tk］时间段内做有限元离散.为计算简便，假设区段内的位移和速度分别为则根据式(12)与四元数约束条件可以得到离散的区段作用量(含约束)式中:λk-1和λk为Lagrange 乘子.根据分析结构力学理论［9］给出四元数的保辛积分格式，对式(16)进行迭代便可逐步积分求解.对于式(16)的具体推导以及作为参考的具体算法见文献［8］.3 重陀螺的数值模拟3.1 算例1图1 对称重陀螺Fig.1 Symmetric heavy top考察如图1 所示的对称重陀螺绕其尖点O 的运动.该尖点固定于惯性空间;取陀螺的对称轴为随体坐标Ox'y'z'的z'轴;陀螺质心与尖点的距离为l，陀螺的基本参数为:m=1 kg，l=0.04 m，I1=I2=0.002 0 kg·m2，I3=0.000 8 kg·m2.取重力加速度g=9.8 m/s2.对于此例，可以写出式(10)对应的重力矩的具体表达式以及式(12)对应的重力势能的具体形式对于初始时刻，选取对应于式(16)的初始条件为对于式(4)在k=1 时的p0，结合式(12)和(20)，则由Legendre 变换给出具体见文献［8］.这一初始条件对应于陀螺运动中的“尖点运动”.设陀螺对称轴(z'轴)与单位球面的交点为A，则任意时刻A 点的位置由式(22)确定.采用四元数表示则为对称重陀螺的z'轴与单位球面交点A 的z 坐标-时间曲线见图2.对于对称重陀螺，是有椭圆函数解的，z 随时间应该呈周期变化.由图2 可知，唯有当时间间隔非常小时，用欧拉角表示刚体旋转的辛-欧拉差分格式的数值解才与解析解拟合得较好，步长较大时呈现发散现象;而基于四元数的保辛积分，不论大步长还是小步长，均与解析解吻合得很好.其中，当步长取Δt=10-2 s 时，1 个周期内仅有15 个左右的积分点，与解析解相比，仅仅相位略微超前，表明在大步长下数值积分的结果仍然可信.图2 对称重陀螺的z＇轴与单位球面交点A 的z 坐标-时间曲线Fig.2 z coordinate vs time curves of point A of intersection of symmetric heavytop axis z＇ and unit sphere对称重陀螺A 点长时间轨迹沿x-y 平面的投影见图3.由于是保守系统，z'轴应以z 轴为轴绕其转动，周而复始，所以A 点沿x-y 平面的投影应该限制在一个圆环内.图3(b)表明四元数的保辛积分很好地模拟这一现象;图3(a)表明，在Δt=10-3 s 时，欧拉角的保辛积分结果完全失真，实际上如果继续减小步长至Δt=10-4 s，为四元数积分步长的1/100，这一现象仍未得到缓解.本文采用的是辛-欧拉格式，文献［11］中采用4 阶龙格库塔法，同样出现此现象，被称之为“睡陀螺”.图3 对称重陀螺A 点长时间轨迹沿x-y 平面的投影Fig.3 x-y plane projectionof long time trajectory of point A for symmetric top2 种数值积分的系统能量随时间变化情况见图4.在欧拉角表示中，虽然采用辛-欧拉格式进行数值积分，但是能量却保持得不好，这验证对状态方程应用辛-欧拉格式并不能保辛.与之相反，四元数的数值积分保辛，其能量保持得很好，这也是保辛积分的优势所在.四元数保辛积分的约束误差见图5，表明在时间积分过程中单位长度约束条件满足得很好.3.2 算例2将算例1 中转动惯量改为I1=0.002 25 kg·m2，I2=0.001 75 kg·m2，I3不变，其他参数以及初始条件与算例1 相同.不对称重陀螺A 点长时间轨迹沿x-y 平面的投影见图6，采用的是基于四元数的保辛积分.与图2(b)对比可看出，其轨迹一直限制在一圆环内，这也是保守系统的特点.本例表明，四元数保辛积分，不论对于对称陀螺还是不对称陀螺，均能达到较好的仿真效果.图4 两种数值积分的系统的能量随时间变化情况Fig.4 Energy variation with respect to time for two numerical integration systems图5 四元数保辛积分的约束误差Fig.5 Constraint error of symplectic preservation integration of quaternion图6 不对称重陀螺A 点长时间轨迹沿x-y 平面的投影Fig.6 x-y plane projection of long time trajectory of point A for asymmetric top注：时间间隔Δt=10-2 s，时间长度t=50 s4 结束语介绍2 种刚体旋转的数值积分，一种基于欧拉角表示，另一种基于四元数表示.以重陀螺的高速旋转为例，对2 种数值积分进行比较发现:以欧拉角、角速度组成状态变量，然后直接使用辛-欧拉格式不能保辛，能量衰减很快，数值积分存在缺陷;与之相反，采用四元数表示，根据分析结构力学的保辛积分方法，并结合祖冲之类方法的思想，可以很好地避免约束违约，仿真效果令人满意，可作为陀螺等仿真分析的有力工具.本文仅对以欧拉角、角速度组成状态变量的数值积分进行研究，对其他形式并未涉及，对其积分效果不佳的成因亦未研究，还有很多工作有待展开.参考文献:【相关文献】［1］张树侠，孙静.捷联式惯性导航系统［M］.北京：国防工业出版社，1992：48-80.［2］赵晓颖，温立书，么彩莲.欧拉角参数表示下姿态的2 阶运动奇异性［J］.科学技术与工程，2012，12(3) ：634-637.ZHAO Xiaoying，WEN Lishu，YAO Cailian.The second-order kinematic singularity of orientation in Euler parameters representation［J］.Sci Technol ＆Eng，2012，12(3) ：634-637.［3］黄雪樵.克服欧拉方程奇异性的双欧法［J］.飞行力学，1994，12(4) ：28-37.HUANG Xueqiao.The dual-Euler method for overcoming the singularity of Euler equation［J］.Flight Dynamics，1994，12(4) ：28-37.［4］李跃军，阎超.飞行器姿态角解算的全角度双欧法［J］.北京航空航天大学学报，2007，33(5) ：505-508.LI Yuejun，YAN Chao.Improvement of dual-Euler method for full scale Eulerian angles solution of aircraft［J］.J Beijing Univ Aeronautics ＆Astronautics，2007，33(5) ：505-508.［5］NIKRAVESH P E，WEHAGE R A，KWON K.Euler parameters in computational kinematics and dynamics：Part 1［J］.J Mechanisms，Transmissions ＆ Automation Des，1985，107(3) ：358-365.［6］BETSCH P，SIEBERT R.Rigid body dynamics in terms of quaternions：Hamiltonian formulation and conserving numerical integration［J］.Int J Numer Methods Eng，2009，79(4) ：444-473.［7］UDWADIA F E，SCHUTTE A D.An alternative derivation of the quaternion equationsof motion for rigid-body rotational dynamics［J］.J Appl Mech，2010，77(4) ：44505. ［8］徐小明，钟万勰.刚体动力学的四元数表示及保辛积分［J］.应用数学和力学，2014，35(1) ：1-11.XU Xiaoming，ZHONG Wanxie.Symplectic integration of rigid body motion by quaternion parameters［J］.Appl Math ＆ Mech，2014，35(1) ：1-11.［9］钟万勰，高强.约束动力系统的分析结构力学积分［J］.动力学与控制学报，2006，4(3) ：193-200.ZHONG Wanxie，GAO Qiang.Integration of constrained dynamical system via analytical structrural mechanics［J］.J Dynamics ＆ Contr，2006，4(3) ：193-200.［10］钟万勰，高强，彭海军.经典力学辛讲［M］.大连：大连理工大学出版社，2013：202-241. ［11］周江华，苗育红，李宏，等.四元数在刚体姿态仿真中的应用研究［J］.飞行力学，2000，18(4) ：28-32.ZHOU Jianghua，MIAO Yuhong，LI Hong，et al.Research of attitude simulation using guaternion［J］.Flight Dynamics，2000，18(4) ：28-32.［12］HAIRER E，LUBICH C，WANNER G.Geometric numerical integration：structure-preserving algorithms for ordinary differential equations［M］.Berlin：Springer，2006：189.。
P17四元数
∆θ ω* = ∆t
瞬时角速度
ω = lim ω *
∆t → 0
四元数表示转动 约定
一个坐标系或矢量相对参考坐标系旋转， 转角为θ 一个坐标系或矢量相对参考坐标系旋转， 转角为θ， 与参考系各轴间的方向余弦值为cosα 转轴 n 与参考系各轴间的方向余弦值为 α、cosβ、cosγ。 β γ
n = cos α ⋅ i + cos β ⋅ j + cos γ ⋅ k
= cos cos + sin cos i + sin sin j + cos sin k 2 2 2 2 2 2 2 2
θ
ψ
θ
ψ
θ
ψ
θ
ψ
映象方式1 求方向余弦 映象方式1
以瞬时转轴映象形式给出 以瞬时转轴映象形式给出 映象 转动四元数的表达式并求 出合成转动四元数 第一次转时， 第一次转时，映象形式的 q1 和非映象形式的 q1 是 一致的： 一致的：
四元数表示转动 坐标系旋转
如果坐标系 旋转， 如果坐标系 OXYZ 发生 q 旋转，得到新坐标系 OX’Y’Z’ 一个相对原始坐标系 OXYZ 不发生旋转变换的矢量 V
V = xi + yj + zk
矢量 V 在新坐标系上 OX’Y’Z’ 的投影为
V = x ' i '+ y ' j '+ z ' k '
−1
这个公式的意义是说，在一个超复数空间中， 这个公式的意义是说，在一个超复数空间中，或者在一个固 定坐标系中， 定坐标系中，矢量 VE 按着四元数 q 所表示的方向和大小转 动了一个角度， 动了一个角度，得到一个新的矢量 VE’
旋转矩阵、欧拉角、四元数理论及其转换关系
旋转矩阵、欧拉⾓、四元数理论及其转换关系1. 概述旋转矩阵、欧拉⾓、四元数主要⽤于表⽰坐标系中的旋转关系，它们之间的转换关系可以减⼩⼀些算法的复杂度。
本⽂主要介绍了旋转矩阵、欧拉⾓、四元数的基本理论及其之间的转换关系。
2、原理2.1 旋转矩阵对于两个三维点p1(x1,y1,z1)，p2(x2,y2,z2)，由点 p1 经过旋转矩阵 R 旋转到 p2，则有注：旋转矩阵为正交矩阵RR^T=E任意旋转矩阵：任何⼀个旋转可以表⽰为依次绕着三个旋转轴旋三个⾓度的组合。
这三个⾓度称为欧拉⾓。
三个轴可以指固定的世界坐标系轴，也可以指被旋转的物体坐标系的轴。
三个旋转轴次序不同，会导致结果不同。
2.2 欧拉⾓欧拉⾓有两种：静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静⽌，所以称为静态。
动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态。
使⽤动态欧拉⾓会出现万向锁现象；静态欧拉⾓不存在万向锁的问题。
对于在三维空间⾥的⼀个参考系，任何坐标系的取向，都可以⽤三个欧拉⾓来表现。
参考系⼜称为实验室参考系，是静⽌不动的。
⽽坐标系则固定于刚体，随着刚体的旋转⽽旋转。
如图1，设定xyz-轴为参考系的参考轴。
称xy-平⾯与XY-平⾯的相交为交点线，⽤英⽂字母（N）代表。
zxz顺规的欧拉⾓可以静态地这样定义：α是x-轴与交点线的夹⾓，β是z-轴与Z-轴的夹⾓，γ是交点线与X-轴的夹⾓。
图中三个欧拉⾓分别为：(α,β,γ)；蓝⾊的轴为：xyz轴红⾊的轴为：XYZ轴绿⾊的线为交线：Nα∈[0,2π],β∈[0,π],γ∈[0,2π]很可惜地，对于夹⾓的顺序和标记，夹⾓的两个轴的指定，并没有任何常规。
科学家对此从未达成共识。
每当⽤到欧拉⾓时，我们必须明确的表⽰出夹⾓的顺序，指定其参考轴。
实际上，有许多⽅法可以设定两个坐标系的相对取向。
欧拉⾓⽅法只是其中的⼀种。
此外，不同的作者会⽤不同组合的欧拉⾓来描述，或⽤不同的名字表⽰同样的欧拉⾓。
四点 计算 空间 姿态
四点计算空间姿态空间姿态是指物体在三维空间中的位置和方向。
它是机器人、飞行器、卫星等自动控制系统中的重要参数之一，对于控制和导航具有至关重要的作用。
本文将从四个方面介绍空间姿态的计算方法。
一、欧拉角法欧拉角法是最常用的空间姿态表示方法之一。
它将物体的姿态分解为绕三个坐标轴的旋转角度，分别为俯仰角、滚转角和偏航角。
通过测量物体相对于参考坐标系的三个角度，可以计算出物体的空间姿态。
二、四元数法四元数法是一种更为紧凑和高效的空间姿态表示方法。
它使用四元数来表示物体的旋转姿态，其中包含一个实部和三个虚部。
四元数法能够避免万向锁问题，并且具有较好的数学性质，被广泛应用于航空航天领域。
三、旋转矩阵法旋转矩阵法是一种将物体的姿态表示为一个3×3的旋转矩阵的方法。
旋转矩阵可以描述物体相对于参考坐标系的旋转变换，通过矩阵运算可以得到物体的空间姿态。
旋转矩阵法具有直观性和易于计算的优点，被广泛应用于图像处理和计算机图形学领域。
四、四维时空法四维时空法是一种基于时空变换的空间姿态计算方法。
它将物体的姿态表示为一个四维向量，其中包含三个空间坐标和一个时间坐标。
通过对物体的时空变换进行测量和计算，可以得到物体的空间姿态。
四维时空法适用于高速运动物体的姿态计算，具有较好的准确性和稳定性。
除了以上四种常用的空间姿态计算方法，还有一些其他的方法，如奇异值分解法、李代数法等。
这些方法各有特点，可以根据具体应用的需求选择合适的方法进行空间姿态的计算。
在实际应用中，空间姿态的计算是自动控制系统中的一个重要环节。
它可以用于导航、目标跟踪、图像处理等多个领域。
例如，在飞行器中，通过计算飞行器的空间姿态，可以实现飞行器的稳定控制和姿态调整；在机器人中，通过计算机器人的空间姿态，可以实现机器人的定位和路径规划。
空间姿态的计算是自动控制系统中的重要内容。
欧拉角法、四元数法、旋转矩阵法和四维时空法是常用的空间姿态计算方法，它们各有特点，在不同的应用场景中有着广泛的应用。
刚体运动的四元素表示
刚体运动的四元素表示杨天标【摘要】刚体运动是旋转与平移的合成,文中主要讨论旋转,仅在应用实例中讨论平移的影响.旋转的表达形式很多,例如标准正交矩阵、Euler角、向量表达式、四元数等等.用四元数表达三维的旋转与使用矩阵相比具有计算简单和几何意义明确两大优点,四元数旋转可以避免Euler角旋转在某些情况下产生的自由度丧失.四元数方法在计算机仿真、图形图像、飞行力学、航空航天等领域应用广泛.讨论旋转的四元数形式及其与其它形式,主要是变换矩阵形式,之间的相互转化,并试图给出比较简单的转化算法.【期刊名称】《德州学院学报》【年(卷),期】2010(026)002【总页数】8页(P5-12)【关键词】四元数;向量;提升向量表示;三角表示;刚体运动;旋转;旋转矩阵;Euler角【作者】杨天标【作者单位】长江师范学院数学与计算机学院,重庆,408100【正文语种】中文【中图分类】O1平面上的旋转可以用复数表示.如图1所示的旋转,用矩阵表示为其中 P点坐标为(x,y),P′点坐标为(x′,y′).设 P的径向向量的长度ρ以及辐角α,可以求出P′点,然后利用和角公式,很容易得到上述矩阵表示.更简单的方法是把上述旋转是用复数表示z′=zeiθ,其中z,z′分别是P,P′的径向向量对应的复数,而复数eiθ就代表了旋转角度θ的变换.复数表示的另一个好处是,先后两次旋转的复数 z1、z2,就是复数z=z1·z2.复数乘法比矩阵乘法要简单一些.用矩阵表示一个3维旋转需要9个参数,参数的自由度是3.除了一些简单情况之外,矩阵表示没有明显的几何意义.能否用四元数表示3维旋转,正如复数表示2维旋转那样?答案是肯定的.复数可以表示平面的点与向量,为了寻找复数类似物,以便表示空间的向量,爱尔兰数学家哈密顿(Hamilton)奋斗了15年,终于在1843年发明了四元数[4].四元数代数涵盖了向量代数,实数、复数、向量都可以看作是四元数的特例,都可以统一地按四元数进行运算.文献[3]介绍了四元数的这些功能,讨论了四元数的定义、运算、性质、几何意义和它的3种表达形式.许多性质证明都是很简单的. 四元数代数是数学家哈密顿(W.R.Hamilton, 1805—1865)于1843年创立的,主要成果介绍在《四元数讲义》(1853)和两卷《四元数基础》(1866)中[3-5].1.1 四元数的定义四元数Q作为集合,是 q=w+xi+y j+zk的全体,其中w,x,y,z∈R,均为实数,q也表示为(w, x,y,z).在此集合上自然地定义加法‘+’与乘法‘·’运算.加法公式乘法公式可以把乘法形式地按 i,j,k的多项式乘法展开,在符号i,j,k上按照如下表规定的乘法规则运算四元数代数是实数域 R上的(4-维)线性空间(Q‘+’k·),(其中‘k·’表示数量乘法),并且指定了新的乘法q·q′,该乘法满足左右分配律,标量因子在乘法中可以自由结合.乘法具有结合律,但不具有交换律.四元数代数是结合代数,同时也是可除代数.四元数代数的这些性质可以直接验证,有些也可以采用技巧更简单地加以验证.详细讨论参见文献[3].复数域以自然的方式嵌入到四元数代数之中,即j,k分量置0.此时,可以直接验证,复数乘法与四元数乘法一致.不至混淆的情况下,乘号‘·’也常常省略,而且为了区别其他乘法,例如向量的内积,在以下讨论中一律省略乘号,或将其换为空格,即简单写在一起的两个四元数表示它们的四元数乘积,由于有结合律,多个四元数相乘,可以省略括号,但是注意,它们的位置顺序不可以更改,因为四元数乘法不具有交换律.例如(q1 q2)q3=q1(q2 q3)=q1 q2 q3≠q1 q3 q2.1.2 q的共轭与模长设q=w+xi+yj+zk,定义zk),称为 q的共轭 (四元数).,称为 q的模长.显然可以直接验证利用乘法公式可得由此可以推出对于标量q,|q|就是通常的绝对值.对于向量q=v,|q|就是|v|,即向量的模长.对于复数q=w+ ix,|q|就是复数的通常的模长.1.3 q的逆元与四元数除法设q=w+xi+y j+zk≠0,则满足qq-1=1,定义为 q的逆元,此时称 q为可逆.显然,q 可逆即q≠0.对于可逆四元数 q,r,由于 r-1 q-1(qr)=1,因此(qr)-1=r-1 q-1.除法 q/r 定义为qr-1.其中r≠0.对于标量q,q-1就是通常的1/q.对于复数q= w+ix,q-1就是复数的倒数1/q=q(w-ix)/(w2+ x2),对于向量q=v,q-1就是-v/|v|2.2.1 3维向量空间上的四元数乘法用一对复数表示四元数,并将四元数运算与复数运算联系起来.具体方法参见文献[28].适合本文内容的另一种表示方法是把q看成一个标量与一个向量之和,即q=w+v,其中 v=xi+yj+zk,是3维空间的向量,而 i,j,k表示标准正交标架的标架向量.可以把Q等同于R×V.q=w+v,w∈R,v∈V.称 w为q的标量分量,v为向量分量.这种表示方法称为q的提升向量表示.实数域通过令v=0而自然嵌入到四元数代数中,复数域通过 j,k分量置0而自然嵌入到四元数代数中,3-维向量空间通过标量分量置0而自然嵌入到四元数代数中.但是内积或者向量积并不简单等同于四元数乘积.因此,V中除了有两个基本的向量乘积运算,即数量积u·v和向量积u×v运算之外,还有四元数乘法运算.u ×v与四元数运算相似,如i×j=k,j×k=i,k×j= -i等等.但是i×i=0等说明两个运算还是有差别的.如果′的标量分量为0,那么四元数乘法′=vv′就导致V上一个新的运算,该运算可以由两个基本的向量运算表示该公式可以直接验证.如果v=v′,那么vv′= -|v|2.即v2=-|v|2.这一点与向量的内积有显著的区别.所以,不能够用 v2同时表示v·v与vv.本文中约定vk表示四元数乘法的幂.而v·v不再用v2表示.一般四元数乘法也可以由两个基本的向量运算表示.2.2 四元数乘法与向量乘积运算的相互表示由于vv′=v×v′-v·v′,v′v=v′×v-v′·v,因此向量乘积运算也可以用四元数乘法表示这样,向量的两种乘积运算就化归为四元数运算.另一方面,设v,v′∈V.则有ww′+w v′+vw′+v×v′-v·v′,故有公式其中w w′-v·v′是′的标量分量,w v′+vw′+ v×v′是′的向量分量.在向量空间中,v×v′与v·v′是两个不同性质的对象,其差是毫无意义的,但是嵌入到四元数代数中,向量与标量的加减法意义就很明白了,它们成为同一个四元数的不同分量.3.1 旋转变换通常旋转由绕 x,y,z轴依次旋转的角度roll, yaw,pitch给出.考虑一般的绕指定方向的轴线的旋转.如图2所示,径向向量 x绕过O以单位向量n为方向的直线旋转θ角转到达x′.由于向量的平移不变性,轴线不必固定过O点,可以简单认为向量x绕方向n旋转.易知x′=xp+cosθxv+sinθn×xv,其中xp=(x ·n)n是x在n的平行分量,xv=x-(x·n)n是x在n的垂直分量.于是由于n·xv=0,故即即上述公式说明,x′是x的垂直分量绕n旋转θ角之后与x的平行分量的合成.公式(5)即进一步把向量乘积运算转化为四元数运算,(5)式成为由于|n|=1,故 n-1=-n.因此有其中-nxn=-(xn+nx)n-x=2(x·n)n-x= (x·n)n-(x-(x·n)n),所以-nxn=xp-xv,是xp与xv之差,与 x关于n是轴对称的.(nx-xn)/2 =n×x,是 x的垂直分量旋转+90°的结果. 在特殊情况下,n⊥x,x·n=0,即 xn=-nx,此时nxn=x,故从(6)式得x′=(cosθ+nsinθ)x.由此可见,形如cosθ+sinθn的四元数nθ表示一个旋转,以n为旋转轴的方向,使得垂直于n的任何平面内的向量,当被nθ左乘时,按照右手法则旋转+θ角.q=r(cosθ+n sinθ)称为四元数的三角式,其中n为单位向量,它的几何意义是,qv表示对向量v绕方向n旋转θ角,然后长度扩大为 r倍.任何四元数q=w+v均可以表示为三角式q=r(cosθ+n sinθ),其中r=|q|,n=v/|v|,cosθ=w/|q|,sinθ=|v|/|q|.因此,任何四元数也都有同样的几何意义,都代表旋转与缩放变换.与复数类似,四元数也可以表示为指数形式,但是指数幂相加只有在旋转方向平行时才有意义,因此,指数形式并没有复数的指数形式那么重要.详细讨论见文献[3].当 x与n不垂直时,从(6)式可以看出,需要同时左乘与右乘某些四元数,才能实现对 x 的旋转.3.2 四元数的合同变换给定可逆元q∈Q,定义φ:Q→Q,φ(x)= qxq-1,称为四元数域上的合同变换.φ是齐次线性变换.由于|φ(x)|=|qxq-1|=|q||x||q-1|=|q| |x||q|-1=|x|,因此φ是保持长度不变的.如果 x只有标量分量,那么显然φ(x)=x.因此φ在标量域是恒等映射.若s+n 是x的提升向量表示,则φ(s+ n)=s+φ(n).该等式说明合同变换只影响向量分量.若s+n是q的提升向量表示,则即若q的向量分量n=0,则φ(x)=x,即q给出恒等变换.从(7)式容易看出,也只有这样的q才给出恒等变换.若 x的标量分量为0,则(7)有向量积表示即当q的标量分量s=0时,有比较简单的公式|n|2 φ(x)=2(n·x)n-|n|2 x=-nxn.即φ(x)=-nxn.3.3 旋转变换的四元数为了把向量表示的旋转表示为四元数的合同变换,取q=s+n,比较(6)式与(7)式,发现s2/|q|2= (1+cosθ)/2,s/|q|2=sinθ/2,1/|q|2=(1-cosθ)/ 2.由于|q|2=s2+1,因此,s=ctg(θ/2).即q=ctg (θ/2)+v.于是,绕单位向量n,旋转角度θ的旋转变换可以表示为四元数q=ctg(θ/2)+n的合同变换x’=φ(x)=qxq-1.我们称该旋转的四元数是 q.旋转的四元数不是唯一的.例如,如果φ(x)=qxq-1, r=aq,a≠0是任意标量,那么φ(x)=rxr-1.下面再找到一个四元数.大多文献中都使用这个形式的四元数.设q=a+bn,重复上面的过程,bn)/|q|2.展开得到即φ(x)用四元数运算表示为其中q=a+bn,|n|=1.比较(6)式得到其中|q|2=a2+b2.从而可以找到一个解为a= cos(θ/2),b=sin(θ/2).于是q=cos(θ/2)+sin(θ/2) n.q=cos(θ/2)+sin(θ/2)n称为(绕 n)旋转(角度θ)的四元数.qxq-1表示把向量 x绕n旋转角度θ.将前面的结果q=ctg(θ/2)+n直接乘以sin(θ/ 2),给出同样的结果.事实上,不同的q之间的差别也只能是一个非0标量.设对于任何 x,px p-1=qxq-1,那么,可以令x=q,于是,pqp-1=qqq-1,pqp-1=q,pq=qp,于是p,q交换.简单分析指出,p,q交换的等价条件是其向量分量平行.令q=s+v,不妨设v≠0.此时 p有形式t+av.在 px p-1=qxq-1中取 x=v,可以推出p=aq,或从公式(8)可以看出,φ(x)中受v的符号影响的只有2sv×x一项.通过取x为v的某个垂直的非零向量即可看出不可能成立.因此只有 p=aq,a是某个标量.例:一个圆柱体,轴线方向 v为z轴方向k,向量v绕y轴转动α角,再绕z轴转动β角,最后向着z轴转动α角,结果圆柱体回到了原来的位置,但同时也绕z轴自转了一个角度,问这个角度是多少?解:绕y轴转动α角的四元数p=cos(α/2)+ j sin(α/2),根据公式(8)容易算出v=k 旋转结果为绕z轴转动β角的四元数q=cos(β/2)+k sin(β/2), u旋转结果为其单位向量为绕n旋转α角的四元数计算乘积rq,三次旋转的合成s=rqp=q.故圆柱体回到了原来的位置时也绕z轴自转了一个角度β.最后一步,合成运动s的计算比较繁琐,但都是比较初等的运算.值得注意的是,三个不同的旋转轴实际上等效于一个,即等效于s=q的旋转轴.3.4 四元数的合同变换与旋转变换任何旋转x′=(x·n)n(1-cosθ)+cosθx+ sinθn×x,都有等效的四元数q=cos(θ/2)+n sin(θ/ 2).任给四元数q,x′=φ(x)=qxq-1是否总是旋转变换呢?如果是,它的矩阵是什么?下面讨论这些问题.由于φ(x)是保持长度不变的.又从φ(xy)= qxyq-1=qxq-1 qyq-1=φ(x)φ(y)可知φ保持四元数乘法不变.由于x·y,x×y都可通过(2)式,(3)式用 xy,y x线性表示,而且φ是齐次线性映射,因此φ保持向量的数量积不变.故φ保持向量的夹角不变.因此φ(x)总是旋转变换.其实,由于向量的数量积是标量,因此φ(x·y)=x·y是显然的.四元数旋转的等价向量表达式是公式(5),从(5)式可以很容易地得到四元数旋转的矩阵. 给定旋转变换的四元数q=cos(θ/2)+ n sin(θ/2),怎样确定该旋转的矩阵A?下面给出转换的程序.首先把qxq-1直接写成公式(5)的形式x′=(x·n)n(1-cosθ)+cosθx+sinθn×x.然后化公式(5)为矩阵形式.第一项对矩阵的贡献是其中(a1,a2,a3)=n.中间一项对矩阵的贡献是cosθE,其中E表示3 ×3单位阵.为了推导最后一项的贡献,指定与 i,j,k具有同样含义的符号e1,e2,e3.矩阵的t行元素可以有下面的等式确定,即x′·et=sinθn×x·et= sinθet×n·x,相应的矩阵是这是一个反对称.推导过程中使用了混合积的性质.最后得到旋转变换的矩阵为作为一个简单的例子,写出q=cosθ+isinθ的矩阵给定旋转变换的矩阵A,怎样确定该旋转的四元数q?下面给出转换的程序.首先把x′=A x写成(5)式的形式x′=(x·n)n (1-cosθ)+cosθx+sinθn×x,然后就可以直接写出它的一个四元数q=cos(θ/2)+n sin(θ/2).问题在于,一般情况下,x′=A x到(5)式的转换并不很简单.对于常用的旋转变换,即用 Euler角指定的变换,可以按如下方法处理.写出分别绕 x,y,z轴旋转角度 ,,的旋转变换以及相应得四元数依次为绕x轴旋转角度的旋转变换的矩阵为同样可以写出其它2个矩阵.合成变换矩阵就这样3个矩阵之积.可以比较容易地写出合成变换的四元数为q=q3 q2 q1.q的分量表示为q=w+v,其中把q写成如下三角形时,就可以得到等效旋转轴n与旋转角度θ.具体公式为q=|q|(cos(θ/2)+n sin(θ/2)),cos(θ/2)=w/ |q|,sin(θ/2)=|v|/|q|,n=v/|v|.这里一定成立|q|=1.一般情况下,要把矩阵形式换为四元数形式,把矩阵形式转化为向量形式(5)并不太容易.有一种方法不太实用:A的一个特征向量就是旋转轴的方向n,角度θ由A x·x确定,其中x⊥n,|x|=1,可以任意选取,但要尽量简单,例如取x=i×n,如果非0,再单位化即可.如果i×n=0,那么改 i为j,必有j×n≠0.例:试确定x′=A x的四元数,其中解:从|E-A|=0得到唯一的实特征值 =1.从得到特征向量 n=i+j.单位化为i×n=,故k⊥n,从k·A k=cosθ,k×A k =sinθn得到旋转角为θ.(仅k·A k=cosθ不能完全确定旋转角).故向量形式的变换公式为x′=(x·n)n(1-cosθ)+cosθx+sinθn×x,其中其四元数为还有一种值得推荐的方法,是任选2个(尽量简单的)向量 p,q,然后计算出 P=A p,Q=Aq.于是 n就是向量积u×v=(P-p)×(Q-q)的单位化.为了计算旋转角度,可以仅使用 p,计算出 p与A p的垂直分量,然后计算其间的角度,得到旋转角θ.仍然考虑上例,新的解法如下取 p=i,q=k,则u×v=(cosθ-1,cosθ-1,0),单位化为(1,1, 0)/√2,即n=(i+j)/√2.p对于n的垂直分量是pv=p-(p·n)n=i-(i+j)/2=(i-j)/2,P=A p对于n的垂直分量是 Pv=P-(P·n) n=(cosθ,-cosθ,-sinθ 2)/2,夹角设为α,则cosα=(pv·Pv)/|pv||Pv|= 2cosθ/2=cosθ.故α=θ.所求四元数为确定n时用第二种方法,而确定旋转角时使用前一种方法,似乎更好.第二种方法中,假如u//v怎么办?其实这时可以更简单地得到n.此时当u=0时,p=A p,把 p单位化就可以得到n.否则,u//v说明存在标量λ,使得v=λu,此即A(λp-q)=λp-q把λp-q单位化就可以得到n.选取 p,q时自然令它们独立.例如在上例中,如果取q=j,就会出现 u//v的情况.物体所在空间用一个标准正交标架描述,该标架称为世界坐标系,世界坐标系通常是四元组(o,i, j,k),但也可能相差一个刚体运动.确定物体 B在空间中的方位需要一个标准正交标架,称为物体的局部标架,所以物体的数学等价物是四元组B(P,e1,e2,e3),其中 P是选定的某点(见图3),称为物体的中心(点),ei是向量.现在想要解决的问题是,物体运动时,点的世界坐标与局部坐标之间怎样自由转换.在空间中,任意一点X=P+∑xi ei,其世界坐标为 xB.其中 x是 X的局部齐次坐标(x,y,z,1),为物体的世界矩阵,A是局部标架向量的矩阵,po是 P的世界坐标(xo,yo,zo).这种表示方法是用一种矩阵统一表述平移与旋转变换,只有刚体运动的矩阵,不再区分平移与旋转.关键在于计算B运动之后的世界矩阵.描述B的运动,需要知道 P的平移向量和局部标架的旋转矩阵.旋转的描述,更为直观的形式是四元数,因为通常只知道物体绕着另一个物体旋转,而不是绕着世界标架旋转,这另一个物体,就给出旋转方向 n,而旋转的四元数q=cos(θ/2)+n sin(θ/2).其中θ是旋转角度.例如, B向右运动10(公里),同时绕自己的向上的方向旋转60°,怎样确定物体的新的位置B′?这里位移与旋转都是相对于局部坐标系的,右向规定为 x轴方向.向上的方向规定为y轴正向.位置向量P′=P+ 10e1.旋转的四元数q=cos(π/6)+e2 sin(π/6).从 q写出变换后的世界矩阵的过程,可以设计程序自动完成B,其中i写成行向量形式(1,0,0), e1也是行向量形式,Q从q按照公式(9)计算,具体到这里的例子,(a1,a2,a3)是行向量形式的e2.此时局部坐标为 x的点的世界坐标是xB′.由于矩阵应用的普及程度很高,实际应用中四元数还是辅助性的,一般都要转换为矩阵.完全抛开矩阵,仅使用四元数是可能的,也更方便,而且不必在四元数与矩阵之间来回转换.【相关文献】[1]吕林根,许子道.解析几何(第四版)[M].北京:高等教育出版社,2006.[2]叶至军.Visual C++/DirectX9 3D游戏开发引导[M].北京:人民邮电出版社,2006.[3]刘俊峰.三维转动的四元数表述[J].大学物理,2004, (4).[4]程小红,宋玉靖.哈密尔顿与四元数[J].数学通讯, 2006,(9).[5]文瀚潮.从扩充新数的原则看哈米尔顿四元数[J].新疆教育学院学报(汉文版),1997,(1).[6]蒋德茂,吕强.四元数在骨骼蒙皮动画中的应用[J].苏州大学学报(自然科学版),2008,(2).[7]需晓.基于四元数插值的虚拟人运动平滑过渡研究[J].湖南工程学院学报(自然科学版),2008,(4).[8]任静丽,杨克俭.四元数在关键帧骨骼动画中的应用[J].电脑知识与技术,2006,(36).[9]周江华.四元数在刚体姿态仿真中的应用研究[J].飞行力学,2000,(4).[10]金小刚,彭群生.四元数及其在计算机动画中的应用[J].计算机辅助设计与图形学学报,1994,(3).[11]郑福.四元数矩阵实表示的基本性质及应用[J].数学的实践与认识,2009,(4).[12]袁仕芳.四元数体上广义 Toep litz矩阵反问题[J].吉首大学学报(自然科学版),2009,(1).[13]黄敬频,陈建飞.四元数矩阵实表示运算性质及应用[J].四川师范大学学报(自然科学版),2008,(2).[14]李小平.自共轭四元数矩阵的特征及迹不等式的两个充要条件[J].湘南学院学报,2008,(2).[15]于艳,黄敬频.一类四元数矩阵方程的反中心对称解及其最佳逼近[J].广西科学院学报,2008,(2).[16]周玉兴,黄敬频.关于四元数正则函数的两个充要条件[J].广西科学院学报,2008,(2).[17]吴文静.实四元数矩阵方程的广义反对称酉反对称解[J].合肥学院学报(自然科学版),2008,(2).[18]陈湘赟.四元数矩阵特征值的几个不等式[J].常熟理工学院学报,2008,(4).[19]奚传志.四元数矩阵的分解[J].四川师范大学学报(自然科学版),2006,(4).[20]程学汉.四元数多项式的因式分解[J].河南师范大学学报(自然科学版),2008,(4).[21]刘波.四元数矩阵广义逆的计算方法[J].科技信息(学术研究),2007,(35).[22]丰静,程学翰.四元数二次方程解的显式表示[J].湖南农业大学学报(自然科学版),2008,(3).[23]郎方年.四元数与彩色图像边缘检测[J].计算机科学, 2007,(11).[24]王军安.对基于四元数的飞机本体运动模型的改进[J].系统仿真学报,2006,(S2).[25]凌思涛,姜同松.四元数矩阵的次对角化(英文)[J].临沂师范学院学报,2006,(6).[26]周建华.2个四元数矩阵的同时对角化问题[J].Journal of Southeast University,2003,(2).[27]连德忠.四元数向量和矩阵的秩[J].数学研究,2003, (3).[28]李晟.四元数的矩阵形式[J].贵州教育学院学报, 2005,(2).。
欧拉角速度与姿态四元数角速度的关系
欧拉角速度与姿态四元数角速度的关系姿态控制是机器人领域中的重要问题之一，它涉及到机器人在空间中的姿态变化。
在姿态控制中，欧拉角和姿态四元数是常用的描述姿态的方法。
欧拉角由三个连续旋转角度组成，而姿态四元数是一种四维复数，它可以表示三维空间中的旋转。
欧拉角速度是指机器人在姿态变化过程中的角度变化速度。
它可以通过欧拉角的导数来计算。
而姿态四元数角速度是指机器人在姿态变化过程中的四元数变化速度。
它可以通过姿态四元数的导数来计算。
那么欧拉角速度与姿态四元数角速度之间有什么关系呢？下面我将详细介绍一下它们之间的关系。
我们来看欧拉角速度。
欧拉角速度的计算方法是将欧拉角分别对时间求导。
假设欧拉角分别为α、β、γ，对应的欧拉角速度分别为ωx、ωy、ωz。
那么欧拉角速度可以表示为：ωx = α'ωy = β'ωz = γ'其中，α'、β'、γ'分别表示α、β、γ的导数。
接下来，我们来看姿态四元数角速度。
姿态四元数的计算方法是将旋转轴和旋转角度转化为一个四元数。
假设姿态四元数为q = [qw, qx, qy, qz]，对应的姿态四元数角速度为ω = [ωw, ωx, ωy, ωz]。
那么姿态四元数角速度可以表示为：ωw = 0.5 * (ωx * qw + ωy * qz - ωz * qy)ωx = 0.5 * (ωy * qw - ωz * qx + ωw * qy)ωy = 0.5 * (ωz * qw + ωx * qy - ωw * qx)ωz = 0.5 * (-ωx * qz + ωy * qx + ωw * qz)其中，qw、qx、qy、qz分别表示姿态四元数的四个分量。
从上面的公式可以看出，姿态四元数角速度的计算涉及到姿态四元数和欧拉角速度的乘法和加法运算。
因此，欧拉角速度与姿态四元数角速度之间存在一定的关系。
总结起来，欧拉角速度与姿态四元数角速度之间的关系可以通过一系列的数学公式来表示。
三维旋转：旋转矩阵，欧拉角，四元数
三维旋转：旋转矩阵，欧拉⾓，四元数原⽂见我的，欢迎⼤家过去评论。
如何描述三维空间中刚体的旋转，是个有趣的问题。
具体地说，就是刚体上的任意⼀个点P(x, y, z)围绕过原点的轴(i, j, k)旋转θ，求旋转后的点P\'(x\', y\', z\')。
旋转矩阵旋转矩阵乘以点P的齐次坐标，得到旋转后的点P'，因此旋转矩阵可以描述旋转，$$\begin{bmatrix}x'\\ y'\\ z'\\ 1\end{bmatrix}=R\cdot \begin{bmatrix}x\\ y\\ z\\ 1\end{bmatrix}$$绕x，y，或z轴旋转θ的矩阵为：$$R_{x}(\theta)=\begin{bmatrix}1 & 0 & 0\\ 0 & \cos\theta & -\sin\theta\\ 0 & \sin\theta & \cos\theta\end{bmatrix}$$$$R_{y}(\theta)=\begin{bmatrix}\cos\theta & 0 & -\sin\theta\\ 0 & 1 & 0\\ \sin\theta & 0 & \cos\theta\end{bmatrix}$$$$R_{z}(\theta)=\begin{bmatrix}\cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1\end{bmatrix}$$所以，绕任意轴旋转的矩阵为$$R_{x}(-p)\cdot R_{y}(-q)\cdot R_{z}(\theta)\cdot R_{y}(q)\cdot R_{x}(p)$$这表⽰：1. 绕x轴旋转⾓度p使指定的旋转轴在xz平⾯上2. 绕y轴旋转⾓度q使指定的旋转轴与z轴重合3. 绕z轴旋转⾓度θ4. 绕y轴旋转⾓度-q5. 绕x轴旋转⾓度-p其中，p和q的值需要⽤i,j,k计算出来。
大话多旋翼飞行器--欧拉角与四元数
始先绕着 z-轴旋转 角值。然后，绕着 x-轴旋转 角值。最后，绕着 z-轴作角值 的
旋转” 我们通常使用的欧拉角是服从描述A）的。也就是绕着固定于刚体的坐标轴的三个旋转的复 合。为什么呢？因为对物体施加的转矩通常都是相对于物体自身的坐标轴的，控制上更直观 更容易。 另一个问题，欧拉角的三次旋转的旋转轴和旋转顺序。根据维基百科的说法，“在经典力 学里，时常用 zxz 顺规来设定欧拉角；照着第二个转动轴的轴名，简称为 x 顺规。另外， 还有别种欧拉角组。合法的欧拉角组中，唯一的限制是，任何两个连续的旋转，必须绕着 不同的转动轴旋转。因此，一共有 12 种顺规。例如，y 顺规，第二个转动轴是 y‐轴，时 常用在量子力学，核子物理学，粒子物理学。另外，还有一种顺规，xyz 顺规，是用在航空 航天工程学”。
a2通常用 表示，代表升降或俯仰（elevation or pitch）
a3通常用 表示，代表倾斜或横滚（bank or roll）
注意：有些书上a3用φ表示，其实 和φ是同一个字母的两种写法，本质是一样的。
小常识：
根据第一部分的介绍，可以知道，经过三个欧拉角转动后，世界坐标系下的一个矢量 rW=(xW,yW,zW)与其对应的运载体坐标系下的矢量rB=(xB,yB,zB)之间的关系可以表示为
个多体系统提供一个统一的参照坐标系。在物体上要建立一个局部坐标系，称为 BCS（Body Coordinate System），一方面用来描述物体在 GCS 内的位置和姿态，另一方面，为物体上的 点或其它坐标系提供局部的确定位置和姿态的标准。此外，在物体上可以根据需要建立其它 的坐标系，例如，为描述物体上的约束及列写约束方程，需要建立约束的坐标系。 //==========================================================================
四元数法求解姿态动力学方程
四元数法求解姿态动力学方程四元数是一种用来表示三维空间中旋转的数学工具。
它由一个实部和三个虚部组成，可以表示空间中的旋转角度和旋转轴向量。
在机器人和航空航天领域，四元数经常被用来描述姿态，并且能够方便地进行姿态之间的插值和计算。
姿态动力学方程描述了一个刚体在外力和外力矩的作用下的运动规律。
四元数法可以用来推导解姿态动力学方程。
假设刚体在时间t时的姿态为q(t)，在空间中角速度为ω(t)。
我们可以将刚体的姿态表示为：q(t)=[q0(t),q1(t),q2(t),q3(t)]其中，q0(t)为四元数的实部，q1(t)、q2(t)和q3(t)为四元数的虚部。
我们可以通过以下方程来描述刚体的姿态动力学方程：dq(t)/dt = 1/2 * ω(t) * q(t)其中，dq(t)/dt是四元数的导数，ω(t)是刚体的角速度，*表示四元数的乘法运算。
将q(t)展开，可以得到：dq0(t)/dt = -1/2 * (q1(t) * ω1(t) + q2(t) * ω2(t) + q3(t)* ω3(t))dq1(t)/dt = 1/2 * (q0(t) * ω1(t) - q3(t) * ω2(t) + q2(t) * ω3(t))dq2(t)/dt = 1/2 * (q3(t) * ω1(t) + q0(t) * ω2(t) - q1(t) * ω3(t))dq3(t)/dt = -1/2 * (q2(t) * ω1(t) - q1(t) * ω2(t) + q0(t) * ω3(t))其中，dq0(t)/dt表示四元数实部的导数，dq1(t)/dt、dq2(t)/dt和dq3(t)/dt表示四元数虚部的导数。
通过对上面的四个方程进行求解，我们就可以得到刚体在时间t时的姿态q(t)。
而ω(t)则可以通过刚体的运动学方程和动力学方程来求解。
四元数法求解姿态动力学方程的优势在于，与传统的欧拉角法相比，四元数法不会出现万向锁现象，可以避免在特定情况下姿态计算的不稳定性。
欧拉角与四元数
四元数与旋转一．四元组基础Q(x,y,z,w)，其中x，y，z用来确定旋转轴，w为旋转的角度Q=w+xi+yj+zk，i，j，k为三个虚轴的单位分量I*j=kJ*k=i;K*i=j;叉乘：c=a × b=| i j k||a1 b1 c1||a2 b2 c2|=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)c也为一个向量，且c的长度为|a||b|sin(theta)，垂直于a和b所在的平面，方向由右手法则来判定，用右手的四指先表示向量a的方向，然后手指朝着手心的方向摆动到向量b的方向，大拇指所指的方向就是向量c 的方向1．四元组相乘：Q1=w1+x1i+y1j+z1k=(w1,v1)Q2=w2+x2i+y2j+z2k=(w2,v2)Q1*Q2=(w1*w2-<v1,v2>,w1*v2+w2*v1+v1xv2)( w1+x1i+y1j+z1k)*( w2+x2i+y2j+z2k)=w1*w2-x1*x2-y1*y2-z1*z2+(W1*x2+x1*w2+y1*z2-z1-y2)i+(y1*w2+w1*y2+z1*x2-x1*z2)j+(w1*z2+z1*w2+x1*y2-y1*x2)k对于其中的轴部分，假如v1//v2，则有v1 x v2=0（平行向量的叉乘结果为0）2．四元组的点乘，点乘积为数值：Q1.*Q2=w1*w2+<v1,v2>=w1*w2+x1*x2+y1*y2+z1*z2;3．数乘s为一实数，q为四元组，则有sq=qs4．共轭p=(w,v)，则p*=(w,-v)(pq)*=q*p*N(q)=w2+x2+y2+z2q-1=q*/N(q)---------------显然可得qq-1=(1,0)二．使用四元数旋转向量假如有一表示向量的四元组q=(w,v)，对其应用旋转量p后的结果为：q’=pqp-1=(w,v’)从上可以看出，计算的结果q’的实部和q的实部是相等的，并且有N(v)=N(v’)如果N(q)=1，则可以令q=(cosa,usina)，u也为一个单位向量，则q’是q绕u旋转2a个弧度的结果假如S(q)表示q的实部，则有2S(q)=q+q*2S(pqp-1)= pqp-1+( pqp-1)*=pqp*+(pqp*)*=pqp*+pq*p*=p(q+q*)p*=2S(q)（这里由于p是单位四元数，所以有p-1等于p*）欧拉角到四元数的转换定义pitch, yaw, roll分别为绕X轴、Y轴、Z轴的旋转弧度float p = pitch * PIOVER180 / 2.0;float y = yaw * PIOVER180 / 2.0;float r = roll * PIOVER180 / 2.0;float sinp = sin(p);float siny = sin(y);float sinr = sin(r);float cosp = cos(p);float cosy = cos(y);float cosr = cos(r);this->x = sinr * cosp * cosy - cosr * sinp * siny;this->y = cosr * sinp * cosy + sinr * cosp * siny;this->z = cosr * cosp * siny - sinr * sinp * cosy;this->w = cosr * cosp * cosy + sinr * sinp * siny;normalise();三．使用matlab进行相关计算计算两个向量v1和v2之间的旋转量四元数p，使得v1应用p后到达v2假如v1转到v2的旋转轴为v，旋转角为theta，则q=[v*cos(theta/2)sin(theta/2)]Matlab代码：function q=vector2q(v1,v2)%..normalize....len1=sqrt(v1*v1');len2=sqrt(v2*v2');v1=v1/len1;v2=v2/len2;angle=v1*v2';axis=cross(v1,v2);alen=sqrt(axis*axis');axis=axis/alen;t=acos(angle);t=t/2;q(1)=axis(1)*sin(t);q(2)=axis(2)*sin(t);q(3)=axis(3)*sin(t);q(4)=cos(t);end计算出了q之后，可以获得对应的旋转矩阵，旋转矩阵的计算Matlab里面的矩阵是以列为主顺序的function r=q2rot(q)w=q(4);x=q(1);y=q(2);z=q(3);r=zeros(3,3);r(1,1)=1-2*y*y-2*z*z;r(1,2)=2*x*y+2*w*z;r(1,3)=2*x*z-2*w*y;r(2,1)=2*x*y-2*w*z;r(2,2)=1-2*x*x-2*z*z;r(2,3)=2*z*y+2*w*x;r(3,1)=2*x*z+2*w*y;r(3,2)=2*y*z-2*w*x;r(3,3)=1-2*x*x-2*y*y;r=r';end同时，也可以根据四元数来计算欧拉角function R=q2euler(q)w=q(4);x=q(1);y=q(2);z=q(3);t11=2*(w*x+y*z);t12=1-2*(x*x+y*y);R(1)=atan2(t11,t12);t2=2*(w*y-z*x);R(2)=asin(t2);t31=2*(w*z+x*y);t32=1-2*(y*y+z*z);R(3)=atan2(t31,t32);end计算出来的欧拉角rx，ry，rz，分别为绕X轴、Y轴和Z轴的旋转角，假如有：Rotq=q2rot(q)R=q2euler(q)[rotx roty rotz]=Rotation(R)可以发现Rotq==rotz*roty*rotx从这里可以看出，上面使用四元数这样计算出来的旋转矩阵的旋转顺序分别是X轴、Y轴和Z轴的ra=pi/4;qz=[0 0 -sin(ra) cos(ra)] %绕z旋转-90度qy=[0 sin(ra) 0 cos(ra) ] %绕y旋转90度qyz=qmult(qy,qz)r=q2euler(qyz)上面的r得出的结果为r = -1.5708 0.0000 -1.5708也就是说其几何意义变成先绕X轴旋转-90度，再绕Z轴旋转-90度，而根据qy和qz的相乘我们实际进行的操作却是先绕Z轴旋转-90度，再绕Y轴旋转90度，但是结果却是这两种操作等价，这说明由四元数到欧拉角可以有多个解两个四元数，假如它们的方向是相反的，用它们作用于向量得到的新向量的值仍然相等q1=[0.024666 -0.023954 0.504727 0.862594];arm=[-8.881719 6.037597 -2.36776];q2=-q1;rot1=q2rot(q1);rot2=q2rot(q2);v1=rot1*arm'v2=rot2*arm'上面计算出来的v1等于v2四元数的余弦值为它们的内积假如余弦值小于0，则需要将其中的一个取反，因为上面我们知道一个四元数和它的反方向的四元数对一个向量起相同的作用四元数的相乘，代表旋转的累积pq=p*q;rotp=q2rot(p);rotq=q2rot(q);rotpq=q2rot(pq);rotmul=rotp*rotq;这里rotpq与rotmul相等四． OGRE中Quaternion类的几个函数1.四元数到旋转向量void Quaternion::ToRotationMatrix (Matrix3& kRot) const1 - 2*qy2 -2*qz22*qx*qy -2*qz*qw2*qx*qz +2*qy*qw2*qx*qy + 2*qz*qw 1 - 2*qx2 -2*qz22*qy*qz -2*qx*qw2*qx*qz -2*qy*qw 2*qy*qz +2*qx*qw1 - 2*qx2 -2*qy22.旋转量到四元数根据1中的表格，有：4 *(1-qx2-qy2-qz2) = 1 + m00 + m11 + m22又qw2=1-qx2-qy2-qz2，可得4 *qw2= 1 + m00 + m11 + m22这里解qw必须保证1 + m00 + m11 + m22>=0，如果不是的话，就构造其他的等式来计算，OGRE中分成两种情况，一种是m00 + m11 + m22>=0，就可以直接先解出qw，否则的采用另外的等式计算3．Local axisVector3 xAixs(void) const;取得旋转矩阵的第一列，旋转矩阵和一个向量相乘的话，第一列的数据均和向量的x分量相乘Vecotr3 yAxis(void) const;取得旋转矩阵的第二列，旋转矩阵和一个向量相乘的话，第二列的数据均和向量的y分量相乘Vecotr3 zAxis(void) const;取得旋转矩阵的第三列，旋转矩阵和一个向量相乘的话，第三列的数据均和向量的z分量相乘。
多体动力学系统的四元数表示及其保辛积分算法
多体动力学系统的四元数表示及其保辛积分算法【多体动力学系统的四元数表示及其保辛积分算法】1. 导言多体动力学系统是指由多个质点组成的系统，在受到外力作用时，各个质点之间存在相互作用。
研究多体动力学系统的数学描述和数值模拟一直是一个重要的课题。
本文将从四元数表示和保辛积分算法两个方面对多体动力学系统进行深入探讨，帮助读者更全面地理解这一复杂的系统。
2. 多体动力学系统的数学描述在研究多体动力学系统时，我们需要对系统的状态进行描述。
常用的方法包括欧拉角、旋转矩阵等，但这些方法在描述多自由度系统时存在一定的局限性。
为了克服这些局限性，引入四元数表示是一种非常有效的方法。
2.1 四元数的定义和性质四元数是一种超复数，可以用一个实部和三个虚部来表示。
一般形式为q = w + xi + yj + zk，其中w、x、y、z分别是实数，i、j、k是虚部，满足关系i²=j²=k²=ijk=-1。
四元数具有良好的性质，包括单位四元数的乘法仍为单位四元数，四元数的共轭和模长等。
2.2 四元数在多体动力学系统中的应用在多体动力学系统中，可以将刚体的旋转用四元数来表示。
这种表示方法不仅能够避免奇点问题，而且还能够减小计算量，提高计算效率。
四元数表示在多体动力学系统中得到了广泛的应用。
通过四元数表示，我们可以更加直观地理解多体系统的运动状态，为进一步的数值模拟奠定基础。
3. 保辛积分算法在多体动力学系统中的应用在数值模拟多体动力学系统时，保持系统的物理结构和守恒量是非常重要的。
保辛积分算法就是一种能够保持系统哈密顿量守恒的数值积分方法。
3.1 保辛积分算法的基本原理保辛积分算法是一种辛格式的数值积分方法，其基本原理是在每个时间步内保持系统的哈密顿量守恒。
通过适当选择数值格式和积分步长，可以保证系统的长期稳定性和数值精度。
在多体动力学系统中，采用保辛积分算法能够有效减小数值误差，保持系统的物理结构，对于长时间的数值模拟非常重要。
基于四元数的姿态解算器或欧拉角解算器算法
基于四元数的姿态解算器或欧拉角解算器算法四元数姿态解算器和欧拉角姿态解算器都是用于表示三维空间中的旋转。
四元数是一种扩展了复数的数学概念，可以表示三维空间中的旋转，而欧拉角是一种用三个角度表示旋转的方法。
这两种方法都可以用于计算物体在三维空间中的姿态。
1. 四元数姿态解算器算法：
四元数由一个实部和一个虚部组成，可以表示为q = w + xi + yj + zk，其中w、x、y、z是实数，i、j、k是虚数单位。
四元数的运算包括加法、减法、乘法和共轭等。
四元数姿态解算器的算法步骤如下：
a) 初始化四元数q = [1, 0, 0, 0]，表示初始时刻物体的姿态。
b) 读取陀螺仪的角速度数据，将其转换为四元数形式。
c) 使用四元数乘法更新物体的姿态。
d) 将更新后的四元数转换为欧拉角，以便进行其他计算或显示。
2. 欧拉角姿态解算器算法：
欧拉角是用三个角度表示旋转的方法，通常包括绕x轴的滚动角（roll）、绕y轴的俯仰角（pitch）和绕z轴的偏航角（yaw）。
欧拉角的运算包括加法、减法和乘法等。
欧拉角姿态解算器的算法步骤如下：
a) 初始化欧拉角θ = [0, 0, 0]，表示初始时刻物体的姿态。
b) 读取陀螺仪的角速度数据，将其转换为欧拉角形式。
c) 使用欧拉角乘法更新物体的姿态。
d) 将更新后的欧拉角用于其他计算或显示。
需要注意的是，欧拉角在某些情况下可能会出现万向节死锁（gimbal lock）现象，这时需要使用四元数来表示旋转。
而在实际应用中，通常会将四元数和欧拉角结合起来使用，以便在不同场景下进行灵活切换。
欧拉角与四元数
四元数与旋转一．四元组基础Q(x,y,z,w)，其中x，y，z用来确定旋转轴，w为旋转的角度Q=w+xi+yj+zk，i，j，k为三个虚轴的单位分量I*j=kJ*k=i;K*i=j;叉乘：c=a × b=| i j k||a1 b1 c1||a2 b2 c2|=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)c也为一个向量，且c的长度为|a||b|sin(theta)，垂直于a和b所在的平面，方向由右手法则来判定，用右手的四指先表示向量a的方向，然后手指朝着手心的方向摆动到向量b的方向，大拇指所指的方向就是向量c的方向1．四元组相乘：Q1=w1+x1i+y1j+z1k=(w1,v1)Q2=w2+x2i+y2j+z2k=(w2,v2)Q1*Q2=(w1*w2-<v1,v2>,w1*v2+w2*v1+v1xv2)( w1+x1i+y1j+z1k)*( w2+x2i+y2j+z2k)=w1*w2-x1*x2-y1*y2-z1*z2+(W1*x2+x1*w2+y1*z2-z1-y2)i+(y1*w2+w1*y2+z1*x2-x1*z2)j+(w1*z2+z1*w2+x1*y2-y1*x2)k对于其中的轴部分，假如v1//v2，则有v1 x v2=0（平行向量的叉乘结果为0）2．四元组的点乘，点乘积为数值：Q1.*Q2=w1*w2+<v1,v2>=w1*w2+x1*x2+y1*y2+z1*z2;3．数乘s为一实数，q为四元组，则有sq=qs4．共轭p=(w,v)，则p*=(w,-v)(pq)*=q*p*N(q)=w2+x2+y2+z2q-1=q*/N(q)--------------- 显然可得qq-1=(1,0)二．使用四元数旋转向量假如有一表示向量的四元组q=(w,v)，对其应用旋转量p后的结果为：q’=pqp-1=(w,v’)从上可以看出，计算的结果q’的实部和q的实部是相等的，并且有N(v)=N(v’)如果N(q)=1，则可以令q=(cosa,usina)，u也为一个单位向量，则q’是q绕u旋转2a个弧度的结果假如S(q)表示q的实部，则有2S(q)=q+q*2S(pqp-1)= pqp-1+( pqp-1)*=pqp*+(pqp*)*=pqp*+pq*p*=p(q+q*)p*=2S(q)（这里由于p是单位四元数，所以有p-1等于p*）欧拉角到四元数的转换定义pitch, yaw, roll分别为绕X轴、Y轴、Z轴的旋转弧度float p = pitch * PIOVER180 / 2.0;float y = yaw * PIOVER180 / 2.0;float r = roll * PIOVER180 / 2.0;float sinp = sin(p);float siny = sin(y);float sinr = sin(r);float cosp = cos(p);float cosy = cos(y);float cosr = cos(r);this->x = sinr * cosp * cosy - cosr * sinp * siny;this->y = cosr * sinp * cosy + sinr * cosp * siny;this->z = cosr * cosp * siny - sinr * sinp * cosy;this->w = cosr * cosp * cosy + sinr * sinp * siny;normalise();三．使用matlab进行相关计算计算两个向量v1和v2之间的旋转量四元数p，使得v1应用p后到达v2假如v1转到v2的旋转轴为v，旋转角为theta，则q=[v*cos(theta/2) sin(theta/2)] Matlab代码：function q=vector2q(v1,v2)%..normalize....len1=sqrt(v1*v1');len2=sqrt(v2*v2');v1=v1/len1;v2=v2/len2;angle=v1*v2';axis=cross(v1,v2);alen=sqrt(axis*axis');axis=axis/alen;t=acos(angle);t=t/2;q(1)=axis(1)*sin(t);q(2)=axis(2)*sin(t);q(3)=axis(3)*sin(t);q(4)=cos(t);end计算出了q之后，可以获得对应的旋转矩阵，旋转矩阵的计算Matlab里面的矩阵是以列为主顺序的function r=q2rot(q)w=q(4);x=q(1);y=q(2);z=q(3);r=zeros(3,3);r(1,1)=1-2*y*y-2*z*z;r(1,2)=2*x*y+2*w*z;r(1,3)=2*x*z-2*w*y;r(2,1)=2*x*y-2*w*z;r(2,2)=1-2*x*x-2*z*z;r(2,3)=2*z*y+2*w*x;r(3,1)=2*x*z+2*w*y;r(3,2)=2*y*z-2*w*x;r(3,3)=1-2*x*x-2*y*y;r=r';end同时，也可以根据四元数来计算欧拉角function R=q2euler(q)w=q(4);x=q(1);y=q(2);z=q(3);t11=2*(w*x+y*z);t12=1-2*(x*x+y*y);R(1)=atan2(t11,t12);t2=2*(w*y-z*x);R(2)=asin(t2);t31=2*(w*z+x*y);t32=1-2*(y*y+z*z);R(3)=atan2(t31,t32);end计算出来的欧拉角rx，ry，rz，分别为绕X轴、Y轴和Z轴的旋转角，假如有：Rotq=q2rot(q)R=q2euler(q)[rotx roty rotz]=Rotation(R)可以发现Rotq==rotz*roty*rotx从这里可以看出，上面使用四元数这样计算出来的旋转矩阵的旋转顺序分别是X轴、Y轴和Z轴的ra=pi/4;qz=[0 0 -sin(ra) cos(ra)] %绕z旋转-90度qy=[0 sin(ra) 0 cos(ra) ] %绕y旋转90度qyz=qmult(qy,qz)r=q2euler(qyz)上面的r得出的结果为r = -1.5708 0.0000 -1.5708也就是说其几何意义变成先绕X轴旋转-90度，再绕Z轴旋转-90度，而根据qy和qz的相乘我们实际进行的操作却是先绕Z轴旋转-90度，再绕Y轴旋转90度，但是结果却是这两种操作等价，这说明由四元数到欧拉角可以有多个解两个四元数，假如它们的方向是相反的，用它们作用于向量得到的新向量的值仍然相等q1=[0.024666 -0.023954 0.504727 0.862594];arm=[-8.881719 6.037597 -2.36776];q2=-q1;rot1=q2rot(q1);rot2=q2rot(q2);v1=rot1*arm'v2=rot2*arm'上面计算出来的v1等于v2四元数的余弦值为它们的内积假如余弦值小于0，则需要将其中的一个取反，因为上面我们知道一个四元数和它的反方向的四元数对一个向量起相同的作用四元数的相乘，代表旋转的累积pq=p*q;rotp=q2rot(p);rotq=q2rot(q);rotpq=q2rot(pq);rotmul=rotp*rotq;这里rotpq与rotmul相等四．OGRE中Quaternion类的几个函数1.四元数到旋转向量void Quaternion::ToRotationMatrix (Matrix3& kRot) const1 - 2*qy2 - 2*qz22*qx*qy - 2*qz*qw2*qx*qz + 2*qy*qw2*qx*qy + 2*qz*qw 1 - 2*qx2 - 2*qz22*qy*qz - 2*qx*qw2*qx*qz - 2*qy*qw2*qy*qz + 2*qx*qw 1 - 2*qx2 - 2*qy22.旋转量到四元数根据1中的表格，有：4 *(1-qx2-qy2-qz2) = 1 + m00 + m11 + m22又qw2=1-qx2-qy2-qz2，可得4 *qw2= 1 + m00 + m11 + m22这里解qw必须保证1 + m00 + m11 + m22>=0，如果不是的话，就构造其他的等式来计算，OGRE中分成两种情况，一种是m00 + m11 + m22>=0，就可以直接先解出qw，否则的采用另外的等式计算3．Local axisVector3 xAixs(void) const;取得旋转矩阵的第一列，旋转矩阵和一个向量相乘的话，第一列的数据均和向量的x分量相乘Vecotr3 yAxis(void) const;取得旋转矩阵的第二列，旋转矩阵和一个向量相乘的话，第二列的数据均和向量的y分量相乘Vecotr3 zAxis(void) const;取得旋转矩阵的第三列，旋转矩阵和一个向量相乘的话，第三列的数据均和向量的z分量相乘。
刚体在三维空间的旋转（关于旋转矩阵、DCM、旋转向量、四元数、欧拉角）
刚体在三维空间的旋转（关于旋转矩阵、DCM、旋转向量、四元数、欧拉角）最近学习了一些关于三维空间旋转相关的知识，借此梳理一下备忘。
三维空间的旋转(3D Rotation)是一个很神奇的东东：如果对某个刚体在三维空间进行任意次的旋转，只要旋转中心保持不变，无论多少次的旋转都可以用绕三维空间中某一个轴的一次旋转来表示。
表示三维空间的旋转有多种互相等价的方式，常见的有旋转矩阵、DCM、旋转向量、四元数、欧拉角等。
本篇文章主要梳理一下这些表示方式及相互转换的方法。
1. 欧拉角(Euler Angle)最直观的表示方式是绕刚体自身的X、Y、Z三个轴分别进行旋转某个角度，这就是所谓的欧拉角(Euler Angle)表示方式。
Euler Angle需要注意的是，欧拉角的表示方式里，yaw、pitch、roll的顺序对旋转的结果是有影响的。
给定一组欧拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转，最终刚体的朝向是不同的！换言之，若刚体需要按照两种不同的旋转顺序旋转到相同的朝向，所需要的欧拉角角度值则是不同的！另外需要注意的是，在欧拉角的表示方式里，三个旋转轴一般是随着刚体在运动，即wikipedia中所谓的intrinsic rotation，见下图动画所示(图来自wikipedia)。
相对应的另一种表示方式是，三个旋转轴是固定的，不随刚体旋转而旋转，即extrinsic rotation，这种表示方式在计算机视觉中不是很常用。
欧拉角的表示方式比较直观，但是有几个缺点：(1) 欧拉角的表示方式不唯一。
给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。
比如，同样的yaw-pitch-roll顺序，(0,90,0)和(90,90,90)会将刚体转到相同的位置。
浅析欧拉角与四元数
Z
p'
YБайду номын сангаас
用另一个四元数表示旋转：
X
p
Machine Perception and Interaction Group (MPIG)
欧拉角转四元数
设三次旋转对应的四元数分别为： 则： 绕x轴单位向量(1, 0, 0)旋转角度φ 绕y轴单位向量(1, 0, 0)旋转角度θ 绕z轴单位向量(1, 0, 0)旋转角度ψ
四元数的基本性质
3. 共轭
4. 模长
5.两个四元数乘积的模即为模的乘积， 这保证单位四元数 相乘后仍是单位四元数。
Machine Perception and Interaction Group (MPIG)
四元数的基本性质
6. 逆
（1） 四元数和自己的逆的乘积为实四元数1：
MPIG Seminar 0048
欧拉角和四元数
郑雪鹤
Machine Perception and Interaction Group (MPIG)
zxh@
MPIG Seminar 0048
郑雪鹤
Machine Perception and Interaction Group (MPIG)
四元数表示旋转
假设某个旋转是绕单位向量：
则描述该转动的四元数可以表示成:
反之，我们亦可通过任意一个长度为1的四元数，计算对应旋转轴与夹角
右手法则旋转
Machine Perception and Interaction Group (MPIG)
四元数表示旋转
用一个虚四元数来描述一个三维空间点：
( y)
( x)
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
摘 要 ：为推 广 四元 数保 辛积 分在 工程 中的应 用 ， 对 欧拉 角表 示 的状 态方程 数 值 积 分与 四 元数 的保
辛积 分进 行 比较 ． 重陀螺 的数 值仿 真结 果表 明四元数 保 辛 积 分 的数 值 结果 明显优 于欧拉 角状 态方
程 积分 ． 与 欧拉 角状 态方程 积 分相 比 ， 四元数 保 辛积 分在 刚体 动力 学 的数 值仿 真 中更 具优 势．
… ’ ‘ ● ・ ｎ Ｊ ● １ 一 ’
Ｄ ａ ｌ ｉ ａ ｎ Ｕ ｎ ｉ ｖ ｅ ｒ ｓ ｉ ｔ ｙ ｏ ｆ Ｔ ｅ ｃ ｈ ｎ ｏ ｌ ｏ ｇ ｙ ， Ｄ ａ ｌ ｉ ａ ｎ １ １ ６ ０ ２ ４， Ｌ ｉ ａ ｏ ｎ ｉ ｎ ｇ ， Ｃ ｈ ｉ ｎ ａ ）
ＤＯＩ ： １ ０ ． １ ３ ３ ４ ０ ／ ｊ ． ｃ ａ ｅ ． ２ ０ １ ４ ． ０ １ ． Ｏ １ ２
四元 数 与 欧 拉 角 刚体 动 力 学 数 值 积 分 算 法 及 其 比较
徐 小 明 ， 钟 万勰 ’
（ 大连 理工大学 ａ ． 工程 力学 系； ｂ ． 工业装备 结构分析 国家重点 实验 室， 辽 宁 大连 １ １ ６ ０ ２ ４ ）
Ａｂｓ ｔ ｒ ａ ｃ ｔ ：Ｔｏ ｐ ｒ ｏ ｍｏ ｔ ｅ ｔ ｈ ｅ ａ ｐ ｐ ｌ ｉ ｃ ａ ｔ ｉ ｏ ｎ ｏ ｆ ｓ ｙ ｍｐ ｌ ｅ ｃ ｔ ｉ ｃ ｐ ｒ ｅ ｓ ｅ ｒ ｖ ａ ｔ ｉ ｏ ｎ ｉ ｎ ｔ ｅ ｇ ｒ ａ ｔ ｉ ｏ ｎ ｏ ｆ ｑ ｕ ａ ｔ ｅ ｒ ｎ ｉ ｏ ｎ ｉ ｎ ｅ ｎ ｇ ｉ ｎ ｅ ｅ ｒ ｉ ｎ ｇ，
ｐ ｒ ｅ ｓ ｅ ｖａ ｒ ｔ ｉ ｏ ｎ ｉ ｎ ｔ ｅ ｇ ｒ ａ ｔ ｉ ｏ ｎ ｏ ｆ ｑ ｕ ａ ｔ ｅ ｍｉ ｏ ｎ． Ｔｈ ｅ ｎ ｕ ｍｅ ｉｃ ｒ ａ ｌ ｓ ｉ ｍｕ ｌ ａ ｔ ｉ ｏ ｎ ｒ ｅ ｓ ｕ ｌ ｔ ｓ ｏ ｆ ｈｅ ａ ｖ ｙ ｔ ｏ ｐ ｓ ｈ ｏ ｗ ｔ ｈ ａ ｔ ｔ ｈｅ ｎ ｕ ｍｅ ｒ ｉ ｃ ａ ｌ ｒ ｅ ｓ ｕ ｌ ｔ ｏ ｆ ｓ ｙ ｍｐ ｌ ｅ ｃ ｔ ｉ ｃ ｐ ｒ ｅ ｓ ｅ ｒ ｖ ａ ｔ ｉ ｏ ｎ ｉ ｎ ｔ ｅ ｇ ｒ ａ ｔ ｉ ｏ ｎ ｏ ｆ ｑ ｕａ ｔ ｅ ｒ ｎ ｉ ｏ ｎ ｉ ｓ ｍｕ ｃ ｈ ｂ ｅ ｔ ｔ ｅ ｒ ｔ ｈ ａ ｎ ｔ ｈ ａ ｔ ｏ ｆ ｔ ｈ ｅ ｓ ｔ ａ ｔ ｅ
ｒ ｉ ｇ ｉ ｄ ｄｙｎａ ｍ ｉ ｃ ｓ ｉ ｎ ｔ ｅ ｒ ｍｓ ０ ｔ ａ Ｕａｔ ｅ ｒ ｎｌ ０ ｎ ａ ｎｄ Ｅ ｕｌ ｅ ｒ ａｎＲｌ ｅ
ＸＵ Ｘｉ ａ ｏ ｍｉ ｎ ｇ 一．ＺＨＯＮＧ Ｗａ ｎ ｘ ｉ ｅ ，
（ ａ ． Ｄ ｅ ｐ ａ ｒ ｔ ｍ ｅ ｎ ｔ ｏ ｆ Ｅ ｎ ｇ ｉ ｎ ｅ ｅ ｉ ｒ ｎ ｇ Ｍｅ ｃ ｈ ａ ｎ ｉ ｃ ｓ ；ｂ ． Ｓ ｔ ａ ｔ ｅ Ｋ ｅ ｙ Ｌ ａ ｂ ｏ ｒ ａ ｔ ｏ ｒ ｙ ｏ ｆ Ｓ ｔ ｒ ｕ ｃ ｔ ｕ ｒ ａ ｌ Ａ ｎ ａ ｌ ｙ ｓ ｉ ｓ ｆ ｏ ｒ Ｉ ｎ ｄ ｕ ｓ ｔ ｉ ｒ ａ ｌ Ｅ ｑ ｕ ｉ ｐ ｍ ｅ ｎ ｔ ，
ｔ ｈ ｅ ｎ ｕｍｅ ｒ ｉ ｃ ａ ｌ ｉ ｎ ｔ ｅ ｇ ｒ ａ ｔ ｉ ｏ ｎ ｏ ｆ ｓ ｔ ａ ｔ ｅ ｅ ｑ ｕ ａ ｔ ｉ ｏ ｎ ｒ ｅ ｐ ｒ ｅ ｓ ｅ ｎ ｔ ｅ ｄ ｂ ｙ Ｅｕ ｌ ｅ ｒ ａ ｎ ｇ ｌ ｅ ｉ ｓ ｃ ｏ ｍｐ ａ ｒ ｅ ｄ ｗｉ ｔ ｈ ｓ ｙ ｍｐ ｌ ｅ ｃ ｔ ｉ ｃ
ｅ ｑ ｕａ ｔ ｉ ｏ ｎ ｉ ｎ ｔ ｅ ｇ ｒ ａ ｔ ｉ ｏ ｎ ｏ ｆ Ｅｕ ｌ ｅ ｒ ａ ｎｇ ｌ ｅ ． Ｃｏ ｍｐ ａ ｒ ｅ ｄ ｗｉ ｔ ｈ ｔ ｈ ｅ ｓ ｔ ａ ｔ ｅ ｅ ｑ ｕ ａ ｔ ｉ ｏ ｎ ｉ ｎ ｔ ｅ ｇ ｒ ａ ｔ ｉ ｏ ｎ ｏ ｆ Ｅｕ ｌ ｅ ｒ ａ ｎ ｇ ｌ ｅ，ｔ ｈ ｅ ｓ ｙ ｍｐｌ ｅ ｃ ｔ ｉ ｃ ｐ ｒ ｅ ｓ ｅ ｖａ ｒ ｔ ｉ ｏ ｎ ｉ ｎ ｔ ｅ ｇ ｒ ａ ｔ ｉ ｏ ｎ ｏ ｆ ｑ ｕ ａ ｔ ｅ ｒ ｎ ｉ ｏ ｎ ｈ ａ ｓ ｍｏ ｒ ｅ ａ ｄ ｖ ａ ｎ ｔ ａ ｇ ｅ ｓ ｉ ｎ ｔ ｈｅ ｎｕ ｍｅ ｉｃ ｒ ａ ｌ ｓ ｉ ｍｕ ｌ ａ ｔ ｉ ｏ ｎ ｆ ｏ ｒ ｉｇ ｒ ｉ ｄ ｂ ｏ ｄ ｙ ｄ ｙ ｎ ａ ｍｉ ｃ ｓ ． Ｋｅ ｙ ｗｏｒ ｄｓ：ｑ ｕ ａ ｔ ｅ ｒ ｎ ｉ ｏ ｎ；Ｅｕ ｌ ｅ ｒ ａ ｎ ｇ ｌ ｅ；ｒ ｉ ｇ ｉ ｄ ｄ ｙ ｎ ａ ｍｉ ｃ ｓ ；ｓ ｙ ｍｐ ｌ ｅ ｃ ｔ ｉ ｃ ｐｒ ｅ ｓ ｅ ｖａ ｒ ｔ ｉ ｏ ｎ；ｈ ｅ ａ ｖ ｙ ｔ ｏ ｐ
关键 词 ：四元数 ；欧拉 角 ； 刚 体动 力 学 ； 保辛 １ ． ９ ；０ ３ １ ３ ． ３ 文 献标 志码 ： Ａ
Ｎｕ ｍｅ ｒ ｉ ｃ ａ ｌ ｉ ｎ ｔ ｅ ｇ ｒ ａ ｔ ｉ ｏ ｎ ａ ｌ ｇ ｏ ｒ ｉ ｔ ｈ ｍｓ ａ ｎ ｄ ｃ ｏ ｍ ｐａ ｒ ｉ ｓ ｏ ｎ ｆ ｏ ｒ
第２ ３卷 第 １ 期
２ ０ １ ４年 ２月
计 算 机 辅 助 工 程
Ｃｏ ｍｐ ｕ ｔ ｅ ｒ Ａｉ ｄｅ ｄ Ｅｎｇ ｉ ｎ ｅ ｅ ｉｎ ｒ ｇ
Ｖｏ ｌ ＿ ２ ３ Ｎｏ ．１ Ｆｅ ｂ． ２ ０１ ４
文章编号 ： １ ０ ０ ６—０ ８ ７ １ （ ２ ０ １ ４） ０ １ ０ ０ ５ ９ — ０ ５