Hybrid A*论文解析(5)

本文解析论文Enhanced path smoothing based on conjugate gradient descent for firefighting robots in petrochemical complexes，文中依然在解决混合A* 加上梯度方法轨迹平滑的问题，但是对于CMU的方案中的轨迹优化部分，做出了很多改进建议。这里着重说一下这部分，A* 部分基本沿用了CMU方案，也就没什么要说的了。

对于曲线的前提条件是：

1，曲线的曲率要小于0.2，也就是曲率半径要比5m大。
2，曲线要尽量平滑。
3，被平滑后的曲线要求首尾的heading方向不能变化。

针对以上的要求，文章在原来的曲线平滑基础上分析了问题并做了改进：

问题点如下：

1，对于第一个曲率优化问题。原来的优化方案，在只有curvature这一项进行优化时可能会出现kink,也就是曲线打结。比如这个

本来好好的曲线，优化完curvature term之后，反而不好了。

2，对于第二个要求的问题，当迭代次数变大后会出现曲线塌缩的现象。可以看到随着iteration次数的上升，曲线最后发生了塌缩。

3，优化后的曲线的首尾的heading方向，和原来的heading方向无法保证一致：

针对以上三个问题，文章提出了解决这三个问题的方案：

第一个问题的解决方案：

本文声称原文公式有问题？我感觉好像是这么回事

在curvature的三个方向的求导的符号，有改变的地方被加粗了，如上图所示。

第二个问题的解决方案：

添加关于曲率的终止条件。文章在每次迭代完成后计算一次曲线的平均曲率，当这个值不在收敛，而是开始发散的时候就终止迭代。看到下面的这个情况，迭代到第十次左右的时候平均曲率就达到了最低，然后就开始上升了，所以迭代会终止在十次左右。

还有一个终止条件就是当任何一个点上的curvature超出了设定的curvature_max也要终止迭代。

第三个问题的解决方案：

对曲线上的不同点施加不同的下降步长的权重：

这是一个logistic function形式的分布函数，长得样子基本就像一个高斯分布一样，首尾都很小，中间是1，这就保证了曲线的首尾的梯度下降是非常小的，heading 的变化也就非常小了。

最后看一下效果：
不改公式前，直接出现了曲线打结，更换公式，并且添加了迭代终止条件后，效果就好很多，且curvature的最大值条件也满足了。

再看smooth iteration问题，添加终止条件前曲线发生了塌缩，添加后曲线没有塌缩，并且曲率月满足了条件。

最后是首尾heading的问题，添加可变的权重后，首尾的heading就符合要求了。