局部路径规划器teb_local_planner详解之初识teb中,我们初步认识了teb算法,知道如何在move_base中实现运行。
本部分我们详细探讨,TEB避障的实现,以及与避障相关的各个参数对算法性能表现的影响。
避障是整个轨迹优化的一部分。优化的目的是寻找代价最小的轨迹解决方案。
teb算法将避障硬约束转换为软约束。举个例子如图所示:
min_obstacle_dist
: 最小避障距离0.2m
weight_obstacle
: 避障在整个优化函数中的权重
extra margin
:给min_obstacle_dist额外加个边
penalty_eposilon
: 一次性修改全部惩罚项。注意,谨慎操作
陷入局部极值的状态:
使用多条轨迹的teb算法:the homotopy class planning algorithm
会尝试沿着障碍物构成的拓扑地图,寻找多条轨迹。避免陷入极值。
dt_ref
:参考轨迹的离散间隔
obstacle_poses_affected
: 因为障碍物而受到影响的poses数量(基于距离障碍物最近的pose,向两边扩展的点数)
只有被affected的pose会被选中拿去做优化。
上图中,也能看到,机器人的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"
机器人模型为点模型,这种模型消耗资源最小
根据给定的参数~/footprint_model/radius
来决定的圆半径模型。
计算碰撞的方法和point差不多,只是将radius加入了参数min_obstacle_dist
非常复杂的模型可以用多边形表示。通过一系列的顶点来构成。
下一章: 局部路径规划器teb_local_planner详解3:跟随全局planner