TEB算法详解 参数详解

teb算法的基本思路之前已经看完了，今天主要看一下teb算法的参数配置文件，分析一下每个配置参数的作用：

teb的参数主要可以包含以下几个部分：

1、Trajectory

Trajectory的参数顾名思义，就是对路径生效的一些参数，仔细看一下每个参数的作用：

1.1、teb_autosize

	if (cfg_->trajectory.teb_autosize)
    {
      //teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples);
      //该函数可以新插入或是删除路径中的点（初始化后的路径），并且一次只能操作一个点。 
      //1.障碍物添加或移除时，会造成teb曲线的延长或缩短，teb为了继续遵循dt，所以要重排路径。 
      //2. 在接近终点时，dt会变小（不懂）这会导致优化运算负荷大且不稳定。（第二点还看不懂）。 
      //添加点的规则是 if （dt>dt(ref) + dt(hysteresis)) 时插点， if（dt
      // 这样可以让点到点所需的时间dt维持在dt（ref）附近。两个条件有难以同时满足，程序中定义了最大迭代次数，以避免卡死。
      //查新值的方法是插入两点的平均值作为中间点。 删点的方法是，删去i点的时间，i+1点的位置。
      //dt_ref 预设的dt， dt_hysteresis dt的缓冲区间， min_sample， max_samples 最小和最大路径点数， fast_mode 是否自动迭代（重复resize）直到整条路径resize完成的布尔值。
      teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);

    }

teb_autosize参数用于是否自动更新/插入或是删除路径中的点（初始化后的路径） trajectory.teb_autosize函数使用。teb的优化是基于g2o的，而g2o本身需要点与边的概念，这里的点就是局部路径规划的位姿点，为了保证优化的稳定性需要保证两个位姿之间距离不能太远也不能太近，如果该参数为true时会对路径长度进行优化。

1.2、dt_ref

这个参数在两个地方用到了：

第一个地方：

//dt_ref 预设的dt， dt_hysteresis dt的缓冲区间， min_sample， max_samples 最小和最大路径点数， fast_mode 是否自动迭代（重复resize）直到整条路径resize完成的布尔值。
      teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);

这个地方跟1.1参数配合使用，用于判断是否插入新的点位。即两个点之间的距离/最大速度超过dt_ref，则会插入新的点位

第二个地方：

for(int counter = 0; counter < look_ahead_poses; ++counter)
  {
    //计算一个运动时间，从当前点到前向多少个点的理论时间和
    dt += teb_.TimeDiff(counter);
    //dt_ref为两个点之间允许的最大时间，如果超过这个时间会进行点的插入
    if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses)  // TODO: change to look-ahead time? Refine trajectory?
    {
        look_ahead_poses = counter + 1;
        break;
    }
  }

这里是在优化位姿之后速度计算的地方，因为位姿变化了，所以需要重新计算一下点与点之间的时间，如果时间差比较大则需要重新插入点位。

1.3、dt_hysteresis

预设时间缓冲区，与dt_ref联用，如果两点之间的运动时间超过dt_ref+dt_hysteresis会进行点的插入 ，如果两点之间的运动时间超过dt_ref-dt_hysteresis会进行点的删除。trajectory.teb_autosize函数使用。

1.4、global_plan_overwrite_orientation

//根据当前local planer 的goal的后面几个waypoint计算平均角度，用这个角度来重写当前local  planer 的goal的角度
  if (cfg_.trajectory.global_plan_overwrite_orientation)
  {
    robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, transformed_plan.back(), goal_idx, tf_plan_to_global);
    // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)
    //  (enable using the plan as initialization) 用真实的目标点朝向覆盖transformed_plan的目标点朝向
    tf2::Quaternion q;
    q.setRPY(0, 0, robot_goal_.theta());
    tf2::convert(q, transformed_plan.back().pose.orientation);
  }  

这个参数用于决定是否重写每个点位的朝向，因为考虑到某些全局路径规划中不会给出中间点位的朝向，则这里会重写一下每个点的朝向。

1.5、allow_init_with_backwards_motion

// 将起点终点固定，并计算每两个点之间的理想时间
    teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
      cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);

