机器人逆运动学

4.1 本章任务

本章是将思路逆转过来，已知机械臂的位置和姿态，计算关节角，由此即为逆运动学。

这个任务可以分为两个小问：第一，进行坐标系变换，求出相对于基坐标系｛B｝的腕部坐标系｛W｝；然后，应用逆运动学求关节角。

4.2 解的存在性

首先，操作臂运动学方程是一个非线性问题，已知$^0_NT$，求出$\theta_1,\theta_2,\theta_3...,\theta_n$。

着重考虑如下方程：

对于具有6个自由度的操作臂来说，有12个方程，其中6个是未知的。然而，在由$^0_6T$的旋转矩阵分量生成的9个方程中，只有3个是独立的。将这3个方程与由$^0_T$的位置矢量分量生成的3个方程联立，6个方程中含有6个未知量。这些方程为非线性超越方程，难以求解。 而现实的问题可能要更复杂，因为连杆偏距、长度、$\alpha_i$不一定为0。所以，我们需要考虑解的存在性，并且解决多解问题。

解的存在性

解的存在必须满足：被指定的目标点必须在工作空间（操作臂末端执行器所能到达的范围）内。

工作空间有两种，一是灵巧工作空间，指的是机器人的末端执行器能够从各个方向到达的空间区域。二是可达工作空间，指的是其末端执行器至少从一个方向能到达的区域。

上图操作臂的工作空间：

如果$l_1=l_2$，则可达工作空间是半径$2l_1$的圆，而灵巧工作空间仅是单独的一点，这就是原点。

如果$l_1≠l_2$，则不存在灵巧工作空间，可达工作空间为外径$l_1+l_2$、内径为$\lvert{l}_1-l_2\rvert$的圆环。在可达工作空间的内部，末端执行器有2种可能的方向，而在工作空间的边界上只有一种可能的方向。

需要注意的是这里的假设：所有关节能够旋转360°，但这并不常见。所以实际上能够成立的姿态会减少。而且，有一些运动范围是联动的。

操作臂少于6自由度时，它不能达到三维空间内一般的目标位置和姿态。上图的操作臂自由度被限制，不能伸出平面，Z坐标不为0的点都到达不了。我们只有这一种办法—逐个研究这种操作臂，弄清楚它的工作空间。还有就是，计算最近的可达目标点来研究操作臂可以执行近似操作的区域。

还有一个问题，那就是工具坐标系也决定了工作空间。作为技术人员可能更关心腕部坐标系｛W｝的计算，但是实际上工具执行的任务可能更为重要，如果腕部坐标系被工具坐标系的工作空间包含，那么至少存在一个解。

多解问题

3个旋转关节的平面操作臂已经可以从任何姿态到达工作空间的任何位置，赋予了其较大的灵巧工作空间，如下图所示：

虚线表示的是机械臂的第二个位形。此位形下，末端执行器（工具）可达位置和姿态与第一个位形相同。

多解问题，意味着选择。系统必须选择一个最终解作为执行目标，因此，我们需要讨论如何产生最优解。我们知道，机械臂应当选取最易到达的位姿进行解算，所以我们需要的是最近解。这牵扯到的问题即是如何定义最近解。 不同的机械臂可能有不同的机构设计，比如配备小连杆和大连杆两种连杆，这种情况下，当然是移动小连杆更经济。所以，我们需要对连杆大小加权计算，给小连杆更高的权重。这是理想的状况，现实是，小连杆移动的解可能造成碰撞。此时，我们需要选择备用解——也就是“较远”解。所以设计算法的时候，需要对全部解进行保留操作。

不只是关节数量影响了解的数量，连杆参数和关节运动范围也造成了影响。换言之，解是关节数量、连杆参数和关节运动范围的函数。PUMA 560机器人就是一个好的例子，末端执行器姿态、位置相同，但是机械臂可以以4种方式达到这一位姿。

$${\theta}_4'={\theta}_4+180°$$$${\theta}_5'={\theta}_5$$$${\theta}_6'={\theta}_6+180°$$

连杆的非零参数越多，到达某一特定目标的方式也越多。下图表示，解的最大数量和等于0的连杆参数的数量有关：不等于0的连杆参数的数量越多，解的最大数量就越多。

解法

非线性方程组没有通用的求解算法，所以，最好对操作臂“解”的形式加以定义。

解的求解程序必须是能够算出所有解，能够算出与已知位置和姿态相关的全部关节变量。这样，就可以说操作臂是可解的。

操作臂的全部求解方法分为两大类：封闭解和数值解，然而数值解具有迭代性质，它一般比相应封闭解的求解速度慢上许多。实际情况是封闭解往往更受青睐。

