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

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

之前我们已经讨论过旋转矩阵。需要再次强调的是，旋转的顺序很重要，并且会影响最终的结果。先旋转 $X$ 轴，再旋转 $Y$ 轴，最后旋转 $Z$ 轴得到的结果与先旋转 $Z$ 轴，再旋转 $Y$ 轴，最后旋转 $X$ 轴得到的结果是不同的。这种顺序的差异导致了不同的方向和空间方向的变化。这也是为什么在实际应用中，我们需要明确指定旋转顺序，以确保我们得到正确和一致的结果。

这次基于三维空间，讨论下旋转矩阵的两种表示方法，分别是欧拉角表示法，四元数表示法，以及二者之间的转换关系如何。

在三维空间中，旋转矩阵 $R$ 的维度为 $3\times 3$，其是一个正交矩阵，行列式为 $1$。

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

欧拉角通常由三个角度组成

• 滚转角（roll），常用符号为 $\varphi$
• 俯仰角（pitch），常用符号为 $\theta$
• 偏航角（yaw），常用符号为 $\psi$

这三个角度分别描述了绕 $X,Y,Z$ 轴旋转的角度。

绕 $X$ 轴旋转 $\varphi$ 角度的旋转矩阵为

$$R_x(\varphi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\varphi )& -\sin (\varphi )\\ 0& \sin (\varphi )& \cos (\varphi )\end{array}\right]$$

绕 $Y$ 轴旋转 $\theta$ 角度的旋转矩阵为

$$R_y(\theta )=\left[\begin{array}{ccc}\cos (\theta )& 0& \sin (\theta )\\ 0& 1& 0\\ -\sin (\theta )& 0& \cos (\theta )\end{array}\right]$$

绕 $Z$ 轴旋转 $\psi$ 角度的旋转矩阵为

$$R_z(\psi )=\left[\begin{array}{ccc}\cos (\psi )& -\sin (\psi )& 0\\ \sin (\psi )& \cos (\psi )& 0\\ 0& 0& 1\end{array}\right]$$

例如，对于一个分别依次绕固定轴 $XYZ$ 的欧拉角表示，其旋转矩阵为

$$\begin{array}{rl}R& =R_z(\varphi )R_y(\theta )R_x(\psi )\\ & =\left[\begin{array}{ccc}\cos (\theta )\cos (\psi )& \sin (\varphi )\sin (\theta )\cos (\psi )-\cos (\varphi )\sin (\psi )& \cos (\varphi )\sin (\theta )\cos (\psi )+\sin (\varphi )\sin (\psi )\\ \cos (\theta )\sin (\psi )& \sin (\varphi )\sin (\theta )\sin (\psi )+\cos (\varphi )\cos (\psi )& \cos (\varphi )\sin (\theta )\sin (\psi )-\sin (\varphi )\cos (\psi )\\ -\sin (\theta )& \sin (\varphi )\cos (\theta )& \cos (\varphi )\cos (\theta )\end{array}\right]\end{array}$$

这个矩阵代表了首先绕 $X$ 轴旋转 $\varphi$ 角，然后绕 $Y$ 轴旋转 $\theta$ 角，再然后绕 $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');

---

2. 四元数（Quaternion）表示法

四元数是由 $1$ 个实数加上 $3$ 个复数组合而成，通常可以表示为

$$q=q_w+q_x\text{i}+q_y\text{j}+q_z\text{k}$$

其中 $q_w,q_x,q_y,q_z$ 都是实数，$\text{i, j, k}$ 是四元数的基元，满足如下所示的乘法关系

• ${\text{i}}^2={\text{j}}^2={\text{k}}^2=\text{i}\text{j}\text{k}=-1$
• ${\text{i}}^0={\text{j}}^0={\text{k}}^0=1$

四元数还可看作由一个标量和一个向量组成，其中 $q_w$ 是四元数的标量部分，$q_x,q_y,q_z$ 构成四元数的向量部分。

---

假设有两个四元数分别为 $q_1=(q_{w1},[q_{x1},q_{y1},q_{z1}])$，$q_2=(q_{w2},[q_{x2},q_{y2},q_{z2}])$，同时令 $v_1=[q_{x1},q_{y1},q_{z1}]$，$v_2=[q_{x2},q_{y2},q_{z2}]$，则有如下运算法则

