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

这篇具有很好参考价值的文章主要介绍了【路径规划】局部路径规划算法——贝塞尔曲线法（含python实现 | c++实现）。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

参考资料

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

1. 算法简介

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

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

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

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

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

2. 公式原理及python实现

2.1 一阶贝塞尔曲线

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

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

贝塞尔曲线路径规划,# 规划,算法,贝塞尔曲线,路径规划,自动驾驶,python

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_1$ 和 $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.$
在生成的两个一阶点基础上，可以生成二阶贝塞尔点：
$p_{2}(t)=(1-t) p_{1,1}+t p_{1,2}$
则贝塞尔点与3个控制点的关系为：
$\tag{2} p_2(t)=(1-t)^2P_0+2t(1-t)P_1+t^2P_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_1和 P_2、P_2和 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.$
在生成的三个一阶点基础上，可以生成两个二阶贝塞尔点：
$\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.$
在生成的两个二阶点基础上，可以生成三阶贝塞尔点:
$p_{3}(t)=(1-t) p_{2,1}+t p_{2,2}$

故整理后可得
$\tag{3} p_3(t)=(1-t)^3P_0+3t(1-t)^2P_1+3t^2(1-t)P_2+t^3P_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$ 阶贝塞尔曲线的求解公式。

对于 $P_0 ， P_1 ， P_2 ， \ldots ， P_n$ 共 $n + 1$ 个控制点而言，贝塞尔点定义为：
$\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} \quad$
式中 $B_{i, n}(t)=C_{n}^{i} \cdot(1-t)^{n-i} \cdot t^{i}$ 称为伯恩斯坦基函数。伯恩斯坦基函数的一阶导数为：
$\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) !} \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\left[B_{i-1, n-1}(t)-B_{i, n-1}(t)\right] \end{aligned}$
式中 $i\ge 1$ . 当 $i = 0$ 时， $B_{0, n}(t)=C_{n}^{0} \cdot(1-t)^{n-0} \cdot t^{0}=(1-t)^n$ , 故 $B'_{0, n}(t)=-n(1-t)^{n-1}=-nB_{0, n-1}$ .

进一步地，贝塞尔点求导为：
$\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} \\ &=-nB_{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{aligned}$

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

曲率连续需要二阶导数连续，三阶贝塞尔曲线求两次导还有变量t，是关于t连续的

城市环境下局部路径规划，如贝塞尔曲线能够拟合直道和弯道，在曲率变化较大的地方可以选用两个贝塞尔曲线来拟合。

无人驾驶车辆的运动规划，目标轨迹曲率是连续的且轨迹的曲率不超过车辆可行驶轨迹曲率的限制。

3. c++实现

由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了n阶贝塞尔曲线，代码结构与python代码实现类似，这边就不再做相关代码解释了。完整代码详见我的github仓库。

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