本章主要讨论封闭解方法，封闭解指的是基于解析形式的解法，对不高于4次的多项式，不需要迭代即可完全求解。

封闭解的求解方法又可以分为两类：代数法和几何法，而几何法引入了代数描述，所以这两种方法是相似的，或许仅仅是求解过程不同。

我们可以利用以下结论：所有包含转动关节和移动关节的串联型6自由度机构均是可解的。但是这种解一般是数值解。特殊情况下，可以产生解析解。存在解析解的机械臂具有以下特性：

存在几个正交关节轴或者有多个$\alpha_i$为0或者$±90°$。

最重要且最基本的，是保证封闭解存在。所以，目前的机械臂设计比较简单，这样能够保证得到封闭解。

具有6个旋转关节的操作臂存在封闭解的充分条件是相邻的三根关节轴相交于一点。

4.3 当n≤6时操作臂子空间的描述

对于一个n自由度操作臂（n<6），可达工作空间可看成是n自由度子空间的一部分。我们可以回想在图4.1中出现的机械臂，其工作空间是该平面的一个子集，即当$l_1=l_2$时为一个半径为$l_1+l_2$的圆。

确定n自由度操作臂子空间的一种方法就是给出腕部坐标系或工具坐标系的表达式，它是含有n个变量的函数。将这n个变量看作自由变量，其所有可能的取值构成了这个子空间。

对于具有n自由度操作臂的目标点进行定义，通常采用n个参数来确定这个目标点。也就是说，如果给定6个自由度的目标点，一般自由度n＜6的操作臂无法到达这个目标点。此时，可以寻找近似解，尽可能“靠近”目标点。

对于少于6个自由度的操作臂来说，当确定一般目标点时，可以如此求解：

1. 已知一般目标坐标系$^S_GT$，计算一个修正的目标坐标系$^S_{G^{'}}T$，使得$^S_{G^{'}}T$位于操作臂子空间内，并且和$^S_GT$尽可能“靠近”，应该预先确定“靠近”的标准。
2. 将$^S_{G'}T$作为期望目标，计算逆运动学来求关节角，注意，如果目标点不在操作臂工作空间内，将可能没有解。

先确定工具坐标系原点到期望目标点的位置，然后选择一个接近期望姿态的可达姿态。子空间的计算取决于操作臂的几何特征，对于每个操作臂必须单独考虑。

4.4 代数解法和几何解法

本节我们讨论运动学方程的两大求解方法。

代数解法

采用第三章介绍的三连杆平面操作臂，它的连杆参数如下图所示。

如果你忘了$\alpha_i$、$a_i$、$d_i$和$\theta_i$的含义，请你翻阅一下《机器人学导论》的第三章或直接翻开我的上一篇笔记：【机器人学导论笔记】三、操作臂正运动学，找到“3.4 连杆坐标系的定义”。

在这里，$\alpha_i$为0的原因是平面坐标系的$\hat{Z_i}$轴都是垂直纸面的，也就不需要旋转。$a_i$是连杆坐标系原点公垂线距离，此处是连杆长度。$d_i$为0的原因是因为沿$\hat{Z_i}$轴$\hat{X}$轴并没有移动（所有$\hat{Z_i}$轴都处于一个平面上）。$\theta_i$是$\hat{X_i}$绕$\hat{Z_i}$轴转过的角度，大小不一。

应用连杆参数，求得机械臂的运动学方程（还记得吗？连乘变换矩阵就能得到了）：

为了集中讨论逆运动学问题，假设必要的变换已经完成，即$^B_WT$已经完成，使得目标点的位置由腕部坐标系相对基坐标系来确定，由于研究的是平面操作臂，因此，可以通过确定三个量——$x、y$和$\phi$很容易确定这些目标点的位置，其中$\phi$是连杆3在平面内的姿态（与$+\hat{X}$相关）。因此，假定变换矩阵具有以下形式：

所有可达目标点必须位于上式描述的子空间上，令式(4.6)和(4.7)相等，从而求得4个非线性方程，进而求出$\theta_1$、$\theta_2$和$\theta_3$：

$$c_{\phi}=c_{123}(4.8)$$$$s_{\phi}=s_{123}(4.9)$$$$x=l_1c_1+l_2c_{12}(4.10)$$$$x=l_1s_1+l_2s_{12}(4.11)$$

使用代数方法求解（4.8）-（4.11），将（4.10）和（4.11）同时平方，然后相加得到：

$$x^2+y^2=l_1^2+l_2^2+2l_1l_2c_2(4.12)$$

利用公式：

$$c_{12}=c_1c_2-s_1s_2$$$$s_{12}=c_1s_2+s_1c_2(4.13)$$

由（4.12）求解$c_2$，得到：

$$c_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}(4.14)$$

