运动规划——B样条曲线

一、贝塞尔曲线（Bézier curve）

贝塞尔曲线主要用于二维图形应用程序中的数学曲线，曲线由起始点，终止点（也称锚点）和控制点组成，通过调整控制点，通过一定方式绘制的贝塞尔曲线形状会发生变化。

n阶贝塞尔曲线公式
$$B(t)=\sum _{i=0}^n{\text{C}}_n^i{P}_i(1-t{)}^{n-i}{t}^i,t\in [0,1]$$
式中${\text{C}}_n^i=\frac{n!}{(n-i)!\cdot i!}$，n阶贝塞尔曲线需要n+1个控制点。

1.2 曲线公式导出

上图为二阶贝塞尔曲线，在$P_0{P}_1$上利用一阶公式求出点$P_0^{{}^{\prime }}$，然后在$P_1{P}_2$上利用一阶公式求出点$P_1^{{}^{\prime }}$，最后在$P_1^{{}^{\prime }}{P}_2^{{}^{\prime }}$上再利用一阶公式就可以求出最终贝塞尔曲线上的点$P_0^{{}^{\prime \prime }}$。

• 线段上的控制点

$$\begin{gathered} P_0^{{}^{\prime }}=(1-t)P_0+t{P}_1 \\ {P}_1^{{}^{\prime }}=(1-t)P_1+t{P}_2 \end{gathered}$$

• 将上面的公式带入至下列公式中

$$\begin{array}{rl}{B}_2(t)& =(1-t)P_0^{{}^{\prime }}+t{P}_1^{{}^{\prime }}\\ & =(1-t)((1-t)P_0+t{P}_1)+t((1-t)P_1+t{P}_2)\\ & =(1-t{)}^2{P}_0+2t(1-t)P_1+t^2{P}_2\end{array}$$

• 曲线公式

$$B_2(t)=P_0(1-t{)}^2+2P_1(1-t)t+P_2{t}^2,t\in [0,1]$$

• 标准化

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

注意：详细可参考深入理解贝塞尔曲线

二、B样条曲线（B-Spline）

B-样条是贝塞尔曲线(Bézier curve)的一种一般化，B样条不能表示一些基本的曲线，比如圆，所以引入了NURBS，可以进一步推广为非均匀有理B-样条(NURBS)

2.1 基本概念

• 节点（knot）

  样条曲线（蓝色曲线）映射到定义域（底部黑线）内，在定义域内的点称为节点，定义域范围并不固定，一般使用[0,1]

• 节点点（knot point）

  为节点在曲线上的对应点，图上黄色点

• 节点矢量（knot vector）
  
  对应的节点矢量：$[0\frac{1}{7}\frac{2}{7}\frac{3}{7}\frac{4}{7}\frac{5}{7}\frac{6}{7}1]$

节点向量$U=u_0,\cdots ,u_m$为一个非递减数的集合，将定义域划分为几个区间，当节点在不同区间，采用不同计算，需要满足$m=n+k+1$（n为控制点数量，k为次数）

• 系数（spline coefficients）
  
  每个系数对应一个控制点$P_i=p_0,\cdots ,p_n$，控制点为输入路径点，程序根据控制点拟合样条曲线

• 基函数的次数（B-spline order）

  样条基函数的次数$k$

2.2 样条曲线公式

$$p(u)=\sum _{i=0}^n{d}_i{N}_{i,k}(u)$$

$p(u)$为曲线点，$u$为节点域内的细分，$d_i$为控制点，$N_{i,k}(u)$为基函数。

B样条的基函数通常采用Cox-deBoor递推公式

2.3 曲线分类

2.3.1 均匀B样条曲线

注意：均匀B样条曲线一般来说无法通过起始点和终点，在生成曲线时，需要调整生成曲线的区间（小于$[0,1]$），否则生成的曲线有部分无法使用，一般不太建议采用均匀B样条曲线

2.3.2 准均匀B样条曲线

节点矢量中两端节点具有重复度k+1，内节点均匀分布重复度为1。

