草庐IT

六轴UR机械臂标准DH正逆运动学公式推导+代码验证C++

FuYongqing0412 2023-08-02 原文

目录

DH参数介绍

 正运动学

代码

测试

 逆运动学

变换矩阵

关节1的求解

关节5的求解

关节6的求解

关节3的求解

关节2的求解

关节4的求解

正逆解完整代码

测试结果

不足之处

一些有用链接


最近做项目需要用到UR5的逆解,在网上找资料自己实现碰到一堆坑,现在终于完美解决。本文用的是标准DH参数,包含从末端位姿变换到关节角的全部过程,网上许多教程都是用正运动学算出的变换矩阵进行逆运动学求解,在我的项目中完全没有办法应用。其中UR末端姿态有两种表示方法,这也是排查了好久才找到的问题。现在将完整过程分享出来,希望可以帮到和我遇到同样问题的小伙伴。URsim使用及安装可参考我另一篇博客URSim+Unity联合仿真以及Socket通信_sinat_32804425的博客-CSDN博客

(改进DH参数方法可参考UR3机械臂正逆运动学详解及c++完整代码_Scolin丶-CSDN博客_ur3机械臂)

DH参数介绍

改进DH

 图源《机器人学导论》

参考Universial robot 运动学_Peace-CSDN博客,此处是为了方便对比。

  •  第i个坐标系固连在第i个连杆的左端
  • 轴i固连于i-1杆,在i-1杆的右端。 
  • i坐标系固定在i杆上,随i杆转动。

每个连杆有四个参数,第i个连杆:

  1. ai = (沿着Xi轴,从Zi移动到Zi+1的距离)  ,即 连杆i的抽象长度。
  2. αi= (从Zi旋转到Zi+1的角度,转轴为Xi,右手定则),即 连杆i与连杆i+1的夹角。
  3. di = (沿着Zi轴,从Xi-1移动至Xi的距离), Xi-1和Xi都是垂于Zi的。
  4. θi = (绕Zi轴,从Xi-1旋转至Xi的角度),即连杆i-1与连杆i的夹角。

前两个参数的作用是建立 i系与i+1系的关系;

后两个参数的作用是建立i-1系与i系的关系; 

标准DH

 图源https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters

坐标系的建立:

z轴指向旋转轴。

基座的x轴可以自由选择,之后的x轴与两个连续z轴的公垂线共线。

原点选在公垂线与新z轴的交点,注意不在关节的中心,可以在空间的任何地方

y轴由右手定则给出。

有了这些关节坐标系,就可以得到由以下四个变量决定的关节之间的转换:

  • di : 沿着Zi-1轴,原点Oi-1至公垂线的距离。
  • θi : 绕Zi-1轴,从Xi-1旋转至Xi的角度。
  • ai : 沿着Xi轴,从Zi-1移动至Zi的距离, 即 连杆Linki关于Zi-1的旋转半径。
  • αi : 绕Xi轴,从Zi-1旋转到Zi的角度。

特殊情况:如果Z轴是平行的,那选择最方便的那个di。此时的αi一定是0。

辅助理解动画

https://www.youtube.com/watch?v=rA9tm0gTln8https://www.youtube.com/watch?v=rA9tm0gTln8

下图是官方给的UR机械臂坐标系位置

 UR5的DH参数如下表所示 ,其他型号可以参考Universal Robots - DH Parameters for calculations of kinematics and dynamics

 正运动学

已知关节角度求变换矩阵T,根据标准DH参数法,可以得到坐标系i-1到i的变换矩阵

 所以正运动学可以求得,只要知道六个关节角,就可以求出变换矩阵

代码

可自行下载,链接如下:

https://download.csdn.net/download/sinat_32804425/70045943

测试

在URSim上打开UR5的虚拟机,进入到move界面,选择Base基座坐标系,随意设置一个姿态,将六个关节角输入,可以检验正运动学计算正确与否。

 运行正运动学程序,输入:93.14 -62.68 108.27 -135.56 -66.46 15.59

 将变换矩阵T前三行打印出来,可以看到最后的位置与URSim中TCP的三个位置相同。

 逆运动学

逆运动学就是已知末端的位姿,求六个关节角的过程,一般都会求出八组解,但是只有一组是最优解,通常试两次就可确定哪一组。

变换矩阵

根据末端执行器的位姿(绕静轴的姿态),求解末端坐标系到基座坐标系的变换矩阵。这里推荐一个视频,可以帮助理解绕静轴和 绕动轴两种变换方式【机械臂运动学教程】机械臂+旋转矩阵+变换矩阵+DH+逆解+轨迹规划+机器人+教程_哔哩哔哩_bilibili

