Fast Planner规划算法（二）—— Fast Planner后端之贝塞尔曲线与B样条曲线

慕羽★

于 2024-08-08 10:00:37 发布

阅读量502

点赞数 19

分类专栏：运动规划和控制数值优化方法文章标签：算法 Fast Planner 轨迹优化路径规划 B样条曲线机器人数值优化

本文链接：https://blog.csdn.net/qq_44339029/article/details/132601858

版权

运动规划和控制同时被 2 个专栏收录

69 篇文章 213 订阅

订阅专栏

数值优化方法

29 篇文章 73 订阅

订阅专栏

本系列文章用于学习记录Fast-Planner规划算法的相关内容，主要学习资料是深蓝学院的移动机器人运动规划课程

二、Fast Planner后端之贝塞尔曲线与B样条曲线

Fast Planner后端的主要任务是基于前端生成的轨迹，生成一条满足其他约束的轨迹，Fast Planner后端包含轨迹优化和迭代时间调整两部分，我们先来看轨迹优化部分中的B样条曲线的相关内容。

B样条是一种轨迹的表达方式，就像用多项式来表示轨迹也是一种轨迹的表达方式。B样条通过控制点来表示轨迹，如下图中的轨迹上的圆点所示，通过拉动控制点就可以使轨迹发送变化。

在这里插入图片描述

1、贝塞尔曲线

假设我们用两个点来表示轨迹，即 $\boldsymbol{B}(t)=(1-t)\boldsymbol{P}_0+t\boldsymbol{P}_1\quad t\in[0,1]$ ，随着时间t从0到1的变化，就可以得到该轨迹上的任意一点

在这里插入图片描述

现在我们将点的个数增加到三个 $P_0$ 、 $P_1$ 、 $P_2$ ，通过上面的方法，我们可以得到线段 $P_0$ $P_1$ 上任意一点X，和线段 $P_1$ $P_2$ 上任意一点Y，同理在线段XY上也可以得到任意一点B，如下图所示，它们满足 $\frac{\|XB\|}{\|XY\|}=\frac{\|P_0X\|}{\|P_0P_1\|}=\frac{\|P_1Y\|}{\|P_1P_2\|}=t\quad t\in[0,1]$ ，

在这里插入图片描述

进一步可以得到B点的表达式，如下所示：

$\boldsymbol{B}(t)=(1-t)^2\boldsymbol{P}_0+2(1-t)t\boldsymbol{P}_1+t^2\boldsymbol{P}_2\quad t\in[0,1]$

随着时间t的变化，我们可以获得不同的B点,构成了B的轨迹曲线，动态演示：

在这里插入图片描述

由B点的表达式可知，我们改变 $P_0$ 、 $P_1$ 、 $P_2$ 三个点中的任意一个点都可让B点的轨迹发生变化，，基于这三个控制点，我们先推出X和Y，再推出了B，基于这种递推的特性，我们可以扩展到四个控制点情况：

$B(t)=(1-t)^3P_0+3(1-t)^2tP_1+3(1-t)t^2P_2+t^3P_3\quad t\in[0,1]$

在这里插入图片描述

同理，我们可以得到五个控制点、六个控制点的情况：

通过上面的介绍，我们得到了一种不同于多项式的曲线表达方式，这种方式也称为贝塞尔曲线(Bezier Curve)，我们可以推出n个控制点时，贝塞尔曲线轨迹的表达式，如下所示：

$B(t)=\sum_{i=0}^{n-1}R_{i,n}(t)P_{i}\quad\text{其中}\quad R_{i,n}(t)=C_{n}^{i}t^{i}(1-t)^{n-i}$

上式中的 $R_{i,n}(t)$ 称为贝塞尔曲线的基函数，t的取值范围为[0,1]，上述的贝塞尔曲线中任意一个控制点的改变都会使得曲线的整体形状发生改变，感兴趣的可以去贝塞尔曲线和B样条曲线的在线交互网站进行体验，链接如下：

链接：http://nurbscalculator.in/

在这里插入图片描述

贝塞尔曲线存在两个明显的缺点，第一个是曲线的阶数随着控制点的增加而增加，如果有n个控制点，则由贝塞尔曲线的表达式可知，曲线的阶数为n-1。第二个缺点是，当我们改变整条轨迹中的一个控制点的时候，整条轨迹都会发生变化。

而我们想要的是阶数不太高的轨迹，且我们动某个控制点对轨迹的局部进行调整时，不会使得整条轨迹发送变化，此时，可以使用接下来介绍的一种特殊的贝塞尔曲线——B样条曲线

