【ROS2机器人入门到实战】齐次坐标变换

5.齐次坐标变换

写在前面

0. 当前平台文章汇总地址：ROS2机器人从入门到实战
1. 获取完整教程及配套资料代码，请关注公众号<鱼香ROS>获取
2. 教程配套机器人开发平台：两驱版| 四驱版
3. 为方便交流，搭建了机器人技术问答社区：地址 fishros.org.cn

前面几节中，小鱼带你一起学习了使用TF进行坐标的变换，也带你通过旋转和平移求解了坐标的变换关系，但计算的过程中旋转和平移是分开计算的，那有没有一种方法，可以让旋转矩阵和平移向量合并到同一个矩阵里呢？

答案是有的，我们可以将$3\ast 3$的旋转矩阵和$3\ast 1$的平移矩阵进行组合，并添加一行(0,0,0,1)使其变成一个$4\ast 4$的方阵，其组合方式如下：

有旋转矩阵
$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\text{\hspace{1em}(旋转矩阵)}$$
平移矩阵
$$P=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]\text{\hspace{1em}(平移矩阵)}$$

合并成齐次变换矩阵
$$T=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& x\\ r_{21}& r_{22}& r_{23}& y\\ r_{31}& r_{32}& r_{33}& z\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(齐次矩阵)}$$

为什么要这样写，我们可以简单的推导一下，矩阵是支持分块运算的，我们将上面的矩阵进行分块
$$T=\left[\begin{array}{cc}R& P\\ 0& 1\end{array}\right]\text{\hspace{1em}(齐次矩阵)}$$
假设${}_B^{A}T$表示B坐标系到A坐标系的齐次变换，B坐标系下的点C坐标为${}_C^{B}P$，求C在A坐标系下的坐标${}_C^{A}P$

我们将${}_B^{A}T$乘${}_C^{B}P$上，可得
$${}_C^{A}P=\left[\begin{array}{cc}{}_B^{A}R& {}_B^{A}P\\ 0& 1\end{array}\right]\left[\begin{array}{c}{}_C^{B}P\\ 1\end{array}\right]={}_B^{A}R{}_C^{B}P{+}_B^{A}P$$
根据前面学习的平移+旋转复合坐标变换公式，正确的结果如下
$${}_C^{A}P={}_B^{A}R{}_C^{B}P{+}_B^{A}P$$
你会发现，两者最终结果完全相同，也就是说，我们的平移加旋转复合变换，可以直接用齐次变换矩阵代替。

1.齐次变换矩阵特性

接着我们来探索一下齐次变换矩阵的一些特性

2.1.齐次变换矩阵的符号表示

一般使用H或者T来表示齐次变换矩阵，矩阵的左上角标明参考坐标系，矩阵左下角标明目标坐标系，比如${}_B^{A}T$表示B坐标系到A坐标系的变换关系(平移+旋转)

2.2.齐次变换矩阵的逆的几何含义

就像矩阵的逆一样，齐次变换矩阵也有逆，其逆也有对应的几何含义，比如

比如${}_B^{A}T$表示B坐标系到A坐标系的变换关系

那么

${}_B^{A}T$的逆${}_B^A{T}^{-1}{=}_A^{B}T$表示A坐标系到B坐标系的变换关系

2.3.齐次变换矩阵的乘法的几何含义

3.3.1齐次矩阵与平移向量相乘

齐次矩阵与平移向量相乘，即可求出某个向量在另一坐标系下的表示，上面例子中即是如此。

3.3.2齐次矩阵与齐次矩阵相乘

齐次矩阵与齐次矩阵相乘，可以转换不同坐标系之间的关系，比如：
$${}_B^A{T}_C^{B}T{=}_C^{A}T$$
比如当我们有一个六自由度的机械臂，知道两两相邻关节之间的关系，那么就可以通过其次矩阵相乘的方法求出，关节6在关节0下的位置和姿态：
$${}_1^0{T}_2^1{T}_3^2{T}_4^3{T}_5^4{T}_6^{5}T{=}_6^{0}T$$

3.练习

练习小鱼放到了下一节了，毕竟不希望大家用手来算~