MyMath数学库(五) - 欧拉旋转

我的无人车之路 mymath

---

$\quad$欧拉旋转在源文件euler_angles_zxy.hpp中。通过围绕正交坐标系的轴组成三个旋转，可以实现物体在3-D空间上的任何定向。如果假定轴是不动的，则这些旋转被称为是外在的，否则为固有的。在这里，我们使用固有参照。它相对于汽车的方向。轴x/y/z分别为右/前/上(RFU)。特别是，我们通过三个角度来描述汽车的方向：

1. $(-\pi/2, \pi/2)$中的螺旋对应于围绕x轴的旋转；
2. $[-\pi, \pi)$的滚动对应于绕y轴的旋转；
3. $[-\pi, \pi)$中的偏航对应于围绕z轴的旋转。
4. 当汽车水平时，俯仰为零；当机头朝上时，俯仰为正。
5. 当汽车水平时，侧倾为零；当左侧部分向上时，侧倾为正。
6. 当汽车朝北时，偏航为零，而朝西时，偏航为正。

反过来，在世界框架中，x/y/z轴指向东/北/上(ENU)。这些角度表示从世界到车架的旋转另外需要说明的是此类是一个模板类，接收double与float类型。
$\quad$此类主要目的是实现物体的依照RFU的各种旋转。在无人车应用方面则主要针对车的偏转，所以此类是为了给四元组类提供支持。
$\quad$在模板类定义之后直接定义了两个实体类型，可以直接使用。

using EulerAnglesZXYf = EulerAnglesZXY<float>;
using EulerAnglesZXYd = EulerAnglesZXY<double>;

5.1 函数表

函数名	说明
EulerAnglesZXY	此类提供了四种不同参数的构造函数。这里不一一列出。主要参数就是roll,pitch,yaw三种绕不同轴旋转的角度（弧度）。
roll	获取当前的roll角度。
pitch	获取当前的pitch角度。
yaw	获取当前的yaw角度。
normalize	将角度标准化到$[-\pi, \pi)$内
is_valid	验证一个指定的旋转角度是否有效，检查pitch角度是否在$[-2\pi, 2\pi]$之间
to_quaternion	转换到一个Eigen的四元组。

5.2 关于角度旋转的定义

5.2.1 二维旋转矩阵

$\quad$在欧几里得坐标系下，二维的旋转矩阵可以用一个旋转角 $\theta$ 来表达出来。具体为，

$$R = \begin{bmatrix} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \ \end{bmatrix}$$

二维点坐标表示为列向量乘以旋转矩阵，得到的列向量就是在欧几里得坐标系下的旋转后的坐标。

5.2.2 三维旋转矩阵

$\quad$一般地，欧几里得理论下，三维旋转矩阵可以表达为绕着某个方向向量以及绕着这个方向向量的旋转角（默认采用右手原则）。也就是说，确定一个三维旋转矩阵有$4$个变量，方向向量 $x, y, z$ 以及旋转角 $\theta$ 。欧拉旋转指出，三维旋转矩阵的自由度是3。
$\quad$三维旋转矩阵有多种表示方法，主要有两种方法：欧拉旋转、四元组。

欧拉旋转

$\quad$欧拉旋转的想法和旋转向量相似。但是不是给定一个旋转向量，而是分别给出观测对象沿$x, y, z$ 三个坐标轴旋转（转化为了一个二维的旋转问题）。其中，沿着给定三个方向的旋转矩阵是这个样子的：

$$\begin{align}\nonumber R_x(\theta) &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{bmatrix} \\ \nonumber R_y(\theta) &= \begin{bmatrix} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \ \end{bmatrix} \\ \nonumber R_z(\theta) &= \begin{bmatrix} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0\\ 0 & 0 & 1\ \end{bmatrix} \end{align}$$

然后，旋转矩阵表示为：

$$R=R_z R_y R_x$$

这种方法的缺点之一就是复杂，且由于欧拉万向角问题而表示不唯一。

四元组

$\quad$四元组是扩展自复数的一个概念，延伸自欧拉旋转。定义三个复变量 $i^2=j^2=k^2=-1$ ，然后把向量的各个分量表示为 $i, j, k$ 的系数，即 (2, 3, 4) 表示为 $2i+3j+4k$ 。一个围绕单位向量 $\overrightarrow{u}=(u_x, u_y, u_z)=u_x i + u_y j + u_z k$的旋转$\theta$角度的旋转，可以利用欧拉公式的扩展来表达：

$$q = e^{\frac{\theta}{2}(u_x i + u_y j + u_z k)} = \cos \frac{\theta}{2} + (u_x i + u_y j + u_z k) \sin \frac{\theta}{2}$$

然后，利用四元组乘法（注意，此处不是矩阵乘法！），上式可以应用于计算一个普通的三维点 $p=(p_x, p_y, p_z) = p_x i + p_y j + p_z k$ 的旋转：

$$p' = q p q^{-1}$$

而 $p'=({p_x}', {p_y}', {p_z}')$ 是旋转后的坐标。如果 $\overrightarrow{u}$ 和 点 $p$ 是沿原点同方向的，那么该旋转将绕 $\overrightarrow{u}$ 顺时针旋转 $\theta$。