2、B样条曲线

通过定义基函数N和控制点Q，B样条曲线可用下式表示

$\mathbf{C}(t)=\sum_{i=0}^nN_{i,p}(t)\mathbf{Q}_i$

B样条曲线可以理解成一种分段的特殊贝塞尔曲线，我们可以用多段低阶的贝塞尔曲线来表示一条轨迹，然而在分段点处的平滑性很难得到保证，而B样条曲线是一种特殊的分段贝塞尔曲线，它天然的满足分段点处的平滑性连续性。

与贝塞尔曲线不同的是，B样条曲线并不要求时间范围限定在[0,1]区间上，B样条曲线与贝塞尔曲线的最大区别是基函数不同

B样条曲线唯一的由三个变量决定，即阶数 $\color{red}p_b$ 、N+1个控制点 $\color{red}\{{\mathbb{Q}_0,\mathbb{Q}_1,...,\mathbb{Q}_N}\}$ 、时间节点向量 $\color{red}{[t_0,t_1,...,t_M]}$ ，其中 $M=N+p_b+1$ ，完整的B样条轨迹对应的时间区间是 $t_{p_b},t_{M-p_b}]$ 。

时间区间就是两两相邻的时间节点之间的区间，3阶B样条曲线的每段分段曲线都由4个控制点来控制，假设分段曲线中某段曲线由控制点 $Q_0、Q_1、Q_2、Q_3$ 控制，对应的时间区间是 $t_3,t_4]$ ，则控制点 $Q_1、Q_2、Q_3、Q_4$ 控制的分段曲线，对应的时间区间是 $t_4,t_5]$ ，以此类推，每相邻的四个控制点控制一个时间区间内的轨迹。
在这里插入图片描述

若相邻的节点向量之间的差值是相同的，即每段分段曲线对应的时间区间是相同的，则称为均匀B样条曲线,，对于总时间区间中某段分段轨迹对应的时间区间 $[t_{m},t_{m+1})\subset[t_{p_{b}},t_{M-p_{b}}]$ 中的时间t，可以通过 $s(t)=(t-t_{m})/\Delta t.$ 来归一化到[0，1]上，其中 $\Delta t=t_{m+1}-t_m.$

我们先来看一下B样条曲线的基函数，它是以递推的形式给出的，如下所示，这里的u跟前面用t含义相同：

$\begin{aligned} &N_{i,0}(u)=\begin{cases}1&\mathrm{if}u_i\leq u<u_{i+1}\\0&\mathrm{otherwise}\end{cases} \\ &N_{i,p}(u)=\frac{u-u_{i}}{u_{i+p}-u_{i}}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u) \end{aligned}$

其中，p是阶数，对于0阶的情况可以直接写出来，然后可以由0阶的推1阶的，再由1阶的推2阶的 …以此类推，其关系如下图所示，比如可由 $N_{0,0}$ 、 $N_{1,0}$ 得到 $N_{0,1}$

在这里插入图片描述

、

我们来看一个具体的实例，我们取 $u_{0}=0,u_{1}=1,u_{2}=2,u_{3}=3,u_{4}=4$ ，则由0阶的 $N_{0,0}$ 、 $N_{1,0}$ 、 $N_{2,0}$ 得到1阶的 $N_{0,1}$ 、 $N_{1,1}$ 随u变化的曲线如下所示：

在这里插入图片描述

同理，我们可以由1阶的 $N_{0,1}$ 、 $N_{1,1}$ 推出2阶的 $N_{0,2}$

$N_{0,2}(u)=\frac{u-u_{0}}{u_{2}-u_{0}}N_{0,1}(u)+\frac{u_{3}-u}{u_{3}-u_{1}}N_{1,1}(u)=0.5uN_{0,1}(u)+0.5(3-u)N_{1,1}(u)$

即：

