贝塞尔曲线原理

一、 低阶贝塞尔曲线

1.一阶贝塞尔曲线

如下图所示，$P_0$, $P_1$ 是平面中的两点，则 $P(t)$ 代表平面中的一段线段。

考虑这样一条曲线方程
$$\begin{array}{r}P(t)=P_0+(P_1-P_0)t=(1-t)P_0+t{P}_1\end{array}$$
一阶曲线很好理解， 就是根据$t$来的线性插值，$t$的取值范围为 $[0,1]$。

python实现

# 保存动图时用，pip install celluloid
from celluloid import Camera 
import numpy as np
import matplotlib.pyplot as plt
P0=np.array([0,0])
P1=np.array([1,1])
fig=plt.figure(1)
camera = Camera(fig)
x =[]
y=[]
for t in np.arange(0,1,0.01):
    plt.plot([P0[0],P1[0]],[P0[1],P1[1]],'r')
    p1_t=(1-t)*P0+t*P1
    x.append(p1_t[0])
    y.append(p1_t[1])
    # plt.plot(x,y,c='b')
    plt.scatter(x,y,c='b')
    # plt.pause(0.001)
    camera.snap()
animation = camera.animate()
animation.save('一阶贝塞尔.gif')

matlab实现

from celluloid import Camera  # 保存动图时用，pip install celluloid
import numpy as np
import matplotlib.pyplot as plt
P0=np.array([0,0])
P1=np.array([1,1])
fig=plt.figure(1)
camera = Camera(fig)
x =[]
y=[]
for t in np.arange(0,1,0.01):
    plt.plot([P0[0],P1[0]],[P0[1],P1[1]],'r')
    p1_t=(1-t)*P0+t*P1
    x.append(p1_t[0])
    y.append(p1_t[1])
    # plt.plot(x,y,c='b')
    plt.scatter(x,y,c='b')
    # plt.pause(0.001)
    camera.snap()
animation = camera.animate()
animation.save('一阶贝塞尔.gif')

2. 二阶贝塞尔曲线

