四元数与欧拉角(RPY角)的相互转换
- 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: 偏航(航向)
由于是绕固定坐标系旋转,则旋转矩阵为(cαcα is shorthand for cosαcosα, sαsα is shorthand for sinαsinα,and so on.)
另一种姿态描述方式是绕自身坐标轴旋转:假设开始两个坐标系重合,先将{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与四元数
绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设等效旋转轴方向向量为K⃗ =[kx,ky,kz]TK→=[kx,ky,kz]T,等效旋转角为θθ,则四元数q=(x,y,z,w)q=(x,y,z,w),其中:
且有x2+y2+z2+w2=1x2+y2+z2+w2=1
即四元数存储了旋转轴和旋转角的信息,它能方便的描述刚体绕任意轴的旋转。
四元数转换为旋转矩阵:
已知旋转矩阵为:
则对应的四元数为:
- 四元数与欧拉角的相互转换
定义两个四元数:
![](https://i-blog.csdnimg.cn/blog_migrate/d7c11503b83409552ce907cc2820e3f4.png)
![](https://i-blog.csdnimg.cn/blog_migrate/aa8329b84a08ad61d9e726193e33f1f0.png)
![](https://i-blog.csdnimg.cn/blog_migrate/4c2a7a35a3a9dcc33ddf7f431258fd74.png)
![](https://i-blog.csdnimg.cn/blog_migrate/3cfb6dd34785c1946165c920190701c6.png)
![](https://i-blog.csdnimg.cn/blog_migrate/6011627f1c0168ec5b89d3a9957df048.png)
![](https://i-blog.csdnimg.cn/blog_migrate/16260bdff2c42ab90219230fa0408be3.png)
四元数加法:
![](https://i-blog.csdnimg.cn/blog_migrate/0ab995796ef1208eb74f90b277730096.png)
四元数乘法:
四元数的乘法的意义类似于矩阵的乘法,可以表示旋转的合成。当有多次旋转操作时,使用四元数可以获得更高的计算效率。
![](https://i-blog.csdnimg.cn/blog_migrate/6f758c29713a6acdd08d448bad766321.png)
![](https://i-blog.csdnimg.cn/blog_migrate/0c75635076d425ba146b0f6a1dba8caa.png)
![](https://i-blog.csdnimg.cn/blog_migrate/240a3dd56f5d5e36c458c92b4058aeee.png)
<<Quaternions` (* This loads the package *) Quaternion[2, 1, 1, 3] ** Quaternion[2, 1, 1, 0] ** Quaternion[1, 1, 1, 1] (* Be sure to use ** rather than * when multiplying quaternions *)
计算结果为:Quaternion[-12, 4, 14, 2]
θ = ATan(y / x)求出的θ取值范围是[-PI/2, PI/2];
θ = ATan2(y, x)求出的θ取值范围是[-PI, PI]。
当 (x, y) 在第一象限, 0 < θ < PI/2
当 (x, y) 在第二象限 PI/2 < θ≤PI
当 (x, y) 在第三象限, -PI < θ < -PI/2
当 (x, y) 在第四象限, -PI/2 < θ < 0
enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx};
![](https://i-blog.csdnimg.cn/blog_migrate/8f900a89c6347c561fdf2122f13be562.gif)
上面的代码存在一个问题,即奇异性没有考虑。下面看一种特殊的情况(参考Maths - Conversion Quaternion to Euler):假设一架飞机绕Y轴旋转了90°(俯仰角pitch=90),机头垂直向上,此时如何计算航向角和横滚角?
这时会发生自由度丢失的情况,即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题:
arcsin(2(q0q2−q1q3))arcsin(2(q0q2−q1q3))的定义域为[−1,1][−1,1],因此(q0q2−q1q3)∈[−0.5,0.5](q0q2−q1q3)∈[−0.5,0.5],当q0q2−q1q3=0.5q0q2−q1q3=0.5时(在程序中浮点数不能直接进行等于判断,要使用合理的阈值),俯仰角ββ为90°,将其带入正向公式计算出四元数(q0,q1,q2,q3)(q0,q1,q2,q3),然后可以发现逆向公式中atan2函数中的参数全部为0,即出现了0000的情况!无法计算。
β=π/2β=π/2时,sinβ2=cosβ2=0.707sinβ2=cosβ2=0.707,将其带入公式中有
则xw=zy=tanα−γ2xw=zy=tanα−γ2,于是有
通常令α=0α=0,这时γ=−2⋅atan2(x,w)γ=−2⋅atan2(x,w)。可以进行验证:当四元数为(w,x,y,z)=(0.653,-0.271,0.653,0.271)时,根据这些规则计算出来的ZYX欧拉角为α=0°,β=90°,γ=45°
当俯仰角为-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 method --Computes the product of two quaternions.
- 旋转矩阵转四元数:XMQuaternionRotationMatrix method --Computes a rotation quaternion from a rotation matrix.
- 四元数转旋转矩阵:XMMatrixRotationQuaternion method -- Builds a rotation matrix from a quaternion.
- 欧拉角转四元数:XMQuaternionRotationRollPitchYaw method --Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles).
- 四元数转Axis-Angle:XMQuaternionToAxisAngle method --Computes an axis and angle of rotation about that axis for a given quaternion.
- 欧拉角转旋转矩阵:XMMatrixRotationRollPitchYaw method --Builds a rotation matrix based on a given pitch, yaw, and roll (Euler angles).
- Axis-Angle转旋转矩阵:XMMatrixRotationAxis method --Builds a matrix that rotates around an arbitrary axis.
- 构造绕X/Y/Z轴的旋转矩阵:XMMatrixRotationX method --Builds a matrix that rotates around the x-axis.(Angles are measured clockwise when looking along the rotation axis toward the origin)
下面的代码中坐标系绕X轴旋转90°(注意这里不是按照右手定则的方向,而是沿着坐标轴向原点看过去以顺时针方式旋转,因此与传统的右手定则刚好方向相反),来进行变换:
![](https://i-blog.csdnimg.cn/blog_migrate/8f900a89c6347c561fdf2122f13be562.gif)
结果如下图所示:
参考:
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