梯度下降法平滑路径

  在实际过程中，为了使机器人能够在环境中流畅地运行，就需要一条平滑的路径，而普通的路径搜索算法Dijkstra、A*、rrt等无法满足要求，在此基础上，可以通过一些优化算法平滑路径，如凸优化的方法，但凸优化的方法构造起来比较麻烦，而且操作性较差（如对路径曲率进行约束），对于太长、障碍物比较多的环境，计算的实时性比较差。本文将使用梯度下降的方法将一条曲折的初始路径优化成平滑的无碰撞路径。

整体思路

  算法的整体思路如下图所示。初始路径规划可以使用A*算法，将路径离散成等距的离散点是为优化做准备，最后以一定的迭代次数，对离散的等距路径点依次进行梯度下降法平滑处理。

  优化的原理为依次选取5个路径点定义平滑度与路径点的关系，通过移动第3个点的位置，求解出使5个路径点平滑度最小的值，梯度方向为第3个点的移动方向，如图（1）所示。平滑度的定义如式（1）所示 

图1

 图（1）中,P −2 ,P −1 ,P,P 1 ,P 2 依次为路径中的5个离散点，P a 为P −2 P的中点，P b 为P −1 P 1 的中点，P c 为P 2P的中点，平滑度可为P a P −1 ,PP b​,P 1 P c的平方和，如式（1）所示。

  在定义了平滑度后，可以发现通过移动P 点的位置，平滑度会发生改变，那么当平滑度最小时，P 的位置在哪个地方，或者说P 点朝哪个方向移动多少距离？这是一个最小值问题。定义P 点的最优位移ΔP为梯度方向，通过平滑度对ΔP 的一阶导等于0可求得梯度方向，如式（2）所示。

  求解式（2）得到P 点的移动方向ΔP，即梯度：

  将路径点按照式（3）计算的梯度移动，多次迭代后，可以得到比较平滑的路径。在移动过程中，为了保证路径无碰撞，需要进行碰撞检测，当移动后的点发生碰撞时，不对该点移动操作。

实验效果

  使用A*做为初始路径规划，在ROS下进行仿真实验，设置迭代次数为100，路径点之间的距离为0.2m，优化效果如图2、图3所示。

 
图2
 
图3

虽然路径在障碍物顶点出会发生碰撞，可通过添加膨胀层解决。总体而言，路径光滑，能满足机器人流畅运行的要求。

注意事项

  1. 不要将梯度乘以大于1的系数，否则容易造成平滑失败的情况；
  2. 迭代的次数越大，得到的平滑效果越好，但计算量也就越大；
  3. 平滑度的定义不唯一；
  4. 离散点之间的距离越小，计算量越大，路径也越安全；