基于代价函数的无人车轨迹规划Matlab实现

1. 项目概述无人车轨迹规划是自动驾驶领域的核心技术之一其核心任务是在满足各种约束条件下为车辆寻找一条从起点到终点的最优运动轨迹。基于代价函数的轨迹规划方法通过构建合理的评价体系能够在复杂环境中实现安全、高效的路径规划。本项目使用Matlab实现了一种基于代价函数的最优轨迹探索算法重点解决了以下问题如何在动力学约束下生成平滑轨迹如何设计合理的代价函数评估轨迹质量如何优化计算效率实现实时规划2. 核心算法原理2.1 代价函数设计代价函数是轨迹优化的核心本项目采用多目标加权的方式构建J w1·J_smooth w2·J_obstacle w3·J_dynamic其中J_smooth平滑性代价评估轨迹的导数连续性J_obstacle障碍物代价保证避障安全性J_dynamic动力学代价确保符合车辆运动约束2.2 轨迹参数化方法采用五次多项式参数化轨迹x(t) a0 a1t a2t² a3t³ a4t⁴ a5t⁵ y(t) b0 b1t b2t² b3t³ b4t⁴ b5t⁵这种表示方法可以保证轨迹在位置、速度、加速度层面的连续性。2.3 优化求解过程使用序列二次规划(SQP)算法进行优化求解初始化轨迹参数计算当前代价函数值构建二次近似模型求解QP子问题线搜索确定步长更新参数迭代优化3. Matlab实现详解3.1 环境建模% 创建仿真环境 map binaryOccupancyMap(100,100,1); % 添加障碍物 obsPos [30 30; 70 40; 50 80]; obsRadius [10; 15; 12]; for i 1:size(obsPos,1) circleObs insertShape(zeros(100),FilledCircle,... [obsPos(i,:) obsRadius(i)],Color,black); setOccupancy(map, im2single(circleObs)0); end3.2 轨迹优化核心代码function [optTraj, cost] optimizeTrajectory(start, goal, map) % 初始化参数 n 10; % 轨迹段数 T 5; % 总时间 % 构建优化问题 prob optimproblem; coeffs optimvar(coeffs,n,6,2); % x,y方向系数 % 定义约束 prob.Constraints.continuity ... [coeffs(1:end-1,3:6,1) coeffs(2:end,1:4,1); coeffs(1:end-1,3:6,2) coeffs(2:end,1:4,2)]; % 定义代价函数 prob.Objective computeCost(coeffs, map, T); % 求解优化问题 [sol, cost] solve(prob); optTraj sol.coeffs; end3.3 代价函数计算function cost computeCost(coeffs, map, T) % 平滑性代价 jerk computeJerk(coeffs, T); smoothCost sum(jerk.^2,all); % 障碍物代价 [x,y] sampleTrajectory(coeffs, T); obsCost computeObstacleCost(x,y,map); % 动力学代价 [v,a] computeDerivatives(coeffs, T); dynCost sum(max(0, abs(v)-3).^2) sum(max(0, abs(a)-1.5).^2); % 总代价 cost 0.5*smoothCost 0.3*obsCost 0.2*dynCost; end4. 关键实现技巧4.1 轨迹采样优化为提高碰撞检测效率采用自适应采样策略初始粗采样(20点/段)在接近障碍物区域加密采样(50点/段)使用KD-tree加速最近邻查询4.2 梯度计算加速利用Matlab的自动微分功能prob optimproblem(ObjectiveSense,minimize); prob.Objective fcn2optimexpr(computeCost, coeffs, map, T);4.3 多分辨率规划分层规划策略全局粗规划(低分辨率)局部精细优化(高分辨率)实时重规划(5Hz更新率)5. 性能优化建议5.1 并行计算利用parfor加速代价计算parfor i 1:size(trajSamples,1) costs(i) pointCost(trajSamples(i,:), map); end5.2 热启动策略保留上一周期解作为初始猜测if exist(prevSol,var) prob.x0 prevSol; end5.3 代码向量化避免循环使用矩阵运算% 低效实现 for i 1:n traj(i,:) coeffs(i,1) coeffs(i,2)*t ...; end % 高效实现 tVec t.^(0:5); traj coeffs * tVec;6. 实际应用注意事项实时性保障单次规划时间控制在100ms内使用Coder生成可部署代码参数调优建议先调整平滑项权重保证基础轨迹质量再调整障碍物权重确保安全性最后微调动力学权重特殊场景处理狭窄通道增加采样密度动态障碍引入预测模块极端工况设计应急轨迹7. 扩展应用方向多车协同规划引入交互代价项分布式优化框架学习增强方法使用强化学习优化权重神经网络预测初始解复杂场景适应非结构化道路恶劣天气条件混合交通场景本实现已在多种仿真场景中验证包括城市道路、停车场等典型环境。实际测试表明算法能够在100ms内完成20m范围内的轨迹规划满足实时性要求。通过调整代价权重可以适应不同的驾驶风格偏好从保守型到激进型均可实现。