【机器人学】使用解析法求解6轴机械臂的逆运动学解

        本文是承接上一篇求3轴拟人机械臂逆解内容（链接），扩展到求6轴机械臂的逆解，研究的仍然是目前比较流行的工业机械臂构型：拟人臂+球形腕关节（如下图1和图2所示），因为这种构型的机械臂具有闭合形式的逆运动学解，并且可以将机械臂末端的位置和方向分开求解，即根据手腕的位置可求出前三个关节的值，根据手腕的姿态可求出后三个关节（手腕关节）的值。

图1 工业机械臂

图2 拟人臂+球形手腕

        我们已经知道拟人臂（链接）和球形手腕（链接）的求逆解方法，现在介绍一种方法可以将求6个关节的值的步骤分成以上求臂和求手腕两个过程。对带球形腕的机械臂而言，可以将一个点$W$定位于3个手腕关节的转轴交点，一旦末端执行器的位置和姿态由$p_e$和$R_e=\begin{bmatrix} n_e&s_e&a_e\\ \end{bmatrix}$ （或者$R_e=\begin{bmatrix} x_6&y_6&z_6\\ \end{bmatrix}$）指定，就可以找到手腕的位置为：$p_w=p_e-d_6a_e$(或$p_w=p_e-d_6z_6$)，如图1所示，可按照如下伪代码来编程，即可求得拟人臂的8组解，然后根据实际情况来删减不符合的解。

        具体求解的伪代码如下：

         需要注意的是，在建立机械臂坐标系的时候，可以按照图2所示建立坐标系，但是会发现图2中拟人臂和腕关节的第3个坐标系是不一致的，在建立坐标系的时候可以在这两个坐标系之间增加一个常变换矩阵，这样可以使两坐标系共线，这一操作会影响求解代码过程中的第4个步骤。

参考文献：
布鲁诺・西西里安诺.《机器人学：建模，规划和控制》 西安交通大学出版社 2015