一、题外话
经济的持续低迷让一线打工者们情绪焦虑、对未来丧失信心，导致保守消费；企业端也是想着降本增效，裁员收缩。而在主流经济界有两种拉动经济的方式，第一是通过生产拉动经济、第二是通过消费拉动经济，毫无疑问我国的现状更偏向于前者，汽车行业的卷就是最好的印证，但无论如何，提升国民信心才是当务之急。
提升个人信心的最好方式就是 终身学习，提升自己的眼界，锻炼极强的适应能力。
先看效果：前面白色的线条就是需要优化的局部全局路径
TEB演示demo
二、源码解读
1、算法原理的理解
TEB全称Time Elastic Band（时间弹性带），该方法通对全局轨迹进行修正，从而优化机器人的局部运动轨迹，属于局部路径规划。在轨迹优化过程中，该算法拥有多种优化目标，包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动力学、运动学以及几何约束的符合性。
它和MPC同属于优化方法，只不过TEB计算最优的轨迹，再通过计算得到最优控制量，而MPC可以理解为直接计算最优控制量；MPC使用OSPQ优化器求解，TEB使用g2o来求解。
不重复造轮子，先直接推荐几篇比较好的博客
TEB概念理解：https://www.leiphone.com/category/transportation/0TCtaBOIcFOIBN69.html
TEB参数理解：https://blog.csdn.net/weixin_44917390/article/details/107568507
TEB论文翻译：http://t.csdnimg.cn/FJIww
TEB算法理解：https://blog.csdn.net/xiekaikaibing/article/details/83417223、 https://blog.csdn.net/flztiii/article/details/121545662
TEB源码地址：https://github.com/rst-tu-dortmund/teb_local_planner
2、源码解读
1、进行初始化：teb参数，障碍物，机器人模型，全局路径点
cpp
复制代码
TebOptimalPlanner::TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, const ViaPointContainer* via_points)
{
initialize(cfg, obstacles, robot_model, visual, via_points);
}
2、initOptimizer函数进行优化器构造，在函数中，首先是注册自定义的顶点和边，之后构造了一个线性方程求解器 linear_solver，求解器使用的是g2o::LinearSolverCSparse，在此求解器基础上，定义了类型为g2o::BlockSolver的 block_solver。在此基础上，又定义了LM算法的优化器g2o::OptimizationAlgorithmLevenberg，最终得到稀疏优化器 g2o::SparseOptimizer。并且定义优化器可以以多线程形式进行。
cpp
复制代码
void TebOptimalPlanner::initialize(const TebConfig& cfg, ObstContainer* obstacles,
RobotFootprintModelPtr robot_model,
const ViaPointContainer* via_points) {
// 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;
vel_start_.first = true;
vel_start_.second.linear.x() = 0;
vel_start_.second.linear.y() = 0;
vel_start_.second.angular.z() = 0;
vel_goal_.first = true;
vel_goal_.second.linear.x() = 0;
vel_goal_.second.linear.y() = 0;
vel_goal_.second.angular.z() = 0;
initialized_ = true;
}
cpp
复制代码
boost::shared_ptr
// 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
std::unique_ptr
new TEBLinearSolver()); // see typedef in optimization.h
linear_solver->setBlockOrdering(true);
std::unique_ptr
g2o::OptimizationAlgorithmLevenberg* solver =
new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));
optimizer->setAlgorithm(solver);
optimizer->initMultiThreading(); // required for >Eigen 3.1
return optimizer;
}
3、registerG2OTypes函数中注册顶点和边
cpp
复制代码
void TebOptimalPlanner::registerG2OTypes() {
g2o::Factory* factory = g2o::Factory::instance();
factory->registerType("VERTEX_POSE",
std::make_shared
factory->registerType("VERTEX_TIMEDIFF",
std::make_shared
factory->registerType("EDGE_TIME_OPTIMAL",
std::make_shared
factory->registerType("EDGE_SHORTEST_PATH",
std::make_shared
factory->registerType("EDGE_VELOCITY",
std::make_shared
factory->registerType("EDGE_VELOCITY_HOLONOMIC",
std::make_shared
factory->registerType("EDGE_ACCELERATION",
std::make_shared
factory->registerType("EDGE_ACCELERATION_START",
std::make_shared
factory->registerType("EDGE_ACCELERATION_GOAL",
std::make_shared
factory->registerType(
"EDGE_ACCELERATION_HOLONOMIC",
std::make_shared
factory->registerType(
"EDGE_ACCELERATION_HOLONOMIC_START",
std::make_shared
factory->registerType(
"EDGE_ACCELERATION_HOLONOMIC_GOAL",
std::make_shared
factory->registerType(
"EDGE_KINEMATICS_DIFF_DRIVE",
std::make_shared
factory->registerType("EDGE_KINEMATICS_CARLIKE",
std::make_shared
factory->registerType("EDGE_OBSTACLE",
std::make_shared
factory->registerType("EDGE_INFLATED_OBSTACLE",
std::make_shared
factory->registerType("EDGE_DYNAMIC_OBSTACLE",
std::make_shared
factory->registerType("EDGE_VIA_POINT",
std::make_shared
factory->registerType("EDGE_PREFER_ROTDIR",
std::make_shared
}
4、规划函数plan。先判断teb是否初始化完成，如果没有初始化，则进行初始化， initTrajectoryToGoal功能是生成从起点到终点的连线，并在连线上进行采样，填充teb中的顶点。如果已经初始化过了，则判断teb_中顶点数量是否为0，如果不为0且终点没有发生巨大变化，则使用updateAndPruneTEB函数添加顶点，反之则重新初始化顶点。最后调用优化函数optimizeTEB进行优化。
cpp
复制代码
bool TebOptimalPlanner::plan(const std::vector
bool free_goal_vel) {
if (!teb_.isInit()) {
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);
} 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_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
else // goal too far away -> reinit
{
printf(
"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);
}
}
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);
}
5、optimizeTEB函数中 构建图，具体过程可阅读源码
cpp
复制代码
bool TebOptimalPlanner::buildGraph(double weight_multiplier) {
if (!optimizer_->edges().empty() || !optimizer_->vertices().empty()) {
// ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!");
return false;
}
// add TEB vertices
AddTEBVertices();
// add Edges (local cost functions)
if (cfg_->obstacles.legacy_obstacle_association)
AddEdgesObstaclesLegacy(weight_multiplier);
else
AddEdgesObstacles(weight_multiplier);
if (cfg_->obstacles.include_dynamic_obstacles) AddEdgesDynamicObstacles();
AddEdgesViaPoints();
AddEdgesVelocity();
AddEdgesAcceleration();
AddEdgesTimeOptimal();
AddEdgesShortestPath();
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.
AddEdgesPreferRotDir();
return true;
}
6、图的构建完成后就对图进行优化，函数 optimizeGraph
先设置优化器属性，然后进行迭代
cpp
复制代码
bool TebOptimalPlanner::optimizeGraph(int no_iterations, bool clear_after) {
if (cfg_->robot.max_vel_x < 0.01) {
// ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing
// aborted...");
if (clear_after) clearGraph();
return false;
}
if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples) {
// ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping
// optimization.");
if (clear_after) clearGraph();
return false;
}
optimizer_->setVerbose(cfg_->optim.optimization_verbose);
optimizer_->initializeOptimization();
int iter = optimizer_->optimize(no_iterations);
// Save Hessian for visualization
// g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast
// (optimizer_->solver()); lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt");
if (!iter) {
// ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);
return false;
}
if (clear_after) clearGraph();
return true;
}
7、优化成功后，更新目标函数权重。组后优化完成的轨迹保存在变量teb_中。
cpp
复制代码
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;
8、调用getVelocityCommand函数获取控制指令
cpp
复制代码
bool TebOptimalPlanner::getVelocityCommand(float& vx, float& vy, float& omega,
int look_ahead_poses) const {
if (teb_.sizePoses() < 2) {
// ROS_ERROR("TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2
// poses. Make sure to init and optimize/plan the trajectory fist.");
vx = 0;
vy = 0;
omega = 0;
return false;
}
look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));
double dt = 0.0;
for (int counter = 0; counter < look_ahead_poses; ++counter) {
dt += teb_.TimeDiff(counter);
if (dt >= cfg_->trajectory.dt_ref *
look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory?
{
look_ahead_poses = counter + 1;
break;
}
}
if (dt <= 0) {
// ROS_ERROR("TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!");
vx = 0;
vy = 0;
omega = 0;
return false;
}
// Get velocity from the first two configurations
extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);
return true;
}
至此，结束。
三、总结
1、优化的局部轨迹越长，TEB耗时越长，如果太短，容易陷入局部最优。全局规划器和局部规划器的配合才是完美的解决途径。
2、相比于DWA，TEB的优势是优化轨迹，机器人运动更加丝滑，这都是以庞大的计算作为代价的。
3、TEB的参数很多，实际工程应用需要对每个参数有深刻的理解。
4、花了很多的精力在源码的阅读及剥离ROS上。
5、对于优化器的理解还不透彻，底层是数学问题。