ROS常用局部路径规划算法比较

本博文主要讨论ROS导航包中集成的局部路径规划算法，DWA、TEB、MPC等算法在使用过程中的各自的优缺点。以下均为自己在使用过程中总结的经验及查阅资料得来，如有理解不到位的地方，还希望在评论区多多讨论。

1. 动态窗口法(DWA)

算法简介：
DWA算法全称为dynamic window approach，其原理主要是在速度空间（v,w）中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的速度驱动机器人运动。

动态窗口法与ROS默认局部路径规划算法TrajectoryPlanner类似，不同之处在于对机器人控制空间的采样：在给定机器人的加速度极限的情况下，TrajectoryPlanner在整个前向模拟周期内从可实现的速度集合中进行采样，而DWA在给定机器人的加速度极限的情况下仅针对一个模拟步骤从可实现的速度集合中进行采样。在实际使用过程中，TrajectoryPlanner和DWA算法效果类似，但是DWA算法更加高效，占用内存更少，所以在这两种算法中间一般直接选择DWA。

DWA算法在ROS中为dwa_local_planner
DWA算法分析参考博文DWA算法分析及实现

优点：

• 计算复杂度低：考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小
• 可以实现避障：可以实时避障，但是避障效果一般
• 适用与差分和全向车模

缺点：

• 前瞻性不足：只模拟并评价了下一步，如在机器人前段遇见“C”字形障碍时，不能很好的避障
• 动态避障效果差： 模拟运动轨迹断，动态避障效果差
• 非全局最优路径： 每次都选择下一步的最佳路径，而非全局最优路径
• 不适用于阿克曼模型车模

2. 时间弹性带(TEB)

算法简介：
TEB全称为Time Elastic Band，算法浅析参考博文TEB浅析，文中关于eletic band（橡皮筋）的定义：连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。关于time eletic band的简述：起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time，即为Timed-Elastic-Band算法。通过此方法可以把问题描述为一个多目标优化问题，通过构建超图(hyper-graph)，使用g2o框架中的图优化来求解。

TEB算法在ROS中为teb_local_planner
TEB算法分析参考以上链接中所列论文如图

亦可参考博文TEB轨迹优化算法-代码解析
针对速度障碍模型(VO)参考博文VO避障

优点：

• 适用于各种常见车模：如差分、全向、阿克曼模型
• 有很强的前瞻性： 对前方一段轨迹进行优化
• 动态避障： 对动态障碍有较好的避障效果，可直接使用其封装好障碍类Obstacle
  如：静态障碍时TEB算法轨迹规划效果
  
  当障碍有个0.15m/s向右的速度时，TEB算法轨迹规划效果如下图
  

缺点：

• 计算复杂度较大：可通过牺牲预测距离来降低复杂度
• 速度和角度波动较大、控制不稳定： 源码中是通过两状态之间的距离和角度差及时间差来计算该控制周期内的速度和角速度，使得在控制过程中速度和角度波动较大，如下图所示。在计算资源足够的情况下，提高控制频率可以改善此现象。
  
• 非全局最优： 但是好于DWA

改进策略
针对其控制不稳定问题，主要原因是每个控制周期内，速度变化较大。一种方法是提高控制频率，另一种方法是使用优化的方法，即修改TEB算法的评价函数，把每次速度和角度的变化量除以时间再乘一个代价系数。

3. 模型预测控制(MPC)

算法简介
MPC(Model Predictive Control)与上文提到的DWA和TEB算法不同，MPC只是一个控制器，在自动驾驶领域，其与PID控制器一样，控制器输入包括车辆下一步的运行轨迹，车辆的当前状态，输出是速度和转角。不同之处在于，PID控制器是实时处理当前车辆与目标轨迹的差距来调整输出，使车辆接近目标轨迹，而MPC控制器将未来一个时间段 t 分成 N 个节点，预测每个节点的车辆状态，再调整控制器的输出使车辆尽可能接近参考轨迹。相比于PID控制器的单输入单输出特性，模型预测控制更加适用于多输入多输出的复杂控制系统，可以通过调参，使得车辆的控制更加平稳、更接近于期望轨迹等。

MPC参考博文基于阿克曼模型和差速模型小车的MPC算法推到及分析
MPC在ros中为mpc_local_planner

优缺点：
目前正在调试MPC,由于现在对MPC的认知还不是太深刻所以具体调试效果及调试经验日后分享，敬请期待。