旋转矩阵、变换矩阵、旋转向量、欧拉角、四元数

旋转矩阵表、变换矩阵表、旋转向量表、欧拉角表、四元数表

视觉SLAM十四讲(三)——三维空间刚体运动(上)

在三维空间中描述刚体运动的方法包括旋转矩阵、变换矩阵、旋转向量、欧拉角和四元数；下面将逐一介绍这些方法

一、旋转矩阵

1. 点、向量、坐标系

• 点——存在于三维空间之中，点和点组成向量，点本身由原点指向它的向量所描述
• 向量——带指向性的箭头，可以进行加法减法等运算，定义坐标系后，向量可以由 R^3 当中的三个数表示， 如何理解这句话呢。如下图所示：

在代数中，我们用一组基底和向量 a 在每个坐标轴上的投影来表示一个向量，对于 ，通过某种线性组合，可以表示为a = a_xe_1+a_ye_2+a_ze_3
而上面那句话的意思是在矩阵运算中， 可以表示为 \left[ \begin{matrix} a_x \\ a_y \\ a_z \end{matrix} \right]，因为(e_1,e_2,e_3)\left[ \begin{matrix} a_x \\ a_y \\ a_z \end{matrix} \right] = a_xe_1+a_ye_2+a_ze_3

• 坐标系——三个正交的轴，构成线性空间的一组基，分为左手系和右手系
• 向量的运算可以由坐标运算来表达：加减法，内积，外积
  2. 问题的出现——一个最简单的情况，机器人从某一点直线运动到另一点，假设机器人是质点，并且和目标点处于同一平面上，分别以机器人和目标点建立坐标系，在移动过程中机器人的坐标系位置一直在变，要计算与目标点的距离，就需要描述坐标系之间如何变化
• 进而——如何计算同一个向量在不同坐标系里的坐标
• 如果刚才的机器人不是直线运动，而会有拐弯，这时坐标系就会旋转，因此描述整个运动过程就是三个轴的旋转和原点间的平移，这就是所谓的欧式变换，保证同一个向量在各个坐标系下的长度和夹角都不会发生变化，通过旋转和平移两部分组成
  3. 问题解决
