【路径规划】局部路径规划算法——贝塞尔曲线法（含python实现）

文章目录

参考资料
1. 算法简介
2. 公式原理及python实现
- 2.1 一阶贝塞尔曲线
- 2.2 二阶贝塞尔曲线
- 2.3 三阶贝塞尔曲线
- 2.4 n阶贝塞尔曲线
- 2.5 贝塞尔曲线性质
3. python代码实现车辆轨迹规划

参考资料

路径规划与轨迹跟踪系列算法
曲线杂谈（二）：Bezier曲线的特殊性质
贝塞尔曲线的特性总结

1. 算法简介

贝塞尔曲线于1962年由法国工程师皮埃尔·贝塞尔（ Pierre Bézier）发表，他运用贝塞尔曲线来为汽车的主体进行设计。

贝塞尔曲线是应用于二维图形应用程序的数学曲线，由一组称为控制点的向量来确定，给定的控制点按顺序连接构成控制多边形，贝塞尔曲线逼近这个多边形，进而通过调整控制点坐标改变曲线的形状。控制点的作用是控制曲线的弯曲程度

贝塞尔曲线只需要很少的控制点就能够生成较复杂的平滑曲线。该方法能够保证输入的控制点与生成的曲线之间的关系非常简洁、明确。

对于车辆系统，规划的轨迹应满足以下准则：轨迹连续；轨迹曲率连续；轨迹容易被车辆跟随，且容易生成。

贝塞尔曲线是参数化曲线，n次贝塞尔曲线由n+1个控制点决定。它的生成动画过程可以查看这个网址。

2. 公式原理及python实现 2.1 一阶贝塞尔曲线

一阶贝塞尔曲线需要2个控制点，假设分别为 P 0 , P 1 P_0,P_1 P0,P1。则贝塞尔曲线上的生成点 p 1 ( t ) p_1(t) p1(t)可以表达为：
p 1 ( t ) = P 0 + ( P 1 − P 0 ) t → p 1 ( t ) = ( 1 − t ) P 0 + t P 1 (1) tag{1} p_1(t)=P_0+(P_1-P_0)t rightarrow p_1(t)=(1-t)P_0+tP_1 p1(t)=P0+(P1−P0)t→p1(t)=(1−t)P0+tP1(1)
t t t的取值范围为 [ 0 , 1 ] [0,1] [0,1]，下面不再重复说明。

一阶曲线很好理解，就是根据 t t t来的线性插值。

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])
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.2 二阶贝塞尔曲线

二阶贝塞尔曲线需要3个控制点，假设分别为 P 0 , P 1 , P 2 P_0,P_1,P_2 P0,P1,P2。 P 0 P_0 P0和 P 1 P_1 P1构成一阶， P 1 P_1 P1和 P 2 P_2 P2也构成一阶，即：