对于二阶贝塞尔曲线，需要3个控制点，假设分别为 $P_0$，$P_1$，$P_2$​。 $P_0$ 和$P_1$ ​构成一阶$P_{1,1}(t)$， $P_1$​和 $P_2$​​也构成一阶$P_{1,2}(t)$，即：
$$\begin{array}{r}\{\begin{array}{c}{P}_{1,1}(t)=(1-t)P_0+t{P}_1\\ P_{1,2}(t)=(1-t)P_1+t{P}_2\end{array}\end{array}$$
在生成的两个一阶点$P_{1,1}(t)$，$P_{1,2}(t)$基础上，可以生成二阶贝塞尔点$P_2(t)$:
$$\begin{array}{r}{P}_2(t)=(1-t)P_{1,1}+t{P}_{1,2}=(1-t{)}^2{P}_0+2t(1-t)P_1+t^2{P}_2\end{array}$$

注意：最多一阶导数不为常数

python实现

from celluloid import Camera  # 保存动图时用，pip install celluloid
import numpy as np
import matplotlib.pyplot as plt
P0 = np.array([0, 0])
P1 = np.array([1,1])
P2 = np.array([2, 1])
fig = plt.figure(2)
camera = Camera(fig)

x_2 = []
y_2 = []
for t in np.arange(0, 1, 0.01):
    plt.cla()
    plt.plot([P0[0], P1[0]], [P0[1], P1[1]], 'k')
    plt.plot([P1[0], P2[0]], [P1[1], P2[1]], 'k')
    p11_t = (1-t)*P0+t*P1
    p12_t = (1-t)*P1+t*P2
    p2_t = (1-t)*p11_t+t*p12_t
    
    x_2.append(p2_t[0])
    y_2.append(p2_t[1])
    plt.scatter(x_2, y_2, c='r')
    plt.plot([p11_t[0],p12_t[0]],[p11_t[1],p12_t[1]],'g')
    plt.title("t="+str(t))
    plt.pause(0.001)
#     camera.snap()
# animation = camera.animate()
# animation.save('2阶贝塞尔.gif')

matlab实现

clc;
clear;
close all;
%% 二次贝塞尔曲线
P0=[0,0];
P1=[1,1];
P2=[2,1];
P=[P0;
    P1;
    P2];
figure(2);
plot(P(:,1),P(:,2),'k');
MakeGif('二次贝塞尔曲线.gif',1);
hold on
 
scatter(P(:,1),P(:,2),200,'.b');
for t=0:0.01:1
    P_t_1=(1-t) * P0 + t * P1;
    P_t_2=(1-t) * P1 + t * P2;
    P_t_3=(1-t) * P_t_1 + t * P_t_2;
    
    m1=scatter(P_t_1(1),P_t_1(2),300,'g');
    m2=scatter(P_t_2(1),P_t_2(2),300,'g');
    
    m3=plot([P_t_1(1),P_t_2(1)],[P_t_1(2),P_t_2(2)],'g','linewidth',2);
    
    scatter(P_t_3(1),P_t_3(2),300,'.r');  
    stringName = "二次贝塞尔曲线：t="+num2str(t);
    title(stringName);
    MakeGif('二次贝塞尔曲线.gif',t*100+1);
    delete(m1);
    delete(m2);
    delete(m3);
end

3. 三阶贝塞尔曲线

三阶贝塞尔曲线有4个控制点，假设分别为 $P_0$，$P_1$，$P_2$，$P_3$。 $P_0$ 和 $P_1$， $P_1$ 和 $P_2$ ， $P_2$ 和 $P_3$ 分别构成一阶$P_{1,1}(t)$，$P_{1,2}(t)$，$P_{1,3}(t)$，即：
$$\begin{array}{r}\{\begin{array}{c}{P}_{1,1}(t)=(1-t)P_0+t{P}_1\\ P_{1,2}(t)=(1-t)P_1+t{P}_2\\ P_{1,3}(t)=(1-t)P_2+t{P}_3\end{array}\end{array}$$
在生成的三个一阶点$P_{1,1}(t)$，$P_{1,2}(t)$，$P_{1,3}(t)$基础上，可以生成两个二阶贝塞尔点 $P_{2,1}(t)$，$P_{2,2}(t)$：
$$\begin{array}{r}\{\begin{array}{c}{P}_{2,1}(t)=(1-t)P_{1,1}+t{P}_{1,2}\\ P_{2,2}(t)=(1-t)P_{1,2}+t{P}_{1,3}\end{array}\end{array}$$
在生成的两个二阶点$P_{2,1}(t)$，$P_{2,2}(t)$基础上，可以生成三阶贝塞尔点$P_3(t)$:
$$\begin{array}{r}{P}_3(t)=(1-t)P_{2,1}+t{P}_{2,2}=(1-t{)}^3{P}_0+3t(1-t{)}^2{P}_1+3t^2(1-t)P_2+t^3{P}_3\end{array}$$

注意：最多二阶导数不为常数

python实现

from celluloid import Camera  # 保存动图时用，pip install celluloid
import numpy as np
import matplotlib.pyplot as plt
	
P0 = np.array([0, 0])
P1 = np.array([1, 1])
P2 = np.array([2, 1])
P3 = np.array([3, 0])
fig = plt.figure(3)
camera = Camera(fig)

x_2 = []
y_2 = []
for t in np.arange(0, 1, 0.01):
    plt.cla()
    plt.plot([P0[0], P1[0]], [P0[1], P1[1]], 'k')
    plt.plot([P1[0], P2[0]], [P1[1], P2[1]], 'k')
    plt.plot([P2[0], P3[0]], [P2[1], P3[1]], 'k')
    p11_t = (1-t)*P0+t*P1
    p12_t = (1-t)*P1+t*P2
    p13_t = (1-t)*P2+t*P3
    p21_t = (1-t)*p11_t+t*p12_t
    p22_t = (1-t)*p12_t+t*p13_t
    p3_t = (1-t)*p21_t+t*p22_t

    x_2.append(p3_t[0])
    y_2.append(p3_t[1])
    plt.scatter(x_2, y_2, c='r')

    plt.plot([p11_t[0], p12_t[0]], [p11_t[1], p12_t[1]], 'b')
    plt.plot([p12_t[0], p13_t[0]], [p12_t[1], p13_t[1]], 'b')

    plt.plot([p21_t[0], p22_t[0]], [p21_t[1], p22_t[1]], 'r')
    plt.title("t="+str(t))
    plt.pause(0.001)
#     camera.snap()
# animation = camera.animate()
# animation.save('3阶贝塞尔.gif')

matlab实现

clc;
clear;
close all;
P0=[0,0];
P1=[1,1];
P2=[2,1];
P3=[3,0];
P=[P0;
    P1;
    P2;
    P3];
figure(3);
plot(P(:,1),P(:,2),'k');
MakeGif('三次贝塞尔曲线.gif',1);
hold on
scatter(P(:,1),P(:,2),200,'.b');
 
for t=0:0.01:1
    P_t_1=(1-t) * P0 + t * P1;
    P_t_2=(1-t) * P1 + t * P2;
    P_t_3=(1-t) * P2 + t * P3;
 ss
    P_t_4=(1-t) * P_t_1 + t * P_t_2;
    P_t_5=(1-t) * P_t_2 + t * P_t_3;
    
    P_t_6=(1-t) * P_t_4 + t * P_t_5;
    
    m1=scatter(P_t_1(1),P_t_1(2),300,'g');
    m2=scatter(P_t_2(1),P_t_2(2),300,'g');
    m3=scatter(P_t_3(1),P_t_3(2),300,'g');
    
    m4=scatter(P_t_4(1),P_t_4(2),300,'m');
    m5=scatter(P_t_5(1),P_t_5(2),300,'m');
    
    m6=plot([P_t_1(1),P_t_2(1),P_t_3(1)],[P_t_1(2),P_t_2(2),P_t_3(2)],'g','linewidth',2);
    m7=plot([P_t_4(1),P_t_5(1)],[P_t_4(2),P_t_5(2)],'m','linewidth',2);
    
    scatter(P_t_6(1),P_t_6(2),300,'.r');  
    
    stringName = "三次贝塞尔曲线：t="+num2str(t);
    title(stringName);
    MakeGif('三次贝塞尔曲线.gif',t*100+1);
    delete(m1);
    delete(m2);
    delete(m3);
    delete(m4);
    delete(m5);
    delete(m6);
    delete(m7);
end

二、 N阶贝塞尔曲线

由上述可知，贝塞尔点的求解满足递归的性质，$N$阶贝塞尔曲线的求解公式如下：
假设存在 $P_0$， $P_1$， $P_2$，$\dots$， $P_n$，​ 共 $n+1$ 个控制点，$N$阶贝塞尔点定义为：
$$\begin{array}{r}{P}_n(t)=\sum _{i=0}^n{C}_n^i\cdot (1-t{)}^{n-i}\cdot t^i\cdot P_i=\sum _{i=0}^n{B}_{i,n}(t)\cdot P_i\end{array}$$
式中，$B_{i,n}(t)=C_n^i\cdot (1-t{)}^{n-i}\cdot t^i$ 称为伯恩斯坦基函数。其(对$t$求)一阶导数为：
$$\begin{array}{rl}{B}_{i,n}^{\prime }(t)& ={\left[C_n^i\cdot (1-t{)}^{n-i}\cdot t^i\right]}^{\prime }\\ & =\frac{n!}{i!\cdot (n-i)!}\cdot \left[-(n-i)\cdot (1-t{)}^{n-i-1}\cdot t^i+i\cdot t^{i-1}\cdot (1-t{)}^{n-i}\right]\\ & =\frac{n!}{i!\cdot (n-i)!}\cdot i\cdot t^{i-1}\cdot (1-t{)}^{n-i}-\frac{n!}{i!\cdot (n-i)!}\cdot (n-i)\cdot (1-t{)}^{n-i-1}\cdot t^i\\ & =n\frac{(n-1)!}{(i-1)!\cdot (n-i)!}{t}^{i-1}\cdot (1-t{)}^{n-i}-n\frac{(n-1)!}{i!\cdot (n-i-1)!}\cdot (1-t{)}^{n-i-1}\cdot t^i\\ & =n{C}_{n-1}^{i-1}{t}^{i-1}\cdot (1-t{)}^{n-i}-n{C}_{n-1}^i(1-t{)}^{n-i-1}\cdot t^i\\ & =n[B_{i-1,n-1}\left(t-B_{i,n-1}(t)\right]\end{array}$$

说明：上式 $i⩾1$。
当 $i=0$ 时，$B_{0,n}(t)=C_n^0\cdot (1-t{)}^{n-0}\cdot t^0=(1-t{)}^n$，因此， $B_{0,n}^{\prime }(t)=((1-t{)}^n{)}^{\prime }=-n(1-t{)}^{n-1}=-n{B}_{0,n-1}$
 
$C_n^i=\frac{n!}{i!\cdot (n-i)!}$

进一步地，贝塞尔点求导为：
$$\begin{array}{rl}{P}_n^{\prime }(t)& ={\left[\sum _{i=0}^n{C}_n^i\cdot (1-t{)}^{n-i}\cdot t^i\cdot P_i\right]}^{\prime }\\ & =B_{0,n}^{\prime }{P}_0+B_{1,n}^{\prime }{P}_1+\cdots +B_{n,n}^{\prime }\cdot P_n\\ & =-n{B}_{0,n-1}{P}_0+n\left(B_{0,n-1}-B_{1,n-1}\right)P_1+n\left(B_{1,n-1}-B_{2,n-1}\right)P_2\cdots +n\left(B_{n-1,n-1}-B_{n,n-1}\right)P_n\\ & =n\left[B_{0,n-1}\left(P_1-P_0\right)+B_{1,n-1}\left(P_2-P_1\right)+\cdots +B_{n-1,n-1}\left(P_n-P_{n-1}\right)\right]\\ & =n\sum _{i=1}^n{B}_{i-1,n-1}(t)\cdot \left(P_i-P_{i-1}\right)\end{array}$$

## 递归方式实现贝塞尔曲线
import numpy as np
import matplotlib.pyplot as plt
## 递归的方式实现贝塞尔曲线
def bezier(Ps,n,t):
    """递归的方式求解贝塞尔点

    Args:
        Ps (_type_): 控制点，格式为numpy数组：array([[x1,y1],[x2,y2],...,[xn,yn]])
        n (_type_): n个控制点，即Ps的第一维度
        t (_type_): 时刻t

    Returns:
        _type_: 当前t时刻的贝塞尔点
    """
    if n==1:
        return Ps[0]
    return (1-t)*bezier(Ps[0:n-1],n-1,t)+t*bezier(Ps[1:n],n-1,t)

# 画图验证
Ps = np.array([[0,0],[1,1],[2,1],[3,0],[3,1]])
x_=[]
y_=[]
for t in np.arange(0,1,0.01):
    plt.cla()
    pos = bezier(Ps,len(Ps),t)
    x_.append(pos[0])
    y_.append(pos[1])
    plt.plot(Ps[:,0],Ps[:,1])
    plt.scatter(x_,y_,c='r')
    # print(pos)
    # plt.plot(pos[0],pos[1])
    plt.pause(0.001)

三、 贝塞尔曲线性质

贝塞尔曲线有许多性质，在此，列举几个在自动驾驶运动规划中的常见性质：
性质1：几何特性不随坐标系的变换而变化；
性质2：$P_0$​ 和 $P_n$ 分别位于贝塞尔曲线的起点和终点；
性质3：至少需要三阶贝塞尔曲线（四个控制点）才能生成曲率连续变化的路径；
性质4：起点和终点处的切线方向与和特征多边形的第一条边及最后一条边分别相切，即，可根据曲线的起始点和终止点的切线方向确定车辆起始点姿态和目标点姿态；
性质5：若要求两端弧线拼接在一起依然是曲率连续的，必须要求两段弧线在连接处的曲率是相等的；

曲率连续需要二阶导数连续，三阶贝塞尔曲线求两次导还有变量t，是关于t连续的
城市环境下局部路径规划，如贝塞尔曲线能够拟合直道和弯道，在曲率变化较大的地方可以选用两个贝塞尔曲线来拟合。
无人驾驶车辆的运动规划，目标轨迹曲率是连续的且轨迹的曲率不超过车辆可行驶轨迹曲率的限制。

3.1 性质1：几何特性不随坐标系的变换而变化
   因为贝塞尔曲线依据控制点生成，当各控制点相对关系确定，得到贝塞尔曲线的几何特性使唯一的，不同的是平移、旋转。

3.2 性质2: $P_0$​ 和 $P_n$ 分别位于贝塞尔曲线的起点和终点
   当 $t=0$时, $P_n(t)={\sum }_{i=0}^n{C}_n^i\cdot (1-0{)}^{n-i}\cdot 0^i\cdot P_i=P_0$；
   当 $t=1$时, $P_n(t)={\sum }_{i=0}^n{C}_n^i\cdot (1-1{)}^{n-i}\cdot 1^i\cdot P_i=P_n$；

3.3 性质3：至少需要三阶贝塞尔曲线（四个控制点）才能生成曲率连续变化的路径
   曲率 $k$ 公式如下：
$$k=\left|\frac{f^{\prime \prime }}{(1+{{f^{\prime }}^2)}^{\frac{3}{2}}}\right|$$

分析：要求 曲率$k$ 连续变化，则要求至少二阶导数连续变化，则至少需要 三阶贝塞尔曲线(四个控制点)

3.4 性质4：起点和终点处的切线方向与和特征多边形的第一条边及最后一条边分别相切
   由 贝塞尔点求导 可知：$P_n^{\prime }(t)=n{\sum }_{i=1}^n{B}_{i-1,n-1}(t)\cdot \left(P_i-P_{i-1}\right)$
   当 $t=0$时, $P_n^{\prime }(0)=n{\sum }_{i=1}^n{B}_{i-1,n-1}(0)\cdot \left(P_i-P_{i-1}\right)$；

伯恩斯坦基函数: $B_{i,n}(t)=C_n^i\cdot (1-t{)}^{n-i}\cdot t^i$