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

一、机械臂的逆解

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

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

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

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

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

        建立直角坐标系后，机械臂末端的位置，也就是坐标系3原点的位置可表示为：
<script type="math/tex; mode=display" id="MathJax-Element-1">\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}</script>
        已知 px <script type="math/tex" id="MathJax-Element-2">{{p}_{x}}</script>， py <script type="math/tex" id="MathJax-Element-3">{{p}_{y}}</script>， pz <script type="math/tex" id="MathJax-Element-4">{{p}_{z}}</script>3个已知量，求解3个未知量关节角度 ϑ1 <script type="math/tex" id="MathJax-Element-5">{{\vartheta }_{1}}</script> ， ϑ2 <script type="math/tex" id="MathJax-Element-6">{{\vartheta }_{2}}</script> ， ϑ3 <script type="math/tex" id="MathJax-Element-7">{{\vartheta }_{3}}</script>。 从代数的角度来看，由于限定关节角度的范围为 [−π,π] <script type="math/tex" id="MathJax-Element-8">\left[ -\pi ,\pi \right]</script> ，故会有多解，从几何角度来看，就是如下图所示的多解情形。



      求解的过程是从关节3 → <script type="math/tex" id="MathJax-Element-9">\to </script>关节2 → <script type="math/tex" id="MathJax-Element-10">\to</script>关节1。 首先求解关节3的解：
      由 (1)2+(2)2+(3)2 <script type="math/tex" id="MathJax-Element-11">{{(1)}^{2}}+{{(2)}^{2}}+{{(3)}^{2}}</script> 可得 <script type="math/tex; mode=display" id="MathJax-Element-12">{{c}_{3}}=\frac{{{p}_{x}}^{2}+{{p}_{y}}^{2}+{{p}_{z}}^{2}-{{a}_{2}}^{2}-{{a}_{3}}^{2}}{2{{a}_{2}}{{a}_{3}}}</script>
      那么 s3=±1−c32−−−−−−√ <script type="math/tex" id="MathJax-Element-13">{{s}_{3}}=\pm \sqrt{1-{{c}_{3}}^{2}}</script>。
      故 ϑ3,I=atan2（s3，c3） <script type="math/tex" id="MathJax-Element-14">{{\vartheta }_{3,I}}=atan2（s3，c3）</script>，使用 atan2() <script type="math/tex" id="MathJax-Element-15">atan2()</script>函数是为了得到范围在 [−π,π] <script type="math/tex" id="MathJax-Element-16">\left[ -\pi ,\pi \right]</script>的关节角度。根据 s3 <script type="math/tex" id="MathJax-Element-17">{{s}_{3}}</script>的符号和 atan2() <script type="math/tex" id="MathJax-Element-18">atan2()</script>的性质，可知关节3的第二个解是 ϑ3,II=atan2（−s3，c3）=−atan2(s3,c3)=−ϑ3,I <script type="math/tex" id="MathJax-Element-19">{{\vartheta }_{3,II}}=atan2（-s3，c3）=-atan2(s3,c3)=-{\vartheta }_{3,I}</script>。 到此，关节3的两个解就得出来了。
       接下来求关节2的解：
      由 (1)2+(2)2 <script type="math/tex" id="MathJax-Element-20">{{\left( 1 \right)}^{2}}+{{\left( 2 \right)}^{2}}</script>可以消掉 s1 <script type="math/tex" id="MathJax-Element-21">s_1</script>和 c1 <script type="math/tex" id="MathJax-Element-22">c_1</script>，再结合 (3) <script type="math/tex" id="MathJax-Element-23">(3)</script>，这两个方程含 c2 <script type="math/tex" id="MathJax-Element-24">c_2</script>和 s2 <script type="math/tex" id="MathJax-Element-25">s_2</script>两个未知数，可求得 <script type="math/tex; mode=display" id="MathJax-Element-26">{{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}}}</script>
<script type="math/tex; mode=display" id="MathJax-Element-27">{{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}}}</script>
      由 ϑ2=atan2（s2，c2） <script type="math/tex" id="MathJax-Element-28">{{\vartheta }_{2}}=atan2（s2，c2）</script>可求得关节2的值，但是由于 s3 <script type="math/tex" id="MathJax-Element-29">s_{3}</script>的符号和 px2+py2−−−−−−−√ <script type="math/tex" id="MathJax-Element-30">\sqrt{{{p}_{x}}^{2}+{{p}_{y}}^{2}}</script>前的符号会造成多解，当 s3+=1−c32−−−−−−√ <script type="math/tex" id="MathJax-Element-31">{{s}_{3}}^{+}=\sqrt{1-{{c}_{3}}^{2}}</script>时：
<script type="math/tex; mode=display" id="MathJax-Element-32">{{\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}}^{+})</script>
<script type="math/tex; mode=display" id="MathJax-Element-33">{{\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}}^{+})</script>
      当 s3−=−1−c32−−−−−−√ <script type="math/tex" id="MathJax-Element-34">{{s}_{3}}^{-}=-\sqrt{1-{{c}_{3}}^{2}}</script>时，
<script type="math/tex; mode=display" id="MathJax-Element-35">{{\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}}^{+})</script>
<script type="math/tex; mode=display" id="MathJax-Element-36">{{\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}}^{-})</script>
       在列出关节2的解的时候要注意，因为从直观上看 c2 <script type="math/tex" id="MathJax-Element-37">c_2</script>和 s2 <script type="math/tex" id="MathJax-Element-38">s_2</script>的组合一共有8种（4个 c2 <script type="math/tex" id="MathJax-Element-39">c_2</script>，2个 s2 <script type="math/tex" id="MathJax-Element-40">s_2</script>），由于需要满足三角平方公式，所以只有4种是合法的。
      最后求解关节1的值，和教材上的求解方法不同，我更愿意在几何上直观快速地求解出其值，根据机械臂末端在X-Y平面上投影可知其中一个解是：
<script type="math/tex; mode=display" id="MathJax-Element-41">{{\vartheta }_{1,I}}=atan2({{p}_{y}},{{p}_{x}})</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-42">{{\vartheta }_{1,II}}=atan2({{-p}_{y}},{{-p}_{x}})</script>。
      最后我们需要获得机械臂的解，首先说结论，这四个解是：
<script type="math/tex; mode=display" id="MathJax-Element-43">\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}</script>
      前面根据关节3的解后可以确定关节2的解，再结合 px <script type="math/tex" id="MathJax-Element-44">{{p}_{x}}</script>、 py <script type="math/tex" id="MathJax-Element-45">{{p}_{y}}</script>和 (a2c2+a3c23) <script type="math/tex" id="MathJax-Element-46">\left( {{a}_{2}}{{c}_{2}}+{{a}_{3}}{{c}_{23}} \right)</script>的符号确定关节1的值，即可得到上述4组解，如下图所示。

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