对ROS局部运动规划器Teb的理解
TEB算法过程
算法原理方面可参考下面的博文:
《Trajectory modification considering dynamic constraints of autonomous robots》理解
TEB与DWA对比:
teb在运动过程中会调整自己的位姿朝向,当到达目标点时,通常机器人的朝向也是目标朝向而不需要旋转。dwa则是先到达目标坐标点,然后原地旋转到目标朝向。对于两轮差速底盘,teb在运动中调节朝向会使运动路径不流畅,在启动和将到达目标点时出现不必要的后退。这在某些应用场景里是不允许的。因为后退可能会碰到障碍物。而原地旋转到合适的朝向再径直走开是更为合适的运动策略。这也是teb需要根据场景要优化的地方。
上图为在到达终点前调整朝向的路径。(两轮差速模型下)
上图为从起点出发时调整朝向的路径。(两轮差速模型下)
TEB参数特性
enable_homotopy_class_planning参数
当enable_homotopy_class_planning = False,规划的局部路径会陷入局部最小值。因为路径向上面障碍物或下面障碍物偏,总体的代价值都会增加(避障的代价函数是离障碍物越近代价值越大),只有往中间走总体代价值才最低。
当enable_homotopy_class_planning = True,teb会同时搜寻多条路径并选取一条更可行的。
global_plan_overwrite_orientation 参数
全局路径规划生成路径时会调用OrientationFilter类的processPath方法给全局路径中的每一个点分配一个方向角。
当TEB的配置参数global_plan_overwrite_orientation = True时,teb_local_planner 规划局部路径时会覆盖掉全局路径点的方位角。
当global_plan_overwrite_orientation=false,teb_local_planner会使用全局路径点的方位角。
路径中方位角的不同会极大的影响机器人的运动方式,不同的场景需要的不同的运动方式。比如Forward 模式可以适用于大部分场景,但有些狭小的地方,目标点在当前位置的正后方,则Backward 模式就更合适了。
对HomotopyClassPlanner类的理解
TEB工程里可以使用两种类实例化局部规划器。
// create the planner instanceif (cfg_.hcp.enable_homotopy_class_planning){planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));ROS_INFO("Parallel planning in distinctive topologies enabled.");}else{planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));ROS_INFO("Parallel planning in distinctive topologies disabled.");}
其中TebOptimalPlanner类是单一的TEB规划器。使用全局规划器生成的初始轨迹来初始化TEB局部规划器。HomotopyClassPlanner类像是多个TebOptimalPlanner类实例的组合。HomotopyClassPlanner类中也会实例化一个由全局规划器生成的路径作为参考的对象。除此之外,它还会使用probabilistic roadmap (PRM) methods在障碍物周边采样一些keypoints,将这些keypoints连接起来,去除方向没有朝向目标点的连接和与障碍物重叠的连接。这样就形成了一个网络,然后将起始点和终点接入到这个网络。如下图所示:
使用Depth First Search(深度优先方法)搜索所有可行的路径。将这些路径作为参考,实例化多个TebOptimalPlanner类的实例。采用多线程并行优化,得到多条优化后的路径。将这些路径进行可行性分析,选出代价值最小的最优路径。不得不说HomotopyClassPlanner类里的方法是一个鲁棒性和可靠性更高的方法。因为单一的TEB规划 (TEB without homology class exploration)在某些场景会陷入局部最小值,可能出现卡死的情况。
参考材料:
S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
下面的论文详细讲解了HomotopyClassPlanner类是如何实现的。
C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015.
可尝试的改进
起始点的旋转
TEB局部规划时,当起始点的机器人朝向没有朝向目标点,规划出的路线会考虑倒车旋转来把朝向修正过来。但在一些场景这样的倒退和修正角度的挪动是不允许的。所以针对两轮差速的运动模型,可以先让底盘原地旋转,使其朝向目标点。然后再执行优化函数,得到最优轨迹。这样能省去一些不必要的朝向调整的轨迹操作。
终点的旋转
先将终点的机器人朝向缓存下来。然后可以使用起始点向终点的朝向覆盖掉终点的朝向。开启优化操作。当机器人已经到达目标点时,再对比此时朝向与缓存的目标朝向。当朝向有差异时,原地旋转到目标朝向即可。这样也是为了省去一些不必要的朝向调整的轨迹操作。
根据前方障碍物距离调整最大线速度
在updateObstacleContainerWithCostmap()函数中,会根据costmap中的障碍物信息更新障碍物容器。在这里,可以找到前方距离障碍物的最短距离。然后根据该距离值调整max_vel_x。这样优化时最大线速度的约束会随着障碍物的远近而调整。在一定程度上会改善避障效果。
防止过冲
有时机器人会冲过全局目标点,然后又回转。这样的现象肯定是影响效果的。所以可以根据机器人当前位置距离全局目标点的距离来调整max_vel_x。距离全局目标点远时可以把max_vel_x放大一点,距离全局目标点近时可以把max_vel_x放小一点。我们可以设计一个公式来做这件事。输入为与全局目标点的距离,输出为max_vel_x。这样可以防止过冲现象。
增加自己的边约束
模仿TEB工程中的edge头文件,实现自己基于自定义约束的edge,这样就可以根据使用场景实现自己的优化效果。
参考材料
下面的博文对代码中主要函数的作用做了描述
https://blog.csdn.net/qq_41986495/article/details/87892108
https://blog.csdn.net/qq_41986495/article/details/85060538
关注公众号《首飞》回复“机器人”获取精心推荐的C/C++,Python,Docker,Qt,ROS1/2等机器人行业常用技术资料。
对ROS局部运动规划器Teb的理解相关推荐
- ROS局部运动规划器Teb/DWA
算法原理方面可参考下面的博文: <Trajectory modification considering dynamic constraints of autonomous robots> ...
- ros局部路径规划器dwa
ROS的路径规划器分为全局路径和局部路径规划,其中局部路径规划器使用的最广的为dwa,个人理解为: 首先全局路径规划会生成一条大致的全局路径,局部路径规划器会把全局路径给分段,然后根据分段的全局路径的 ...
- 局部路径规划器teb_local_planner详解3:跟随全局planner
局部路径规划器teb_local_planner详解3:跟随全局planner 前言 一.相关优化项 二.与topologies相关的参数 前言 在局部路径规划器teb_local_planner详解 ...
- 局部路径规划器teb_local_planner详解6:关于动态障碍物与常见问题
前言 在局部路径规划器teb_local_planner详解5:关于car-like robots中,我们介绍了与car-like机器人模型相关的参数,说明了在对接底盘的时候需要注意底盘接收的控制指令 ...
- 局部路径规划器teb_local_planner详解5:关于 robots models
前言 局部路径规划器teb_local_planner详解4:关于代价地图中,我们介绍了如何使用costmap_converter 本章,我们来详细看下,对于不同的robot model ,teb是如 ...
- 局部路径规划器DWAplannerros
1 体系结构 (1)主要成员 base_local_planner::LocalPlannerUtil planner_util_; 用来存储运动控制参数以及costmap2d.tf等,会被传入dp_ ...
- 局部路径规划器的实现:有限状态机+conformal lattice
局部路径规划(Local Motion planning) 1. 路径生成 Path generation 2. 碰撞检测 Path collision checking 3. 行为规划 Behavi ...
- 【硬核】 ROS Navigation 局部路径规划常见算法
简介 最近,作者参加了关于RMUS 高校 SimReal挑战赛,首次接触到了机器人导航领域,这里记录一下这段时间的收货.sim2real的全称是simulation to reality,是强化学 ...
- ROS运动规划学习六---dwa_local_planner
文章目录 前言 一.dwa_local_planner结构 二.setPlan.initialize.isGoalReached 三.computeVelocityCommands() 总结 前言 在 ...
最新文章
- 使用EF框架实现MVC的增删改查功能!!!Entity Framework
- C语言中的预处理详解
- 【数据平台】sklearn库特征工程之特征选择和降维
- MAVEN 自定义骨架
- 如何在线查找成千上万的免费电子书
- C++——错误	error LNK2019: 无法解析的外部符号 _main,该符号在函数 ___tmainCRTStartup 中被引用
- 从虚幻 4 中采集 360 度立体电影
- IOS学习笔记06---C语言函数
- Shell命令对整数求和,每行一个?
- 团队-学生成绩管理-模块开发过程
- 深入思考内积运算,再看傅里叶系数、伽辽金法本质
- (转)移动端自适应方案
- 破解navicat时出现No All Pattern Found File Already Patched
- kaka-manager和kafka-offset-monitor的安装和使用
- sql计算用户留存率和时间留存率
- 相关性和差异的显著性
- 假如你来发明编程语言
- Map key 和 value 的输出;
- Cisco 路由器与ASA防火墙 Site-to-Site IPsec
- 一键备份导出虾米音乐数据,包含收藏专辑/音乐/艺人/歌单