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

最近做Omega6+UR5机械臂虚拟现实遥操作项目需要用到UR5的逆解，在网上找资料自己实现碰到一堆坑，现在终于完美解决。（标准DH参数)DH参数图源《机器人学导论》四个参数中，有两个表示关节轴相对前一关节轴的变化，另外两个表示关节沿轴的运动。下面是对四个参数的理解，参考Universial robot 运动学_Peace-CSDN博客第i个坐标系固连在第i个连杆的左端。轴i固连于i-1杆，在i-

FuYongqing0412

22205人浏览 · 2021-12-24 16:51:25

FuYongqing0412 · 2021-12-24 16:51:25 发布

最近做项目需要用到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=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的变换矩阵 ${ }_{i}^{i-1} T=$

$\left[\begin{array}{cccc} \cos \theta_{i} & -\sin \theta_{i} \cos \alpha_{i} & \sin \theta_{i} \sin \alpha_{i} & a_{i} \cos \theta_{i} \\ \sin \theta_{i} & \cos \theta_{i} \cos \alpha_{i} & -\cos \theta_{i} \sin \alpha_{i} & a_{i} \sin \theta_{i} \\ 0 & \sin \alpha_{i} & \cos \alpha_{i} & d_{i} \\ 0 & 0 & 0 & 1 \end{array}\right]$

所以正运动学可以求得 ${ }_{6}^{0} T={ }_{1}^{0} T \times{ }_{2}^{1} T \times{ }_{3}^{2} T \times{ }_{4}^{3} T \times{ }_{5}^{4} T \times{ }_{6}^{5} T$ ，只要知道六个关节角，就可以求出变换矩阵

${ }_{6}^{0} T=\left[\begin{array}{cccc} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{array}\right]$

代码

可自行下载，链接如下：

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的公式来计算旋转矩阵

则位姿为 $\left[p_{x}, p_{y}, p_{z}, \gamma, \beta, \alpha\right]$ 时，旋转矩阵可以由如下公式计算得出。

${ }_{6}^{0} R=\left[\begin{array}{lll} n_{x} & o_{x} & a_{x} \\ n_{y} & o_{y} & a_{y} \\ n_{z} & o_{z} & a_{z} \end{array}\right]$

$=\left[\begin{array}{ccc} c \alpha c \beta & c \alpha s \beta s \gamma-s \alpha c \gamma & c \alpha s \beta c \gamma+s \alpha s \gamma \\ s \alpha c \beta & s \alpha s \beta s \gamma+c \alpha c \gamma & s \alpha s \beta c \gamma-c \alpha s \gamma \\ -s \beta & c \beta s \gamma & c \beta c \gamma \end{array}\right]$

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

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

关节1的求解

已知

${ }_{6}^{0} T=\left[\begin{array}{cccc} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{array}\right]={ }_{1}^{0} T \cdot{ }_{2}^{1} T \cdot{ }_{3}^{2} T \cdot{ }_{4}^{3} T \cdot{ }_{5}^{4} T \cdot{ }_{6}^{5} T$

则 ${ }_{1}^{0} T^{-1} \cdot T \cdot{ }_{6}^{5} T^{-1}={ }_{5}^{1} T={ }_{2}^{1} T \cdot{ }_{3}^{2} T \cdot{ }_{4}^{3} T \cdot{ }_{5}^{4} T$

${ }_{1}^{0} T^{-1}=\left[\begin{array}{cccc} \cos \theta_{1} & \sin \theta_{1} & 0 & 0 \\ 0 & 0 & 1 & -d_{1} \\ \sin \theta_{1} & -\cos \theta_{1} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right],{ }_{6}^{5} T^{-1}=\left[\begin{array}{cccc} \cos \theta_{6} & \sin \theta_{6} & 0 & 0 \\ -\sin \theta_{6} & \cos \theta_{6} & 0 & 0 \\ 0 & 0 & 1 & -d_{6} \\ 0 & 0 & 0 & 1 \end{array}\right]$

所以 ${ }_{1}^{0} T^{-1} \cdot T \cdot{ }_{6}^{5} T^{-1}=$

