ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)

ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)

 

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

一、转换矩阵

 ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)_第1张图片

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

ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)_第2张图片

图1

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

ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)_第3张图片

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

            ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)_第4张图片

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

ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)_第5张图片式1

其中(s1指θ1,s2指θ2,以此类推):

  nx = - 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));
  ny = 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));
  nz = cos(s5)*(cos(s2)*sin(s4) + cos(s3)*cos(s4)*sin(s2)) - sin(s2)*sin(s3)*sin(s5);
  ox = 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));
  oy = 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));
  oz = - sin(s5)*(cos(s2)*sin(s4) + cos(s3)*cos(s4)*sin(s2)) - cos(s5)*sin(s2)*sin(s3);
  ax = sin(s4)*(sin(s1)*sin(s3) - cos(s1)*cos(s2)*cos(s3)) - cos(s1)*cos(s4)*sin(s2);
  ay = - sin(s4)*(cos(s1)*sin(s3) + cos(s2)*cos(s3)*sin(s1)) - cos(s4)*sin(s1)*sin(s2);
  az = cos(s2)*cos(s4) - cos(s3)*sin(s2)*sin(s4);
  Px = 40*cos(s1)*cos(s2) - (764*cos(s1)*sin(s2))/5; 

  Py = 40*cos(s2)*sin(s1) - (764*sin(s1)*sin(s2))/5; 

  Pz = (764*cos(s2))/5 + 40*sin(s2);

 

由于时间关系,暂时先不把具体过程写下来,其实源代码里面全部都有,需要的请先去文章开头的代码仓下载代码来看。

tip:

  源码是一个VS工程,基于VS2010创建。

<-- 未完待续 --> 

 

 

 

 

欢迎留言、私信、邮箱、微信等任何形式的技术交流。

作者信息:

名称:Shawn

邮箱:[email protected]

微信二维码:↓

          

 

 

 

 

 

 

posted on 2018-08-26 10:31 Shawn0102 阅读( ...) 评论( ...) 编辑 收藏

转载于:https://www.cnblogs.com/shawn0102/p/9392884.html

你可能感兴趣的:(ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++))