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

1. Lattice规划算法核心原理回顾

Lattice规划算法是自动驾驶领域常用的局部路径规划方法，其核心思想是在Frenet坐标系下对车辆运动进行离散化采样和优化。与传统的全局路径规划不同，Lattice规划更注重实时性和动态避障能力。

Frenet坐标系将车辆运动分解为沿参考线（s方向）和垂直于参考线（d方向）两个分量。这种分解方式使得我们可以独立处理纵向和横向运动规划，大大简化了问题复杂度。在实际应用中，参考线通常是全局规划给出的理想路径中心线。

2. 轨迹采样技术实现

2.1 多项式轨迹生成原理

轨迹采样的本质是在状态空间中进行有目的性的搜索。我们通常采用五次多项式来描述轨迹：

s(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀

选择五次多项式是因为它可以满足位置、速度、加速度三个维度的边界条件约束。在实际采样时，我们需要考虑：

1. 初始状态约束（s₀, ṡ₀, ̈s₀）
2. 终止状态约束（s₁, ṡ₁, ̈s₁）
3. 动态可行性约束（最大加速度、最大曲率等）

2.2 MATLAB实现细节

matlab复制function trajectories = sampleTrajectories(init_state, target_states, time_horizon, dt)
    % init_state: [s0, ds0, dds0]
    % target_states: N x 3矩阵，每行代表一个目标状态[s1, ds1, dds1]
    % time_horizon: 规划时间范围
    % dt: 时间步长
    
    num_samples = size(target_states, 1);
    t = 0:dt:time_horizon;
    trajectories = zeros(length(t), num_samples);
    
    for i = 1:num_samples
        % 计算五次多项式系数
        A = [time_horizon^5, time_horizon^4, time_horizon^3;
             5*time_horizon^4, 4*time_horizon^3, 3*time_horizon^2;
             20*time_horizon^3, 12*time_horizon^2, 6*time_horizon];
        
        b = [target_states(i,1) - (init_state(1) + init_state(2)*time_horizon + 0.5*init_state(3)*time_horizon^2);
             target_states(i,2) - (init_state(2) + init_state(3)*time_horizon);
             target_states(i,3) - init_state(3)];
        
        x = A\b;
        a5 = x(1); a4 = x(2); a3 = x(3);
        a2 = 0.5*init_state(3);
        a1 = init_state(2);
        a0 = init_state(1);
        
        % 生成轨迹
        trajectories(:,i) = a5*t.^5 + a4*t.^4 + a3*t.^3 + a2*t.^2 + a1*t + a0;
    end
end

2.3 C++实现优化

在C++实现中，我们需要注意性能优化和实时性要求：

cpp复制class QuinticPolynomial {
public:
    QuinticPolynomial(double s0, double ds0, double dds0,
                     double s1, double ds1, double dds1, double T) {
        // 系数计算
        Eigen::Matrix3d A;
        A << pow(T,5), pow(T,4), pow(T,3),
             5*pow(T,4), 4*pow(T,3), 3*pow(T,2),
             20*pow(T,3), 12*pow(T,2), 6*T;
        
        Eigen::Vector3d b;
        b << s1 - (s0 + ds0*T + 0.5*dds0*pow(T,2)),
             ds1 - (ds0 + dds0*T),
             dds1 - dds0;
        
        Eigen::Vector3d x = A.colPivHouseholderQr().solve(b);
        
        coefficients.resize(6);
        coefficients << s0, ds0, 0.5*dds0, x(2), x(1), x(0);
    }
    
    double evaluate(double t) const {
        return coefficients(0) + coefficients(1)*t + coefficients(2)*pow(t,2) + 
               coefficients(3)*pow(t,3) + coefficients(4)*pow(t,4) + coefficients(5)*pow(t,5);
    }
    
private:
    Eigen::VectorXd coefficients;
};

提示：在实际工程中，建议使用Eigen等线性代数库来处理矩阵运算，避免手动实现可能引入的数值稳定性问题。

3. 轨迹评估指标体系

3.1 评估指标设计

完整的轨迹评估应该考虑多个维度：

1. 安全性指标：

   • 与障碍物的最小距离
   • 碰撞风险概率
   • 紧急制动距离

2. 舒适性指标：

   • 加速度变化率(Jerk)
   • 横向加速度
   • 曲率连续性

3. 效率指标：

   • 到达时间
   • 路径长度
   • 与参考路径的偏离

3.2 MATLAB多目标评估实现

matlab复制function [scores] = evaluateTrajectories(trajectories, ref_path, obstacles, params)
    % trajectories: N x M矩阵，N条轨迹，每条M个点
    % ref_path: 参考路径
    % obstacles: 障碍物信息
    % params: 评估参数
    
    num_traj = size(trajectories, 1);
    scores = zeros(num_traj, 3); % [安全分, 舒适分, 效率分]
    
    for i = 1:num_traj
        traj = trajectories(i,:);
        
        % 安全性评估
        min_dist = inf;
        for j = 1:size(obstacles,1)
            dist = min(sqrt((traj.x - obstacles(j,1)).^2 + (traj.y - obstacles(j,2)).^2));
            if dist < min_dist
                min_dist = dist;
            end
        end
        safety_score = sigmoid(min_dist, params.safety_thresh);
        
        % 舒适性评估
        jerk = mean(abs(diff(trajectories(i).acc, 2)));
        comfort_score = exp(-params.jerk_weight * jerk);
        
        % 效率评估
        progress = traj.s(end) / ref_path.s(end);
        efficiency_score = progress * exp(-params.lateral_weight * mean(abs(traj.d)));
        
        scores(i,:) = [safety_score, comfort_score, efficiency_score];
    end
end

function y = sigmoid(x, threshold)
    y = 1 ./ (1 + exp(-10*(x - threshold)));
end

3.3 C++评估优化技巧

在C++实现中，我们可以采用并行计算加速评估过程：

cpp复制std::vector<TrajectoryScore> evaluateInParallel(
    const std::vector<Trajectory>& trajectories,
    const ReferencePath& ref_path,
    const ObstacleList& obstacles,
    const EvaluationParams& params) {
    
    std::vector<TrajectoryScore> scores(trajectories.size());
    
    #pragma omp parallel for
    for (size_t i = 0; i < trajectories.size(); ++i) {
        const auto& traj = trajectories[i];
        
        // 安全性评估
        double min_dist = std::numeric_limits<double>::max();
        for (const auto& obs : obstacles) {
            double dist = (traj.position - obs.position).norm();
            if (dist < min_dist) min_dist = dist;
        }
        double safety = 1.0 / (1.0 + exp(-10.0*(min_dist - params.safety_threshold)));
        
        // 舒适性评估
        double jerk = computeJerk(traj);
        double comfort = exp(-params.jerk_weight * jerk);
        
        // 效率评估
        double progress = traj.s.back() / ref_path.length();
        double efficiency = progress * exp(-params.lateral_weight * computeLateralDeviation(traj, ref_path));
        
        scores[i] = {safety, comfort, efficiency};
    }
    
    return scores;
}

4. 碰撞检测关键技术

4.1 层次化碰撞检测策略

在实际应用中，我们通常采用层次化检测策略来提高效率：

1. 粗略检测：使用包围盒(Bounding Box)快速筛选可能碰撞的物体
2. 精确检测：对筛选后的物体进行多边形级别的精确碰撞检测
3. 连续检测：考虑物体运动轨迹，预测未来可能发生的碰撞

4.2 MATLAB实现示例

matlab复制function [collision_flag, ttc] = checkCollision(trajectory, obstacle_trajectory, vehicle_size, obstacle_size)
    % 输入轨迹应为时间序列上的位置和朝向
    time_steps = length(trajectory.t);
    collision_flag = false;
    ttc = inf; % time to collision
    
    for t = 1:time_steps
        % 获取当前时刻车辆和障碍物的状态
        vehicle_state = trajectory.getState(t);
        obstacle_state = obstacle_trajectory.getState(t);
        
        % 计算两个矩形的位置关系
        [overlap, distance] = rectIntersection(...
            vehicle_state.x, vehicle_state.y, vehicle_state.theta, ...
            vehicle_size.length, vehicle_size.width, ...
            obstacle_state.x, obstacle_state.y, obstacle_state.theta, ...
            obstacle_size.length, obstacle_size.width);
        
        if overlap
            collision_flag = true;
            ttc = trajectory.t(t);
            return;
        end
    end
end

function [overlap, distance] = rectIntersection(x1, y1, theta1, l1, w1, x2, y2, theta2, l2, w2)
    % 实现两个旋转矩形的碰撞检测
    % 使用分离轴定理(SAT)算法
    % 此处省略具体实现细节...
end

4.3 C++高效碰撞检测

在C++中，我们可以使用专门的几何计算库来提高性能：

cpp复制#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Polygon_2.h>

typedef CGAL::Exact_predicates_exact_constructions_kernel K;
typedef K::Point_2 Point;
typedef CGAL::Polygon_2<K> Polygon;

bool checkCollision(const VehicleState& ego, const VehicleState& obstacle, 
                   const VehicleParams& ego_params, const VehicleParams& obs_params) {
    // 构建自车多边形
    Polygon ego_poly;
    buildVehiclePolygon(ego, ego_params.length, ego_params.width, ego_poly);
    
    // 构建障碍物多边形
    Polygon obs_poly;
    buildVehiclePolygon(obstacle, obs_params.length, obs_params.width, obs_poly);
    
    // 使用CGAL进行多边形相交检测
    return CGAL::do_intersect(ego_poly, obs_poly);
}

void buildVehiclePolygon(const VehicleState& state, double length, double width, Polygon& poly) {
    double half_l = length / 2;
    double half_w = width / 2;
    
    // 四个角点的局部坐标
    std::vector<Point> points = {
        Point(-half_l, -half_w),
        Point(-half_l, half_w),
        Point(half_l, half_w),
        Point(half_l, -half_w)
    };
    
    // 应用旋转和平移变换
    double cos_theta = cos(state.theta);
    double sin_theta = sin(state.theta);
    
    for (auto& p : points) {
        double x = p.x() * cos_theta - p.y() * sin_theta + state.x;
        double y = p.x() * sin_theta + p.y() * cos_theta + state.y;
        p = Point(x, y);
    }
    
    poly = Polygon(points.begin(), points.end());
}

5. 系统集成与可视化

5.1 MATLAB可视化增强

matlab复制function visualizeScenario(trajectories, best_traj, obstacles, ref_path)
    figure('Position', [100, 100, 1200, 800]);
    
    % 绘制参考路径
    plot(ref_path.s, ref_path.d, 'k--', 'LineWidth', 1.5);
    hold on;
    
    % 绘制障碍物
    for i = 1:size(obstacles,1)
        rectangle('Position', [obstacles(i,1)-obstacles(i,3)/2, obstacles(i,2)-obstacles(i,4)/2, ...
                  obstacles(i,3), obstacles(i,4)], 'FaceColor', [1 0.5 0.5], 'EdgeColor', 'r');
    end
    
    % 绘制所有采样轨迹
    for i = 1:size(trajectories,2)
        plot(trajectories(i).s, trajectories(i).d, 'Color', [0.7 0.7 0.7], 'LineWidth', 0.5);
    end
    
    % 高亮显示最优轨迹
    plot(best_traj.s, best_traj.d, 'b', 'LineWidth', 2.5);
    
    % 绘制车辆当前位置
    rectangle('Position', [best_traj.s(1)-2.5, best_traj.d(1)-1, 5, 2], ...
              'Curvature', [0.3 0.3], 'FaceColor', [0.2 0.6 1]);
    
    axis equal;
    grid on;
    xlabel('纵向距离 s (m)');
    ylabel('横向距离 d (m)');
    title('Lattice规划结果可视化');
    legend('参考路径', '障碍物', '采样轨迹', '最优轨迹', '车辆');
end

5.2 Qt可视化实现要点

在Qt中实现专业可视化需要注意以下要点：

cpp复制class PlanningVisualizer : public QWidget {
public:
    explicit PlanningVisualizer(QWidget *parent = nullptr) : QWidget(parent) {
        setFixedSize(1200, 800);
        setAutoFillBackground(true);
        QPalette palette;
        palette.setColor(QPalette::Window, Qt::white);
        setPalette(palette);
    }
    
    void setData(const PlanningData &data) {
        planning_data = data;
        update();
    }
    
protected:
    void paintEvent(QPaintEvent *) override {
        QPainter painter(this);
        painter.setRenderHint(QPainter::Antialiasing);
        
        // 坐标变换
        QTransform transform;
        transform.translate(100, height() - 100);
        transform.scale(10, -10); // 1像素=0.1米，Y轴向上为正
        painter.setTransform(transform);
        
        // 绘制参考路径
        painter.setPen(QPen(Qt::black, 0.1, Qt::DashLine));
        painter.drawPolyline(planning_data.ref_path);
        
        // 绘制障碍物
        painter.setBrush(QBrush(QColor(255, 128, 128)));
        painter.setPen(QPen(Qt::red, 0.1));
        for (const auto &obs : planning_data.obstacles) {
            painter.drawRect(obs.rect);
        }
        
        // 绘制采样轨迹
        painter.setPen(QPen(QColor(180, 180, 180), 0.05));
        for (const auto &traj : planning_data.trajectories) {
            painter.drawPolyline(traj.points);
        }
        
        // 绘制最优轨迹
        painter.setPen(QPen(Qt::blue, 0.2));
        painter.drawPolyline(planning_data.best_trajectory.points);
        
        // 绘制车辆
        painter.setBrush(QBrush(QColor(51, 153, 255)));
        painter.setPen(QPen(Qt::darkBlue, 0.1));
        painter.drawEllipse(planning_data.vehicle_pos, 0.5, 0.5);
    }
    
private:
    PlanningData planning_data;
};

6. 工程实践中的关键问题

6.1 实时性优化技巧

1. 采样空间剪枝：根据车辆动力学约束提前剔除不可能实现的轨迹
2. 多分辨率采样：先粗采样再局部精细采样
3. 并行计算：利用多线程同时评估多条轨迹
4. 缓存复用：对不变的计算结果进行缓存

6.2 典型问题排查指南

6.3 参数调试经验

1. 时间分辨率：通常选择0.1-0.3秒，太大会漏检碰撞，太小会增加计算量
2. 采样数量：横向采样5-7个，纵向采样3-5个速度级别是较好的起点
3. 安全距离：建议设置为车辆宽度的一半加上0.3-0.5米缓冲
4. 权重调整：先确定安全，再调舒适，最后优化效率

在实际项目中，我发现一个实用的调试方法是先固定横向采样，调整纵向速度级别直到找到平衡点。例如：

1. 固定横向采样7个点（-1.5m到1.5m，间隔0.5m）
2. 从3个速度级别（减速、匀速、加速）开始
3. 逐步增加速度级别直到计算时间接近预算上限

7. 进阶功能实现

7.1 场景加载模块优化

在实际工程中，我们需要支持多种场景格式：

cpp复制class ScenarioLoader {
public:
    bool loadFromFile(const std::string &filename) {
        std::string ext = getFileExtension(filename);
        
        if (ext == "mat") {
            return loadMatlabScenario(filename);
        } else if (ext == "json") {
            return loadJsonScenario(filename);
        } else if (ext == "xml") {
            return loadXmlScenario(filename);
        } else {
            std::cerr << "Unsupported file format: " << ext << std::endl;
            return false;
        }
    }
    
private:
    bool loadMatlabScenario(const std::string &filename) {
        // 使用matio库实现
        // ...
    }
    
    bool loadJsonScenario(const std::string &filename) {
        // 使用nlohmann/json等库实现
        // ...
    }
    
    std::string getFileExtension(const std::string &filename) {
        size_t dot_pos = filename.find_last_of(".");
        if (dot_pos == std::string::npos) return "";
        return filename.substr(dot_pos + 1);
    }
};

7.2 轨迹预测模块实现

完整的轨迹预测需要考虑：

1. 障碍物运动模型（恒定速度、加速度模型等）
2. 预测不确定性处理
3. 多假设预测生成

cpp复制class TrajectoryPredictor {
public:
    std::vector<PredictedTrajectory> predict(const Obstacle &obstacle, double time_horizon) {
        std::vector<PredictedTrajectory> predictions;
        
        // 恒定速度模型预测
        if (obstacle.velocity_history.size() >= 2) {
            Eigen::Vector2d vel = estimateVelocity(obstacle);
            predictions.push_back(constantVelocityPrediction(obstacle, vel, time_horizon));
        }
        
        // 恒定加速度模型预测
        if (obstacle.velocity_history.size() >= 3) {
            Eigen::Vector2d acc = estimateAcceleration(obstacle);
            predictions.push_back(constantAccelerationPrediction(obstacle, acc, time_horizon));
        }
        
        // 基于运动模式的预测
        if (!obstacle.motion_patterns.empty()) {
            for (const auto &pattern : obstacle.motion_patterns) {
                predictions.push_back(patternBasedPrediction(obstacle, pattern, time_horizon));
            }
        }
        
        return predictions;
    }
    
private:
    PredictedTrajectory constantVelocityPrediction(const Obstacle &obs, const Eigen::Vector2d &vel, double T) {
        PredictedTrajectory traj;
        traj.probability = 0.7; // 根据实际情况调整
        
        for (double t = 0; t <= T; t += 0.1) {
            traj.points.emplace_back(obs.position + vel * t);
            traj.time_points.push_back(t);
        }
        
        return traj;
    }
};

在工程实践中，轨迹预测的准确性直接影响规划安全性。建议在实际部署时采用多模型融合的方法，并结合实时感知数据不断修正预测结果。