第九章 机器人轨迹规划

机器人轨迹规划属于机器人低层规划，基本上不涉及工人智能问题，而是在机械手运动学和动力学的基础上，讨论在关节空间和笛卡儿空间中机器人运动的轨迹规划和轨迹生成方法。所谓轨迹，是指机械手在运动过程中的位移、速度和加速度。而轨迹规划是根据作业任务的要求，计算出预期的运动轨迹。首先对机器人的任务、运动路径和轨迹进行描述。轨迹规划器可使编程手续简化，只要求用户输入有关路径和轨迹的若干约束和简单描述，而复杂的细节问题则由规划器解决。例如，用户只需给出手部的目标位姿，让规划器确定到该目标的路径点、持续时间、运动速度等轨迹参数，并在计算机内部描述所要求的轨迹，即选择习惯规定及合理的软件数据结构。最后，对内部描述的轨迹，实时计算机器人运动的位移、速度和加速度，生成运动轨迹。

9.1 轨迹规划应考虑的问题

通常将机械手的运动看做是工具坐标系 $ \{T\} $相对于工作坐标系 $ \{S\} $的运动。这种描述方法既适用于各种机械手，也适用于同一机械手上装夹的各种工具。对于移动工作台（例如传送带），这种方法同样适用。这时，工作坐标系 $ \{S\} $的位姿随时间而变化。

对抓放作业(pick and place operation)的机器人(如用于上、下料)，需要描述它的起始状态和目标状态，即工具坐标系的起始值 $ \{T_{0}\} $和目标值 $ \{T_{g}\} $。在此，用“点”这个词表示工具坐标系的位置和姿态(简称位姿)，例如起始点和目标点等。

对于另外一些作业，如弧焊和曲面加工等，不仅要规定机械手的起始点和终止点，而且要指明两点之间的若干中间点（称路径点），必须沿特定的路径运动（路径约束）。这类运动称为连续路径运动（continuous-path motion）或轮廓运动（contour motion），而前者称为点到点运动（point-to-point motion，PTP）。

在规划机器人的运动轨迹时，还需要弄清楚在其路径上是否存在障碍物（障碍约束）。路径约束和障碍约束的组合把机器人的规划与控制方式划分为四类，如表9.1所示。本节主要讨论连续路径的无障碍的轨迹规划方法。轨迹规划器可形象地看成为一个黑箱（见图9.1），其输入包括路径的设定和约束，输出的是机械手末端手部的位姿序列，表示手部在各离散时刻的中间位形（configurations）。机械手最常用的轨迹规划方法有两种：第一种方法要求用户对于选定的转变节点（插值点）上的位姿、速度和加速度给出一组显式约束（例如连续性和光滑程度等），轨迹规划器从一类函数（例如n次多项式）中选取参数化轨迹，对节点进行插值，并满足约束条件。第二种方法要求用户给出运动路径的解析式；如为直角坐标空间中的直线路径，轨迹规划器在关节空间或直角坐标空间中确定一条轨迹来逼近预定的路径。在第一种方法中，约束的设定和轨迹规划均在关节空间进行。因此可能会发生与障碍物相碰。第二种方法的路径约束是在直角坐标空间中给定的，而关节驱动器是在关节空间中受控的。因此，为了得到与给定路径十分接近的轨迹，首先必须采用某种函数逼近的

方法将直角坐标路径约束转化为关节坐标路径约束，然后确定满足关节路径约束的参数化路径。

轨迹规划既可在关节空间也可在直角空间中进行，但是所规划的轨迹函数都必须连续和平滑，使得操作臂的运动平稳。在关节空间进行规划时，是将关节变量表示成时间的函数，并规划它的一阶和二阶时间导数；在直角空间进行规划是指将手部位姿、速度和加速度表示为时间的函数。而相应的关节位移、速度和加速度由手部的信息导出。通常通过运动学反解得出关节位移，用逆雅可比求出关节速度，用逆雅可比及其导数求解关节加速度。

用户根据作业给出各个路径节点后，规划器的任务包含：解变换方程、进行运动学反解和插值运算等；在关节空间进行规划时，大量工作是对关节变量的插值运算。

9.2 关节轨迹的插值计算

下面讨论关节轨迹的插值计算。机械手运动路径点（节点）一般用工具坐标系 $ \{T\} $相对于工作坐标系 $ \{S\} $的位姿来表示。为了求得在关节空间形成所求轨迹，首先用运动学反解将路径点转换成关节矢量角度值，然后对每个关节拟合一个光滑函数，使其从起始点开始，依次通过所有路径点，最后到达目标点。对于每一段路径，各个关节运动时间均相同，这样保证所有关节同时到达路径点和终止点，从而得到工具坐标系 $ \{T\} $应有的位置和姿态。尽管每个关节在同一段路径中的运动时间相同，但各个关节函数之间却是相互独立的。

关节空间法是以关节角度的函数描述机器人轨迹的，关节空间法不必在直角坐标系中描述两个路径点之间的路径形状，计算简单、容易。此外，由于关节空间与直角坐标空间之间并不是连续的对应关系，因而不会发生机构的奇异性问题。

在关节空间中进行轨迹规划，需要给定机器人在起始点和终止点手臂的位形。对关节进行插值时，应满足一系列的约束条件，例如抓取物体时，手部运动方向（初始点），提升物体离开的方向（提升点），放下物体（下放点）和停止点等节点上的位姿、速度和加速度的要求；与此相应的各个关节位移、速度、加速度在整个时间间隔内连续性要求；其极值必须在各个

关节变量的容许范围之内等。在满足所要求的约束条件下，可以选取不同类型的关节插值函数，生成不同的轨迹。

下面着重讨论关节轨迹的插值方法。

关节轨迹插值计算的方法较多，现简述如下。

1. 三次多项式插值

在机械手运动过程中，由于相应于起始点的关节角度 $ \theta_{0} $ 是已知的，而终止点的关节角 $ \theta_{f} $ 可以通过运动学反解得到。因此，运动轨迹的描述，可用起始点关节角度与终止点关节角度的一个平滑插值函数 $ \theta(t) $ 来表示， $ \theta(t) $ 在 $ t_{0}=0 $ 时刻的值是起始关节角度 $ \theta_{0} $，在终端时刻 $ t_{f} $ 的值是终止关节角度 $ \theta_{f} $。显然，有许多平滑函数可作为关节插值函数，如图 9.2 所示。

为了实现单个关节的平稳运动，轨迹函数 $ \theta(t) $ 至少

需要满足四个约束条件。其中两个约束条件是起始点和终止点对应的关节角度：

$$ \left.\begin{array}{l}\theta(0)=\theta_{0}\\ \theta(t_{f})=\theta_{f}\end{array}\right\} $$

为了满足关节运动速度的连续性要求，另外还有两个约束，即在起始点和终止点的关节速度要求。在当前情况下，规定

$$ \left.\begin{array}{l}{\dot{\theta}(0)=0}\\ {\dot{\theta}(t_{f})=0}\end{array}\right\} $$

上述四个边界约束条件(9.1)和约束条件(9.2)唯一地确定了一个三次多项式：

$$ \theta(t)=a_{0}+a_{1}t+a_{2}t^{2}+a_{3}t^{3} $$

运动轨迹上的关节速度和加速度则为

$$ \left.\begin{array}{l}{\dot{\theta}(t)=a_{1}+2a_{2}t+3a_{3}t^{2}}\\ {\ddot{\theta}(t)=2a_{2}+6a_{3}t}\end{array}\right\} $$

对式(9.3)和式(9.4)代入相应的约束条件，得到有关系数 $ a_{0},a_{1},a_{2} $和 $ a_{3} $的四个线性方程：

