对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 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)在某些场景会陷入局部最小值，可能出现卡死的情况。

参考材料：
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