$N_{0,2}(u)=\left\{\begin{matrix}0.5u^2&u\in[0,1)\\0.5u(2-u)+0.5(3-u)(u-1)u\in[1,2)\\0.5(3-u)(3-u)&u\in[2,3)\end{matrix}\right.$ 在这里插入图片描述

同理，我们可以推出3阶的公式

$N_{0,3}(u)=\frac{u-u_{0}}{u_{3}-u_{0}}N_{0,2}(u)+\frac{u_{4}-u}{u_{4}-u_{1}}N_{1,2}(u)=\frac{1}{3}uN_{0,2}(u)+\frac{1}{3}(4-u)N_{1,2}(u)$

即

$N_{0,3}(u)=\dfrac{1}{6}\begin{cases}u^3&u\in[0,1)\\u^2(2-u)+u(3-u)(u-1)+(4-u)(u-1)^2&u\in[1,2)\\u(3-u)^2+(u-1)(3-u)(4-u)+(4-u)^2(u-2)&u\in[2,3)\\(4-u)^3&u\in[3,4)\end{cases}$

可以发现，随着阶数的增加，图像中非零的区域在变大，0阶的 $N_{0,0}$ 的非零区域是 $u_0,u_1)$ ，1阶的 $N_{0,1}$ 的非零区域是 $u_0,u_2)$ ，2阶的 $N_{0,2}$ 的非零区域是 $u_0,u_3)$ ，3阶的 $N_{0,3}$ 的非零区域是 $u_0,u_4)$ …可以在图中用三角形圈出来

在这里插入图片描述
假设阶数 $p_b$ =3，控制点有3个： $Q_0$ 、 $Q_1$ 、 $Q_2$ ，即N+1=3，由上图可知这三个控制点对应的区间为[ $u_0$ ， $u_6$ ]，即M=6，显然，关系式M=N+ $p_b$ +1=6成立。

另外，B样条轨迹由时间t参数化，其中 $t\in[t_{p_{b}},t_{M-p_{b}}]$ ，为什么不是 $t\in[t_{0},t_{M}]$ ？因为，若取 $t\in[t_{0},t_{M}]$ 其导数的含义不清，且某个区间受相邻几个控制点控制的结论也不清晰了

在这里插入图片描述

总的来说，我们可以得到以下递推的形式的B样条曲线的基函数

$\color{red}\begin{aligned} &N_{i,0}(u)=\begin{cases}1&\mathrm{if}u_i\leq u<u_{i+1}\\0&\mathrm{otherwise}\end{cases} \\ &N_{i,p}(u)=\frac{u-u_{i}}{u_{i+p}-u_{i}}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u) \end{aligned}$

然后由下式即可得到B样条曲线：

$\color{red}\mathbf{C}(u)=\sum_{i=0}^nN_{i,p}(u)\mathbf{Q}_i$

3、 B样条曲线的特性

（1）B样条特性1：移动控制点

$\mathbf{C}(u)=\sum_{i=0}^nN_{i,p}(u)\mathbf{Q}_i$

我们将其中一个控制点Q移动到新的位置Q+v，整理后得到的B样条曲线的表达式相比于原表达式多了一项 $N_{k,p}(u)\mathbf{v}$ ,也就是说对某个控制点进行移动后，只需要在原有轨迹的基础上，加上一个偏移量就可以了

$\begin{aligned} \mathbf{c}_{new}(u)& =\sum_{i=0}^{k-1}N_{i,p}(u)\mathbf{Q}_{i}+N_{k,p}(u)(\mathbf{Q}_{i}+\mathbf{v})+\sum_{i=k+1}^{n}N_{i,p}(u)\mathbf{Q}_{i} \\ &=\boxed{\sum_{i=0}^nN_{i,p}(u)\mathbf{Q}_i}+N_{k,p}(u)\mathbf{v} \\ &=\mathbf{C}(u)+\boxed{N_{k,p}(u)\mathbf{v}} \end{aligned}$

下面看一个具体的例子，假设B样条的阶次p=3，对控制点1进行移动，则 $\mathbf{c}_{new}(u)=\mathbf{C}(u)+\boxed{N_{1,3}(u)\mathbf{v}}$ ，通过前面的介绍可知基函数 $N_{1,3}$ 的非零区间为 $u_1，u_5)$ ，如下图的红框所示，所以移动控制点1仅对位于 $u_1，u_5)$ 区间内的轨迹产生影响，改善了贝塞尔曲线牵一发而动全身的缺陷

在这里插入图片描述

（2）B样条特性2：每个区间仅由相邻的几个控制点完全描述

把上面的结构图竖起来，如下所示：

在这里插入图片描述

由特性1可知，3阶B样条曲线，控制点 $Q_0$ 对应的非零区间是 $u_0,u_4)$ 、控制点 $Q_1$ 对应的非零区间是 $u_1,u_5)$ 、控制点 $Q_2$ 对应的非零区间是 $u_2,u_6)$ 、控制点 $Q_3$ 对应的非零区间是 $u_3,u_7)$ 、控制点 $Q_4$ 对应的非零区间是 $u_4,u_9)$ …

所以区间 $u_3,u_4)$ 完全且仅由控制点 $Q_0$ ~ $Q_3$ 控制，这也是B样条可以分段描述的原因，可以绘制成倒三角的形式，如下图所示：

