局部代价地图更新频率应当不低于planner和costmap_conveter的转换频率

话题

• global_plan (nav_msgs/Path)

• local_plan (nav_msgs/Path) 局部路径，主要是可视化目的

• teb_poses (geometry_msgs/PoseArray) 当前本地计划的离散姿态列表(SE2)，主要用于可视化目的， 个数是 teb.sizePoses() ，每个位姿点其实就是teb.Pose(i)

这两个话题本质都是一样的，发布在TebVisualization::publishLocalPlanAndPoses

• teb_markers (visualization_msgs/Marker) teb_local_planner通过具有不同名称空间的标记来提供规划场景的其他信息。 Namespaces PointObstacles and PolyObstacles: visualize all point and polygon obstacles that are currently considered during optimization. Namespace TebContainer: Visualize all found and optimized trajectories that rest in alternative topologies (only if parallel planning is enabled). Some more information is published such as the optimization footprint model

话题teb_markers_array 和 eb_markers是绑定的，不过不必关注。

话题的发布在TebVisualization::publishRobotFootprintModel

• teb_feedback (teb_local_planner/FeedbackMsg)
  反馈消息包含计划的轨迹，包括速度轮廓和时间信息以及障碍物列表。主要用于评估和调试。必须启用参数publish_feedback

话题的发布在TebVisualization::publishFeedbackMessage

参数

某些参数不会加载到参数服务器，需要用户在yaml里设置，这是源码决定的

速度

• max_vel_x: 1.0

• max_vel_x_backwards: 0.5 最大倒车速度。将此速度设置为0或者负数将导致错误。禁止倒车应在penalty部分将前向行驶的权重设置得极高。 参阅优化参数weight_kinematics_forward_drive

• acc_lim_x: 0.5 最大线加速度，m²/s

• max_vel_theta: 1 最大转向角速度，作用在车辆高速行驶时，防止高重心转弯过快翻车

• acc_lim_theta: 0.3 角加速度限制可以避免车轮打滑，车辆失控。对于低速平稳运行的车辆可以不约束此两项，将二者设为一个足够大的值即可

• max_vel_y: 0.2

• acc_lim_y: 0.5

以上几个最大值约束会用于saturateVelocity函数

以下参数仅适用于汽车型机器人

• min_turning_radius: 0.0 最小转弯半径，对于差动机器人为0，代表可以原地转弯。源码在TebLocalPlannerROS::convertTransRotVelToSteeringAngle， 转弯半径应在低速时生效. 把这个值稍微调大一点，可以防止车采用过分的差速转向策略，而导致速度非常缓慢。 转向半径肯定在一米以内，可以按需设置，转向半径如果大了，U形弯就会外道入弯，能比较轻松过弯，不过小弯道的时候也会贴外侧，这样会比较浪费时间，但是如果小了，小弯会贴内，节省时间，但是U形弯就会内侧入弯，那就有可能过不了弯了。
  这个参数让TEB更适合阿克曼模型。为避免规划出不可能实现的移动路径，请动手测量实际车辆的转弯半径。此参数事实上约束了舵机的最大转角。有些车辆转向性能不佳，前轮实际转过角度小于舵机角度，则应当给指令转角乘上一增益后再控制舵机，否则车辆将总是不能实现设置的最小转弯半径。请注意此项应当与最大角速度配合设置。

• cmd_angle_instead_rotvel: false # not used, is differential 是否将收到的角速度消息转换为操作上的角度变化。设置成True时，话题cmd_vel.angular.z内的数据是舵机角度。

cmd_angle_instead_rotvel 和 min_turning_radius不能同时为0，否则报警 You are mixing a carlike and a diffdrive robot

• wheelbase: 默认0.0，目前是1，只有在cmd_angle_instead_rotvel为true时才有效

轮廓

• footprint_model: 可用的有 point, circular, two_circles, line, polygon. point是最简单的，所需的计算时间最少，但准确度也最低；polygon多边形模型最复杂，完全考虑到机器人的轮廓形状，计算准确度最高。circular类型把机器人建模为一个简单的圆，半径为/footprint_model/radius，计算距离和point相似，不同点是每次调用函数时机器人半径会被加入到参数min_obstacle_dist。对圆形轮廓，可以因此选择point类型，然后将半径加到最小障碍物距离来替换，减小计算量

• is_footprint_dynamic: 目前False，是否footprint为动态的. If true, 在检查trajectory feasibility之前更新footprint

GoalTolerance

