【路径规划】贝塞尔曲线平滑路径

贝塞尔曲线一般是用于二维图形的一种数学曲线，一般是用于一些矢量图的设计，不过在路径规划中，也可以应用上，例如之前的RRT随机搜索算法，因为是随机搜索，因此得到的路径点的曲折度是很大的，除了RRT，在其他的搜索算法得到路径点之后，依然也可以使用贝塞尔曲线来优化路径，使其更加平滑。

一阶贝塞尔与二阶贝塞尔

对于一阶贝塞尔的话，最终得到的曲线还是一条直线，不过还是可以列一下通式方便后续推导：

对上图，B1为一阶贝塞尔的点，则B1点随时间 t 的坐标为：

B1(t) = P1 + t * (P2 - P1) = (1 - t) * P1 + t * P2

二阶贝塞尔的话，与一阶贝塞尔是递归的关系，如下图：

当达到时间t时，P1-P2线段上对应的一阶贝塞尔点移动到了P’的位置，P2-P3线段上的一阶贝塞尔点移动到了P2’的位置，此时在P1’-P2’线段上的t时刻的位置即为二阶贝塞尔此时的位置，即为图中B2的位置。B2的坐标点的计算公式为：

B2 = (1 - t) * P1' + t P2' = (1 - t) * ((1 - t) * P1 + t * P2) + t * ((1 - t) * P2 + t * P3);

从上式可以看出其递归关系，不过也可以将其全部展开，方便编程。

结合之前的RRT算法，将RRT算法得到的路径点，每3个一组，组成若干个二阶贝塞尔曲线，可以得到一个稍微好一点的曲线。

用二阶贝塞尔的话，得到的效果如下：

二阶贝塞尔

直接使用的话，应该已经发现了一些问题，会有部分的点还是有些尖锐，就像我圈出来的这样：

不过也是可以通过一些特殊的处理去优化的，只不过我没有继续做二阶贝塞尔的细节优化。

n阶贝塞尔

在二阶贝塞尔之后，就可以继续递归，得到n阶贝塞尔的通式，进而一次性将所有的路径点都考虑进去，一次得到n阶贝塞尔，就不会有上面的尖锐问题。

n阶贝塞尔通式：

这里需要求一个组合数，matlab里有这个函数，我自己编了一个，这个比较简单：

最终实现的效果如下：

贝塞尔曲线

看起来就比单纯的RRT舒服多了。