机器人学——2.2-分段轨迹

机器人应用中经常要求机器人平滑地沿一条路径运动，并不停顿地经过一个或多个中间节点。这样做或许是要在工作空间中避开障碍物，也可能是为了执行一项需要高精度连续轨迹的任务，如在制造业中要涂抹一圈密封胶。

为了使问题一般化，考虑该轨迹含有 $N$ 个中间点 $x_k$，$kin [1, N]$，因此有 $N-1$ 个运动段。设 $x_kinmathbb{R}^M$ 是位姿的一种向量表示。

机器人从静止状态 $x_1$ 开始运动，到 $x_N$ 结束停止，但在通过（或接近）中间点时不能停顿。生成这个运动轨迹的最大问题是过约束。为了实现速度连续，我们只能舍弃让轨迹能达到每个中间点。在一维情况下这个问题更容易理解，如图所示。该运动轨迹包括直线运动段与多项式曲线段的混合，如同lspb函数，但在这里我们选择了五次多项式，因为它们能够满足位置、速度和加速度在起点和终点的边界条件。

轨迹的第一段从速度为零的初始位姿 $x_1$ 开始加速，然后汇接入指向第二个位姿 $x_2$ 的直线段。汇接时间被设为常数 $t_{acc}$，在到达 $x_2$ 之前的 $t_{acc}/2$ 时间，直线轨迹又汇接上一段多项式曲线，持续 $t$ 时间后再次汇接到从 $x_2$ 到 $x_3$ 的直线段，然后就一直重复这一过程。可以给每个直线段设定一个恒速值 $dot{x}_k$。因此，汇接混合中的平均加速度是
$ddot{x}_{av}=dfrac{dot{x}_{k+1}-dot{x}_k}{t_{acc}}$如果该轴的最大加速能力已知，那么就可以计算出最小混合时间（该轴会达到其峰值加速度，而不是平均加速度。在五次多项式系数已知时，可以用五阶多项式的二阶导数计算出混合的峰值加速度）。
对于多轴的情况，很可能在某个特定运动段其中一些轴要比其他维度需要移动更多距离，而如果各个关节有不同的速度限制时，将会使轨迹生成变得复杂。为解决上述问题，第一步需要确定哪个轴将是完成最慢的，依据是每个轴对应这个运动段的运动距离以及该轴最大可达速度。这样可以算出该段的运动持续时间和各轴实际所需的速度。这确保了所有轴在同一时间达到下一个目标 $x_k$。
工具箱函数mstraj可以基于中间点矩阵生成一个多段多轴轨迹。例如，有 $4$ 个中间点的两轴运动可以用以下方法生成：

>> via = [ 4,1; 4,4; 5,2; 2,5 ];
>> q = mstraj(via, [2,1], [], [4,1], 0.05, 0);

函数mstraj的第一个参数是中间点矩阵，每点对应矩阵的一行。剩余的参数分别是：每轴的最大速度向量，每段的运动时间向量，起点各轴坐标，采样时间间隔，以及加速时间。函数mstraj返回一个矩阵，行对应的是时间每步，列对应的是各个轴。在没有提供输出参数时mstraj将直接绘制出轨迹曲线，如图所示。从这个例子所给的参数表明，第一个轴的最大速度比第二个轴更大。然而在最后一个运动段，两轴是以相同的速度运动的，因为这段的运动时间是由最慢的轴决定的。

如果增加加速时间：

>> q = mstraj(via, [2,1], [], [4,1], 0.05, 1);

轨迹会变得更圆滑，如上图所示。这是因为增加加速时间意味着轨迹上可以有更多的时间来完成多项式曲线段的混合。该函数也接受可选的起点和终点的速度参数，而且 $t_{acc}$ 可以是一个向量，给出每次混合的加速度时间。

值得注意的是，这个函数只是简单地对向量代表的位姿进行插值。在这个例子中，向量被假定为笛卡儿坐标向量，但这个函数也适用于欧拉角或横滚-俯仰-偏航角，然而这个函数不是对坐标系旋转进行插值的理想方式。有关对姿态的插值我们后面会继续讨论。