如果为true，则当机器人初始位姿在起点位姿前面时，机器人可能通过倒退走到起点位置。

1.6、max_global_plan_lookahead_dist

单次local plan 考虑的全局plan的最大距离。从全局路径到局部路径优化时需要截取长度，长度由costmap与max_global_plan_lookahead_dist共同决定，取其中更小的那个

1.7、force_reinit_new_goal_dist

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) // actual warm start!
        //裁剪已经有的teb路径 裁剪掉node　顶点 更新起点或者终点。teb第一次初始化后，后续每次仅需更新起点和终点即可
      teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
    else // goal too far away -> reinit
    {
      ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
      teb_.clearTimedElasticBand();
      teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
        cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
    }

参数1.7与参数1.8合用，一个是距离阈值一个是角度阈值，当目标点的距离超过一定距离时会更新轨迹，这里的目标点应该是局部路径规划的终点？这个点是会一直随着路径更新的。

1.8、force_reinit_new_goal_angular

与1.7联用，一个是距离阈值一个是角度阈值。如果上一个目标更新的旋转差大于指定的弧度值，则强制规划器重新初始化轨迹

1.9、feasibility_check_no_poses

bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);

用于优化后的轨迹检查，前向检查多少个位姿点的可行性。判断优化后的一段轨迹需要判断是否可以运动，这个参数用于判断可以运动的轨迹的长度。

1.10、exact_arc_length

用于g2o中添加边约束是使用，如果为true，则规划器在速度、加速度和转弯率计算中使用精确的弧长[增加的cpu时间]，否则使用欧氏近似。

1.11、publish_feedback

用于可视化发布，发布包含完整轨迹和活动障碍物列表的计划器反馈（应仅出于评估或调试目的启用）

1.12、control_look_ahead_poses

if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.trajectory.control_look_ahead_poses))

这个参数用于在速度计算时取前向多少个点的位姿作为终点计算。teb的速度比较简单，就是受cfg_.trajectory.control_look_ahead_poses影响，取向前多少个位姿点的姿态。然后用当前点的位姿与目标点做差。在前面运动优化部分有计算过每个位姿之间的理论运动时间。例如这里如果要取前向第五个点，那么时间就是前面计算过的点1-2-3-4-5的时间和，距离就是点1与点5的距离差。速度的话就是两点间距离/前面计算过的点与点之间理想时间和。

1.13、prevent_look_ahead_poses_near_goal

与control_look_ahead_poses联用，control_look_ahead_poses在整条路径上使用， prevent_look_ahead_poses_near_goal在终点附近做限制。

//取前向多少个姿态点进行计算，在局部路径中取点的数量由cfg_.trajectory.control_look_ahead_poses决定，在目标点附近受cfg_->trajectory.prevent_look_ahead_poses_near_goal参数决定
  look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1 - cfg_->trajectory.prevent_look_ahead_poses_near_goal));

1.14、visualize_with_time_as_z_axis_scale

该值用于可视化参数，如果该值大于0，则使用时间作为按该值缩放的z轴，在3d中可视化轨迹和障碍物。对动态障碍物最有用

2、ViaPoints

ViaPoints相关的参数是跟路径点相关的一些参数

2.1、global_plan_viapoint_sep

if (!custom_via_points_active_)
    updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);

遍历transformed_plan将其中二点间隔大于global_plan_viapoint_sep的点加入到 ViaPointContainer。也就是从全局路径中提取局部路径。

2.2、via_points_ordered

这个参数在g2o添加边约束时使用，如果为true，则计划器将遵守存储容器中通过点的顺序。

3、Robot

Robot的参数自然就是跟机器人相关的啦

3.1、max_vel_x

顾名思义，向前运动的最大速度。最大速度在几个地方使用到：

首先在轨迹初始化的时候会用于计算点与点之间的理想时间：

// 将起点终点固定，并计算每两个点之间的理想时间
    teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
      cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);

其次，速度硬约束，保证计算结果不超过设定值：

saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z,
                   cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_trans, cfg_.robot.max_vel_theta, 
                   cfg_.robot.max_vel_x_backwards);

3.2、max_vel_x_backwards

后退时候的最大速度，这个参数跟3.1第二个使用点一样的作用，对于计算后的速度，发布前做最后的判断。

