（一）轨迹规划：贝塞尔曲线的python实现

一、为什么要使用贝塞尔曲线？

在参数方程中，参数不都是有明显几何意义的。

参数方程可以表示空间中的曲线，也可以表示空间中的曲面。如半径长为r、圆心在(a,b)的平面圆，其参数方程为：

$$\{\begin{array}{rl}& x=a+r\cos \theta \\ & y=b+r\sin \theta \end{array}\text{\hspace{1em}(1.1)}$$
其中：$\theta \in [0,2\pi )$。$\theta$则为直观的角度,$\theta$从0变化到$2\pi$，直线顺时针变化。

又如球面，球心在坐标原点，半径为R的球面。参数方程：
$$\{\begin{array}{rl}& x=Rsin\varphi \cos \theta \\ & y=Rsin\varphi \sin \theta \\ & z=Rcos\varphi \end{array}\text{\hspace{1em}(1.2)}$$
对于球面，如果我们改变$\theta$，那么曲面上的点的变化方向是什么？如果同时修改$\theta$和$\varphi$又是如何变化的？显然我们几乎不可能预测形状变化，因此我们说几何意义不明显。更重要的是，在工程、设计领域上不是所有人都关心方程式，人们更加关心如何去设计其产品、完成其生产任务。贝塞尔曲线就是这样一种方法，具有很多优点如：几何直观、灵活（控制点操纵）、统一（与形状无关）和平移、旋转不变等优点，还具有数值稳定性。

二、定义

2.1 数值定义

给定n+1个空间上的点$P_0,P_1,P_2\dots ,P_n$，贝塞尔曲线的计算公式为：
$$C(u)=\sum _{i=0}^n{B}_{n,i}(u)P_i\text{\hspace{1em}(1.3)}$$

其中参数为u的权重系数$B_{n,i}(u)$计算方法如下：
$$B_{n,i}=\frac{n!}{i!(n-i)!}{u}^i(1-u{)}^{n-i}\text{\hspace{1em}(1.4)}$$

由表达式(1.3)可以看出，最终生成的点C(u)与每个点$P_0,P_1,P_2\dots ,P_n$均有关系，这些点“控制”了曲线的最终走向，因此也称为控制点，贝塞尔阶数为n,由n+1个控制点控制。

给出以下性质：

• $u\in [0,1]$，当u=0时位于起点，u=1时终点；
• 给定参数u基函数和为1。观察可知表达式是二项展开式$(u+(1-u){)}^n=1$，下图是四阶贝塞尔基函数随着u变化的图像；
• 凸包性。曲线在凸包之内，可预测曲线行进方向；
  

2.2 德卡斯特里奥（DeCasteljau）算法

德卡斯特里奥（DeCasteljau）算法是一个数值稳定的方法，而（2.1）节提到的方法是一种数值不稳定的方法。对于一个已经存在的算法，若输入数据的误差在计算过程中迅速增长而得不到控制，则称该算法是不稳定的，否则是数值稳定的[1]。尤其是计算基函数时需要用到u的n次幂函数，计算机存储精度影响下，数值就变得更不稳定了。

算法的原理也比较直观，依次连接每个控制点，形成多个线段，每个线段在比例u:1-u处获取新控制点，在新的控制点基础上再进行划分，当控制点数仅剩一个时，该点就是系数u对应的C(u)。如果还不理解的话，可以去这个地方感受一下https://zh.javascript.info/bezier-curve。

三、python的实现

import matplotlib.pyplot as plt
import numpy as np
import math

class Bezier:
    # 输入控制点，Points是一个array,num是控制点间的插补个数
    def __init__(self,Points,InterpolationNum):
        self.demension=Points.shape[1]   # 点的维度
        self.order=Points.shape[0]-1     # 贝塞尔阶数=控制点个数-1
        self.num=InterpolationNum        # 相邻控制点的插补个数
        self.pointsNum=Points.shape[0]   # 控制点的个数
        self.Points=Points
        
    # 获取Bezeir所有插补点
    def getBezierPoints(self,method):
        if method==0:
            return self.DigitalAlgo()
        if method==1:
            return self.DeCasteljauAlgo()
    
    # 数值解法
    def DigitalAlgo(self):
        PB=np.zeros((self.pointsNum,self.demension)) # 求和前各项
        pis =[]                                      # 插补点
        for u in np.arange(0,1+1/self.num,1/self.num):
            for i in range(0,self.pointsNum):
                PB[i]=(math.factorial(self.order)/(math.factorial(i)*math.factorial(self.order-i)))*(u**i)*(1-u)**(self.order-i)*self.Points[i]
            pi=sum(PB).tolist()                      #求和得到一个插补点
            pis.append(pi)            
        return np.array(pis)

    # 德卡斯特里奥解法
    def DeCasteljauAlgo(self):
        pis =[]                          # 插补点
        for u in np.arange(0,1+1/self.num,1/self.num):
            Att=self.Points
            for i in np.arange(0,self.order):
                for j in np.arange(0,self.order-i):
                    Att[j]=(1.0-u)*Att[j]+u*Att[j+1]
            pis.append(Att[0].tolist())

        return np.array(pis)

class Line:
    def __init__(self,Points,InterpolationNum):
        self.demension=Points.shape[1]    # 点的维数
        self.segmentNum=InterpolationNum-1 # 段数
        self.num=InterpolationNum         # 单段插补(点)数
        self.pointsNum=Points.shape[0]   # 点的个数
        self.Points=Points                # 所有点信息
        
    def getLinePoints(self):
        # 每一段的插补点
        pis=np.array(self.Points[0])
        # i是当前段
        for i in range(0,self.pointsNum-1):
            sp=self.Points[i]
            ep=self.Points[i+1]
            dp=(ep-sp)/(self.segmentNum)# 当前段每个维度最小位移
            for i in range(1,self.num):
                pi=sp+i*dp
                pis=np.vstack((pis,pi))         
        return pis

# points=np.array([
#     [1,3,0],
#     [1.5,1,0],
#     [4,2,0],
#     [4,3,4],
#     [2,3,11],
#     [5,5,9]
#     ])
points=np.array([
    [0.0,0.0],
    [1.0,0.0],
    [1.0,1.0],
    [0.0,1.0],
    ])
    


if points.shape[1]==3:
    
        fig=plt.figure()
        ax = fig.gca(projection='3d')
        
        # 标记控制点
        for i in range(0,points.shape[0]):
            ax.scatter(points[i][0],points[i][1],points[i][2],marker='o',color='r')
            ax.text(points[i][0],points[i][1],points[i][2],i,size=12)
        
        # 直线连接控制点
        l=Line(points,1000)
        pl=l.getLinePoints()
        ax.plot3D(pl[:,0],pl[:,1],pl[:,2],color='k')
        
        # 贝塞尔曲线连接控制点
        bz=Bezier(points,1000)
        matpi=bz.getBezierPoints(0)
        ax.plot3D(matpi[:,0],matpi[:,1],matpi[:,2],color='r')
        plt.show()
if points.shape[1]==2:  
    
        # 标记控制点
        for i in range(0,points.shape[0]):
                plt.scatter(points[i][0],points[i][1],marker='o',color='r')
                plt.text(points[i][0],points[i][1],i,size=12)
                
         # 直线连接控制点
        l=Line(points,1000)
        pl=l.getLinePoints()
        plt.plot(pl[:,0],pl[:,1],color='k')
        
        # 贝塞尔曲线连接控制点
        bz=Bezier(points,1000)
        matpi=bz.getBezierPoints(1)
        plt.plot(matpi[:,0],matpi[:,1],color='r')
        plt.show()

下面是实现的效果：

[1] 曲线篇: 贝塞尔曲线

20201021：重写数值解求贝塞尔曲线的方法；
20201022：增加德卡斯特里奥（DeCasteljau）方法，该方法在求解贝塞尔曲线具有数值稳定性；
20201023：重新整理文字部分说明；
20201223：控制点控制着曲线的形状，连成的段数等于贝塞尔阶数。就好像给定n个点（控制点）其段数为n-1(阶数)，NUBRS学习的对比有感。