c++运动学正反解 ros_ROS系统MoveIt玩转双臂机器人系列（六）–D-H逆运动学求解程序（C++）...

注：本篇博文全部源码下载地址为：Git Repo。

一、转换矩阵

经过上一篇博客介绍，我们已经获得了Rob一个手臂的D-H参数表，如上表所示，我们要把这些参数转换成相邻坐标系的转换矩阵，D和H两位老前辈已经推导出通用公式了，通用公式如图1，其中cθi = cos(θi) ，sθi =  sin(θi )。这是一个4×4的矩阵，它表征了相邻两个坐标系的位置和姿态两个维度的转换关系，具体说明见上一篇博文。

图1

套用图1中的公式，我们分别得到坐标系{1}相对于坐标系{0}的转换矩阵T1如下：

同理，我们得到坐标系{2}相对于坐标系{1}的转换矩阵T2，{3}相对于{2}的转换矩阵T3，{4}相对于{3}的转换矩阵T4，{5}相对于{4}的转换矩阵T5，{6}相对于{5}的转换矩阵T6，分别如下 ：

至此，我们得到了所有相邻坐标系的转换矩阵，由于他们是串联的，我们可以得到坐标系{6}相对于坐标系{0}的转换矩阵T2：

式1

其中(s1指θ1，s2指θ2ÿ