UR3机器人运动学分析之逆运动学分析

2 逆运动学分析

  逆运动学就是已知末端执行器相对于基坐标系的位置和姿态，求对应的六个关节角度。设末端执行器相对于基坐标系的位姿矩阵为：

2.1 逆运动学求解

2.1.1 关节1的求解

已知

将式(2)两边分别左乘和右乘，有：

因为：

所以有：

式(6)中，各简式分别为：

而式(3)右边结果为：

由式(6)和式(8)第二行第四列元素相等，可以得到：

令，则式(9)可化为：

利用辅助角公式可将上式化为：

即：

其中，，，则：

由式(12)可得：

由式(12)和式(14)可得：

即：

将式(13)代入上式可求得：

共两个解。

2.1.2 关节5的求解

由式(6)和式(8)第二行第二列元素相等，可以得到：

由于已知，则可求得为：

共两个解。

2.1.3 关节6的求解

由式(6)和式(8)第二行第一列元素相等，可以得到：

由式(6)和式(8)第二行第三列元素相等，可以得到：

令，，式(20)和式(21)可写成：


当时，将式(22)和式(23)两边平方后相加，可得：

由于式(22)和式(10)形式类似，因此，可得到：

共一个解。

若，此时机器人处于奇异位置。

2.1.4 关节3的求解

对式(3)两边同时右乘可得：

因为为：

所以式(26)左边为：

式(28)中部分简式如下：

由于下述计算过程并未使用矩阵中其它元素，故此处未给出。

式(26)右边为：

由式(28)和式(30)第一行第四列元素和第三行第四列元素分别相等，得：

两边平方后相加得到：

注意到.
所以为：

共两个解。

2.1.5 关节2的求解

利用三角函数公式，方程组式(31)可以化为：

求解方程组式(34)，结果如下：

则为：

共一个解。

2.1.6 关节4的求解

由式(6)和式(8)第一行第三列元素和第三行第三列元素分别相等，可以得到：

则为：

共一个解。

2.1.7 总结

综上所述，可以得到六个关节角的解析解表达式如下：

其中，，。

其中，，。

其中，，。

其中，，。

其中，，。

  在上述求解公式中，由于、、存在“”号的影响，所以该机器人对同一种末端位姿可能存在八组解。

2.2 程序编写

  在2.1.7节中已经给出各个关节角的解析解表达式，利用表达式编写逆运动学求解程序，由于UR3机器人位姿表示方法采用旋转矢量形式，因此程序有齐次变换矩阵及旋转矢量两种输入方式，程序流程图如下图所示。

  注意到求解的表达式为：

如果

但是余弦函数的值域为，所以式(46)无意义，这种情况下解不存在。同理，求解时也存在同样的问题。也就是说，八组解析解并不一定全都存在。

  同时，由于计算机内部存储数据类型的原因，当求解出的角度为零时实际存储的数值并不一定是零，由于误差的累计，后面角度的求解结果会产生较大的偏差，因此在每个角度求解完毕后需要添加如下程序块进行手动判零：

if abs(theta_5) < eps
	theta_5 = 0;
end

2.3 验证

  采用正逆运动学相互求解的方法验证解的正确与否。首先利用正运动学求得关节角为时的位姿矩阵为：

  利用上述位姿矩阵进行逆运动学求解，求得八组解如下所示:

  分别将每一组解利用正运动学计算位姿矩阵，发现得到的位姿矩阵同初始位姿矩阵相同，证明逆运动学解析解表达式无误。

源码请前往MyShare/UR3 at master · TyroGzl/MyShare (github.com)

如无法访问，也可关注公众号【G的科研生活】，回复【UR3】