在这里插入图片描述

当阶次 $p_b$ =3，m也取为3时，Fast Planner中的B样条曲线中某段分段曲线的表达式（在下一部分会进行详细介绍）可具体写为下式，相比于传统的基函数表示，该表达形式很容易看出该段曲线仅受控制点 $Q_0$ ~ $Q_3$ 控制

在这里插入图片描述

（3）B样条特性3：连续性

B样条曲线在阶数p=0时是不连续的，在阶数p>=1时是连续的

在这里插入图片描述

随着控制点的增加，B样条曲线会在原曲线的基础上不断进行扩展，而不会改变整个轨迹:

在这里插入图片描述

（4）B样条特性4：B样条的导数也是B样条

B样条曲线的导数也是B样条曲线，而且B样条曲线的导数的控制点也可以很容易算出来，证明略

在这里插入图片描述

（5）凸包性

贝塞尔曲线和B样条曲线都具有凸包性，凸包就是包含所有顶点（控制点）的最小凸多边形。凸多边形的任一边延长，其它边都在它的一侧。贝塞尔曲线和B样条曲线得到的轨迹始终会在包含了所有控制点的最小凸多边形中,因此，可以通过控制点的凸包来限制轨迹曲线的范围

由性质4可知，B样条的导数也是B样条，所以B样条的导数同样具有凸包性，下面的左图是p=3的B样条曲线，右图是其导数，也即p=2的B样条曲线

在这里插入图片描述

凸包性质在轨迹避障中有重要应用，为了保证B样条轨迹的安全性，若我们保证它的所有凸包都是无碰撞的，由凸包性可知，B样条轨迹属于该凸包中，必然也是无碰撞的。

保证它的所有凸包都是无碰撞的，等价于凸包中的点与障碍物的距离 $d_h > 0$ ，但是凸包中任意一点不好表示，凸包的顶点比较好表示，所以把 $d_h > 0$ 转换成对障碍物与凸包顶点距离 $d_c$ 的约束

在这里插入图片描述

由三角形的性质可知 $d_h>d_c-r_h$ ，由点Q在凸包里面可知 $r_h\leq r_{12}+r_{23}+r_{34}$ ，这两个关系式叠加后可得， $d_h>d_c-(r_{12}+r_{23}+r_{34})>0$ ，也即：

$d_c>0,r_{j,j+1}<\frac{d_c}3,(j\in\{1,2,3\})$

即，我们只需要保证障碍物到凸包顶点的距离大于0，并且满足 $r_{j,j+1}<\frac{d_c}3,(j\in\{1,2,3\})$ ,就可以保证凸包一定在障碍物外面。

4、 Fast Planner中的B样条曲线

在Fast Planner中并没有写出基函数到底是什么，而是用控制点去左乘一个M矩阵，再左乘一个时间归一化后的向量，即B样条曲线的某段分段曲线的表达式由下面的左式，变为右式：

$N_{i,p}(u)\mathbf{Q}_{i}\quad\textbf{ }{\rightarrow}{ }\quad\mathbf{s}(t)^{\mathrm{T}}\mathbf{M}_{p_{b}+1}\mathbf{q}_{m}$

其中，

$p_{b}=3$ 时，M矩阵如下所示：

$\mathbf{M}_{p_b+1}=\dfrac{1}{3!}\begin{bmatrix}1&4&0&0\\-3&0&3&0\\3&-6&3&0\\-1&3&-3&1\end{bmatrix}$

接下来来看一下上面的这些式子是怎么来的，我们前面推导出3阶B样条的基函数的表达式如下所示：

$N_{0,3}(t)=\dfrac{1}{6}\left\{\begin{matrix}t^3&t\in[0,1)\\t^2(2-t)+t(3-t)(t-1)+(4-t)(t-1)^2&t\in[1,2)\\t(3-t)^2+(t-1)(3-t)(4-t)+(4-t)^2(t-2)&t\in[2,3)\\(4-t)^3&t\in[3,4)\end{matrix}\right.$

现在，我们想要写出 $u_m.u_{m+1})$ 区间上的B样条曲线的表达式，根据B样条曲线的特性2可知，我们仅需要知道 $\mathbf{Q}_{m-3}$ 、 $\mathbf{Q}_{m-2}$ 、 $\mathbf{Q}_{m-1}$ 、 $\mathbf{Q}_m$ 、 $N_{m-3,3}$ 、 $N_{m-2,3}$ 、 $N_{m-1,3}$ 、 $N_{m,3}$ 这八个量，就可以根据 $\mathbf{C}(u)=\sum_{i=0}^nN_{i,p}(u)\mathbf{Q}_i$ 写出该段B样条曲线表达式。