上式有解的条件是式（4.14）右边的值必须在-1~1.在这个解法中，这个约束条件可以用来检查解是否存在。从物理结构上看，如果约束条件不满足，则目标点位置太远，操作臂不可达。

假定目标点在工作空间内，$s_2$的表达式为

$$s_2=±\sqrt{1-c_2^2}(4.15)$$

最后，应用双变量反正切公式计算$\theta_2$，得：

$$\theta_2=Atan2(s_2,c_2)(4.16)$$

式（4.15）的符号选择对应于多解，我们可选择“肘部朝上”解或“肘部朝下”解。确定$\theta_2$时，再次应用循环方法来求解运动学参数，即常用的先确定期望关节角的正弦和余弦，然后应用双变量反正切公式的方法。这样确保得出所有的解，且所求的角度是在适当的象限里。

求出了$\theta_2$，可以根据式(4.10)和式(4.11)求出$\theta_1$。将式(4.10)和式(4.11)写成如下形式：

$$x=k_1c_1-k_2s_1(4.17)$$$$y=k_1s_1+k_2c_1(4.18)$$

式中

$$k_1=l_1+l_2c_2(4.19)$$$$k_2=l_2s_2(4.20)$$

为了求解这种形式的方程，可进行变量代换，实际上是改变常数$k_1$和$k_2$的形式。

如果

$$r=+\sqrt{k_1^2+k_2^2}(4.20)$$

并且

$$\gamma=Atan2(k_2,k_1)$$

则

$$k_1=rcos\gamma(4.21)$$$$k_2=rsin\gamma(4.21)(4.22)$$

式(4.17)和式(4.18)可以写成（三角变换）

$$\frac{x}{r}=cos\gamma cos\theta_1-sin\gamma sin\theta_1(4.22)$$$$\frac{y}{r}=cos\gamma sin\theta_1+sin\gamma cos\theta_1(4.23)$$

因此

$$cos(\gamma+\theta_1)=\frac{x}{r}(4.24)$$$$sin(\gamma+\theta_1)=\frac{y}{r}(4.25)$$

利用双变量反正切公式，得

$$\gamma+\theta_1=Atan2(\frac{y}{r},\frac{x}{r})=Atan2(y,x)(4.26)$$

从而

$$\theta_1=Atan2(y,x)-Atan2(k_2,k_1)(4.27)$$

注意，$\theta_2$符号的选取将导致$k_2$符号的变化，因此影响到$\theta_1$。

式(4.20)和式(4.21)比较重要，经常出现在运动学求解问题中，即式(4.10)和(4.11)的求解方法。当然，因为(4.27)是正切定义，所以细心地注意到如果$x=y=0$，式子会失去定义。此时，$\theta_1$可以取得任意值。

由(4.8)、(4.9)求出$\theta_1$、$\theta_2$、$\theta_3$的和：

$$\theta_1+\theta_2+\theta_3=Atan2(s_{\phi},c_{\phi})=\phi(4.28)$$

从而，可以求解$\theta_3$。

代数方法求解的过程，本质上是将给定的方程转换到解已知的形式。

几何解法

将操作臂的空间几何参数分解成平面几何问题，用这种方法求解操作臂比较容易。当$\alpha_i=0/±90°$时尤为如此。

代数解法中，我们讨论的机械臂是一个平面机械臂（ $\hat{Z}$ 轴的特殊性），继续举这个例子再好不过了。我们可以直接利用平面几何关系求解。

上图说明了机械臂还有一种对称的方式到达末端执行器的位置。利用余弦定理，求解实线的情况：

$$x^2+y^2=l_1^2+l_2^2-2l_1l_2cos(180+\theta_2)(4.29)$$

现在，替换 $cos(180+\theta_2)$ 为更简单的 $-cos(\theta_2)$，式子变成了：

$$c_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}(4.30)$$

观察这个式子。上面的三角形成立的条件是 $\sqrt{x^2+y^2}≤l_1+l_2$（可回忆一下机械原理中的连杆机构的知识），用计算方法对目标点的这个条件进行验证，以便证明该解的存在性。目标点超出运动范围则不满足。再仔细一些会发现 ${\theta}_2$ 的问题：它应该是一个 $[-180°,0]$ 之间的角度数。另一个可能的解（虚线三角形）可以通过对称关系 ${\theta}_2'=-{\theta}_2$ 得到。

