teb_local_planner/Tutorials
teb_local_planner/Tutorials
4. Obstacle Avoidance and Robot Footprint Model
4.1 How Obstacle Avoidance works
4.1.1 Penalty terms(惩罚项)
避障是全部轨迹优化(trajectory optimization)的一部分。如果得到的轨迹碰到了障碍物，就会得到一个大的代价值。理想情况下，这种情况的代价应该无限大，但这需要优化器处理硬约束（hard constraints，解决非线性）。为了提高效率，teb_local_planner没有考虑硬约束，将硬约束转为软约束，得到具有有限代价值的二次惩罚项组合。
上图展示了避障惩罚项。允许到障碍物的最小欧式距离（Euclidean distance）被设为0.2米（参数min_obstacle_dist）。因此，小于0.2米的距离会得到非0的cost。想象一下，优化问题由更多的代价项组成。其中有些是冲突的，比如时间最优time-optimality。因此，优化器（optimizer ）可能会加入一个小的惩罚项，以便最小化全局的最优代价函数。这里有两个选项可以调整机器人的行为：
- 调整优化权重（每个独立的代价的系数，这里参数是
weight_obstacle）。但如果选择过高的值，可能会出现不收敛的情况 - 通过添加“额外边距extra margin”来调整参数。通过添加一个小的额外距离
min_obstacle_dist参数可以在0.2米出隐含的提高代价。也可以用一个参数penalty_epsilon一次性改变所有的惩罚项，但是要小心，这样做可能会严重影响优化结果
4.1.2 局部优化轨迹 Locally optimal solutions
注意，optimizer本身只能找到局部最优轨迹。如下图，机器人可能被两个障碍物挡住，惩罚项确实不为0，但是optimizer陷入了这个局部最小值，因为把轨迹移到障碍物的另一边将增加总的cost。可以使用test_optim_node来尝试这种情况（参考教程Set up and test Optimization并关闭homotopy class planning）。情况应该和下图一样。
轨迹跳不出障碍物，即便poses本身被挤到了障碍物之间的范围外（障碍物之间没有红色箭头）。显然，实际情况下应该避免这种情况，因此，homotopy class planning算法被用来寻找替代轨迹，然后可行性检查（feasibility check，见下一个部分）会在给机器人发送指令前抛弃这一轨迹。
4.1.3 关联Poses和Obstacles
下图展示了一个常见的规划场景：
场景中有一个移动机器人在向当前的目标移动时接近了一个多边形障碍物。规划的离散轨迹由多个robot poses组成（圆点）。planner的目标是根据期望的时间分辨率（temporal resolution, 参数dt_ref）决定两个连续的poses。注意，实际分辨率不是固定/冻结的，因为优化器需要调整转换时间（transition time）以寻求时间最优性。
图中示例轨迹由8个可变的poses组成（起点和目标点固定）。为了不碰到障碍物，需要多次计算距离（optimizer多次计算cost）。为了提高速度，这是实现了一个专用关联策略（dedicated association strategy）。
对于每一个障碍物（点、线、多边形），距其最近的poses都会被定位出来（上图中的红色点）。参数obstacle_poses_affected个相邻的最近poses都会被考虑进来（上图中是3）。在随后的优化步骤中仅考虑该选定的poses子集（这里是3个poses，因此是3个惩罚项）。在每个外部优化迭代中，这个关联操作会被重复no_inner_iterations次。参数obstacle_poses_affected的值略微影响障碍物周围轨迹的平滑度。更大的障碍需要更多的连接poses以避免不允许的捷径。 还可以选择较高的值（大于轨迹长度），以便将所有poses与每个障碍物连接起来。
注意，机器人 footprint 模型在计算距离时也会被考虑在内，跟计算资源相关，详细会在下一部分介绍。
4.2 Robot Footprint Model
机器人的 footprint 模型是为了达到优化目的而近似机器人的2D轮廓。它对计算距离的复杂度和时间至关重要，因此机器人 footprint 模型单独构建了一个参数，而没有直接调用 common costmap_2d 参数中的 footprint。优化 footprint 模型可能与costmap footprint 模型不同（后者用于可行性检查，请参阅下面的部分）。
footprint 模型使用 parameter server 配置，可以像下面这样在 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"
默认的 footprint 是”point“
注意，footprint 被发布到 ~ 可以通过rviz查看。下面介绍了所有不同类型的footprint。
4.2.1 Point
机器人被建模为一个点，这种类型所需的计算时间最少。
4.2.2 Circular
机器人被建模为一个简单的圆，半径为/footprint_model/radius。计算距离和Point类型相似，不同点是每次调用函数时机器人半径会被加入到参数min_obstacle_dist。可以通过选择Point类型，然后将半径加到最小障碍物距离来替换。
4.2.3 Line
适用于长方形机器人，可以通过参数/footprint_model/line_start和/footprint_model/line_end来配置线段。机器人（旋转轴）被定在[0, 0]点，确保通过参数min_obstacle_dist来包含整个机器人（如下图）：
4.2.4 Two Circles
还可以通过两个圆来近似机器人轮廓。每个圆由机器人x轴的偏移量和半径来描述。/footprint_model/front_offset， /footprint_model/front_radius， /footprint_model/rear_offset， /footprint_model/rear_radius。Offsets 可能为负值。
每个相连的poses需要计算两个距离。
4.2.5 Polygon
可以用多边形表示复杂模型，多边形由定点的列表组成。假设机器人旋转轴位于[0,0]（单位：米）。 请勿重复第一个顶点，因为多边形会自动闭合。
请记住，每个额外的边缘都会显着增加所需的计算时间！ 您可以从costmap公共参数文件中复制多边形 footprint 模型。
4.3 可行性检查 Feasibility Check
在 optimizer 返回一个轨迹后，并将速度指令发送给机器人之前，会运行一个可行性检查。目的是判定optimizer产生的无效/不可行的轨迹（软约束 ）。
现在，算法迭代从当前机器人pose开始的前 n 个 poses（n = ~/feasibility_check_no_poses），检查这些poses有无碰撞。为了检测碰撞的发生，使用了costmap footprint。 所以，该验证模型可能比之前优化的footprint更加复杂。
~/feasibility_check_no_poses不能太大，因为optimizer可能不会完全收敛：形象地说，small obstacle violations in the (far) future could be corrected while the robot is moving towards the goal.
4.4 示例
You can find an example setup with the stage simulator in the teb_local_planner_tutorials package.
5 Following the Global Plan (Via-Points， 穿过点沿着全局路径移动)
本教程将会介绍如何让local planner更加紧密地沿着全局路径移动。尤其是如何调整时间最优（time-optimality）和路径跟随紧密度（path-following）之间的权衡。
5.1 Optimization with Via-Points
与之前的教程类似，我们首先研究优化器optimizer在存在过程点（via-points）的情况下的行为方式。 启动测试节点：
roslaunch teb_local_planner test_optim_node.launch
为了方便地调整参数（默认情况下，via-points被禁用），启动：
rosrun rqt_reconfigure rqt_reconfigure
现在，将参数global_plan_viapoint_sep设为一个正值（例如1.0），正值会激活via-points。这里的实际值没有任何意义，但它对于机器人导航非常重要，并在下一节中进行了解释。
在rviz上点击Publish Point按钮在起点和目标点之间添加一个via-point（在网格上随意选择一个位置，如下入所示，实测点必须在网格的交线上，此时鼠标右边出现一个小图标，其他地方不能加入这个点）
您可能会注意到，轨迹确实被viz-point吸引，但它没有达到它。 主要原因是优化器不仅试图找到时间最短轨迹（time-optimality，时间最优性）同时最小化到via-point的距离。 注意，只要via-point不直接位于最小时间轨迹上，我们就有两个相互冲突的目标。 由此产生的全局最小值变成是两个目标之间的权衡。利用代价函数中的附加权重，用户现在可以设置期望的偏好，使得全局最小值被改变。
最小化via-point距离的权重的参数是weight_viapoint，默认值是1.0，下图是10.0的结果：
现在你可以添加更多点，并观察调整weight_viapoint会发生什么。
注意：避障（与障碍物保持一个最小距离）可能也会与最小化viz-points距离相冲突，所以weight_viapoint不能太大，否则会导致obstacles costs被否定或忽略不计。
planner甚至适用于更加复杂的场景，例如在用最短时间移向目标点的过程中经过一系列随意摆放的via-points（如下图）。当然点的数量是有限的。
注意，如果轨迹上的最近点在起点或者目标点，该viz-point就会被忽略（当前版本）。
5.2 Planning in Alternative Topologies
完成之前的部分你会发现homotopy class planning可能还是被激活的。可以使用参数viapoints_all_candidates选择两种不同的策略：
- 如果
viapoints_all_candidates为true，则所有生成的候选轨迹与该组via-points相关联，并且每个相应的优化器尝试最小化到它们的距离。 - 如果设为false，只有数据全局计划拓扑的候选轨迹才会被优化通过，其他的会被忽略（不经过某些via-points）。
注意，由于测试节点没有global planner，这里的选项1不依赖与特定的参数值
有时用户可能会想让选择的候选轨迹更靠近via-points，而不是时间更短（独立于优化权重weight_viapoint），铜鼓增加参数selection_viapoint_cost_scale，via-points代价函数的权重被进一步缩放，但这是用于选择候选集的轨迹，而不是优化。
5.3 Navigation: Follow the Global Plan
下面几步假设配置好了teb
关闭其他节点，启动差速轮机器人示例：
roslaunch teb_local_planner_tutorials robot_diff_drive_in_stage.launch
并启动rqt_reconfigure
根据前面的教程，将参数global_plan_via_point_sep调至正值来激活via-points（0.5）。在rviz中设置一个目标点观察机器人如何移动。
global_plan_via_point_sep定义了两个连续via-points之间的最小间隔（单位：米）。例如设置为0.5，则每0.5米就会从global planner选择一个新的via-point（只要global planner的分辨率足够）。因此，通过调整该值，可以指定是否应将粗略或精细参考路径考虑在路径跟踪中。
注意，太精细的分辨率和大的优化权重可能会影响（动态/出现）障碍物的避障行为（因为在这种情况下障碍物成本可以忽略不计）。
下图显示了path-following mode的场景：
还可以测试car-like机器人：
roslaunch teb_local_planner_tutorials robot_carlike_in_stage.launch