机器人控制算法——局部规划器TEB算法原理及C++可视化仿真

已于 2023-11-06 09:15:37 修改
阅读量3.1k 收藏 7
点赞数 6
分类专栏： 自动驾驶核心算法与代码 自动驾驶算法 文章标签： 机器人 算法 c++
于 2023-11-06 09:12:23 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/qq_40464599/article/details/134238696
版权
本文介绍了TEB算法的原理，将其视为一个带有时间弹性的优化问题，用于路径规划。重点讲解了如何在Ubuntu环境下使用C++进行TEB算法的可视化仿真，包括障碍物处理、全局规划点的生成以及两轮车模型的模拟。通过源码解析，展示了TEB算法如何处理控制指令和约束条件，提供了相关代码资源。

摘要生成于 C知道 ，由 DeepSeek-R1 满血版支持， 前往体验 >

1.背景介绍

最近一段时间，由于项目需要，一直在做TEB算法的工程化的工作，于是就考虑写下一篇系统些的文章，作为笔记，后续自己看也方便，TEB的英文名Time elastic band”，是一种局部规划器,它的核心思想是将路径规划问题转化为一个带有时间弹性的优化问题,通过对时间弹性的控制，来实现路径的优化。
TEB算法网上有很多的相关资料，本文主要侧重于如何在ubuntu系统下对C++开源的TEB算法进行可视化仿真，这件事是很有意义的，如果做纯理论算法的，可以通过此代码验证自己对TEB算法改进后的效果，如果要做工程化的产品，可用此代码研究TEB算法的不同的参数和算法的效果之间的关系，然后使其在真正的产品上调试出一组好的参数。那现在开始正文吧！

2.TEB算法原理

正如开头引言所介绍，TEB算法是一种局部规划器，它的核心思想是将路径规划问题转化为一个带有时间弹性的优化问题,通过对时间弹性的控制，来实现路径的优化。但是我不想这么去解释，如此解释，只会让人很疑惑。

问题一：那我们该如何理解这个TEB算法？

如下图公式（2），是TEB算法核心思想，从公式中，我们注意到

了解本专栏 订阅专栏 解锁全文 超级会员免费看
机器人规划算法——基于A-star和CostMap膨胀地图的全局规划（附Github源码链接）
qq_40464599的博客
03-29 1577
这段时间，笔者用c++写了一套机器人局部避障算法，并工程落地，机器人可以正常避障，避障所采用的技术方案是A*+TEB算法。并取得一定成果，后续还要针对复杂的场景继续优化。主要涉及到技术模块有：(1) costmap:代价地图模块，分为静态层和障碍物层以及膨胀层，静态层将读取地图信息并根据机器人内切半径进行膨胀，障碍物层是根据搭载的传感器检测到的障碍物点云并且以机器人内切圆半径进行膨胀。（2）全局规划器：这部分规划部分主要是A根据costmap规划一条路径。
FastPlanner后端轨迹优化算法—如何在2D地面机器人项目中将其工程落地？文中附源代码GitHub仓库
最新发布
qq_40464599的博客
05-11 111
本文介绍了作者在研究机器人动态避障过程中，对无人机领域的FastPlanner算法进行改进，并将其适配到二维移动机器人上的工作。FastPlanner算法主要包括运动动力学路径搜索、B样条轨迹优化和时间调整三个部分，作者重点研究了后端基于B样条的轨迹优化。由于原算法适用于三维无人机，作者通过修改点云数据，使其适用于二维移动机器人，并去除了ROS依赖，封装为纯C++接口。作者还简化了开源代码，仅保留了有符号距离场（ESDF）的局部地图和B样条轨迹优化模块，并使用C++ matplotlib进行可视化。最终，作
TEB算法详解（TebLocalPlannerROS::computeVelocityCommands（1））
YiYeZhiNian的博客
04-04 1万+
TebLocalPlannerROS::computeVelocityCommands作为teb算法与move_base算法的接口函数，是作为teb算法的重点部分去分析的，这一节先看该算法的前半部分，也就是局部路径规划前的数据处理部分，这部分算法的基本思路如下：
TEB（Time Elastic Band）局部路径规划算法详解及代码实现
qq_37705385的博客
06-30 8060
TEB局部路径规划算法详解
【ROS】TEB 规划器
AbaAbaxxx_的博客
04-18 1781
TEB 规划器是 ROS 导航中常用的一个局部路径规划器，它的名字叫“时间弹力带”。简单来说，它会在机器人前方的一小段路径上进行优化，让路径尽量避开障碍物、贴近全局路径，并考虑走这条路要花多少时间。“弹力带”就像一根橡皮筋，全局路径把它拉住，障碍物推开它，TEB 就在这些力的作用下调整路径形状；而“时间”则代表它还会选择最快能走完的路径。所以，TEB 规划器能让机器人走得既安全又高效，特别适合速度要求高的场景，比如竞速比赛。
TEB算法
☜@_@达奚黑雁的博客
10-21 7720
听说现在自动驾驶很火，所以我也做了一个 | 雷锋网 TEB算法流程_陈_呵呵的博客-优快云博客 Category: TEB算法 | 沉默杀手 TEB轨迹优化算法-代码解析与参数建议_zz123456zzss的博客-优快云博客_teb算法 浅谈Time Elastic Band - 知乎 ...
TEB算法原理与代码分析 详细文档+代码分析+matlab程序包
m0_73884566的博客
09-22 353
详细文档+代码分析+matlab程序包。TEB算法原理与代码分析。
TEB算法１－teb原理详解
ゃ﹏淸凨ゞ
05-03 1万+
TEB local planner 原论文：Trajectory modification considering dynamic constraints of autonomous robots TEB概述 TEB轨迹规划的主要流程
Timed-Elastic-Band局部路径规划算法
热门推荐
cabinx的博客
10-30 4万+
    早前做工程时尝试了teb局部规划算法，觉得效果非常好。由于时间关系，并未深入了解，仅依靠做对比实验调节作者给出的接口参数满足工程需求。抽空看了一下作者关于teb算法的论文，记录一些笔记。     teb局部路径规划算法github地址：https://github.com/rst-tu-dortmund/teb_local_planner。     作者列出的几篇文章均推荐阅读了解。本...
