• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

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

人工智能 Shawn0102 2675次浏览 0个评论

注:本篇博文全部源码下载地址为:Git Repo。   一、转换矩阵   ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)   经过上一篇博客介绍,我们已经获得了Rob一个手臂的D-H参数表,如上表所示,我们要把这些参数转换成相邻坐标系的转换矩阵,D和H两位老前辈已经推导出通用公式了,通用公式如图1,其中cθi = cos(θi) ,sθi =  sin(θi )。这是一个4×4的矩阵,它表征了相邻两个坐标系的位置和姿态两个维度的转换关系,具体说明见上一篇博文。   ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)

图1

  套用图1中的公式,我们分别得到坐标系{1}相对于坐标系{0}的转换矩阵T1如下:   ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)   同理,我们得到坐标系{2}相对于坐标系{1}的转换矩阵T2,{3}相对于{2}的转换矩阵T3,{4}相对于{3}的转换矩阵T4,{5}相对于{4}的转换矩阵T5,{6}相对于{5}的转换矩阵T6,分别如下 :   ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++) ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++) 至此,我们得到了所有相邻坐标系的转换矩阵,由于他们是串联的,我们可以得到坐标系{6}相对于坐标系{0}的转换矩阵T2:   ROS系统MoveIt玩转双臂机器人系列(六)--D-H逆运动学求解程序(C++)

式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创建。   <– 未完待续 –>   


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS系统MoveIt玩转双臂机器人系列(六)–D-H逆运动学求解程序(C++)
喜欢 (0)

您必须 登录 才能发表评论!

加载中……