六轴UR机械臂正逆运动学公式推导+代码验证C++
六轴UR机械臂正逆运动学公式推导+代码验证C++
本文详细介绍了UR5机械臂的正逆运动学公式推导及代码验证过程。通过DH参数建立机械臂的坐标系模型，推导出正运动学和逆运动学的计算方法，并通过URSim进行测试验证。文章内容详尽，适合对机器人学和机械臂控制感兴趣的读者。
DH参数介绍
改进DH
参考《机器人学导论》中的改进DH参数方法，具体如下：
- 第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系的关系；
UR5的官方DH参数如下表所示 ，是改进DH参数。其他型号可以参考Universal Robots - DH Parameters for calculations of kinematics and dynamics
下图是官方给的UR机械臂坐标系位置
标准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
https://www.youtube.com/watch?v=rA9tm0gTln8
正运动学
已知关节角度求变换矩阵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