旋转矩阵、欧拉角

旋转矩阵、欧拉角

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

一、旋转矩阵的引出

下面坐标系0的基向量 ( x 0 , y 0 ) (x_{0},y_{0}) (x0y0)坐标系1的基向量 ( x 1 , y 1 ) (x_{1},y_{1}) (x1y1)
旋转矩阵、欧拉角_第1张图片
拓展到三维空间
旋转矩阵、欧拉角_第2张图片
以此类推,绕不同的轴的旋转矩阵如下:
在这里插入图片描述

二、坐标变换

在坐标系1下,向量p表示为:
p = u x 1 + v y 1 + w z 1 p=ux_{1}+vy_{1}+wz_{1} p=ux1+vy1+wz1
将其转换到坐标系0下,为p在坐标系0的基向量 ( x 0 , y 0 , z 0 ) (x_{0},y_{0},z_{0}) (x0y0z0)下的表示: p 0 p^{0} p0
旋转矩阵、欧拉角_第3张图片
具体推导过程如下,最终得到p在1坐标系下的表示 p 1 p^{1} p1,经过0到1的旋转矩阵 R 1 0 R_{1}^{0} R10或者称为1到0的坐标变换矩阵),得到p在0坐标下的表示 p 0 p^{0} p0
旋转矩阵、欧拉角_第4张图片

三、向量的旋转

同一坐标系下,向量的旋转。
注意下面假设的连体坐标系 o 1 o_{1} o1
旋转矩阵、欧拉角_第5张图片

四、旋转的复合

第一类情况:绕当前轴的旋转,也就是始终绕着转动之后的自身轴旋转,也称之为内旋
这种情况,连续右乘即可。
旋转矩阵、欧拉角_第6张图片

第二种情况:绕着固定轴的旋转,绕着不动的坐标系的轴旋转,也称之为外旋
这种情况,需要左乘旋转矩阵。推导过程涉及到下面提到的相似变换。
旋转矩阵、欧拉角_第7张图片

五、旋转矩阵的相似变换

下面的 ( R i 0 ) 0 (R_{i}^{0})^{0} (Ri0)0 是指, 0 0 0 i i i 的旋转矩阵 R i 0 R_{i}^{0} Ri0 0 0 0 中的表示。
同理, ( R i 0 ) 1 (R_{i}^{0})^{1} (Ri0)1 是指, 0 0 0 i i i 的旋转矩阵 R i 0 R_{i}^{0} Ri0 1 1 1 中的表示。
旋转矩阵、欧拉角_第8张图片
推导过程如下:
旋转矩阵、欧拉角_第9张图片
旋转矩阵、欧拉角_第10张图片
所以上面绕固定轴的旋转,推导过程如下:
旋转矩阵、欧拉角_第11张图片
旋转矩阵、欧拉角_第12张图片

六、齐次坐标变换

前面考虑的都是只有旋转的情况,当既有旋转,也有平动的情况了。
可以看做先旋转,然后在平动。
旋转矩阵、欧拉角_第13张图片
旋转矩阵、欧拉角_第14张图片
但是如果有连续平移旋转的情况下,这种带有加号的情况,不太简洁。所有采用齐次变换这种方式,把旋转和平移写到一个矩阵里面。这样连续变换,只需要连乘齐次变换矩阵就好了。
H 1 0 H_{1}^{0} H10为例, R R R是指 R 1 0 R_{1}^{0} R10 d d d是指 d 1 0 d_{1}^{0} d10(1在坐标系0下的向量)
旋转矩阵、欧拉角_第15张图片
旋转矩阵、欧拉角_第16张图片

七、欧拉角以及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 ) roll(x) , pitch(y), yaw(z) roll(x),pitch(y),yaw(z),是指3-2-1转序的欧拉角。
对应内旋,绕自身坐标系,从左到右:
→ R z ψ R y θ R x φ \underset{R_{z\psi }R_{y\theta }R_{x\varphi }}{\rightarrow} RzψRyθRxφ
和下面的外旋等价,绕固定坐标系,从右到左:
← R z ψ R y θ R x φ \underset{R_{z\psi }R_{y\theta }R_{x\varphi }}{\leftarrow} RzψRyθRxφ

关于不同转序的欧拉角(内旋),对应的旋转矩阵为:
旋转矩阵、欧拉角_第17张图片

八、欧拉角的万向锁(Gimbal Lock)

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

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

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

万向锁的几何解释

首先,我们沿着 轴旋转任意的度数:
旋转矩阵、欧拉角_第18张图片
接下来,我们沿 轴旋转 /2 弧度:
旋转矩阵、欧拉角_第19张图片
注意,经过这一次的变换之后,我们将 轴变换到原来 轴方向,而 轴变到原来 − 的方向。
这个变换执行完毕后,我们仅仅只剩下一个 轴的旋转矩阵了。
然而,当前的 轴与原来的 轴重合,也就是说,最后 轴的旋转与 轴的旋转其实操纵的是同一个轴。
三次旋转变换仅仅覆盖了两个轴的旋转,一个自由度就这样丢失了,这也就导致了 Gimbal Lock 的现象

万向锁的数学解释

这里采用3-2-1转序,内旋的欧拉角来解释。
绕着各个轴旋转的旋转矩阵,分别如下:
旋转矩阵、欧拉角_第20张图片
当绕着 y y y轴,旋转 π / 2 π/2 π/2时,组合出矩阵如下:
旋转矩阵、欧拉角_第21张图片
将原本绕着三个轴旋转,所组成的变换化,简成了绕两个轴的变化。
即便我们分别对 -- 三轴进行了旋转,实际上这个矩阵仅仅旋转了 - 两轴,它并没有对(初始的) 轴进行变换。也就是无论 轴与 轴的旋转角是多少,变换都会丧失一个自由度。

九、对应的eigen代码

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

#include 
#include   // Eigen/Geometry 模块提供了各种旋转和平移的表示
using namespace std;
using namespace Eigen;
  // 1、旋转矩阵相关
  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f, Matrix3d 实质上是 Eigen::Matrix
  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讲所附代码

你可能感兴趣的:(数学,旋转矩阵,坐标变换,齐次变换,相似变换,欧拉角)