3.3、max_vel_theta

机器人运动的最大角速度，作用同3.1

3.4、acc_lim_x

最大加速度，仅在EdgeAcceleration类中使用，用于g2o添加运动学约束边。

3.5、acc_lim_theta

最大角加速度，与3.4一样在EdgeAcceleration类中使用，用于g2o添加运动学约束边。

3.6、is_footprint_dynamic

是否动态更新footprint，footprint代表的是机器人的模型，如果机器人模型是变化的，则需要设置为true，一般我们机器人模型都是固定的。

3.7、use_proportional_saturation

这个参数在速度计算完成后进行速度约束时使用，作用是当机器人的速度、角速度超过一定阈值时，会进行限幅。考虑一种情况：当我的线速度x超过最大值时但是其他两项在范围内时，我是只对x单独进行速度限幅还是将另外两个速度也等比例限制？

if (cfg_.robot.use_proportional_saturation)
  {
    double ratio = std::min(std::min(ratio_x, ratio_y), ratio_omega);
    vx *= ratio;
    vy *= ratio;
    omega *= ratio;
  }
  else
  {
    vx *= ratio_x;
    vy *= ratio_y;
    omega *= ratio_omega;
  }

这里参数如果设置为true，则会对所有参数作同比例限幅，如果是false，则是各管各的。

3.8、transform_tolerance

ros::Duration(cfg_.robot.transform_tolerance)。在TF获取的时候设置的时间参数。

geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp,
                                                                                  plan_pose.header.frame_id, ros::Duration(cfg_.robot.transform_tolerance));

4、Carlike

Carlike参数主要是用于类似于汽车的机器人的运动学控制，什么是类似于汽车的？就是那种不能原地转向的小车。

4.1、min_turning_radius

顾名思义就是最小转弯半径：

cmd_vel.twist.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.twist.linear.x, cmd_vel.twist.angular.z,
                                                                cfg_.robot.wheelbase, 0.95*cfg_.robot.min_turning_radius);

在速度发布前会根据该参数确定角速度，如果速度/角速度得到的转弯半径小于设定值*系数0.95，则会相应的修改角速度。：

if (fabs(radius) < min_turning_radius)
    radius = double(g2o::sign(radius)) * min_turning_radius; 

4.2、wheelbase

驱动轴和转向轴之间的距离（仅适用于启用“cmd_angle_instead_rotvel”的车载机器人）；对于后轮机器人，该值可能为负值！
同样用于角速度修正，不论4.1参数对角速度是否进行修正，这里都会根据wheelbase做二次修正：

  double radius = v/omega;
  
  if (fabs(radius) < min_turning_radius)
    radius = double(g2o::sign(radius)) * min_turning_radius; 
  return std::atan(wheelbase / radius);

4.3、cmd_angle_instead_rotvel

用于判断是否为carlike机器人，只有该参数为true时，参数4.1、4.2才生效。

5、Omnidirectional

全向轮机器人

5.1、max_vel_y

全向轮在y方向的最大速度，作用与x方向的速度类似，两个作用，一个是用于添加运动学约束，二是限制y方向的最大速度。

5.2、max_vel_trans

机器人的最大线速度。当它们的线性组合超过其值时，将限制max_vel_x和max_vel_y。当设置为默认值0.0时，它将被设置为等于max_vel_x。作用与5.1类似，也是在这两个地方分别起作用。

5.3、acc_lim_y

y方向的最大加速度，用于g2o的加速度约束项。

6、GoalTolerance

这个类别下的参数主要是对于目标点的一些参数设定：

6.1、xy_goal_tolerance

允许到终点位置的最终欧几里得距离。这个值用于判断当前机器人是否到达目标点位置，如果机器人到达目标点，则当前姿态与目标点的欧式距离应小于该值。

if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance
    && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance
    && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0)
    && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel)
        || cfg_.goal_tolerance.free_goal_vel))
  {
    goal_reached_ = true;
    return mbf_msgs::ExePathResult::SUCCESS;
  }

当然到终点的判定不止这一个，这只是其中一个判定要求。

6.2、yaw_goal_tolerance

允许的目标方向的最终方向误差，作用与6.1类似。

