动手学齐次坐标变换

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

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

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])

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

2.1 练习-眼在手外

如图🔓示，已知：

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

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

可以参考下图看题目

求：

{B}为参考坐标系，坐标系{P}的位置矢量和旋转矩阵

解体思路也很简单，我们只要得出

得到之后将两者相乘即可得出： $$ ^B_PT=^B_CT^C_PT $$ 求出$^B_PT$我们再将其分解成位置矢量和旋转矩阵即可

动手写代码：

3.齐次矩阵求逆 3.1练习-眼在手上

4.练习 4.1 map坐标系转换 4.2 机械臂运动学正解

技术交流&&问题求助：