ROS导航小车1 teb_local_planner参数(仅作记录/收集)

发布时间：2023-04-19 文章分类：电脑百科 投稿人：樱花 字号： 默认 | 大 | 超大 打印

参考以下链接

http://wiki.ros.org/teb_local_planner

http://wiki.ros.org/teb_local_planner/Tutorials/Obstacle%20Avoidance%20and%20Robot%20Footprint%20Model

http://wiki.ros.org/teb_local_planner/Tutorials/Configure%20and%20run%20Robot%20Navigation

http://wiki.ros.org/action/fullsearch/teb_local_planner/Tutorials/Planning%20for%20car-like%20robots?action=fullsearch&context=180&value=linkto%3A%22teb_local_planner%2FTutorials%2FPlanning+for+car-like+robots%22

https://blog.csdn.net/weixin_44917390/article/details/107568507?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166977853616782390571244%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166977853616782390571244&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-6-107568507-null-null.142^v67^wechat_v2,201^v3^control_2,213^v2^t3_control1&utm_term=teb%20local&spm=1018.2226.3001.4187

https://blog.csdn.net/weixin_41349117/article/details/100636681?spm=1001.2101.3001.6661.1&utm_medium=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-100636681-blog-107568507.pc_relevant_default&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-100636681-blog-107568507.pc_relevant_default&utm_relevant_index=1

https://blog.csdn.net/zz123456zzss/article/details/104692548?ops_request_misc=&request_id=&biz_id=102&utm_term=%20TebLocalPlannerROS:%20trajector&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-7-104692548.142^v67^wechat_v2,201^v3^control_2,213^v2^t3_control1&spm=1018.2226.3001.4187

TEB算法简介

“TEB”全称Time Elastic Band（时间弹性带）Local Planner，该方法针对全局路径规划器生成的初始轨迹进行后续修正(modification)，从而优化机器人的运动轨迹，属于局部路径规划。在轨迹优化过程中，该算法拥有多种优化目标，包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动力学、运动学以及几何约束的符合性。“TEB方法”明确考虑了运动状态下时空方面的动态约束，如机器人的速度和加速度是有限制的。”TEB”被表述为一个多目标优化问题，大多数目标都是局部的，只与一小部分参数相关，因为它们只依赖于几个连续的机器人状态。这种局部结构产生了一个稀疏的系统矩阵，使得它可以使用快速高效的优化技术，例如使用开源框架“g2o”来解决“TEB”问题。详细理解参考qqfly文章qqfly
TEB规划器的性能问题的总结参考

参数解析

ros中wiki有详细解释http://wiki.ros.org/teb_local_planner#Parameters
也可参考参数
teb参数配置文件

TebLocalPlannerROS:
 odom_topic: odom
 map_frame: odom
 # Trajectory
 teb_autosize: True
 dt_ref: 0.45
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 3   
 # Robot
 max_vel_x: 0.5
 max_vel_y: 0.5
# max_vel_x: 0.7
 max_vel_x_backwards: 0.2
 max_vel_theta: 0.4
# max_vel_theta: 1.5
 acc_lim_x: 1
 acc_lim_y: 1 #0.25
 acc_lim_theta: 0.80
 min_turning_radius: 0.0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
 #radius: 0.12 # for type "circular"
   type: "polygon"
   vertices: [[-0.18, -0.11], [-0.18, 0.11], [0.18, 0.11], [0.18, -0.11]]
 # GoalTolerance
 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.5
 free_goal_vel: False
# cmd_angle_instead_rotvel: True
 # Obstacles
 min_obstacle_dist: 0.25
 inflation_dist: 0.6
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 7
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5
 # Optimization
 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 1
 weight_max_vel_y: 1
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_y: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1 
 weight_kinematics_forward_drive: 1000
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 selection_alternative_time_cost: False # not in use yet
 # Homotopy Class Planner
 enable_homotopy_class_planning: False
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False
 # Recovery
 shrink_horizon_backup: True
 shrink_horizon_min_duration: 10
 oscillation_recovery: False
 oscillation_v_eps: 0.1
 oscillation_omega_eps: 0.1
 oscillation_recovery_min_duration: 10
 oscillation_filter_duration: 10

