0


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

参考以下链接

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规划器的性能问题的总结参考

  • 关闭多路径并行规划(效果非常显著)
  • 使用Costmap Converter (非常显著)
  • 降低迭代次数(no_inner/outer_iterations) (显著)
  • 降低 max_lookahead_distance (一般)
  • 减小局部耗费地图的大小 (显著)
  • 增大规划周期和控制周期 (影响效果)
  • 使用单点footprint,配合最小障碍物距离约束 (不太显著且影响效果)

参数解析

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
ob

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来包含整个机器人(如下图):
line
4.Two Circles
还可以通过两个圆来近似机器人轮廓。每个圆由机器人x轴的偏移量和半径来描述。/footprint_model/front_offset, /footprint_model/front_radius, /footprint_model/rear_offset, /footprint_model/rear_radius。Offsets 可能为负值。
two_circles
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 belowif(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 poseif(dist < cfg_->obstacles.min_obstacle_dist * cfg_->obstacles.obstacle_association_force_inclusion_factor){
        relevant_obstacles.push_back(obst.get());continue;}// cut-off distanceif(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 oneif(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    障碍物位置与轨迹上最接近的姿态相连,以减少计算量,但同时也考虑了许多相邻的障碍物

ob

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

no_inner_iterations    int    被外循环调用后内循环执行优化次数    15100
no_outer_iterations    int    执行的外循环的优化次数    14100
optimization_activate    bool    激活优化    False    True    True
optimization_verbose    bool    打印优化过程详情    False    False    True
penalty_epsilon    double    对于硬约束近似,在惩罚函数中添加安全范围    0.00.11.0
weight_max_vel_x    double    最大x速度权重    0.02.01000.0
weight_max_vel_y    double    最大y速度权重    0.02.01000.0
weight_max_vel_theta    double    最大加速度权重    0.01.01000.0
weight_acc_lim_x    double    最大x 加速度权重    0.01.01000.0
weight_acc_lim_y    double    最大y 加速度权重    0.01.01000.0
weight_acc_lim_theta    double    最大角速度权重    0.01.01000.0
weight_kinematics_nh    double    满足非完整运动学的优化权值    0.01000.010000.0
weight_kinematics_forward_drive    double    优化过程中,迫使机器人只选择前进方向,差速轮适用    0.01.01000.0
weight_kinematics_turning_radius    double    优化过程中,车型机器人的最小转弯半径的权重    0.01.01000.0
weight_optimaltime    double    优化过程中,基于轨迹的时间上的权重    0.01.01000.0
weight_obstacle    double    优化过程中,和障碍物最小距离的权重    0.050.01000.0
no_inner_iterations:5 图优化optimizer的迭代次数
no_outer_iterations:4 每次外部循环迭代都会根据所需的时间分辨率dt_ref自动调整轨迹的大小,并调用内部优化器
penalty_epsilon 0.1 一次性改变所有的惩罚项,为惩罚函数增加一个小的安全余量,以实现硬约束近似. 例如为速度的约束提供一个缓冲的效果,在到达速度限制前会产生一定的惩罚,让其提前减速达到缓冲的效果

1
2
如果选择过高的值,会影响最终的信息矩阵,可能会出现不收敛的情况

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,中间缺少路径点,此时机器人卡死了。
1
2
设置True,teb会同时搜寻多条路径并选取一条更可行的,feasibility check环节会抛弃不可行的路径

  • enable_multithreading (bool,默认:true) 激活多个线程以计划不同线程中的每个轨迹
  • max_number_classes (int,默认值:4,设置为2) 指定要考虑的最大不同轨迹数(限制计算工作量),影响CPU
  • selection_cost_hysteresis (double,默认值:1.0) 指定新候选人必须具有多少轨迹成本才能选择之前的轨迹 (如果new_cost < old_cost * factor,则进行选择
  • selection_prefer_initial_plan 默认0.95 Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan
  • selection_obst_cost_scale (double,默认值:100.0) 仅选择“最佳”候选者的障碍物成本术语的额外缩放
  • selection_viapoint_cost_scale (double,默认值:1.0) 仅为了选择“最佳”候选者而对通孔成本条款进行额外的缩放
  • selection_alternative_time_cost (bool,默认值:false) 如果为true,则将时间成本 (时间差平方的总和) 替换为总过渡时间 (时间差之和)
  • simple_exploration 默认false. 如果为true,不同的 trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal.
  • roadmap_graph_no_samples (int,默认值:15) 指定为创建roadmap graph而生成的样本数,前提是simple_exploration为false
  • roadmap_graph_area_width (double,默认值:6) 在起点和目标之间的矩形区域中采样随机关键点/航路点。以米为单位指定该区域的宽度
  • delete_detours_backwards 默认true. planner will discard the plans detouring backwards with respect to the best plan
  • h_signature_prescaler (double,默认值:1.0) 标度内部参数 (H-signature) ,用于区分同伦类。警告:仅当您在本地成本图中观察到障碍物过多的问题时,才减小此参数,请勿将其选得太低,否则障碍物无法彼此区分 (0.2 <值 <= 1)
  • h_signature_threshold (double,默认值:0.1) 如果实部和复杂部的差都低于指定的阈值,则假定两个H签名相等
  • obstacle_heading_threshold (double,默认值:1.0) 在障碍物航向和目标航向之间指定标量乘积的值,以便将障碍物)考虑在内进行探索
  • visualize_hc_graph (bool,默认:false) 如果为true,在rviz里可看到优化使用的graph
  • viapoints_all_candidates (bool,默认:true) 如果为true,则将不同拓扑的所有轨迹附加到该组通孔点,否则,仅将与初始/全局计划共享相同拓扑的轨迹与它们连接 (对test_optim_node无效)
  • switching_blocking_period (double,默认值:0.0) 指定允许切换到新的等效类之前需要终止的持续时间 (以秒为单位)

本文转载自: https://blog.csdn.net/weixin_50371003/article/details/128118591
版权归原作者 Crush Mome 所有, 如有侵权,请联系我们删除。

“ROS导航小车1 teb_local_planner参数(仅作记录/收集)”的评论:

还没有评论