6.3、free_goal_vel

出于规划目的，允许机器人的速度为非零（机器人可以以最大速度到达目标）。如果为false，则判定机器人到达目标点的条件会严格一点，需要判断当前速度是否也为一个很小的值cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel。同时planner函数在终点前面也会做对应的减速处理；如果为true，则对终点不做减速处理也不会判断终点附近的速度。

6.4、trans_stopped_vel

当6.3为false时，对于是否到达目标点需要做速度判断作为到达终点的条件。判定的条件即是该值小于设定值6.4.低于我们认为机器人在平移中停止的最大速度

6.5、theta_stopped_vel

作用同6.4，但是这里是对角速度做判断。用于到达目标点的角速度判断，机器人到达目标点的判定条件之一，当前角速度低于该值。

7、Obstacles

障碍物相关参数

7.1、min_obstacle_dist

与障碍物的最小期望间距，参数在g2o优化项AddEdgesObstacles中使用，作为姿态点的约束。

7.2、inflation_dist

膨胀距离。具有非零惩罚成本的障碍物周围的缓冲区（应大于min_obstacle_dist才能生效）

bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;
if (inflated)
    {
      EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle;
      dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
      dist_bandpt_obst->setInformation(information_inflated);
      dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obstacle);
      optimizer_->addEdge(dist_bandpt_obst);
    }
    else
    {
      EdgeObstacle* dist_bandpt_obst = new EdgeObstacle;
      dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
      dist_bandpt_obst->setInformation(information);
      dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obstacle);
      optimizer_->addEdge(dist_bandpt_obst);
    };

这个参数跟7.1参数在同一个地方使用，判断添加的约束类型。

7.3、dynamic_obstacle_inflation_dist

动态障碍物的膨胀半径，在g2o优化添加约束过程中，如果参数7.4为true则约束中会添加动态障碍物约束。

7.4、include_dynamic_obstacles

这个参数有两个作用：

首先用于判断是否需要添加动态障碍物约束

	//添加动态障碍物边
  if (cfg_->obstacles.include_dynamic_obstacles)
    AddEdgesDynamicObstacles();

其次也用于判断路径更新模式：

bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles;
teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);

autoResize函数会更新局部路径点，根据参数include_dynamic_obstacles决定是只更新前面的一个点还是更新一段路径上的点。

7.5、include_costmap_obstacles

是否添加costmap障碍物，如果为true时会在更新障碍物地图时添加costmap的内容。

7.6、legacy_obstacle_association

用于判断以何种方式添加障碍物边 如果为true，则使用旧的关联策略（对于每个障碍物，找到最近的TEB姿势），否则使用新的关联策略。（对于每个TEB姿势，只找到“相关”障碍物）。

  // 添加障碍物边
  if (cfg_->obstacles.legacy_obstacle_association)
    AddEdgesObstaclesLegacy(weight_multiplier);
  else
    AddEdgesObstacles(weight_multiplier);

7.7、obstacle_association_force_inclusion_factor

在优化期间仅将相关障碍物与离散轨迹连接，强制包括指定距离内的所有障碍物（作为min_obstacle_dist的倍数），例如，选择2.0以考虑半径为2.0*min_obstocle_dist内的障碍物。

		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)
          {
              iter_obstacle->push_back(obst);
              continue;
          }

添加障碍物约束的时候考虑一定距离内的障碍物添加到约束。

7.8、obstacle_association_cutoff_factor

裁减距离，在添加障碍物边约束的时候，如果障碍物距离超过这个值就直接不考虑了。

		  // cut-off distance
          if (dist > cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_cutoff_factor)
            continue;

7.7与7.8两个分别代表了添加障碍物的最近距离与最远距离。如果这两个值不想等，则它们中间还有一部分空间不满足这两个条件的，则会再做其他判断是否添加到障碍物约束中。

7.9、costmap_obstacles_behind_robot_dist

添加代价地图中的障碍物时考虑的朝后距离？用于障碍物添加函数updateObstacleContainerWithCostmap中。

