四元数、欧拉角、旋转矩阵笔记

四元数

单位四元数

$$\begin{aligned} & Q=\left[\cos\frac\phi 2,i\sin\frac\phi 2,j\sin\frac\phi 2, k\sin\frac\phi 2\right] \\ \end{aligned}$$

四元数性质

$$\begin{aligned} & q_aq_b=[s_a,\vec{a}][s_b,\vec{b}] =[s_as_b-\vec{a}\cdot\vec{b}, s_a\vec{b} + s_b\vec{a} + \vec{a}\times\vec{b}] \\ & q^{-1} = \frac{q^*}{|q|^2} \end{aligned}$$

任意向量 $\vec{v}$ 沿着以单位向量定义的旋转轴 $\vec{u}$ 旋转 $\theta$ 度之后的 $\vec{v}’$ 可以使用四元数乘法来获得。令 $q_v=[0,\vec{v}]$，$q=[\cos\frac{\theta}{2},\sin\frac{\theta}{2}\vec{u}]$，那么

$$\vec{v}'=qq_vq^*$$

四元数微分方程

$$\begin{aligned} \begin{bmatrix} \dot{q}_0 \\ \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \\ \end{bmatrix} =& \frac{1}{2}\begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & -\omega_z & \omega_y \\ \omega_y & -\omega_z & 0 & -\omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} \\ =& \frac{1}{2}\begin{bmatrix} q_{0} & -q_{1} & -q_{2} & -q_{3} \\ q_{1} & q_{0} & q_{3} & -q_{2} \\ q_{2} & -q_{3} & q_{0} & q_{1} \\ q_{3} & q_{2} & -q_{1} & q_{0} \end{bmatrix}\begin{bmatrix} 0 \\ \omega_{x} \\ \omega_{y} \\ \omega_{z} \end{bmatrix} \end{aligned}$$

旋转矩阵

横滚角 $\phi$ 绕x轴旋转，向左横滚为正

$$R_x=\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix}$$

俯仰角 $\theta$ 绕y轴旋转，抬头为正

$$R_y=\begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \\ \end{bmatrix}$$

偏航角 $\psi$ 绕z轴旋转，向左偏航为正

$$R_z=\begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

由罗德里格斯公式

$$R=\cos\theta I+(1-\cos\theta)\vec{n}\vec{n}^{\text{T}} +\sin\theta\vec{n}^{\wedge}$$

可以将旋转矩阵和轴角互相转换，有了轴角即可转换成四元数。
由旋转矩阵转换成轴角的公式为

$$\theta=\pm\arccos(\frac{\text{tr}(\mathbf{R})-1}{2})$$

由于$\boldsymbol{n}$为旋转轴且满足

$$\mathbf{R}\boldsymbol{n}=\boldsymbol{n}$$

所以 $\boldsymbol{n}$ 为旋转矩阵 $\mathbf{R}$ 的特征值为1对应的模长为1的特征向量。

欧拉角

  统一按照本体系zyx轴的旋转顺序，先绕z轴偏航，再绕y轴俯仰，最后绕x轴横滚。本体系x轴朝后为横滚轴，向左横滚为正；y轴朝右为俯仰轴，抬头为正；z轴朝上为偏航轴，向左偏航为正。旋转后横滚俯仰偏航三个角度的定义见后文四元数、旋转矩阵转欧拉角一节。

四元数转旋转矩阵

$$\begin{aligned} R =& \begin{bmatrix} 1-2q_2^2-2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2-2q_2^2 \end{bmatrix} \\ =& \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3 & q_0^2-q_1^2+q_2^2-q_3^2 & 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix} \\ =& (q_0^2-q_v^\text{T}q_v)I+2q_vq_v^\text{T}+2q_0q_v^{\wedge} \\ =& (q_0^2-q_1^2-q_2^2-q_3^2)I +2\begin{bmatrix} q_1^2 & q_1q_2 & q_1q_3 \\ q_1q_2 & q_2^2 & q_2q_3 \\ q_1q_3 & q_2q_3 & q_3^2 \\ \end{bmatrix} +2q_0\begin{bmatrix} 0 & -q_3 & q_2 \\ q_1 & 0 & -q_1 \\ -q_2 & q_3 & 0 \\ \end{bmatrix} \end{aligned}$$