• 四元数的和：$q_1+q_2=(q_{w1}+q_{w2},(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))$
• 四元数的模：$\Vert q_1\Vert =\sqrt{q_{w1}^2+q_{x1}^2+q_{y1}^2+q_{z1}^2}$
• 单位四元数：$\Vert q_1\Vert =1$
• 四元数的共轭：$q_1^{\ast }=(q_{w1},-v_1)$
• 四元数的逆：$q_1^{-1}=\frac{q_1^{\ast }}{\Vert q_1\Vert }$

---

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

一个单位四元数（长度为 $1$）可以表示 3D 空间中的旋转。将一个点旋转到另一个位置可以通过四元数乘法来完成。

绕 $X$ 轴旋转 $\varphi$ 角度的四元数为

$$q_{\varphi }=(\cos (\frac{\varphi }{2}),\sin (\frac{\varphi }{2}),0,0)$$

绕 $Y$ 轴旋转 $\theta$ 角度的四元数为

$$q_{\theta }=(\cos (\frac{\theta }{2}),0,\sin (\frac{\theta }{2}),0,0)$$

绕 $Z$ 轴旋转 $\psi$ 角度的四元数为

$$q_{\psi }=(\cos (\frac{\psi }{2}),0,0,\sin (\frac{\psi }{2}))$$

总旋转的四元数是这三个四元数的乘积。四元数乘法不是通常的标量乘法，它有特定的乘法规则。

给定一个四元数 $q$（模长为 1，有关系 $\sqrt{q_w^2+q_x^2+q_y^2+q_z^2}=1$），假设采用的旋转顺序为 $XYZ$，其对应的旋转矩阵 $R$ 可以表示为

$$\begin{array}{rl}R& =\left[\begin{array}{ccc}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{array}\right]\end{array}$$

单位四元数在描述 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)$，若想将它转换为 $XYZ$ 顺序的欧拉角 $(\varphi ,\theta ,\psi )$。以下是从四元数到欧拉角的转换方法

$$\begin{array}{rl}\varphi & =\text{atan2}(2(q_w{q}_x+q_y{q}_z),1-2(q_x^2+q_y^2))\\ \theta & =\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{array}$$

其中 $\text{atan2}()$ 不是 $\arctan ()$。

举例说明，因为若使用 $\arctan (y/x)$，其返回值在 $[-\pi /2,\pi /2]$ 之间，因为它不能区分 $x$ 的正负。而 $\text{atan2}(y,x)$，其返回值在 $[-\pi ,\pi ]$ 之间，可以区分 $x$ 的正负，因此更为实用，尤其是在计算欧拉角时。更重要的是，$\text{atan2}(y,x)$ 能够处理 $x=0$ 的情况，这在计算角度或欧拉角时非常有用。

注意，由于使用 $\arcsin ()$，当 $\theta$ 接近 $\pm 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. 欧拉角转四元数

给定三个欧拉角 $\varphi ,\theta ,\psi$，相应的四元数为

$$\begin{array}{rl}{q}_w& =\cos (\frac{\varphi }{2})\cos (\frac{\theta }{2})\cos (\frac{\psi }{2})+\sin (\frac{\varphi }{2})\sin (\frac{\theta }{2})\sin (\frac{\psi }{2})\\ q_x& =\sin (\frac{\varphi }{2})\cos (\frac{\theta }{2})\cos (\frac{\psi }{2})-\cos (\frac{\varphi }{2})\sin (\frac{\theta }{2})\sin (\frac{\psi }{2})\\ q_y& =\cos (\frac{\varphi }{2})\sin (\frac{\theta }{2})\cos (\frac{\psi }{2})+\sin (\frac{\varphi }{2})\cos (\frac{\theta }{2})\sin (\frac{\psi }{2})\\ q_z& =\cos (\frac{\varphi }{2})\cos (\frac{\theta }{2})\sin (\frac{\psi }{2})-\sin (\frac{\varphi }{2})\sin (\frac{\theta }{2})\cos (\frac{\psi }{2})\end{array}$$

得到的四元数是 $(q_w,q_x,q_y,q_z)$。

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. 干货整理：欧拉角、旋转矩阵、四元数合辑