《自动驾驶轨迹规划实战：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 成本函数设计

三、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. 模块化设计便于功能扩展

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