旋转矩阵、欧拉角

注：下面为学习空间机器人技术系列课程笔记，加上一些自己的整理，方便复习。

一、旋转矩阵的引出

下面坐标系0的基向量为 $x_{0}，y_{0})$ ，坐标系1的基向量为 $x_{1}，y_{1})$ 。
在这里插入图片描述
拓展到三维空间

以此类推，绕不同的轴的旋转矩阵如下：

二、坐标变换

在坐标系1下，向量p表示为：
$p=ux_{1}+vy_{1}+wz_{1}$
将其转换到坐标系0下，为p在坐标系0的基向量 $x_{0}，y_{0}，z_{0})$ 下的表示： $p^{0}$
在这里插入图片描述
具体推导过程如下，最终得到p在1坐标系下的表示 $p^{1}$ ，经过0到1的旋转矩阵 $R_{1}^{0}$ （或者称为1到0的坐标变换矩阵），得到p在0坐标下的表示 $p^{0}$

三、向量的旋转

同一坐标系下，向量的旋转。
注意下面假设的连体坐标系 $o_{1}$
在这里插入图片描述

四、旋转的复合

第一类情况：绕当前轴的旋转，也就是始终绕着转动之后的自身轴旋转，也称之为内旋。
这种情况，连续右乘即可。
在这里插入图片描述

第二种情况：绕着固定轴的旋转，绕着不动的坐标系的轴旋转，也称之为外旋。
这种情况，需要左乘旋转矩阵。推导过程涉及到下面提到的相似变换。
在这里插入图片描述

五、旋转矩阵的相似变换

下面的 $R_{i}^{0})^{0}$ 是指， $0$ 到 $i$ 的旋转矩阵 $R_{i}^{0}$ 在 $0$ 中的表示。
同理， $R_{i}^{0})^{1}$ 是指， $0$ 到 $i$ 的旋转矩阵 $R_{i}^{0}$ 在 $1$ 中的表示。
在这里插入图片描述
推导过程如下：

所以上面绕固定轴的旋转，推导过程如下：

六、齐次坐标变换

前面考虑的都是只有旋转的情况，当既有旋转，也有平动的情况了。
可以看做先旋转，然后在平动。
在这里插入图片描述

但是如果有连续平移旋转的情况下，这种带有加号的情况，不太简洁。所有采用齐次变换这种方式，把旋转和平移写到一个矩阵里面。这样连续变换，只需要连乘齐次变换矩阵就好了。
以 $H_{1}^{0}$ 为例， $R$ 是指 $R_{1}^{0}$ ， $d$ 是指 $d_{1}^{0}$ (1在坐标系0下的向量)
在这里插入图片描述

七、欧拉角以及rpy

围绕坐标轴旋转的先后次序不同，得到的最终欧拉角姿态就不同，所以根据坐标系绕坐标轴旋转顺序的不同会有12种欧拉角。

常规欧拉角 (Z-X-Z, X-Y-X, Y-Z-Y, Z-Y-Z, X-Z-X, Y-X-Y)
泰特 - 布赖恩角 (X-Y-Z, Y-Z-X, Z-X-Y, X-Z-Y, Z-Y-X, Y-X-Z)

在机器人中常用的 $r o l l (x), p i t c h (y), y a w (z)$ ，是指3-2-1转序的欧拉角。
对应内旋，绕自身坐标系，从左到右：
$\underset{R_{z\psi }R_{y\theta }R_{x\varphi }}{\rightarrow}$
和下面的外旋等价，绕固定坐标系，从右到左：
$\underset{R_{z\psi }R_{y\theta }R_{x\varphi }}{\leftarrow}$

