自动驾驶-决策规划算法七：B样条曲线(C++)

自动驾驶-决策规划算法七：B样条曲线(C++)

搞要：

本文介绍无人驾驶决策规划算法中的B样条曲线。

语言用的是C++，图形基于easyX库输出。

如有错误或者遗漏之处，欢迎指出。

//由于本文关注的是曲线的轨迹生成算法，所以并没有对航向角进行规划，也没有加上控制，所以在动态展示时只用平移来示意车辆沿着轨迹行驶。

//限于时间关系，公式就不用公式编辑器了，用截图替代，大家看得懂就行；

---

附赠自动驾驶学习资料和量产经验：链接

基本概念：

B样条曲线概述：

样条是一根富有弹性的细木条或塑料条，在应用CAD/CAM技术以前，制造业普遍采用手工绘制自由曲线。绘制员用压铁压住样条，使其通过所有给定的型值点，再适当地调整压铁，改变样条形态，直到符合设计要求；

B样条曲线是B样条基函数（给定区间上的所有样条函数组成一个线性空间）的线性组合；

贝塞尔曲线的缺陷：

1，确定了多边形的顶点数（n+1个），也就决定了所定义的贝塞尔曲线的阶次（n次），这样很不灵活；
2，当顶点数（n+1）较大时，曲线的次数较高，曲线的导数次数也会较高，因此曲线会出现较多的峰谷值，不利于车辆轨迹的规划；
3，贝塞尔曲线无法进行局部修改，不够灵活；
B样条曲线除了保持Bezier曲线所具有的有点外，还弥补了上述所有的缺陷；

B样条曲线的数学描述：

设有P0, P1 … Pn一共n+1个控制点，这些控制点用于定义样条曲线的走向、界限范围，则k阶B样条曲线的定义为：

式中，k是阶数，Bik(u)是第i个k阶B样条基函数，与控制点Pi相对应，k≥1；u是自变量，相当于贝塞尔曲线中的t，但定义域可以不是[0, 1]，而首末值通常还是定义为0和1，然后取中间一段作为最终定义域；

基函数具有如下德布尔考克斯递推式：

约定：0/0=0；

ui是一组被称为节点矢量的非递减序列的连续变化值，首末值可以定义为0和1，该序列如下：

根据递推式，当阶数k=1时，不同基函数的非零域如下图：

也就是说，k=1时，只有在i对应的u的区间内，Bik才等于1，其他区间都等于0，这也反应了基函数Bik和第i个控制点的对应关系；

k=1时，i从1到k，每个Bik是最基本的基函数，通过上述递推式构成了更高阶的基函数，最终构成整个B样条曲线的表达式，如下图所示；

这是一个4阶B样条曲线，k=4，控制点个数为n，P0为第0个控制点，对应的德布尔考克斯基函数是B04, B14一直到Bn4；

而B04通过递推式可以分解为由B01, B11, B21, B31构成，而这几个最基本的基函数分别在[u0, u1), [u1, u2), [u2, u3), [u3, u4)上才有值，为1；

Bik递推到Bi1后，u的次幂会变为k-1，也就是说K阶B样条曲线是关于u的k-1次曲线，这里和贝塞尔曲线有区别，贝塞尔曲线的阶数和t的次数是相等的，例如3次贝塞尔曲线：

B样条曲线的段数为(n+1) - (k-1) = n-k+2，每段是一小段贝塞尔曲线，证明过程略；

Bik(u)涉及到的节点为ui, u(i+1)…u(i+k)，一共k+1个节点，k个区间，因此从B0k(u)到Bnk(u)共涉及n+k+1个节点；

开B样条曲线：起点和终点没有相连；闭B样条曲线：起点和终点相连；

对于闭B样条曲线，u的定义域可以设为[0, 1]；

对于汽车轨迹规划，通常用开B样条曲线，u的定义域为[u(k-1), u(n+1)]，从u(k-1)开始可以让每个控制点对应的基函数Bik(u)都不为0，可以让每个控制点都对曲线有控制作用，可以表达出完整的线性组合；

阶数k必须小于等于控制点数n+1；

k阶B样条曲线上的一点至多与k个控制顶点有关，与其他控制顶点无关。移动曲线一个控制顶点，至多影响定义在区间上那部分曲线的形状，对曲线的