转：https://www.cnblogs.com/21207-iHome/p/6894128.html
- RPY角与Z-Y-X欧拉角
描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。第一种是绕固定（参考）坐标轴旋转：假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转γγ，然后绕{A}的Y轴旋转ββ，最后绕{A}的Z轴旋转αα，就能旋转到当前姿态。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。
Roll:横滚
Pitch: 俯仰
Yaw: 偏航（航向）
另一种姿态描述方式是绕自身坐标轴旋转：假设开始两个坐标系重合，先将{B}绕自身的Z轴旋转αα，然后绕Y轴旋转ββ，最后绕X轴旋转γγ，就能旋转到当前姿态。称其为Z-Y-X欧拉角，由于是绕自身坐标轴进行旋转，则旋转矩阵为：
可以发现这两种描述方式得到的旋转矩阵是一样的，即绕固定坐标轴X-Y-Z旋转(γ,β,α)(γ,β,α)和绕自身坐标轴Z-Y-X旋转(α,β,γ)(α,β,γ)的最终结果一样，只是描述的方法有差别而已。In gerenal: three rotations taken about fixed axes yield the same final orientation as the same three rotations taken in opposite order about the axes of the moving frame.
- Axis-Angle与四元数
且有x2+y2+z2+w2=1x2+y2+z2+w2=1
即四元数存储了旋转轴和旋转角的信息，它能方便的描述刚体绕任意轴的旋转。
四元数转换为旋转矩阵：
则对应的四元数为：
- 四元数与欧拉角的相互转换
定义两个四元数：
其中
四元数加法：
跟复数、向量和矩阵一样，两个四元数之和需要将不同的元素加起来。
加法遵循实数和复数的所有交换律和结合律。
四元数乘法：
四元数的乘法的意义类似于矩阵的乘法，可以表示旋转的合成。当有多次旋转操作时，使用四元数可以获得更高的计算效率。
由于四元数乘法的非可换性，pq并不等于qp，qp乘积的向量部分是：
Mathematica中有四元数相关的程序包Quaternions Package，需要先导入才能使用。下面计算了三个四元数的乘积：
计算结果为：Quaternion[-12, 4, 14, 2]
那么将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转αα,ββ,γγ角）转换为四元数：
atan2函数代替arctan函数：
四元数转换为欧拉角可以参考下面的代码。需要注意欧拉角有12种旋转次序，而上面推导的公式是按照Z-Y-X顺序进行的，所以有时会在网上看到不同的转换公式（因为对应着不同的旋转次序），在使用时一定要注意旋转次序是什么。比如ADAMS软件里就默认Body 3-1-3次序，即Z-X-Z欧拉角，而VREP中则按照X-Y-Z欧拉角旋转。
enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx}; 
上面的代码存在一个问题，即奇异性没有考虑。下面看一种特殊的情况（参考Maths - Conversion Quaternion to Euler）：假设一架飞机绕Y轴旋转了90°（俯仰角pitch=90），机头垂直向上，此时如何计算航向角和横滚角？
这时会发生自由度丢失的情况，即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题：
当俯仰角为-90°，即机头竖直向下时的情况也与之类似，可以推导出奇异姿态时的计算公式。比较完整的四元数转欧拉角（Z-Y-X order）的代码如下：
CameraSpacePoint QuaternionToEuler(Vector4 q) // Z-Y-X Euler angles { CameraSpacePoint euler = { 0 }; const double Epsilon = 0.0009765625f; const double Threshold = 0.5f - Epsilon; double TEST = q.w*q.y - q.x*q.z; if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90° { int sign = Sign(TEST); euler.Z = -2 * sign * (double)atan2(q.x, q.w); // yaw euler.Y = sign * (PI / 2.0); // pitch euler.X = 0; // roll } else { euler.X = atan2(2 * (q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z); euler.Y = asin(-2 * (q.x*q.z - q.w*q.y)); euler.Z = atan2(2 * (q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z); } return euler; }
在DirectXMath Library中有许多与刚体姿态变换相关的函数可以直接调用：
- 四元数乘法：XMQuaternionMultiply
- 旋转矩阵转四元数：XMQuaternionRotationMatrix
- 四元数转旋转矩阵：XMMatrixRotationQuaternion
- 欧拉角转四元数：XMQuaternionRotationRollPitchYaw
- 四元数转Axis-Angle：XMQuaternionToAxisAngle
- 欧拉角转旋转矩阵：XMMatrixRotationRollPitchYaw method
- Axis-Angle转旋转矩阵：XMMatrixRotationAxis
- 构造绕X/Y/Z轴的旋转矩阵：XMMatrixRotationX
结果如下图所示:
参考：
DirectXMath Library Quaternion Functions
Convert quaternion to euler rotations
Conversion between quaternions and Euler angles
Maths - Conversion Quaternion to Euler
Coordinate Transformations in Robotics―MATLAB
Introduction to Robotics - Mechanics and Control.Chapter 2 Spatial descriptions and transformations