在这里插入图片描述

这八个量中，4个控制点是直接给定的，只需要计算4个基函数，上面我们已经给出了 $N_{0,3}$ 的表达式，其对应的非零区间为[0,4)，如果我们可以将 $N_{m-3,3}$ 、 $N_{m-2,3}$ 、 $N_{m-1,3}$ 、 $N_{m,3}$ 的非零区间分别映射到区间[0,4)上，就可以直接调用 $N_{0,3}$ 的表达式了，比如 $N_{m-3,3}$ 我们进行以下的变化（注： $s(t)=(t-t_{m})/\Delta t$ ， ${\Delta u}=u_m-u_{m-1}=1$ ）

$\begin{aligned} \text{t}& =\frac{u-u_{m-3}}{\Delta u} \\ &=\frac{u-u_{m-3}+u_{m}-u_{m}}{\Delta u} \\ &=\frac{u-u_m+u_m-u_{m-3}}{\Delta u} \\ &=\frac{u-u_m}{\Delta u}+\frac{u_m-u_{m-3}}{\Delta u} \\ &=s(u)+3 \end{aligned}$

在这里插入图片描述

由上面的示意图可知 $u_m.u_{m+1})$ 位于 $N_{m-3,3}$ 的第4段小区间上，所以应该把上面得到的t的表达式代入到 $N_{0,3}(t)$ 的第4段小区间，也就是[3,4)的表达式上，所以

$\begin{aligned} N_{m-3,3}& =\frac16(4-s(u)-3)^3 \\ &=\frac16(1-s(u))^3 \\ &=\frac{1}{6}[-s(u)^{3}+3s(u)^{2}-3s(u)+1] \end{aligned}$

同理， $N_{m-2,3}$ 进行 $t=\frac{u-u_{m-2}}{\Delta u}=s(u)+2$ 变化， $u_m.u_{m+1})$ 位于 $N_{m-2,3}$ 第3段小区间上，所以代入到 $N_{0,3}(t)$ 的第3段小区间，也就是[2,3)的表达式上，所以

$\begin{aligned} &N_{m-2,3} \\ &=\frac{1}{6}[(s(u)+2)(1-s(u))^{2} \\ &+(s(u)+1)(1-s(u))(2-s(u)) \\ &+\left(2-s(u)\right)^2s(u)] \\ &=\frac{1}{6}[3s(u)^{3}-6s(u)^{2}+4] \end{aligned}$

同理， $N_{m-1,3}$ 进行 $t=\frac{u-u_{m-1}}{\Delta u}=s(u)+1$ 变化， $u_m.u_{m+1})$ 位于 $N_{m-1,3}$ 第2段小区间上，所以代入到 $N_{0,3}(t)$ 的第2段小区间，也就是[1,2)的表达式上，所以

$\begin{aligned} &N_{m-1,3} \\ &=\frac{1}{6}[(s(u)+1)^{2}(1-s(u)) \\ &+(s(u)+1)(2-s(u))s(u) \\ &+s(u)^2(3-s(u))] \\ &=\frac{1}{6}[-3s(u)^{3}+3s(u)^{2}+s(u)+1] \end{aligned}$

同理， $N_{m,3}$ 进行 $t=\frac{u-u_{m}}{\Delta u}=s(u)$ 变化， $u_m.u_{m+1})$ 位于 $N_{m,3}$ 第1段小区间上，所以代入到 $N_{0,3}(t)$ 的第1段小区间，也就是[0,1)的表达式上，所以

$N_{m,3}=\frac16s(u)^3$

我们将这四个表达式放在一起写成矩阵的形式，就是Fast Planner中的B样条曲线的表现形式

在这里插入图片描述

参考资料：

1、深蓝学院-移动机器人运动规划

慕羽★

关注

19
点赞
踩
12

收藏

觉得还不错? 一键收藏
打赏
0
评论
Fast Planner规划算法（二）—— Fast Planner后端之贝塞尔曲线与B样条曲线

本系列文章用于学习记录Fast-Planner规划算法的相关内容，主要学习资料是深蓝学院的移动机器人运动规划课程Fast Planner后端的主要任务是基于前端生成的轨迹，生成一条满足其他约束的轨迹，Fast Planner后端包含轨迹优化和迭代时间调整两部分
复制链接

扫一扫