旋转矩阵、欧拉角

旋转矩阵、欧拉角

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

一、旋转矩阵的引出

下面坐标系0的基向量为$(x_0，y_0)$，坐标系1的基向量为$(x_1，y_1)$。

拓展到三维空间

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

二、坐标变换

在坐标系1下，向量p表示为：
$$p=u{x}_1+v{y}_1+w{z}_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)

在机器人中常用的$roll(x),pitch(y),yaw(z)$，是指3-2-1转序的欧拉角。
对应内旋，绕自身坐标系，从左到右：
$$\underset{R_{z\psi }{R}_{y\theta }{R}_{x\phi }}{\to }$$
和下面的外旋等价，绕固定坐标系，从右到左：
$$\underset{R_{z\psi }{R}_{y\theta }{R}_{x\phi }}{\leftarrow }$$

关于不同转序的欧拉角(内旋），对应的旋转矩阵为:

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

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

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

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

万向锁的几何解释

首先，我们沿着 𝑥 轴旋转任意的度数：

接下来，我们沿 𝑦 轴旋转 𝜋/2 弧度：

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

万向锁的数学解释

这里采用3-2-1转序，内旋的欧拉角来解释。
绕着各个轴旋转的旋转矩阵，分别如下：

当绕着$y$轴，旋转$\pi /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::Isometry
  Isometry3d 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+t
  cout << "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;

参考资料

空间机器人技术
Bonus: Gimbal Lock — Krasjet
slam14讲所附代码