TEB（Time Elastic Band）局部路径规划算法详解及代码实现

一、题外话

经济的持续低迷让一线打工者们情绪焦虑、对未来丧失信心，导致保守消费；企业端也是想着降本增效，裁员收缩。而在主流经济界有两种拉动经济的方式，第一是通过生产拉动经济、第二是通过消费拉动经济，毫无疑问我国的现状更偏向于前者，汽车行业的卷就是最好的印证，但无论如何，提升国民信心才是当务之急。

提升个人信心的最好方式就是 终身学习，提升自己的眼界，锻炼极强的适应能力。

先看效果：前面白色的线条就是需要优化的局部全局路径

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参数，障碍物，机器人模型，全局路径点

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。并且定义优化器可以以多线程形式进行。

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;
}

boost::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer() {// Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)static boost::once_flag flag = BOOST_ONCE_INIT;boost::call_once(&registerG2OTypes, flag);// allocating the optimizerboost::shared_ptr<g2o::SparseOptimizer> optimizer = boost::make_shared<g2o::SparseOptimizer>();std::unique_ptr<TEBLinearSolver> linear_solver(new TEBLinearSolver()); // see typedef in optimization.hlinear_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.1return optimizer;
}

3、registerG2OTypes函数中注册顶点和边

void TebOptimalPlanner::registerG2OTypes() {g2o::Factory* factory = g2o::Factory::instance();factory->registerType("VERTEX_POSE",std::make_shared<g2o::HyperGraphElementCreator<VertexPose>>());factory->registerType("VERTEX_TIMEDIFF",std::make_shared<g2o::HyperGraphElementCreator<VertexTimeDiff>>());factory->registerType("EDGE_TIME_OPTIMAL",std::make_shared<g2o::HyperGraphElementCreator<EdgeTimeOptimal>>());factory->registerType("EDGE_SHORTEST_PATH",std::make_shared<g2o::HyperGraphElementCreator<EdgeShortestPath>>());factory->registerType("EDGE_VELOCITY",std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocity>>());factory->registerType("EDGE_VELOCITY_HOLONOMIC",std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocityHolonomic>>());factory->registerType("EDGE_ACCELERATION",std::make_shared<g2o::HyperGraphElementCreator<EdgeAcceleration>>());factory->registerType("EDGE_ACCELERATION_START",std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationStart>>());factory->registerType("EDGE_ACCELERATION_GOAL",std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationGoal>>());factory->registerType("EDGE_ACCELERATION_HOLONOMIC",std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomic>>());factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START",std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicStart>>());factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL",std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicGoal>>());factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE",std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsDiffDrive>>());factory->registerType("EDGE_KINEMATICS_CARLIKE",std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsCarlike>>());factory->registerType("EDGE_OBSTACLE",std::make_shared<g2o::HyperGraphElementCreator<EdgeObstacle>>());factory->registerType("EDGE_INFLATED_OBSTACLE",std::make_shared<g2o::HyperGraphElementCreator<EdgeInflatedObstacle>>());factory->registerType("EDGE_DYNAMIC_OBSTACLE",std::make_shared<g2o::HyperGraphElementCreator<EdgeDynamicObstacle>>());factory->registerType("EDGE_VIA_POINT",std::make_shared<g2o::HyperGraphElementCreator<EdgeViaPoint>>());factory->registerType("EDGE_PREFER_ROTDIR",std::make_shared<g2o::HyperGraphElementCreator<EdgePreferRotDir>>());
}

4、规划函数plan。先判断teb是否初始化完成，如果没有初始化，则进行初始化， initTrajectoryToGoal功能是生成从起点到终点的连线，并在连线上进行采样，填充teb中的顶点。如果已经初始化过了，则判断teb_中顶点数量是否为0，如果不为0且终点没有发生巨大变化，则使用updateAndPruneTEB函数添加顶点，反之则重新初始化顶点。最后调用优化函数optimizeTEB进行优化。

bool TebOptimalPlanner::plan(const std::vector<PoseStamped>& initial_plan, const Twist* start_vel,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 TEBelse // 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();elsevel_goal_.first = true; // we just reactivate and use the previously set velocity (should be// zero if nothing was modified)// now optimizereturn optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
}

5、optimizeTEB函数中 构建图，具体过程可阅读源码

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 verticesAddTEBVertices();// add Edges (local cost functions)if (cfg_->obstacles.legacy_obstacle_association)AddEdgesObstaclesLegacy(weight_multiplier);elseAddEdgesObstacles(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 robotelseAddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded// from below.AddEdgesPreferRotDir();return true;
}

6、图的构建完成后就对图进行优化，函数 optimizeGraph
先设置优化器属性，然后进行迭代

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<g2o::OptimizationAlgorithmLevenberg*>//  (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_中。

        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 iterationcomputeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);clearGraph();weight_multiplier *= cfg_->optim.weight_adapt_factor;

8、调用getVelocityCommand函数获取控制指令

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 configurationsextractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);return true;
}

