自动驾驶算法详解(5): 贝塞尔曲线进行路径规划的python实现

一、理论知识

1、路径规划定义

路径规划智能物流、无人驾驶等智能领域中重要的组成部分。路径规划的目标是实现从目的地到终点之间寻找一条安全（无碰撞）、高效（最短距离或 最短时间）的一条最优或接近最优的路径。在自动驾驶算法中，路径规划一般特指局部路径规划，即规划一条路径，引导自车从当前位置驶向导航routing模块期望的位置。

自动驾驶中路径规划不仅要考虑一条安全（无碰撞）、高效（最短距离或 最短时间），还应该考虑自车动力学的约束，以及曲线的平滑。

贝塞尔曲线是一种常用的求解自动驾驶规划路径的方法。

2、贝塞尔曲线理论

Bezier贝塞尔曲线是由伯恩斯坦多项式发展而来的， 控制简单，能够处理光滑曲线，应用较为广泛。

贝塞尔曲线方法是法国工程师 Bezier 在 1962 年为了设计汽车车身形状提出的，之后贝塞尔曲线由于具 有良好的数学特性而被广泛应用到车辆路径规划领域。

如下图 2 所示，五个控制点可以唯一确定平面内一条四阶贝塞尔曲线，曲线具有仿射变换不变的特性，其参 数化表达式如下：

在实际应用中，根据障碍物、期望位置、曲率限制等条件确定控制点，从而求解贝塞尔曲线获得规划的路径。

二、代码实现

import matplotlib.pyplot as plt
import numpy as np
import scipy.special


show_animation = True


start_x = 0 # [m]
start_y = -5 # [m]
start_yaw = 0 # [rad]
end_x = 40 # [m]
end_y = 5 # [m]
end_yaw = 0 # [rad]
control_points = np.array([[0,-5],[20,-5],[20,5],[40,5]])
n_points = 100


def bernstein_poly(n, i, t): 
 return scipy.special.comb(n, i) * t ** i * (1 - t) ** (n - i)




def bezier(t, control_points): 
    n = len(control_points) - 1
 return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)


traj = []


for t in np.linspace(0, 1, n_points):
    traj.append(bezier(t, control_points))


path = np.array(traj)

三、结果可视化

1、控制点

2、控制点

四、最新文章推荐

Apollo规划决策算法仿真调试(6):pnc_map模块详解GetRouteSegments规划局部地图生成上篇_

Apollo规划决策算法仿真调试(7):pnc_map模块详解GetRouteSegments规划局部地图生成下篇_ 

Apollo Planning决策规划算法代码详解 (22):决策规划算法最完整介绍 

Apollo规划决策算法仿真调试(3)：ReferenceLineProvider参考线生成流程

 Apollo规划决策算法仿真调试(5):动态障碍物绕行_自动驾驶 Player的博客-CSDN博客_apollo 障碍物

Apollo Planning决策规划算法代码详细解析 (1)：Scenario选择

Apollo Planning决策规划算法代码解析 (17):SPEED_HEURISTIC_OPTIMIZER 速度动态规划下

自动驾驶算法详解(4): 横向LQR、纵向PID控制进行轨迹跟踪以及python实现

自动驾驶算法详解(2): prescan联合simulink进行FCW的仿真

五、专栏推荐

Apollo Planning决策规划算法代码详细解析：

https://blog.csdn.net/nn243823163/category_11685852.htmlhttps://blog.csdn.net/nn243823163/category_11685852.html

Apollo规划决策算法仿真调试：

https://blog.csdn.net/nn243823163/category_11807746.html?spm=1001.2014.3001.5482https://blog.csdn.net/nn243823163/category_11807746.html?spm=1001.2014.3001.5482