Trajectory 参数调整轨迹参考链接

dt_ref 局部路径规划的解析度（默认0.3）

dt_hysteresis 允许改变的时域解析度的浮动范围， 一般为 dt_ref 的 10% 左右（默认0.1）

dt_ref和dt_hysteresis 最优路径上的两个相邻姿态（即位置、速度、航向信息，可通过TEB可视化在rivz中看到）的默认距离
此距离不固定，规划器自动根据速度大小调整这一距离，速度越大，相邻距离自然越大。笔者认为，将dt_ref设置为与车辆长度大致是可行的。较小的值理论上可提供更高精度，但笔者并未进行验证。对于一般的模型车而言，不需要改变默认配置。当相邻姿态距离和dt_ref的差超过正负dt_hysteresis时，规划器将改变这一距离。

global_plan_overwrite_orientation 覆盖全局路径中局部路径点的朝向，有些全局规划者在开始和全局目标之间没有考虑局部子目标的方向，因此自动确定

覆盖全局路径的方向是预留给3D路径规划使用的。对于车辆的2D规划，可以设置为False，可实现对全局路径的更好跟踪

max_global_plan_lookahead_dist 考虑优化的全局计划子集的最大长度（累积欧几里得距离）（如果为0或负数：禁用；长度也受本地Costmap大小的限制），默认3.0

该参数决定局部规划初始轨迹的最大长度，实际调试发现此参数无需过大，因为局部轨迹在每个控制周期都被更新，实际执行的指令仅是轨迹上第一个点的速度值，这里设置为1.5（默认3.0）即可，过长也可能导致优化结果无法有效收敛。
此距离1.应随车辆最大速度的增大而增大 2.不应超过激光雷达等传感器的可靠测量范围 3. 不应超过局部耗费地图的大小，即不能要求TEB对局部耗费地图以外的部分进行规划。

feasibility_check_no_poses 检测位姿可到达的时间间隔

在判断生成的轨迹是否冲突时使用，此时设置为3，即从轨迹起点开始逐个检查轨迹上的3个点，若3个点均不发生碰撞，则认为本次轨迹有效，由于teb优化并非硬约束，这里相当于是轨迹生成之后的一层保障，这个参数因根据机器人的速度和环境复杂程度调整，否则极有可能出现在狭窄环境中走走停停的情况。
在 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。
这个参数根据机器人的速度和环境复杂程度调整，否则极有可能出现在狭窄环境中走走停停的情况，甚至可以减小到1或0
ROS导航小车1 teb_local_planner参数(仅作记录/收集)

robot 参数根据实际配置机器人

ROS下的导航中local planner多数都针对两轮差速结构设计的，阿克曼转向结构由于结构的特点会有最小转向半径的限制，所以常见的local planner例如base_local_planner，dwa_local_planner等在这里都不适用里，这里使用的是teb_local_planner
它提供了一种car-like的动力学模型（即有最小转向半径限制），这样他所规划的路径也就符合了阿克曼结构的运动特点了

min_turning_radius 最小转弯半径（麦轮设为0）

为避免规划出不可能实现的移动路径，请动手测量实际车辆的转弯半径。此参数事实上约束了舵机的最大转角。有些车辆转向性能不佳，前轮实际转过角度小于舵机角度，则应当给指令转角乘上一增益后再控制舵机，否则车辆将总是不能实现设置的最小转弯半径。请注意此项应当与最大角速度配合设置

max_vel_x  acc_lim_x   x轴方向速度与加速度约束

max_vel_y acc_lim_y    y轴方向速度与加速度约束

注意加速度同样约束减速过程。若电机性能良好且速度不快可以立即刹车，可直接将acc_lim_x设置为0，表示没有约束。若电机不能承受阶跃输入或者响应时间很长，则应当设置加速度限制

max_vel_x_backwards 最大倒车速度

将此速度设置为0或者负数将导致错误。禁止倒车应在penalty部分将前向行驶的权重设置得极高。

