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

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

一、机械臂的逆解

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

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

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

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

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

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

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

\begin{align}_{x}}={{c}_{1}}\left( {{a}_{2}}{{c}_{2}}+{{a}_{3}}{{c}_{23}} \right) \\ _{y}}={{s}_{1}}\left( {{a}_{2}}{{c}_{2}}+{{a}_{3}}{{c}_{23}} \right) \\ & pz={{a}_{2}}{{s}_{2}}+{{a}_{3}}{{s}_{23}}\\ \end{align}
已知 px{{p}_{x}}， py{{p}_{y}}， pz{{p}_{z}}3个已知量，求解3个未知量关节角度 ϑ1{{\vartheta }_{1}} ， ϑ2{{\vartheta }_{2}} ， ϑ3{{\vartheta }_{3}}。 从代数的角度来看，由于限定关节角度的范围为[−π,π]\left[ -\pi ,\pi \right] ，故会有多解，从几何角度来看，就是如下图所示的多解情形。

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

c3=px2+py2+pz2−a22−a322a2a3

{{c}_{3}}=\frac{{{p}_{x}}^{2}+{{p}_{y}}^{2}+{{p}_{z}}^{2}-{{a}_{2}}^{2}-{{a}_{3}}^{2}}{2{{a}_{2}}{{a}_{3}}}
      那么 s3=±1−c32−−−−−−√{{s}_{3}}=\pm \sqrt{1-{{c}_{3}}^{2}}。
      故 ϑ3,I=atan2（s3，c3）{{\vartheta }_{3,I}}=atan2（s3，c3），使用 atan2()atan2()函数是为了得到范围在 [−π,π]\left[ -\pi ,\pi \right]的关节角度。根据 s3{{s}_{3}}的符号和 atan2()atan2()的性质，可知关节3的第二个解是 ϑ3,II=atan2（−s3，c3）=−atan2(s3,c3)=−ϑ3,I{{\vartheta }_{3,II}}=atan2（-s3，c3）=-atan2(s3,c3)=-{\vartheta }_{3,I}。 到此，关节3的两个解就得出来了。
       接下来求关节2的解：
      由 (1)2+(2)2{{\left( 1 \right)}^{2}}+{{\left( 2 \right)}^{2}}可以消掉 s1s_1和 c1c_1，再结合 (3)(3)，这两个方程含 c2c_2和 s2s_2两个未知数，可求得

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

{{c}_{2}}=\frac{\pm \sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}}\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)+{{p}_{z}}{{a}_{3}}{{s}_{3}}}{{{a}_{2}}^{2}+{{a}_{3}}^{2}+2{{a}_{2}}{{a}_{3}}{{c}_{3}}}

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

{{s}_{2}}=\frac{\pm \sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}}{{a}_{3}}{{s}_{3}}+{{p}_{z}}\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)}{{{a}_{2}}^{2}+{{a}_{3}}^{2}+2{{a}_{2}}{{a}_{3}}{{c}_{3}}}
由 ϑ2=atan2（s2，c2）{{\vartheta }_{2}}=atan2（s2，c2）可求得关节2的值，但是由于 s3s_{3}的符号和 px2+py2−−−−−−−√\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}}前的符号会造成多解，当 s3+=1−c32−−−−−−√{{s}_{3}}^{+}=\sqrt{1-{{c}_{3}}^{2}}时：

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

{{\vartheta }_{2,I}}=atan2({{p}_{z}}\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)-{{a}_{3}}{{s}_{3}}^{+}\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}},\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}}+{{p}_{z}}{{a}_{3}}{{s}_{3}}^{+})

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

{{\vartheta }_{2,II}}=atan2({{p}_{z}}\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)+{{a}_{3}}{{s}_{3}}^{+}\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}},-\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}}+{{p}_{z}}{{a}_{3}}{{s}_{3}}^{+})
当 s3−=−1−c32−−−−−−√{{s}_{3}}^{-}=-\sqrt{1-{{c}_{3}}^{2}}时，

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

{{\vartheta }_{2,III}}=atan2({{p}_{z}}\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)-{{a}_{3}}{{s}_{3}}^{-}\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}},\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}}+{{p}_{z}}{{a}_{3}}{{s}_{3}}^{+})

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

{{\vartheta }_{2,IV}}=atan2({{p}_{z}}\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)+{{a}_{3}}{{s}_{3}}^{-}\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}},-\left( {{a}_{2}}+{{a}_{3}}{{c}_{3}} \right)\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}}+{{p}_{z}}{{a}_{3}}{{s}_{3}}^{-})
在列出关节2的解的时候要注意，因为从直观上看c2c_2和s2s_2的组合一共有8种（4个c2c_2，2个s2s_2），由于需要满足三角平方公式，所以只有4种是合法的。
最后求解关节1的值，和教材上的求解方法不同，我更愿意在几何上直观快速地求解出其值，根据机械臂末端在X-Y平面上投影可知其中一个解是：

ϑ1,I=atan2(py,px)

{{\vartheta }_{1,I}}=atan2({{p}_{y}},{{p}_{x}})

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

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

{{\vartheta }_{1,II}}=atan2({{-p}_{y}},{{-p}_{x}})。
最后我们需要获得机械臂的解，首先说结论，这四个解是：

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

\begin{align}& \left( \begin{matrix}{{\vartheta }_{1,I}} _{2,I}} _{3,I}} \\ \end{matrix} \right) \\ & \left( \begin{matrix}{{\vartheta }_{1,I}} _{2,III}} _{3,II}} \\ \end{matrix} \right) \\ & \left( \begin{matrix}{{\vartheta }_{1,II}} _{2,II}} _{3,I}} \\ \end{matrix} \right) \\ & \left( \begin{matrix}{{\vartheta }_{1,II}} _{2,IV}} _{3,II}} \\ \end{matrix} \right) \\ \end{align}
前面根据关节3的解后可以确定关节2的解，再结合 px{{p}_{x}}、 py{{p}_{y}}和 (a2c2+a3c23)\left( {{a}_{2}}{{c}_{2}}+{{a}_{3}}{{c}_{23}} \right)的符号确定关节1的值，即可得到上述4组解，如下图所示。

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