求完了 ${\theta}_2$，紧接着让我们来看 ${\theta}_1$ 怎么求：首先，需要建立 ${\psi}$ 和 ${\beta}$ 的表达式。首先，${\beta}$ 可以位于任意象限，这是由 $x$ 和 $y$ 的符号决定的。应用双变量反正切公式定义 ${\beta}$：

$$\beta=Atan2(y,x)(4.31)$$

利用余弦定理，求出 ${\psi}$：

$$cos\psi=\frac{x^2+y^2+l_1^2-l_2^2}{2l_1\sqrt{x^2+y^2}}(4.32)$$

求出反余弦，使得 $0≤\psi≤180°$，这样（4.32）在数值上便是成立的。接着可得：

$$\theta_1=\beta±\psi(4.33)$$

式中，当 ${\theta}_2<0$ 时，取 $+$ 号，当 ${\theta}_2>0$ 时，取 $-$ 号。

同样进行加和操作从而求出 ${\theta}_3$：

$$\theta_1+\theta_2+\theta_3=\psi(4.34)$$

几何解法结束。

4.5 简化成多项式的代数解法

实际求解过程中比较难处理的是超越方程。然而，并不是全无办法———我们可以使用关于三角函数的多项式进行几何变换。如下三个式子需要牢记（以单一变量 $u$ 表示）：

$$u=tan\frac{\theta}{2}$$$$cos\theta=\frac{1-u^2}{1+u^2}$$$$sin\theta=\frac{2u}{1+u^2}(4.35)$$

在原书的附录中列出了更多的变换关系和三角恒等式帮助我们求解。感兴趣的可以自己去翻翻~

借原书的例子来求解一下：

有一超越方程如下：

$$acos\theta+bsin\theta=c(4.36)$$

正弦和余弦皆在，那么转化为一个正切函数是比较好的。我们使用(4.35)的变换式，上式乘以 $1+u^2$ 得：

$$a(1-u^2)+2bu=c(1+u^2)(4.37)$$

取 $u$ 的幂次排序：

$$(a+c)u^2-2bu+(c-a)=0(4.38)$$

由一元二次方程求解公式得：

$$u=\frac{b±\sqrt{b^2+a^2-c^2}}{a+c}(4.39)$$

因此

$$\theta=2tan^{-1}(\frac{b±\sqrt{b^2+a^2-c^2}}{a+c})(4.40)$$

$u$ 如果是复数，那么超越方程可能不存在实根。如果 $a+c=0$，那么反正切的自变量就会无穷大，因此 ${\theta}=180°$。这一步最好人工干预，也就是验证 $a+c$ 是否为0，因为计算机可能会解错。如果为0，那么上溯到(4.38)可以直接消去二次项，这样我们就只需解一个线性方程。

4次多项式导出的是封闭解，能够用4阶及以下的代数方程求解的操作臂是相当简单的。

4.6 三轴相交的Pieper解法

一般的6自由度机器人没有封闭解，但某些特殊情况下也可解。比如，3个连续轴相交于一点的6自由度操作臂，可以认为它们的交点在无穷远处。这是Pieper所做的研究工作，他提出的解法针对6个关节均为旋转关节且后面3个轴相交的操作臂。一些商业化的工业机器人正使用这样的解法。

最后三个轴相交时，连杆坐标系4、5、6的原点均位于这个交点上。这点在基坐标系中的位置是:

回忆一下式(3.6)：

或者，当i=4时，由式(3.6)的第4列有：

或

式中

上式对于$^2_3T$应用(3.6)得出$f_1$的表达式：

对$^0_1T$和$^1_2T$应用(3.6)得：

式中

现在写出$^0P_{4ORG}$绝对值平方的表达式，这里$r=x^2+y^2+z^2$，从(4.46)可以看出:

$$r=g_1^2+g_2^2+g_3^2(4.48)$$

则对于$g_i$，由式(4.47)得:

$$r=f_1^2+f_2^2+f_3^2+a_1^2+d_2^2+2d_2f_3+2a_1(c_2f_1-s_2f_2) (4.49)$$

现在写出(4.46)中Z方向分量的方程，那么表示这个方程组的两个方程如下：

式中

式（4.50）消去了因变量$\theta_1$，使得$\theta_2$的关系式更简单。剩下的任务就是求解$\theta_3$，分为3种情况：