max_vel_theta  acc_lim_theta   角速度和角加速度约束

正确配置下最小转弯半径应在低速时生效，而角速度限制应作用在车辆高速行驶时。角速度约束可以防止高重心转弯过快翻车，角加速度限制可以避免车轮打滑，车辆失控。对于低速平稳运行的车辆可以不约束此两项，将二者设为一个足够大的值即可。

footprint_model: # types: "point", "circular", "line", "two_circles", "polygon

机器人的 footprint 模型是为了达到优化目的而近似机器人的2D轮廓。它对计算距离的复杂度和时间至关重要，因此机器人 footprint 模型单独构建了一个参数，而没有直接调用 common costmap_2d 参数中的 footprint。优化 footprint 模型可能与costmap footprint 模型不同
1.Point
机器人被建模为一个点，这种类型所需的计算时间最少。
2.Circular
机器人被建模为一个简单的圆，半径为/footprint_model/radius。计算距离和Point类型相似，不同点是每次调用函数时机器人半径会被加入到参数min_obstacle_dist。可以通过选择Point类型，然后将半径加到最小障碍物距离来替换
3. Line
适用于长方形机器人，可以通过参数/footprint_model/line_start和/footprint_model/line_end来配置线段。机器人（旋转轴）被定在[0, 0]点，确保通过参数min_obstacle_dist来包含整个机器人（如下图）：
ROS导航小车1 teb_local_planner参数(仅作记录/收集)
4.Two Circles
还可以通过两个圆来近似机器人轮廓。每个圆由机器人x轴的偏移量和半径来描述。/footprint_model/front_offset， /footprint_model/front_radius， /footprint_model/rear_offset， /footprint_model/rear_radius。Offsets 可能为负值。
ROS导航小车1 teb_local_planner参数(仅作记录/收集)
5. Polygon
可以用多边形表示复杂模型，多边形由定点的列表组成。假设机器人旋转轴位于[0,0]（单位：米）。 请勿重复第一个顶点，因为多边形会自动闭合。
请记住，每个额外的边缘都会显着增加所需的计算时间！ 您可以从costmap公共参数文件中复制多边形 footprint 模型

GoalTolerance 参数设置机器人停止运行的容差

xy_goal_tolerance和yaw_goal_tolerance 目标跟踪误差

根据车辆运行精度设置。例如，笔者使用一台攀爬RC车构建导航系统，这种车辆转向间隙特性很大，则不应设置严格的航向限制

free_goal_vel 自由目标速度

设为False时，车辆到达终点时的目标速度为0。我们已经指出，TEB是时间最优规划器。缺少目标速度约束将导致车辆“全速冲线”，即使前方有一堵墙也是如此（因为撞墙的时刻不在规划器考虑范围内了）。不过此选项在竞速比赛中比较有用

Obstacles 参数用于对环境中障碍物的处理方式，体现在轨迹优化阶段

costmap_converter_plugin  是否使用costmap_converter插件  若设置为空字符，则视为禁用转换

原始costmap中障碍物全部以“点”来表示，计算机器人到障碍物的距离实际需要计算机器人到每一个“障碍物点”的距离，当环境非常复杂时计算代价会非常大。costmap_converter插件的作用是将障碍物预先表示成线段或多边形的形式，可以在一定程度上减轻后续计算距离的压力，具体介绍可见ROS wiki页面：http://wiki.ros.org/costmap_converter或中文介绍https://www.ncnynl.com/archives/201809/2604.html。但同时这种预处理的方法也会耗费资源，需要根据实际环境的情况来判断是否启用。如果使用的话"costmap_converter::CostmapToPolygonsDBSMCCH"是一个较为精确的方法，它将环境转换为非凸多边形；在将障碍物距离加入g2o优化框架中（障碍物距离是目标函数之一，描述为超图的边）
TEB默认情况下不使用Costmap Converter。事实上，此插件可以在复杂场景下极大提高运算效率，尤其是处理激光雷达分散的测量数据时。因为将障碍物视为系列孤立点效率极低

