目录
DH参数介绍
正运动学
代码
测试
逆运动学
变换矩阵
关节1的求解
关节5的求解
关节6的求解
关节3的求解
关节2的求解
关节4的求解
正逆解完整代码
测试结果
不足之处
一些有用链接
最近做项目需要用到UR5的逆解,在网上找资料自己实现碰到一堆坑,现在终于完美解决。本文用的是标准DH参数,包含从末端位姿变换到关节角的全部过程,网上许多教程都是用正运动学算出的变换矩阵进行逆运动学求解,在我的项目中完全没有办法应用。其中UR末端姿态有两种表示方法,这也是排查了好久才找到的问题。现在将完整过程分享出来,希望可以帮到和我遇到同样问题的小伙伴。URsim使用及安装可参考我另一篇博客URSim+Unity联合仿真以及Socket通信_sinat_32804425的博客-CSDN博客
(改进DH参数方法可参考UR3机械臂正逆运动学详解及c++完整代码_Scolin丶-CSDN博客_ur3机械臂)
图源《机器人学导论》
参考Universial robot 运动学_Peace-CSDN博客,此处是为了方便对比。
前两个参数的作用是建立 i系与i+1系的关系;
后两个参数的作用是建立i-1系与i系的关系;
图源https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters
坐标系的建立:
z轴指向旋转轴。
基座的x轴可以自由选择,之后的x轴与两个连续z轴的公垂线共线。
原点选在公垂线与新z轴的交点,注意不在关节的中心,可以在空间的任何地方。
y轴由右手定则给出。
特殊情况:如果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型六轴机械臂运动学推导及效果演示 - 古月居
已知
则
所以
而
根据两式第三行第四列相等,则可以得到
另,有
共两组解。
根据两式第三行第三列相等,有,则
至此共四组解。
根据两式第三行第三列相等,有
令,有
至此共四组解。
由,而且
所以有
因为
根据两式第一行第四列相等,第二行第四列相等,有
令
则有
至此共八组解。
由上述推导,可令
所以
至此共八组解。
由,可设
所以
总共八组解。
按照上面公式,即可完美求解。代码链接如下:
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