1. 若$a_1=0$，则$r=k_3$，这里$r$是已知的，右边$(k_3)$仅是关于$\theta_3$的函数。代入(4.35)后，由包含$tan\frac{\theta_3}{2}$的二次方程可以解出$\theta_3$。
2. 若$s \alpha_1=0$，则$z=k_4$，这里$z$是已知的，再次代入(4.35)后，利用上面的一元二次方程可以解出$\theta_3$。
3. 否则，从式(4.50)中消去$s_2$和$c_2$，得到

代入(4.35)后，可得到一个4次方程，由此解出$\theta_3$。

解出$\theta_3$后，可以根据(4.50)解出$\theta_2$，再根据(4.46)解出$\theta_1$。

这是一个6自由度的机械臂，所以我们还需要求出$\theta_4$、$\theta_5$、$\theta_6$。由于这些轴相交，故这些关节角只影响末端连杆的方向，只需要$^0_6R$的旋转分量就计算出这三个角度。

我们可以先由$\theta_4=0$时连杆坐标系4相对于基坐标系的方向计算出$^4_0R|_{\theta_4=0}$。坐标系6的期望方向与连杆坐标系4的方向的差别仅在于最后三个关节的作用。由于$^0_6R$已知，因此可以如此计算：

对于大多数操作臂来说，可以使用$Z-Y-Z$欧拉角解法（第二章曾提及）应用于$^4_6R|_{\theta_4=0}$解出最后三个关节角。当然，我们需要定义一组合适的欧拉角。最后的三个关节通常由两种解，所以这种操作臂解的总数就是前三个关节解的数量的2倍。

4.7 操作臂逆运动学实例

这一部分的内容实在太多了，请自行移步《机器人学导论》求解！！！

4.8 标准坐标系

回到我们曾讨论的坐标系规范命名问题，现在我们要做一些拓展，确定如何应用这些坐标系：

1. 由用户确定系统中固定坐标系的位置，这个坐标系可能在工作面的一角，如下图所示。或者附于一个移动的传送带上。固定坐标系{S}是相对于基坐标系{B}定义的。
2. 机器人工具的描述通过给定坐标系{T}的参数给出，每种工具都有一个相应的工具坐标系{T}。然而，不同方式抓持相同的工具使得{T}不同。工具坐标系{T}是相对于腕部坐标系{W}的，即$^W_TT$。
3. 用户通过给定相对于固定坐标系的目标坐标系{G}的描述来指定机器人运动的目标点。对于机器人的某些运动，{T}和{S}的定义经常保持不变。这种情况下，它们一旦被定义，用户仅需给出一系列{G}的参数。在许多系统中，$^W_TT$是一个常量，固定坐标系可以固定，也可以用户通过机器人简单示教。这样并不需要用户搞清楚这5种坐标系，只需要考虑根据相对于工作区域（由固定坐标系确定）移动工具到达位置（目标）。
4. 机器人系统需要计算一系列的关节角使关节运动，工具坐标系从起始位置以光滑的方式运动，直到{T}={G}时，运动结束。

4.9 操作臂求解

SOLVE函数可以进行笛卡尔变换，也称为逆运动学函数。这个逆运动学是广义的，使得工具坐标系和固定坐标系的定义可以应用于基本逆运动学。基本逆运动学求解相对于基坐标系的腕部坐标系。

给定目标坐标系$^STT$，SOLVE应用工具坐标系和固定坐标系的定义来计算{W}相对于{B}的位置$^B_WT$：

$^B_WT=^B_ST^S_TT^W_TT^{-1}$

然后，逆运动学将$^B_WT$作为输入，计算$\theta_1$到$\theta_n$。

4.10 重复精度和精度

示教点是操作臂运动实际要达到的点，同时关节位置传感器读取关节角并存储。当命令机器人返回这个空间点时，每个关节都移动到已存储的关节角的位置。在这样简单的“示教和再现”的操作臂中，不存在逆运动学问题，因为没有在笛卡尔坐标系里指定目标点。当制造商在确定操作臂返回示教点的精度时，就是在确定操作臂的重复精度。

如果需要用逆运动学求出关节角并移动到未示教过的点，就需要计算，称他们为计算点。到达这个计算点的精度称为操作臂的精度。操作臂的精度不会超过其重复精度。精度受到机器人运动学方程中参数精度的影响。

4.11 计算问题

1. 计算效率会深深影响操作臂的逆运动学求解。一个关于$Atan2$的查表法子程序常被用于提高计算速度。
2. 多解的计算结构十分重要。并行计算所有的解通常效率很高，而不是依次顺序计算。当然只需要一个解的特殊情况，还可以更快。
3. 几何解法有时可以通过对各种角度做简单操作来计算多解问题。即，第一个解的计算是相当费时的，但通过计算角度的和或差以及加减$π$等方法很快可以求得其余解。