Quaternion to Rotation Matriq_1 -OpenGL

旋转矩阵转四元数

Conversion of rotation matrix to quaternion -stackexchange
Converting a Rotation Matrix to a Quaternion -stackexchange
Maths - Conversion Matrix to Quaternion -EuclideanSpace

旋转矩阵与四元数的各个元素间有如下关系

$$1+r_{11}-r_{22}-r_{33}=4x^2 \\ 1-r_{11}+r_{22}-r_{33}=4y^2 \\ 1-r_{11}-r_{22}+r_{33}=4z^2 \\ r_{12}+r_{21}=4xy \\ r_{13}+r_{31}=4xz \\ r_{23}+r_{32}=4yz \\ r_{21}-r_{12}=4wz \\ r_{13}-r_{31}=4wy \\ r_{32}-r_{23}=4wx \\$$

已知旋转矩阵求四元数，上面方程组可以等价写为已知 $a_1$ 到 $a_9$ 求 $x,y,z,w$：

$$4x^2=a_1 \\ 4y^2=a_2 \\ 4z^2=a_3 \\ 4xy=a_4 \\ 4xz=a_5 \\ 4yz=a_6 \\ 4wz=a_7 \\ 4wy=a_8 \\ 4wx=a_9 \\$$

这9个方程中，等式左边包含 $4x,4y,4z,4w$ 项的方程分别有4,4,4,3个，等式两侧同时除以共同项后即可剩下 $x,y,z,w$ 项，而共同项 $4x,4y,4z$ 的计算方法为

$$4x=\pm 2\sqrt{4x^2}$$

例如，

$$x=\frac{a_1}{4x}=\frac{a_1}{\pm 2\sqrt{4x^2}}=\frac{a_1}{\pm 2\sqrt{a_1}} \\ y=\frac{a_4}{4x}=\frac{a_4}{\pm 2\sqrt{4x^2}}=\frac{a_4}{\pm 2\sqrt{a_1}} \\ z=\frac{a_5}{4x}=\frac{a_5}{\pm 2\sqrt{4x^2}}=\frac{a_5}{\pm 2\sqrt{a_1}} \\ w=\frac{a_9}{4x}=\frac{a_9}{\pm 2\sqrt{4x^2}}=\frac{a_9}{\pm 2\sqrt{a_1}} \\$$

这个例子以共同项 $4x$ 或 $a_1$ 为分母，但有时候分母可能为0，所以需要根据实际情况分别以4个共同项为分母。共同项 $4y$、$4z$ 与 $4x$ 类似，而 $4w$ 的计算方法为，将前3式相加得到

$$3-r_{11}-r_{22}-r_{33}=4(1-w^2) \\ 4w^2=1+r_{11}+r_{22}+r_{33} \\ 4w=\pm 2\sqrt{4w^2}$$

实际中有一些比较常用的特殊情况。例如，四元数

$$Q_1=[0, 1, 0, 0] \\ Q_2=[0, 0, 1, 0] \\ Q_3=[0, 0, 0, 1] \\$$

对应的旋转矩阵分别为

$$\begin{bmatrix} 1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & -1 \end{bmatrix} \begin{bmatrix} -1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix} \begin{bmatrix} -1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

四元数

$$[0.5, 0.5, 0.5, 0.5] \\ [0.5, -0.5, 0.5, 0.5] \\ [0.5, 0.5, -0.5, 0.5] \\ [0.5, 0.5, 0.5, -0.5] \\$$ $$[-0.5, 0.5, 0.5, 0.5] \\ [-0.5, -0.5, 0.5, 0.5] \\ [-0.5, 0.5, -0.5, 0.5] \\$$

对应的旋转矩阵分别为

$$\begin{bmatrix} 0 & 0 & 1 \\ 1 & 0 & 0 \\ 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} 0 & -1 & 0 \\ 0 & 0 & 1 \\ -1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & -1 \\ -1 & 0 & 0 \end{bmatrix} \\ \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & 0 & -1 \\ -1 & 0 & 0 \\ 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} 0 & 0 & 1 \\ -1 & 0 & 0 \\ 0 & -1 & 0 \end{bmatrix}$$

欧拉角转旋转矩阵

https://en.wikipedia.org/wiki/Euler_angles

$$\begin{aligned} & \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \\ \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix} \\ =& \begin{bmatrix} c_1c_2 & -s_1 & c_1s_2 \\ s_1c_2 & c_1 & s_1s_2 \\ -s_2 & 0 & c_2 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & c_3 & -s_3 \\ 0 & s_3 & c_3 \end{bmatrix} \\ =& \begin{bmatrix} c_1c_2 & c_1s_2s_3-s_1c_3 & c_1s_2 \\ s_1c_2 & s_1s_2s_3+c_1c_3 & s_1s_2 \\ -s_2 & c_2s_3 & c_2 \end{bmatrix} \end{aligned}$$

四元数、旋转矩阵转欧拉角

航模飞控上用的四元数转欧拉角代码如下

rol=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
pit=asin(2*(q0*q2-q1*q3));
yaw=atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));

四元数转旋转矩阵代码

Mat matqr(3, 3, vecdble{
    q0*q0+q1*q1-q2*q2-q3*q3, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2,
    2*q1*q2+2*q0*q3, q0*q0-q1*q1+q2*q2-q3*q3, 2*q2*q3-2*q0*q1,
    2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, q0*q0-q1*q1-q2*q2+q3*q3,
});

对应得到旋转矩阵转欧拉角

rol=atan2(r23, r33);
pit=asin(-r31);
yaw=atan2(r21, r11);

二值反正切函数为atan2(y,x)，横滚角的二值反正切函数中的y、x值分别为本体系y坐标轴和z坐标轴在世界系的z坐标(r23, r33)，俯仰角为0时也可以理解成世界系z轴在本体系YOZ平面上的投影与z轴的夹角（但俯仰角不为0时不能这样等价）。偏航角的二值反正切函数中的y、x值分别为本体系x轴在世界系的y坐标和x坐标(r21, r11)，可以理解成本体系x轴在世界系XOY平面上的投影与世界系x轴的夹角。俯仰角为本体系x轴与世界系XOY平面的距离，因为x轴朝向本体后方，所以取负号，表示抬头为正。

欧拉角速度

欧拉角速度与角速度的关系推导 -CSDN博客
Angular Velocity expressed via Euler Angles -Physics Stack Exchange

assume that $x=x(t),y=y(t)$, then let $z=\text e^x\sin y$, how to use sympy to calculate the total differentiation of $z$, i.e. $\sin y+\text e^x\cos y$?

角速度 $\vec\omega$ 是表示在惯性坐标系的，可分解为

$$\vec\omega=\omega_x\vec i+\omega_y\vec j+\omega_z\vec k$$

同时，又可以将它分解到刚体固连坐标系三次旋转的转轴上：

$$\vec\omega=\text{d}\phi\vec e_x+\text{d}\theta\vec e_y+\text{d}\psi\vec e_z$$

首先绕 $z$ 轴旋转

$$\text{d}\psi e_z=\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} 0 \\ 0 \\ \text{d}\psi \end{bmatrix}$$

其次绕 $y$ 轴旋转

$$\text{d}\theta e_y=\begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} 0 \\ \text{d}\theta \\ 0 \end{bmatrix}$$

最后绕 $x$ 轴旋转

$$\text{d}\phi e_x=\begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \\ \end{bmatrix}\begin{bmatrix} \text{d}\phi \\ 0 \\ 0 \end{bmatrix}$$

将三者相加，就可得到角速度与欧拉角速度的关系

