TEB轨迹规划算法教程-避障和模型

TEB轨迹规划算法教程-避障和模型

说明：

• 介绍如何实现避障。
• 描述了必要的参数设置，主要关注机器人足迹模型及其影响

Penalty terms罚款条款：

• 避障是作为整体轨迹优化的一部分实现的。
• 显然，优化涉及找到指定成本函数（目标函数）的最小成本解（轨迹）。
• 简单地说：离障碍物越近成本函数的成本值越大增加，反之越小。
• 理想情况下，在这些情况下，成本函数值必须是无穷大，否则优化程序可能会更好地完全拒绝这些区域。
• 但是，这需要优化器处理硬约束（分别解决非线性程序）。
• teb_local_planner放弃了考虑硬约束的能力，以便更好地考虑效率。
• 将硬约束转换为软约束，导致具有有限成本的二次惩罚项的组合。

• 上图显示了一个示例性惩罚术语（用于避障）。

• 允许到障碍物的最小欧几里德距离（参数min_obstacle_dist）设置为0.2米。

• 因此，低于0.2米的距离导致非零成本。

• 现在想象一下，优化问题包含更多的成本条件。

• 其中一些是冲突的，例如时间最优性。

• 因此，可能会发生这样的情况：优化程序会考虑一个小的违规（因此小额罚款），以便最小化整体组合成本函数。

• 在这里，您有两个选项可以调整行为：

  • 调整优化权重（单个成本的缩放，这里是参数weight_obstacle）。 但是如果你选择了太高的值，优化问题会变得病态，导致不良的收敛行为。

  • 通过添加“额外边距”来移动参数。 通过向min_obstacle_dist参数添加一个小的额外边距，您可以隐含地将成本值增加到0.2米。 您可以使用单个参数penalty_epsilon立即移动所有惩罚项，但要小心，因为这样做会严重影响优化结果。

本地最优解决方案

• 请注意，优化器本身只能找到本地最优解决方案。

• 想象一下，机器人可能被两个障碍物横向封装。

• 惩罚条件确实不为零，但优化器被卡住（达到这个局部最小值），因为横向移动相应的姿势朝向其中一个障碍将进一步增加总成本。

• 您可以使用test_optim_node轻松尝试（请参阅教程设置和测试优化并关闭并行计划）。

• 行为应该类似于下图中的行为：
  

• 轨迹不能越过障碍物。

• 即使姿势本身也被推离障碍物之间的区域（红色箭头）。 显然，在实践中应该避免这种情况。

• 因此，并行计划算法寻求（拓扑学上）替代解决方案，并且可行性检查（参见下面的部分）在实际命令机器人之前拒绝这样的解决方案。

姿势与障碍之间的关联

• 下图描绘了常见计划方案的快照：

• 该场景包括移动机器人，其在前往当前目标的同时接近多边形障碍物。

• 计划的（离散的）轨迹由多个机器人姿势组成。

• 规划器旨在根据期望的时间分辨率（参数dt_ref）来布置每两个连续的姿势。

• 注意，实际分辨率不是固定/冻结的，因为优化器需要调整转换时间以寻求时间最优性。

• 为避免障碍，计划姿势与障碍物之间的距离必须从下方开始。

• 图中的示例轨迹由8个可变姿势组成（开始和目标姿势是固定的）。

• 您可能会同意，为了实现无碰撞轨迹，需要进行许多距离计算（优化程序会多次调用成本函数值）。

• 为了加速优化，实施专用关联策略。

• 对于每个障碍物（点/占用的costmap单元格，线，多边形），计划轨迹的最近姿势被定位（参见上图）。

• 根据参数obstacle_poses_affected的值，也考虑最近姿势的邻居。

• 在随后的优化步骤中仅考虑该选定的姿势子集（这里是3个姿势，因此是3个惩罚项）。

• 在no_inner_iterations（参数）之后重复关联过程。

• 在每个外部优化迭代中。 obstacle_poses_affected的值略微影响障碍物周围轨迹的平滑度。

• 更大的障碍需要更多连接姿势以避免不允许的捷径。

• 您还可以选择较高的值（>轨迹长度），以便将所有姿势与每个障碍物连接起来。

• 注意，机器人足迹模型被考虑用于距离计算，因此对于所需的计算资源是至关重要的。

