南宁网站建设免费推广怎样申请做c c 网站

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, localplannerif(tc_->isGoalReached()){ROS_DEBUG_NAMED("move_base","Goal reached!");resetState();//disable the planner threadboost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);runPlanner_ = false;lock.unlock();as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");return true;}//check for an oscillation conditionif(oscillation_timeout_ > 0.0 &&last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now()){publishZeroVelocity();state_ = CLEARING;recovery_trigger_ = OSCILLATION_R;}{boost::unique_lock<costmap_2d::Costmap2D::mutex_t> 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 basevel_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 limitif(ros::Time::now() > attempt_end){//we'll move into our obstacle clearing modepublishZeroVelocity();state_ = CLEARING;recovery_trigger_ = CONTROLLING_R;}else{//otherwise, if we can't find a valid control, we'll go back to planninglast_valid_plan_ = ros::Time::now();planning_retries_ = 0;state_ = PLANNING;publishZeroVelocity();//enable the planner thread in case it isn't running on a clockboost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);runPlanner_ = true;planner_cond_.notify_