ROS局部运动规划器Teb/DWA

ROS局部运动规划器Teb/DWA算法原理方面可参考下面的博文:《Trajectorymodificationconsideringdynamicconstraintsofautonomousrobots》理解TEB与DWA对比:DWA算法DWA算法是比较普遍使用的,但是这个算法对我们需求来说并不是最合适的算法。#Differential-driverobotconfiguration-necessary?差分机器人配置参数holonomic_robot:false#是否为全向机器人。值为fal.

TEB算法过程
算法原理方面可参考下面的博文:
《Trajectory modification considering dynamic constraints of autonomous robots》理解

TEB与DWA对比:

DWA算法
DWA算法是比较普遍使用的,但是这个算法对我们需求来说并不是最合适的算法。

# Differential-drive robot configuration - necessary? 差分机器人配置参数
holonomic_robot: false
#是否为全向机器人。 值为false时为差分机器人; 为true时表示全向机器人

其中的参数很明显的说明了DWA算法针对的机器人模型是差分机器人,或者全向机器人,这些机器人都是至少能够做到原地自转的。不过由于我们比赛采用的是阿克曼模型的车模,所以严格来说这套算法是不太适合的。再加之整个比赛是以竞速为目标的,因此在使用该算法调参的过程中,会有很多矛盾的地方。
https://www.guyuehome.com/Uploads/old-md/uploads/20200725_45904.png
像上图这种地形,最佳的路径其实应该是近似一条直线的。但是DWA的算法则有一个矛盾的点,就是由于针对的模型不匹配,导致无法设置转向半径这个很重要的参数。因此想让这种弯道的路径变得尽可能接近直线,只有通过减小墙体的膨胀系数才能实现。否则路线则会接近地图上这条弯道的弧度,不过减小膨胀层又会面临另一个问题,那就是无法很好的避开障碍物。这就让人陷入两难的境地了。

TEB算法
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 模式就更合适了。
利用doxywizard+graphiz对包进行解析,查看类继承图
teb的优化问题的解决利用了开源软件包g2o,先看看对g2o边和顶点的定义。
二元边

主要是考虑了运动学的约束关系(车的相邻的两个位姿态必须落在一个圆弧上)。二元边的两个顶点分别是两个位姿。还限制了最小转弯半径。
在这里插入图片描述
EdgeViaPoint:以指定的经过点位置与待优化位姿(顶点)的误差作为目标。(保证轨迹会经过指定点)
EdgeObstacle:以障碍物与待优化位姿(顶点)的距离经过惩罚函数后的输出作为目标(保证离障碍物大于一定距离)
EdgeTimeOptimal:以时间间隔dt最小作为目标。(保证时间最短轨迹)

多元边(note:构造函数中resize顶点的个数):
在这里插入图片描述
EdgeVelocity:以实际速度和限定最大速度经过惩罚函数的输出作为目标函数,顶点为相邻的位姿和时间间隔。(限制实际速度不超过最大,程序中引入了sigmoid函数决定速度的符号(根据论文内的说法,引入此函数是因为优化算法只能求解连续函数))。
Edgevelocityholonomic:与EdgeVelocity的区别在于,ds是机器人本体坐标系下的,然后去求速度在本体坐标系下的表示。其余思路一致。

teb在运动过程中会调整自己的位姿朝向,当到达目标点时,通常机器人的朝向也是目标朝向而不需要旋转。dwa则是先到达目标坐标点,然后原地旋转到目标朝向。对于两轮差速底盘,teb在运动中调节朝向会使运动路径不流畅,在启动和将到达目标点时出现不必要的后退。这在某些应用场景里是不允许的。因为后退可能会碰到障碍物。而原地旋转到合适的朝向再径直走开是更为合适的运动策略。。

对HomotopyClassPlanner类的理解

