机器人学笔记-4逆运动学

---

好，现在你已经掌握了从关节空间到末端位姿的映射了，差不多就是一个little baby知道自己的胳膊这个样子转动可以到达那个位置啦，但是你该怎么用你的手去握住奶瓶捏（还好有个瓶，小baby哪要考虑审核呀）？这就是逆运动学的问题，也是实际作业中真实的问题。
讲人话就是：对于有n个关节的机械臂，我们已知从关节空间映射到末端位姿的函数T(q),这个通过DH表或者运动旋量的PoE方式可以简单得到，对一个可达的末端位姿X，求解这个未知数为n维向量的高阶非线性方程组： $T(q)=X$,就是逆运动学的问题了。
很难，就算不是非线性方程，线性方程求解还要考虑n是不是等于6，等于6是个方阵还要考虑是不是奇异矩阵，不是6或者不是线性就更不用说了，难，使人听此凋朱颜。

1. 求解方案

1. 迭代
   小baby是怎么做到的呢？小baby可不会解方程啊，很简单，小baby生来带着一台高性能计算机，人家先差不多给各个关节一个初始值，够不到就微调，说人话就是迭代，甚至是带反馈的迭代，智能算法~
   用指数方程搭配牛顿-拉弗森方法可以轻松迭代，当然这对计算机要求很高，除非不要求实时响应。
   想了解可以参考 Morden Robotics, B站上也有配套的公开课。
2. 运动学解耦
   这是一种很多经典机械臂采用的方案，对一个六轴机械臂，如果末端三个旋转轴相交于一点，也就是设计成一种球形手腕，和基座相连的前三个轴则用来确定球形手腕的中心位置，而那三个相交的旋转轴和前三个关节确定的旋转矩阵共同确定末端位姿。
   这就把逆运动学分解（解耦）为两个子问题：先从球形手腕中心位置求解前三个关节的坐标，根据前三个关节确定的旋转矩阵和末端位姿求解出球形手腕对应欧拉角的位姿，再对欧拉角矩阵求逆运算解出最后三个关节变量。

下面主要介绍可以解耦的逆运动学方法：

2. 前三个关节的逆向位置求解

我们已知的是末端位姿，也就是位置 $o_6^0$ ,和姿态 $R_6^0$ ,最开始我们利用 $o_6^0$ 可以求解出球形手腕的坐标 $o_c^0$ 因为$o_6^0$ 是第六个坐标系的原点，也就是最后一个坐标系的原点，而$o_c^0$ 是坐标系3、4、5共同的原点，根据DH约定，我们经常规定最后一个Z轴 $z_6$ 和 $z_5$ 共线，那两原点之间只需在$z_6$ 方向平移 $d_6$ 就可以了。
$z_6$ 方向在基坐标系里怎么表示？这就用到旋转矩阵列向量的本质了，其实 $R_6^0$ 的第三列就是$z_6$ 在坐标系0里的向量对应的坐标。
所以我们可以求出球形手腕中心坐标：
$$\begin{array}{c}{o}_6^0=o_c^0+d_6\left(R\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\right)\\ \\ \Rightarrow o_c^0=o_6^0-d_6\left(R\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\right)=\left[\begin{array}{c}{\mathrm{x}}_c\\ y_c\\ z_c\end{array}\right]\end{array}$$
对不同的运动学配置（也就是机械臂机构的结构RRR、RRP啥的）有不同的解法，但只要是人设计出来的，求解都比较简单：

2.1 一种RRR结构配置求解

如图：

已知球形手腕 $z_c$的位置，求 ${\theta }_1,{\theta }_2,{\theta }_3$
我们可以用简单的几何方法求解：
观察俯视图：

显然
$$\begin{array}{l}{\theta }_1=A\tan 2(x_c,y_c)\\ \\ {\theta }_1=A\tan 2(x_c,y_c)+\pi \end{array}$$
这两个结果差了180度，如果第二、三个关节不变的话球形手腕的位置就变成中心对称的了，所以实际上这两个不同的第一关节角对应的剩余两个关节角也要对称排布。

---

这里Atan2（x,y）是一个有两个参数的反正切函数：
if x>=0
Atan2(x,y)=arctan $\frac{y}{x}$
else
Atan2(x,y)=arctan $\frac{y}{x}$+$\pi$
其实就是x（也就是 $cos\theta$ ）大于零的时候（$-\frac{\pi }{2}<=\theta <=\frac{\pi }{2}$）Atan2和arctan返回一样的值（$-\frac{\pi }{2}<=\theta <=\frac{\pi }{2}$）
x小于零的时候，返回反正切函数值域加上 $\pi$之后的值，这样值域就变成了 $[-\frac{\pi }{2},\frac{3\pi }{2}]$

---

对一些具有偏置的机构，第一个关节角也会有两个解，被称为左型、右型手臂位形，差不多这个样子：

---

对于剩下的 ${\theta }_2,\theta 3$:

就到了我们中学送分大题，解三角形了
有以下结果：
$$\begin{array}{l}r=\sqrt{x_c^2+y_c^2}\\ \\ s=z_c-d_1\\ \\ \cos {\theta }_3=\frac{r^2+s^2-a_2^2-a_3^2}{2a_2{a}_3}=D\\ \\ \sin {\theta }_3=\pm \sqrt{1-{\mathrm{D}}^2}\\ \\ {\theta }_3=A\tan 2(D,\pm \sqrt{1-{\mathrm{D}}^2})\\ \\ {\theta }_2=A\tan 2(r,s)-A\tan (a_2+a_3{c}_3,a_3{s}_3)\end{array}$$
ps：r、s在第一个图里画出来了，自己参考，求s的时候用了 $z_c-d_1$,这里的 $d_1$是DH参数，最好可以一眼就看出来他对应哪里的长度，还是讲个熟练度啊。
psps：这里第三个关节角有两个解，对应了肘部向上和肘部向下两个情况，如果再有个偏执算起来就有4个解了。

至此只用几何方法就把第一个求逆子问题解决了，相当简单和高效。

---

2.2 作业（大同小异，不想写了 : ) ）

对于另一种球坐标形式的机械臂，你要不试试怎么求逆（md，体会到老师留作业的快感了）

3. 逆向姿态

回到我们的问题
知道末端位姿，也就是位置和姿态，末端的位置简化为了求球形手腕的位置对应的前三个关节变量现在，我们已经求出来了 ${\theta }_1,{\theta }_2,{\theta }_3$, 所以通过DH法我们可以求出 $R_3^0$
现在的问题就是：已知 $R_6^0$ 和 $R_3^0$, 求球形手腕的三个欧拉角：
三个欧拉角决定的旋转矩阵其实就是 $R_6^3$ ,所以：
$$\begin{array}{c}{R}_6^0=R_3^0{R}_6^3\\ \\ \text{得到：}{R}_6^3=(R_3^0{)}^T{R}_6^0\end{array}$$

$R_6^3$ 如果是ZYZ欧拉角生成的话（我见到的好像大部分都是ZYZ），我们把老朋友搬过来：
$$\mathrm{R}=\left[\begin{array}{ccc}{c}_{\varphi }{c}_{\theta }{c}_{\psi }-s_{\varphi }{s}_{\psi }& -c_{\varphi }{c}_{\theta }{s}_{\psi }-s_{\varphi }{c}_{\psi }& c_{\varphi }{s}_{\theta }\\ s_{\varphi }{c}_{\theta }{c}_{\psi }+c_{\varphi }{c}_{\psi }& -s_{\varphi }{c}_{\theta }{s}_{\psi }+c_{\varphi }{c}_{\psi }& s_{\varphi }{s}_{\theta }\\ -s_{\theta }{c}_{\psi }& s_{\theta }{s}_{\psi }& c_{\theta }\end{array}\right]$$
我们接下来的任务就是求三个欧拉角 $\varphi ,\theta ,\psi$
下文中R中的元素用 $r_{ij}$ 来表示：

• case1：
  当 $\theta$不为零的时候
  $$\theta =A\tan 2(r_{33},\sqrt{1-r_{33}^2}),\text{或}\theta =A\tan 2(r_{33},-\sqrt{1-r_{33}^2})$$

1. $s_{\theta }>0$
   这时：
   $$\begin{array}{l}\varphi =A\tan 2(r_{13},r_{23})\\ \\ \psi =A\tan 2(-r_{31},r_{32})\end{array}$$
2. $s_{\theta }<0$
   这时
   $$\begin{array}{l}\varphi =A\tan 2(-r_{13},-r_{23})\\ \\ \psi =A\tan 2(r_{31},r_{32})\end{array}$$
   区分这两种情况是因为Atan2的算法中第一个参数的正负会影响结果的区间，所以 $r_{13},r_{31}$的符号会受 $s_{\theta }$影响，所以分两个情形进行修正。

• case2
  当 $\theta$为零的时候
  这时候旋转矩阵退化为
  $$\left[\begin{array}{ccc}{c}_{\varphi +\psi }& -s_{\varphi +\psi }& 0\\ s_{\varphi +\psi }& c_{\varphi +\psi }& 0\\ 0& 0& 1\end{array}\right]\text{或者}\left[\begin{array}{ccc}-c_{\varphi -\psi }& -s_{\varphi -\psi }& 0\\ s_{\varphi -\psi }& c_{\varphi -\psi }& 0\\ 0& 0& -1\end{array}\right]$$
  分别可以解出 $\varphi +\psi \text{和}\varphi -\psi$的值
  这时候有无穷多解

---

至此逆运动学结束。