18、机器人轨迹优化：在线速度缩放与路径质量提升

机器人轨迹优化：在线速度缩放与路径质量提升

1. 在线速度缩放算法

在机器人的任务空间轨迹规划中，在线速度缩放算法是一个关键的研究领域。该算法将预测范围内所有时间步的运动推进参数 $s_i$ 的连续值存储在内存缓冲区 $sBuf$ 中，而针对 $sBuf$ 中存储的点计算得到的缩放因子 $\lambda_i$ 则存于另一个缓冲区 $\lambdaBuf$。

在估计速度和加速度时，采用前一个 $q$ 值（利用后向有限差分公式）。可以从 $sBuf$ 中获取先前的 $s$ 值（如 $s_{i - 1}$，$s_{i - 2}$ 等）来计算 $q_{i - 1}$ 和 $q_{i - 2}$。由于在缩放过程中缓冲区中存储的 $s$ 值会发生变化，因此存储 $q$ 没有意义。

经典方法会对整个轨迹进行统一缩放（且仅进行一次），以确保不违反关节的速度和加速度限制。而提出的方法则是对轨迹进行部分缩放，并且同一位置可能在预测范围内（内存缓冲区中）被多次缩放。这可能导致在先前缩放（或尚未缩放）的轨迹与新缩放的轨迹之间的过渡点处，所需的运动减速较大，从而可能违反关节的速度或加速度限制。

为解决此问题，在增强版本的算法中，会检测并消除这些问题，但需要额外的计算。具体步骤如下：
1. 计算 $sBuf$ 末尾新添加点的缩放因子 $\lambda_{end}$。
2. 计算 $sBuf$ 中第一个点的最大可行缩放因子 $\lambda_{start}$。
3. 使用两个 $\lambda$ 中的较小值（$\min{\lambda_{start}, \lambda_{end}}$）对 $sBuf$ 内的点进行缩放。
4. 若 $\lambda_{