teb_local_planner/Tutorials

2024-02-06 08:50:46

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 被发布到 ~<name>/teb_markers 可以通过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.

码农公寓