至此，结束。

三、总结

1、优化的局部轨迹越长，TEB耗时越长，如果太短，容易陷入局部最优。全局规划器和局部规划器的配合才是完美的解决途径。
2、相比于DWA，TEB的优势是优化轨迹，机器人运动更加丝滑，这都是以庞大的计算作为代价的。
3、TEB的参数很多，实际工程应用需要对每个参数有深刻的理解。
4、花了很多的精力在源码的阅读及剥离ROS上。
5、对于优化器的理解还不透彻，底层是数学问题。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/pingmian/38101.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

android 定时调用方法

android 定时调用方法

在Android中&#xff0c;可以使用Handler类和Runnable接口来实现定时调用方法。以下是一个简单的例子&#xff0c;展示了如何每隔一定时间调用一个方法。 import android.os.Handler; import android.os.SystemClock; import androidx.appcompat.app.AppCompatActivity; impor…
阅读更多...
【Python系列】Python 项目 Docker 部署指南

【Python系列】Python 项目 Docker 部署指南

&#x1f49d;&#x1f49d;&#x1f49d;欢迎来到我的博客&#xff0c;很高兴能够在这里和您见面&#xff01;希望您在这里可以感受到一份轻松愉快的氛围&#xff0c;不仅可以获得有趣的内容和知识&#xff0c;也可以畅所欲言、分享您的想法和见解。 推荐:kwan 的首页,持续学…
阅读更多...
Linux多进程和多线程(三)进程间通讯-信号处理方式和自定义处理函数

Linux多进程和多线程(三)进程间通讯-信号处理方式和自定义处理函数

进程间通信之信号 信号信号的种类 信号在操作系统中的定义如下: 信号的处理流程在 Linux 中对信号的处理⽅式 自定义信号处理函数 信号的发送 kill() 函数:raise() 函数: 示例 : 创建⼀个⼦进程&#xff0c;⼦进程通过信号暂停&#xff0c;⽗进程发送 终⽌信号等待信号 pause()…
阅读更多...
风控图算法之社群发现算法（小数据集Python版）

风控图算法之社群发现算法（小数据集Python版）

风控图算法之社群发现算法&#xff08;小数据集Python版&#xff09; 在风险控制领域&#xff0c;图算法扮演着日益重要的角色。&#xff08;这方面的资料有很多&#xff0c;不再赘述&#xff09; 图算法在风控场景的应用 图分析方法在业务风控中的应用 特别是社群发现算法&a…
阅读更多...
基于pytorch实现的 MobileViT 的图像识别（迁移学习）

基于pytorch实现的 MobileViT 的图像识别（迁移学习）

1、介绍 MobileViT 轻量级的分类识别网络&#xff0c;结合了CNN卷积和Transformer 混合的网络架构 关于更多介绍可以自行百度&#xff0c;本文通过pytorchpython进行实现 更多基础的图像分类网络&#xff0c;参考&#xff1a;图像分类_听风吹等浪起的博客-CSDN博客 2、相关代…
阅读更多...
npm简介与安装方法/管理与依赖管理/脚本命令与自定义

npm简介与安装方法/管理与依赖管理/脚本命令与自定义

npm简介与安装方法 什么是npm&#xff1f; npm&#xff08;Node Package Manager&#xff09;是Node.js的包管理工具和包管理系统&#xff0c;广泛用于JavaScript生态系统中。npm主要有以下几个功能&#xff1a; 包管理器&#xff1a;安装、更新、卸载Node.js包。项目管理工…
阅读更多...
DB-GPT 文档切分报错

DB-GPT 文档切分报错

感谢阅读 配置完知识库&#xff0c;进行切分报错切分完成后&#xff0c;进行问答时后台日志报错 配置完知识库&#xff0c;进行切分报错 报的错如下 document sync error cryptography>3.1 is required for AES algorithm pip install -U cryptography 之后重新运行程序 …
阅读更多...
SimpleService 一个简单的Windows Service定时服务

SimpleService 一个简单的Windows Service定时服务

SimpleService 介绍 一个简单的Windows Service定时服务 SimpleService: 一个简单的Windows Service定时服务 使用说明
阅读更多...
kimi AI,文生流程图，一句话kimi当场出图

kimi AI,文生流程图，一句话kimi当场出图

kimi AI善于读长文、搜网页&#xff0c;主打功能包括整理资料、解读文件、辅助编程、文案写作等。 今天我们就让kimi帮我们一键生成流程图&#xff0c;再也不用自己画了&#xff1a; 不看广告看疗效&#xff1a; 告诉kimi使用Mermaid直接生成流程图&#xff0c;kimi直接生成…
阅读更多...
AI是如何与快充技术结合的？

AI是如何与快充技术结合的？

针对AI技术在快充领域的运用&#xff0c;我们可以进一步深入探讨AI如何与快充技术结合&#xff0c;提升充电效率和用户体验。以下是一些具体的AI技术在快充领域的应用场景&#xff1a; 一、智能充电算法 学习充电模式&#xff1a;AI算法可以学习用户的充电习惯&#xff0c;比…
阅读更多...
容器技术-docker4

