位姿描述和坐标变换

引子

想象我们用手去拿起桌子上的杯子，我们会观察杯子所处的状态调整我们的手的状态。我们观察杯子状态是观察杯子的位姿，调整手状态也为调整手的位姿。位姿包括位置和姿态。调整手状态的过程即为坐标变换，将一个位姿转换为另一个位姿。那么位姿和坐标变换如何表达呢？

首先考虑刚体具有几个自由度？

1. 刚体描述

二维平面的刚体描述

如上图所示，对于二维平面的刚体，其可以沿着X轴和Y轴移动，可以绕着某个垂直平面的固定轴转动。因此二维平面刚体具有三个自由度：

• 两个自由度的移动
• 一个自由度的转动

三维空间的刚体描述

三维空间需要三个相互垂直的X、Y和Z坐标轴描述。三维平面的刚体可以沿着X轴、Y轴和Z轴移动，也可以绕着三维空间的X轴、Y轴和Z轴旋转。因此三维空间刚体具有六个自由度：

• 三个自由度的移动
• 三个自由度的转动

三维空间的刚体的三自由度移动和三自由度转动如何描述呢？如下图所示：

• 其移动可以使用坐标系原点位置判定
• 其转动可以使用坐标系姿态判定

使用什么描述刚体的位姿，即移动和转动呢？

2. 位姿描述

首先建立参考坐标系$\{A\}$和刚体坐标系$\{B\}$。

位置描述

刚体位置可以使用含有三个元素的位置矢量进行表示，如下图所示。
$${}^A{\text{P}}_{BORG}=\left[\begin{array}{c}{P}_x\\ P_y\\ P_z\end{array}\right]$$

姿态描述

刚体姿态可以使用刚体坐标系$\{B\}$相对于参考坐标系$\{A\}$的主轴矢量${}^A{\hat{X}}_B$、${}^A{\hat{Y}}_B$和${}^A{\hat{Z}}_B$进行表示。通过将刚体坐标系$\{B\}$的主轴矢量${\hat{X}}_B$、${\hat{Y}}_B$和${\hat{Z}}_B$投影到参考坐标系$\{A\}$的主轴向量${\hat{X}}_A$、${\hat{Y}}_A$和${\hat{Z}}_A$得到。因此刚体姿态可以使用旋转矩阵表示：
$${}_B^A\text{R}={(}^A{\hat{X}}_B{,}^A{\hat{Y}}_B{,}^A{\hat{Z}}_B)=\left[\begin{array}{ccc}{\hat{X}}_B\cdot {\hat{X}}_A& {\hat{Y}}_B\cdot {\hat{X}}_A& {\hat{Z}}_B\cdot {\hat{X}}_A\\ {\hat{X}}_B\cdot {\hat{Y}}_A& {\hat{Y}}_B\cdot {\hat{Y}}_A& {\hat{Z}}_B\cdot {\hat{Y}}_A\\ {\hat{X}}_B\cdot {\hat{Z}}_A& {\hat{Y}}_B\cdot {\hat{Z}}_A& {\hat{Z}}_B\cdot {\hat{Z}}_A\end{array}\right]$$
进一步观察，参考坐标系$\{A\}$相对于刚体坐标系$\{B\}$的姿态表示为：
$$\begin{gathered} {}_A^B\text{R}=\left[\begin{array}{c}{}^B{\hat{X}}_A\\ {}^B{\hat{Y}}_A\\ {}^B{\hat{Z}}_A\end{array}\right]=\left[\begin{array}{ccc}{\hat{X}}_A\cdot {\hat{X}}_B& {\hat{X}}_A\cdot {\hat{Y}}_B& {\hat{X}}_A\cdot {\hat{Z}}_B\\ {\hat{Y}}_A\cdot {\hat{X}}_B& {\hat{Y}}_A\cdot {\hat{Y}}_B& {\hat{Y}}_A\cdot {\hat{Z}}_B\\ {\hat{Z}}_A\cdot {\hat{X}}_B& {\hat{Z}}_A\cdot {\hat{Y}}_B& {\hat{Z}}_A\cdot {\hat{Z}}_B\end{array}\right]{ \\ }_A^B\text{R}{=}_B^A{\text{R}}^T \end{gathered}$$

