运动控制 双轮差速模型轨迹规划

一、轨迹规划

1.1轨迹平滑与轮迹

• 初始时，我们有一条由若干坐标点构成的机器人运行路径。通过对这些坐标点进行插值，我们可以得到一个更加平滑的轨迹，使得机器人在移动时能够更平稳地过渡。
• 接着，我们基于这条中心轨迹，计算出左轮和右轮的轨迹。这是通过向中心轨迹的左侧和右侧分别偏移一定宽度来实现的。
  

1.2 目标距离

• 对于中心轨迹以及左轮和右轮的轨迹，我们分别计算出每个点到终点的距离。这个距离是为了确保机器人在接近终点时能够逐渐减速并停下来。
• 同时，我们还计算出每个点到其下一个点的距离。这个距离将用于后续的速度计算。

1.3 速度限制

• 对于轨迹上的每个点，我们都根据当前速度、最大速度限制、最大加速度以及到下一个点和终点的距离，计算出三个速度限制：最大速度限制（max_v）、基于到下一个点距离的加速限制速度（max_v_acc）和基于到终点距离的减速限制速度（max_v_dec）。
• 注：相邻两个点之间计算max_v_acc时，以前一点的速度计算下一点能达到的最大速度 (v^2 = u^2 + 2ax)。
• 然后，我们选择这三个速度限制中的最小值作为该点的最大允许速度。

1.4 候选速度的计算与调整

• 在计算出中心轨迹上每个点的最大允许速度后，我们得到一个候选速度。这个速度是基于中心轨迹的当前速度、最大速度限制、最大加速度以及到下一个点和终点的距离来计算的。
• 接着，我们根据左轮和右轮轨迹上对应点的距离比例，将候选速度分配到左轮和右轮上，得到左轮和右轮的候选速度。
• 如果左轮或右轮的候选速度超过了它们各自的最大允许速度，我们就需要对候选速度进行调整。调整的原则是：确保左轮和右轮的速度都不会超过它们的最大允许速度，并且尽量保持中心轨迹的速度不变。
• 调整后的候选速度被用作当前点的实际速度，并更新到结果路径中。

1.5 路径生成

• 经过上述计算，我们得到了一个包含每个点速度和位置的结果路径。这个路径考虑了机器人的最大速度限制、最大加速度以及左轮和右轮的速度限制。
• 最后，我们在结果路径的末尾添加了一个速度为0的点，表示机器人已经完全停止。

二、双轮轨迹

2.1 计算梯度

• 首先遍历中心轨迹上的每个点（除了最后一个点），计算相邻两点之间的差值（即位移向量）。
• 使用atan2函数，根据位移向量的y分量和x分量计算出每段轨迹的倾斜角度（即梯度）。这个角度表示了轨迹在该点的切线方向。
• 为了确保最后一段轨迹也有一个梯度值，将最后一个计算出的梯度值复制到梯度数组的末尾。

2.2 计算偏移轨迹

• 接着创建一个新的路径（即偏移轨迹），这个路径将包含与中心轨迹相同数量的点。
• 对于中心轨迹上的每个点，根据该点的梯度值和给定的偏移量（offset），计算出偏移轨迹上对应点的坐标。这个计算过程考虑了是计算左轮轨迹还是右轮轨迹（通过left参数来控制）。
  • 如果是计算左轮轨迹，会给梯度值加上90度（即M_PI / 2.0），这样计算出的偏移点就会位于中心点的左侧。
  • 如果是计算右轮轨迹，会给梯度值减去90度（即-M_PI / 2.0），这样计算出的偏移点就会位于中心点的右侧。
• 使用三角函数（cos和sin）和偏移量，计算出偏移轨迹上每个点的x坐标和y坐标。
• 最后，将计算出的偏移点添加到偏移轨迹中。

2.3 返回结果

• 返回计算出的偏移轨迹，这个轨迹是一系列坐标点构成的路径，表示了左轮或右轮的行驶轨迹。