在URSim中也有两种表示位姿的方式,一种是Rotation Vector,另一种是RPY。所谓RPY就是绕参考坐标系,绕定轴X(Roll)—Y(Pitch)—Z(Yaw)旋转,旋转矩阵左乘。我们计算变换矩阵一定要采用RPY[rad]这种模式下的姿态角,要不然始终会有问题。RPY是绕静轴的一种旋转方式,理解可参考机器人RPY角和Euler角 -- 基本公式_lyhbkz的博客-CSDN博客_rpy【机器人运动学/姿态角】欧拉角和RPY角_Amanda1m的博客-CSDN博客_rpy角

 我这里采用绕静轴RPY的公式来计算旋转矩阵

 则位姿为时,旋转矩阵可以由如下公式计算得出。

至此得到了变换矩阵T,下面开始逐步计算关节角。下列公式参考UR5型六轴机械臂运动学推导及效果演示 - 古月居

求逆解时一定要用atan2()来求解,Eigen库中自带这个函数。原理可参考UR机械臂正逆运动学求解_fengyu19930920的博客-CSDN博客_机械臂正逆运动学

关节1的求解

已知

则                                                                                                

所以

根据两式第三行第四列相等,则可以得到

,有

共两组解。

关节5的求解

根据两式第三行第三列相等,有,则

至此共四组解。

关节6的求解

根据两式第三行第三列相等,有

,有

至此共四组解。

关节3的求解

,而且

所以有

因为

根据两式第一行第四列相等,第二行第四列相等,有

 

则有

至此共八组解。

关节2的求解

由上述推导,可令

所以

至此共八组解。

关节4的求解

,可设

所以

总共八组解。

正逆解完整代码

按照上面公式,即可完美求解。代码链接如下:

https://download.csdn.net/download/sinat_32804425/70047254

测试结果

输入:0.17269 -0.55555 0.11106 3.025 0.395 2.901

结果如下,可以看出,第三组与URSim中的完美匹配。

不足之处

没有考虑奇异解的特殊情况,后面需要的话会再进行研究。

其他UR构型的机械臂换一下DH参数应该也是也可用这种方法来求解的。有问题的话可以评论区交流~~

一些有用链接

UR运动学GitHub代码

https://github.com/ros-industrial/universal_robot/blob/kinetic-devel/ur_kinematics/src/ur_kin.cpp

 手册下载

UR Download | Support Site | Universal Robots