$\left[\begin{array}{cccc} c_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)-s_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right) & s_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)+c_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right) & a_{x} c_{1}+a_{y} s_{1} & p_{x} c_{1}-d_{6}\left(a_{x} c_{1}+a_{y} s_{1}\right)+p_{y} s_{1} \\ n_{x} c_{6}-o_{z} s_{6} & o_{z} c_{6}+n_{z} s_{6} & a_{z} & p_{z}-d_{1}-a_{z} d_{6} \\ s_{6}\left(o_{y} c_{1}-o_{x} s_{1}\right)-c_{6}\left(n_{y} c_{1}-n_{x} s_{1}\right) & -s_{6}\left(n_{y} c_{1}-n_{x} s_{1}\right)-c_{6}\left(o_{y} c_{1}-o_{x} s_{1}\right) & a_{x} s_{1}-a_{y} c_{1} & -p_{y} c_{1}+d_{6}\left(a_{y} c_{1}-a_{x} s_{1}\right)+p_{x} s_{1} \\ 0 & 0 & 0 & 1 \end{array}\right]$

而

${ }_{2}^{1} T \cdot{ }_{3}^{2} T \cdot{ }_{4}^{3} T \cdot{ }_{5}^{4} T=\left[\begin{array}{cccc} c_{234} c_{5} & -s_{234} & -c_{234} s_{5} & a_{3} c_{23}+a_{2} c_{2}+d_{2} s_{234} \\ s_{234} c_{5} & c_{234} & -s_{234} s_{5} & a_{3} s_{23}+a_{2} s_{2}-d_{5} c_{234} \\ s_{5} & 0 & c_{5} & d_{4} \\ 0 & 0 & 0 & 1 \end{array}\right]$

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

$d_{4}=-p_{y} c_{1}+d_{6}\left(a_{y} c_{1}-a_{x} s_{1}\right)+p_{x} s_{1}$

另 $m=d_{6} a_{y}-p_{y}, n=a_{x} d_{6}-p_{x}$ ，有

$\theta_{1}=a \tan 2(m, n)-a \tan 2\left(d_{4}, \pm \sqrt{m^{2}+n^{2}-d_{4}^{2}}\right)$

共两组解。

关节5的求解

根据两式第三行第三列相等，有 $c_{5}=a_{x} s_{1}-a_{y} c_{1}$ ，则

$\theta_{5}=\pm \arccos \left(a_{x} s_{1}-a_{y} c_{1}\right)$

至此共四组解。

关节6的求解

根据两式第三行第三列相等，有 $s_{5}=s_{6}\left(o_{y} c_{1}-o_{x} s_{1}\right)-c_{6}\left(n_{y} c_{1}-n_{x} s_{1}\right)$

令 $m m=n_{x} s_{1}-n_{y} c_{1}, \quad n n=o_{x} s_{1}-o_{y} c_{1}$ ，有

$\theta_{6}=a \tan 2(m m, n n)-a \tan 2\left(s_{5}, 0\right)$

至此共四组解。

关节3的求解

由 ${ }_{1}^{0} T^{-1} \cdot T \cdot{ }_{6}^{5} T^{-1} \cdot{ }_{5}^{4} T^{-1}={ }_{4}^{1} T={ }_{2}^{1} T \cdot{ }_{3}^{2} T \cdot{ }_{4}^{3} T$ ，而且

${ }_{5}^{4} T^{-1}=\left[\begin{array}{cccc} \cos \theta_{5} & \sin \theta_{5} & 0 & 0 \\ 0 & 0 & -1 & d_{5} \\ -\sin \theta_{5} & \cos \theta_{5} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]$

所以有 ${ }_{1}^{0} T^{-1} \cdot T \cdot{ }_{6}^{5} T^{-1} \cdot{ }_{5}^{4} T^{-1}=$