容器技术-docker4

一、docker资源限制 在使用 docker 运行容器时&#xff0c;一台主机上可能会运行几百个容器&#xff0c;这些容器虽然互相隔离&#xff0c;但是底层却使用着相同的 CPU、内存和磁盘资源。如果不对容器使用的资源进行限制&#xff0c;那么容器之间会互相影响&#xff0c;小的来说…
阅读更多...
获取键盘事件的keyCode属性

获取键盘事件的keyCode属性

获取键盘事件的keyCode属性 大家好&#xff0c;我是免费搭建查券返利机器人省钱赚佣金就用微赚淘客系统3.0的小编&#xff0c;也是冬天不穿秋裤&#xff0c;天冷也要风度的程序猿&#xff01;在本文中&#xff0c;我们将深入探讨在Java中如何获取键盘事件的keyCode属性。键盘事…
阅读更多...
gin 服务端无法使用sse流式nginx配置

gin 服务端无法使用sse流式nginx配置

我在本地使用 gin 可以流式的将大模型数据传递给前端。但是当我部署到服务器中时&#xff0c;会阻塞一段时间&#xff0c;然后显示一大段文本。 起初我怀疑是gin 没有及时将数据刷到管道中&#xff0c;但是经过测试&#xff0c;还是会阻塞。 c.Writer.(http.Flusher).Flush()最…
阅读更多...
（超详细）数据结构——“栈”的深度解析

（超详细）数据结构——“栈”的深度解析

前言&#xff1a; 在前几章我们介绍了线性表的基本概念&#xff0c;也讲解了包括顺序表&#xff0c;单链表&#xff0c;双向链表等线性表&#xff0c;相信大家已经对线性表比较熟悉了&#xff0c;今天我们要实现线性表的另一种结构——栈。 1.栈的概念 栈&#xff1a;一种特殊…
阅读更多...
【Docker】存储数据卷

【Docker】存储数据卷

目录 1、挂载数据卷到容器里 2、查询挂载文件 3、容器与主机之间映射共享卷 4、三个容器之间使用共享卷 5、卷数据的备份与恢复 5.1 备份 5.2 恢复 1、挂载数据卷到容器里 docker run -itd --name test02 -v /data nginx docker exec -it test02 bashls / docker inspe…
阅读更多...
如何对C++代码进行性能调优

如何对C++代码进行性能调优

对C代码进行性能调优是一个涉及多个方面的过程&#xff0c;包括代码优化、数据结构设计、算法选择、内存管理、并行化等多个方面。以下是一些常用的C性能调优技巧&#xff1a; 代码优化&#xff1a; 避免不必要的复制&#xff1a;使用引用或指针传递大型对象或数据结构。常量优…
阅读更多...
解决IDEA的Web项目右键无法创建Servlet问题

解决IDEA的Web项目右键无法创建Servlet问题

右键新建没有servlet? 在pom.xml文件中需要导入servlet依赖&#xff0c;很简单的&#xff0c;别担心&#xff0c;就20秒解决 看我操作&#xff01;&#xff01;&#xff01; 1. 找到自动生成的pom.xml文件 只要你创建了maven项目&#xff0c;就会自动生成pom.xml文件&#xf…
阅读更多...
ESP32-C3模组上跑通MQTT（7）—— tcp例程（2）

ESP32-C3模组上跑通MQTT（7）—— tcp例程（2）

接前一篇文章：ESP32-C3模组上跑通MQTT（6）—— tcp例程（1） 《ESP32-C3 物联网工程开发实战》 一分钟了解MQTT协议 ESP32 MQTT API指南-CSDN博客 ESP-IDF MQTT 示例入门_mqtt outbox-CSDN博客 ESP32用自签CA进行MQTT的TLS双向认证通信_esp32 mqtt ssl-CSDN博客 特此致谢…
阅读更多...
qiankun微前端：qiankun+vite+vue3+ts（未完待续..）

qiankun微前端：qiankun+vite+vue3+ts（未完待续..）

目录 什么是微前端 目前现有的微前端 好处 使用 子应用的页面在主应用里显示 什么是微前端 微前端是一种多个团队通过独立发布功能的方式来共同构建现代化 web 应用的技术手段及方法策略。 我的理解就是将一个大型的前端应用拆分成多个模块&#xff0c;每个微前端模块可以由…
阅读更多...
目标检测的常用算法和框架

目标检测的常用算法和框架

一、常见算法 下面介绍几种常见的目标检测算法： Haar特征+级联分类器：该算法使用Haar特征作为特征提取器，并通过级联分类器来检测目标。这种算法运行速度快，在处理实时视频时表现良好，但对于复杂场景的目标检测效果可能不理想。 HOG特征+SVM：该算法使用方向梯度直方图（…
阅读更多...
最新文章

Copyright @ 2022~2023