B-spline and Bezier Curve

介绍一下robotics运动规划方向的B样条曲线与贝塞尔曲线相关知识。

0728：TODO，节点向量如何得到？

• 贝塞尔曲线，B-样条，非均匀有理B样条梳理
• 曲线篇: 贝塞尔曲线
• Animated Bézier Curves
• Bézier curve Wikipedia，建议看维基百科上的图片
• 详解样条曲线（上）（包含贝塞尔曲线）

1. Bezier, 贝塞尔曲线

Bézier curves can be used in robotics to produce trajectories of an end-effector due to the virtue of the control polygon’s ability to give a clear indication of whether the path is colliding with any nearby obstacle or object.[30] Furthermore, joint space trajectories can be accurately differentiated using Bézier curves. Consequently, the derivatives of joint space trajectories are used in the calculation of the dynamics and control effort (torque profiles) of the robotic manipulator.[30]

1.1 贝塞尔曲线的定义

$m$次贝塞尔曲线定义为：
$$\mathbf{b}(u)=\sum _{j=0}^m{B}_j^m(u){\mathbf{p}}_{j}0\le u\le 1$$

其中，${\mathbf{p}}_j$ 为控制点，基函数 $B_j^m(u)$ 为 $m$ 次伯恩斯坦多项式，定义为：

$$B_j^m(u)=\frac{m!}{j!(m-j)!}{u}^j(1-u{)}^{m-j}$$

且 $j=0,...,m$，给定$m+1$个控制点，得到$m$阶贝塞尔曲线；贝塞尔曲线的阶数与控制点的个数相关。

下图为四个控制点情况下的Cubic Bezier curve：

1.2 贝塞尔曲线的导数

$m$次贝塞尔曲线的导数为$m-1$次贝塞尔曲线，定义为：
$${\mathbf{b}}^{(1)}(u)=m\sum _{i=0}^{m-1}{B}_i^{m-1}(u)({\mathbf{p}}_{i+1}-{\mathbf{p}}_i)$$

特别地，贝塞尔曲线端点处的$k$阶导数只取决于此端点前的$k+1$个控制点，如端点处的一阶导数为：

$${\mathbf{p}}^{(1)}(0)=m({\mathbf{p}}_1-{\mathbf{p}}_0),{\mathbf{p}}^{(1)}(1)=m({\mathbf{p}}_{\mathbf{m}}-{\mathbf{p}}_{m-1})$$

端点处的二阶导数为：

$${\mathbf{p}}^{(2)}(0)=m(m-1)({\mathbf{p}}_0-2{\mathbf{p}}_1+{\mathbf{p}}_2),{\mathbf{p}}^{(2)}(1)=m(m-1)({\mathbf{p}}_m-2{\mathbf{p}}_{m-1}+{\mathbf{p}}_{m-2})$$

其余各阶导数也可以由上式推出。

1.3 贝塞尔曲线的性质

贝塞尔曲线的性质有：

• 凸包特性：由控制点构成的控制多边形逼近样条曲线的形状，曲线完全包含在其控制点形成的凸包中
• 终点插值特性：即一段贝塞尔曲线初始于第0个控制点，终止于第m个也就是最后一个控制点，并且不会经过其他控制点（除非所有控制点共线），公式表示为${\mathbf{p}}_0=\mathbf{b}(0)$和${\mathbf{p}}_m=\mathbf{b}(1)$
• 固定时间间隔特性：每一条贝塞尔曲线总是定义在固定时间间隔$t\in [0,1]$上
• ${\mathbf{p}}_0$处的切线方向平行于${\mathbf{p}}_0$与${\mathbf{p}}_1$连线的方向，${\mathbf{p}}_m$处的切线方向平行于${\mathbf{p}}_m$与${\mathbf{p}}_{m-1}$连线的方向
• 速端曲线（Hodograph）：贝塞尔曲线对时间的一阶微分得到的仍然是贝塞尔曲线，新的控制点可以有原始曲线的控制点表示，${\mathbf{p}}_i^{(1)}=m({\mathbf{p}}_{i+1}-{\mathbf{p}}_i)$；注，速端曲线代表物体的向量运动

1.4 贝塞尔曲线与多项式的转换

贝塞尔曲线与标准多项式的转换：
$$\mathbf{b}(u)=\sum _{i=0}^m{\mathbf{a}}_i{u}^i$$
其中，系数${\mathbf{a}}_i$为：

$${\mathbf{a}}_i=\frac{m!}{(m-i)!}\sum _{j=0}^i\frac{(-1{)}^{i+j}}{j!(i-j)!}{\mathbf{p}}_j$$

例，$m=3$时，贝塞尔曲线的形式为：
$$\mathbf{b}(u)=(1-u{)}^3{\mathbf{p}}_0+3u(1-u{)}^2{\mathbf{p}}_1+3u^2(1-u){\mathbf{p}}_2+u^3{\mathbf{p}}_3$$

2. B-Spline B样条函数

