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

1. 项目背景与核心挑战

大学生方程式无人赛车（FSAE Autonomous）是近年来高校智能车辆领域的热门竞赛项目。与传统的有人驾驶赛车不同，无人赛车需要完全依靠车载传感器和算法自主完成锥桶赛道识别、路径规划和运动控制。这其中，路径规划算法的优劣直接决定了赛车能否在复杂赛道中快速而稳定地行驶。

在真实比赛中，赛道由一系列锥桶标记边界，这些锥桶的排布往往呈现不规则形态。传统的基于规则几何（如三次样条曲线）的路径规划方法在这种离散点环境下表现不佳，容易出现路径震荡或计算效率低下的问题。而Delaunay三角剖分作为一种计算几何方法，恰好擅长处理离散点的空间关系建模。

提示：Delaunay三角剖分的核心特性是"空圆性"——即任何一个三角形的外接圆内不包含其他输入点。这一特性使得生成的三角形尽可能接近等边三角形，避免了"瘦长"三角形带来的数值不稳定问题。

2. Delaunay三角剖分的赛道建模

2.1 锥桶点集的预处理

实际比赛中，激光雷达或摄像头检测到的锥桶坐标可能存在噪声和离群点。我们首先需要对原始点集进行预处理：

matlab复制% 读取锥桶坐标数据（示例）
cones = load('cone_positions.mat'); 

% 离群点过滤（基于统计距离）
mean_pos = mean(cones);
distances = vecnorm(cones - mean_pos, 2, 2);
valid_idx = distances < 3*std(distances);
filtered_cones = cones(valid_idx, :);

% 可视化对比
figure;
subplot(1,2,1); scatter(cones(:,1), cones(:,2), 'r'); title('原始锥桶');
subplot(1,2,2); scatter(filtered_cones(:,1), filtered_cones(:,2), 'g'); title('过滤后锥桶');

2.2 三角剖分的实现

Matlab内置的delaunayTriangulation函数可以直接实现Delaunay三角剖分：

matlab复制DT = delaunayTriangulation(filtered_cones);
triplot(DT); % 可视化三角网格

关键点在于理解三角剖分生成的拓扑结构。每个三角形代表赛道的一个局部区域，而三角形的边则隐含了锥桶之间的空间关系。我们需要特别关注那些连接左右两侧锥桶的"跨道边"——它们标志着赛道宽度的变化区域。

2.3 赛道中心线提取

基于三角剖分结果，我们采用以下策略提取中心线：

1. 识别所有连接左右赛道锥桶的边（通过判断边的两个端点分别属于左侧还是右侧锥桶集合）
2. 计算这些边的中点
3. 使用平滑样条曲线连接这些中点

matlab复制% 假设已定义left_cones和right_cones的索引
cross_edges = [];
for i = 1:size(DT.ConnectivityList,1)
    tri = DT.ConnectivityList(i,:);
    % 检查三角形是否包含左右锥桶
    if any(ismember(tri, left_cones)) && any(ismember(tri, right_cones))
        % 找出连接左右的边
        [edge1, edge2] = find_crossing_edges(tri, left_cones, right_cones);
        cross_edges = [cross_edges; edge1; edge2];
    end
end

% 计算中点
mid_points = (DT.Points(cross_edges(:,1),:) + DT.Points(cross_edges(:,2),:))/2;

% 样条拟合
center_line = fit_spline(mid_points);

3. 路径优化与速度规划

3.1 基于曲率的路径优化

单纯的中心线可能不符合赛车动力学要求。我们需要考虑：

1. 曲率连续性：避免突变导致转向电机过载
2. 最大曲率限制：确保赛车能以目标速度通过弯道

采用优化算法调整中心线：

matlab复制function optimized_path = optimize_curvature(initial_path, max_curvature)
    % 定义优化变量（路径点的横向偏移）
    opt_var = zeros(size(initial_path,1),1);
    
    % 设置优化选项
    options = optimoptions('fmincon', 'Algorithm','sqp',...
        'MaxIterations',1000);
    
    % 运行优化
    opt_var = fmincon(@(x) curvature_cost(x,initial_path),...
        opt_var,[],[],[],[],[],[],...
        @(x) curvature_constraint(x,initial_path,max_curvature),...
        options);
    
    % 应用优化结果
    optimized_path = apply_offset(initial_path, opt_var);
