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

  其中（s₁指θ₁，s2指θ₂，以此类推）：   n_x = – cos(s5)*(cos(s4)*(sin(s1)*sin(s3) – cos(s1)*cos(s2)*cos(s3)) + cos(s1)*sin(s2)*sin(s4)) – sin(s5)*(cos(s3)*sin(s1) + cos(s1)*cos(s2)*sin(s3)); n_y = cos(s5)*(cos(s4)*(cos(s1)*sin(s3) + cos(s2)*cos(s3)*sin(s1)) – sin(s1)*sin(s2)*sin(s4)) + sin(s5)*(cos(s1)*cos(s3) – cos(s2)*sin(s1)*sin(s3)); n_z = cos(s5)*(cos(s2)*sin(s4) + cos(s3)*cos(s4)*sin(s2)) – sin(s2)*sin(s3)*sin(s5); o_x = sin(s5)*(cos(s4)*(sin(s1)*sin(s3) – cos(s1)*cos(s2)*cos(s3)) + cos(s1)*sin(s2)*sin(s4)) – cos(s5)*(cos(s3)*sin(s1) + cos(s1)*cos(s2)*sin(s3)); o_y = cos(s5)*(cos(s1)*cos(s3) – cos(s2)*sin(s1)*sin(s3)) – sin(s5)*(cos(s4)*(cos(s1)*sin(s3) + cos(s2)*cos(s3)*sin(s1)) – sin(s1)*sin(s2)*sin(s4)); o_z = – sin(s5)*(cos(s2)*sin(s4) + cos(s3)*cos(s4)*sin(s2)) – cos(s5)*sin(s2)*sin(s3); a_x = sin(s4)*(sin(s1)*sin(s3) – cos(s1)*cos(s2)*cos(s3)) – cos(s1)*cos(s4)*sin(s2); a_y = – sin(s4)*(cos(s1)*sin(s3) + cos(s2)*cos(s3)*sin(s1)) – cos(s4)*sin(s1)*sin(s2); a_z = cos(s2)*cos(s4) – cos(s3)*sin(s2)*sin(s4); P_x = 40*cos(s1)*cos(s2) – (764*cos(s1)*sin(s2))/5; P_y = 40*cos(s2)*sin(s1) – (764*sin(s1)*sin(s2))/5; P_z = (764*cos(s2))/5 + 40*sin(s2);   由于时间关系，暂时先不把具体过程写下来，其实源代码里面全部都有，需要的请先去文章开头的代码仓下载代码来看。   tip： 源码是一个VS工程，基于VS2010创建。   <– 未完待续 –>