• xy_goal_tolerance: 0.1
• yaw_goal_tolerance: 0.05
• free_goal_vel: False 自由目标速度。设为False时，车辆到达终点时的目标速度为0。缺少目标速度约束将导致车辆“全速冲线”，即使前方有一堵墙也是如此 (因为撞墙的时刻不在规划器考虑范围内了)

Trajectory

• teb_autosize: True 优化期间允许改变轨迹的时域长度，也就是改变 dt_ref

• dt_ref: 0.3 局部路径规划的解析度; Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)。 TEB通过状态搜索树寻找最优路径，而dt_ref则是最优路径上的两个相邻姿态（即位置、速度、航向信息，可通过TEB可视化在rivz中看到）的默认距离。 此距离不固定，规划器自动根据速度大小调整这一距离，速度越大，相邻距离自然越大，较小的值理论上可提供更高精度。
  从autoResize函数可以看到，当相邻时间差距离(TimeDiff(i) )和dt_ref的差超过正负dt_hysteresis时，规划器将改变这一距离。 增大到0.4，teb_pose个数大大减少，车速明显加快，多数接近最大速度

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

• min_samples: 3，必须大于2。 源码在initTrajectoryToGoal和updateAndPruneTEB以及autoResize

• max_samples: 500. 源码在optimizeTEB中的autoResize

• allow_init_with_backwards_motion: 默认true，设置为false. 只影响初始化，而不是路径优化的结果。 源码在initTrajectoryToGoal，只有使用的TebOptimalPlanner::plan第一个参数是initial_plan才有效

• force_reinit_new_goal_dist: 1.0 源码在TebOptimalPlanner::plan的热启动阶段，判断局部目标(transformed_plan最后一个元素)与teb_poses最后一个元素之差的范数是否达到阈值，其实是判断局部目标是否太远了。如果小于阈值则调用updateAndPruneTEB

• global_plan_overwrite_orientation: True 覆盖全局路径中的临时局部路径点的朝向，有些全局规划器不考虑局部子目标的朝向，因此在这里校正。将未来一段的规划的姿态角的平均值作为估计的局部子目标的姿态。源码estimateLocalGoalOrientation和initTrajectoryToGoal

• global_plan_viapoint_sep: 0.3 # 正数才启用。调整全局路径上选取的航迹点的间隔，应根据机器人的尺寸大小调整.

• max_global_plan_lookahead_dist: 1.5 最大向前看距离. 指定要进行优化的全局计划子集的最大长度 (累积欧几里得距离) 。然后，实际长度由本地成本图大小和此最大界限的逻辑结合确定。设置为零或负数以停用此限制。 此距离 1.应随车辆最大速度的增大而增大 2.不应超过激光雷达等传感器的可靠测量范围 3. 不应超过局部代价地图的大小，即不能要求TEB对局部耗费地图以外的部分进行规划。

在TebLocalPlannerROS::transformGlobalPlan()函数中被使用，决定局部规划初始轨迹的最大长度，实际调试发现此参数无需过大，因为局部轨迹在每个控制周期都被更新，实际执行的指令仅是轨迹上第一个点的速度值，这里设置为1.5即可， 过长也可能导致优化结果无法有效收敛

• global_plan_prune_distance: 0.6 在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用，因为全局路径是从全局起始点到全局目标点的一条轨迹，而初始的局部路径仅是从机器人当前位置到局部目标点的一小段路径，全局路径裁剪其中一部分即局部路径，该参数决定了从机器人当前位置的后面一定距离开始裁剪

• force_reinit_new_goal_angular: 0.78 原理同上，不过是角度，弧度制

  if ( teb_.sizePoses()>0
      && (goal_.position() - teb_.BackPose().position()).norm() < 
      cfg_->trajectory.force_reinit_new_goal_dist
      && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta()) ) < cfg_->trajectory.force_reinit_new_goal_angular )

• feasibility_check_no_poses: 3 在判断生成的轨迹是否冲突时使用，此时设置为3，即从轨迹起点开始逐个检查轨迹上的3个点，若3个点均不发生碰撞，则认为本次轨迹有效。若小于0则检查所有路径点。由于teb优化并非硬约束，这里相当于是轨迹生成之后的一层保障， 这个参数根据机器人的速度和环境复杂程度调整，否则极有可能出现在狭窄环境中走走停停的情况，甚至可以减小到1或0 源码在 isTrajectoryFeasible。 如果isTrajectoryFeasible函数检查的局部路径点太多，有的会超出局部代价地图，结果函数一直返回false

