1、三次多项式插值

  当已知末端操作器的起始位姿和终止位姿时，由逆向运动学，即可求出对应于两位姿的各个关节角度。因此，末端操作器实现两位姿的运动轨迹描述，可在关节空间中用通过起始点关节角和终止点关节角的一个平滑轨迹函数来表示。

  为了实现关节的平稳运动，每个关节的轨迹函数θ(t)至少需要满足四个约束条件：两端点位置约束和两端点速度约束。

  端点位置约束是指起始位姿和终止位姿分别所对应的关节角度。在t＝0时刻的值是起始关节角度,在终端时刻的值是终止关节角度,即

  为满足关节运动速度的连续性要求，在起始点和终止点的关节速度可简单地设定为零，即

上述给出的四个约束条件可以唯一地确定一个三次多项式：

运动过程中的关节速度和加速度则为

为了求得三次多项式的系数，代以给定的约束条件，有方程组

求解上述方程组，可得

所以，对于初速及终速为零的关节运动，满足连续平稳运动要求的三次多项式插值函数为

其关节角速度和角加速度表达式为

三次多项式插值的运动轨迹曲线

图5.3 三次多项式插值关节运动轨迹

（a）角位移—时间曲线；（b）角速度—时间曲线；（c）角加速度—时间曲线

例：

  设机械手的某个关节的起始关节角＝15°，并且机械手原来是静止的。要求在3秒钟内平滑地运动到=75°时停下来(即要求在终端时速度为零)。规划出满足上述条件的平滑运动的轨迹，并画出关节角位置、角速度及角加速度随时间变化的曲线。

解：    

  根据所给约束条件，直接代入公式，可得：

       =15， =0， =20， =-4.44

  所求关节角的位置函数为：

对上式求导，可以得到角速度和角加速度

  根据上式可画出它们随时间的变化曲线如图5.5所示。由图看出，速度曲线为一抛物线，加速度则为一直线。

图5.5  三次多项式规划出的关节角的运动轨迹

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

  如图5.6所示，机器人作业除在A、B点有位姿要求外，在路径点C、D…也有位姿要求。对于这种情况，假如终端执行器在路径点停留，即各路径点上速度为0，则轨迹规划可连续直接使用前面介绍的三次多项式插值方法；但如果只是经过，并不停留，就需要将前述方法推广。

图5.6  机器人作业路径点

  在某段路径上,起始点为和"，终止点为和"。这时，确定三次多项式系数的方法与前面所述的完全一致，只不过是速度约束条件变为

利用约束条件确定三次多项式系数，有下列方程组：

求解方程组

路径点上的关节速度，可由以下任一规则确定:

1)如果机器人末端操作器在经过路径点时有速度要求，则可以利用此路径点上的逆雅可比矩阵, 把该路径点的直角坐标速度转换成关节坐标速度。轨迹规划时则以此作为速度约束条件。如果某个路径点是机器人的奇异点，即此点的逆雅可比不可求，这时就无法求关节速度了；此外，在求各点关节速度时，要逐点计算逆雅可比矩阵，并依此计算关节速度，耗时较多。

2)由控制系统采用某种启发式方法自动地选取合适的路径点速度。用三次多项式插值前，先假设各路径点之间关节运动速度是均匀的，即图中所示用直线段将这些路径点依次连接起来。

规则选定：如果相邻线段的斜率在路径点处改变符号，则速度选为零；如果相邻线段斜率不改变符号，则选取路径点两侧的线段斜率的平均值作为该点的速度。因此，只要给定路径点，系统就能依此规则自动生成相应的路径点速度。

3)按照保证每个路径点的加速度连续的原则，由控制系统自动地选择路径点的速度。为此，可以设法用两条三次曲线在路径点处按一定规则连接起来，拼凑成所要求的轨迹。拼凑的约束条件是：连接处速度连续，而且加速度也连续。

3、五次多项式插值

  除了指定运动段的起始点和终止点的位置和速度外，也可以指定该运动段的起始点和终止点加速度。这样，约束条件的数量就增加到了6个，相应地可采用下面的五次多项式来规划轨迹运动，即

  根据这些方程，可以通过角位置、角速度和角加速度的约束条件计算五次多项式的系数。

例 ：

  要求为一个六自由度机器人的第一关节进行轨迹规划。该关节在5s内从初始角30°运动到终端角75°，且起始点和终止点速度均为零，起始点角加速度和终止点角均为５/，求该机器人第一关节的角位置、角速度和角加速度随时间的变化规律。

解  由给出的条件得到

将起始和终止约束条件带入上3个公式得

可得到关节随时间变化的运动方程，即

如图5.8所示机器人关节的角位置、角速度、角加速度曲线，其最大角加速度为  .

图5.8  中关节的角位置、角速度和角加速度曲线

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

在关节空间轨迹规划中，对于给定起始点和终止点的情况，选择线性函数插值较为简单。然而，单纯线性插值会导致起始点和终止点的关节运动速度不连续，以及加速度无穷大，显然，这样在两端点会造成刚性冲击。

为此，对线性函数插值方案进行修正，在线性插值两端点的邻域内设置一段抛物线形缓冲区段。由于抛物线函数对于时间的二阶导数为常数，即相应区段内的加速度恒定，这样可保证起始点和终止点的速度平滑过渡，从而使整个轨迹上的位置和速度连续。

线性函数与两段抛物线函数平滑地衔接在一起形成的轨迹称为带有抛物线过渡域的线性轨迹，如图5.9所示。设两端的抛物线轨迹具有相同的持续时间和大小相同而符号相反的恒加速度。这种路径规划存在多个解，其轨迹并不唯一，如图5.10所示。但是，每条路径都对称于时间中点和位置中点。

 图5.10　轨迹的多解性与对称性     图5.9 带有抛物线过渡域的线性轨迹

若要保证路径轨迹的连续、光滑，则要求抛物线轨迹的终止点角速度必须等于线性段的角速度，故有下列关系

 式中：为对应于抛物线持续时间的关节角度。其的值可由下式求出：

设关节从起始点到终止点的总运动时间为，则=2并注意到

    则由前三式得

一般情况下，都是按任务规划好的参数，是已知条件，这样，根据式可以选择相应的和，得到相应的轨迹。通常的做法是先选定角加速度的值，然后求出相应的．即，即

由式可知，为保证有解，角加速度值必须选的足够大，即

当上式中的等号成立时，轨迹线性段的长度缩减为零，整个轨迹由两个过渡抛物线组成，这两个过渡抛物线在衔接处的斜率（关节速度）相等。角加速度值的值愈大，过渡抛物线的长度会愈短；若角加速度的值趋于无穷大，轨迹又复归到简单的线性插值情况。