include_costmap_obstacles 必须设置为True后才能规避实时探测到的、建图时不存在的障碍物。

min_obstacle_dist 最小障碍物距离

若设置了车辆的轮廓，则不建议使用此参数。使用车辆轮廓配置footprint_model配合膨胀距离即可实现较好效果。障碍物惩罚权重很高，TEB不会违背这一最小距离约束，因此会把可以通过的缝隙视作不能通过，并终止运行。请注意，此处设置的机器人轮廓必须与局部耗费地图中的一致。
增大min_obstacle_dist可以防止机器人离墙太近，但是对于狭窄通道的情况，反而应该设置的很小。 可以增大机器人轮廓，但减小min_obs_dist，提高防撞性，又保证过窄通道
min_obstacle_dist”参数限制机器人与障碍物的最小距离，实际还配合“obstacle_association_force_inclusion_factor”和“obstacle_association_cutoff_factor”这两个参数生效参考*TebOptimalPlanner::AddEdgesObstacles()*函数中的如下代码：

// iterate obstacles，obst变量只存储障碍物形式的一个单元，例如一个点，一条线，所以这里是迭代环境中的所有障碍物单元
for (const ObstaclePtr &obst : *obstacles_) {
    // we handle dynamic obstacles differently below
    if (cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic())
        continue;
    // calculate distance to robot model
    //! 根据不同的机器人模型（点，圆，多边形等），不同的障碍物模型（点，线，多边形），有不同的距离计算方法
    double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get());
    // force considering obstacle if really close to the current pose
    if (dist < cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_force_inclusion_factor) {
        relevant_obstacles.push_back(obst.get());
        continue;
    }
    // cut-off distance
    if (dist > cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_cutoff_factor)
        continue;
    // determine side (left or right) and assign obstacle if closer than the previous one
    if (cross2d(pose_orient, obst->getCentroid()) > 0) // left
    {
        if (dist < left_min_dist) {
            left_min_dist = dist;
            left_obstacle = obst.get();
        }
    } else {
        if (dist < right_min_dist) {
            right_min_dist = dist;
            right_obstacle = obst.get();
        }
    }
}

距离小于min_obstacle_dist * obstacle_association_force_inclusion_factor值的“障碍物点”，被强制加入优化框架中，距离大于min_obstacle_dist * obstacle_association_cutoff_factor的“障碍物点”被直接抛弃不再考虑，然后在剩余的障碍物点中计算机器人左侧最小距离和右侧最小距离。这三个参数的设置非常重要，需要根据机器人的外形尺寸小心调整，否则极易出现狭窄空间机器人无法通过或优化不收敛的情况。

inflation_dist 障碍物膨胀距离

默认0.0 障碍物膨胀距离。这个值必须大于min_obstacle_dist才有效。源码在AddEdgesObstacles. 此膨胀只是降低通过这些区域的优先级，不应当用此距离迫使车辆远离障碍物。障碍物周边的缓冲区，零惩罚代价，缓冲区会导致规划器减速

costmap_obstacles_behind_robot_dist	double	限制在机器人后面规划时考虑到的占用的本地成本图障碍(指定距离，单位为米)

obstacle_poses_affected	int	障碍物位置与轨迹上最接近的姿态相连，以减少计算量，但同时也考虑了许多相邻的障碍物

ROS导航小车1 teb_local_planner参数(仅作记录/收集)

Optimization 参数主要是设置优化框架中各部分的权重大小参考链接

