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

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

图1 工业机械臂

图2 拟人臂+球形手腕

        我们已经知道拟人臂（链接）和球形手腕（链接）的求逆解方法，现在介绍一种方法可以将求6个关节的值的步骤分成以上求臂和求手腕两个过程。对带球形腕的机械臂而言，可以将一个点 W W <script type="math/tex" id="MathJax-Element-18">W</script>定位于3个手腕关节的转轴交点，一旦末端执行器的位置和姿态由pe$p_e$<script type="math/tex" id="MathJax-Element-19">p_e</script>和 Re=[neseae] R e = [ n e s e a e ] <script type="math/tex" id="MathJax-Element-20">R_e=\begin{bmatrix} n_e&s_e&a_e\\ \end{bmatrix}</script> （或者 Re=[x6y6z6] R e = [ x 6 y 6 z 6 ] <script type="math/tex" id="MathJax-Element-21">R_e=\begin{bmatrix} x_6&y_6&z_6\\ \end{bmatrix}</script>）指定，就可以找到手腕的位置为： pw=pe−d6ae p w = p e − d 6 a e <script type="math/tex" id="MathJax-Element-22">p_w=p_e-d_6a_e</script>(或 pw=pe−d6z6 p w = p e − d 6 z 6 <script type="math/tex" id="MathJax-Element-23">p_w=p_e-d_6z_6</script>)，如图1所示，可按照如下伪代码来编程，即可求得拟人臂的8组解，然后根据实际情况来删减不符合的解。

        具体求解的伪代码如下：

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

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