teb_local_planner避障和机器人足迹模型

teb_local_planner 避障和机器人足迹模型

描述：此文档为官方教程翻译文档，在本教程中，您将学习如何实现避障。描述了主要关注机器人足迹模型和它的影响的必要参数设置。

关键词：轨迹优化 局部规划器 导航 障碍 足迹

教程等级：中级

这个教程针对版本大于v0.3的teb_local_planner包

1.避障如何工作

1.1 惩罚项

避障是实现整体轨迹优化的一部分。显然，优化涉及找到指定代价函数（目标函数）的最小代价解（轨迹）。简单的说：如果一个规划的（未来的）位姿违反了与障碍物的预期距离，那么代价函数的代价就必须增加。理想情况下，代价函数值必须为无穷大，否则优化器可能会完全拒绝这些区域。然而，这需要优化器处理硬约束（依次地解决非线性程序）。考虑到更好的效率，teb_local_planner放弃了处理硬约束的能力。硬约束被转化为软约束，组成导致有限代价的二次惩罚项。

上图展示了一个惩罚项的示例（用于避障）。允许到障碍物的最小欧氏距离（参数为min_obstacle_dist）设置为0.2米。因此，小于0.2米的距离导致非零代价。现在假设优化问题包含更多的代价项。其中一些是相互矛盾的，例如时间最优性。因此，优化器可能为了最小的整体代价函数组成考虑一个小的违规（也就是小的惩罚）。这里有两个选项来调优行为：

1. 调整优化权重（单个代价的缩放，这里是参数weight_obstacle）。但是，如果选择的值太高，优化问题就会变得病态，从而导致较差的收敛行为。
2. 通过添加“额外边际”来改变论点。通过向参数min_obstacle_dist添加一个小的额外的边距隐式地增加代价值到0.2m。可以使用单个参数 penalty_epsilon一次性移动所有惩罚项，但是要小心，因为这样做将极大地影响优化结果。

1.2 局部最优解

注意，优化器只能找到局部最优解。想象一下，机器人可能会被两个障碍横向包围。惩罚项确实不为零，但是优化器被卡住了(到这个局部最小值)，因为当前位姿不管向一个哪个障碍横向移动都会进一步增加总的代价。可以使用test_optim_node程序轻松地进行尝试(参见教程：设置和测试优化，并关闭 homotopy class planning)。行为应该类似下图所示:

轨迹不能跳过障碍物。甚至位姿本身也被推离障碍物之间的区域(红色箭头)。显然，在实践中应该避免这种情况。因此，homotopy class planning算法寻找(拓扑上的)替代方案和可行性检查(见下节)在实际指挥机器人之前拒绝这样的方案。

1.3 位姿和障碍之间关联

下图是一个常见规划场景的快照:

该场景由一个移动机器人组成，它正在接近一个多边形形状的障碍，同时向当前目标移动。规划的(离散)轨迹由多个机器人位姿组成。规划器的目标是根据所需的时间分辨率(参数dt_ref)排列每两个连续的位置。注意，实际的分辨率不是固定的，因为优化器需要调整转换时间以寻求时间最优。

为了避开障碍物，一个计划的位姿和障碍物之间的距离必须从下方限定。图中的示例轨迹由8个可变的位姿组成(起始和目标的姿态是固定的)。您可能会同意，为了实现无碰撞轨迹，需要进行许多距离计算(优化器多次调用代价函数值的计算)。为了加快优化速度，实现了专用的关联策略。

在规划的路径中每个离障碍(点/已占用代价地图单元格、线、多边形)最近的位姿被定位(见上图)。根据参数 obstacle_poses_affected的值， 最近位姿的相邻位姿也被考虑在内。在接下来的优化步骤中，只考虑这个选定的位姿子集(这里有3个姿势，因此有3个惩罚项)。根据no_inner_iterations(参数)，关联过程在每个外部优化迭代中分别重复执行。obstacle_poses_affected的值会轻微影响绕过障碍物的轨迹的平滑度。此外，更大的障碍需要更多连接的位姿，以避免不可接受的捷径。你也可以选择一个较高的值(>轨迹长度)，以连接所有位姿与每个障碍。