$$ \left.\begin{array}{l}\theta_{0}=a_{0}\\ \theta_{f}=a_{0}+a_{1}t_{f}+a_{2}t_{f}^{2}+a_{3}t_{f}^{3}\\ 0=a_{1}\\ 0=a_{1}+2a_{2}t_{f}+3a_{3}t_{f}^{2}\end{array}\right\} $$

求解上述方程组可得

$$ \left.\begin{aligned}a_{0}&=\theta_{0}\\ a_{1}&=0\\ a_{2}&=\frac{3}{t_{f}^{2}}(\theta_{f}-\theta_{0})\\ a_{3}&=-\frac{2}{t_{f}^{3}}(\theta_{f}-\theta_{0})\end{aligned}\right\} $$

这组解只适用于关节起始速度和终止速度为零的运动情况。对于其他情况，后面另行讨论。

2. 过路径点的三次多项式插值

一般情况下，要求规划过路径点的轨迹。如果机械手在路径点停留，则可直接使用前面三次多项式插值的方法；如果只是经过路径点，并不停留，则需要推广上述方法。

实际上，可以把所有路径点也看做是“起始点”或“终止点”，求解逆运动学，得到相应的关节矢量值。然后确定所要求的三次多项式插值函数，把路径点平滑地连接起来。但是，在这些“起始点”和“终止点”的关节运动速度不再是零。

路径点上的关节速度可以根据需要设定，这样一来，确定三次多项式的方法与前面所述的完全相同，只是速度约束条件(9.2)变为

$$ \left.\begin{array}{l}{\dot{\theta}(0)=\dot{\theta}_{0}}\\ {\dot{\theta}(t_{f})=\dot{\theta}_{f}}\end{array}\right\} $$

确定三次多项式的四个方程为

$$ \left.\begin{aligned}\theta_{0}&=a_{0}\\\theta_{f}&=a_{0}+a_{1}t_{f}+a_{2}t_{f}^{2}+a_{3}t_{f}^{3}\\\dot{\theta}_{0}&=a_{1}\\\dot{\theta}_{f}&=a_{1}+2a_{2}t_{f}+3a_{3}t_{f}^{2}\end{aligned}\right\} $$

求解以上方程组，即可求得三次多项式的系数

$$ \left.\begin{aligned}a_{0}&=\theta_{0}\\ a_{1}&=\dot{\theta}_{0}\\ a_{2}&=\frac{3}{t_{f}^{2}}(\theta_{f}-\theta_{0})-\frac{2}{t_{f}}\dot{\theta}_{0}-\frac{1}{t_{f}}\dot{\theta}_{f}\\ a_{3}&=-\frac{2}{t_{f}^{3}}(\theta_{f}-\theta_{0})+\frac{1}{t_{f}^{2}}(\dot{\theta}_{0}+\dot{\theta}_{f})\end{aligned}\right\} $$

实际上，由上式确定的三次多项式描述了起始点和终止点具有任意给定位置和速度的运动轨迹，是式(9.6)的推广。剩下的问题就是如何确定路径点上的关节速度。可由以下三种方法规定：

（1）根据工具坐标系在直角坐标空间中的瞬时线速度和角速度来确定每个路径点的关节速度。

（2）在直角坐标空间或关节空间中采用适当的启发式方法，由控制系统自动地选择路径点的速度。

（3）为了保证每个路径点上的加速度连续，由控制系统按此要求自动地选择路径点的速度。

3. 高阶多项式插值

如果对于运动轨迹的要求更为严格，约束条件增多，那么三次多项式就不能满足需要，必须用更高阶的多项式对运动轨迹的路径段进行插值。例如，对某段路径的起始点和终止

点都规定了关节的位置、速度和加速度，则要用一个五次多项式进行插值，即

$$ \theta(t)=a_{0}+a_{1}t+a_{2}t^{2}+a_{3}t^{3}+a_{4}t^{4}+a_{5}t^{5} $$

多项式的系数 $ a_{0}, a_{1}, \cdots, a_{5} $ 必须满足 6 个约束条件：

$$ \left.\begin{aligned}\theta_{0}&=a_{0}\\\theta_{f}&=a_{0}+a_{1}t_{f}+a_{2}t_{f}^{2}+a_{3}t_{f}^{3}+a_{4}t_{f}^{4}+a_{5}t_{f}^{5}\\\dot{\theta}_{0}&=a_{1}\\\dot{\theta}_{f}&=a_{1}+2a_{2}t_{f}+3a_{3}t_{f}^{2}+4a_{4}t_{f}^{3}+5a_{5}t_{f}^{4}\\\ddot{\theta}_{0}&=2a_{2}\\\ddot{\theta}_{f}&=2a_{2}+6a_{3}t_{f}+12a_{4}t_{f}^{2}+20a_{5}t_{f}^{3}\end{aligned}\right\} $$

这个线性方程组含有6个未知数和6个方程，其解为

$$ \left.\begin{aligned}&a_{0}=\theta_{0}\\ &a_{1}=\dot{\theta}_{0}\\ &a_{2}=\frac{\ddot{\theta}_{0}}{2}\\ &a_{3}=\frac{20\theta_{f}-20\theta_{0}-(8\dot{\theta}_{f}+12\dot{\theta}_{0})t_{f}-(3\ddot{\theta}_{0}-\ddot{\theta}_{f})t_{f}^{2}}{2t_{f}^{3}}\\ &a_{4}=\frac{30\theta_{0}-30\theta_{f}+(14\dot{\theta}_{f}+16\dot{\theta}_{0})t_{f}+(3\ddot{\theta}_{0}-2\ddot{\theta}_{f})t_{f}^{2}}{2t_{f}^{4}}\\ &a_{5}=\frac{12\theta_{f}-12\theta_{0}-(6\dot{\theta}_{f}+6\dot{\theta}_{0})t_{f}-(\ddot{\theta}_{0}-\ddot{\theta}_{f})t_{f}^{2}}{2t_{f}^{5}}\\ \end{aligned}\right\} $$

4. 用抛物线过渡的线性插值

对于给定的起始点和终止点的关节角度，也可以选择直线插值函数来表示路径的形状。值得指出的是，这样做，尽管每个关节都作匀速运动，但是手部的运动轨迹一般不是直线。

显然，单纯线性插值将导致在节点处关节运动速度不连续，加速度无限大。为了生成一条位移和速度都连续的平滑运动轨迹，在使用线性插值时，在每个节点的邻域内增加一段抛物线的缓冲区段。由于抛物线对于时间的二阶导数为常数，即相应区段内的加速度恒定不变，这样使得平滑过渡，不致在节点处产生跳跃，从而使整个轨迹上的位移和速度都连续。线性函数与两段抛物线函数平滑地衔接在一起形成的轨迹称为带有抛物线过渡域的线性轨迹。如图9.3(a)所示。

为了构造这段运动轨迹，假设两端的过渡域（抛物线）具有相同的持续时间，因而在这两个域中采用相同的恒加速度值，只是符号相反。正如图9.3(b)所示，存在有多个解，得到的轨迹不是唯一的。但是，每个结果都对称于时间中点 $ t_{h} $和位置中点 $ \theta_{h} $。由于过渡域 $ [t_{0},t_{b}] $终点的速度必须等于线性域的速度，所以

$$ \dot{\theta}_{t_{b}}=\frac{\theta_{h}-\theta_{b}}{t_{h}-t_{b}} $$

式中， $ \theta_{b} $ 为过渡域终点 $ t_{b} $ 处的关节角度。用 $ \dot{\theta} $ 表示过渡域内的加速度， $ \theta_{b} $ 的值可按下式