$\left[\begin{array}{cc} c_{5}\left(c_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)-s_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right)\right)-s_{5}\left(a_{x} c_{1}+a_{y} s_{1}\right) & s_{5}\left(c_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)-s_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right)\right)+c_{5}\left(a_{x} c_{1}+a_{y} s_{1}\right) \\ c_{5}\left(n_{x} c_{6}-o_{z} s_{6}\right)-s_{5} a_{z} & s_{5}\left(n_{x} c_{6}-o_{z} s_{6}\right)+c_{5} a_{z} \\ c_{5}\left(s_{6}\left(o_{y} c_{1}-o_{x} s_{1}\right)-c_{6}\left(n_{y} c_{1}-n_{x} s_{1}\right)\right)-s_{5}\left(a_{x} s_{1}-a_{y} c_{1}\right) & s_{5}\left(s_{6}\left(o_{y} c_{1}-o_{x} s_{1}\right)-c_{6}\left(n_{y} c_{1}-n_{x} s_{1}\right)\right)+c_{5}\left(a_{x} s_{1}-a_{y} c_{1}\right) \\ 0 & 0 \end{array}\right.$ $\left.\begin{array}{cc} -\left(s_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)+c_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right)\right) & d_{5}\left(s_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)+c_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right)\right)+p_{x} c_{1}-d_{6}\left(a_{x} c_{1}+a_{y} s_{1}\right)+p_{y} s_{1} \\ -\left(o_{z} c_{6}+n_{z} s_{6}\right) & d_{5}\left(o_{z} c_{6}+n_{z} s_{6}\right)+p_{z}-d_{1}-a_{z} d_{6} \\ s_{6}\left(n_{y} c_{1}-n_{x} s_{1}\right)+c_{6}\left(o_{y} c_{1}-o_{x} s_{1}\right) & d_{5}\left(-s_{6}\left(n_{y} c_{1}-n_{x} s_{1}\right)-c_{6}\left(o_{y} c_{1}-o_{x} s_{1}\right)\right)-p_{y} c_{1}+d_{6}\left(a_{y} c_{1}-a_{x} s_{1}\right)+p_{x} s_{1} \\ 0 & 1 \end{array}\right]$

因为

${ }_{2}^{1} T \cdot{ }_{3}^{2} T \cdot{ }_{4}^{3} T=\left[\begin{array}{cccc} c_{234} & 0 & s_{234} & a_{3} c_{23}+a_{2} c_{2} \\ s_{234} & 0 & -c_{234} & a_{3} s_{23}+a_{2} s_{2} \\ 0 & 1 & 0 & d_{4} \\ 0 & 0 & 0 & 1 \end{array}\right]$

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

$\begin{aligned} &d_{5}\left(s_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)+c_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right)\right)+p_{x} c_{1}-d_{6}\left(a_{x} c_{1}+a_{y} s_{1}\right)+p_{y} s_{1}=a_{3} c_{23}+a_{2} c_{2} \\ &d_{5}\left(o_{z} c_{6}+n_{z} s_{6}\right)+p_{z}-d_{1}-a_{z} d_{6}=a_{3} s_{23}+a_{2} s_{2} \end{aligned}$

令

$m m m=d_{5}\left(s_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)+c_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right)\right)+p_{x} c_{1}-d_{6}\left(a_{x} c_{1}+a_{y} s_{1}\right)+p_{y} s_{1}$

$\text { nnn }=d_{5}\left(o_{z} c_{6}+n_{z} s_{6}\right)+p_{z}-d_{1}-a_{z} d_{6}$

则有

$\theta_{3}=\pm \arccos \left(\frac{m m m^{2}+n n n^{2}-a_{2}^{2}-a_{3}^{2}}{2 a_{2} a_{3}}\right)$

至此共八组解。

关节2的求解

由上述推导，可令

$s_{2}=\frac{\left(a_{3} c_{3}+a_{2}\right) n n n-a_{3} s_{3} m m m}{a_{2}^{2}+a_{3}^{2}+2 a_{2} a_{3} c_{3}}, \quad c_{2}=\frac{m m m+a_{3} s_{3} s_{2}}{a_{3} c_{3}+a_{2}}$

所以

$\theta_{2}=a \tan 2\left(s_{2}, c_{2}\right)$

至此共八组解。

关节4的求解

由 ${ }_{1}^{0} T^{-1} \cdot T \cdot{ }_{6}^{5} T^{-1}={ }_{5}^{1} T={ }_{2}^{1} T \cdot{ }_{3}^{2} T \cdot{ }_{4}^{3} T \cdot{ }_{5}^{4} T$ ，可设

$\begin{gathered} -s_{234}=s_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)+c_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right) \\ c_{234}=o_{z} c_{6}+n_{z} s_{6} \end{gathered}$

所以

$\theta_{4}=a \tan 2\left(-s_{6}\left(n_{x} c_{1}+n_{y} s_{1}\right)-c_{6}\left(o_{x} c_{1}+o_{y} s_{1}\right), o_{z} c_{6}+n_{z} s_{6}\right)-\theta_{2}-\theta_{3}$