Delaunay三角剖分在FSAE无人赛车路径规划中的应用

1. 项目背景与核心挑战大学生方程式无人赛车FSAE Autonomous是近年来高校智能车辆领域的热门竞赛项目。与传统的有人驾驶赛车不同无人赛车需要完全依靠车载传感器和算法自主完成赛道识别、路径规划和运动控制。其中路径规划作为连接感知与控制的桥梁直接决定了赛车的行驶效率和稳定性。在实际比赛中赛道边界通常由颜色鲜明的交通锥桶标记。这些锥桶的位置信息通过车载摄像头、激光雷达等传感器获取后需要快速转化为可行驶路径。这里面临几个核心挑战实时性要求赛车时速可达60km/h以上算法必须在毫秒级完成计算赛道复杂度锥桶排列可能形成急弯、S弯等复杂几何形状计算资源限制车载计算单元如Jetson Xavier的算力需兼顾其他模块2. Delaunay三角剖分的算法优势2.1 基础原理Delaunay三角剖分是一种将离散点集连接成三角形网格的算法其核心特性是空圆准则——任意三角形的外接圆内不包含其他输入点。这种特性带来两个关键优势最大化最小角避免出现瘦长三角形提高网格质量局部最优性每个三角形都是局部最优的连接方式% MATLAB基础实现示例 points rand(20,2); % 随机生成锥桶坐标 dt delaunayTriangulation(points); triplot(dt);2.2 在路径规划中的特殊价值相比其他空间分割方法如Voronoi图Delaunay三角剖分特别适合赛车场景拓扑保持准确反映锥桶间的空间关系可行驶区域提取通过过滤特定方向的三角形获得路径走廊计算效率O(nlogn)的时间复杂度满足实时需求实践提示实际应用中建议先对原始点集进行凸包计算避免边缘出现过大三角形影响路径质量3. 完整算法实现流程3.1 数据预处理模块function [filteredPoints] preprocess(points) % 去除重复点 [~, uniqueIdx] unique(points,rows); filteredPoints points(uniqueIdx,:); % 坐标归一化适应不同赛道尺寸 filteredPoints (filteredPoints - mean(filteredPoints)) ./ std(filteredPoints); end3.2 三角剖分核心算法function [triangles] delaunayPath(points) % 创建约束条件避免路径穿越锥桶 C [1:size(points,1); 2:size(points,1) 1]; % 执行带约束的Delaunay三角剖分 dt delaunayTriangulation(points, C); triangles dt.ConnectivityList; % 移除长边三角形阈值设为平均边长的1.5倍 edgeLengths pdist2(points, points); avgLength mean(edgeLengths(edgeLengths0)); validTriangles []; for i 1:size(triangles,1) tri triangles(i,:); maxEdge max([norm(points(tri(1),:)-points(tri(2),:)), norm(points(tri(2),:)-points(tri(3),:)), norm(points(tri(3),:)-points(tri(1),:))]); if maxEdge 1.5*avgLength validTriangles [validTriangles; tri]; end end end3.3 路径优化与A*算法结合获得三角网格后需要提取最优路径构建图模型将三角形重心作为节点代价函数设计距离代价欧氏距离曲率代价惩罚急转弯安全代价远离锥桶A*搜索实现function [path] astarSearch(graph, start, goal) openSet start; cameFrom containers.Map(KeyType,double,ValueType,any); gScore containers.Map(KeyType,double,ValueType,double); fScore containers.Map(KeyType,double,ValueType,double); gScore(start) 0; fScore(start) heuristic(start, goal); while ~isempty(openSet) [~, current] min(cell2mat(values(fScore))); if current goal path reconstructPath(cameFrom, current); return; end openSet(openSet current) []; for neighbor graph.neighbors{current} tentative_gScore gScore(current) distance(current, neighbor); if ~isKey(gScore, neighbor) || tentative_gScore gScore(neighbor) cameFrom(neighbor) current; gScore(neighbor) tentative_gScore; fScore(neighbor) gScore(neighbor) heuristic(neighbor, goal); if ~ismember(openSet, neighbor) openSet [openSet, neighbor]; end end end end error(Path not found); end4. 实际应用中的关键参数参数名称推荐值调整策略最大三角形边长1.5×平均边长赛道宽度变化大时适当增加A*启发式权重1.2平衡搜索速度与路径最优性曲率惩罚系数0.3根据赛车最大侧向加速度调整重规划频率10Hz计算资源允许时可提高到20Hz5. 性能优化技巧5.1 内存预分配MATLAB中频繁扩增数组会严重影响性能% 错误做法 validTriangles []; for i 1:N validTriangles [validTriangles; newTri]; end % 正确做法 validTriangles zeros(N,3); count 0; for i 1:N if condition count count 1; validTriangles(count,:) newTri; end end validTriangles validTriangles(1:count,:);5.2 并行计算加速利用MATLAB的并行工具箱加速三角剖分if isempty(gcp(nocreate)) parpool(local,4); % 启用4个工作线程 end parfor i 1:numScans % 并行处理传感器数据 end5.3 算法混合策略在直线段使用纯跟踪算法弯道切换为DelaunayA*组合function [steering] hybridController(path, carState) curvature estimateCurvature(path); if abs(curvature) 0.05 % 直线段 steering purePursuit(path, carState); else % 弯道 steering modelPredictiveControl(path, carState); end end6. 典型问题排查指南6.1 路径震荡问题现象规划的路径在连续帧间剧烈波动解决方案对原始锥桶坐标进行卡尔曼滤波对输出路径应用滑动平均增加三角剖分的边长约束6.2 实时性不足表现算法耗时超过100ms优化步骤使用MATLAB Coder生成C代码限制三角剖分最大点数如50个锥桶采用分层规划策略6.3 特殊赛道情况处理发卡弯场景function [points] handleHairpin(points) % 检测相邻锥桶角度变化 angles atan2(diff(points(:,2)), diff(points(:,1))); deltaAngles diff(angles); % 在急弯处插入虚拟点 insertIdx find(abs(deltaAngles) pi/2); for i 1:length(insertIdx) newPoint mean(points(insertIdx(i):insertIdx(i)1,:)); points [points(1:insertIdx(i),:); newPoint; points(insertIdx(i)1:end,:)]; end end7. 进阶改进方向7.1 动态权重调整根据赛车状态实时调整A*算法的代价权重function [weight] dynamicWeight(carState) % 速度越高曲率权重越大 speedFactor carState.speed / 15; % 归一化到0-1 weight 0.5 0.5*speedFactor; end7.2 机器学习增强使用强化学习优化局部路径% 构建深度Q网络 dqn rlDQNAgent(obsInfo, actInfo); trainOpts rlTrainingOptions(... MaxEpisodes,1000,... StopTrainingCriteria,AverageReward); trainStats train(dqn,env,trainOpts);7.3 硬件在环测试搭建MATLAB/Simulink HIL测试平台在Simulink中建立车辆动力学模型通过ROS连接真实ECU使用xPC Target进行实时测试我在实际比赛中发现当锥桶间距不均匀时简单的边长阈值过滤可能导致路径不连续。这时可以引入三角形面积作为辅助判断条件面积过大的三角形即使边长合格也应剔除。另一个实用技巧是在算法初始化时预计算常见赛道模板的三角剖分运行时只需做局部更新可降低50%以上的计算耗时。