$$\begin{aligned} \vec\omega =& R_zR_y\text{d}\vec\phi+R_z\text{d}\vec\theta+\text{d}\vec\psi \\ =& I(\text{d}\vec\psi+R_z(\text{d}\vec\theta+R_y\text{d}\vec\phi)) \\ % =& R_x(\text{d}\vec\phi+R_y(\text{d}\vec\theta+R_z\text{d}\vec\psi)) \\ =& \begin{bmatrix} \cos\psi\cos\theta\text{d}\phi \\ \sin\psi\cos\theta\text{d}\phi \\ -\sin\theta\text{d}\phi \\ \end{bmatrix} +\begin{bmatrix} -\sin\psi\text{d}\theta \\ \cos\psi\text{d}\theta \\ 0 \\ \end{bmatrix} +\begin{bmatrix} 0 \\ 0 \\ \text{d}\psi \end{bmatrix} \\ =& \begin{bmatrix} \cos\psi\cos\theta & -\sin\psi & 0 \\ \sin\psi\cos\theta & \cos\psi & 0 \\ -\sin\theta & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} \text{d}\phi \\ \text{d}\theta \\ \text{d}\psi \end{bmatrix} \\ \end{aligned}$$

反过来得到

$$\begin{aligned} \begin{bmatrix} \text{d}\phi \\ \text{d}\theta \\ \text{d}\psi \end{bmatrix} = \begin{bmatrix} \cos\psi/\cos\theta & -\sin\psi/\cos\theta & 0 \\ \sin\psi & \cos\psi & 0 \\ -cos\psi\tan\theta & \sin\psi\tan\theta & 1 \\ \end{bmatrix}\vec\omega \end{aligned}$$

罗德里格斯参数

$$g=\frac{1}{q_w}\begin{bmatrix} q_x \\ q_y \\ q_z \end{bmatrix}=e\tan\frac{\Phi}{2}$$

xyz固定角和zyx欧拉角相等的直观理解

  xyz固定角指所有的旋转均在世界坐标系下，刚体先后绕世界坐标系的xyz轴旋转，设旋转前的向量为 $\vec{v}$ (为了便于理解，可以想象 $\vec{v}=(1,0,0)^T$)，则旋转后的向量为

$$\vec{v}_1=R_zR_yR_x\vec{v}$$

  zyx欧拉角指3次旋转均为绕与刚体固连的坐标系的轴旋转，顺序为zyx。旋转前刚体坐标系与世界坐标系重合，绕刚体坐标系z轴旋转一次后的向量在世界坐标系下的坐标为 $R_z\vec{v}$。一直在动的刚体坐标系称为刚体系，刚体系在绕z轴旋转一次后的坐标系称为”刚1系”。此时再绕刚体系(刚1系)y轴旋转，旋转后的坐标在刚体系中仍为 $\vec{v}$，在刚1系中的坐标为 $R_y\vec{v}$。此时注意对向量乘一个矩阵的另一个作用，即将向量从一个坐标系变换到另一个坐标系，刚1系下的任意坐标 $\vec{x}$ 在世界系下的坐标为 $R_z\vec{x}$，所以现在旋转了两次的向量 $\vec{v}$ 在刚1系中的坐标为 $R_y\vec{v}$，在世界系中的坐标就是 $R_zR_y\vec{v}$。
  同理，再旋转最后一次后，向量 $\vec{v}$ 在刚体系的坐标仍然为 $\vec{v}$，在刚2系中的坐标为 $R_x\vec{v}$，刚2系的任意坐标 $\vec{x}$ 在刚1系下的坐标为 $R_y\vec{x}$ 所以向量 $\vec{v}$ 在刚2系中的坐标为 $R_x\vec{v}$，在刚1系中的坐标为 $R_yR_x\vec{v}$，在世界系中的坐标为 $R_zR_yR_x\vec{v}$，与xyz固定角相同。

• John J. Craig. 机器人学导论.

  其他参考

• 理解四元数 -Wyman的原创技术博客
• Understanding Quaternions -3D Game Engine Programming
• 四元数微分方程的推导和代码实现 -CSDN博客
• 四元数——基本概念 -知乎
• 如何形象地理解四元数？ -知乎
• 四元数与三维旋转 -知乎
• Quaternions and Rotations (PDF) -stanford