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

6.齐次坐标变换实战

写在前面

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

上一节我们对齐次矩阵的组成和齐次矩阵的求逆和乘法两个运算的几何意义进行了介绍。

本节课我们就通过对应的函数和库实现齐次矩阵的生成，齐次矩阵的乘法和求逆。

1.齐次矩阵的合成与分解

齐次矩阵的的生成可以一个姿态和一个平移向量组成，因为姿态可以用四元数、欧拉角、轴角、旋转矩阵四种方式来表示

所以我们考虑先将对应的姿态转成旋转矩阵，然后使用numpy讲旋转矩阵和平移向量填写到齐次矩阵对应的位置即可

1.1旋转矩阵+平移向量

#导入库
import numpy as np
import transforms3d as tfs

# 定义旋转矩阵R和平移向量T
R = np.asarray([[1., 0., 0.],[0., 1., 0.],[0., 0., 1.]])
T = np.asarray([1,0,1])
R,T

1.1.1 使用numpy方法合成齐次变换矩阵

temp = np.hstack((R,T.reshape(3,1)))
np.vstack((temp,[0,0,0,1]))

1.1.2 使用tfs中的函数合成齐次变换矩阵

tfs.affines.compose(T,R,[1,1,1])

1.2四元数+平移向量

思路:先将四元数转换成旋转矩阵，然后再利用1.1合成齐次矩阵

R = tfs.quaternions.quat2mat([1,0,0,0])
tfs.affines.compose(T,R,[1,1,1])

1.3 练习

1.3.1 练习1

已知相机坐标系{C}为参考坐标系，工具坐标系{P}的位置矢量在相机坐标系{C}x,y,z各轴投影为，并且工具坐标系和相机坐标系姿态相同，求

1.3.2 练习2

已知机器人基坐标系{B}为参考坐标系，相机坐标系{C}在的位置矢量在{B}各轴的投影为,坐标系{C}和绕着坐标系{B}的x轴转了180度，求

2.齐次矩阵的分解

齐次矩阵的分解指的是已有齐次矩阵的情况下，将其分解为姿态和平移两部分

2.1 将qcjz分解为固定轴欧拉角和平移向量

tfs.euler.mat2euler(T[0:3,0:3]),T[:3,3:4]

2.3 将qcjz分解为四元数和平移向量

tfs.quaternions.mat2quat(T[0:3,0:3]),T[:3,3:4]

3.齐次矩阵的乘法

对应numpy中矩阵的乘法np.dot讲两个矩阵相乘即可，我们以一道例题来讲解这个问题。

3.1 练习-眼在手外

如图