从零学习自动驾驶Lattice规划算法(下) 轨迹采样 轨迹评估 碰撞检测 包含matlab代码实现和cpp代码实现，方便对照学习。 cpp代码用vs2019编译 依赖qt5.15做可视化 更新: 1 优化绘图 2 增加轨迹预测模块 3 增加从mat文件加载场景的功能，方便场景自定义

在轨迹生成环节，我们首先得解决如何用代码描述车辆可能的运动路径。横向采样我喜欢用五次多项式，纵向用匀加速模型——Matlab里一行linspace就能搞定，C++这边得老老实实写循环：

% 横向采样
lat_samples = linspace(-2, 2, 5); % 车道左右各2米范围
for i = 1:length(lat_samples)
    coeff = polyfit([0,5], [0,lat_samples(i)], 5); % 五次多项式参数
end

C++的实现就啰嗦多了，但Qt的可视化确实比Matlab的plot香：

vector<double> lateralSampling(double width, int num){
    vector<double> samples;
    double step = 2*width/(num-1);
    for(int i=0; i<num; ++i){
        samples.push_back(-width + i*step);
    }
    return samples;
}

轨迹评估环节的核心是代价函数设计。最近给项目加了动态权重机制，速度快的轨迹不再无脑优先。Matlab用矩阵运算处理多个轨迹点超方便：

costs = 0.3*abs(velocities - target_speed) + 0.7*path_curvatures;
[~, idx] = min(costs);
best_traj = trajectories(idx,:);

C++这边得自己管理数据结构，不过用lambda表达式排序还挺酷：

sort(trajectories.begin(), trajectories.end(), [](const Trajectory& a, const Trajectory& b){
    return 0.3*fabs(a.velocity - targetSpeed) + 0.7*a.curvature 
         < 0.3*fabs(b.velocity - targetSpeed) + 0.7*b.curvature;
});

碰撞检测升级后支持动态障碍物预测，这里用分离轴定理检测矩形碰撞。Matlab版本注意用向量化避免循环：

function collision = checkCollision(ego, obstacle)
    % 投影到障碍物坐标系
    rel_pos = ego(1:2) - obstacle(1:2);
    rot_mat = [cos(obstacle(3)) sin(obstacle(3)); 
              -sin(obstacle(3)) cos(obstacle(3))];
    proj_pos = rot_mat * rel_pos';
    % 分离轴检测
    overlap_x = abs(proj_pos(1)) < (ego(4)/2 + obstacle(4)/2);
    overlap_y = abs(proj_pos(2)) < (ego(5)/2 + obstacle(5)/2);
    collision = overlap_x && overlap_y;
end

C++实现用了Qt的QRectF自带碰撞检测，但自定义实现更灵活：

bool collisionCheck(const VehicleRect& ego, const VehicleRect& obs){
    // 坐标变换到障碍物局部坐标系
    QPointF delta = ego.center - obs.center;
    float rotatedX = delta.x()*cos(-obs.theta) - delta.y()*sin(-obs.theta);
    float rotatedY = delta.x()*sin(-obs.theta) + delta.y()*cos(-obs.theta);
    
    return fabs(rotatedX) < (ego.width + obs.width)/2 
        && fabs(rotatedY) < (ego.length + obs.length)/2;
}

新增的mat场景加载功能让调试效率翻倍。Matlab直接load就行，C++得自己解析二进制：

void loadScenario(const string& path){
    MATFile *mat = matOpen(path.c_str(), "r");
    mxArray *obsArr = matGetVariable(mat, "obstacles");
    double *data = mxGetPr(obsArr);
    for(int i=0; i<mxGetN(obsArr); i++){
        obstacles.emplace_back(data[i*5], data[i*5+1], data[i*5+2],
                              data[i*5+3], data[i*5+4]);
    }
    matClose(mat);
}

可视化方面，Qt的QCustomPlot比Matlab的动画函数更适合实时显示。轨迹预测模块用红色半透明带表示可能路径，障碍物运动预测用卡尔曼滤波简单实现：

void PredictionThread::run(){
    while(!stopFlag){
        for(auto& obj : movingObjects){
            // 匀速模型预测
            obj.predictedPos = obj.pos + obj.velocity * predictTime;
            emit updatePrediction(obj.id, obj.predictedPos);
        }
        QThread::msleep(100);
    }
}

建议把采样间隔控制在0.3秒以内，不然规划出的轨迹会有明显顿挫感。实测在i5处理器上，C++版本能跑到120fps，Matlab加上加速代码也能到60fps左右。

从零学习自动驾驶Lattice规划算法(下) 轨迹采样 轨迹评估 碰撞检测 包含matlab代码实现和cpp代码实现，方便对照学习。 cpp代码用vs2019编译 依赖qt5.15做可视化 更新: 1 优化绘图 2 增加轨迹预测模块 3 增加从mat文件加载场景的功能，方便场景自定义

代码仓库已更新道路渐变显示和夜间模式，车道线用三次贝塞尔曲线渲染比原来顺滑不少。碰到轨迹震荡问题可以尝试在代价函数里加入加速度变化率惩罚项，亲测有效。