位姿描述

刚体坐标系$\{B\}$的位姿表示为：
$$\{B\}={\{}_B^A\text{R}{,}^A{\text{P}}_{BORG}\}$$

如何描述坐标系$\{B\}$相对于坐标系$\{A\}$的关系呢？

3. 坐标变换

位置矢量和旋转矩阵不但可以描述刚体位姿，还可以用来表示对矢量或者点进行移动变换和旋转变换：
$$\begin{gathered} {}^A\text{P}{=}_B^A{\text{R}}^B\text{P}{+}^A{\text{P}}_{BORG} \\ \left[\begin{array}{c}{}^A\text{P}\\ 1\end{array}\right]=\left[\begin{array}{cc}{}_B^A\text{R}& {}^A{\text{P}}_{BORG}\\ 0& 1\end{array}\right]\cdot \left[\begin{array}{c}{}^B\text{P}\\ 1\end{array}\right] \end{gathered}$$
可以进一步定义变换矩阵：
$$\begin{gathered} {}_B^A\text{T}=\left[\begin{array}{cc}{}_B^A\text{R}& {}^A{\text{P}}_{BORG}\\ 0& 1\end{array}\right]{ \\ }^A\text{P}{=}_B^A{\text{T}}^B\text{P} \end{gathered}$$
位姿(坐标变换)具有六个自由度，坐标变换矩阵$T$具有四个基础矩阵：

• 1个位移矩阵：

$$Trans(p_x,p_y,p_z)=\left[\begin{array}{cccc}1& 0& 0& p_x\\ 0& 1& 0& p_y\\ 0& 0& 1& p_z\\ 0& 0& 0& 1\end{array}\right]$$

• 三个旋转矩阵扩展为变换矩阵：

$$\begin{gathered} Rot(x,\alpha )=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos(\alpha )& sin(\alpha )& 0\\ 0& -sin(\alpha )& cos(\alpha )& 0\\ 0& 0& 0& 1\end{array}\right] \\ Rot(y,\alpha )=\left[\begin{array}{cccc}cos(\alpha )& 0& -sin(\alpha )& 0\\ 0& 1& 0& 0\\ sin(\alpha )& 0& cos(\alpha )& 0\\ 0& 0& 0& 1\end{array}\right] \\ Rot(z,\alpha )=\left[\begin{array}{cccc}cos(\alpha )& sin(\alpha )& 0& 0\\ -sin(\alpha )& cos(\alpha )& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right] \end{gathered}$$

坐标系$\{B\}$与坐标系$\{A\}$的相对关系如何转换？

4. 逆变换

已知坐标系$\{B\}$相对于坐标系$\{A\}$，即${}_B^A\text{T}$已知，计算坐标系$\{A\}$相对于坐标系$\{B\}$的变换矩阵${}_A^B\text{T}$。

旋转矩阵为：
$${}_A^B\text{R}{=}_B^A{\text{R}}^T$$
将${}^A{\text{P}}_{BORG}$转换为坐标系$\{B\}$的描述为：
$${}^B{(}^A{\text{P}}_{BORG}){=}_A^B{\text{R}}^A{\text{P}}_{BORG}{+}^B{\text{P}}_{AORG}$$
公式左边为0，可得：
$${}^B{\text{P}}_{AORG}={-}_A^B{\text{R}}^A{\text{P}}_{BORG}={-}_B^A{\text{R}}^T{}^A{\text{P}}_{BORG}$$
坐标系$\{A\}$相对于坐标系$\{B\}$的变换矩阵${}_A^B\text{T}$为：

$$\begin{gathered} {}_A^B\text{T}=\left[\begin{array}{cc}{}_B^A{\text{R}}^T& {-}_B^A{\text{R}}^T{}^A{\text{P}}_{BORG}\\ 0& 1\end{array}\right]{ \\ }_A^B\text{T}{=}_B^A{\text{T}}^{-1} \end{gathered}$$

参考

台大林沛群机器人：https://www.bilibili.com/video/BV1v4411H7ez?spm_id_from=333.851.header_right.fav_list.click