规控算法工程师的技术图谱和学习路径
专注Java（全栈）应用开发，求知若渴，虚心若愚，talk is cheap, show me the code.
02-27 164
规控算法工程师（规划与控制算法工程师）是自动驾驶领域的核心岗位之一，涉及路径规划、行为决策、运动控制等多个技术模块。以下为技术图谱与学习路径的整合，结合行业需求和技术发展趋势。规控算法工程师需要理论深度与工程能力的结合，建议通过项目实践逐步积累经验，并持续跟踪行业技术演进。规控算法工程师技术图谱与学习路径。进阶阶段（6-12个月）基础阶段（3-6个月）一、技术图谱核心模块。深化阶段（1-2年）
0基础学习ros（机器人操作系统）
qq_56908984的博客
09-26 647
Hector Mapping是一种无需里程计数据的SLAM（Simultaneous Localization and Mapping，即同时定位与地图构建）方法，它主要利用激光雷达（LIDAR）来获得二维姿态估计。运行rosrun hector_mapping hector_mapping出现错误：（不过不影响后续操作）
teb_local_planner.zip
07-18
一种ROS本地规划器，支持ackermann底盘. 使用g2o近似实现各种约束(速度，加速度，最小转弯半径，非全向轮)，使得时间最短。支持闭环控制，类似模型预测控制（MPC）。
对ROS局部运动规划器Teb的理解
01-07
TEB算法过程 算法原理方面可参考下面的博文： 《Trajectory modification considering dynamic constraints of autonomous robots》理解 TEB与DWA对比： teb在运动过程中会调整自己的位姿朝向，当到达目标点时，通常机器人的朝向也是目标朝向而不需要旋转。dwa则是先到达目标坐标点，然后原地旋转到目标朝向。对于两轮差速底盘，teb在运动中调节朝向会使运动路径不流畅，在启动和将到达目标点时出现不必要的后退。这在某些应用场景里是不允许的。因为后退可能会碰到障碍物。而原地旋转到合适的朝向再径直走开是更为合适的运动策略。这也是
算法学习——TEB算法
小墨迹的博客
02-28 3774
TEB 算法通过引入时间信息和弹性带的概念，结合动力学约束和环境约束，生成平滑且可行的路径。其核心在于优化问题的构建和求解，通过合理设置约束条件和调节参数，可以实现高效的局部路径规划。
TEB算法原理与代码分析，详细文档和代码分析以及matlab程序包
m0_70989251的博客
05-15 1527
TEB算法原理与代码分析，详细文档和代码分析以及matlab程序包
TEB算法原理与代码分析 实现了一个路径规划算法，用于寻找从起点到终点的最优路径。它考虑了速度约束、运动学约束和障碍物避障，并使用优化算法来搜索最优解
2301_78782294的博客
06-18 1475
总结一下，这段代码实现了一个路径规划算法，用于寻找从起点到终点的最优路径。它根据起点和终点的位置，以及N的数量，计算了中间状态顶点的位置和朝向，并将它们存储在x0中。它使用plot函数绘制了起点、中间状态顶点和终点的位置，使用quiver函数绘制了起点和中间状态顶点的朝向。首先，代码定义了起点和终点的位置，以及障碍物的位置（如果有）。然后，它设置了一些参数，如路径中的中间状态顶点数量N、最大速度MAX_V和时间步长dT。它使用了优化算法来寻找从起点到终点的最优路径，考虑了速度约束、运动学约束和障碍物避障。
TEB算法详解（TebLocalPlannerROS::computeVelocityCommands（3））
YiYeZhiNian的博客
04-16 3098
第一章主要分析了teb算法的准备条件，包括获取当前位姿与速度、对全局路径的裁减以获取局部路径等以及局部地图的获取等。第二章主要讲述了在获取前置条件后，如何根据前置条件进行位姿优化，teb的路径优化主要是调用了g2o优化算法，以全局路径的点以及理论上点与点之间的运动时间作为g2o的优化顶点，以距离障碍物的距离、机器人最大运动速度、点与点之间的最短路径等约束条件作为g2o的边对局部路径规划的姿态进行了优化。这一章则是主要看一下算法怎么从优化后的点位到运动输出之间的关系。
TEB算法原理与代码分析详细文档+代码分析+matlab程序包
m0_71051138的博客
05-17 1466
TEB算法原理与代码分析详细文档+代码分析+matlab程序包 YID:6979654252546507
TEB局部路径规划算法可视化
02-06
### TEB局部路径规划算法可视化实现 TEB（Timed Elastic Band）局部路径规划算法的可视化可以通过ROS中的rviz工具来完成。为了展示TEB的具体工作过程以及优化后的路径，可以利用`teb_local_planner`包提供的功能。 #### 启动与配置RVIZ环境 要使TEB的结果能够在RVIZ中显示出来，需先启动带有导航栈的机器人模拟器，并加载必要的参数文件： ```bash roslaunch teb_local_planner_tutorials robot_diff_drive.launch ``` 这会自动打开一个预设好的RVIZ界面，在该界面上可以看到机器人的当前位置、传感器数据以及由TEB计算得到的最佳路径。 #### 显示路径和其他信息 为了让用户更好地理解TEB的工作原理及其决策依据，可以在RVIZ里添加多个Display Types: - **Path**: 使用Topic `/move_base/TebLocalPlannerROS/teb_poses`订阅并绘制出当前最优解对应的位姿序列。 - **MarkerArray (Pose Sequence)**: Topic为`/move_base/TebLocalPlannerROS/visualization_marker_array`, 这个主题发布了一系列标记数组，其中包含了所有候选轨迹的信息，有助于分析不同阶段下的探索情况。 - **Costmap2D**: 订阅话题如`/move_base/global_costmap/costmap` 和 `/move_base/local_costmap/costmap`. 成本地图对于了解障碍物分布至关重要，同时也反映了TEB如何避开这些区域寻找安全路线。 通过上述设置，不仅能看到最终选定的一条平滑曲线代表的目标路径[^2]，还能直观感受到整个求解过程中各个可能的选择是如何被评估和筛选出来的。 #### Python代码示例：获取并处理来自TEB的数据 如果希望进一步定制化视觉效果或者与其他组件交互，则可通过编写Python脚本来访问相关消息队列。下面是一个简单的例子，展示了怎样接收并解析来自TEB的消息： ```python import rospy from geometry_msgs.msg import PoseStamped, PoseArray from visualization_msgs.msg import MarkerArray def pose_callback(data): poses = [] for p in data.poses: position = f"x={p.position.x}, y={p.position.y}" orientation = ( f"w={p.orientation.w}, " f"x={p.orientation.x}, " f"y={p.orientation.y}, " f"z={p.orientation.z}" ) print(f"Received pose with Position({position}) and Orientation({orientation})") if __name__ == '__main__': try: rospy.init_node('teb_visualizer', anonymous=True) # Subscribe to the topic that publishes optimized path as a set of poses. rospy.Subscriber("/move_base/TebLocalPlannerROS/teb_poses", PoseArray, pose_callback) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): rate.sleep() except rospy.ROSInterruptException: pass ``` 此程序监听到了由TEB发布的姿态集合，并将其转换成易于阅读的形式打印到控制台。当然也可以根据实际需求修改逻辑，比如保存至文件或将结果发送给其他节点进行后续处理[^3].
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Jack Ju

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值