end

3.2 速度剖面生成

结合路径曲率和赛车动力学参数生成速度建议：

matlab复制function speed_profile = generate_speed_profile(path, max_lat_acc)
    curvature = compute_curvature(path);
    max_speed = sqrt(max_lat_acc ./ abs(curvature));
    max_speed(isinf(max_speed)) = max_forward_speed;
    
    % 考虑加速度限制平滑速度曲线
    speed_profile = smooth_speed(max_speed, max_accel, max_decel);
end

4. 实车测试中的关键问题

4.1 实时性保障

Matlab算法需要移植到嵌入式平台（如NVIDIA Jetson）。我们采用以下优化措施：

1. 预计算静态三角剖分，仅对新增锥桶进行增量更新
2. 限制三角剖分区域为当前视野范围内的锥桶
3. 使用mex函数加速核心计算部分

matlab复制% 增量更新示例
function DT = update_triangulation(DT, new_cones)
    % 移除视野外的点
    DT.Points = remove_out_of_view(DT.Points);
    
    % 添加新点
    DT.Points = [DT.Points; new_cones];
    
    % 增量更新
    DT = delaunayTriangulation(DT.Points);
end

4.2 噪声与动态环境处理

实际场景中可能遇到：

• 锥桶被碰倒导致位置突变
• 临时障碍物出现
• 传感器短暂失效

我们采用多帧融合策略：

matlab复制% 历史锥桶加权融合
function stable_cones = fuse_cones(current_cones, history)
    % 简单示例：取最近3次检测的交集
    persistent cone_history;
    if isempty(cone_history)
        cone_history = repmat({current_cones},3,1);
    else
        cone_history = [cone_history(2:end); {current_cones}];
    end
    
    % 寻找稳定出现的锥桶
    stable_cones = find_consistent_cones(cone_history);
end

5. 完整算法流程与参数调优

5.1 算法主循环

matlab复制function main_loop()
    % 初始化
    DT = []; 
    path_history = [];
    
    while racing
        % 获取最新锥桶检测
        [new_cones, is_valid] = get_detection();
        
        % 数据有效性检查
        if ~is_valid
            use_last_path();
            continue;
        end
        
        % 历史数据融合
        stable_cones = fuse_cones(new_cones);
        
        % 更新三角剖分
        if isempty(DT)
            DT = delaunayTriangulation(stable_cones);
        else
            DT = update_triangulation(DT, stable_cones);
        end
        
        % 路径生成与优化
        raw_path = extract_centerline(DT);
        smooth_path = optimize_curvature(raw_path);
        
        % 速度规划
        speed_profile = generate_speed_profile(smooth_path);
        
        % 控制指令下发
        send_to_controller(smooth_path, speed_profile);
    end
end

5.2 关键参数经验值

根据我们车队实测经验，以下参数效果较好：

6. 进阶优化方向

6.1 混合A*搜索改进

对于急弯区域，可以结合A*算法在三角网格上进行局部重规划：

matlab复制function hybrid_path = hybrid_planner(DT, current_pos)
    % 在三角网格上定义可通行区域
    traversable = define_traversable_triangles(DT);
    
    % 运行A*搜索
    [path_nodes] = astar_search(DT, current_pos, traversable);
    
    % 路径后处理
    hybrid_path = smooth_astar_path(path_nodes);
end

6.2 机器学习增强

使用神经网络预测最优路径参数：

matlab复制function params = predict_params(scene_features)
    % 加载预训练模型
    persistent net;
    if isempty(net)
        net = load('path_nn.mat');
    end
    
    % 预测
    params = predict(net, scene_features);
end

实际测试表明，这种数据驱动的方法可以提升算法在特殊赛道布局（如S弯连续组合）下的表现。