ROS源代码阅读（10）——TEB算法
2021SC@SDUSC
ROS源代码阅读（10）
接下来我们看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算法中包含有冲突检测的部分，在生成轨迹之后逐点判断轨迹上的点是否与障碍物冲突，此过程考虑机器人的实际轮廓。
我们首先要知道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);
大致流程就是先对全局规划器规划出的*global_plan_*进行一些预处理，包括截取部分路径、转换坐标系，然后更新via_points（在全局路径上等间隔选出），更新障碍物信息（若使用costmap_converter插件，可以将障碍物做一定的简化，简化为线段、多边形等，可在一定程度上提高规划效率），之后开始执行局部轨迹规划，最后保证轨迹是无碰撞的情况下则返回规划结果即机器人需要执行的速度指令。
其中planner_->plan局部轨迹规划的部分最为核心。查看*planner_*变量的信息可见又是一个接口类，teb_local_planner包中实现了两种规划器，一个就是普通的TebOptimalPlanner，另一个是HomotopyClassPlanner，HomotopyClassPlanner是一种同时优化多个轨迹的方法，由于目标函数的非凸性会生成一系列最优的候选轨迹，最终在备选局部解的集合中寻求总体最佳候选轨迹.
*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的数目是不受限制的。