【数理知识】三维空间旋转矩阵的欧拉角表示法,四元数表示法,两者之间的转换,Matlab 代码实现

序号 内容
1 【数理知识】自由度 degree of freedom 及自由度的计算方法
2 【数理知识】刚体 rigid body 及刚体的运动
3 【数理知识】刚体基本运动,平动,转动
4 【数理知识】向量数乘,内积,外积,matlab代码实现
5 【数理知识】最小二乘法,从线性回归出发,数值举例并用最小二乘法求解回归模型
6 【数理知识】最小二乘法,一般线性情况,矩阵化表示过程,最佳参数的求解公式过程
7 【数理知识】协方差,随机变量的的协方差,随机变量分别是单个数字和向量时的协方差
8 【数理知识】奇异值分解,从数据的线性变换角度来理解
9 【数理知识】旋转矩阵的推导过程,基于向量的旋转来实现,同时解决欧式变换的非线性局限
10 【数理知识】三维空间旋转矩阵的欧拉角表示法,四元数表示法,两者之间的转换,Matlab 代码实现
11 【数理知识】已知 N>=3 个点在前后时刻的坐标,求刚体平移矩阵,旋转矩阵,且这 N>=3 点间距离始终不变代表一个刚体

文章目录

  • 1. 欧拉角(Euler Angles)表示法
  • 2. 四元数(Quaternion)表示法
  • 3. 四元数转欧拉角
  • 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(ϕ)0sin(ϕ)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(θ)0sin(θ)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(ψ)0sin(ψ)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章-数理知识基础 -> 坐标转换和【数理知识】旋转矩阵的推导过程,基于向量的旋转来实现,同时解决欧式变换的非线性局限。

% 给定欧拉角 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)];
R =
    0.7595   -0.5116    0.4018
    0.5318    0.8440    0.0694
   -0.3746    0.1610    0.9131

% 给定点坐标
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');

【数理知识】三维空间旋转矩阵的欧拉角表示法,四元数表示法,两者之间的转换,Matlab 代码实现_第1张图片


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=qw1qw2v1v2+qw1v2+qw2v1+v1×v2=(qw1qw2v1v2,(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\|} q11=q1q1

四元数是一个扩展的复数系统,常用于表示三维空间中的旋转。

一个单位四元数(长度为 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= 12(qy2+qz2)2(qxqy+qwqz)2(qxqzqwqy)2(qxqyqwqz)12(qx2+qz2)2(qyqz+qwqx)2(qxqz+qwqy)2(qyqzqwqx)12(qx2+qy2)

单位四元数在描述 3D 旋转时有一些优势,其不受欧拉角中的 “万向锁” 问题的影响。

% 给定四元数
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);
R =
    0.7595   -0.5116    0.4017
    0.5318    0.8440    0.0694
   -0.3746    0.1609    0.9131

3. 四元数转欧拉角

从四元数到欧拉角的转换并不是唯一的,因为对于某些旋转,存在多种欧拉角表示。但是,对于大多数实际应用,可以从一个特定的四元数计算一个特定的欧拉角集。

给定四元数 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),12(qx2+qy2))=arcsin(2(qwqyqxqz))=atan2(2(qwqz+qxqy),12(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° 时,可能会出现数值不稳定。这是因为在这些极端情况下,航向和滚动变得不可区分,这就是所谓的万向锁问题。

% 给定四元数
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);
phi_deg =
    9.9953

theta_deg =
   21.9990

psi_deg =
   34.9983

4. 欧拉角转四元数

给定三个欧拉角 ϕ , θ , ψ \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)

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];
quaternion =
    0.9376    0.0244    0.2070    0.2782

Ref

  1. 旋转矩阵 - Wikipedia
  2. 干货整理:欧拉角、旋转矩阵、四元数合辑

你可能感兴趣的:(数理知识,矩阵,matlab,线性代数)