{ p 1 , 1 ( t ) = ( 1 − t ) P 0 + t P 1 p 1 , 2 ( t ) = ( 1 − t ) P 1 + t P 2 left{begin{array}{l} p_{1,1}(t)=(1-t) P_{0}+t P_{1} \ p_{1,2}(t)=(1-t) P_{1}+t P_{2} end{array}right. {p1,1(t)=(1−t)P0+tP1p1,2(t)=(1−t)P1+tP2
在生成的两个一阶点基础上，可以生成二阶贝塞尔点：
p 2 ( t ) = ( 1 − t ) p 1 , 1 + t p 1 , 2 p_{2}(t)=(1-t) p_{1,1}+t p_{1,2} p2(t)=(1−t)p1,1+tp1,2
则贝塞尔点与3个控制点的关系为：
p 2 ( t ) = ( 1 − t ) 2 P 0 + 2 t ( 1 − t ) P 1 + t 2 P 2 (2) tag{2} p_2(t)=(1-t)^2P_0+2t(1-t)P_1+t^2P_2 p2(t)=(1−t)2P0+2t(1−t)P1+t2P2(2)

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')

2.3 三阶贝塞尔曲线

三阶贝塞尔曲线有4个控制点，假设分别为 P 0 ， P 1 ， P 2 ， P 3 P_0， P_1，P_2， P_3 P0，P1，P2，P3， P 0 和 P 1 、 P 1 和 P 2 、 P 2 和 P 3 P_0和P_1、P_1和 P_2、P_2和 P_3 P0和P1、P1和P2、P2和P3 都构成一阶，即：
{ 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 left{begin{array}{l} 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}right. ⎩⎨⎧p1,1(t)=(1−t)P0+tP1p1,2(t)=(1−t)P1+tP2p1,3(t)=(1−t)P2+tP3
在生成的三个一阶点基础上，可以生成两个二阶贝塞尔点：
{ 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 left{begin{array}{l} 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}right. {p2,1(t)=(1−t)p1,1+tp1,2p2,2(t)=(1−t)p1,2+tp1,3
在生成的两个二阶点基础上，可以生成三阶贝塞尔点:
p 3 ( t ) = ( 1 − t ) p 2 , 1 + t p 2 , 2 p_{3}(t)=(1-t) p_{2,1}+t p_{2,2} p3(t)=(1−t)p2,1+tp2,2

故整理后可得
p 3 ( t ) = ( 1 − t ) 3 P 0 + 3 t ( 1 − t ) 2 P 1 + 3 t 2 ( 1 − t ) P 2 + t 3 P 3 (3) tag{3} p_3(t)=(1-t)^3P_0+3t(1-t)^2P_1+3t^2(1-t)P_2+t^3P_3 p3(t)=(1−t)3P0+3t(1−t)2P1+3t2(1−t)P2+t3P3(3)

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')

2.4 n阶贝塞尔曲线

通过上面一阶到三阶的贝塞尔曲线，可以发现贝塞尔点的求解满足递归的性质。这里我们直接给出 n n n阶贝塞尔曲线的求解公式。

对于 P 0 ， P 1 ， P 2 ， … ， P n P_0 ， P_1 ， P_2 ， ldots ， P_n P0，P1，P2，…，Pn 共 n + 1 n+1 n+1 个控制点而言，贝塞尔点定义为：
p n ( t ) = ∑ i = 0 n C n i ⋅ ( 1 − t ) n − i ⋅ t i ⋅ P i = ∑ i = 0 n B i , n ( t ) ⋅ P i ( i = 0 时, B i , n = 0 ) (4) tag{4} 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} quadleft(i=0 text { 时, } B_{i, n}=0right) pn(t)=i=0∑nCni⋅(1−t)n−i⋅ti⋅Pi=i=0∑nBi,n(t)⋅Pi(i=0 时, Bi,n=0)(4)
式中 B i , n ( t ) = C n i ⋅ ( 1 − t ) n − i ⋅ t i B_{i, n}(t)=C_{n}^{i} cdot(1-t)^{n-i} cdot t^{i} Bi,n(t)=Cni⋅(1−t)n−i⋅ti 称为伯恩斯坦基函数。伯恩斯坦基函数的一阶导数为：
B i , n ′ ( t ) = [ C n i ⋅ ( 1 − t ) n − i ⋅ t i ] ′ = n ! i ! ⋅ ( n − i ) ! ⋅ [ − ( n − i ) ⋅ ( 1 − t ) n − i − 1 ⋅ t i + i ⋅ t i − 1 ⋅ ( 1 − t ) n − i ] = n ! i ! ⋅ ( n − i ) ! ⋅ i ⋅ t i − 1 ⋅ ( 1 − t ) n − i − n ! i ! ⋅ ( n − i ) ! ⋅ ( n − i ) ⋅ ( 1 − t ) n − i − 1 ⋅ t i = n ( n − 1 ) ! ( i − 1 ) ! ⋅ ( n − i ) ! t i − 1 ⋅ ( 1 − t ) n − i − n ( n − 1 ) ! i ! ⋅ ( n − i − 1 ) ! ⋅ ( 1 − t ) n − i − 1 ⋅ t i = n C n − 1 i − 1 t i − 1 ⋅ ( 1 − t ) n − i − n C n − 1 i ( 1 − t ) n − i − 1 ⋅ t i = n [ B i − 1 , n − 1 ( t ) − B i , n − 1 ( t ) ] (5) tag{5} begin{aligned} 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) !} cdotleft[-(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}\ &=nleft[B_{i-1, n-1}(t)-B_{i, n-1}(t)right] end{aligned} Bi,n′(t)=[Cni⋅(1−t)n−i⋅ti]′=i!⋅(n−i)!n!⋅[−(n−i)⋅(1−t)n−i−1⋅ti+i⋅ti−1⋅(1−t)n−i]=i!⋅(n−i)!n!⋅i⋅ti−1⋅(1−t)n−i−i!⋅(n−i)!n!⋅(n−i)⋅(1−t)n−i−1⋅ti=n(i−1)!⋅(n−i)!(n−1)!ti−1⋅(1−t)n−i−ni!⋅(n−i−1)!(n−1)!⋅(1−t)n−i−1⋅ti=nCn−1i−1ti−1⋅(1−t)n−i−nCn−1i(1−t)n−i−1⋅ti=n[Bi−1,n−1(t)−Bi,n−1(t)](5)

进一步地，贝塞尔点求导为：
p n ′ ( t ) = [ ∑ i = 0 n C n i ⋅ ( 1 − t ) n − i ⋅ t i ⋅ P i ] ′ = B 0 , n ′ P 0 + B 1 , n ′ P 1 + ⋯ + B n , n ′ ⋅ P n = 0 + n ( B 0 , n − 1 − B 1 , n − 1 ) P 1 + n ( B 1 , n − 1 − B 2 , n − 1 ) P 2 ⋯ + n ( B n − 1 , n − 1 − B n , n − 1 ) P n = n [ B 0 , n − 1 ( P 1 − P 0 ) + B 1 , n − 1 ( P 2 − P 1 ) + ⋯ + B n − 1 , n − 1 ( P n − P n − 1 ) ] = n ∑ i = 1 n B i − 1 , n − 1 ( t ) ⋅ ( P i − P i − 1 ) (6) tag{6} begin{aligned} 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} \ &=0+nleft(B_{0, n-1}-B_{1, n-1}right) P_{1}+nleft(B_{1, n-1}-B_{2, n-1}right) P_{2} cdots+nleft(B_{n-1, n-1}-B_{n, n-1}right) P_{n} \ &=nleft[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) cdotleft(P_{i}-P_{i-1}right) end{aligned} pn′(t)=[i=0∑nCni⋅(1−t)n−i⋅ti⋅Pi]′=B0,n′P0+B1,n′P1+⋯+Bn,n′⋅Pn=0+n(B0,n−1−B1,n−1)P1+n(B1,n−1−B2,n−1)P2⋯+n(Bn−1,n−1−Bn,n−1)Pn=n[B0,n−1(P1−P0)+B1,n−1(P2−P1)+⋯+Bn−1,n−1(Pn−Pn−1)]=ni=1∑nBi−1,n−1(t)⋅(Pi−Pi−1)(6)

普通方式实现n阶贝塞尔曲线

import numpy as np
import matplotlib.pyplot as plt
import math
def bezier_normal(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]
    p_t = np.array([0,0])
    n = len(Ps)-1
    for i in range(n+1):
        C_n_i = math.factorial(n)/(math.factorial(i)*math.factorial(n-i))
        p_t =p_t+C_n_i*(1-t)**(n-i)*t**i*Ps[i]
    return p_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_normal(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)

使用递归的方式实现n阶贝塞尔曲线

## 递归的方式实现贝塞尔曲线
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)

2.5 贝塞尔曲线性质

贝塞尔曲线具有许多性质，具体可参考这篇知乎文章，总结的很好。这里就列举几个在自动驾驶运动规划中的常见性质。

性质1： P 0 P_0 P0和 P n P_n Pn分别位于贝塞尔曲线的起点和终点；
性质2：几何特性不随坐标系的变换而变化；
性质3：起点和终点处的切线方向与和特征多边形的第一条边及最后一条边分别相切，换句话说，可根据曲线的起始点和终止点的切线方向确定车辆起始点姿态和目标点姿态；
性质4：若要求两端弧线拼接在一起依然是曲率连续的，必须要求两段弧线在连接处的曲率是相等的；
性质5：至少需要三阶贝塞尔曲线（四个控制点）才能生成曲率连续的路径。

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

3. python代码实现车辆轨迹规划

下面实现一个简单的车辆轨迹规划。

import numpy as np
import matplotlib.pyplot as plt
import copy
from celluloid import Camera  # 保存动图时用，pip install celluloid

## 递归的方式实现贝塞尔曲线
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)


if __name__=='__main__':
    d = 3.5  # 道路标准宽度

	# 控制点
    Ps = np.array([
        [0, -d / 2],
        [25, -d / 2],
        [25, d / 2],
        [50, d / 2]
        ])

    n = len(Ps) - 1  # 贝塞尔曲线的阶数

    path=[]  # 路径点存储
	# 贝塞尔曲线生成
    for t in np.arange(0,1.01,0.01):
        p_t = bezier(Ps,len(Ps),t)
        path.append(p_t)
    path = np.array(path)


    ## 画图
    fig = plt.figure(1)
    # plt.ylim(-4, 4)
    camera = Camera(fig)
    len_line = 50  # 车道长
    # 画灰色路面图
    GreyZone = np.array([[- 5, - d - 0.5], [- 5, d + 0.5],
                        [len_line, d + 0.5], [len_line, - d - 0.5]])
    for i in range(len(path)):
        # plt.cla()

        plt.fill(GreyZone[:, 0], GreyZone[:, 1], 'gray')
        # 画分界线
        plt.plot(np.array([- 5, len_line]), np.array([0, 0]), 'w--')

        plt.plot(np.array([- 5, len_line]), np.array([d, d]), 'w')

        plt.plot(np.array([- 5, len_line]), np.array([- d, - d]), 'w')

        plt.plot(Ps[:, 0], Ps[:, 1], 'ro') # 画控制点
        plt.plot(Ps[:, 0], Ps[:, 1], 'y') # 画控制点连线
        # 设置坐标轴显示范围
        # plt.axis('equal')
        plt.gca().set_aspect('equal')
        # 绘制路径

        plt.plot(path[0:i, 0], path[0:i, 1], 'g')  # 路径点
        plt.pause(0.001)
    #     camera.snap()
    # animation = camera.animate()
    # animation.save('trajectory.gif')

效果如下：

以上所有代码均存于github仓库，欢迎访问。

【路径规划】局部路径规划算法——贝塞尔曲线法（含python实现）

Python相关栏目本月热门文章