关节轨迹插值计算优化

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

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

在关节空间中进行轨迹规划，需要给定机器人在起始点和终止点手部的位形。对关节进行插值时，应满足一系列的约束条件，例如：初始点（抓取物体）、提升点（提升物体）、下放点（放下物体）和停止点等节点上的位姿、速度和加速度的要求；与此相应的各个关节位移、速度、加速度在整个时间间隔内的连续性要求；其极值必须在各个关节变量的容许范围之内的要求；等等。在满足约束条件的前提下，可以选取不同类型的关节插值函数，生成不同的轨迹。

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

图6-2　单个关节的不同轨迹曲线

1.三次多项式插值

在机械手运动过程中，由于相应于起始点的关节角度θ0是已知的，而终止点的关节角θf可以通过运动学方法反解得到，因此，运动轨迹的描述，可用起始点关节角度与终止点关节角度的一个平滑插值函数θ（t）来表示，在t=0时刻的值是起始点关节角度θ0，在终端时刻tf的值是终止点关节角度θf。显然，有许多平滑函数可作为关节插值函数，如图6-2所示。

为了实现单个关节的平稳运动，轨迹函数θ（t）至少需要满足四个约束条件，其中两个约束条件是对起始点和终止点关节角度的约束：

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

上述四个边界约束条件（式（6-1）和式（6-2））唯一地确定了一个三次多项式：

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

对于式（6-3）和式（6-4），代入相应的约束条件得到有关系数a0，a1，a2和a3的四个线性方程：

求解此方程组可得

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

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

一般情况下，要求规划过路径点的轨迹。如果机械手在路径点停留，则可直接使用前面三次多项式插值的方法；如果只是经过路径点并不停留，则需要推广上述方法。实际上，可以把所有路径点也看作起始点或终止点，用求解逆运动学的方法得到相应的关节矢量值。然后确定所要求的三次多项式插值函数，把路径点平滑地连接起来。但是，这些起始点和终止点的关节运动速度不再是零。

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

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

求解方程组（6-8），即可求得三次多项式的系数

实际上，由式（6-9）确定的三次多项式描述了起始点和终止点具有任意给定位置和速度约束条件的运动轨迹，是式（6-6）的推广。剩下的问题就是如何确定路径点上的关节速度。解决方法有以下三种：

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

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

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

3.高阶多项式插值(www.xing528.com)

如果对运动轨迹的要求更为严格，约束条件增多，那么三次多项式就不能满足需要，必须用更高阶的多项式对运动轨迹的路径段进行插值。例如，对某段路径的起始点和终止点都规定了关节的位置、速度和加速度，则要用一个五次多项式进行插值，即

多项式的系数a0，a1，…，a5必须满足6个约束条件：

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

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

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

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

为了构造这段运动轨迹，假设两端的过渡域（抛物线）具有相同的持续时间，因而在这两个域中采用大小相同而符号相反的恒加速度。正如图6-3（b）所示，存在多个解，得到的轨迹不是唯一的。但是，每个结果都对称于时间中点th和位置中点θh。另外，图6-3中，θf为终止点的关节角，tf为终端时刻，θ0为起始关节角，tb为过渡域上限。

图6-3　带抛物线过渡域的线性轨迹

由于过渡域[t0，tb]终点的速度必须等于线性域的速度，所以

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

则由式（6-13）至式（6-15）得

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

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

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

图6-4　多段带有抛物线过渡的线性函数插值轨迹

第一个路径段和最后一个路径段的处理与式（6-19）略有不同，因为轨迹端部的整个过渡域的持续时间都必须计入这一路径段内。对于第一个路径段，令线性域速度的两个表达式相等，就可求出t1：

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

根据式（6-22）即可求出：

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

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