UR机械臂正逆运动学求解

       最近有个任务：求解UR机械臂正逆运动学，在网上参考了一下大家的求解办法，众说纷纭，其中有些朋友求解过程非常常规，但是最后求解的8组解，只有4组可用。在这里我介绍一个可以求解8组解析解的方法，供大家参考。

       以UR5机械臂结构和尺寸参数为例进行正逆运动学求解，下图分别是UR5结构图和标准DH系参数：

 

1. 正运动学求解

正运动学是已知关节六个角度求变换矩阵T

其中：

整理得：

带入DH参数，求解：

        

最终变换矩阵：

正运动学求解完毕。

2. 逆运动学求解

逆运动学是已知变换矩阵T，求六个关节角度 。逆运动学求解有解析法，几何法，迭代法，这里采用解析法求解。

2.1 两个简单的数学方法

2.1.1 求角度

 这个逆运动学算法求解的角度范围是

 

因为标准的反正切arctan的值域是

所以不能使用，这里介绍一个改进的反正切求法 Atan2(y, x)（Matlab里有这个函数）,它的值域可以满足要求。

2.1.2 解方程

首先进行三角恒等变换，令

其中：

然后带入原方程：

则

2.2 约定

为了简化书写，约定：

2.3 求解1，5，6关节角度

已知：

则

其中：

等式左边：

等式右边：

2.3.1 求关节角1

利用等式左右两边第3行，第4列对应相等求关节角1。

整理得：

设：

则  

根据前面介绍的解方程的方法：

2.3.2 求关节角5

利用等式左右两边第3行，第3列对应相等求关节角5。

解得：

2.3.3 求关节角6

利用等式左右两边第3行，第1列对应相等求关节角6。

设：

则

根据前面介绍的方法：

其实可以通过化简得到式中

则

2.4 求解2，3，4关节角度

已知：

则

其中：

等式左边等于

等式右边等于

2.4.1 求解关节角3

利用等式左右两边第1行，第4列对应相等，第2行，第4列对应相等，求关节角3。

为了简化，设：

将m,n带入上式得

式子③④平方和为

因为

所以

2.4.2  求解关节角2

将③④展开得：

将关节角3带入⑤⑥，求关节角2得

则

2.4.3  求解关节角4

用的第2行第2列，第1行第2列求 

则

2.5 总结

2.5.1 求解公式

2.5.2 奇异位置

1.肩关节奇异位置

此时末端执行器参考点O6位于轴线z1和z2构成的平面内，关节角1无法求解。

2.肘关节奇异位置

此时关节角2无法求解。

3.腕关节奇异位置

此时轴线z4和z6平行，关节角6无法求解。

2.6 实例

利用Matlab机器人库 ur5 DH参数：

alpha1 = pi/2;          a1=0;                 d1=89.459;

alpha2 = 0;              a2=-425;           d2=0;

alpha3 = 0;              a3=-392.25;      d3=0;

alpha4 = pi/2;           a4=0;               d4=109.15;

alpha5 = -pi/2;          a5=0;               d5=94.65;

alpha6 = 0;               a6=0;               d6=82.3;

取 theta1 = 1;  theta2 = 1;  theta3 = 1;  theta4 = 1;  theta5 = 1;  theta6 = 1;  (不要纠结theta选这6个数值是否有实际意义，这里只验证算法的有效性)

• 将theta带入正运动学公式，求T:

• 将T带入逆运动学公式, 反求theta

theta = 

• 再将8个theta带入正运动学公式，反求8个T:

8个T均等于

验证了算法的有效性