关于不同转序的欧拉角(内旋），对应的旋转矩阵为:
在这里插入图片描述

八、欧拉角的万向锁（Gimbal Lock）

欧拉角看起来比较直观，比较适合人机交互。但是也有一些问题：

需要注意旋转次序，内旋还是外旋。这个不加注意，容易出错。
三个量的欧拉角，表示三维旋转，比较紧凑，但是会具有奇异性，比如说 $p i t c h$ 角为 $\pm π / 2$ 时，出现万向锁。

虽然有万向锁问题，但是一些平面运动，比如车辆，是不会出现 $p i t c h$ 角为 $\pm π / 2$ 的情况的，为了方便交互，往往也会采用。

万向锁的几何解释

首先，我们沿着 𝑥 轴旋转任意的度数：
在这里插入图片描述
接下来，我们沿 𝑦 轴旋转 𝜋/2 弧度：

注意，经过这一次的变换之后，我们将 𝑧 轴变换到原来 𝑥 轴方向，而 𝑥轴变到原来 −𝑧 的方向。
这个变换执行完毕后，我们仅仅只剩下一个 𝑧 轴的旋转矩阵了。
然而，当前的 𝑧 轴与原来的 𝑥 轴重合，也就是说，最后 𝑧 轴的旋转与 𝑥 轴的旋转其实操纵的是同一个轴。
三次旋转变换仅仅覆盖了两个轴的旋转，一个自由度就这样丢失了，这也就导致了 Gimbal Lock 的现象

万向锁的数学解释

这里采用3-2-1转序，内旋的欧拉角来解释。
绕着各个轴旋转的旋转矩阵，分别如下：
在这里插入图片描述
当绕着 $y$ 轴，旋转 $π / 2$ 时，组合出矩阵如下：

将原本绕着三个轴旋转，所组成的变换化，简成了绕两个轴的变化。
即便我们分别对 𝑧-𝑦-𝑥 三轴进行了旋转，实际上这个矩阵仅仅旋转了 𝑥-𝑦 两轴，它并没有对（初始的）𝑧 轴进行变换。也就是无论 𝑥轴与 𝑧 轴的旋转角是多少，变换都会丧失一个自由度。

九、对应的eigen代码

上面数学公式，所对应的eigen代码如下，根据实际使用需要可以方便查询。

#include <Eigen/Core>
#include <Eigen/Geometry>  // Eigen/Geometry 模块提供了各种旋转和平移的表示
using namespace std;
using namespace Eigen;// 1、旋转矩阵相关// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f， Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>Matrix3d rotation_matrix = Matrix3d::Identity();// 旋转向量使用轴角 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //轴角，用matrix()转换成矩阵rotation_matrix = rotation_vector.toRotationMatrix(); // 也可以直接赋值// 2、坐标变换相关// 用 AngleAxis 可以进行坐标变换Vector3d v(1, 0, 0);Vector3d v_rotated = rotation_vector * v;cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;// 或者用旋转矩阵v_rotated = rotation_matrix * v;cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;//6、齐次变换（欧氏变换矩阵）使用 Eigen::IsometryIsometry3d T = Isometry3d::Identity();  // 虽然称为3d，实质上是4＊4的矩阵T.rotate(rotation_vector);              // 按照rotation_vector进行旋转T.pretranslate(Vector3d(1, 3, 4));      // 把平移向量设成(1,3,4)cout << "Transform matrix = \n" << T.matrix() << endl;// 用变换矩阵进行坐标变换Vector3d v_transformed = T * v;         // 相当于R*v+tcout << "v tranformed = " << v_transformed.transpose() << endl;//7、 欧拉角: 可以将旋转矩阵直接转换成欧拉角Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序cout << "yaw pitch roll = " << euler_angles.transpose() << endl;// 四元数// 可以直接把AngleAxis赋值给四元数，反之亦然Quaterniond q = Quaterniond(rotation_vector);cout << "quaternion from rotation vector = " << q.coeffs().transpose()<< endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部// 也可以把旋转矩阵赋给它q = Quaterniond(rotation_matrix);cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;// 使用四元数旋转一个向量，使用重载的乘法即可v_rotated = q * v; // 注意数学上是qvq^{-1}cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;