teb局部规划代码地址：https://github.com/rst-tu-dortmund/teb_local_planner
teb局部规划论文：C. Rösmann, F. Hoffmann and T. Bertram: Kinodynamic Trajectory Optimization and Control for Car-Like Robots, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, Sept. 2017.
teb局部规划算法是通过松弛约束的方法将规划问题转为了无约束优化问题，之后通过图优化的方法完成对于无约束优化问题的求解，图优化的求解使用的g2o库，优化使用的算法是LM算法。
在进行代码解读之前，需要先了解图优化与g2o库。所谓的图优化，就是把一个常规的优化问题，以图（Graph）的形式来表述。图优化适用于无约束优化问题，并且保证该优化的目标函数的海森矩阵存在且是稀疏的。g2o是一个优秀的图优化求解库，其数据接收如下图所示，它主要包括三种数据类型，第一种是边，第二种是定点，第三种是求解器。在使用g2o库的过程中，通常首先是定义优化器，优化器的定义包括三个过程，第一个过程是定义一个稀疏线性方程求解器，如PCG，CSparse，Cholmod等，用于计算雅克比和海森矩阵的解析形式。第二个过程是定义所使用的优化算法，通常为GN或LM算法。第三个过程是完成最终优化器的定义。之后，定义图的顶点和边，将其加入优化器，最终求解得到优化结果。
了解完图优化和g2o库后，接下来是对teb代码的解读。
主函数main位于test_optim_node.cpp中，在该部分，首先得到了规划的所需参数
// load ros parameters from node handle
config.loadRosParamFromNodeHandle(n);
之后定义了两个定时器，一个执行CB_mainCycle，用于进行规划，一个执行CB_publishCycle，用于进行可视化发布
ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle);
ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle);
之后定义优化器
// Setup planner (homotopy class planning or just the local teb planner)
if (config.hcp.enable_homotopy_class_planning)
planner = PlannerInterfacePtr(new HomotopyClassPlanner(config, &obst_vector, robot_model, visual, &via_points));
else
planner = PlannerInterfacePtr(new TebOptimalPlanner(config, &obst_vector, robot_model, visual, &via_points));
综上所述，在主函数部分，主要是进行了规划器planner的定义，并且循环调用规划器的plan函数进行规划，这里我们以 TebOptimalPlanner作为当前规划器进行下一阶段的解读。
TebOptimalPlanner的构造函数中调用了initialize函数。
TebOptimalPlanner::TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, const ViaPointContainer* via_points)
{
initialize(cfg, obstacles, robot_model, visual, via_points);
}
在 initialize函数中，定义了优化器optimizer，参数，障碍物，机器人模型，全局路径点等。
// init optimizer (set solver and block ordering settings)
optimizer_ = initOptimizer();
cfg_ = &cfg;
obstacles_ = obstacles;
robot_model_ = robot_model;
via_points_ = via_points;
cost_ = HUGE_VAL;
prefer_rotdir_ = RotType::none;
优化器是通过调用 initOptimizer函数进行构造。在函数中，首先是注册自定义的顶点和边，之后构造了一个线性方程求解器 linear_solver，求解器使用的是g2o::LinearSolverCSparse，在此求解器基础上，定义了类型为g2o::BlockSolver的 block_solver。在此基础上，又定义了LM算法的优化器 g2o::OptimizationAlgorithmLevenberg，最终得到稀疏优化器 g2o::SparseOptimizer。并且定义优化器可以以多线程形式进行。
// Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
static boost::once_flag flag = BOOST_ONCE_INIT;
boost::call_once(®isterG2OTypes, flag);
// allocating the optimizer
boost::shared_ptr<g2o::SparseOptimizer> optimizer = boost::make_shared<g2o::SparseOptimizer>();
std::unique_ptr<TEBLinearSolver> linear_solver(new TEBLinearSolver()); // see typedef in optimization.h
linear_solver->setBlockOrdering(true);
std::unique_ptr<TEBBlockSolver> block_solver(new TEBBlockSolver(std::move(linear_solver)));
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));
optimizer->setAlgorithm(solver);
optimizer->initMultiThreading(); // required for >Eigen 3.1
定义完成规划器planner后，接下来进行规划函数plan。在规划函数中，第一步就是判断teb是否初始化完成，如果没有初始化，则进行初始化。（teb为TimedElasticBand类，其中保存在两类g2o顶点列表，第一类是VertexPose，第二类是VertexTimeDiff，前者是x,y,theta的坐标点，后者是dt时间差）
if (!teb_.isInit())
{
// init trajectory
teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); // 0 intermediate samples, but dt=1 -> autoResize will add more samples before calling first optimization
}
初始化函数 initTrajectoryToGoal功能是生成从起点到终点的连线，并在连线上进行采样，填充teb中的顶点。如果已经初始化过了，则判断teb_中顶点数量是否为0，如果不为0且终点没有发生巨大变化，则使用updateAndPruneTEB函数添加顶点，反之则重新初始化顶点。
else // warm start
{
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_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples);
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(start, goal, 0, cfg_->robot.max_vel_x, 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)
最后调用优化函数optimizeTEB进行优化。
// now optimize
return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
在优化函数optimizeTEB中，第一步是对teb进行调整。 autoResize函数功能是对teb顶点进行增添或删除，以保证时间差顶点相近。
teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);
完成调整后，下一步是进行图的构建，功能函数是 buildGraph。
success = buildGraph(weight_multiplier);
在 buildGraph内首先判断当前图是否为空，如果不为空需要先清空之前的图。
if (!optimizer_->edges().empty() || !optimizer_->vertices().empty())
{
ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!");
return false;
}
之后，设定优化器的属性，决定是否记录中间结果和统计信息。
optimizer_→setComputeBatchStatistics(cfg_→recovery.divergence_detection_enable);
之后，添加图的顶点，功能函数为AddTEBVertices。
AddTEBVertices();
AddTEBVertices函数如下所示，其过程是从TEB中将坐标顶点和时间顶点取出，给定id，加入到图优化器optimizer中。同时，清空障碍物列表obstacles_per_vertex_。
unsigned int id_counter = 0; // used for vertices ids
obstacles_per_vertex_.resize(teb_.sizePoses());
auto iter_obstacle = obstacles_per_vertex_.begin();
for (int i=0; i<teb_.sizePoses(); ++i)
{
teb_.PoseVertex(i)->setId(id_counter++);
optimizer_->addVertex(teb_.PoseVertex(i));
if (teb_.sizeTimeDiffs()!=0 && i<teb_.sizeTimeDiffs())
{
teb_.TimeDiffVertex(i)->setId(id_counter++);
optimizer_->addVertex(teb_.TimeDiffVertex(i));
}
iter_obstacle->clear();
(iter_obstacle++)->reserve(obstacles_->size());
}
接下来是添加障碍物边，功能函数是AddEdgesObstacles，目的是增大坐标点离障碍物的距离。
AddEdgesObstacles(weight_multiplier);
在函数AddEdgesObstacles内，首先判断是否进行了障碍物膨胀，之后定义了一个创建边的函数，如下所示，障碍物边是名为EdgeObstacle的类。之后，遍历每一个坐标点，找到离坐标点距离小于阈值的障碍物，以及左侧和右侧最近的障碍物，构建EdgeObstacle对象，作为图的障碍物边。边误差的计算为坐标点到障碍物的距离，再过一个激活函数。
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);
接下来是添加动态障碍物边，基本和添加障碍物类型，只是障碍物随时间变化。
AddEdgesDynamicObstacles();
接下来是添加经过路径点边，目的是减小坐标点离路径点的距离。
AddEdgesViaPoints();
AddEdgesViaPoints函数中首先遍历每一个路径点，计算与当前路径点最近的坐标点，构建路径点边，类型为 EdgeViaPoint，边的误差计算就是欧氏距离。
for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it)
{
int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx);
if (cfg_->trajectory.via_points_ordered)
start_pose_idx = index+2; // skip a point to have a DOF inbetween for further via-points
// check if point conicides with goal or is located behind it
if ( index > n-2 )
index = n-2; // set to a pose before the goal, since we can move it away!
// check if point coincides with start or is located before it
if ( index < 1)
{
if (cfg_->trajectory.via_points_ordered)
{
index = 1; // try to connect the via point with the second (and non-fixed) pose. It is likely that autoresize adds new poses inbetween later.
}
else
{
ROS_DEBUG("TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose.");
continue; // skip via points really close or behind the current robot pose
}
}
Eigen::Matrix<double,1,1> information;
information.fill(cfg_->optim.weight_viapoint);
EdgeViaPoint* edge_viapoint = new EdgeViaPoint;
edge_viapoint->setVertex(0,teb_.PoseVertex(index));
edge_viapoint->setInformation(information);
edge_viapoint->setParameters(*cfg_, &(*vp_it));
optimizer_->addEdge(edge_viapoint);
}
接下来是添加速度边，速度包括了线速度和角速度两个维度，目的是防止线速度和角速度超过给定阈值。
AddEdgesVelocity();
AddEdgesVelocity函数如下所示，定义了协方差矩阵，然后定义了速度边，速度边是一个多顶点边，和三个顶点有关，类型为EdgeVelocity。
int n = teb_.sizePoses();
Eigen::Matrix<double,2,2> information;
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;
for (int i=0; i < n - 1; ++i)
{
EdgeVelocity* velocity_edge = new EdgeVelocity;
velocity_edge->setVertex(0,teb_.PoseVertex(i));
velocity_edge->setVertex(1,teb_.PoseVertex(i+1));
velocity_edge->setVertex(2,teb_.TimeDiffVertex(i));
velocity_edge->setInformation(information);
velocity_edge->setTebConfig(*cfg_);
optimizer_->addEdge(velocity_edge);
}
接下来是添加加速度边，同样包含了线加速度和角加速度，目的是防止加速度超过给定阈值。
AddEdgesAcceleration();
AddEdgesAcceleration函数的主要内容如下所示，为创建 EdgeAcceleration对象，EdgeAcceleration对象是一个5个顶点的边。
EdgeAcceleration* acceleration_edge = new EdgeAcceleration;
acceleration_edge->setVertex(0,teb_.PoseVertex(i));
acceleration_edge->setVertex(1,teb_.PoseVertex(i+1));
acceleration_edge->setVertex(2,teb_.PoseVertex(i+2));
acceleration_edge->setVertex(3,teb_.TimeDiffVertex(i));
acceleration_edge->setVertex(4,teb_.TimeDiffVertex(i+1));
acceleration_edge->setInformation(information);
acceleration_edge->setTebConfig(*cfg_);
optimizer_→addEdge(acceleration_edge);
接下来是添加时间最优化边，用于优化时间。
AddEdgesTimeOptimal();
AddEdgesTimeOptimal函数主要内容如下，对于每一个时间顶点，添加 EdgeTimeOptimal对象。
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);
}
接下来是添加轨迹最短化边，目的是优化轨迹长度。
AddEdgesShortestPath();
AddEdgesShortestPath函数的内容如下所示，对于每两个相邻坐标点，生成一个 EdgeShortestPath对象，EdgeShortestPath是一个两顶点边。
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);
}
接下来是添加动力学限制边，目的是让生成的轨迹满足运动学约束。
AddEdgesKinematicsDiffDrive();
AddEdgesKinematicsDiffDrive函数的内容如下所示，对于每两个相邻坐标顶点构建 EdgeKinematicsDiffDrive对象， EdgeKinematicsDiffDrive是两顶点边，并且拥有雅克比计算的定义。
for (int i=0; i < teb_.sizePoses()-1; i++) // ignore twiced start only
{
EdgeKinematicsDiffDrive* kinematics_edge = new EdgeKinematicsDiffDrive;
kinematics_edge->setVertex(0,teb_.PoseVertex(i));
kinematics_edge->setVertex(1,teb_.PoseVertex(i+1));
kinematics_edge->setInformation(information_kinematics);
kinematics_edge->setTebConfig(*cfg_);
optimizer_->addEdge(kinematics_edge);
}
最后是添加朝向倾向性，用于使机器人在避障过程中倾向左侧还是右侧。
AddEdgesPreferRotDir();
AddEdgesPreferRotDir函数的内容如下，对于前三个坐标点，构建 EdgePreferRotDir对象，并给出左侧或右侧的倾向性。
for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i) // currently: apply to first 3 rotations
{
EdgePreferRotDir* rotdir_edge = new EdgePreferRotDir;
rotdir_edge->setVertex(0,teb_.PoseVertex(i));
rotdir_edge->setVertex(1,teb_.PoseVertex(i+1));
rotdir_edge->setInformation(information_rotdir);
if (prefer_rotdir_ == RotType::left)
rotdir_edge->preferLeft();
else if (prefer_rotdir_ == RotType::right)
rotdir_edge->preferRight();
optimizer_->addEdge(rotdir_edge);
}
至此，图的构建就完成了，接下来是对图进行优化，功能函数为 optimizeGraph。
success = optimizeGraph(iterations_innerloop, false);
在函数 optimizeGraph中，首先进行了一些验证，之后设置优化器属性
optimizer_->setVerbose(cfg_->optim.optimization_verbose);
optimizer_→initializeOptimization();
进行no_iterations次优化迭代，判断优化是否成功。
int iter = optimizer_→optimize(no_iterations);
图优化完成后，下一步是判断是否到达最后一次迭代，如果是，则计算当前损失
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;
重复上述建图、图优化、以及清除图的过程iterations_outerloop次，完成轨迹的优化。优化完成的轨迹保存在变量teb_中，在主函数中存在定时器CB_publishCycle函数不断读取teb_并进行发布。
————————————————
版权声明：本文为CSDN博主「flztiii」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/flztiii/article/details/121545662