注意，机器人足迹模型考虑了距离计算，因此对所需的计算资源至关重要。详细信息将在下一节中总结。

2. 机器人足迹模型

机器人足迹模型近似于机器人的二维轮廓以进行优化。该模型对于距离计算的复杂性和计算时间是至关重要的。因此，机器人足迹模型构成一个专用参数，而不是从公共的costmap_2d参数中加载足迹。优化足迹模型可能与代价地图足迹模型不同(后者用于可行性检查，请参见下文)。

使用参数服务器选择和配置足迹模型。您可以将以下参数结构添加到您的 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] 位于后轴（旋转轴）！

以下段落描述了所有不同的类型：

2.1 足迹类型：Point 点

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

2.2 足迹类型：Circular 圆形

机器人被建模为一个具有给定半径的圆圈 ~/footprint_model/radius 。距离计算类似于单点机器人，但例外的是在每个函数调用中将机器人的半径添加到参数 min_obstacle_dist中一起计算。你可以通过选择点模型机器人并将机器人半径直接加到min_obstacle_dist来摆脱这种额外的添加。

2.3 足迹类型：Line 线

线机器人对于横竖长度不同的机器人很有用。 可以使用参数_{/footprint_model/line_start和}/footprint_model/line_end（ [x,y] 坐标）来配置线（段）。假设机器人（旋转轴）位于 [0,0]（单位：米）。 通过进一步调整参数min_obstacle_dist确保封装完整的机器人（参见以下示例）。

你也可以看看“两个圆圈”模型

2.4 足迹模型：Two Circles 两个圆圈

近似机器人轮廓的另一种可能性包括定义两个圆。每个圆由沿机器人 x 轴的偏移量和半径描述：_{/footprint_model/front_offset、}/footprint_model/front_radius、_{/footprint_model/rear_offset和}/footprint_model/rear_radius。偏移量可能是负数。

参考下图作为示例：

对于每个相关的机器人位姿，需要进行两次距离计算。

2.5 足迹类型： Polygon 多边形

可以通过定义一个封闭的多边形来合并一个复杂的模型。多边形是根据顶点列表定义的（为每个顶点提供 x 和 y 坐标）。假设机器人的旋转轴位于 [0,0]（单位：米）。请不要重复第一个顶点，因为多边形会自动闭合。

请记住，每增加一条边都会显着增加所需的计算时间！你可以从你的 costmap 公共参数文件中复制你的足迹模型。

3. 可行性检查

在优化器返回轨迹之后和发送速度命令到机器人之前执行可行性检查。此检查的目的是识别优化器可能产生的无效/不可行轨迹（请记住：软约束、局部最小值等）。

目前，该算法从当前机器人位姿开始迭代前 n 个位姿（n = ~/feasibility_check_no_poses （参数！））并检查这些位姿是否无碰撞。为了检测是否发生碰撞，这里使用costmap的足迹参数（参见导航教程 ！因此，此验证模型可能比用于优化的足迹更复杂（见上文）。

~/feasibility_check_no_poses的值不应该选得太高，因为优化器可能不会完全收敛：形象地说，未来的小障碍物违规可能在机器人向目标移动时进行纠正。

如果您在狭窄的环境中运行，请确保正确配置避障行为（本地规划器和全局规划器）。否则，局部规划器可能会拒绝一条不可行的轨迹（从它的角度来看），但全局规划器可能会认为所选（全局）规划是可行的：所以机器人可能会被卡住。

4. 示例设置

你可以在teb_local_planner_tutorials包 中找到使用模拟器的示例设置。

1. 安装teb_local_planner_tutorials包
2. 检查并启动 carlike 配置