TEB工程里可以使用两种类实例化局部规划器。

    // create the planner instance
    if (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连接起来,去除方向没有朝向目标点的连接和与障碍物重叠的连接。这样就形成了一个网络,然后将起始点和终点接入到这个网络。如下图所示:
ROS局部运动规划器Teb/DWA
使用Depth First Search(深度优先方法)搜索所有可行的路径。将这些路径作为参考,实例化多个TebOptimalPlanner类的实例。采用多线程并行优化,得到多条优化后的路径。将这些路径进行可行性分析,选出代价值最小的最优路径。不得不说HomotopyClassPlanner类里的方法是一个鲁棒性和可靠性更高的方法。因为单一的TEB规划 (TEB without homology class exploration)在某些场景会陷入局部最小值,可能出现卡死的情况。

其他类

在这里插入图片描述
类的简介:
此类定义了一条时变弹性带的轨迹模型,里面包含两个最重要的数据,数据类型为位姿(teb定义的g2o顶点)指针的vector,数据类型为时间(teb定义g2o顶点)指针的vector。
类的接口(仅列举一部分):对位姿、时间顶点的入栈、出栈、插入、删除、读取等操作;初始化轨迹(以指定的起点和终点进行插值产生的轨迹);更新和改变轨迹(根据现在的位置更新起点(vector中的第一个点),改变终点);找到最近点(离指定的点、直线或者其他);计算是走过的路程之和;计算花费时间之和;探测轨迹是否转弯等操作。
在这里插入图片描述
类简介:此类中主要存储了用于配置TEB的参数,包括轨迹参数、障碍参数、机器人参数、优化参数、目标tolerance参数、同伦类参数(这是啥?)。
轨迹参数:包含轨迹的时空分辨率的指定、采用、是否以全局目标覆盖局部子目标、via_point序列等等。
障碍参数:与障碍的最小分隔距离、膨胀半径、是否包含动态障碍(会启用常速度模型预测障碍位置)、等参数。
机器人参数:机器人的x,y,theta速度、加速度限制、最小转弯半径限制
目标容忍参数:机器人到达目标时允许的位姿误差。
优化参数:迭代次数、各个子目标的权重等。
同伦类参数:是否启动一次规划多个轨迹、是否多线程规划、同伦类的数量等等
恢复参数:解决一些震荡的问题。

接口:从ros服务器中加载参数、动态设置ros服务器中的参数、返回内部配置的互斥锁、检查参数。
在这里插入图片描述
PlannerInterface:
类简介:定义了局部规划器的接口。
接口:
virtual plan():根据初始的参照规划(位姿序列)、或者起点终点,提供方法去规划和创造一条轨迹。
virtual getVelocityCommand():获取用于控制机器人的速度命令
virtual void clearPlanner():重置规划器。
virtual bool isTrajectoryFeasible():通过检测轨迹的一部分是否与障碍碰撞而检查轨迹是否可行。
virtual bool isHorizonReductionAppropriate():该方法是在确定规划器提供的轨迹不可行的情况下调用的。在某些情况下,视界长度的缩短可能会解决问题,例如,如果计划的轨道抄近路。由于轨迹表示由规划器管理,所以它是基本planner_interface的一部分。实现是可选的。如果没有指定,该方法返回false。
virtual void computeCurrentCost():返回目前的图优化的花费。

TebOptimalPlanner
类简介:这个类使用了g2o优化了时变弹性带的轨迹。
接口:
TebOptimalPlanner() :构造函数,初始化了cfg参数、机器人模型、viapoint和起点终点速度。
~TebOptimalPlanner() :析构函数,清除图。
plan():调用方法去生成和优化轨迹(根据先前产生的轨迹,调用optimizeTEB()来优化轨迹。
getVelocityCommand():若存在规划好的轨迹,则根据位姿、时间间隔求取速度。
optimizeTEB():里面有两层循环,分别叫外部循环和内部循环。外部循环通过调用TimedElasticBand::autoResize()来根据时间分辨率调整轨迹。内部循环调用optimizeGraph()进行优化。外部的循环时间应根据cpu的控制速率要求来定,内部循环的时间经过实验2-6次足够。最后调用 see computeCurrentCost().计算轨迹花费。
setVelocityStart():设定轨迹的初始速度。
setObstVector ():设定障碍用于轨迹规划。
visualize():发布局部规划pose、footprint模型等。

保护成员函数:主要负责构建图优化问题。
initOptimizer():初始化优化器。设定线性求解器、块求解器、求解算法等。
buildGraph ():根据超图问题描述构建图优化问题。若顶点和边非空,则add。
optimizeGraph():根据之前建好的优化问题,调用优化器optimizer优化图。
AddTEBVertices():将timedElasticBand类型中的位姿顶点序列、时间顶点序列依次加入图中。note:添加顺序会影响稀疏矩阵的结构进而影响优化效率。
AddEdgesVelocity():对于timedElasticBand类型中的位姿顶点序列-1条边,设置该边的顶点和权重矩阵,添加该边。
clearPlanner():清除图,清除时变弹性带轨迹
computeCurrentCost():将图中所有边的error的平方和构成cost,存在支持多条轨迹的重载版本。
extractVelocity():计算,提取机器人的速度在本体坐标系下的表示。
Compute the velocity profile of the trajectory():计算整条轨迹的速度曲线,以vector矢量方式保存。
getFullTrajectory():获得整个轨迹的位姿、速度、时间。将其存储在TrajectoryPointMsg类型的vector中。
isHorizonReductionAppropriate():根据一些情况,判断是否建议更短的Horizon。
registerG2OTypes():将为TEB定义的顶点和边注册到g2o::Factory。
例如,这允许用户将内部图形导出到文本文件。访问优化器()了解更多细节。

HomotopyClassPlanner
类简介:
本地规划器,探索可选的同伦类,为每个可选类创建一个计划,最后返回当前最佳路径的机器人控件(在每个采样间隔中重复)
等价类(例如同伦)是利用搜索图来研究的。
对几个可能的候选项进行采样/生成,然后进行过滤,以便每个同伦类只保留一个候选项。
相关参考论文:
S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015.
在这里插入图片描述
TebLocalPlannerROS
类简介:
将teb_localplanner插件化,使之能被集成到ROS导航包中去。
私有属性:
指向来自ros的costmap包装器的指针、指向costmap2d的指针、指向tflistener的指针、指向规划器接口的指针、障碍和viapoint序列、参数配置对象、全局规划的序列、各种mutex、目标位姿、一些状态量bool。
公有接口:
TebLocalPlannerROS ():构造函数,初始化以上属性。
initialize ():创建roshandle句柄、创建动态参数配置、创建规划器实例、加载costmap插件、订阅障碍和viapoint信息。
setPlan():将ros中全局规划的序列传递到这个类中的全局规划序列中保存。
computeVelocityCommands():首先获得机器人当前位姿和速度,去掉当前位置前的全局规划,更新viapoints,判断是否到达全局终点,更新障碍,锁住cfg配置参数,调用planner进行规划,根据footprint检查可行性,获得速度命令,判断是否符合约束,速度命令到转向角转化(carlike),可视化轨迹、障碍等。
isGoalReached():返回是否到达目标的状态。

静态成员函数
tfPoseToEigenVector2dTransRot():将tf::Pose类型转换为包含平移速度和角速度的Eigen::Vector2d。平移速度(x坐标和y坐标)被组合成一个单一的平移速度(第一个分量)。
makeFootprintFromXMLRPC():从给定的XmlRpcValue设置footprint.

保护成员函数
updateObstacleContainerWithCostmap():根据costmap占有栅格更新障碍序列。note:All occupied cells will be added as point obstacles.
updateObstacleContainerWithCostmapConverter():从costmapConverter中提取并更新到障碍序列。
reconfigureCB():允许在线修改cfg参数。
pruneGlobalPlan():修改全局规划,全局规划器规划出的路径中机器人已经经过的位姿删去。
transformGlobalPlan():根据localcostmap地图大小,将此地图内的全局规划器的规划保存到transformed_plan中。transformed_plan所在的坐标系仍然为global_frames
estimateLocalGoalOrientation():估计位姿方向局部目标的位姿。将未来一段的规划的姿态角的平局值返回为估计的局部目标姿态。
saturateVelocity():若规划出来的速度超过限制速度,则令其等于最大限制速度。
convertTransRotVelToSteeringAngle():将旋转平移速度转换为车转角

可尝试的改进

起始点的旋转

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/shoufei403/article/details/104268893

今天的文章ROS局部运动规划器Teb/DWA分享到此就结束了,感谢您的阅读,如果确实帮到您,您可以动动手指转发给其他人。

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
如需转载请保留出处:https://bianchenghao.cn/29116.html

(0)
编程小号编程小号

相关推荐

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注