解得：

$$ \theta_{b}=\theta_{0}+\frac{1}{2}\ddot{\theta}t_{b}^{2} $$

令 $ t=2t_{h} $，据式(9.13)和式(9.14)可得

$$ \dot{\theta}t_{b}^{2}-\ddot{\theta}t t_{b}+(\theta_{f}-\theta_{0})=0 $$

式中，t 为所要求的运动持续时间。

这样，对于任意给定的 $ \theta_{f} $， $ \theta_{0} $ 和 t，可以按式(9.15)选择相应的 $ \theta $ 和 $ t_{b} $，得到路径曲线。通常的作法是先选择加速度 $ \ddot{\theta} $ 的值，然后按式(9.15)算出相应的 $ t_{b} $ 为

$$ t_{b}=\frac{t}{2}-\frac{\sqrt{\ddot{\theta}^{2}t^{2}-4\ddot{\theta}(\theta_{f}-\theta_{0})}}{2\ddot{\theta}} $$

由上式可知，为保证 $ t_{b} $有解，过渡域加速度值 $ \theta $必须选得足够大，即

$$ \ddot{\theta}\geqslant\frac{4(\theta_{f}-\theta_{0})}{t^{2}} $$

当式(9.17)中的等号成立时，线性域的长度缩减为零，整个路径段由两个过渡域组成，这两个过渡域在衔接处的斜率（代表速度）相等。当加速度的取值越来越大时，过渡域的长度会越来越短。如果加速度选为无限大，路径又回复到简单的线性插值情况。

5. 过路径点的用抛物线过渡的线性插值

如图 9.4 所示，某个关节在运动中设有 n 个路径点，其中三个相邻的路径点表示为 j，k 和 l，每两个相邻的路径点之间都以线性函数相连，而所有路径点附近则由抛物线过渡。

在图9.3中，在k点的过渡域的持续时间为 $ t_{k} $；点j和点k之间线性域的持续时间为 $ t_{jk} $；连接j与k点的路径段的全部持续时间为 $ t_{djk} $。另外，j与k点之间的线性域速度为 $ \dot{\theta}_{jk} $，j点过渡域的加速度为 $ \ddot{\theta}_{j} $。现在的问

题是在含有路径点的情况下，如何确定带有抛物线过渡域的线性轨迹。

与上述用抛物线过渡的线性插值相同，这个问题有许多解，每一解对应于一个选取的速度值。给定任意路径点的位置 $ \theta_{k} $、持续时间 $ t_{djk} $ 以及加速度的绝对值 $ \left|\ddot{\theta}_{k}\right| $，可以计算出过渡域的持续时间 $ t_{k} $。对于那些内部路径段 $ (j, k \neq 1, 2; j, k \neq n-1) $，根据下列方程求解：

$$ \left.\begin{aligned}&\dot{\theta}_{jk}=\frac{\theta_{k}-\theta_{j}}{t_{djk}}\\ &\ddot{\theta}_{k}=sgn(\dot{\theta}_{kl}-\dot{\theta}_{jk})\mid\ddot{\theta}_{k}\mid\\ &t_{k}=\frac{\dot{\theta}_{kl}-\dot{\theta}_{jk}}{\ddot{\theta}_{k}}\\ &t_{jk}=t_{djk}-\frac{1}{2}t_{j}-\frac{1}{2}t_{k}\\ \end{aligned}\right\} $$

第一个路径段和最后一个路径段的处理与上式(9.18)略有不同，因为轨迹端部的整个过渡域的持续时间都必须计入这一路径段内。对于第一个路径段，令线性域速度的两个表达式相等，就可求出 $ t_{1} $：

$$ \frac{\theta_{2}-\theta_{1}}{t_{d12}-\frac{1}{2}t_{1}}=\ddot{\theta}_{1}t_{1} $$

用式(9.19)算出起始点过渡域的持续时间 $ t_{1} $ 之后，进而求出 $ \dot{\theta}_{12} $ 和 $ t_{12} $

$$ \left.\begin{aligned}&\ddot{\theta}_{1}=\operatorname{sgn}(\dot{\theta}_{2}-\dot{\theta}_{1})\mid\ddot{\theta}_{1}\mid\\ &t_{1}=t_{d12}-\sqrt{t_{d12}^{2}-\frac{2(\theta_{2}-\theta_{1})}{\ddot{\theta}_{1}}}\\ &\dot{\theta}_{12}=\frac{\theta_{2}-\theta_{1}}{t_{d12}-\frac{1}{2}t_{1}}\\ &t_{12}=t_{d12}-t_{1}-\frac{1}{2}t_{2}\\ \end{aligned}\right\} $$

对于最后一个路径段，路径点 n-1 与终止点 n 之间的参数与第一个路径段相似，即

$$ \frac{\theta_{n-1}-\theta_{n}}{t_{d(n-1)n}}-\frac{1}{2}t_{n}=\ddot{\theta}_{n}t_{n} $$

根据上式即可求出：

$$ \begin{aligned}&\ddot{\theta}_{n}=\mathrm{sgn}(\dot{\theta}_{n-1}-\dot{\theta}_{n})\mid\ddot{\theta}_{n}\mid\\ &t_{n}=t_{d(n-1)n}-\sqrt{t_{d(n-1)n}^{2}+\frac{2(\theta_{n}-\theta_{n-1})}{\ddot{\theta}_{n}}}\\ &\dot{\theta}_{(n-1)n}=\frac{\theta_{n}-\theta_{n-1}}{t_{d(n-1)n}-\frac{1}{2}t_{n}}\\ &t_{(n-1)n}=t_{d(n-1)n}-t_{n}-\frac{1}{2}t_{n-1}\\ \end{aligned} $$

公式(9.18)至公式(9.22)可用来求出多段轨迹中各个过渡域的时间和速度。通常用户只需给定路径点，以及各个路径段的持续时间。在这种情况下，系统使用各个关节的隐含加速度值。有时，为了简便起见，系统还可按隐含速度值来计算持续时间。对于各段的过渡域，加速度值应取得足够大，以使各路径段有足够长的线性域。

值得注意的是，多段用抛物线过渡的直线样条函数一般并不经过那些路径点，除非在这些路径点处停止。若选取的加速度充分大，则实际路径将与理想路径点十分靠近。如果要求机器人途经某个节点，那么将轨迹分成两段，把些节点作为前一段的终止点和后一段的起始点即可。

9.3 笛卡儿路径轨迹规划

在这种轨迹规划系统中，作业是用机械手终端夹手位姿的笛卡儿坐标节点序列规定的。因此，节点指的是表示夹手位姿（位置和姿态）的齐次变换矩阵。

1. 物体对象的描述

利用第二章有关物体空间的描述方法，任一刚体相对参考系的位姿是用与它固接的坐标系来描述的。相对于固接坐标系，物体上任一点用相应的位置矢量 p 表示；任一方向用方向余弦表示。给出物体的几何图形及固接坐标系后，只要规定固接坐标系的位姿，即可重构该物体。

例如，图9.5所示的螺栓，其轴线与固接坐标系的z轴重合。螺栓头部直径为32mm，中心取为坐标原点，螺栓长80mm，直径20mm，则可根据固接坐标系的位姿重构螺栓在空间（相对参考系）的位姿和几何形状。

2. 作业的描述

作业和机械手的运动可用手部位姿节点序列来规定，每个节点是由工具坐标系相对于作业坐标系的齐次变换来描述。相应的关节变量可用运动学反解程序计算。

