资讯

历史

科技

环境与自然

成长

游戏

财经

文学与艺术

美食

健康

家居

文化

情感

汽车

三农

军事

旅行

运动

教育

生活

星座命理

运动规划中的曲线表示：贝塞尔曲线与B样条曲线详解

创作时间:

作者:

@小白创作中心

运动规划中的曲线表示：贝塞尔曲线与B样条曲线详解

引用

CSDN

https://blog.csdn.net/Kalenee/article/details/105090187

运动规划是机器人学中的一个重要领域，其中曲线规划是实现平滑运动的关键技术之一。贝塞尔曲线和B样条曲线作为两种常用的曲线表示方法，在运动规划中有着广泛的应用。本文将详细介绍这两种曲线的基本概念、数学公式以及实现方法，帮助读者更好地理解它们在运动规划中的应用。

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

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

1.1 n阶贝塞尔曲线公式

n阶贝塞尔曲线的数学公式如下：

$$
B(t) = \sum_{i=0}^{n} \textrm{C}_{n}^{i}P_i(1-t)^{n-i}t^i, \quad t \in [0,1]
$$

其中，

$$
\textrm{C}_{n}^{i} = \frac{n!}{(n-i)! \cdot i! }
$$

n阶贝塞尔曲线需要n+1个控制点。

1.2 曲线公式导出

以二阶贝塞尔曲线为例，其推导过程如下：

线段上的控制点

$$
P_0^{'} = (1 - t)P_0 + tP_1 \
P_1^{'} = (1 - t)P_1 + tP_2
$$

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

$$
\begin{aligned}
B_{2}(t) &= (1 - t)P_0^{'} + tP_1^{'} \
&= (1 - t)((1 - t)P_0 + tP_1) + t((1 - t)P_1 + tP_2) \
&= (1 - t)^2P_0 + 2t(1 - t)P_1 + t^2P_2
\end{aligned}
$$

曲线公式

$$
B_{2}(t) = P_0(1 - t)^2 + 2P_1(1 - t)t + P_2 t^2 , \quad 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 , \quad 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 曲线分类

均匀B样条曲线

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

准均匀B样条曲线

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

分段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()