• 平移是一个向量
• 旋转
  • 设某坐标系(用三个方向上的单位向量表示) (e_1,e_2,e_3) 发生了一次旋转，变成了(e_1^{'},e_2^{'},e_3^{'})
  • 对于某个固定的向量 (向量不随坐标系旋转)，它的坐标怎么变化，其中 \left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right] 是 在第一个坐标系中的坐标，\left[ \begin{matrix} a_1^{'} \\ a_2^{'} \\ a_3^{'} \end{matrix} \right] 是 在另一个坐标系中的坐标，如图，P为向量

• 在坐标系中相乘的结果即为自身向量。
• 左乘\left[ \begin{matrix} e_1^{T} \\ e_2^{T} \\ e_3^{T} \end{matrix} \right]后计算得到：\left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right] = \left[ \begin{matrix} e_1^{T}e_1^{'} & e_1^{T}e_2^{'} & e_1^{T}e_3^{'}\\ e_2^{T}e_1^{'} & e_2^{T}e_2^{'} & e_2^{T}e_3^{'} \\ e_3^{T}e_1^{'} & e_3^{T}e_2^{'} & e_3^{T}e_3' \end{matrix}\right]\left[ \begin{matrix} a'_1 \\ a'_2 \\ a'_3 \end{matrix}\right], 其中\Delta = Ra'。
• 中间的矩阵R被称为旋转矩阵。
• 通过定义验证可知该矩阵具有逆等于转置的性质。
• 矩阵R具有行列式的值为+1。
• 满足上述两个条件的矩阵则被称为旋转矩阵。

_旋转矩阵描述了两个坐标的变换关系，ex： a_1 = R_{12}a_2，反之 a_2 = R_{21}a_1，于是R_{21} = R_{12}^{-1} = R_{12}^T， 进一步，三个坐标系亦有 a_3 = R_{32}a_2 = R_{32}R_{21}a_1 = R_{31}a_1
_ 加上平移 a^{'} = Ra + t，因此两个坐标系的刚体运动可以由 R， t 完全描述

二、变换矩阵

• 变换矩阵的引入
• 用旋转和平移方式有一点不便之处，比如发生了两次变换 b = R_1a + t_1，c = R_2b + t_2
• 这时 c = R_2(R_1a + t_1) + t_2，叠加起来过于复杂
• 改变形式，写成 \left[ \right] = \left[ \right]\left[ \right] T\left[ \right]
• 例如 b = T_1a，c = T_2b ，得出 c = T_2T_1a
  • 这种用四个数表达三维向量的做法称为齐次坐标，这样旋转和平移可以放入同一个矩阵，称为变换矩阵，即 \left[ \begin{matrix} R & t \\ 0^T & 1\end{matrix} \right],其反向变换为 \left[ \begin{matrix} R^T & -R^Tt \\ 0^T & 1 \end{matrix} \right]，即矩阵的逆
  • 在 SLAM 中，通常定义世界坐标系 T_W 与 机器人坐标系 T_R，一个点的世界坐标为 p_W，机器人坐标系下为 p_R，那么满足关系：p_R = T_{RW}p_W，反之亦然，实际编程中可以使用 T_{RW} 或 T_{WR} 来描述机器人位姿

三、旋转向量和欧拉角

1. 旋转向量

• 旋转矩阵和变换矩阵固然可以表示旋转，但是要求太多：每次旋转只需要三个自由度，也就是x，y，z，但是旋转矩阵用9个量表达了3个自由度，变换矩阵用16个量表达了6个自由度，这种表达方式是冗余的，而且旋转矩阵和变换矩阵都必须是正交矩阵，自身约束会在实际求解中增加困难
• 任意旋转都可以用一个旋转轴和一个旋转角刻画，如何理解这句话呢，我们引入向量的外积的概念，别的你先不用知道，记住这句话：向量的外积的方向垂直于这两个向量。是不是一下子明白了，对于某个坐标系的一次旋转，可以通过移动后与该向量的外积来确定它是怎么旋转的
• 方向为旋转轴，长度为转过的角度，这就称为轴角或旋转向量 ，w = \theta n
• 轴角与旋转矩阵的不同：旋转矩阵需要九个量，有正交性约束和行列式约束，轴角：三个量，没有约束
• 只是表达方式不同，表达内容一样
• 轴角即为李代数，这里只是简单了解一下，下一篇会介绍它的原理
• 轴角转旋转矩阵——R = \cos \theta I+ (1-\cos\theta)nn^T + \sin\theta n^{\land}
• 旋转矩阵转轴角——角度： \theta = \arccos (\frac{tr (R)-1}{2})，轴： Rn = n
  2. 欧拉角
• 将旋转分解为三次不同轴上的转动，以便理解
• 例如：按照 Z-Y-X 顺序转动
• 轴可以使定轴或动轴，顺序亦可不同，因此存在许多种定义方式不同的欧拉角
• 常见的有 yaw-pitch-roll (偏航-俯仰-滚转) 角等等
  3. 欧拉角的万向锁问题
• ZYX顺序中，若 pitch 为正负90度，则第三次旋转和第一次绕同一个轴，使得系统丢失了一个自由度——存在奇异性问题
• 有点难理解，可以看看这个视频：视频传送门

四、四元数

4. 一种扩展的复数

• 回忆：复数可以表达二维平面的旋转

• 怎么理解这句话，首先看一下复数的另一种表达方式——矩阵表达式

• a+ib \leftrightarrow \begin{pmatrix}a & -b \\ b & \;\; a \end{pmatrix} = r\begin{bmatrix} \cos\varphi & -\sin\varphi \\ \sin\varphi & \cos\varphi \end{bmatrix} = r\exp\left(\varphi\begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix}\right)

• a,b 为实数，此类矩阵的和、积及乘法逆都是此类矩阵，\begin{pmatrix}a & -b \\ b & \;\; a \end{pmatrix} = a\begin{pmatrix}1 & 0 \\ 0 & 1 \end{pmatrix} + b\begin{pmatrix}0 & -1 \\ 1 & 0 \end{pmatrix},即实数1对应着单位矩阵\begin{pmatrix}1 & 0 \\ 0 & 1 \end{pmatrix}，虚数单位 i 对应着 \begin{pmatrix}0 & -1 \\ 1 & 0 \end{pmatrix}

• 现假设 = 1, b = 1，在复平面上代表点(1,1)，给 1+i 乘以 ，即原实数单位对应的矩阵成为 \begin{pmatrix}0 & -1 \\ -1 & 0 \end{pmatrix}，行列式值为 -1，代表点(-1,1)，相当于逆时针旋转了90度

• 对于 a+ib ，如果 本身是 m+jn ，虚部为 j， 也是一个复数 x+ky，虚部为 k，则构成的代数就是四元数
  5. 四元数有三个虚部，可以表达三维空间的旋转

• q = q_0 + q_1i + q_2j + q_3k

• q = [s,v]，s = q_0 ∈ R，v= [q_1,q_2,q_3]^T ∈ R^3

• 虚部之间的关系\begin{cases} i^2 = j^2 = k^2 = -1 \\ ij = k,ji = -k \\ jk = i,kj = -i \\ ki = j,ik = -j\end{cases}

• 四元数表示空间中的点——若三维空间的一个点的坐标为(x,y,z)，则它用纯四元数(类似于纯虚数，实部为0) xi+yj+zk 表示

• 单位四元数——单位四元数的欧拉公式: \cos\frac{\theta}{2} + (xi+yj+zk)\sin\frac{\theta}{2}，则q = [\cos\frac{\theta}{2},n_x\sin\frac{\theta}{2},n_y\sin\frac{\theta}{2},n_z\sin\frac{\theta}{2}]^T 表示单位四元数，其中 [n_x,n_y,n_z]^T 是一个单位向量

• 四元数的一些运算和性质

  • 加减法：q_a \pm q_b = [s_a \pm s_b,v_a \pm v_b]
  • 乘法：q_aq_b = s_as_b - x_ax_b -y_ay_b -z_az_b + (s_ax_b - x_as_b -y_az_b -z_ay_b)i + (s_ay_b - x_az_b -y_as_b -z_ax_b)j + (s_az_b - x_ay_b -y_ax_b -z_as_b)k
  • 乘法(向量表示)：q_aq_b = [s_as_b - v_a^Tv_b , s_av_b+s_bv_a+v_a×v_b]
  • 共轭：q_a^* = s_a - x_ai - y_aj - z_ak = [s_{a,} - v_a]
  • 模长：||q_a|| = \sqrt{s_a^2 + s_a^2 + y_a^2 + z_a^2}
  • 逆：q^{-1} = q^*/||q||^2
  • 数乘：kq = [k_s,k_v]
  • 点乘：q_a · q_b = s_as_b + x_ax_bi + y_ay_bj + z_az_bk
  1. 四元数和轴角的关系

• 来看看旋转向量，某个旋转是绕着单位向量 n = [n_x,n_y,n_z] 进行了角度为 \theta 的旋转，那么其四元数形式为： ，你可能会产生疑问，为什么是 \frac{\theta}{2} , 这个问题下面再说

• 四元数到轴角：\begin{cases}\theta = 2\arccos q_0 \\ [n_x,n_yn_z]^T = [q_1,q_2,q_3]^T/\sin \frac{\theta}{2}\end{cases}

• 类似可知四元数亦可转换为旋转矩阵，欧拉角
  2. 如何用单位四元数表示一个三维空间旋转

• 考虑一个点 p 经过一次通过四元数 q 实现的旋转变换后得到新的位置点 p^{'} ，那么它们之间的数学关系如何表达?

  • 将一个点 p 的坐标用纯四元数形式表示为：
    p = [0, x, y, z] = [0, \mathbf{v}]
  • 旋转变换后的坐标满足关系式：
    p^{'} = q p q^{-1}
  • 四元数相较于轴角与欧拉角的优势在于其紧凑性且能够有效避免奇异性的存在
  1. 问题

• 为了实现旋转效果，在三维空间中通常采用什么方法？

  • 为了理解三维空间中如何通过单位四元数描述旋转变换及其结果关系式p^{'} = qpq^{'}。

感恩我的知乎粉丝 Yang Eninala 的分享！

• 单位四元数有四个变量，完全可以被看作一个四维向量。单位四元数（norm=1）则存在于四维空间的一个球面上。q_aq_b，四元数q_a乘以四元数q_b其实看作（1）对进行左旋转，或者（2）对进行右旋转。所以从始至终，四元数定义的都是四维旋转，而不是三维旋转！任意的四维旋转都可以唯一的拆分为一个左旋转和一个右旋转，表达出来就是。这里，我们对单位四元数（四维向量）进行了一个左旋转和一个右旋转。结果当然是一个单位四元数
• 三维旋转就是四维旋转的一个特例，就像二维旋转是三维旋转的一个特例一样。说是特例其实不准确，准确的说是一个子集或者subgroup。为了进行三维旋转运算，汉密尔顿首先在四维空间里划出了一块三维空间。汉密尔顿定义了一种纯四元数（pure quaternion），其表达式为。纯四元数第一项为零，它存在于四维空间的三维超平面上，与三维空间中的三维向量一一对应。然后，就有了我们常见的这种左乘单位四元数，右乘其共轭的表达式。简单的说，当对一个三维向量进行三维旋转后，我们希望得到的是一个三维向量。那么这个左乘单位四元数，右乘其共轭的运算保证了结果是一个在三维超平面上中的纯四元数。
• 至于为什么是 呢，原因如下：

执行的就是一个旋转变换操作；而其逆元 q^{-1} 执行的也是一个类似的旋转变换；两次操作的整体效果等价于一个总旋转变换，并具有相同的总旋转角度

发布于 2018-08-10 17:21 [Nikki_o3o](https://www.cnblogs.com/N NikkiNikita/) 访问( ...) 参与评论 编辑 收藏夹