【机器人学导论笔记】四、操作臂逆运动学

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

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

首先，操作臂运动学方程是一个非线性问题，已知 N 0 T ^0_NT N0​T，求出 θ 1 , θ 2 , θ 3 . . . , θ n \theta_1,\theta_2,\theta_3...,\theta_n θ1​,θ2​,θ3​...,θn​。

着重考虑如下方程：

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

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

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

上图操作臂的工作空间：

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

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

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

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

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

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

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

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

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

θ 4 ′ = θ 4 + 180 ° \theta_4^{'}=\theta_4+180° θ4′​=θ4​+180° θ 5 ′ = θ 5 \theta_5^{'}=\theta_5 θ5′​=θ5​ θ 6 ′ = θ 6 + 180 ° \theta_6^{'}=\theta_6+180° θ6′​=θ6​+180°

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

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

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

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

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

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

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

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

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

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

对于一个n自由度操作臂（n0时，取 − - −号。

同样进行加和操作从而求出 θ 3 \theta_3 θ3​：

θ 1 + θ 2 + θ 3 = ψ ( 4.34 ) \theta_1+\theta_2+\theta_3=\psi(4.34) θ1​+θ2​+θ3​=ψ(4.34)

几何解法结束。

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

u = t a n θ 2 u=tan\frac{\theta}{2} u=tan2θ​

c o s θ = 1 − u 2 1 + u 2 cos\theta=\frac{1-u^2}{1+u^2} cosθ=1+u21−u2​

s i n θ = 2 u 1 + u 2 ( 4.35 ) sin\theta=\frac{2u}{1+u^2}(4.35) sinθ=1+u22u​(4.35)

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

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

有一超越方程如下：

a c o s θ + b s i n θ = c ( 4.36 ) acos\theta+bsin\theta=c(4.36) acosθ+bsinθ=c(4.36)

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

a ( 1 − u 2 ) + 2 b u = c ( 1 + u 2 ) ( 4.37 ) a(1-u^2)+2bu=c(1+u^2)(4.37) a(1−u2)+2bu=c(1+u2)(4.37)

取 u u u的幂次排序：

( a + c ) u 2 − 2 b u + ( c − a ) = 0 ( 4.38 ) (a+c)u^2-2bu+(c-a)=0(4.38) (a+c)u2−2bu+(c−a)=0(4.38)

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

u = b ± b 2 + a 2 − c 2 a + c ( 4.39 ) u=\frac{b±\sqrt{b^2+a^2-c^2}}{a+c}(4.39) u=a+cb±b2+a2−c2 ​​(4.39)

因此

θ = 2 t a n − 1 ( b ± b 2 + a 2 − c 2 a + c ) ( 4.40 ) \theta=2tan^{-1}(\frac{b±\sqrt{b^2+a^2-c^2}}{a+c})(4.40) θ=2tan−1(a+cb±b2+a2−c2 ​​)(4.40)

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

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

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

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

回忆一下式(3.6)：

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

或

式中

上式对于 3 2 T ^2_3T 32​T应用(3.6)得出 f 1 f_1 f1​的表达式：

对 1 0 T ^0_1T 10​T和 2 1 T ^1_2T 21​T应用(3.6)得：

式中

现在写出 0 P 4 O R G ^0P_{4ORG} 0P4ORG​绝对值平方的表达式，这里 r = x 2 + y 2 + z 2 r=x^2+y^2+z^2 r=x2+y2+z2，从(4.46)可以看出

则对于 g i g_i gi​，由式(4.47)得

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

式中

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

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

解出 θ 3 \theta_3 θ3​后，可以根据(4.50)解出 θ 2 \theta_2 θ2​，再根据(4.46)解出 θ 1 \theta_1 θ1​。

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

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

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

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

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

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

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

W B T = S B T T S T T W T − 1 ^B_WT=^B_ST^S_TT^W_TT^{-1} WB​T=SB​TTS​TTW​T−1

然后，逆运动学将 W B T ^B_WT WB​T作为输入，计算 θ 1 \theta_1 θ1​到 θ n \theta_n θn​。

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

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