例如，要求机器人按直线运动，把螺栓从槽中取出并放入托架的一个孔中，如图9.6所示。用符号表示沿直线运动的各节点的位姿，使机器人能沿虚线运动并完成作业。令 $ P_{i}(i=0,1,2,3,4,5) $为夹手必须经过的直角坐标节点。参照这些节点的位姿将作业描述为如表9.2所示的手部的一连串运动和动作。

| 节点 | $ P_{0} $ | $ P_{1} $ | $ P_{2} $ | $ P_{2} $ | $ P_{3} $ | $ P_{4} $ | $ P_{5} $ | $ P_{5} $ | $ P_{a} $ |

| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- |

| 运动 | INIT | MOVE | MOVE | GRASP | MOVE | MOVE | MOVE | RELEASE | MOVE |

| 目标 | 原始 | 接近螺栓 | 到达 | 抓住 | 提升 | 接近托架 | 放入孔中 | 松夹 | 移开 |

第一节点 $ P_{i} $ 对应一个变换方程(2.37)，从而解出相应的机械手的变换 $ ^{0}T_{6} $。由此得到作业描述的基本结构：作业节点 $ P_{i} $ 对应机械手变换 $ ^{0}T_{6} $，从一个变换到另一变换通过机械手运动实现。

3. 两个节点之间的“直线”运动

机械手在完成作业时，夹手的位姿可用一系列节点 $ P_{i} $ 来表示。因此，在直角坐标空间中进行轨迹规划的首要问题是由两节点 $ P_{i} $ 和 $ P_{i+1} $ 所定义的路径起点和终点之间，如何生成一系列中间点。两节点之间最简单的路径是在空间的一个直线移动和绕某定轴的转动。若运动时间给定之后，则可以产生一个使线速度和角速度受控的运动。如图9.6所示，要生成从节点 $ P_{0} $（原位）运动到 $ P_{1} $（接近螺栓）的轨迹。更一般地，从一节点 $ P_{i} $ 到下一节点 $ P_{i+1} $ 的运动可表示为从

$$ \mathbf{\Psi}_{6}^{0}\mathbf{T}_{6}=^{0}\mathbf{T}_{B}^{B}\mathbf{P}_{i}^{6}\mathbf{T}_{E}^{-1} $$

到

$$ ^{0}\mathbf{T}_{6}=^{0}\mathbf{T}_{B}{}^{B}\mathbf{P}_{i+1}{}^{6}\mathbf{T}_{E}^{-1} $$

的运动。其中 $ ^{6}T_{E} $是工具坐标系 $ \{T\} $相对末端连杆系 $ \{6\} $的变换。 $ ^{B}P_{i} $和 $ ^{B}P_{i+1} $分别为两节点 $ P_{i} $和 $ P_{i+1} $相对坐标系 $ \{B\} $的齐次变换。如果起始点 $ P_{i} $是相对另一坐标系 $ \{A\} $描述的，那么可通过变换过程得到

$$ \mathbf{P}_{i}=^{0}\mathbf{T}_{B}^{-10}\mathbf{T}_{A}^{A}\mathbf{P}_{i} $$

基于式(9.23)和式(9.24)，则从节点 $ P_{i} $ 到 $ P_{i+1} $ 的运动可由“驱动变换” $ \pmb{D}(\lambda) $ 来表示。

$$ ^{0}\mathbf{T}_{6}(\lambda)=^{0}\mathbf{T}_{B}^{B}\mathbf{P}_{i}\mathbf{D}(\lambda)^{6}\mathbf{T}_{E}^{-1} $$

其中，驱动变换 $ D(\lambda) $ 是归一化时间 $ \lambda $ 的函数； $ \lambda = t/T $， $ \lambda \in [0,1] $； $ t $ 为自运动开始算起的实际时间； $ T $ 为走过该轨迹段的总时间。

在节点 $ P_{i} $，实际时间 t=0，因此 $ \lambda=0, \pmb{D}(0) $ 是 $ 4\times4 $ 的单位矩阵，因而式(9.26)与式(9.23)相同。

在节点 $ P_{i+1}, t=T, \lambda=1 $，有

$$ ^{B}\mathbf{P}_{i}\mathbf{D}(1)=^{B}\mathbf{P}_{i+1} $$

式（9.26）与式（9.24）相同，因此得

$$ \boldsymbol{D}(1)=\boldsymbol{^{B}P_{i}^{-1}B}\boldsymbol{P}_{i+1} $$

可将工具（夹手）从一个节点 $ P_{i} $ 到下一节点 $ P_{i+1} $ 的运动看成和夹手固接的坐标系的运动。在第二章中，规定手部坐标系的三个坐标轴用 n,o 和 a 表示，坐标原点用 p 表示。

因此，节点 $ P_{i} $ 和 $ P_{i+1} $ 相对于目标坐标系 $ \{B\} $ 的描述可用相应的齐次变换矩阵来表示，即

$$ ^{B}\boldsymbol{P}_{i}=\begin{bmatrix}\boldsymbol{n}_{i}&\boldsymbol{o}_{i}&\boldsymbol{a}_{i}&\boldsymbol{p}_{i}\\ 0&0&0&1\end{bmatrix}=\begin{bmatrix}n_{i_{x}}&o_{i_{x}}&a_{i_{x}}&\dot{p}_{i_{x}}\\ n_{i_{y}}&o_{i_{y}}&a_{i_{y}}&\dot{p}_{i_{y}}\\ n_{i_{z}}&o_{i_{z}}&a_{i_{z}}&\dot{p}_{i_{z}}\\ 0&0&0&1\end{bmatrix} $$

$$ {}^{B}\mathbf{P}_{i+1}=\left[\begin{array}{cccc}\mathbf{n}_{i+1}&\mathbf{o}_{i+1}&\mathbf{a}_{i+1}&\mathbf{p}_{i+1}\\ 0&0&0&1\end{array}\right]=\left[\begin{array}{cccc}n_{i+1_{x}}&o_{i+1_{x}}&a_{i+1_{x}}&\dot{p}_{i+1_{x}}\\ n_{i+1_{y}}&o_{i+1_{y}}&a_{i+1_{y}}&\dot{p}_{i+1_{y}}\\ n_{i+1_{z}}&o_{i+1_{z}}&a_{i+1_{z}}&\dot{p}_{i+1_{z}}\\ 0&0&0&1\end{array}\right] $$

利用矩阵求逆公式(2.36)求出 $ ^{B}P_{i}^{-1} $，再右乘 $ ^{B}P_{i+1} $，则得

$$ \mathbf{D}(1)=\begin{bmatrix}\boldsymbol{n}_{i}\cdot\boldsymbol{n}_{i+1}&\boldsymbol{n}_{i}\cdot\boldsymbol{o}_{i+1}&\boldsymbol{n}_{i}\cdot\boldsymbol{a}_{i+1}&\boldsymbol{n}_{i}\cdot(\boldsymbol{p}_{i+1}-\boldsymbol{p}_{i})\\\boldsymbol{o}_{i}\cdot\boldsymbol{n}_{i+1}&\boldsymbol{o}_{i}\cdot\boldsymbol{o}_{i+1}&\boldsymbol{o}_{i}\cdot\boldsymbol{a}_{i+1}&\boldsymbol{o}_{i}\cdot(\boldsymbol{p}_{i+1}-\boldsymbol{p}_{i})\\\boldsymbol{a}_{i}\cdot\boldsymbol{n}_{i+1}&\boldsymbol{a}_{i}\cdot\boldsymbol{o}_{i+1}&\boldsymbol{a}_{i}\cdot\boldsymbol{a}_{i+1}&\boldsymbol{a}_{i}\cdot(\boldsymbol{p}_{i+1}-\boldsymbol{p}_{i})\\0&0&0&1\end{bmatrix} $$

