ROS局部运动规划器Teb/DWA

算法原理方面可参考下面的博文：

​​《Trajectory modification considering dynamic constraints of autonomous robots》理解​

TEB与DWA对比：

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

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

其中的参数很明显的说明了DWA算法针对的机器人模型是差分机器人，或者全向机器人，这些机器人都是至少能够做到原地自转的。不过由于我们比赛采用的是阿克曼模型的车模，所以严格来说这套算法是不太适合的。再加之整个比赛是以竞速为目标的，因此在使用该算法调参的过程中，会有很多矛盾的地方。

像上图这种地形，最佳的路径其实应该是近似一条直线的。但是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_orientatinotallow=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连接起来，去除方向没有朝向目标点的连接和与障碍物重叠的连接。这样就形成了一个网络，然后将起始点和终点接入到这个网络。如下图所示：

使用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，这样就可以根据使用场景实现自己的优化效果。