TEB轨迹规划算法教程-全局航点导航

TEB轨迹规划算法教程-全局航点导航

说明：

• 介绍如何配置本地计划程序以更严格地遵循全局计划。
• 特别是，您将学习如何调整时间最优性和路径跟踪之间的权衡

使用Via-Points进行优化

• 与之前的教程类似，我们首先研究优化器在存在点的情况下的行为。
• 为此，我们启动测试节点：

roslaunch teb_local_planner test_optim_node.launch

• 新终端，执行

rosrun rqt_reconfigure rqt_reconfigure

• 为了方便调整参数（默认情况下，通过点被禁用）。

• 现在将参数global_plan_viapoint_sep的值设置为正值（例如1.0）。

• 正值激活了对点的考虑。 这里的实际值没有任何意义，但它对于机器人导航非常重要，并在下一节中进行了解释。

• 将您的rviz窗口置于顶部，并使用菜单中的Publish Point按钮在开始和目标之间添加一个via-point（参见下图）。

• 您需要选择一个对象才能添加一个点，但您只需在网格上选择一个任意点即可。

• 结果应类似于以下示例：

• 您可能会注意到，轨迹确实被通过点吸引，但它没有达到它。
• 主要原因是优化器都试图找到最小时间轨迹（时间最优性）并同时最小化到通孔点的距离。
• 注意，只要via-point不直接位于最小时间轨迹上，我们就有两个相互矛盾的目标。
• 由此产生的全局小值最终成为两个目标之间的权衡。
• 利用成本函数中的附加权重，用户现在可以设置期望的偏好，使得全局最小值被移位。
• 下图显示了权重10.0的结果：
  

• 现在，您可以尝试添加更多的via-points，并观察调整weight_viapoint时发生的情况。
• 注意，避障（保持与障碍物的最小距离）也可能与最小化到通孔点的距离相冲突。
• 不要选择太大的weight_viapoint值，以使障碍成本“被否定”/“可忽略不计”。
• 规划器甚至适用于更复杂的场景，例如： 在访问一组任意位置的通过点时，在最短时间内达到目标（见下图）。
• 当然它们是有限的，只需使用测试节点和一些参数/障碍物/通过点配置来感受通过点的规划。

• 注意，如果轨迹上的最近点是w.r.t. 通过点是开始或目标姿势，然后忽略通过点（在当前版本中）。
• 因此，在此方案中，不考虑在开始之前或目标之后添加的点数。
• 这不会影响导航行为，因为在这种情况下根据全局计划初始化轨迹。

可变拓扑中的规划

• 完成上一节时，您可能会注意到仍然可以启用并行类计划。

• 根据参数viapoints_all_candidates的选择，可以使用两种不同的策略：

  • 如果将viapoints_all_candidates设置为true，则所有生成的候选轨迹与该组via-points相关联，并且每个相应的优化器尝试最小化到它们的距离。
  • 如果该值设置为false，则仅优化属于全局计划的拓扑（等价类）的候选轨迹w.r.t. 通过点。 所有其他人都忽略了它们。

• 请注意，由于测试节点不包含全局参考计划，因此此处的选项1处于活动状态，与特定参数值无关。

• 有时，用户可能希望更喜欢用于选择的候选轨迹（在存在多个替代解决方案的情况下），其更靠近经点而不是具有更短的过渡时间（独立于优化权重）。

• 通过增加参数selection_viapoint_cost_scale，进一步缩放经度点成本函数（weight_viapoint）的权重，但是用于选择候选集的轨迹而不是优化。

• 您可以通过在开始和目标之间添加一个通过点并通过进一步移动中间的障碍物来生成两个不同的轨迹来尝试它。

• 通过更改selection_viapoint_cost_scale，您应该注意到所选轨迹（红色箭头）会切换。

导航：遵循全局规划

• 以下步骤假定您使用的是teb_local_planner_tutorials包提供的导航设置。
• 但是，如果您已经配置了teb_local_planner，则还可以使用自己的机器人导航设置测试所有内容。
• 关闭所有先前的节点并启动差分驱动机器人示例：

roslaunch teb_local_planner_tutorials robot_diff_drive_in_stage.launch 

• 在新的终端启动rqt_reconfigure：

rosrun rqt_reconfigure rqt_reconfigure

• 根据上面的优化示例，通过将参数global_plan_via_point_sep设置为正值（从0.5开始）来启用via-points。
• 现在驾驶机器人（通过设置rviz中的新目标）并观察正在发生的事情。
• 参数global_plan_via_point_sep的实际值定义了沿全局计划的两个连续通过点之间的最小间隔（以米为单位）。
• 例如。 通过将值设置为0.5，每0.5米从全局计划中选择一个新的通过点（只要全局计划的分辨率足够）。
• 因此，通过调整值，您可以指定是否应将粗略或精细参考路径考虑在路径跟踪中。
• 现在在rviz中设置不同的导航目标，并使用global_plan_via_point_sep和weight_viapoint的不同值进行播放。
• 注意，太精细的分辨率和大的优化权重可能会影响（动态/出现）障碍物的避障行为（因为在这种情况下障碍物成本可以忽略不计）。
• 下图描绘了一个屏幕截图，显示了路径跟踪模式（w.r.t.全局计划）：

• 您可以使用类似汽车的机器人模型重复此部分：

roslaunch teb_local_planner_tutorials robot_carlike_in_stage.launch