文章目录
- [1. 欧拉角（Euler Angles）表示法](#1. 欧拉角（Euler Angles）表示法)
- [2. 四元数（Quaternion）表示法](#2. 四元数（Quaternion）表示法)
- [3. 四元数转欧拉角](#3. 四元数转欧拉角)
- [4. 欧拉角转四元数](#4. 欧拉角转四元数)
- Ref
之前我们已经讨论过旋转矩阵。需要再次强调的是，旋转的顺序很重要，并且会影响最终的结果。先旋转 X X X 轴，再旋转 Y Y Y 轴，最后旋转 Z Z Z 轴得到的结果与先旋转 Z Z Z 轴，再旋转 Y Y Y 轴，最后旋转 X X X 轴得到的结果是不同的。这种顺序的差异导致了不同的方向和空间方向的变化。这也是为什么在实际应用中，我们需要明确指定旋转顺序，以确保我们得到正确和一致的结果。
这次基于三维空间，讨论下旋转矩阵的两种表示方法，分别是欧拉角表示法，四元数表示法，以及二者之间的转换关系如何。
在三维空间中，旋转矩阵 R R R 的维度为 3 × 3 3 \times 3 3×3，其是一个正交矩阵，行列式为 1 1 1。
1. 欧拉角（Euler Angles）表示法
欧拉角通常由三个角度组成
- 滚转角（roll），常用符号为 ϕ \phi ϕ
- 俯仰角（pitch），常用符号为 θ \theta θ
- 偏航角（yaw），常用符号为 ψ \psi ψ
这三个角度分别描述了绕 X , Y , Z X, Y, Z X,Y,Z 轴旋转的角度。
绕 X X X 轴旋转 ϕ \phi ϕ 角度的旋转矩阵为
R x ( ϕ ) = 1 0 0 0 cos ( ϕ ) − sin ( ϕ ) 0 sin ( ϕ ) cos ( ϕ ) R_x(\phi) = \left\\begin{matrix} 1 \& 0 \& 0 \\\\ 0 \& \\cos(\\phi) \& -\\sin(\\phi) \\\\ 0 \& \\sin(\\phi) \& \\cos(\\phi) \\\\ \\end{matrix}\\right Rx(ϕ)= 1000cos(ϕ)sin(ϕ)0−sin(ϕ)cos(ϕ)
绕 Y Y Y 轴旋转 θ \theta θ 角度的旋转矩阵为
R y ( θ ) = cos ( θ ) 0 sin ( θ ) 0 1 0 − sin ( θ ) 0 cos ( θ ) R_y(\theta) = \left\\begin{matrix} \\cos(\\theta) \& 0 \& \\sin(\\theta) \\\\ 0 \& 1 \& 0 \\\\ -\\sin(\\theta) \& 0 \& \\cos(\\theta) \\\\ \\end{matrix}\\right Ry(θ)= cos(θ)0−sin(θ)010sin(θ)0cos(θ)
绕 Z Z Z 轴旋转 ψ \psi ψ 角度的旋转矩阵为
R z ( ψ ) = cos ( ψ ) − sin ( ψ ) 0 sin ( ψ ) cos ( ψ ) 0 0 0 1 R_z(\psi) = \left\\begin{matrix} \\cos(\\psi) \& -\\sin(\\psi) \& 0 \\\\ \\sin(\\psi) \& \\cos(\\psi) \& 0 \\\\ 0 \& 0 \& 1 \\\\ \\end{matrix}\\right Rz(ψ)= cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001
例如，对于一个分别依次绕固定轴 X Y Z XYZ XYZ 的欧拉角表示，其旋转矩阵为
R = R z ( ϕ ) R y ( θ ) R x ( ψ ) = cos ( θ ) cos ( ψ ) sin ( ϕ ) sin ( θ ) cos ( ψ ) − cos ( ϕ ) sin ( ψ ) cos ( ϕ ) sin ( θ ) cos ( ψ ) + sin ( ϕ ) sin ( ψ ) cos ( θ ) sin ( ψ ) sin ( ϕ ) sin ( θ ) sin ( ψ ) + cos ( ϕ ) cos ( ψ ) cos ( ϕ ) sin ( θ ) sin ( ψ ) − sin ( ϕ ) cos ( ψ ) − sin ( θ ) sin ( ϕ ) cos ( θ ) cos ( ϕ ) cos ( θ ) \begin{aligned} R &= R_z(\phi) R_y(\theta) R_x(\psi) \\ &= \left\\begin{matrix} \\cos(\\theta)\\cos(\\psi) \& \\sin(\\phi)\\sin(\\theta)\\cos(\\psi) - \\cos(\\phi)\\sin(\\psi) \& \\cos(\\phi)\\sin(\\theta)\\cos(\\psi) + \\sin(\\phi)\\sin(\\psi) \\\\ \\cos(\\theta)\\sin(\\psi) \& \\sin(\\phi)\\sin(\\theta)\\sin(\\psi) + \\cos(\\phi)\\cos(\\psi) \& \\cos(\\phi)\\sin(\\theta)\\sin(\\psi) - \\sin(\\phi)\\cos(\\psi) \\\\ -\\sin(\\theta) \& \\sin(\\phi)\\cos(\\theta) \& \\cos(\\phi)\\cos(\\theta) \\\\ \\end{matrix}\\right \end{aligned} R=Rz(ϕ)Ry(θ)Rx(ψ)= cos(θ)cos(ψ)cos(θ)sin(ψ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)
这个矩阵代表了首先绕 X X X 轴旋转 ϕ \phi ϕ 角，然后绕 Y Y Y 轴旋转 θ \theta θ 角，再然后绕 Z Z Z 轴旋转 ψ \psi ψ 角的总的旋转效果。
更多关于欧拉角的推导和细节可参考文章：第3章-数理知识基础 -＞ 坐标转换和【数理知识】旋转矩阵的推导过程，基于向量的旋转来实现，同时解决欧式变换的非线性局限。
m
% 给定欧拉角 phi theta psi
phi = deg2rad(10); % 示例：10度，记得转换为弧度
theta = deg2rad(22); % 示例：22度
psi = deg2rad(35); % 示例：35度
R_x = [ 1 0 0
0 cos(phi) -sin(phi)
0 sin(phi) cos(phi)];
R_y = [ cos(theta) 0 sin(theta)
0 1 0
-sin(theta) 0 cos(theta)];
R_z = [ cos(psi) -sin(psi) 0
sin(psi) cos(psi) 0
0 0 1];
R = R_z * R_y * R_x;
R = [cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)
cos(theta)*sin(psi) sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi)
-sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)];
m
R =
0.7595 -0.5116 0.4018
0.5318 0.8440 0.0694
-0.3746 0.1610 0.9131
m
% 给定点坐标
point_1 = [ 10
22
35];
point_2 = R * point_1;
figure()
scatter3(point_1(1), point_1(2), point_1(3), 150, 'r'); hold on;
scatter3(point_2(1), point_2(2), point_2(3), 150, 'b');
2. 四元数（Quaternion）表示法
四元数是由 1 1 1 个实数加上 3 3 3 个复数组合而成，通常可以表示为
q = q w + q x i + q y j + q z k q = q_w + q_x \text{i} + q_y \text{j} + q_z \text{k} q=qw+qxi+qyj+qzk
其中 q w , q x , q y , q z q_w, q_x, q_y, q_z qw,qx,qy,qz 都是实数， i, j, k \text{i, j, k} i, j, k 是四元数的基元，满足如下所示的乘法关系
- i 2 = j 2 = k 2 = ijk = − 1 \text{i}^2 = \text{j}^2 = \text{k}^2 = \text{i}\text{j}\text{k} = -1 i2=j2=k2=ijk=−1
- i 0 = j 0 = k 0 = 1 \text{i}^0 = \text{j}^0 = \text{k}^0 = 1 i0=j0=k0=1
四元数还可看作由一个标量和一个向量组成，其中 q w q_w qw 是四元数的标量部分， q x , q y , q z q_x, q_y, q_z qx,qy,qz 构成四元数的向量部分。
假设有两个四元数分别为 q 1 = ( q w 1 , q x 1 , q y 1 , q z 1 ) q_1 = (q_{w1}, q_{x1}, q_{y1}, q_{z1}) q1=(qw1,qx1,qy1,qz1)， q 2 = ( q w 2 , q x 2 , q y 2 , q z 2 ) q_2 = (q_{w2}, q_{x2}, q_{y2}, q_{z2}) q2=(qw2,qx2,qy2,qz2)，同时令 v 1 = q x 1 , q y 1 , q z 1 v_1 = q_{x1}, q_{y1}, q_{z1} v1=qx1,qy1,qz1， v 2 = q x 2 , q y 2 , q z 2 v_2 = q_{x2}, q_{y2}, q_{z2} v2=qx2,qy2,qz2，则有如下运算法则
- 四元数的和： q 1 + q 2 = ( q w 1 + q w 2 , ( v 1 + v 2 ) ) q_1 + q_2 = (q_{w1}+q_{w2}, (v_1 +v_2)) q1+q2=(qw1+qw2,(v1+v2))
- 四元数乘法： q 1 q 2 = q w 1 q w 2 − v 1 ⋅ v 2 + q w 1 v 2 + q w 2 v 1 + v 1 × v 2 = ( q w 1 q w 2 − v 1 ⋅ v 2 , ( q w 1 v 2 + q w 2 v 1 + v 1 × v 2 ) ) q_1 q_2 = q_{w1} q_{w2} - v_1 \cdot v_2 + q_{w1} v_2 + q_{w2} v_1 + v_1 \times v_2 = (q_{w1} q_{w2} - v_1 \cdot v_2, (q_{w1} v_2 + q_{w2} v_1 + v_1 \times v_2)) q1q2=qw1qw2−v1⋅v2+qw1v2+qw2v1+v1×v2=(qw1qw2−v1⋅v2,(qw1v2+qw2v1+v1×v2))
- 四元数的模： ∥ q 1 ∥ = q w 1 2 + q x 1 2 + q y 1 2 + q z 1 2 \|q_1\| = \sqrt{q_{w1}^2 + q_{x1}^2 + q_{y1}^2 + q_{z1}^2} ∥q1∥=qw12+qx12+qy12+qz12
- 单位四元数： ∥ q 1 ∥ = 1 \|q_1\| = 1 ∥q1∥=1
- 四元数的共轭： q 1 ∗ = ( q w 1 , − v 1 ) q_1^* = (q_{w1}, -v_1) q1∗=(qw1,−v1)
- 四元数的逆： q 1 − 1 = q 1 ∗ ∥ q 1 ∥ q_1^{-1} = \frac{q_1^*}{\|q_1\|} q1−1=∥q1∥q1∗
四元数是一个扩展的复数系统，常用于表示三维空间中的旋转。
一个单位四元数（长度为 1 1 1）可以表示 3D 空间中的旋转。将一个点旋转到另一个位置可以通过四元数乘法来完成。
绕 X X X 轴旋转 ϕ \phi ϕ 角度的四元数为
q ϕ = ( cos ( ϕ 2 ) , sin ( ϕ 2 ) , 0 , 0 ) q_\phi = (\cos(\frac{\phi}{2}), \sin(\frac{\phi}{2}), 0, 0) qϕ=(cos(2ϕ),sin(2ϕ),0,0)
绕 Y Y Y 轴旋转 θ \theta θ 角度的四元数为
q θ = ( cos ( θ 2 ) , 0 , sin ( θ 2 ) , 0 , 0 ) q_\theta = (\cos(\frac{\theta}{2}), 0, \sin(\frac{\theta}{2}), 0, 0) qθ=(cos(2θ),0,sin(2θ),0,0)
绕 Z Z Z 轴旋转 ψ \psi ψ 角度的四元数为
q ψ = ( cos ( ψ 2 ) , 0 , 0 , sin ( ψ 2 ) ) q_\psi = (\cos(\frac{\psi}{2}), 0, 0, \sin(\frac{\psi}{2})) qψ=(cos(2ψ),0,0,sin(2ψ))
总旋转的四元数是这三个四元数的乘积。四元数乘法不是通常的标量乘法，它有特定的乘法规则。
给定一个四元数 q q q（模长为 1，有关系 q w 2 + q x 2 + q y 2 + q z 2 = 1 \sqrt{q_w^2+q_x^2+q_y^2+q_z^2}=1 qw2+qx2+qy2+qz2 =1），假设采用的旋转顺序为 X Y Z XYZ XYZ，其对应的旋转矩阵 R R R 可以表示为
R = 1 − 2 ( q y 2 + q z 2 ) 2 ( q x q y − q w q z ) 2 ( q x q z + q w q y ) 2 ( q x q y + q w q z ) 1 − 2 ( q x 2 + q z 2 ) 2 ( q y q z − q w q x ) 2 ( q x q z − q w q y ) 2 ( q y q z + q w q x ) 1 − 2 ( q x 2 + q y 2 ) \begin{aligned} R &= \left\\begin{matrix} 1 - 2(q_y\^2 + q_z\^2) \& 2(q_x q_y - q_w q_z) \& 2(q_x q_z + q_w q_y) \\\\ 2(q_x q_y + q_w q_z) \& 1 - 2(q_x\^2 + q_z\^2) \& 2(q_y q_z - q_w q_x) \\\\ 2(q_x q_z - q_w q_y) \& 2(q_y q_z + q_w q_x) \& 1 - 2(q_x\^2 + q_y\^2) \\\\ \\end{matrix}\\right \end{aligned} R= 1−2(qy2+qz2)2(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)1−2(qx2+qz2)2(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)1−2(qx2+qy2)
单位四元数在描述 3D 旋转时有一些优势，其不受欧拉角中的 "万向锁" 问题的影响。
m
% 给定四元数
quaternion = [0.9376 0.0244 0.2070 0.2782];
q_w = quaternion(1);
q_x = quaternion(2);
q_y = quaternion(3);
q_z = quaternion(4);
% 计算旋转矩阵 R
R(1,1) = 1 - 2*(q_y^2 + q_z^2);
R(1,2) = 2*(q_x*q_y - q_w*q_z);
R(1,3) = 2*(q_x*q_z + q_w*q_y);
R(2,1) = 2*(q_x*q_y + q_w*q_z);
R(2,2) = 1 - 2*(q_x^2 + q_z^2);
R(2,3) = 2*(q_y*q_z - q_w*q_x);
R(3,1) = 2*(q_x*q_z - q_w*q_y);
R(3,2) = 2*(q_y*q_z + q_w*q_x);
R(3,3) = 1 - 2*(q_x^2 + q_y^2);
m
R =
0.7595 -0.5116 0.4017
0.5318 0.8440 0.0694
-0.3746 0.1609 0.91313. 四元数转欧拉角
从四元数到欧拉角的转换并不是唯一的，因为对于某些旋转，存在多种欧拉角表示。但是，对于大多数实际应用，可以从一个特定的四元数计算一个特定的欧拉角集。
给定四元数 q = ( q w , q x , q y , q z ) q = (q_w, q_x, q_y, q_z) q=(qw,qx,qy,qz)，若想将它转换为 X Y Z XYZ XYZ 顺序的欧拉角 ( ϕ , θ , ψ ) (\phi, \theta, \psi) (ϕ,θ,ψ)。以下是从四元数到欧拉角的转换方法
ϕ = atan2 ( 2 ( q w q x + q y q z ) , 1 − 2 ( q x 2 + q y 2 ) ) θ = arcsin ( 2 ( q w q y − q x q z ) ) ψ = atan2 ( 2 ( q w q z + q x q y ) , 1 − 2 ( q y 2 + q z 2 ) ) \begin{aligned} \phi &= \text{atan2} (2(q_w q_x + q_y q_z), 1-2(q_x^2 + q_y^2)) \\ \theta &= \text{} \arcsin (2(q_w q_y - q_x q_z)) \\ \psi&= \text{atan2} (2(q_w q_z + q_x q_y), 1-2(q_y^2 + q_z^2)) \end{aligned} ϕθψ=atan2(2(qwqx+qyqz),1−2(qx2+qy2))=arcsin(2(qwqy−qxqz))=atan2(2(qwqz+qxqy),1−2(qy2+qz2))
其中 atan2 ( ) \text{atan2}() atan2() 不是 arctan ( ) \arctan() arctan()。
举例说明，因为若使用 arctan ( y / x ) \arctan(y/x) arctan(y/x)，其返回值在 − π / 2 , π / 2 -\\pi/2, \\pi/2 −π/2,π/2 之间，因为它不能区分 x x x 的正负。而 atan2 ( y , x ) \text{atan2}(y, x) atan2(y,x)，其返回值在 − π , π -\\pi, \\pi −π,π 之间，可以区分 x x x 的正负，因此更为实用，尤其是在计算欧拉角时。更重要的是， atan2 ( y , x ) \text{atan2}(y, x) atan2(y,x) 能够处理 x = 0 x=0 x=0 的情况，这在计算角度或欧拉角时非常有用。
注意，由于使用 arcsin ( ) \arcsin() arcsin()，当 θ \theta θ 接近 ± 90 ° \pm 90\degree ±90° 时，可能会出现数值不稳定。这是因为在这些极端情况下，航向和滚动变得不可区分，这就是所谓的万向锁问题。
m
% 给定四元数
quaternion = [0.9376 0.0244 0.2070 0.2782];
q_w = quaternion(1);
q_x = quaternion(2);
q_y = quaternion(3);
q_z = quaternion(4);
% 转换四元数到欧拉角
phi = atan2(2*(q_w*q_x + q_y*q_z), 1 - 2*(q_x^2 + q_y^2));
theta = asin(2*(q_w*q_y - q_z*q_x));
psi = atan2(2*(q_w*q_z + q_x*q_y), 1 - 2*(q_y^2 + q_z^2));
% 如果需要角度形式而不是弧度，可以转换为度
phi_deg = rad2deg(phi);
theta_deg = rad2deg(theta);
psi_deg = rad2deg(psi);
m
phi_deg =
9.9953
theta_deg =
21.9990
psi_deg =
34.99834. 欧拉角转四元数
给定三个欧拉角 ϕ , θ , ψ \phi, \theta, \psi ϕ,θ,ψ，相应的四元数为
q w = cos ( ϕ 2 ) cos ( θ 2 ) cos ( ψ 2 ) + sin ( ϕ 2 ) sin ( θ 2 ) sin ( ψ 2 ) q x = sin ( ϕ 2 ) cos ( θ 2 ) cos ( ψ 2 ) − cos ( ϕ 2 ) sin ( θ 2 ) sin ( ψ 2 ) q y = cos ( ϕ 2 ) sin ( θ 2 ) cos ( ψ 2 ) + sin ( ϕ 2 ) cos ( θ 2 ) sin ( ψ 2 ) q z = cos ( ϕ 2 ) cos ( θ 2 ) sin ( ψ 2 ) − sin ( ϕ 2 ) sin ( θ 2 ) cos ( ψ 2 ) \begin{aligned} q_w &= \cos(\frac{\phi}{2}) \cos(\frac{\theta}{2}) \cos(\frac{\psi}{2}) + \sin(\frac{\phi}{2}) \sin(\frac{\theta}{2}) \sin(\frac{\psi}{2}) \\ q_x &= \sin(\frac{\phi}{2}) \cos(\frac{\theta}{2}) \cos(\frac{\psi}{2}) - \cos(\frac{\phi}{2}) \sin(\frac{\theta}{2}) \sin(\frac{\psi}{2}) \\ q_y &= \cos(\frac{\phi}{2}) \sin(\frac{\theta}{2}) \cos(\frac{\psi}{2}) + \sin(\frac{\phi}{2}) \cos(\frac{\theta}{2}) \sin(\frac{\psi}{2}) \\ q_z &= \cos(\frac{\phi}{2}) \cos(\frac{\theta}{2}) \sin(\frac{\psi}{2}) - \sin(\frac{\phi}{2}) \sin(\frac{\theta}{2}) \cos(\frac{\psi}{2}) \end{aligned} qwqxqyqz=cos(2ϕ)cos(2θ)cos(2ψ)+sin(2ϕ)sin(2θ)sin(2ψ)=sin(2ϕ)cos(2θ)cos(2ψ)−cos(2ϕ)sin(2θ)sin(2ψ)=cos(2ϕ)sin(2θ)cos(2ψ)+sin(2ϕ)cos(2θ)sin(2ψ)=cos(2ϕ)cos(2θ)sin(2ψ)−sin(2ϕ)sin(2θ)cos(2ψ)
得到的四元数是 ( q w , q x , q y , q z ) (q_w, q_x, q_y, q_z) (qw,qx,qy,qz)。
m
phi = deg2rad(10); % 示例：10度，记得转换为弧度
theta = deg2rad(22); % 示例：22度
psi = deg2rad(35); % 示例：35度
% 计算四元数
q_w = cos(phi/2) * cos(theta/2) * cos(psi/2) + sin(phi/2) * sin(theta/2) * sin(psi/2);
q_x = sin(phi/2) * cos(theta/2) * cos(psi/2) - cos(phi/2) * sin(theta/2) * sin(psi/2);
q_y = cos(phi/2) * sin(theta/2) * cos(psi/2) + sin(phi/2) * cos(theta/2) * sin(psi/2);
q_z = cos(phi/2) * cos(theta/2) * sin(psi/2) - sin(phi/2) * sin(theta/2) * cos(psi/2);
quaternion = [q_w, q_x, q_y, q_z];
m
quaternion =
0.9376 0.0244 0.2070 0.2782