序号	内容
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）表示法](#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 ψ 角的总的旋转效果。

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.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),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.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)。

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

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

文章目录

1. 欧拉角（Euler Angles）表示法

2. 四元数（Quaternion）表示法

3. 四元数转欧拉角

4. 欧拉角转四元数

Ref