其中， $ n \cdot o $ 表示矢量 n 与 o 的标积。

工具坐标系从节点 $ P_{i} $ 到 $ P_{i+1} $ 的运动可分解为一个平移运动和两个旋转运动：第一个转动使工具轴线与预期的接近方向 a 对准；第二个转动是绕工具轴线 (a) 转动，使方向矢量 o 对准。则驱动函数 $ \pmb{D}(\lambda) $ 由一个平移运动和两个旋转运动构成，即

$$ \mathbf{D}(\lambda)=\mathbf{L}(\lambda)\mathbf{R}_{a}(\lambda)\mathbf{R}_{o}(\lambda) $$

其中， $ L(\lambda) $ 是表示平移运动的齐次变换，其作用是把节点 $ P_{i} $ 的坐标原点沿直线运动到 $ P_{i+1} $ 的原点。第一个转动用齐次变换 $ R_{a}(\lambda) $ 表示，其作用是将 $ P_{i} $ 的接近矢量 $ a_{i} $ 转向 $ P_{i+1} $ 的接近矢量 $ a_{i+1} $；第二个转动齐次变换 $ R_{o}(\lambda) $ 表示，其作用是将 $ P_{i} $ 的方向矢量 $ o_{i} $ 转向 $ P_{i+1} $ 的方向矢量 $ o_{i+1} $。

$$ \boldsymbol{L}(\lambda)=\begin{bmatrix}1&0&0&\lambda x\\ 0&1&0&\lambda y\\ 0&0&1&\lambda z\\ 0&0&0&1\end{bmatrix} $$

$$ \boldsymbol{R}_{a}\left(\lambda\right)=\begin{bmatrix}s^{2}\psi v\left(\lambda\theta\right)+c\left(\lambda\theta\right)&-s\psi c\psi v\left(\lambda\theta\right)&c\psi c\left(\lambda\theta\right)&0\\ -s\psi c\psi v\left(\lambda\theta\right)&c^{2}\psi v\left(\lambda\theta\right)+c\left(\lambda\theta\right)&s\psi s\left(\lambda\theta\right)&0\\ -c\psi s\left(\lambda\theta\right)&-s\psi s\left(\lambda\theta\right)&c\left(\lambda\theta\right)&0\\ 0&0&0&1\end{bmatrix} $$

$$ \boldsymbol{R}_{o}(\lambda)=\begin{bmatrix}{{{c(\lambda\phi)}}}&{{{-s(\lambda\phi)}}}&{{{0}}}&{{{0}}} \\{{{s(\lambda\phi)}}}&{{{c(\lambda\phi)}}}&{{{0}}}&{{{0}}} \\{{{0}}}&{{{0}}}&{{{1}}}&{{{0}}} \\{{{0}}}&{{{0}}}&{{{0}}}&{{{1}}}\end{bmatrix} $$

其中， $ v(\lambda\theta)=\operatorname{vers}(\lambda\theta)=1-\cos(\lambda\theta) $； $ c(\lambda\theta)=\cos(\lambda\theta) $； $ s(\lambda\theta)=\sin(\lambda\theta) $； $ c(\lambda\phi)=\cos(\lambda\phi) $； $ s(\lambda\phi)=\sin(\lambda\phi) $； $ \lambda\in[0,1] $。

旋转变换 $ R_{a}(\lambda) $ 表示绕矢量 k 转动 $ \theta $ 角得到的，而矢量 k 是 $ P_{i} $ 的 y 轴绕其 z 轴转过 $ \psi $ 282 ·

角得到的，即

$$ \boldsymbol{k}=\begin{bmatrix}-s\boldsymbol{\psi}\\ c\boldsymbol{\psi}\\ 0\\ 1\end{bmatrix}=\begin{bmatrix}c\boldsymbol{\psi}&-s\boldsymbol{\psi}&0&0\\ s\boldsymbol{\psi}&c\boldsymbol{\psi}&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix}\begin{bmatrix}0\\ 1\\ 0\\ 1\end{bmatrix} $$

根据旋转变换通式(2.45)，即可得到式(9.30)。旋转变换 $ R_{0}(\lambda) $表示绕接近矢量 $ a $转 $ \phi $角的变换矩阵。显然，平移量 $ \lambda x, \lambda y, \lambda z $和转动量 $ \lambda\theta $及 $ \lambda\phi $将与 $ \lambda $成正比。若 $ \lambda $随时间线性变化，则 $ D(\lambda) $所代表的合成运动将是一个恒速移动和两个恒速转动的复合。

将矩阵(9.29)至矩阵(9.31)相乘代入式(9.28)，得到

$$ \boldsymbol{D}(\lambda)=\begin{bmatrix}d\boldsymbol{n}&d\boldsymbol{o}&d\boldsymbol{a}&d\boldsymbol{p}\\ 0&0&0&1\end{bmatrix} $$

其中，

$$ \mathrm{d}\mathbf{o}=\begin{bmatrix}-s(\lambda\phi)\left[s^{2}\psi\upsilon(\lambda\theta)+c(\lambda\theta)\right]+c(\lambda\phi)\left[-s\psi c\psi\upsilon(\lambda\theta)\right]\\ -s(\lambda\phi)\left[-s\psi c\psi\upsilon(\lambda\phi)\right]+c(\lambda\phi)\left[c^{2}\psi\upsilon(\lambda\theta)+c(\lambda\theta)\right]\\ -s(\lambda\phi)\left[-c\psi s(\lambda\theta)\right]+c(\lambda\phi)\left[-s\psi s(\lambda\theta)\right]\end{bmatrix} $$

$$ \mathrm{d}\boldsymbol{a}=\begin{bmatrix}c\psi s(\lambda\theta)\\ s\psi s(\lambda\theta)\\ c(\lambda\theta)\end{bmatrix} $$

$$ \mathrm{d}\boldsymbol{p}=\begin{bmatrix}\lambda x\\ \lambda y\\ \lambda z\end{bmatrix} $$

$$ \mathrm{d}\boldsymbol{n}=\mathrm{d}\boldsymbol{o}\times\mathrm{d}\boldsymbol{a} $$

将逆变换方法用于式(9.28)，在式(9.28)两边右乘 $ R_{0}^{-1}(\lambda)R_{a}^{-1}(\lambda) $，使位置矢量的各元素分别相等，令 $ \lambda=1 $，则得

$$ \left.\begin{aligned}&x=\boldsymbol{n}_{i}\bullet(\boldsymbol{p}_{i+1}-\boldsymbol{p}_{i})\\ &y=\boldsymbol{o}_{i}\bullet(\boldsymbol{p}_{i+1}-\boldsymbol{p}_{i})\\ &z=\boldsymbol{a}_{i}\bullet(\boldsymbol{p}_{i+1}-\boldsymbol{p}_{i})\\ \end{aligned}\right\} $$

上式中矢量 $ n_i, o_i, a_i $ 和 $ p_i, p_{i+1} $ 都是相对于目标坐标系 $ \{B\} $ 表示的。将方程 (9.28) 两边右乘 $ R_o^{-1}(\lambda) $，再左乘 $ L^{-1}(\lambda) $，并使得第三列元素分别相等，可解得 $ \theta $ 和 $ \psi $：

$$ \psi=\arctan\left[\frac{\boldsymbol{o}_{i}\cdot\boldsymbol{a}_{i+1}}{\boldsymbol{n}_{i}\cdot\boldsymbol{a}_{i+1}}\right],\quad-\pi\leqslant\psi<\pi $$

