自动驾驶Lattice规划算法原理与Matlab实现

1. Lattice规划算法核心思想解析

Lattice（晶格）规划算法是自动驾驶运动规划领域的经典方法，其核心思想是通过在Frenet坐标系下构建规则化的采样空间，将连续的高维规划问题转化为离散的轨迹选择问题。这种结构化采样方式相比随机采样（如RRT）具有计算效率高、轨迹质量稳定的特点。

Frenet坐标系以道路中心线为参考，将车辆运动分解为纵向（s方向）和横向（d方向）两个维度。这种表示方法天然适配道路结构，使得轨迹规划可以独立考虑沿道路前进和车道保持/变换两个目标。

实际工程中，Frenet坐标系的转换需要考虑道路曲率的影响。当参考线曲率较大时，笛卡尔坐标系与Frenet坐标系之间的转换会出现非线性误差，这是实现时需要注意的关键点。

2. 轨迹采样策略实现细节

2.1 纵向轨迹生成

纵向运动规划主要解决"开多快"和"跟车距离"的问题。我们采用五次多项式进行纵向轨迹参数化：

matlab复制% 纵向五次多项式参数化
s(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5

采样时通常固定时间间隔T（如3-5秒），通过调整末端状态(s_T, ṡ_T, ̈s_T)生成不同特征的轨迹：

• 巡航轨迹：末端加速度̈s_T=0，速度ṡ_T设为期望巡航速度
• 跟车轨迹：根据前车预测轨迹调整末端s_T保持安全距离
• 停车轨迹：末端位置s_T设为停车线，速度ṡ_T=0

2.2 横向轨迹生成

横向运动采用四次多项式，在保证平滑性的同时减少过拟合风险：

matlab复制% 横向四次多项式参数化
d(t) = b0 + b1*t + b2*t^2 + b3*t^3 + b4*t^4

采样策略需考虑：

1. 车道保持：d_T保持在当前车道中心
2. 变道准备：d_T偏移至相邻车道中心
3. 避障偏移：根据障碍物位置动态调整d_T

实际项目中，横向采样需要与高精地图的车道信息联动。当检测到车道线类型变化（如虚线变实线）时，应及时调整可选的d_T采样范围。

3. 轨迹代价函数设计

3.1 基础代价项

完整的代价函数通常包含以下核心项：

3.2 动态权重调整

不同驾驶场景需要调整代价权重。例如：

• 高速公路：提高效率权重（0.6）和舒适度权重（0.3）
• 城市道路：增加安全性权重（0.5）和合规性权重（0.4）
• 泊车场景：侧重安全性（0.7）和精确性（0.3）

matlab复制% 动态权重示例
if road_type == "highway"
    w = [0.1, 0.6, 0.1, 0.2]; % [舒适,效率,安全,合规] 
elseif road_type == "urban"
    w = [0.2, 0.2, 0.5, 0.1];
end

4. 碰撞检测优化实践

4.1 层次化检测流程

1. 快速筛选：基于轨迹包络线（矩形框）与障碍物AABB包围盒的相交测试
2. 精确检测：将车辆轮廓离散为多个圆形，与障碍物多边形进行几何碰撞检测
3. 时空验证：检查轨迹上所有时间步的车辆位姿是否与动态障碍物预测轨迹冲突

4.2 计算加速技巧

• 空间哈希：将场景网格化，只检测相邻网格内的障碍物
• 时间剪枝：对轨迹分段线性化，利用凸包性质提前终止检测
• 并行计算：对多条轨迹的碰撞检测任务进行GPU加速

实测发现，在Matlab中实现时，将车辆轮廓简化为4-6个圆形可以在检测精度和计算速度间取得较好平衡。过多圆形会导致计算量剧增，而过少则可能漏检。

5. Matlab实现关键代码解析

5.1 轨迹采样核心代码

matlab复制function trajectories = sample_lattice(s0, d0, road_info)
    % 参数初始化
    T = 3.0; % 时间跨度
    s_samples = linspace(s0+10, s0+50, 5); % 纵向采样
    d_samples = [d0, d0+3.5, d0-3.5]; % 横向采样（当前车道及相邻车道）
    
    % 生成轨迹组合
    trajectories = [];
    for s_T = s_samples
        for d_T = d_samples
            % 纵向五次多项式求解
            A_long = [1, 0, 0, 0, 0, 0;
                      0, 1, 0, 0, 0, 0;
                      0, 0, 2, 0, 0, 0;
                      1, T, T^2, T^3, T^4, T^5;
                      0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4;
                      0, 0, 2, 6*T, 12*T^2, 20*T^3];
            b_long = [s0; 0; 0; s_T; 0; 0];
            coeff_long = A_long\b_long;
            
            % 横向四次多项式求解（类似省略）
            ...
            
            % 轨迹点插值
            t = linspace(0, T, 30);
            s = polyval(coeff_long, t);
            d = polyval(coeff_lat, t);
            
            % 转换回笛卡尔坐标系
            xy = frenet2cartesian(s, d, road_info.reference_path);
            
            trajectories(end+1).path = xy;
            trajectories(end).coeff = [coeff_long; coeff_lat];
        end
    end
end

5.2 代价计算实现

matlab复制function cost = calculate_cost(trajectory, obstacles)
    % 舒适度代价（jerk积分）
    jerk = 6*trajectory.coeff(6)*t + 2*trajectory.coeff(3); % 三次导数
    comfort_cost = trapz(t, jerk.^2);
    
    % 效率代价
    efficiency_cost = 1/(trajectory.path(end,1) + 0.1);
    
    % 安全性代价
    min_dist = inf;
    for obs = obstacles
        dist = min(pdist2(trajectory.path, obs.polygon));
        if dist < min_dist
            min_dist = dist;
        end
    end
    safety_cost = min(1/min_dist, 10);
    
    % 综合代价
    cost = 0.3*comfort_cost + 0.4*efficiency_cost + 0.3*safety_cost;
end

6. 工程实践中的典型问题

6.1 轨迹震荡问题

现象：车辆在跟踪生成的轨迹时出现横向摆动
解决方案：

1. 在代价函数中增加与上一周期轨迹的相似度项
2. 对多项式系数进行低通滤波
3. 限制相邻周期轨迹的横向偏移变化率

6.2 弯道速度规划

常见错误：在弯道处保持恒定速度导致离心力过大
优化方法：

1. 根据参考线曲率κ动态调整纵向速度：v_max = √(a_max/|κ|)
2. 在代价函数中增加向心加速度惩罚项：a_cent = v²κ
3. 采用速度-曲率联合采样策略

6.3 复杂场景处理

对于交叉口等复杂场景，需要特殊处理：

1. 增加采样密度（更多s_T和d_T组合）
2. 引入虚拟障碍物约束（如停止线、让行区）
3. 采用多阶段规划（先粗略采样再局部优化）

7. 算法评估与调参建议

7.1 量化评估指标

建立评估表格监控算法表现：

7.2 参数调试流程

推荐按以下顺序调整关键参数：

1. 采样时间T：从3.0s开始，以0.5s为步长测试2.0-5.0s范围
2. 代价权重：先固定舒适度权重0.3，调整效率与安全性权重
3. 碰撞检测阈值：根据传感器误差设置合理余量（建议0.5-1.0m）
4. 多项式阶数：纵向5阶/横向4阶是经验最佳平衡点

在Matlab调试时，可以实时可视化以下关键数据：

• 所有采样轨迹的代价分布热力图
• 最优轨迹的s-t、d-t、v-t曲线
• 碰撞检测的几何关系图示

经过多个项目的实践验证，Lattice规划在结构化道路场景中表现优异。但当面对完全非结构化环境（如停车场）时，建议结合随机采样方法（如Hybrid A*）使用。算法的核心优势在于其平衡了计算效率和规划质量，通过精心设计的采样策略和代价函数，能够满足自动驾驶对实时性和安全性的双重需求。