teb详解系列
前言

局部路径规划器teb_local_planner详解之初识teb中，我们初步认识了teb算法，知道如何在move_base中实现运行。

本部分我们详细探讨，TEB避障的实现，以及与避障相关的各个参数对算法性能表现的影响。

一、 避障

1. 避障约束
避障是整个轨迹优化的一部分。优化的目的是寻找代价最小的轨迹解决方案。

teb算法将避障硬约束转换为软约束。举个例子如图所示：

上图中与避障相关的参数：

min_obstacle_dist： 最小避障距离0.2m
weight_obstacle: 避障在整个优化函数中的权重
extra margin：给min_obstacle_dist额外加个边
penalty_eposilon: 一次性修改全部惩罚项。注意，谨慎操作

2. 局部极值
陷入局部极值的状态：

使用多条轨迹的teb算法：the homotopy class planning algorithm会尝试沿着障碍物构成的拓扑地图，寻找多条轨迹。避免陷入极值。

3. 避障与跟随全局pose的关系

重要的参数：

dt_ref:参考轨迹的离散间隔
obstacle_poses_affected: 因为障碍物而受到影响的poses数量（基于距离障碍物最近的pose，向两边扩展的点数）

只有被affected的pose会被选中拿去做优化。

上图中，也能看到，机器人的footprint模型在计算障碍物距离的时候影响很大。

二、机器人footprint模型

模型对碰撞检测的计算很重要。设好了能极大降低运算量。

这个参数是teb自己定的，不是直接从costmap_2d中导入进来的。

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"

1. point
机器人模型为点模型，这种模型消耗资源最小

2. circular
根据给定的参数~/footprint_model/radius来决定的圆半径模型。

计算碰撞的方法和point差不多，只是将radius加入了参数min_obstacle_dist

3. line

4. two circles

5. polygon

非常复杂的模型可以用多边形表示。通过一系列的顶点来构成。