三维空间刚体变换：旋转矩阵

Jason.Li_0012

已于 2022-02-06 12:36:07 修改

阅读量1.9k

点赞数 2

分类专栏：《视觉SLAM十四讲》笔记文章标签：自动驾驶

于 2022-02-06 12:16:35 首次发布

本文链接：https://blog.csdn.net/weixin_45929038/article/details/122796923

版权

《视觉SLAM十四讲》笔记专栏收录该内容

21 篇文章 19 订阅

订阅专栏

点与坐标系

向量与坐标

空间中最基本的元素是点，点没有长度、体积。将两个点连接得到向量，向量表示由某点指向另一个点的有向线段。

对于一空间基底为 $\begin{bmatrix}i&j&k\end{bmatrix}$ 的坐标系，向量 $a$ 的坐标如下：
$a=\begin{bmatrix}i&j&k\end{bmatrix}\cdot\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=a_1i+a_2j+a_3k$
称 $\begin{bmatrix}a_1&a_2&a_3\end{bmatrix}^T$ 为向量 $a$ 在此基底下的坐标，坐标的具体取值同向量本身以及坐标系的选取有关。

坐标系可根据由左手法制或右手法制得到而分左手系和右手系：

在这里插入图片描述

向量的内积用于描述向量间的投影关系：
$a\cdot b=a^Tb=\sum^3_{i=1}a_ib_i=\vert a\vert\vert b\vert\cos\langle a,b\rangle$
式中， $\langle a,b\rangle$ 为向量 $a, b$ 间的夹角。

向量的外积为一个向量，其大小等于 $\vert a\vert\vert b\vert\sin\langle a,b\rangle$ ，方向垂直于这两个向量：
$a\times b=\begin{Vmatrix}i&j&k\\a_1&a_2&a_3\\b_1&b_2&b_3\end{Vmatrix}=\begin{bmatrix}a_2b_3-a_3b_2\\a_3b_1-a_1b_3\\a_1b_2-a_2b_1\end{bmatrix}=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix}b=a^\wedge b$
引入反对称矩阵，并用 $a^\wedge$ 表示向量 $a$ 的反对称矩阵。任何向量仅有唯一一个反对称矩阵：
$a^\wedge=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix}$

坐标系与欧式变换

在机器人中，定义世界坐标系固定不动，而机器人坐标系跟随着机器人一起移动。同时对每个传感器（IMU、激光雷达、摄像头等）建立传感器坐标系。

设世界坐标系内某点坐标为 ${}^Wp$ ，上标 $^W$ 表示为参考坐标系为世界坐标系 ${W\}$ ：
$^Wp=\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix}$
由一个旋转加上一个平移得到的运动称为刚体运动。机器人相对于世界的移动就是一个刚体运动，两坐标系间相差一个欧式变换，可通过变换矩阵 $T$ 得到。

在这里插入图片描述

则向量 $p$ 相对于机器人坐标系的坐标可通过坐标 $^Wp$ 与变换矩阵 $T$ 得到。

旋转矩阵与旋转群

欧式变换由一个旋转和一个平移得到，首先仅考虑旋转运动。

旋转矩阵

设坐标系 ${A\}$ 和坐标系 ${B\}$ ，已知坐标系 ${B\}$ 的基底为 $\begin{bmatrix}x_C&y_C&z_C\end{bmatrix}^T$ ，则可用其相对于 ${A\}$ 的方向余弦组成的矩阵可表示坐标系 ${B\}$ 相对 ${A\}$ 的方位：
$\begin{aligned} ^A_BR=&\begin{bmatrix}{}^Ax_B&{}^Ay_B&{}^Az_B\end{bmatrix}\\ ^A_BR=&\begin{bmatrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\\\end{bmatrix} \end{aligned}$
称 $^W_CR$ 为坐标系 ${B\}$ 相对于坐标系 ${A\}$ 的旋转矩阵。

旋转矩阵的九个元素中仅有三个为独立元素，这是因为基底的三分列向量相互垂直，且长度为1：
$\begin{aligned} ^Ax_B\cdot{}^Ay_B=^Wy_B\cdot{}^Az_B=^Az_B\cdot{}^Ax_B=0\\ ^Ax_B\cdot{}^Ax_B=^Wy_B\cdot{}^Ay_B=^Az_B\cdot{}^Az_B=1 \end{aligned}$
由此可得旋转矩阵为正交矩阵 ，满足如下条件：
$\begin{cases} {}^A_BR^{-1}={}^A_BR^T\\ det({}^A_BR)=1 \end{cases}$

单位旋转

对于一个坐标系绕自身的x轴、y轴、z轴旋转 $\theta$ 角的旋转矩阵如下：
$\begin{aligned} R(x,\theta)=&\begin{bmatrix}1&0&0\\0&\cos\theta&-\sin\theta\\0&\sin\theta&\cos\theta\end{bmatrix}\\ R(y,\theta)=&\begin{bmatrix}\cos\theta&0&\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{bmatrix}\\ R(z,\theta)=&\begin{bmatrix}\cos\theta&-\sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix}\\ \end{aligned}$

旋转群

旋转矩阵 $R\in\Re^{3\times3}$ 满足如下约束条件：

正交条件 ：旋转矩阵的逆等于它的转置， $R^{-1}=R^T$
特殊条件： 旋转矩阵的行列式为一， $d e t (R) = 1$

将满足上述条件的旋转矩阵 $R\in\Re^{3\times3}$ 的集合定义为旋转群 $S O (3)$ ：
$SO(3)=\bigl\{R\in\Re^{3\times3}\big\vert RR^T=I,det(R)=1\bigr\}$
旋转群又叫特殊正交群（Special Orthogonal Group），可推广至 $\Re^{n\times n}$ 空间中：
$SO(n)=\bigl\{R\in\Re^{n\times n}\big\vert RR^T=I,det(R)=1\bigr\}$
其维数为 $\frac{n(n-1)}{2}$ 的流形，当 $n = 2$ 时用于表示平面运动；当 $n = 3$ 时用于表示空间转动。旋转群是李群，满足李群条件。