TEB轨迹优化算法-代码解析与参数建议
TEB算法总结
1. 简介
“TEB”全称Time Elastic Band（时间弹性带）Local Planner，该方法针对全局路径规划器生成的初始轨迹进行后续修正(modification)，从而优化机器人的运动轨迹，属于局部路径规划。在轨迹优化过程中，该算法拥有多种优化目标，包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动力学、运动学以及几何约束的符合性。“TEB方法”明确考虑了运动状态下时空方面的动态约束，如机器人的速度和加速度是有限制的。”TEB”被表述为一个多目标优化问题，大多数目标都是局部的，只与一小部分参数相关，因为它们只依赖于几个连续的机器人状态。这种局部结构产生了一个稀疏的系统矩阵，使得它可以使用快速高效的优化技术，例如使用开源框架“g2o”来解决“TEB”问题。
关于具体的TEB算法理论方面的解释可参考博客https://blog.csdn.net/xiekaikaibing/article/details/83417223以及原作者Christoph Rösmann分别在2012年和2013年发表的论文*《Trajectory modification considering dynamic constraints of autonomous robots》、《Efficient trajectory optimization using a sparse model》*。其中指出eletic band（橡皮筋）：连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。关于time eletic band的简述：起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time，即为Timed-Elastic-Band算法。通俗的解释就是TEB生成的局部轨迹由一系列带有时间信息的离散位姿(pose)组成，g2o算法优化的目标即这些离散的位姿，使最终由这些离散位姿组成的轨迹能达到时间最短、距离最短、远离障碍物等目标，同时限制速度与加速度使轨迹满足机器人的运动学。需要指出的是g2o优化的结果并非一定满足约束，即实际都是软约束条件，若参数设置不合理或环境过于苛刻，teb都有可能失败，规划出非常奇怪的轨迹。所以在teb算法中包含有冲突检测的部分，在生成轨迹之后逐点判断轨迹上的点是否与障碍物冲突，此过程考虑机器人的实际轮廓。
2. 代码分析
我们首先要知道teb_local_planner是作为ROS中move_base包的一个插件(plugin)开发的，本身该规划器无法独立作为一个node运行，或者说它只是一个lib，被move_base包调用。故以下先看move_base中调用局部规划器的逻辑。
*MoveBase::executeCb()是move_base节点在接收到一个全局目标之后的回调函数，其中以controller_frequency_的频率循环调用MoveBase::executeCycle()*函数，该函数中根据特定的规则切换状态机，进行全局路径规划或局部轨迹规划，轨迹规划部分代码如下：
case CONTROLLING:
ROS_DEBUG_NAMED("move_base","In controlling state.");
//check to see if we've reached our goal, localplanner
if(tc_->isGoalReached()){
ROS_DEBUG_NAMED("move_base","Goal reached!");
resetState();
//disable the planner thread
boost::unique_lock lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
return true;
}
//check for an oscillation condition
if(oscillation_timeout_ > 0.0 &&
last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
{
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = OSCILLATION_R;
}
{
boost::unique_lock lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
//! 调用局部规划器中的函数计算本控制周期的运行速度
if(tc_->computeVelocityCommands(cmd_vel)){
ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
last_valid_control_ = ros::Time::now();
//make sure that we send the velocity command to the base
vel_pub_.publish(cmd_vel);
if(recovery_trigger_ == CONTROLLING_R)
recovery_index_ = 0;
}
else {
ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);
//check if we've tried to find a valid control for longer than our time limit
if(ros::Time::now() > attempt_end){
//we'll move into our obstacle clearing mode
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = CONTROLLING_R;
}
else{
//otherwise, if we can't find a valid control, we'll go back to planning
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
state_ = PLANNING;
publishZeroVelocity();
//enable the planner thread in case it isn't running on a clock
boost::unique_lock lock(planner_mutex_);
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
}
}
其中变量*tc_*是一个local_planner对象的共享指针(boost::shared_ptr
//create a local planner
try {
tc_ = blp_loader_.createInstance(local_planner);
ROS_INFO("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
这里就用到了move_base的插件(plugin)机制，本质上就是定义了nav_core::BaseLocalPlanner这样一个基类，或者说是“接口(interface)”，实际的local_planner都是该类的派生类(或者说实现了接口)，并且将派生类注册，createInstance函数即ROS的pluginlib库根据传入的名称(类名，例如teb_local_planner)创建了一个实例，之后*tc_*指针实际指向teb_local_planner类的对象。teb_local_planner类的定义与实现在teb_local_planner包的”teb_local_planner_ros.cpp“文件中。
回过头再看*MoveBase::executeCycle()*函数部分，调用local_planner的函数主要是tc_->computeVelocityCommands(cmd_vel)，就是这个函数开始调用teb_local_planner算法，下面转入”teb_local_planner_ros.cpp“文件中。
*TebLocalPlannerROS::computeVelocityCommands()*函数中核心的操作列出如下：
// 截取全局路径的一部分作为局部规划的初始轨迹，主要是裁切掉机器人后方的一部分路径
pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);
// 将初始的全局路径转换到局部坐标系下，便于后续进行局部规划
if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist,
transformed_plan, &goal_idx, &tf_plan_to_global))
// 更新路径上的航迹点
if (!custom_via_points_active_)
updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);
// 更新障碍物，选择是否使用costmap_converter插件转换障碍物信息
if (costmap_converter_)
updateObstacleContainerWithCostmapConverter();
else
updateObstacleContainerWithCostmap();
// 开始执行局部轨迹规划
bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
// 检查轨迹的冲突情况
bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);
// 获取机器人需要执行的速度指令
if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, cfg_.trajectory.control_look_ahead_poses))
大致流程就是先对全局规划器规划出的*global_plan_*进行一些预处理，包括截取部分路径、转换坐标系，然后更新via_points（在全局路径上等间隔选出），更新障碍物信息（若使用costmap_converter插件，可以将障碍物做一定的简化，简化为线段、多边形等，可在一定程度上提高规划效率），之后开始执行局部轨迹规划，最后保证轨迹是无碰撞的情况下则返回规划结果即机器人需要执行的速度指令。
其中planner_->plan局部轨迹规划的部分最为核心。查看*planner_*变量的信息可见又是一个接口类，teb_local_planner包中实现了两种规划器，一个就是普通的TebOptimalPlanner，另一个是HomotopyClassPlanner，HomotopyClassPlanner是一种同时优化多个轨迹的方法，由于目标函数的非凸性会生成一系列最优的候选轨迹，最终在备选局部解的集合中寻求总体最佳候选轨迹，该工作由Christoph Rösmann等人在2017年发表：《Integrated online trajectory planning and optimization in distinctive topologies》，我们暂不讨论。
*TebOptimalPlanner::plan()*函数在”optimal_planner.cpp”文件中实现如下：
//! Plan a trajectory based on an initial reference plan
bool TebOptimalPlanner::plan(const std::vector& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
if (!teb_.isInit())
{
// init trajectory，局部规划第一次运行
teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
}
else // warm start
{
PoseSE2 start_(initial_plan.front().pose);
PoseSE2 goal_(initial_plan.back().pose);
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第一次初始化后，后续每次仅需更新起点和终点即可
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, true, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
}
}
if (start_vel)
setVelocityStart(*start_vel);
if (free_goal_vel)
setVelocityGoalFree();
else
vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
// now optimize
return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
}
其中主要是根据输入的初始路径初始化或更新了时间弹性带(轨迹)的初始状态，设置了轨迹起始点以及速度加速度的约束，最后调用*optimizeTEB()*函数：
//! Optimize a previously initialized trajectory (actual TEB optimization loop)
bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards,
double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{
if (cfg_->optim.optimization_activate==false)
return false;
bool success = false;
optimized_ = false;
double weight_multiplier = 1.0;
// TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles
// (which leads to better results in terms of x-y-t homotopy planning).
// however, we have not tested this mode intensively yet, so we keep
// the legacy fast mode as default until we finish our tests.
bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles;
for(int i=0; itrajectory.teb_autosize)
{
//teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples);
teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);
}
//! Build the hyper-graph representing the TEB optimization problem.
success = buildGraph(weight_multiplier);
if (!success)
{
clearGraph();
return false;
}
//! Optimize the previously constructed hyper-graph to deform / optimize the TEB.
success = optimizeGraph(iterations_innerloop, false);
if (!success)
{
clearGraph();
return false;
}
optimized_ = true;
if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration
computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
clearGraph();
weight_multiplier *= cfg_->optim.weight_adapt_factor;
}
return true;
}
这部分的工作就是将轨迹优化问题构建成了一个g2o图优化问题并通过g2o中关于大规模稀疏矩阵的优化算法解决，涉及到构建超图(hyper-graph)，简单来说将机器人位姿和时间间隔描述为节点(nodes)（就是优化中调整的东西），目标函数以及约束函数作为边(edges)，该graph中，每一个约束都为一条edge，并且每条edge允许连接的nodes的数目是不受限制的。
3. 参数解析
关于teb_local_planner的所有参数，在ros的wiki页面已经全部列出：http://wiki.ros.org/teb_local_planner#Parameters。以下将针对我们模拟的全向移动机器人解释其中一些参数配置。
首先以下是teb参数配置文件的内容：
TebLocalPlannerROS:
odom_topic: odom
map_frame: map
# Trajectory
teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
min_samples: 3
global_plan_overwrite_orientation: True
global_plan_viapoint_sep: 0.3 # negative, do not use viapoints. positive, use them. the actual value does not matter
max_global_plan_lookahead_dist: 1.5
global_plan_prune_distance: 0.6
force_reinit_new_goal_dist: 1.0
feasibility_check_no_poses: 3
publish_feedback: false
allow_init_with_backwards_motion: true
exact_arc_length: false
shrink_horizon_backup: true
shrink_horizon_min_duration: 10
# Robot
max_vel_x: 1.0
max_vel_x_backwards: 0.5
max_vel_theta: 3.14
max_vel_y: 0.2
acc_lim_y: 0.5
acc_lim_x: 0.5
acc_lim_theta: 1.57
min_turning_radius: 0.0
wheelbase: 0.0 # not used, is differential
cmd_angle_instead_rotvel: false # not used, is differential
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
# type: "circular"
# radius: 0.5 # for type "circular"
# type: "line"
# line_start: [-0.0545, 0.0] # for type "line"
# line_end: [0.0545, 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"
type: "polygon"
vertices: [ [0.4, -0.25], [0.5, 0.0], [0.4, 0.25], [-0.4, 0.25], [-0.4, -0.25] ] # for type "polygon"
# GoalTolerance
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.05
free_goal_vel: False
# Obstacles
min_obstacle_dist: 0.05 # minimum distance to obstacle: it depends on the footprint_model
inflation_dist: 0.0 # greater than min_obstacle_dist to take effect
include_costmap_obstacles: True # use the local costmap
costmap_obstacles_behind_robot_dist: 1.0 # distance at which obstacles behind the robot are taken into account
legacy_obstacle_association: false
obstacle_poses_affected: 30 # unused if legacy_obstacle_association is false
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
# costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
#costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
#costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
# costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
costmap_converter_plugin: "" # deactivate plugin
costmap_converter_spin_thread: True
costmap_converter_rate: 10
# Optimization
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: True # optimize
optimization_verbose: False
penalty_epsilon: 0.1
weight_max_vel_x: 2
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 # is a holonomic robot
weight_kinematics_forward_drive: 10 # prefer forward driving, for differential
weight_kinematics_turning_radius: 0 # prefer turns that respect the min_turning_radius, not used if differential (min_turning_radius = 0)
weight_optimaltime: 1.0 # prefer trajectories with less transition time
weight_obstacle: 50.0 # prefer trajectories that respect the min_obstacle_dist
weight_inflation: 0.1 # prefer trajectories that respect the inflation of the obstacles
#weight_dynamic_obstacle: 10 # not in use yet
weight_viapoint: 1.0 # prefer trajectories that respect the viapoints in the global path
weight_adapt_factor: 2 # factor to multiply some weights (currently only weight_obstacle) at each iteration (gives better results than a huge value for the weight)
# Homotopy Class Planner
enable_homotopy_class_planning: False # currently not used
其中“Trajectory”部分的参数用于调整轨迹，“global_plan_viapoint_sep”参数调整全局路径上选取的航迹点的间隔，应根据机器人的尺寸大小调整，我们机器人的长宽为0.8*0.5(米)，这里修改为0.3，使航迹点更为紧凑；“max_global_plan_lookahead_dist”参数在TebLocalPlannerROS::transformGlobalPlan()函数中被使用，决定局部规划初始轨迹的最大长度，实际调试发现此参数无需过大，因为局部轨迹在每个控制周期都被更新，实际执行的指令仅是轨迹上第一个点的速度值，这里设置为1.5即可，过长也可能导致优化结果无法有效收敛；“global_plan_prune_distance”参数在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用，因为全局路径是从全局起始点到全局目标点的一条轨迹，而初始的局部路径仅是从机器人当前位置到局部目标点的一小段路径，全局路径裁剪其中一部分即局部路径，该参数决定了从机器人当前位置的后面一定距离开始裁剪；“feasibility_check_no_poses”参数在判断生成的轨迹是否冲突时使用，此时设置为3，即从轨迹起点开始逐个检查轨迹上的3个点，若3个点均不发生碰撞，则认为本次轨迹有效，由于teb优化并非硬约束，这里相当于是轨迹生成之后的一层保障，这个参数因根据机器人的速度和环境复杂程度调整，否则极有可能出现在狭窄环境中走走停停的情况；
“Robot”部分的参数主要是根据机器人的实际情况配置最大速度和最大加速度等，不同模型的机器人（差分驱动、全向移动、阿克曼模型）有不同的配置方法。我们使用全向移动机器人（完整模型），所以需要配置y方向的速度与加速度，并且“min_turning_radius”最小转弯半径设置为0；另外“footprint_model”参数用于配置在优化过程中使用的机器人模型（主要是在计算障碍物距离的过程中），有"point", “circular”, “two_circles”, “line”, "polygon"这几种可选，针对每一种模型都有不同的障碍物距离计算方法，其中"point"模型是最简单的，但准确度也最低，"polygon"多边形模型最复杂，完全考虑到机器人的轮廓形状，计算准确度最高。我们这里选择"polygon"模型，然后需要设置多边形的几何参数（多边形的每一个顶点坐标），与move_base中costmap使用的几何参数一致。
“GoalTolerance”部分的参数设置机器人停止运行的容差，根据实际情况调整。其中“free_goal_vel”参数设置机器人在目标点速度的情况，Fasle为默认最终速度为0，即到目标位置的时候应该是保持静止状态。
“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优化框架中（障碍物距离是目标函数之一，描述为超图的边），“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的“障碍物点”被直接抛弃不再考虑，然后在剩余的障碍物点中计算机器人左侧最小距离和右侧最小距离。这三个参数的设置非常重要，需要根据机器人的外形尺寸小心调整，否则极易出现狭窄空间机器人无法通过或优化不收敛的情况。
“Optimization”部分的参数主要是设置优化框架中各部分的权重大小，其中“weight_kinematics_nh”参数应设置较小值，因为我们是完整约束机器人无需限制其运动学约束。
“Homotopy Class Planner”部分的参数与HomotopyClass规划器相关，“enable_homotopy_class_planning”设置False选择不启用。