if (costmap_->getCost(i,j) == costmap_2d::LETHAL_OBSTACLE)
        {
          Eigen::Vector2d obs;
          costmap_->mapToWorld(i,j,obs.coeffRef(0), obs.coeffRef(1));
            
          // check if obstacle is interesting (e.g. not far behind the robot)
          Eigen::Vector2d obs_dir = obs-robot_pose_.position();
          if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_.obstacles.costmap_obstacles_behind_robot_dist  )
            continue;
            
          obstacles_.push_back(ObstaclePtr(new PointObstacle(obs)));
        }

7.10、obstacle_poses_affected

用于函数AddEdgesObstaclesLegacy添加障碍物边约束，似乎影响了障碍物会被添加到哪个顶点作为约束项。

	if (cfg_->obstacles.obstacle_poses_affected >= teb_.sizePoses())
      index =  teb_.sizePoses() / 2;
    else
      index = teb_.findClosestTrajectoryPose(*(obst->get()));

7.11、obstacle_proximity_ratio_max_vel

由于接近静态障碍物而降低速度时用作上限的最大速度的比率。用于AddEdgesVelocityObstacleRatio函数，这个函数在参数weight_velocity_obstacle_ratio>0时添加到动力学约束中。

if (cfg_->optim.weight_velocity_obstacle_ratio > 0)
    AddEdgesVelocityObstacleRatio();

7.12、obstacle_proximity_lower_bound

用于AddEdgesVelocityObstacleRatio函数，作用类似于7.11。

7.13、obstacle_proximity_upper_bound

用于AddEdgesVelocityObstacleRatio函数，作用类似于7.11。
以上三个参数在同一个函数中被使用，且同时需要参数weight_velocity_obstacle_ratio>0时才会用到，用于添加额外的约束项。

8、Optimization

优化相关参数

8.1、no_inner_iterations

内循环迭代的次数。optimizeTEB里面有两层循环,分别叫外部循环和内部循环。外部循环通过调用TimedElasticBand::autoResize()来根据时间分辨率调整轨迹。内部循环调用optimizeGraph()进行优化。

8.2、no_outer_iterations

外循环迭代的次数。作用同上，内循环每次优化完位姿后，在外循环中调用TimedElasticBand::autoResize()来根据时间分辨率调整轨迹。对于距离远的点插入一下新的位姿，对于距离近的点删一删。

8.3、optimization_activate

g2o优化参数，是否激活优化项，感觉似乎没啥意义，肯定得开着不是。

8.4、optimization_verbose

好像是一个专门用于设置优化器属性的参数

optimizer_->setVerbose(cfg_->optim.optimization_verbose);

8.5、penalty_epsilon

这个参数使用的地方还是挺多的，在添加障碍物边、动态障碍物边以及速度边的时候都用到了。说是为硬约束近似的惩罚函数添加一个小的安全裕度。不是特别理解，后面慢慢研究一下。

8.6、weight_max_vel_x

满足最大允许平移速度的优化权重。在函数AddEdgesVelocity中使用，对运动学约束添加权重。

8.7、weight_max_vel_y

作用同上，但是只在全向轮机器人上会存在y方向速度，所以使用的时候需要约束一下，同样作为权重使用。

if (cfg_->robot.max_vel_y == 0) // non-holonomic robot
{
	information(0,0) = cfg_->optim.weight_max_vel_x;
    information(1,1) = cfg_->optim.weight_max_vel_theta;
    information(0,1) = 0.0;
    information(1,0) = 0.0;
}
else
{
	information(0,0) = cfg_->optim.weight_max_vel_x;
    information(1,1) = cfg_->optim.weight_max_vel_y;
    information(2,2) = cfg_->optim.weight_max_vel_theta;
}

8.8、weight_max_vel_theta

角度方向的权重，使用与前两个是一样的。

8.9、weight_acc_lim_x

作用与前三个类似，但是使用的地点不同，前面是添加速度边的时候的权重，这里则是在添加加速度边AddEdgesAcceleration的时候生效

8.10、weight_acc_lim_y

加速度权重，y方向

8.11、weight_acc_lim_theta

角速度权重

8.12、weight_kinematics_nh

满足非完整运动学的优化权值，用于机器人的约束条件。

if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
    AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
  else
    AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.