四元组的矩阵表示

$\quad$四元组表达的旋转也可以通过旋转矩阵来表示，即简化上述$p' = q p q^{-1}$ 的操作。其得到的旋转矩阵为：

$$\begin{bmatrix} c + a_x^2 (1-c) & a_x a_y (1-c) - a_z s & a_x a_z (1-c) + a_y s \\ a_y a_x (1-c) + a_z s & c + a_y^2 (1-c) & a_y a_z (1-c) - a_x s \\ a_z a_x (1-c) - a_y s & a_z a_y (1-c) + a_x s & c + a_z^2 (1-c) \end{bmatrix}$$

其中，$s=\sin \theta, c = \cos \theta$ 。对于一个旋转四元组 $q=q_r+q_i i + q_j j + q_k k$，以上变量可以通过以下方程得到。

$$\begin{align} \nonumber \theta &=2 \arccos q_r =2 \arcsin \sqrt{{q_i}^2+{q_j}^2+{q_k}^2} \\ \nonumber (a_x, a_y, a_z) &=\frac{1}{\sin \frac{1}{2}\theta} (q_i, q_j, q_k) \end{align}$$

$\quad$如果直接从四元组中的变量出发，也可以把矩阵表达为：

$$\begin{bmatrix} 1 - 2 q_j^2 - 2 q_k^2 & 2 (q_i q_j - q_k q_r) & 2 (q_i q_k + q_j q_r) \\ 2 (q_i q_j + q_k q_r) & 1 - 2 q_i^2 - 2 q_k^2 & 2 (q_j q_k - q_i q_r) \\ 2 (q_i q_k - q_j q_r) & 2 (q_j q_k + q_i q_r) & 1 - 2 q_i^2 - 2 q_j^2 \end{bmatrix}$$

或（已知 $q_r$，可以通过变换式代换主轴线上的量）

$$\begin{bmatrix} q_r^2 + q_i^2 - q_j^2 - q_k^2 & 2 (q_i q_j - q_k q_r) & 2 (q_i q_k + q_j q_r) \\ 2 (q_i q_j + q_k q_r) & q_r^2 + q_j^2 - q_i^2 - q_k^2 & 2 (q_j q_k - q_i q_r) \\ 2 (q_i q_k - q_j q_r) & 2 (q_j q_k + q_i q_r) & q_r^2 + q_k^2 - q_i^2 - q_j^2 \end{bmatrix}$$

上述两个矩阵是等价的。

$\quad$以上旋转默认物体绕着原点的旋转，其他形式的旋转，可以把物体平移到相对坐标系，再做四元组旋转，然后再把坐标系变换回去。

to_quaternion

$\quad$有了以上的基础知识，就可以看懂在模板类的这段代码。从欧拉旋转转换到四元组矩阵了。

  Eigen::Quaternion<T> to_quaternion() const {
    T r = _roll * 0.5;
    T p = _pitch * 0.5;
    T y = _yaw * 0.5;
    T sr = std::sin(r);
    T sp = std::sin(p);
    T sy = std::sin(y);
    T cr = std::cos(r);
    T cp = std::cos(p);
    T cy = std::cos(y);
    T qw = cr * cp * cy - sr * sp * sy;
    T qx = cr * sp * cy - sr * cp * sy;
    T qy = cr * sp * sy + sr * cp * cy;
    T qz = cr * cp * sy + sr * sp * cy;
    if (qw < 0.0) return {-qw, -qx, -qy, -qz};
    return {qw, qx, qy, qz};
  }

四元组的矩阵恢复

$\quad$从旋转矩阵恢复四元组可以通过构造矩阵求特征值为$1$的特征向量（即四元组）。设 $Q$ 是一个 $3\times 3$ 的旋转矩阵，构造矩阵：

$$K = \frac13 \begin{bmatrix} Q_{xx}-Q_{yy}-Q_{zz} & Q_{yx}+Q_{xy} & Q_{zx}+Q_{xz} & Q_{yz}-Q_{zy} \\ Q_{yx}+Q_{xy} & Q_{yy}-Q_{xx}-Q_{zz} & Q_{zy}+Q_{yz} & Q_{zx}-Q_{xz} \\ Q_{zx}+Q_{xz} & Q_{zy}+Q_{yz} & Q_{zz}-Q_{xx}-Q_{yy} & Q_{xy}-Q_{yx} \\ Q_{yz}-Q_{zy} & Q_{zx}-Q_{xz} & Q_{xy}-Q_{yx} & Q_{xx}+Q_{yy}+Q_{zz} \end{bmatrix}$$

$\quad$如果 $Q$ 是一个纯旋转矩阵，那么$K$ 将会有一个特征值为$1$的特征向量，该向量即四元组。如果 $Q$不是一个纯旋转矩阵，那么我们求出最大的特征值所对应的向量为四元组，该四元组求得的旋转矩阵将接近$Q$。

5.3 关于旋转的扩展阅读

$\quad$此例是来自"Linear Algebra with Application(第9版)"的例子。