样条曲线为分段多项式函数，一种有效计算样条的方法是基于B样条，也就是基础样条；这个名称的又来是因为一般样条可通过适量的B样条基函数$[B_j^p(u)]$的线性组合获得，即：
$$\mathbf{s}(u)=\sum _{j=0}^m{\mathbf{p}}_j{B}_j^p(u)u_{min}\le u\le u_{max}$$
其中，系数 ${\mathbf{p}}_j(j=0,...,m)$ 称为控制点，在运动规划中，这些控制点一般由前端搜索得到。其定义与贝塞尔曲线的定义基本一致，B样条曲线是贝塞尔曲线的一般化，不同之处在于当有$m+1$个控制点时，贝塞尔曲线为$m$次，而B样条曲线为$[2,m]$次。

2.1 B样条基函数

设$\mathbf{u}=[u_0,...,u_{n-knot}]$为节点向量（注意，此处为节点向量而不是控制点，控制点使用$p$定义），且$\mathbf{u}$不递减，$u_j\le u_{j+1}$，（可以想到，这样的节点向量，或参数向量设计，与实际中的时间不谋而合，因此也可以利用B样条的这一特点进行时间分配）则第$j$个控制点的$p$次B样条函数的递归形式如下：
$$B_j^0(u)=\{\begin{array}{l}1,u_j\le u\le u_{j+1}\\ 0,\text{e}lse\end{array}$$

$$B_j^p(u)=\frac{u-u_j}{u_{j+p}-u_j}{B}_j^{p-1}(u)+\frac{u_{j+p+1}-u}{u_{j+p+1}-u_{j+1}}{B}_{j+1}^{p-1}(u)$$

其中：

• (1) $B_j^p(u)$是分段多项式，其中$\forall u\in [u_{min},u_{max}] $，且$B_j^p(u)$非负
• (2) 在区间$u\in [u_j,u_{j+p+1})$之外，$B_j^p(u)=0$；也就是说，调整$B_j^p(u)$，其影响仅产生在对应的区间上，而不会对全局空间造成影响，可以称为局部支撑作用，这对轨迹replan有很大帮助
• (3) 区间$[u_i,u_{i+1})$称为第$i$节点区间，区间长度可能为0，此时区间重合；这是因为节点$u$有可能是相同的值，而断点则一定是不同的值，断点是一组不同的节点值

(4) B样条基函数归一化如下（权和性，权重的和为1）：

$$\sum _{j=0}^m{B}_j^p(u)=1,\forall u\in [u_0,u_{n_{knot}}]$$

(5) 在每个节点区间$[u_i,u_{i+1})$内，最多$p+1$个基函数$B_j^p$不为零，即$B_{i-p}^p,...,B_i^p$不为零；

可以看出，任何一个基函数，追根溯源的最底层都是$B_j^0$，非1即0；之后再经过截断三角形的传播，得到最后的基函数；当然，任意一个基函数只有在其对应的有定义域的区间才有意义，否则为零。

一个简单的截断三角形构造如下：
$$\begin{array}{ccccc}{B}_0^0& & & & \\ & B_0^1& & & \\ B_1^0& & B_0^2& & \\ & B_1^1& & B_0^3& \\ B_0^2& & B_1^2& & B_0^4\\ & B_2^1& & B_1^3& \\ B_0^3& & B_2^2& & \\ & B_3^1& & & \\ B_0^4& & & & \end{array}$$
由左至右，根据三角形位置和迭代公式求得每一个基函数的表达式（或值）。

2.2 B样条的定义和性质

假设B样条是由非均匀节点向量（大小为$n_{knot}$）定义的：

$$\mathbf{u}=[u_{min},...,u_{min},u_{p+1},...,u_{n_{knot}-p-1},u_{max},...,u_{max}]$$

有$p+1$个$u_{min}$和$u_{max}$，则$p$次B样条曲线定义为：

$$\mathbf{s}(u)=\sum _{j=0}^m{\mathbf{p}}_j{B}_j^p(u)$$

其中，${\mathbf{p}}_j(j=0,...,m)$为控制点，控制点组成了控制多边形，因此，要表示一条B样条曲线，需要有：

• B样条曲线次数，整数$p$，（不是阶数）
• 节点向量$\mathbf{u}$
• $\mathbf{s}(u)$的系数，控制点$\mathbf{P}=[{\mathbf{p}}_0,{\mathbf{p}}_1,...,{\mathbf{p}}_{m-1},{\mathbf{p}}_m]$

所以对于$m+1$个控制点，其$p$次B样条曲线所需的节点向量个数为$1+m+p$个，会将区间划分成$m+p$段。

$$n_{knot}=1+m+p$$

即样条曲线的次数$p$，控制点的数量$m+1$与节点的数量$n_{knot}+1$之间存在上述关系。注意，样条的阶数=次数+1

B样条曲线有利于生成轨迹，其性质有：

• $\mathbf{s}(u)$在节点区间内是无限可微的，且在多重节点$k$处$p-k$次连续可微
• 样条曲线的次数$p$，控制点的数量$m+1$与节点的数量$n_{knot}+1$之间存在$n_{knot}=1+m+p$
• 端点插值特性：$\mathbf{s}(u_{min})={\mathbf{p}}_0$且$\mathbf{s}(u_{max})={\mathbf{p}}_m$
• 该曲线在仿射变换（平移，旋转，缩放，剪切）下不变，可以通过将其应用于控制点来应用于$\mathbf{s}(u)$
• 局部修改：控制点${\mathbf{p}}_j$的变化只在区间$[u_j,u_{j+p+1}]$上修改$\mathbf{s}(u)$
• 通过在节点处使用变换，B样条曲线可以实时缩放；如果假设有${\mathbf{u}}^{\prime }=\lambda \mathbf{u}$，则该${\mathbf{u}}^{\prime }$的B样条的$i$次导数将被缩放为$1/{\lambda }^i$；若$\lambda =2$，则新B样条的速度是原始样条的$1/2$，加速度是原始样条的$1/4$

2.3 B样条曲线的导数

有B样条曲线$\mathbf{s}(u)$的非均匀节点向量：

$$\mathbf{u}=[u_{min},...,u_{min},u_{p+1},...,u_{n_{knot}-p-1},u_{max},...,u_{max}]$$

其一阶导数，即对时间取一次微分为：

$${\mathbf{s}}^{(1)}(u)=\sum _{j=0}^m{B}_j^{p(1)}(u){\mathbf{p}}_{j}u\in [u_{min},u_{max}]$$

其中，B样条基函数的一阶导数为：

$$B_j^{p(1)}(u)=\frac{p}{u_{j+p}-u_j}{B}_j^{p-1}(u)-\frac{p}{u_{j+p+1}-u_{j+1}}{B}_{j+1}^{p-1}(u)$$

求一次导之后新的节点向量为：

$${\mathbf{u}}^{\prime }=[u_{min},...,u_{min},u_{p+1},...,u_{n_{knot}-p-1},u_{max},...,u_{max}]$$

与求导之前不同的是，$u_{min}$与$u_{max}$在节点向量中的个数均由$p+1$变为$p$个。

同时，控制点${\mathbf{q}}_j$也需要更新为：

$${\mathbf{q}}_j=p\frac{{\mathbf{p}}_{j+1}-{\mathbf{p}}_j}{u_{j+p+1}-u_{j+1}}$$

2.4 B样条曲线的求解

对于某固定值的自变量$\bar{u}$，可以通过仅考虑在包含其的第$i$个节点区间，计算不为零的$p+1$个基函数，来计算B样条曲线。
$$\mathbf{s}(\bar{u})=\sum _{j=i-p}^i{\mathbf{p}}_j{B}_j^p(\bar{u})$$

• 找到$\bar{u}$所属的区间索引$i$，即$[u_i,u_{i+1})$
• 给定$i$，计算$\bar{u}$处的基函数
• 通过上式计算$\mathbf{s}(\bar{u})$

一道求解的例题：

现有$m+1=7$个控制点，$n_{knot}+1=11$个节点向量，且$u=1.5$，求其基函数。

$$\mathbf{P}=[{\mathbf{p}}_0,...,{\mathbf{p}}_m]=\left[\begin{array}{ccccccc}1& 2& 3& 4& 5& 6& 7\\ 2& 3& -3& 4& 5& -5& -6\end{array}\right]$$

节点向量$\mathbf{u}=[0,0,0,0,1,2,4,7,7,7,7]$，已知 $u=1.5$，首先判断出其所属区间为$[1,2)$，即$[u_4,u_5)$；

由于$B_7^3(u)$在$[u_7,u_{11}]$以为为0，且$u_{11}$已经不存在，因此只需考虑$B_j^3(u)$，$j=0,...,6$；

对于$B_0^3(u)$，其在$[u_0,u_4)$以外的区域为0，因此不做考虑，$B_6^3(u)$同理；

因此只需计算$B_j^3(u)$，$j=1,2,3,4$；

对于$B_1^3(u)$，根据截断三角形，需要从$B_1^0$到$B_4^0$开始从左向右计算；这四个基函数中仅$B_4^0(u)$在$[u_4,u_5)$上为1，其余全为0；

进而计算$B_3^1(u)$：
$$B_3^1(u)=0+\frac{u_5-u}{u_5-u_4}{B}_4^0(u)=2-u$$
继续递归，计算$B_2^2(u)$：
$$B_2^2(u)=0+\frac{u_5-u}{u_5-u_3}{B}_3^1(u)=\frac{(2-u{)}^2}{2}$$
继续递归，计算$B_1^3(u)$：
$$B_1^3(u)=0+\frac{u_5-u}{u_5-u_2}{B}_2^2(u)=\frac{(2-u{)}^3}{4}$$
代入得到$B_1^3=0.0313,B_2^3=0.5885,B_3^3=0.3733,B_4^3=0.0069$，代入下式：
$$\mathbf{s}(u)=\sum _{j=0}^m{\mathbf{p}}_j{B}_j^p(u)$$
得到$\mathbf{s}(u=1.5)=[3.3245,-0.1753]$；如果将$u$看作时间，则该点即为时间为1.5时，轨迹的位置。