$$ \theta=\arctan\left[\frac{\left[\left(\boldsymbol{n}_{i}\cdot\boldsymbol{a}_{i+1}\right)^{2}+\left(\boldsymbol{o}_{i}\cdot\boldsymbol{a}_{i+1}\right)^{2}\right]^{1/2}}{\boldsymbol{a}_{i}\cdot\boldsymbol{a}_{i+1}}\right],\quad-\pi\leqslant\theta\leqslant\pi $$

为了求出 $ \phi $，可将方程(9.28)两边左乘 $ R_{a}^{-1}(\lambda)L^{-1}(\lambda) $，并使它们的对应元素分别相等，得

$$ \begin{aligned}&s\phi=-\left.s\psi c\psi\upsilon\left(\theta\right)\left(\boldsymbol{n}_{i}\cdot\boldsymbol{n}_{i+1}\right)+\left[c^{2}\psi\upsilon\left(\theta\right)+c\left(\theta\right)\right]\left(\boldsymbol{o}_{i}\cdot\boldsymbol{n}_{i+1}\right)-s\psi s\left(\theta\right)\left(\boldsymbol{a}_{i}\cdot\boldsymbol{n}_{i+1}\right)\right.\\&\left.c\phi=-\left.s\psi c\psi\upsilon\left(\theta\right)\left(\boldsymbol{n}_{i}\cdot\boldsymbol{o}_{i+1}\right)+\left[c^{2}\psi\upsilon\left(\theta\right)+c\left(\theta\right)\right]\left(\boldsymbol{o}_{i}\cdot\boldsymbol{o}_{i+1}\right)-s\psi s\left(\theta\right)\left(\boldsymbol{a}_{i}\cdot\boldsymbol{o}_{i+1}\right)\right.\right.\\&\left.\phi=\arctan\left[\frac{s\phi}{c\phi}\right],\quad-\pi\leqslant\theta\leqslant\pi\right.\\ \end{aligned} $$

4. 两段路径之间的过渡

前面利用驱动变换 $ D(\lambda) $ 来控制一个移动和两个转动生成两节点之间的“直线”运动轨

迹 $ ^{0}T_{6}(\lambda)=^{0}T_{B}^{B}P_{i}D(\lambda)^{6}T_{E}^{-1} $。现在讨论两段路径之间的过渡问题。为了避免两段路径衔接点处速度不连续，当由一段轨迹过渡到下一段轨迹时，需要加速度或减速度。在机械手手部到达节点前的时刻 $ \tau $开始改变速度，然后保持加速度不变，直至到达节点之后 $ \tau $（单位时间）为止，如图9.7所示。在此时间区间 $ [-\tau,\tau] $，每一分量的加速度保持不变，其值为

$$ \ddot{\boldsymbol{x}}(t)=\frac{1}{2\tau^{2}}\Big[\Delta\boldsymbol{C}\frac{\tau}{T}+\Delta\boldsymbol{B}\Big],\quad-\tau

其中，

$$ \ddot{\boldsymbol{x}}(t)=\begin{bmatrix}\ddot{x}\\ \ddot{y}\\ \ddot{\theta}\\ \ddot{\phi}\end{bmatrix},\quad\Delta\boldsymbol{C}=\begin{bmatrix}x_{BC}\\ y_{BC}\\ \theta_{BC}\\ \phi_{BC}\end{bmatrix},\quad\Delta\boldsymbol{B}=\begin{bmatrix}x_{BA}\\ y_{BA}\\ \theta_{BA}\\ \phi_{BA}\end{bmatrix} $$

矢量 $ \Delta C $ 和 $ \Delta B $ 的各元素分别为节点 B 到 C 和节点 B 到 A 的直角坐标距离和角度。T 为机械手手部从节点 B 到 C 所需时间。

由式(9.37)可以得出相应的在区间 $ -\tau < t < \tau $中的速度和位移：

$$ \dot{\boldsymbol{x}}(t)=\frac{1}{\tau}\bigg[\Delta\boldsymbol{C}\frac{\tau}{T}+\Delta\boldsymbol{B}\bigg]\boldsymbol{\lambda}-\frac{\Delta\boldsymbol{B}}{\tau} $$

$$ \boldsymbol{x}(t)=\bigg[\Big(\Delta\boldsymbol{C}\frac{\tau}{T}+\Delta\boldsymbol{B}\Big)\boldsymbol{\lambda}-2\Delta\boldsymbol{B}\bigg]\boldsymbol{\lambda}+\Delta\boldsymbol{B} $$

其中，

$$ \begin{array}{r}{\boldsymbol{x}(t)=\left[\begin{matrix}{x}\\ {y}\\ {z}\\ {\theta}\\ {\phi}\\ \end{matrix}\right],\quad\dot{\boldsymbol{x}}(t)=\left[\begin{matrix}{\dot{x}}\\ {\dot{y}}\\ {\dot{z}}\\ {\dot{\theta}}\\ {\dot{\phi}}\\ \end{matrix}\right],\quad\lambda\overset{\mathrm{d e f}}{=}\frac{t+\tau}{2\tau}}\end{array} $$

在时间区间 $ \tau < t < \tau $，运动方程为

$$ \boldsymbol{x}=\Delta\boldsymbol{C}\boldsymbol{\lambda},\quad\dot{\boldsymbol{x}}=\frac{\Delta\boldsymbol{C}}{T},\quad\ddot{\boldsymbol{x}}=0 $$

其中, $ \lambda=\frac{t}{T} $，代表归一化时间，变化范围是 $ [0,1] $。不过，对于不同的时间间隔，归一化因子通常是不同的。

对于由 A 到 B，再到 C 的运动，把 $ \psi $ 定义为在时间区间 $ -\tau < t < \tau $ 中运动的线性插值，即

$$ \psi^{^{\prime}}=(\psi_{BC}-\psi_{AB})\lambda+\psi_{AB} $$

式中， $ \psi_{AB} $ 和 $ \psi_{BC} $ 分别是由 A 到 B 和由 B 到 C 的运动规定的，与式(9.34)类似。因此， $ \psi $ 将由 $ \psi_{AB} $ 变化到 $ \psi_{BC} $。

总之，为了从节点 $ P_{i} $ 运动到 $ P_{i+1} $，首先由式(9.28)～式(9.36)算出驱动函数。然后按式(9.26)计算 $ {}^{0}T_{6}(\lambda) $，再由运动学反解程序算出相应的关节变量。必要时，可在反解求出的节点之间再用二次多项式进行插值。

笛卡儿空间的规划方法不仅概念上直观，而且规划的路径准确。笛卡儿空间的直线运动仅仅是轨迹规划的一类，更加一般地应包含其他轨迹，如椭圆、抛物线、正弦曲线等。可是缺乏适当的传感器测量手部笛卡儿坐标，进行位置和速度反馈。笛卡儿空间路径规划的结果需要实时变换为相应的关节坐标，计算量很大，致使控制间隔拖长。如果在规划时考虑机械手的动力学特性，就要以笛卡儿坐标给定路径约束，同时以关节坐标给定物理约束（例如，各电机的容许力和力矩，速度和加速度极限），使得优化问题具有在两个不同坐标系中的混合约束。因此，笛卡儿空间规划存在由于运动学反解带来的问题。

9.4 规划轨迹的实时生成

前面所述的计算结果即构成了机器人的轨迹规划。运行中的轨迹实时生成是指由这些数据，以轨迹更新的速率不断产生 $ \theta $， $ \dot{\theta} $和 $ \ddot{\theta} $所表示的轨迹，并将此信息送至机械手的控制系统。

