【机器人学】使用代数法求解3自由度拟人机械臂的逆运动学解

        这篇博客会讨论一下使用解析法求解3自由度拟人机械臂的逆解及分析。

一、机械臂的逆解

        机械臂的逆运动学问题就是由给定的末端执行器位置和方向，确定机械臂各个关节变量的值。机械臂的求解方法可以分为两大类：数值解和解析解（封闭解），解析解又可分为代数解和几何解。
        数值解和解析解有各自的特点，商用的机械臂一般都会采用解析解，因为求解速度快且准确，而不会采用本质迭代的数值解法。

二、拟人机械臂+球形手腕

        如果大家注意到的话，可以发现，市面上的6轴机械臂的构型都是相似的，大都是拟人臂+球形手腕的构型，因为这样的构型可以把球形腕的求逆解耦出来。

三、3自由度拟人机械臂的求解

        这篇博客的内容都出自于《机器人学：建模，规划和控制》这本教材，有兴趣的同学可以找到这本书看一下。本文只讨论一下3自由度拟人机械臂的逆运动学，以后有时间会总结一下6轴工业机械臂的求逆解问题。
        很多机器人学教材上关于机械臂的逆解都是罗列一堆公式，对其中的推导过程并没有讲述很清楚，再加上中文版本的机器人学教材错误百出，很多同学看得一头雾水，对求解推导过程中使用的数学原理没有深入体会，导致对机械臂的求逆解问题一直处于一种似懂非懂的状态。最重要的是，我认为只有使用代码实现对机械臂求逆解，才算是初步达到了学习机器人学的目的。
        3轴机械臂的构型和DH参数如下图所示，使用DH方法进行运动学建模，建立的坐标系如下图所示。

        建立直角坐标系后，机械臂末端的位置，也就是坐标系3原点的位置可表示为：

px=c1(a2c2+a3c23)py=s1(a2c2+a3c23)pz=a2s2+a3s23

        已知 px ， py ， pz 3个已知量，求解3个未知量关节角度 ϑ1 ， ϑ2 ， ϑ3 。 从代数的角度来看，由于限定关节角度的范围为 [−π,π] ，故会有多解，从几何角度来看，就是如下图所示的多解情形。



      求解的过程是从关节3 → 关节2 → 关节1。 首先求解关节3的解：
      由 (1)2+(2)2+(3)2 可得

c3=px2+py2+pz2−a22−a322a2a3

      那么 s3=±1−c32−−−−−−√ 。
      故 ϑ3,I=atan2（s3，c3） ，使用 atan2() 函数是为了得到范围在 [−π,π] 的关节角度。根据 s3 的符号和 atan2() 的性质，可知关节3的第二个解是 ϑ3,II=atan2（−s3，c3）=−atan2(s3,c3)=−ϑ3,I 。 到此，关节3的两个解就得出来了。
       接下来求关节2的解：
      由 (1)2+(2)2 可以消掉 s1 和 c1 ，再结合 (3) ，这两个方程含 c2 和 s2 两个未知数，可求得

c2=±px2+py2−−−−−−−√(a2+a3c3)+pza3s3a22+a32+2a2a3c3

s2=±px2+py2−−−−−−−√a3s3+pz(a2+a3c3)a22+a32+2a2a3c3

      由 ϑ2=atan2（s2，c2） 可求得关节2的值，但是由于 s3 的符号和 px2+py2−−−−−−−√ 前的符号会造成多解，当 s3+=1−c32−−−−−−√ 时：

ϑ2,I=atan2(pz(a2+a3c3)−a3s3+px2+py2−−−−−−−√,(a2+a3c3)px2+py2−−−−−−−√+pza3s3+)

ϑ2,II=atan2(pz(a2+a3c3)+a3s3+px2+py2−−−−−−−√,−(a2+a3c3)px2+py2−−−−−−−√+pza3s3+)

      当 s3−=−1−c32−−−−−−√ 时，

ϑ2,III=atan2(pz(a2+a3c3)−a3s3−px2+py2−−−−−−−√,(a2+a3c3)px2+py2−−−−−−−√+pza3s3+)

ϑ2,IV=atan2(pz(a2+a3c3)+a3s3−px2+py2−−−−−−−√,−(a2+a3c3)px2+py2−−−−−−−√+pza3s3−)

       在列出关节2的解的时候要注意，因为从直观上看 c2 和 s2 的组合一共有8种（4个 c2 ，2个 s2 ），由于需要满足三角平方公式，所以只有4种是合法的。
      最后求解关节1的值，和教材上的求解方法不同，我更愿意在几何上直观快速地求解出其值，根据机械臂末端在X-Y平面上投影可知其中一个解是：

ϑ1,I=atan2(py,px)

      而另外一个解是位于相对于原点对称的那个象限里，可以用

ϑ1,II=atan2(−py,−px)

。
      最后我们需要获得机械臂的解，首先说结论，这四个解是：

(ϑ1,Iϑ2,Iϑ3,I)(ϑ1,Iϑ2,IIIϑ3,II)(ϑ1,IIϑ2,IIϑ3,I)(ϑ1,IIϑ2,IVϑ3,II)

      前面根据关节3的解后可以确定关节2的解，再结合 px 、 py 和 (a2c2+a3c23) 的符号确定关节1的值，即可得到上述4组解，如下图所示。

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