void TebOptimalPlanner::AddEdgesKinematicsDiffDrive()
{
	if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_forward_drive==0)
    return; // if weight equals zero skip adding edges!

8.13、weight_kinematics_forward_drive

与上面的参数是一样的用法，但是这个参数需要在差速轮下才能够使用。

void TebOptimalPlanner::AddEdgesKinematicsDiffDrive()
{
  if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_forward_drive==0)
    return; // if weight equals zero skip adding edges!
  
  // create edge for satisfiying kinematic constraints
  Eigen::Matrix<double,2,2> information_kinematics;
  information_kinematics.fill(0.0);
  information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
  information_kinematics(1, 1) = cfg_->optim.weight_kinematics_forward_drive;
  

8.14、weight_kinematics_forward_drive

与上面的类似，但是这个参数是用在carlike机器人上的权重参数：

void TebOptimalPlanner::AddEdgesKinematicsCarlike()
{
  if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_turning_radius==0)
    return; // if weight equals zero skip adding edges!

  // create edge for satisfiying kinematic constraints
  Eigen::Matrix<double,2,2> information_kinematics;
  information_kinematics.fill(0.0);
  information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
  information_kinematics(1, 1) = cfg_->optim.weight_kinematics_turning_radius;

8.15、weight_optimaltime

时间约束边的权重。

if (cfg_->optim.weight_optimaltime==0) 
    return; // if weight equals zero skip adding edges!

  Eigen::Matrix<double,1,1> information;
  information.fill(cfg_->optim.weight_optimaltime);

  for (int i=0; i < teb_.sizeTimeDiffs(); ++i)
  {
    EdgeTimeOptimal* timeoptimal_edge = new EdgeTimeOptimal;
    timeoptimal_edge->setVertex(0,teb_.TimeDiffVertex(i));
    timeoptimal_edge->setInformation(information);
    timeoptimal_edge->setTebConfig(*cfg_);
    optimizer_->addEdge(timeoptimal_edge);
  }

8.16、weight_shortest_path

最短路径边的权重

if (cfg_->optim.weight_shortest_path==0)
    return; // if weight equals zero skip adding edges!

  Eigen::Matrix<double,1,1> information;
  information.fill(cfg_->optim.weight_shortest_path);

  for (int i=0; i < teb_.sizePoses()-1; ++i)
  {
    EdgeShortestPath* shortest_path_edge = new EdgeShortestPath;
    shortest_path_edge->setVertex(0,teb_.PoseVertex(i));
    shortest_path_edge->setVertex(1,teb_.PoseVertex(i+1));
    shortest_path_edge->setInformation(information);
    shortest_path_edge->setTebConfig(*cfg_);
    optimizer_->addEdge(shortest_path_edge);
  }

8.17、weight_obstacle

添加障碍物边的权重，在AddEdgesObstacles函数中使用

8.18、weight_inflation

膨胀层的权重，与8.18是一起使用的：

  bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;

  Eigen::Matrix<double,1,1> information;
  information.fill(cfg_->optim.weight_obstacle * weight_multiplier);
  
  Eigen::Matrix<double,2,2> information_inflated;
  information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier;
  information_inflated(1,1) = cfg_->optim.weight_inflation;
  information_inflated(0,1) = information_inflated(1,0) = 0;

8.19、weight_dynamic_obstacle