1. 关节空间轨迹的生成

前面介绍了几种关节空间轨迹规划的方法。按照这些方法，其计算结果都是有关各个路径段的一组数据。控制系统的轨迹生成器和这些数据以轨迹更新速率具体计算出 $ \theta $， $ \dot{\theta} $和 $ \ddot{\theta} $。

对于三次样条，轨迹生成器只需随 t 的变化不断地按式(9.3)和式(9.4)计算 $ \theta, \dot{\theta} $ 和 $ \dot{\theta} $。当到达路径段的终点时，调用新路径段的三次样条系数，重新赋 t 为零，继续生成轨迹。

对于带抛物线过渡的直线样条插值，每次更新轨迹时，应首先检测时间 t 的值以判断当前处于路径段的是线性域还是过渡域。当处于线性域时，各关节的轨迹按下式计算：

$$ \left.\begin{array}{l}{\theta=\theta_{j}+\dot{\theta}_{j k}t}\\ {\dot{\theta}=\dot{\theta}_{j k}}\\ {\ddot{\theta}=0}\end{array}\right\} $$

其中，t 是从第 j 个路径点算起的时间； $ \dot{\theta}_{jk} $ 的值在轨迹规划时由式(9.18)算出。当处于过渡域时，各关节轨迹按下式计算：令 $ t_{inb}=t-\left(\frac{1}{2}t_{j}+t_{jk}\right) $，则

$$ \left.\begin{aligned}&\boldsymbol{\theta}=\theta_{j}+\dot{\theta}_{j k}(t-t_{i n b})+\frac{1}{2}\ddot{\theta}_{k}t_{i n b}^{2}\\&\dot{\boldsymbol{\theta}}=\dot{\theta}_{j k}+\ddot{\theta}_{k}t_{i n b}\\&\ddot{\boldsymbol{\theta}}=\ddot{\theta}_{k}\\ &\end{aligned}\right\} $$

其中， $ \dot{\theta}_{jk} $， $ \dot{\theta}_{k} $， $ t_{j} $ 和 $ t_{jk} $ 在轨迹规划时已由式(9.18)至式(9.22)算出。当进入新的线性域时，重

新把 t 置成 $ \frac{1}{2}t_{k} $，利用该路径段的数据，继续生成轨迹。

2. 笛卡儿空间轨迹的生成

前面已经讨论了笛卡儿空间轨迹规划方法。机械手的路径点通常是用工具坐标系相对工作坐标系的位姿表示的。为了在笛卡儿空间中生成运动轨迹，根据路径段的起始点和目标点构造驱动函数 $ \pmb{D}(1) $，见式(9.27)；再将驱动函数 $ \pmb{D}(\lambda) $ 用一个平移运动和两个旋转运动来等效代替，见式(9.28)；然后对平移运动和旋转运动插值，便得到笛卡儿空间路径（包括位置和方向），其中方向的表示方法类似于欧拉角。

仿照关节空间方法，使用带抛物线过渡的线性函数比较合适。在每一路径段的直线域内，描述位置 p 的三元素按线性函数运动，可以得到直线轨迹；然而，若把各种路径点的姿态用旋转矩阵 R 表示，那么就不能对它的元素进行直线插值。因为任一旋转矩阵都是由三个归一正交列组成，如果在两个旋转矩阵的元素间进行插值就难以保证满足归一正交的要求，不过可以用等效转轴-转角来表示函数 $ D(\lambda) $ 的旋转矩阵部分。

实际上，任何两个路径点 $ ^{B}P_{i} $和 $ ^{B}P_{i+1} $，它代表两个坐标系，驱动函数 $ D(1) $表示 $ ^{B}P_{i+1} $相对 $ ^{B}P_{i} $的位置和姿态，即

$$ \pmb{D}(1)={}^{B}\pmb{P}_{i}^{-1}{}^{B}\pmb{P}_{i+1} $$

根据第二章等效转轴-转角的概念， $ \boldsymbol{D}(1) $ 的旋转矩阵可用一个单位矢量-等效转轴 $ \boldsymbol{k}=[k_{x},k_{y},k_{z}]^{\mathrm{T}} $ 和一个等效角度 $ \theta $ 表示，即 $ ^{B}P_{i+1} $ 的姿态可以视为开始与 $ ^{B}P_{i} $ 的一致，然后绕 k 轴按右手规则转 $ \theta $ 角所得。因此， $ ^{B}P_{i+1} $ 相对于 $ ^{B}P_{i} $ 的姿态记为 $ _{i+1}^{i}\boldsymbol{R}(k,\theta) $。

把等价转轴-转角用三维矢量 $ i\boldsymbol{k}_{i+1}=k\boldsymbol{\theta}\left[k_{z},k_{y},k_{z}\right]^{\mathrm{T}}\boldsymbol{\theta} $表示， $ ^{B}\mathbf{P}_{i+1} $相对于 $ ^{B}\mathbf{P}_{i} $的位置用三维矢量 $ ^{B}\boldsymbol{p}_{i+1} $表示，用 $ 6\times1 $的矢量表示 $ ^{i}\mathbf{X}_{i+1} $表示 $ ^{B}\mathbf{P}_{i+1} $相对于 $ ^{B}\mathbf{P}_{i} $的位姿，即

$$ {}^{i}\mathbf{X}_{i+1}=\left[\begin{matrix}{^{i}\boldsymbol{p}_{i+1}}\\ {^{i}\boldsymbol{k}_{i+1}}\\ \end{matrix}\right] $$

两路径点之间的运动采用这种表示之后，就可以选择适当的样条函数，使这6个分量从一个路径点平滑地运动到下一点。例如带抛物线过渡的线性样条，使得两路径点间的路径是直线的，当经过路径点时，夹手运动的线速度和角速度将平稳变化。

另外还要说明的是，等效转角不是唯一的，因为

$$ (k,\theta) 等效于 (k,\theta+n\times360^{\circ}),\quad n 为整数 $$

从一路径点向下一点运动时，总的转角一般应取最小值，即使它小于 $ 180^{\circ} $。

在采用带抛物线过渡的线性轨迹规划方法时，需要附加一个约束条件：每个自由度的过渡域持续时间必须相同，这样才能保证各自由度形成的复合运动在空间形成一条直线。因为相对各自由度在过渡域的时间相同，因而在过渡域的加速度便不相同。所以，在规定过渡域的持续时间时，应该计算相应的加速度，使之不要超过加速度的容许上限。

笛卡儿空间轨迹实时生成方法与关节空间相似，例如，带有抛物线过渡的线性轨迹，在线性域中，照搬式（9.41），X的每一自由度按下式计算：

$$ \left.\begin{array}{l}{x=x_{j}+\dot{x}_{j k}t}\\ {\dot{x}=\dot{x}_{j k}}\\ {\ddot{x}=0}\end{array}\right\} $$

其中，t 是从第 j 个路径点算起的时间； $ \dot{x}_{jk} $ 是在轨迹规划过程中由类似于式(9.18)的方程求出的。在过渡域中，照搬公式(9.42)，每个自由度的轨迹按下式计算：

$$ \left.\begin{aligned}&t_{i n b}=t-\left(\frac{1}{2}t_{j}+t_{j k}\right)\\ &x=x_{j}+\dot{x}_{j k}\left(t-t_{i n b}\right)+\frac{1}{2}\ddot{x}_{k}t_{i n b}^{2}\\ &\dot{x}=\dot{x}_{j k}+\ddot{x}_{k}t_{i n b}\\ &\ddot{x}=\ddot{x}_{k}\\ \end{aligned}\right\} $$