• min_resolution_collision_check_angular: 默认不加载，需要用户赋值，比如 3.14159265。代价地图的检查碰撞所用的最小角分辨率。如果未达到要求，将增加 intermediate samples. 源码在TebOptimalPlanner::isTrajectoryFeasible

• control_look_ahead_poses 非默认参数，需要用户赋值，比如 2。 ROS Wiki对这个参数的解释其实是min_resolution_collision_check_angular

• exact_arc_length: false. 一般用true. 如果为true，则在速度，加速度和转弯速率计算中使用精确的弧长(计算三角函数，会增加cpu负担)，否则使用两个位姿点的直线距离做近似。

可视化

• publish_feedback: false. 发布包含完整轨迹和活动障碍物的反馈，在可视化部分，对应TebOptimalPlanner::visualize()

• visualize_with_time_as_z_axis_scale: 局部路径的z坐标赋值，源码在publishLocalPlanAndPoses， TebVisualization::publishObstacles

恢复措施

恢复措施可以尝试将快要撞到障碍或在空隙间不走的机器人或路径规划错误的机器人恢复至正常状态。TEB实现了由move_base规定的振荡恢复方法。 但是，实测中控制器将高频率振荡速度指令(>10Hz)，用户应当评估自己的电动机能否承受。 (可在 move_base 配置中关闭)

• shrink_horizon_backup: true. 在自动检测到问题(plan not feasible)的情况下，允许planner临时缩小范围(50%). TEB将以更近的点作为规划目标，尝试重新规划出可行路径。调试时可关闭，以在可视化界面上观察原出错路径。另请参阅参数shrink_horizo​​n_min_duration

• shrink_horizon_min_duration: 10. 如果检测到不可行的轨迹，激活缩小的水平线后备模式，本参数为其最短持续时间。个人怀疑这个参数无意义，因为它更新值的地方也都有no_infeasible_plans_变量的更新，判断时，只根据no_infeasible_plans_即可，详见代码configureBackupModes

• oscillation_recovery: 默认true. Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards

• oscillation_recovery_min_duration: 10 在这个时间内，是否再次发生FailureDetector检测的振荡，调用在configureBackupModes中

在小车遇到空隙不向前走时，容易出现下面的信息，然后小车来回振荡，有时能导航成功

[ WARN] Clearing costmap to unstuck robot (3.00 m).
[ WARN] TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] Rotate recovery behavior started.   # 开始振荡  RotateRecovery::runBehavior()
[ INFO] TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).

• oscillation_filter_duration: 10 failure_detector_中buffer容器的大小是oscillation_filter_duration * controller_frequency)

• oscillation_v_eps: (0,1)内的 normalized 线速度的平均值的阈值，默认0.1

• oscillation_omega_eps: (0,1)内的 normalized 角速度的平均值，默认0.1

Obstacles

• min_obstacle_dist: 0.05 最小障碍物距离，用于限制机器人与障碍物的最小距离，默认0.5。若设置了车辆的轮廓，则不建议使用此参数。使用车辆轮廓配置footprint_model配合膨胀距离即可实现较好效果。障碍物惩罚权重很高，TEB不会违背这一最小距离约束，因此会把可以通过的缝隙视作不能通过，并终止运行。
  增大min_obstacle_dist可以防止机器人离墙太近，但是对于狭窄通道的情况，反而应该设置的很小。 可以增大机器人轮廓，但减小min_obs_dist，提高防撞性，又保证过窄通道

实际还配合obstacle_association_force_inclusion_factor 和 obstacle_association_cutoff_factor两个参数生效， 参考TebOptimalPlanner::AddEdgesObstacles()函数

• obstacle_association_force_inclusion_factor: 10.0 the obstacles that will be taken into account are those closer than min_obstacle_dist * factor, if legacy is false

• obstacle_association_cutoff_factor: 40.0 the obstacles that are further than min_obstacle_dist * factor will not be taken into account, if legacy is false

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

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

• include_costmap_obstacles: True 这个不是True就不用干了

• costmap_obstacles_behind_robot_dist: 1.0 # distance at which obstacles behind the robot are taken into account

• legacy_obstacle_association: false 源码在buildGraph

• obstacle_poses_affected: 25 对于每一个障碍物（点、线、多边形），距其最近的poses都会被定位出来。参数obstacle_poses_affected个相邻的最近poses都会被考虑进来。略微影响障碍物周围轨迹的平滑度。 如果legacy_obstacle_association是false，则不启用