no_inner_iterations	int	被外循环调用后内循环执行优化次数	1	5	100
no_outer_iterations	int	执行的外循环的优化次数	1	4	100
optimization_activate	bool	激活优化	False	True	True
optimization_verbose	bool	打印优化过程详情	False	False	True
penalty_epsilon	double	对于硬约束近似，在惩罚函数中添加安全范围	0.0	0.1	1.0
weight_max_vel_x	double	最大x速度权重	0.0	2.0	1000.0
weight_max_vel_y	double	最大y速度权重	0.0	2.0	1000.0
weight_max_vel_theta	double	最大加速度权重	0.0	1.0	1000.0
weight_acc_lim_x	double	最大x 加速度权重	0.0	1.0	1000.0
weight_acc_lim_y	double	最大y 加速度权重	0.0	1.0	1000.0
weight_acc_lim_theta	double	最大角速度权重	0.0	1.0	1000.0
weight_kinematics_nh	double	满足非完整运动学的优化权值	0.0	1000.0	10000.0
weight_kinematics_forward_drive	double	优化过程中，迫使机器人只选择前进方向，差速轮适用	0.0	1.0	1000.0
weight_kinematics_turning_radius	double	优化过程中，车型机器人的最小转弯半径的权重	0.0	1.0	1000.0
weight_optimaltime	double	优化过程中，基于轨迹的时间上的权重	0.0	1.0	1000.0
weight_obstacle	double	优化过程中，和障碍物最小距离的权重	0.0	50.0	1000.0

no_inner_iterations: 5 图优化optimizer的迭代次数

no_outer_iterations: 4 每次外部循环迭代都会根据所需的时间分辨率dt_ref自动调整轨迹的大小，并调用内部优化器

penalty_epsilon 0.1 一次性改变所有的惩罚项，为惩罚函数增加一个小的安全余量，以实现硬约束近似. 例如为速度的约束提供一个缓冲的效果，在到达速度限制前会产生一定的惩罚，让其提前减速达到缓冲的效果

ROS导航小车1 teb_local_planner参数(仅作记录/收集)
ROS导航小车1 teb_local_planner参数(仅作记录/收集)
如果选择过高的值，会影响最终的信息矩阵，可能会出现不收敛的情况

weight_obstacle 默认值：50.0. 优化权重以保持与障碍物的最小距离。可以增大至几百，让机器人提前转向，避免卡死

obstacle_cost_exponent 无默认值，目前设置为0.65. 源码在EdgeObstacle::computeError和EdgeInflatedObstacle::computeError，判断是否为1，同时min_obstacle_dist > 0才有效。用于更新g2o的误差函数

weight_kinematics_nh 默认值：1000. 用于满足非完整运动学的优化权重. 该参数必须很高，因为运动学方程构成了一个等式约束，即使值1000也不意味着由于与其他成本相比较小的“原始”成本值而导致的矩阵条件不佳

weight_kinematics_nh”参数应设置较小值，因为我们是完整约束机器人无需限制其运动学约束。

weight_kinematics_forward_drive 默认值：1，改为1000。

迫使机器人仅选择前进方向 (正向速度) 。较小的权重，仍然会允许向后行驶。我们无法限制优化器的搜索空间为正的速度，因为优化器不支持硬约束，只能大幅提高weight_kinematics_forward_drive以避免后退速度，但是值太大又会减小收敛速度。不管权重多高，无法规划出前进的路径时，还是可能出现停滞或者倒车。

weight_optimaltime 最优时间权重

提高至5时，车辆在直道上线速度明显加快，而且越大越能接近max_vel_x，并靠近路径边缘沿切线过弯。 Optimization weight for contracting the trajectory w.r.t transition time

weight_inflation 默认0.1， Optimization weight for the inflation penalty，应当很小，源码在AddEdgesObstacles的信息矩阵里

Homotopy Class Planner 参数与HomotopyClass规划器相关 并行规划

此功能将严重影响规划器性能。当路径中存在大量不连续分布的障碍物（如锥桶）并产生大量可行路径时，此功能有良好的效果。相反，若在单一路径上运行，则没有必要时用此功能。
当enable_homotopy_class_planning = False，规划的局部路径会陷入局部最小值。因为路径向上面障碍物或下面障碍物偏，总体的代价值都会增加（时间成本），只有往中间走总体代价值才最低。但此时路径invalid，中间缺少路径点，此时机器人卡死了。
ROS导航小车1 teb_local_planner参数(仅作记录/收集)
ROS导航小车1 teb_local_planner参数(仅作记录/收集)
设置True，teb会同时搜寻多条路径并选取一条更可行的，feasibility check环节会抛弃不可行的路径

标签 , ,