• 详细信息总结在以下部分中。

机器人足迹模型

• 机器人足迹模型近似于机器人的2D轮廓以进行优化。
• 该模型对于距离计算的复杂性以及计算时间至关重要。
• 因此，机器人足迹模型构成专用参数，而不是从公共costmap_2d参数加载足迹。
• 优化足迹模型可能与costmap足迹模型不同（后者用于可行性检查，请参阅下面的部分）。
• 使用参数服务器选择和配置足迹模型。
• 您可以将以下参数结构添加到teb_local_planner配置文件中：

TebLocalPlannerROS:
 footprint_model: # types: "point", "circular", "line", "two_circles", "polygon"
   type: "point"
   radius: 0.2 # for type "circular"
   line_start: [-0.3, 0.0] # for type "line"
   line_end: [0.3, 0.0] # for type "line"
   front_offset: 0.2 # for type "two_circles"
   front_radius: 0.2 # for type "two_circles"
   rear_offset: 0.2 # for type "two_circles"
   rear_radius: 0.2 # for type "two_circles"
   vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"

• 默认封装模型的类型为“point”。
• 注意，足迹发布到〜/teb_markers并且可以在rviz中可视化（例如用于验证）。
• 重要提示：对于类似汽车的机器人，姿势[0,0]位于后轴（旋转轴）！

不同类型：

• Footprint Type: Point

• 机器人被建模为单点。 对于这种类型，需要最少的计算时间。

• Footprint Type: Circular

• 机器人被建模为具有给定半径〜/footprint_model/radius的简单圆。

• 距离计算类似于点型机器人的距离计算，但是通过注释，机器人的半径被添加到参数min_obstacle_dist中的每个函数调用。

• 您可以通过选择一个点型机器人并将半径添加到a-priori的最小障碍物距离来消除这种额外的增加。

• Footprint Type: Line

• 线机器人对于在纵向和横向上表现出不同膨胀/长度的机器人是有用的。

• 可以使用参数〜/footprint_model/line_start和〜/footprint_model/line_end（每个[x，y]坐标）来配置线（段）。

• 机器人（旋转轴）假定为[0,0]（单位：米）。

• 确保通过进一步调整参数min_obstacle_dist来封装整个机器人（参见下面的示例）。

• 您可能还会看一下“两个圆圈”模型。

• Footprint Type: Two Circles

• 近似机器人轮廓的另一种可能性是定义两个圆。

• 每个圆由沿着机器人的x轴的偏移和半径描述：

~/footprint_model/front_offset
~/footprint_model/front_radius
~/footprint_model/rear_offset
~/footprint_model/rear_radius.

• offset也可能是负向的。

• 请参考下图作为示例：
  

• 对于每个相关的机器人姿势，需要两个距离计算。

• Footprint Type: Polygon

• 可以通过定义闭合多边形来合并复杂模型。

• 多边形是根据顶点列表定义的（为每个顶点提供x和y坐标）。

• 假设机器人旋转轴位于[0,0]（单位：米）。

• 由于多边形自动关闭，请不要重复第一个顶点。

• 请记住，每个额外的边缘都会显着增加所需的计算时间！

• 您可以从costmap公共参数文件中复制您的足迹模型。

可行性检查

• 在优化器返回轨迹之后并且在将速度命令发送到机器人之前执行可行性检查。

• 此检查的目的是识别可能由优化器生成的无效/不可行轨迹（只记住：软约束，局部最小值等）。

• 目前，该算法从当前机器人姿势开始迭代前n个姿势（n =〜/sustainability_check_no_poses（参数！））并检查这些姿势是否是无碰撞的。

• 要检测是否发生碰撞，请使用costmap足迹（请参阅导航教程）！

• 因此，此验证模型可能比用于优化的占用空间更复杂（请参阅上文）。

• 值〜/sustainability_check_no_poses不应选择得太高，因为优化器可能不会完全收敛

• 形象地说，在（远）未来的小障碍物违规可以在机器人向目标移动时得到纠正。

• 如果您在狭窄的环境中驾驶，请确保正确配置避障行为（本地计划和全局计划）。

• 否则，当地规划可能会拒绝不可行的轨迹（从它的角度来看）

• 但相比之下，全局规划可以进一步认为所选的（全局）计划是可行的：机器人可能会被卡住。