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

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

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

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

{x=a+rcos⁡θy=b+rsin⁡θ(1.1)\left\{ \begin{aligned} &x = a + r\cos\theta\\ &y = b + r\sin\theta \end{aligned} \right. \tag{1.1} {x=a+rcosθy=b+rsinθ(1.1)
其中：θ∈[0,2π)\theta\in[0,2\pi)θ∈[0,2π)。θ\thetaθ则为直观的角度,θ\thetaθ从0变化到2π2\pi2π，直线顺时针变化。

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

二、定义

2.1 数值定义

给定n+1个空间上的点P0,P1,P2…,PnP_0,P_1,P_2\dots,P_nP0,P1,P2…,Pn，贝塞尔曲线的计算公式为：
C(u)=∑i=0nBn,i(u)Pi(1.3)C(u)=\sum^n_{i=0}B_{n,i}(u)P_i\tag{1.3}C(u)=i=0∑nBn,i(u)Pi(1.3)

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

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

给出以下性质：

u∈[0,1]u\in[0,1]u∈[0,1]，当u=0时位于起点，u=1时终点；
给定参数u基函数和为1。观察可知表达式是二项展开式(u+(1−u))n=1(u+(1-u))^n=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 mathclass Bezier:# 输入控制点，Points是一个array,num是控制点间的插补个数def __init__(self,Points,InterpolationNum):self.demension=Points.shape[1]   # 点的维度self.order=Points.shape[0]-1     # 贝塞尔阶数=控制点个数-1self.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.Pointsfor 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*dppis=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学习的对比有感。