costmap_converter

• costmap_converter_plugin: 可取值为costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::CostmapToPolygonsDBSConcaveHull, “” 空字符串表示不启用

目前所用为CostmapToLinesDBSRANSAC"，头文件costmap_to_lines_ransac.h，最终的基类是BaseCostmapToPolygons

决定是否使用costmap_converter插件，原始costmap中障碍物全部以点来表示，计算机器人到障碍物的距离实际需要计算机器人到每一个障碍物点的距离，当环境非常复杂时计算代价会非常大。costmap_converter插件的作用是将障碍物预先表示成线段或多边形的形式，可以在一定程度上减轻后续计算距离的压力，尤其是处理激光雷达分散的测量数据时。因为将障碍物视为系列孤立点效率极低。 “costmap_converter::CostmapToPolygonsDBSMCCH”是一个较为精确的方法，它将环境转换为非凸多边形；在将障碍物距离加入g2o优化框架中 (障碍物距离是目标函数之一，描述为超图的边)

• costmap_converter_spin_thread: True
• costmap_converter_rate: 10

Optimization

除了撞击障碍物不被允许外，其余的约束在没有可行方案时可被打破（如规划出事实上不可行的转弯半径）

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

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

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

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

• weight_max_vel_x 2 满足最大允许平移速度的优化权重，还有最大角速度权重，以及加速度，角加速度权重，在整个运动过程中以主要以高速还是低速运行，则是看这些权重的分配
• weight_max_vel_theta (double，默认值：1.0) 满足最大允许角速度的优化权重
• weight_acc_lim_x (double，默认值：1.0) 满足最大允许平移加速度的优化权重

• weight_acc_lim_theta (double，默认值：1.0) 满足最大允许角加速度的优化重量

• weight_multiplier

• 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_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的信息矩阵里

• weight_dynamic_obstacle Optimization weight for satisfying a minimum separation from dynamic obstacles

• weight_dynamic_obstacle_inflation Optimization weight for the inflation penalty of dynamic obstacles (should be small)

• weight_adapt_factor 2. 源码只有optimizeTEB的最后。 每个外部TEB迭代中，一些特殊权重 (当前为weight_obstacle) 会通过此因子重复缩放 (weight_new = weight_old * factor) 。迭代增加权重而不是设置较大的先验值会导致底层优化问题的更好数值条件。

• optimization_verbose: 默认false. 图优化的过程是否显示，可以用于调试

  iteration= 1	 chi2= 0.458692	 time= 9.3481e-05	 cumTime= 0.000284497	 edges= 49	 schur= 0
  lambda= 5.804560	 levenbergIter= 2

并行规划

• enable_homotopy_class_planning (bool，默认：true) 激活并行规划(占用更多的CPU资源) 若在单一路径上运行，则没有必要时用此功能

当enable_homotopy_class_planning = False，规划的局部路径会陷入局部最小值。因为路径向上面障碍物或下面障碍物偏，总体的代价值都会增加（时间成本），只有往中间走总体代价值才最低。但此时路径invalid，中间缺少路径点，此时机器人卡死了。

设置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) 指定允许切换到新的等效类之前需要终止的持续时间 (以秒为单位)

提高性能的调试

• 使用Costmap Converter
• 减小no_outer_iterations no_inner_iterations
• 减小局部地图大小
• 增大dt_ref obstacle_poses_affected
• 关闭多路径并行规划 (效果非常显著)
• 降低 max_lookahead_distance (一般)

如何提高线速度

增大max_vel_x, dt_ref, weight_max_vel_x。如果还不见效就增大 weight_optimaltime

TEB参数设置如下:

dt_ref: 0.5
max_vel_x: 3.0
max_vel_x_backwards: 0.02
max_vel_y: 0.0
acc_lim_x: 0.8
weight_optimaltime: 30

实际的速度最大到2.7，速度曲线如下：

加大weight_optimaltime到45，最大速度超过了2.8，但修改dt_ref的效果不大了，可见weight_optimaltime提高最大速度的效果比dt_ref更好

尽量避免后退

要求TEB的速度命令只有前进命令是不可能的，因为优化框架不支持硬约束，轻微违反约束是可能的。 应当显著加大weight_kinematics_forward_drive，但是权重太大又会降低收敛速度(这对任何权重都是如此) 。将 max_vel_x_backwards减少到0(或0.02)，至少减少到与参数penalty_epsilon相同。