动态障碍物权重，与8.20联合使用用于添加动态障碍物权重。

  Eigen::Matrix<double,2,2> information;
  information(0,0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier;
  information(1,1) = cfg_->optim.weight_dynamic_obstacle_inflation;
  information(0,1) = information(1,0) = 0;

8.20、weight_dynamic_obstacle_inflation

动态障碍物膨胀层权重，与8.19联合使用添加动态障碍物权重

8.21、weight_velocity_obstacle_ratio

这个参数有两个地方使用：
第一个是用于判断是否添加额外约束：

if (cfg_->optim.weight_velocity_obstacle_ratio > 0)
    AddEdgesVelocityObstacleRatio();

第二个是函数AddEdgesVelocityObstacleRatio中添加对应额外约束的权重：

information(0,0) = cfg_->optim.weight_velocity_obstacle_ratio;
  information(1,1) = cfg_->optim.weight_velocity_obstacle_ratio;
  information(0,1) = information(1,0) = 0;

  auto iter_obstacle = obstacles_per_vertex_.begin();

  for (int index = 0; index < teb_.sizePoses() - 1; ++index)
  {
    for (const ObstaclePtr obstacle : (*iter_obstacle++))
    {
      EdgeVelocityObstacleRatio* edge = new EdgeVelocityObstacleRatio;
      edge->setVertex(0,teb_.PoseVertex(index));
      edge->setVertex(1,teb_.PoseVertex(index + 1));
      edge->setVertex(2,teb_.TimeDiffVertex(index));
      edge->setInformation(information);
      edge->setParameters(*cfg_, robot_model_.get(), obstacle.get());
      optimizer_->addEdge(edge);
    }
  }

8.22、weight_viapoint

经过路径点边的权重，用于函数AddEdgesViaPoints

8.23、weight_adapt_factor

权重的自适应因子，在TebOptimalPlanner::optimizeTEB函数的外循环中，每次内循环结束后，内循环权重都会乘以自适应因子

	//构建图
    success = buildGraph(weight_multiplier);
    //优化图
    success = optimizeGraph(iterations_innerloop, false);
    weight_multiplier *= cfg_->optim.weight_adapt_factor;

8.24、obstacle_cost_exponent

非线性障碍物成本的指数（成本=linear_cost*障碍物成本导出）。设置为1以禁用非线性成本（默认值）

9、HCPlanning

这边的参数是Homotopy方法的一些配置参数，teb中的一些函数存在两种方法，其中TebOptimalPlanner类是单一的TEB规划器。使用全局规划器生成的初始轨迹来初始化TEB局部规划器。HomotopyClassPlanner类像是多个TebOptimalPlanner类实例的组合。HomotopyClassPlanner类中也会实例化一个由全局规划器生成的路径作为参考的对象。除此之外，它还会使用probabilistic roadmap (PRM) methods在障碍物周边采样一些keypoints，将这些keypoints连接起来，去除方向没有朝向目标点的连接和与障碍物重叠的连接。这样就形成了一个网络，然后将起始点和终点接入到这个网络。

使用Depth First Search（深度优先方法）搜索所有可行的路径。将这些路径作为参考，实例化多个TebOptimalPlanner类的实例。采用多线程并行优化，得到多条优化后的路径。将这些路径进行可行性分析，选出代价值最小的最优路径。不得不说HomotopyClassPlanner类里的方法是一个鲁棒性和可靠性更高的方法。因为单一的TEB规划 (TEB without homology class exploration)在某些场景会陷入局部最小值，可能出现卡死的情况。

具体分析参看《对ROS局部运动规划器Teb的理解》

9.1、enable_multithreading

是否激活多线程并行规划多个轨迹

9.2、max_number_classes

指定允许的替代homotopy类的最大数量（限制计算工作量）

9.3、max_number_plans_in_current_class

要尝试的与当前最佳轨迹在同一homotopy类中的轨迹的最大数量（将其设置为2或更多有助于避免局部极小值）。必须小于等于max_number_classes

9.4、selection_cost_hysteresis

指定新的候选轨迹必须具有相对于先前选择的轨迹小于一定范围的轨迹成本才能被选择（如果new_cost＜old_cost*因子则选择）

9.5、selection_prefer_initial_plan

在初始计划的等价类中指定轨迹的间隔

9.6、selection_obst_cost_scale

仅用于选择“最佳”候选者的障碍成本术语的额外缩放（new_obst_cost:obst_cost*因子）

9.7、selection_viapoint_cost_scale

通过点成本条款的额外扩展，仅用于选择“最佳”候选人。（new_viapt_cost:viapt_cost*因子）

9.8、selection_alternative_time_cost

如果为true，则时间成本将替换为总转换时间。

9.9、selection_dropping_probability

在每个计划周期，除了当前“最佳”的TEB之外，TEB将以这种概率随机丢弃。防止“专注”于次优替代同源物。

9.10、switching_blocking_period

指定允许切换到新等效类之前需要过期的持续时间（以秒为单位）

9.11、roadmap_graph_no_samples

如果simple_exploration处于关闭状态，则指定为创建路线图而生成的样本数

9.12、roadmap_graph_area_width

指定将在起点和终点之间生成采样的区域的宽度[m]（高度等于起点到终点的距离）

9.13、roadmap_graph_area_length_scale

矩形区域的长度由起点和终点之间的距离决定。此参数进一步缩放距离，使几何中心保持相等！

9.14、h_signature_prescaler

为了允许大量的障碍物，缩放障碍物的数值。不要选择极低，否则无法区分障碍物（0.2

9.15、h_signature_threshold

如果实部和复部的差值都低于指定的阈值，则假设两个h符号相等

9.16、obstacle_heading_threshold

指定障碍物航向和目标航向之间的标准化标量乘积的值，以便在探索时考虑它们（障碍物）

9.17、viapoints_all_candidates

如果为true，则不同拓扑的所有轨迹都附加到过孔点集，否则仅附加与初始/全局计划共享相同轨迹的轨迹（在test_optim_node中没有影响）。

9.18、visualize_hc_graph

可视化为探索新的homotopy类而创建的图

10、Recovery

故障恢复下使用的几个参数

10.1、shrink_horizon_backup

允许计划器在自动检测到问题的情况下临时缩小范围（50%）。teb局部路径规划有时候也会失败。此时需要进入故障恢复，丢弃一部分路径，这个参数决定了是否丢弃一部分路径，如果设置为true，会裁减掉50%。

	if (cfg_.recovery.shrink_horizon_backup && 
        goal_idx < (int)transformed_plan.size()-1 && // we do not reduce if the goal is already selected (because the orientation might change -> can introduce oscillations)
       (no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).toSec() < cfg_.recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds
    {
        ROS_INFO_COND(no_infeasible_plans_==1, "Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected).", cfg_.recovery.shrink_horizon_min_duration);


        // Shorten horizon if requested
        // reduce to 50 percent:
        int horizon_reduction = goal_idx/2;

10.2、shrink_horizon_min_duration

这个参数在TebLocalPlannerReconfigure.cfg列表中是没有的，但是在函数中被使用，与10.1在同一位置。用于判断当前时间与上一次成功规划之间的时间间隔，如果时间间隔超过该值就不做继续的处理的，说明失败有一段时间了，该进入错误模式了。

10.3、oscillation_recovery

跟前两个参数一样都是在恢复模式下使用，当该参数设置为true时，会进行机器人振荡行为判断。判断机器人是否在某个位置来回振荡运行。即：始终在一个小范围内活动，且角速度来回反复横跳。

10.4、oscillation_recovery_min_duration

振荡判断的时间参数，作用同10.2

11、Divergence Detection

发散性检测相关参数

11.1、divergence_detection_enable

是否使能收敛性判断，对teb优化后的位姿进行收敛性判断。判断其与全局路径之间是否收敛。

11.2、divergence_detection_max_chi_squared

收敛性判断依据。

bool TebOptimalPlanner::hasDiverged() const
{
  // Early returns if divergence
   detection is not active
  // 如果发散检测未激活，则提前返回
  if (!cfg_->recovery.divergence_detection_enable)
    return false;
  //这里涉及到一个g2o中的返回函数batchStatistics，这里似乎存放了优化后的评价指标
  auto stats_vector = optimizer_->batchStatistics();

  // No statistics yet
  if (stats_vector.empty())
    return false;

  // Grab the statistics of the final iteration
  const auto last_iter_stats = stats_vector.back();
  //最后一个点的评价指标不能超过设定值
  //chi2函数来获得（加权）最小二乘误差等，这些可以用来判断某条边的误差是否过大。
  return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared;
}

chi2函数来获得（加权）最小二乘误差等，这些可以用来判断某条边的误差是否过大。

11.3、enable_homotopy_class_planning

是否激活homotopy类规划算法。如果为true算法会使用homotopy方法进行路径优化，false则使用TebOptimalPlanner类。

	if (cfg_.hcp.enable_homotopy_class_planning)
    {
      planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies enabled.");
    }
    else
    {
      planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies disabled.");
    }

teb参数列表一共97个参数，至此应该就差不多了。HCPlanning相关参数由于那块代码还没细看，后面代码看完了再补充一下。