其中， $ \ddot{x}_{k} $， $ \dot{x}_{jk} $， $ t_{j} $ 和 $ t_{jk} $ 的值在轨迹规划过程中算出，与关节空间的情况完全相同。

最后，必须将这些笛卡儿空间轨迹 $ (X, \hat{X} $和 $ X) $转换成等价的关节空间的量。对此，可以通过求解逆运动学得到关节位移；用逆雅可比计算关节速度；用逆雅可比及其导数计算角加速度，在实际中往往采用简便的方法，即将X以轨迹更新速率转换成等效的驱动矩阵 $ D(\lambda) $，再由运动学反解子程序计算相应的关节矢量q，然后由数值微分计算 $ \dot{q} $和 $ \ddot{q} $。算法如下：

$$ \begin{aligned}&\mathbf{X}\rightarrow\mathbf{D}(\lambda)\\&q(t)=Solve(\mathbf{D}(\lambda))\\&\dot{\mathbf{q}}(t)=\frac{q(t)-q(t-\delta t)}{\delta t}\\&\ddot{q}(t)=\frac{\dot{q}(t)-\dot{q}(t-\delta t)}{\delta t}\\ \end{aligned} $$

根据计算结果： $ q, \dot{q} $ 和 $ \ddot{q} $ 由控制系统执行。

9.5 本章小结

本章讨论了属于底层规划的机器人轨迹规划问题，它是在机械手运动学和动力学的基础上，研究关节空间和笛卡儿空间中机器人运动的轨迹规划和轨迹生成方法。在阐明轨迹规划应考虑的问题之后，着重讨论了关节空间轨迹的插值计算方法，包括三次多项式插值、过路径点的三次多项式插值、高阶多项式插值、用抛物线过渡的线性插值和过路径点的用抛物线过渡的线性插值等方法。然后阐述了笛卡儿空间路径轨迹规划方法，涉及物体对象的描述、作业的描述、两个节点之间的“直线”运动和两段路径之间的过渡等技术。最后，本章简介了规划轨迹的实时生成方法，包含关节空间轨迹的生成和笛卡儿空间轨迹的生成等方法。

习题9

9.1 已知一台单连杆机械手的关节静止位置为 $ \theta = -5^{\circ} $ 。该机械手从静止位置开始在 4s 内平滑转动到 $ \theta = 80^{\circ} $ 停止位置。试进行下列计算：

（1）计算完成此运动并使机械臂停在目标点的3次曲线的系数。

（2）计算带抛物线过渡的线性插值的各个参数。

（3）画出该关节的位移、速度和加速度曲线。

9.2 已知一台单连杆机械手的关节静止位置为 $ \theta = -5^{\circ} $ 。该机械手从静止位置开始在 4s 内平滑转动到 $ \theta = 80^{\circ} $ 位置并平滑地停止。试进行下列计算：

（1）计算带抛物线拟合的直线轨迹的各个参数。

（2）画出该关节的位置、速度和加速度曲线。

9.3 平面机械手的两连杆长度均为 1m，要求从初始位置 $ (x_{0}, y_{0}) = (1.96, 0.50) $ 移至终止位置 $ (x_{f}, y_{f}) = (1.00, 0.75) $。初始位置和终止位置的速度和加速度均为 0。试求每一关节的三次多项式的系数。可把关节轨迹分成几段路径来求解。

9.4 六关节机械手沿着一条三次曲线通过2个中间点并停止在目标点，需要计算几条不同的三次曲线？

9.5 针对以下两种情况，用 MATLAB 编写一个程序，以建立单关节多项式关节空间轨迹生成方程。对给定任务输出结果。对于每种情况，给出关节角、角速度、角加速度及角加速度变化率的多项式函数。

（1）三阶多项式 令起始点和终止点的角速度为 0。已知初始点的 $ \theta_{0}=120^{\circ} $，终止点的 $ \theta_{f}=60^{\circ} $， $ t_{f}=1s $。

（2）五阶多项式 令起始点和终止点的角速度和角加速度均为 0。已知初始点的 $ \theta_{0}=120^{\circ} $，终止点的 $ \theta_{f}=60^{\circ} $， $ t_{f}=1s $。把计算结果与（1）加以比较。

9.6 试求单个关节从 $ \theta_{0} $ 运动到 $ \theta_{f} $ 的三次样条（多项式）曲线，要求 $ \dot{\theta}(0)=0,\dot{\theta}(t_{f})=0 $ ，而且 $ \left\|\dot{\theta}(t)\right\|<\dot{\theta}_{\max} $， $ \left\|\ddot{\theta}(t)\right\|<\ddot{\theta}_{\max},t\in\left[0,t_{f}\right] $。求出三次多项式的系数及 $ t_{f} $ 值。

9.7 在 $ [0,1] $时间区间内，使用一条三次样条曲线轨迹 $ \theta(t)=10+90t^{2}-60t^{3} $。试求该轨迹的起始点和终止点的位置、速度和加速度。

9.8 在[0,2]时间区间内，使用一条三次样条曲线轨迹 $ \theta(t)=10+90t^{2}-60t^{3} $。试求该轨迹的起始点和终止点的位置、速度和加速度。

9.9 在 $ [0,1] $ 时间区间内，使用一条三次样条曲线轨迹 $ \theta(t)=10+5t+70t^{2}-45t^{3} $ 。试求该轨迹的起始点和终止点的位置、速度和加速度。

9.10 在[0,2]时间区间区内，使用一条三次样条曲线轨迹 $ \theta(t)=10+5t+70t^{2}-45t^{3} $。试求该轨迹的起始点和终止点的位置、速度和加速度。

9.11 一台单连杆旋转式机械手停在初始位置 $ \theta = -5^{\circ} $ 处，要求在 4s 内平滑移动它至目标位置 $ \phi = 80^{\circ} $，并实现平滑停车。当路径为混合抛物线的线性轨迹时，试计算此轨迹的相应参数，并画出此关节的位置、速度和加速度随时间变化曲线。

9.12 已知

$$ \phi_{1}(t)=a_{10}+a_{11}t+a_{12}t^{2}+a_{13}t^{3} $$

和

$$ \phi_{2}(t)=a_{20}+a_{21}t+a_{22}t^{2}+a_{23}t^{3} $$

为两个描述某个经过中间点的两段连续加速度仿样函数的三次方程式。令初始角度为 $ \theta_{0} $，中间点位置 $ \theta_{v} $，目标点为 $ \theta_{g} $。每个三次方程式将在 t=0（开始时间）至 $ t=t_{fi} $（结束时间，i=1,2）时间隔内进行计算。强加约束如下：

$$ \theta_{0}=a_{10} $$

$$ \theta_{v}=a_{10}+a_{11}t_{f1}+a_{12}t_{f1}^{2}+a_{13}t_{f1}^{3} $$

$$ \theta_{v}=a_{20} $$

$$ \theta_{2}=a_{20}+2a_{21}f_{f2}+a_{22}f_{f2}^{2}+a_{23}f_{f2}^{3} $$

$$ 0=a_{11} $$

$$ 0=a_{21}+2a_{22}t_{f2}+3a_{23}t_{f2}^{2} $$

$$ a_{11}+2a_{12}t_{f1}+3a_{13}t_{f1}^{2}=a_{21} $$

$$ 2a_{12}+6a_{13}t_{f1}=2a_{22} $$

对于 $ \theta_{0}=5^{\circ},\theta_{v}=15^{\circ},\theta_{g}=40^{\circ} $ 以及每段持续时间为 1s 时，画出这两段连续轨迹的关节位置、速度和加速度图。