六轴UR机械臂标准DH正逆运动学公式推导+代码验证C++ |
您所在的位置:网站首页 › 机器人的运动学模型 › 六轴UR机械臂标准DH正逆运动学公式推导+代码验证C++ |
目录 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个连杆: ai = (沿着Xi轴,从Zi移动到Zi+1的距离) ,即 连杆i的抽象长度。αi= (从Zi旋转到Zi+1的角度,转轴为Xi,右手定则),即 连杆i与连杆i+1的夹角。di = (沿着Zi轴,从Xi-1移动至Xi的距离), Xi-1和Xi都是垂于Zi的。θ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=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 |
今日新闻 |
推荐新闻 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |