《自动驾驶轨迹规划实战：Lattice Planner建立避障路径生成（附可运行Python代码）》—— 零基础实现基于离散优化的避障路径规划

---

《自动驾驶轨迹规划实战：Lattice Planner实现避障路径生成（附可运行Python代码）》
—— 零基础实现基于离散优化的避障路径规划

---

一、为什么Lattice Planner成为自动驾驶的核心算法？

在自动驾驶的路径规划领域，Lattice Planner（格子规划器）凭借其结构化采样+动态优化的特性，成为Waymo、Aptiv等头部企业的核心算法。与传统A*、RRT算法不同，Lattice Planner通过横向位移采样+纵向速度规划的复合策略，能够在10ms内生成满足车辆运动学约束的平滑轨迹。本文将手把手实现一个简化版Lattice Planner，并针对经典“避障场景”进行代码级解析。

---

二、算法原理精讲（附数学推导图示）

2.1 轨迹生成三要素

s(t) = s_0 + v_0 t + \frac{1}{2}a t^2 \quad \text{(纵向运动方程)}

d(s) = c_0 + c_1 s + c_2 s^2 + c_3 s^3 \quad \text{(横向多项式拟合)}

2.2 成本函数设计

评价维度	权重系数	计算公式
舒适性	0.4	∑(jerk²)
安全性	0.3	障碍物距离倒数
路径贴合度	0.2	参考线偏移量
能耗效率	0.1	速度变化率

---

三、Python代码实现（可直接运行）

3.1 环境初始化

import numpy as np
import matplotlib.pyplot as plt
class Vehicle:
def __init__(self):
self.width = 2.0 # 车宽
self.length = 4.5 # 车长
self.max_acc = 2.0 # 最大加速度(m/s²)
self.max_jerk = 1.5 # 最大加加速度(m/s³)
obstacles = [
{
'x': 30
, 'y': 1.8
, 'r': 1.2
}
,
{
'x': 45
, 'y': -1.5
, 'r': 0.8
}
] # 障碍物坐标及半径

3.2 轨迹采样核心函数

def generate_lattice_paths(current_state, horizon=5.0
, dt=0.1
):
"""
:param current_state: 当前状态 [s, s_dot, d, d_dot]
:param horizon: 规划时长(s)
:return: 候选轨迹列表
"""
paths = []
# 纵向速度采样（3种模式）
for s_dot_end in np.linspace(8
, 12
, 3
): # 8m/s~12m/s
# 横向位移采样（5种偏移）
for d_end in np.linspace(-1.5
, 1.5
, 5
):
t = np.arange(0
, horizon + dt, dt)
# 纵向四次多项式拟合
s_coeff = np.polyfit([0
, horizon]
,
[current_state[0]
, current_state[0] + s_dot_end*horizon]
, 4
)
# 横向五次多项式拟合
d_coeff = np.polyfit([0
, horizon]
,
[current_state[2]
, d_end]
, 5
)
path = {
's': np.polyval(s_coeff, t)
,
'd': np.polyval(d_coeff, t)
,
's_dot': np.polyval(np.polyder(s_coeff)
, t)
,
'd_dot': np.polyval(np.polyder(d_coeff)
, t)
}
paths.append(path)
return paths

3.3 碰撞检测与最优选择

def select_optimal_path(paths, obstacles):
min_cost = float('inf'
)
best_path = None
for path in paths:
# 计算舒适性成本
jerk_cost = np.mean(np.diff(path['s_dot']
)**2
)
# 计算安全成本
collision_cost = 0
for t in range(len(path['s']
)
):
x = path['s'][t]
y = path['d'][t]
for obs in obstacles:
dist = np.sqrt((x - obs['x']
)**2 + (y - obs['y']
)**2
)
if dist <
(obs['r'] + 1.5
): # 1.5m为安全距离
collision_cost += 1 / max(dist, 0.1
)
# 计算总成本
total_cost = 0.4*jerk_cost + 0.3*collision_cost
if total_cost < min_cost:
min_cost = total_cost
best_path = path
return best_path

3.4 可视化运行

def plot_scenario(paths, best_path, obstacles):
plt.figure(figsize=(10
, 6
)
)
# 绘制障碍物
for obs in obstacles:
circle = plt.Circle((obs['x']
, obs['y']
)
, obs['r']
, color='r'
, alpha=0.3
)
plt.gca(
).add_patch(circle)
# 绘制所有候选路径
for path in paths:
plt.plot(path['s']
, path['d']
, 'gray'
, alpha=0.2
)
# 高亮最优路径
plt.plot(best_path['s']
, best_path['d']
, 'b'
, linewidth=2
)
plt.xlabel('Longitudinal Position (m)'
)
plt.ylabel('Lateral Position (m)'
)
plt.grid(True
)
plt.axis('equal'
)
plt.show(
)
# 主程序
if __name__ == "__main__":
ego_state = [20
, 10
, 0
, 0] # [s, s_dot, d, d_dot]
candidate_paths = generate_lattice_paths(ego_state)
optimal_path = select_optimal_path(candidate_paths, obstacles)
plot_scenario(candidate_paths, optimal_path, obstacles)

---

完整代码

#!/usr/bin/python3
import numpy as np
import matplotlib.pyplot as plt
class Vehicle:
def __init__(self):
self.width = 2.0 # 车宽
self.length = 4.5 # 车长
self.max_acc = 2.0 # 最大加速度(m/s²)
self.max_jerk = 1.5 # 最大加加速度(m/s³)
obstacles = [
{
'x': 30
, 'y': 1.8
, 'r': 1.2
}
,
{
'x': 45
, 'y': -1.5
, 'r': 0.8
}
] # 障碍物坐标及半径
def generate_lattice_paths(current_state, horizon=5.0
, dt=0.1
):
"""
:param current_state: 当前状态 [s, s_dot, d, d_dot]
:param horizon: 规划时长(s)
:return: 候选轨迹列表
"""
paths = []
# 纵向速度采样（3种模式）
for s_dot_end in np.linspace(8
, 12
, 3
): # 8m/s~12m/s
# 横向位移采样（5种偏移）
for d_end in np.linspace(-1.5
, 1.5
, 5
):
t = np.arange(0
, horizon + dt, dt)
# 纵向四次多项式拟合
s_coeff = np.polyfit([0
, horizon]
,
[current_state[0]
, current_state[0] + s_dot_end*horizon]
, 4
)
# 横向五次多项式拟合
d_coeff = np.polyfit([0
, horizon]
,
[current_state[2]
, d_end]
, 5
)
path = {
's': np.polyval(s_coeff, t)
,
'd': np.polyval(d_coeff, t)
,
's_dot': np.polyval(np.polyder(s_coeff)
, t)
,
'd_dot': np.polyval(np.polyder(d_coeff)
, t)
}
paths.append(path)
return paths
def select_optimal_path(paths, obstacles):
min_cost = float('inf'
)
best_path = None
for path in paths:
# 计算舒适性成本
jerk_cost = np.mean(np.diff(path['s_dot']
)**2
)
# 计算安全成本
collision_cost = 0
for t in range(len(path['s']
)
):
x = path['s'][t]
y = path['d'][t]
for obs in obstacles:
dist = np.sqrt((x - obs['x']
)**2 + (y - obs['y']
)**2
)
if dist <
(obs['r'] + 1.5
): # 1.5m为安全距离
collision_cost += 1 / max(dist, 0.1
)
# 计算总成本
total_cost = 0.4*jerk_cost + 0.3*collision_cost
if total_cost < min_cost:
min_cost = total_cost
best_path = path
return best_path
def plot_scenario(paths, best_path, obstacles):
plt.figure(figsize=(10
, 6
)
)
# 绘制障碍物
for obs in obstacles:
circle = plt.Circle((obs['x']
, obs['y']
)
, obs['r']
, color='r'
, alpha=0.3
)
plt.gca(
).add_patch(circle)
# 绘制所有候选路径
for path in paths:
plt.plot(path['s']
, path['d']
, 'gray'
, alpha=0.2
)
# 高亮最优路径
plt.plot(best_path['s']
, best_path['d']
, 'b'
, linewidth=2
)
plt.xlabel('Longitudinal Position (m)'
)
plt.ylabel('Lateral Position (m)'
)
plt.grid(True
)
plt.axis('equal'
)
plt.show(
)
# 主程序
if __name__ == "__main__":
ego_state = [20
, 10
, 0
, 0] # [s, s_dot, d, d_dot]
candidate_paths = generate_lattice_paths(ego_state)
optimal_path = select_optimal_path(candidate_paths, obstacles)
plot_scenario(candidate_paths, optimal_path, obstacles)

四、运行结果分析

（注：实际运行后将显示包含障碍物区域和最优路径的轨迹图，灰色为候选路径，蓝色为最优避障路径）

---

五、工程优化建议

1. 引入Frenet坐标系：将全局坐标转换为(s,d)坐标系，简化横向/纵向解耦计算
2. 动态权重调整：根据场景危险程度自动调整成本函数权重（如障碍物接近时加大安全权重）
3. 运动学约束检查：添加最大曲率、加速度边界条件校验
4. 多线程优化：对候选轨迹的代价计算进行并行加速

---

六、结语

通过不到100行的Python代码，我们实现了一个完整的Lattice Planner避障规划原型系统。该代码在Python 3.8+环境下可直接运行（需安装numpy和matplotlib）。在实际自动驾驶系统中，还需融合高精地图、感知预测模块等，但核心的轨迹生成与优化思想与此文一脉相承。

技术亮点：

1. 采用多项式参数化方法保证轨迹平滑性
2. 引入多维度加权成本函数实现最优决策
3. 模块化设计便于功能扩展

原创声明：本文代码及算法思路均为作者原创，转载需注明出处。关注技术博客获取更多自动驾驶实战教程！