机器人导航学习历程（四）local planner

local planner

  我们先来回顾一下Navigation的框架，可以看出位于红框中的local_planner是move_base节点的一部分，输入包括来自global_planner的全局的规划路径、里程计信息以及local_costmap的信息，输出是速度控制指令，作为控制器的输入。

base_local_planner

  局部规划一般是用base_local_planner包实现的，该包使用Trajectory Rollout和**Dynamic Window approaches(DWA)**算法，根据地图数据，通过算法搜索到达目标的多条路经，利用一些评价标准（是否会撞击障碍物，所需要的时间等等）选取最优的路径，并且计算所需要的实时速度和角度。

DWA算法

  这里有一张DWA算法的示意图，蓝色的方块表示机器人，红色表示障碍物，机器人前方的多条黑色虚线表示备选的路线（可以设置参数来调整虚线的数量，也可以设置参数来调整虚线的长度，虚线越多越长对计算资源的消耗越多，但也会得到更为精确的结果），DWA算法会对每条路线进行评分，最终选择得分最高的路线，而会导致碰撞发生的路线会被排除。

  Trajectory Rollout和DWA算法的主要思路如下：

1. 采样机器人当前的状态（dx,dy,dtheta）；
2. 针对每个采样的速度，计算机器人以该速度行驶一段时间后的状态，得出一条行驶的路线；
3. 利用一些评价标准为多条路线打分；
4. 根据打分，选择最优路径；
5. 重复上面过程。

  Trajectory Rollout和Dynamic Window Approach (DWA)两种方法，理论上来说分别对应base_local_planner和dwa_local_planner两个包，但其实dwa的大部分代码都放在了base_local_planner包里面。

类继承关系

  base_local_planner类继承图如图所示。

• TrajectoryPlanners（红框标出）实现了DWA 和Trajectory Rollout算法。
• TrajectorySampleGenerator产生一系列轨迹
• 然后TrajectoryCostFunction遍历轨迹打分
• TrajectorySearch找到最好的轨迹拿来给小车导航
• 由于小车不是一个质点，WorldModel会检查小车有没有碰到障碍物

teb_local_planner

  teb_local_planner也是local planner的一个插件。teb即Timed Elastic Band，基本思路是根据轨迹执行时间、避障、动力学约束（如最大速度和最大加速度的限制）等来优化机器人的轨迹。

TEB算法

  对于二维路径的描述，有一个有趣的方法，叫做Elatic Band（橡皮筋）。简而言之，就是连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。

  经典的“elastic band”由n个的机器人位姿组成的序列描述$X_i=(x_i,y_{}$