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

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

一、机械臂的逆解

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

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

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

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

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

        建立直角坐标系后，机械臂末端的位置，也就是坐标系3原点的位置可表示为：
$$\begin{align} & {{p}_{x}}={{c}_{1}}\left( {{a}_{2}}{{c}_{2}}+{{a}_{3}}{{c}_{23}} \right) \\ & {{p}_{y}}={{s}_{1}}\left( {{a}_{2}}{{c}_{2}}+{{a}_{3}}{{c}_{23}} \right) \\ & pz={{a}_{2}}{{s}_{2}}+{{a}_{3}}{{s}_{23}}\\ \end{align}$$
        已知${{p}_{x}}$，${{p}_{y}}$，${{p}_{z}}$3个已知量，求解3个未知量关节角度${{\vartheta }_{1}}$ ，${{\vartheta }_{2}}$ ，${{\vartheta }_{3}}$。从代数的角度来看，由于限定关节角度的范围为$\left[ -\pi ,\pi \right]$ ，故会有多解，从几何角度来看，就是如下图所示的多解情形。



      求解的过程是从关节3$\to$关节2$\to$关节1。首先求解关节3的解：
      由${{(1)}^{2}}+{{(2)}^{2}}+{{(3)}^{2}}$ 可得 $${{c}_{3}}=\frac{{{p}_{x}}^{2}+{{p}_{y}}^{2}+{{p}_{z}}^{2}-{{a}_{2}}^{2}-{{a}_{3}}^{2}}{2{{a}_{2}}{{a}_{3}}}$$
      那么${{s}_{3}}=\pm \sqrt{1-{{c}_{3}}^{2}}$。
      故${{\vartheta }_{3,I}}=atan2（s3，c3）$，使用$atan2()$函数是为了得到范围在$\left[ -\pi ,\pi \right]$的关节角度。根据${{s}_{3}}$的符号和$atan2()$的性质，可知关节3的第二个解是${{\vartheta }_{3,II}}=atan2（-s3，c3）=-atan2(s3,c3)=-{\vartheta }_{3,I}$。到此，关节3的两个解就得出来了。
      接下来求关节2的解：
      由${{\left( 1 \right)}^{2}}+{{\left( 2 \right)}^{2}}$可以消掉$s_1$和$c_1$，再结合$(3)$，这两个方程含$c_2$和$s_2$两个未知数，可求得 $${{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}}}$$
$${{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}}}$$
      由${{\vartheta }_{2}}=atan2（s2，c2）$可求得关节2的值，但是由于$s_{3}$的符号和$\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}}$前的符号会造成多解，当${{s}_{3}}^{+}=\sqrt{1-{{c}_{3}}^{2}}$时：
$${{\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}}^{+})$$
$${{\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}}^{+})$$
      当${{s}_{3}}^{-}=-\sqrt{1-{{c}_{3}}^{2}}$时，
$${{\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}}^{+})$$
$${{\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的解的时候要注意，因为从直观上看$c_2$和$s_2$的组合一共有8种（4个$c_2$，2个$s_2$），由于需要满足三角平方公式，所以只有4种是合法的。
      最后求解关节1的值，和教材上的求解方法不同，我更愿意在几何上直观快速地求解出其值，根据机械臂末端在X-Y平面上投影可知其中一个解是：
$${{\vartheta }_{1,I}}=atan2({{p}_{y}},{{p}_{x}})$$

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

$${{\vartheta }_{1,II}}=atan2({{-p}_{y}},{{-p}_{x}})$$ 。
      最后我们需要获得机械臂的解，首先说结论，这四个解是：
$$\begin{align} & \left( \begin{matrix} {{\vartheta }_{1,I}} & {{\vartheta }_{2,I}} & {{\vartheta }_{3,I}} \\ \end{matrix} \right) \\ & \left( \begin{matrix} {{\vartheta }_{1,I}} & {{\vartheta }_{2,III}} & {{\vartheta }_{3,II}} \\ \end{matrix} \right) \\ & \left( \begin{matrix} {{\vartheta }_{1,II}} & {{\vartheta }_{2,II}} & {{\vartheta }_{3,I}} \\ \end{matrix} \right) \\ & \left( \begin{matrix} {{\vartheta }_{1,II}} & {{\vartheta }_{2,IV}} & {{\vartheta }_{3,II}} \\ \end{matrix} \right) \\ \end{align}$$
      前面根据关节3的解后可以确定关节2的解，再结合${{p}_{x}}$、${{p}_{y}}$和$\left( {{a}_{2}}{{c}_{2}}+{{a}_{3}}{{c}_{23}} \right)$的符号确定关节1的值，即可得到上述4组解，如下图所示。



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