有关六轴UR机械臂标准DH正逆运动学公式推导+代码验证C++的更多相关文章

  1. ruby - 如何在 buildr 项目中使用 Ruby 代码? - 2

    如何在buildr项目中使用Ruby?我在很多不同的项目中使用过Ruby、JRuby、Java和Clojure。我目前正在使用我的标准Ruby开发一个模拟应用程序,我想尝试使用Clojure后端(我确实喜欢功能代码)以及JRubygui和测试套件。我还可以看到在未来的不同项目中使用Scala作为后端。我想我要为我的项目尝试一下buildr(http://buildr.apache.org/),但我注意到buildr似乎没有设置为在项目中使用JRuby代码本身!这看起来有点傻,因为该工具旨在统一通用的JVM语言并且是在ruby中构建的。除了将输出的jar包含在一个独特的、仅限ruby​​

  2. ruby-on-rails - Rails 源代码 : initialize hash in a weird way? - 2

    在rails源中:https://github.com/rails/rails/blob/master/activesupport/lib/active_support/lazy_load_hooks.rb可以看到以下内容@load_hooks=Hash.new{|h,k|h[k]=[]}在IRB中,它只是初始化一个空哈希。和做有什么区别@load_hooks=Hash.new 最佳答案 查看rubydocumentationforHashnew→new_hashclicktotogglesourcenew(obj)→new_has

  3. ruby-on-rails - 浏览 Ruby 源代码 - 2

    我的主要目标是能够完全理解我正在使用的库/gem。我尝试在Github上从头到尾阅读源代码,但这真的很难。我认为更有趣、更温和的踏脚石就是在使用时阅读每个库/gem方法的源代码。例如,我想知道RubyonRails中的redirect_to方法是如何工作的:如何查找redirect_to方法的源代码?我知道在pry中我可以执行类似show-methodmethod的操作,但我如何才能对Rails框架中的方法执行此操作?您对我如何更好地理解Gem及其API有什么建议吗?仅仅阅读源代码似乎真的很难,尤其是对于框架。谢谢! 最佳答案 Ru

  4. ruby - 模块嵌套代码风格偏好 - 2

    我的假设是moduleAmoduleBendend和moduleA::Bend是一样的。我能够从thisblog找到解决方案,thisSOthread和andthisSOthread.为什么以及什么时候应该更喜欢紧凑语法A::B而不是另一个,因为它显然有一个缺点?我有一种直觉,它可能与性能有关,因为在更多命名空间中查找常量需要更多计算。但是我无法通过对普通类进行基准测试来验证这一点。 最佳答案 这两种写作方法经常被混淆。首先要说的是,据我所知,没有可衡量的性能差异。(在下面的书面示例中不断查找)最明显的区别,可能也是最著名的,是你的

  5. ruby - 寻找通过阅读代码确定编程语言的ruby gem? - 2

    几个月前,我读了一篇关于ruby​​gem的博客文章,它可以通过阅读代码本身来确定编程语言。对于我的生活,我不记得博客或gem的名称。谷歌搜索“ruby编程语言猜测”及其变体也无济于事。有人碰巧知道相关gem的名称吗? 最佳答案 是这个吗:http://github.com/chrislo/sourceclassifier/tree/master 关于ruby-寻找通过阅读代码确定编程语言的rubygem?,我们在StackOverflow上找到一个类似的问题:

  6. ruby - Net::HTTP 获取源代码和状态 - 2

    我目前正在使用以下方法获取页面的源代码:Net::HTTP.get(URI.parse(page.url))我还想获取HTTP状态,而无需发出第二个请求。有没有办法用另一种方法做到这一点?我一直在查看文档,但似乎找不到我要找的东西。 最佳答案 在我看来,除非您需要一些真正的低级访问或控制,否则最好使用Ruby的内置Open::URI模块:require'open-uri'io=open('http://www.example.org/')#=>#body=io.read[0,50]#=>"["200","OK"]io.base_ur

  7. ruby - 将 spawn() 的标准输出/标准错误重定向到 Ruby 中的字符串 - 2

    我想使用spawn(针对多个并发子进程)在Ruby中执行一个外部进程,并将标准输出或标准错误收集到一个字符串中,其方式类似于使用Python的子进程Popen.communicate()可以完成的操作。我尝试将:out/:err重定向到一个新的StringIO对象,但这会生成一个ArgumentError,并且临时重新定义$stdxxx会混淆子进程的输出。 最佳答案 如果你不喜欢popen,这是我的方法:r,w=IO.pipepid=Process.spawn(command,:out=>w,:err=>[:child,:out])

  8. 程序员如何提高代码能力? - 2

    前言作为一名程序员,自己的本质工作就是做程序开发,那么程序开发的时候最直接的体现就是代码,检验一个程序员技术水平的一个核心环节就是开发时候的代码能力。众所周知,程序开发的水平提升是一个循序渐进的过程,每一位程序员都是从“菜鸟”变成“大神”的,所以程序员在程序开发过程中的代码能力也是根据平时开发中的业务实践来积累和提升的。提高代码能力核心要素程序员要想提高自身代码能力,尤其是新晋程序员的代码能力有很大的提升空间的时候,需要针对性的去提高自己的代码能力。提高代码能力其实有几个比较关键的点,只要把握住这些方面,就能很好的、快速的提高自己的一部分代码能力。1、多去阅读开源项目,如有机会可以亲自参与开源

  9. ruby-on-rails - 标准化文件名的字符串,删除重音和特殊字符 - 2

    我正在尝试找到一种方法来规范化字符串以将其作为文件名传递。到目前为止我有这个:my_string.mb_chars.normalize(:kd).gsub(/[^\x00-\x7F]/n,'').downcase.gsub(/[^a-z]/,'_')但第一个问题:-字符。我猜这个方法还有更多问题。我不控制名称,名称字符串可以有重音符、空格和特殊字符。我想删除所有这些,用相应的字母('é'=>'e')替换重音符号,并将其余的替换为'_'字符。名字是这样的:“Prélèvements-常规”“健康证”...我希望它们像一个没有空格/特殊字符的文件名:“prelevements_routin

  10. 7个大一C语言必学的程序 / C语言经典代码大全 - 2

    嗨~大家好,这里是可莉!今天给大家带来的是7个C语言的经典基础代码~那一起往下看下去把【程序一】打印100到200之间的素数#includeintmain(){ inti; for(i=100;i 【程序二】输出乘法口诀表#includeintmain(){inti;for(i=1;i 【程序三】判断1000年---2000年之间的闰年#includeintmain(){intyear;for(year=1000;year 【程序四】给定两个整形变量的值,将两个值的内容进行交换。这里提供两种方法来进行交换,第一种为创建临时变量来进行交换,第二种是不创建临时变量而直接进行交换。1.创建临时变量来

随机推荐