2.3.3 分段Bezier曲线

节点矢量中两端节点的重复度k+1，内节点重复度为k，条件$\frac{n}{k}=正整数$

2.4 程序实现

• scipy.interpolate.BSpline

• B-Spline Geometry

from geomdl import BSpline

# 创建三维B样条曲线
curve = BSpline.Curve()
# 设置曲线次数k
curve.degree = 3
# 设置控制点
curve.ctrlpts = [[10, 5, 10], [10, 20, -30], [40, 10, 25], [-10, 5, 0]]
# 设置节点矢量
curve.knotvector = [0, 0, 0, 0, 1, 1, 1, 1]
# 设置递增步距，生成总点数为1/0.05=20
curve.delta = 0.05
# 获取曲线点
curve_points = curve.evalpts

• 自己实现

import numpy as np
import math
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

class B_Splines_Class:
    def __init__(self,d,k):
        '''
        d:控制点
        k:曲线次数
        '''
        self.d = d
        self.d_n = len(d)
        self.k = k

        #self.NodeVector = np.linspace(0,1,self.d_n + self.k + 1).tolist() # 均匀
        self.NodeVector = self.U_semiequal(self.d_n,self.k)           # 准均匀


    def gen_spline(self):
        _path = []
        u = 0.0# 起点，默认使用[0,1]
        while u <= 1.0-0.0:# 终点
            point = sum(self.d[i] * self.BaseFunction(i, self.k, u, self.NodeVector) for i in range(self.d_n))
            _path.append(point.tolist())
            u = u + 0.001# 步距

        return _path

    
    def BaseFunction(self,i,k,u,NodeVector):
        if k == 0:
            return 1.0 if NodeVector[i] <= u < NodeVector[i+1] else 0.0

        #计算基函数
        if NodeVector[i+k] == NodeVector[i]:
            c1 = 0.0
        else:
            c1 = (u - NodeVector[i])/(NodeVector[i+k] - NodeVector[i]) * self.BaseFunction(i, k-1, u, NodeVector)

        if NodeVector[i+k+1] == NodeVector[i+1]:
            c2 = 0.0
        else:
            c2 = (NodeVector[i+k+1] - u)/(NodeVector[i+k+1] - NodeVector[i+1]) * self.BaseFunction(i+1, k-1, u, NodeVector)

        return c1 + c2


    def U_semiequal(self,n,k):
        # 准均匀
        piecewise = n - k     #曲线的段数
        if piecewise == 1:    # 一段曲线
            NodeVector = np.ones(n+k+1)
        else:                 # 多段曲线
            NodeVector = np.zeros(n+k+1)
            flag = 0
            while flag != piecewise:
                NodeVector[k+1+flag] =  NodeVector[k+flag] + 1.0/piecewise
                flag = flag+1
            NodeVector[n:] = 1

        return NodeVector.tolist()

if __name__=='__main__':
    # 二维
    # d = np.array([[1,2],[2,4],[3,8],[4,9],[6,9],[7,11],[8,15]])
    # 三维
    d = np.array([[10, 5, 10], [10, 20, -30], [40, 10, 25], [-10, 5, 0]])
    k = 2 
    test = B_Splines_Class(d,k)
    path = test.gen_spline()

    # 绘图
    # 二维
    # plt.plot([x[0] for x in path],[y[1] for y in path])
    # plt.scatter([x[0] for x in d],[y[1] for y in d])
    
    # 三维
    fig = plt.figure()
    ax = Axes3D(fig)
    ax.plot([x[0] for x in path],[y[1] for y in path],[z[2] for z in path])
    ax.scatter([x[0] for x in d],[y[1] for y in d],[z[2] for z in d])
    
    plt.show()

参考

深入理解贝塞尔曲线

1.4.2 B-spline curve - MIT

贝兹曲线(Bezier)和nurms(NURMS)曲线有什么关系吗？

B样条曲线曲面（附代码）

B样条曲线（B-spline Curves）

B-样条曲线教程（B-spline Curves Notes）目录

matlab-B-splines

motionLib