从零学习自动驾驶Lattice规划算法(下)：轨迹采样、评估、碰撞检测 - 包含Matlab与...

从零学习自动驾驶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); % 五次多项式参数 endC的实现就啰嗦多了但Qt的可视化确实比Matlab的plot香vectordouble lateralSampling(double width, int num){ vectordouble samples; double step 2*width/(num-1); for(int i0; inum; 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; endC实现用了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 i0; imxGetN(obsArr); i){ obstacles.emplace_back(data[i*5], data[i*51], data[i*52], data[i*53], data[i*54]); } 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版本能跑到120fpsMatlab加上加速代码也能到60fps左右。从零学习自动驾驶Lattice规划算法(下) 轨迹采样 轨迹评估 碰撞检测 包含matlab代码实现和cpp代码实现方便对照学习。 cpp代码用vs2019编译 依赖qt5.15做可视化 更新: 1 优化绘图 2 增加轨迹预测模块 3 增加从mat文件加载场景的功能方便场景自定义代码仓库已更新道路渐变显示和夜间模式车道线用三次贝塞尔曲线渲染比原来顺滑不少。碰到轨迹震荡问题可以尝试在代价函数里加入加速度变化率惩罚项亲测有效。