【路径规划】辅助点与多段贝塞尔平滑RRT

在上一篇关于贝塞尔曲线平滑路径的文章中，只是大致的介绍了一下贝塞尔曲线的原理，并且初步测试了一下，最终平滑的效果，还是不尽人意的，最开始是使用了简易的多段二阶贝塞尔，得到的曲线有很多的尖锐：

然后使用了全局的贝塞尔，也就是所有的点全一次用上，得到一个N阶贝塞尔，但是这样就会有一个缺点，那就是当转弯很多的时候，它的效果就并不那么好了，就像这样：

有时就会出现上述的情况，比较容易压到障碍，并且与原路径差别较大。

那么在实际的贝塞尔优化路径的使用中，经常使用的方法可以直接告诉大家：使用多段五阶贝塞尔 + 插辅助点的方法。

具体为什么用五阶，而不是四阶，三阶，这些只是前人尝试得出来的经验，不过想要改成非五阶，思路，方法也都是一样的，可以根据自己的需求来自行调整。

辅助点的作用

这里最关键的就是辅助点的添加，在上一篇中的多段二阶贝塞尔中之所以会有各种尖角，是由于两段贝塞尔曲线的连接处，曲率不同，不共线，因而导致曲率突变，因此想要做好多段贝塞尔的效果，辅助点的添加是关键。

辅助点的要求

假设每一段五阶贝塞尔所需要的六个点，从起始到最后记作P0 - P5，那么下一段五阶贝塞尔曲线的起始点要与本段的末端点重合，假设下段的起始点记作Q0，即Q0 = P5，之后的第二个点Q1就是辅助点，在辅助点之后，再读取后面的四个点作为第二段的剩余四个点，如下图所示：

最重要的当然是辅助点Q1的选取，辅助点应该满足几个条件：

1）Q1要与P4-P5共线

2）Q1不能在P4-P5中间 ---> Q1 - P4 ≥ P5 - P4 ≥ Q1 - Q0

3）Q1与Q0的距离比Q2与Q1小 ---> || Q1 - Q0 || ≤ || Q2 - Q1 ||

4）Q1与Q0的距离小于或等于P5与P4的距离 ---> Q1 - Q0 = n * (P5 - P4), n∈(0,1]

5）P5与Q0重合 ---> P5 = Q0

之后根据上述的条件，我们就可以在之前的RRT程序后面，加上贝塞尔的程序，重点还是关于辅助点Q1的选择，由于RRT程序中的随机搜索距离都是固定距离，因此可以按照固定斜率，接近一半长度的原则构造Q1，如下所示：

五阶贝塞尔的公式也比较容易推导：

P(t) = (1 - t)^5 * P0 + 5*(1 - t)^4*t * P1 + 10*(1 - t)^3*t^2 * P2 + 10*(1 - t)^2*t^3 * P3 +

5*(1 - t)*t^4 * P4 + t^5 * P5

如果最后剩余了有3个点，那么最后的三个点，就可以用四阶贝塞尔补上(上段的最后一个点Q0 + 辅助点Q1 + 最后的三个点一共五个点，构造四阶贝塞尔)；如果剩余有2个点，则用三阶，依次类推将最后的几个点也一起用贝塞尔平滑，最终得到的效果如下：

可以放一个完整过程的视频：

五阶贝塞尔优化RRT曲线