基于Delaunay三角剖分的无人驾驶赛车实时路径规划实践

1. 项目概述：从赛道到路径的智能转换

在大学生方程式无人驾驶赛车的世界里，路径规划是连接感知与控制的“大脑”。想象一下，你驾驶一辆赛车，眼前是一条由锥桶标记的复杂赛道，你的任务不是简单地沿着锥桶走，而是规划出一条能让你最快、最稳冲过终点的理想路线。这就是我们项目的核心：为无人赛车在动态、未知的赛道上，实时找出一条最优行驶路径。我参与过多次相关赛事的软件系统开发，深知其中挑战——赛道边界模糊、计算资源有限、实时性要求极高。传统的基于网格或采样的方法在弯道密集的赛道上要么精度不够，要么计算负担太重。

这时，Delaunay三角剖分技术进入了我们的视野。它听起来像是个复杂的数学工具，但用赛车手的语言来说，它就像一个极其聪明的“赛道理解器”。它能把散乱分布的锥桶（赛道边界点）瞬间连接成一个没有重叠、没有狭长三角形的三角网，这个网完美地勾勒出了赛道的“骨架”和“可通行区域”。基于这个三角网，我们就能像在迷宫中寻找最短路径一样，为赛车规划出平滑、安全且逼近理论最优的行驶线。这个项目不仅仅是算法的堆砌，更是一场对实时性、鲁棒性和工程实践能力的极限考验。接下来，我将拆解我们如何将Delaunay三角剖分这一几何工具，转化为赛车在赛道上飞驰的智能决策核心。

2. 核心思路与方案选型：为什么是Delaunay？

在无人驾驶领域，路径规划算法众多，如A*、Dijkstra、RRT*等。但在大学生方程式无人车这个特定场景下，我们需要进行严格的取舍。赛道由离散的锥桶点定义，环境结构化程度相对较高，但实时感知的数据是稀疏且带噪声的。我们的核心需求可以归结为三点：实时性（规划周期需小于50ms）、平滑性（生成的路径需满足车辆动力学约束，避免急转）、对未知区域的探索能力（能处理部分赛道尚未被完全感知的情况）。

2.1 传统方法的局限性

我们最初尝试过栅格法，将赛道区域划分为细密的网格。但问题立刻显现：为了精度，网格必须足够小，这导致状态空间爆炸，搜索耗时无法满足实时性。况且，锥桶间的空隙（赛道宽度）是变化的，固定大小的网格无法优雅适配。我们也考虑过直接使用样条曲线拟合锥桶中心线，但这方法过于“脆弱”——一旦某个锥桶识别错误或丢失，整个拟合曲线就会畸变，且难以处理岔道或临时障碍。

2.2 Delaunay三角剖分的优势

Delaunay三角剖分脱颖而出，正是因为它完美地匹配了我们的问题特征：

1. 从点到面，构建通行空间：输入是一堆无序的锥桶坐标点（赛道边界点），Delaunay算法能输出一个三角网格。这个网格的妙处在于，其三角形的外接圆内不包含任何其他输入点（空圆特性），这保证了生成的三角形尽可能“胖”，避免出现尖锐的角。在赛道语境下，这些“胖”三角形更符合车辆的实际可通行区域。
2. 天然的路网图：三角网格的边和顶点，自然构成了一个图（Graph）。每个三角形重心或边中点可以作为图的节点，三角形之间的邻接关系构成了图的边。这样，路径规划问题就转化为了在这个图上搜索从起点到目标点的最优路径问题，可以直接应用高效的图搜索算法（如A*）。
3. 对噪声和不完整数据的鲁棒性：即使个别锥桶漏检或位置有偏差，Delaunay三角剖分生成的网格整体结构依然是稳定的，不会像直接拟合那样全局崩溃。这对于依赖视觉/激光雷达、难免有感知噪声的赛车环境至关重要。
4. 计算效率与可扩展性：存在成熟的增量式Delaunay三角剖分算法（如Bowyer-Watson算法），可以在新车感知到新的锥桶点时，高效地更新现有三角网格，而不是推倒重算。这为实时在线规划奠定了基础。

基于以上考量，我们确定了“感知点云 -> Delaunay三角剖分 -> 构建拓扑图 -> 图搜索生成粗路径 -> 路径平滑优化”的技术路线。这个方案在理论优雅性和工程可行性之间取得了最佳平衡。

3. 核心细节解析与实操要点

将理论落地，需要攻克一系列工程细节。这里分享几个最关键环节的实现要点和踩过的坑。

3.1 感知数据的预处理与清洗

来自激光雷达或摄像头的原始锥桶点云是不能直接扔给Delaunay算法的。我们的预处理流水线包括：

• 聚类与过滤：使用欧几里得聚类（如DBSCAN）将离散的点云聚合成一个个独立的锥桶对象。这里的关键是设置合适的距离阈值。太大会把不同行的锥桶聚到一起，太小则会把一个锥桶因噪声产生的多个点分成不同类。我们的经验值是0.3米到0.5米，具体需根据传感器噪声水平调整。
• 地面点移除与高度过滤：赛道路面是平坦的，但传感器会扫到地面反射点。通过简单的平面拟合或设置Z轴（高度）阈值，可以滤除大部分地面噪声。只保留高度在锥桶典型范围内的点。
• 赛道边界分类（可选但推荐）：如果能够区分左右侧的锥桶（例如通过颜色识别或基于赛车位姿的简单逻辑判断），将为后续规划带来巨大好处。我们可以构建“左边界点集”和“右边界点集”，分别进行三角剖分，或者将其合并但打上标签，用于在生成图时定义可行区域。

注意：预处理阶段的参数需要在实际赛场上进行大量调试。静态调试的结果和赛车高速动态运行下的结果可能差异很大。务必录制真实赛道数据包进行回放测试。

3.2 Delaunay三角剖分的具体实现与约束

我们选择了Bowyer-Watson增量算法，因为它适合在线更新。实现时有几个陷阱：

• 超级三角形的设置：算法需要一个能包含所有点的大三角形作为初始。这个三角形的尺寸要足够大，确保所有点都在其内部，但也不能太大，否则在浮点数计算中可能引入精度问题。通常取点集坐标最大最小值外扩一个比例（如20%）。
• 局部重三角化的效率：当插入新点时，需要删除所有外接圆包含该点的三角形，形成一个“空洞”，然后在空洞内重新三角化。这里的核心是高效地找到受影响的三角形。维护一个良好的邻接关系数据结构至关重要。
• 引入赛道约束：标准的Delaunay三角剖分会在所有点之间连边，这可能导致三角形穿过赛道（即连接了左右边界的点）。我们必须将其转化为约束Delaunay三角剖分。我们需要预先定义一组“约束边”，例如所有相邻的、同侧的锥桶之间的连线。算法会保证这些约束边出现在最终的三角网格中。这样，赛道边界就被强制固定下来，三角网只在赛道内部和外部生成，不会“穿墙”。

3.3 从三角网格到拓扑路网

生成三角网格后，我们需要从中提取一个用于路径搜索的图。这里有两种主流思路：

1. 以三角形重心为节点：将每个三角形的重心作为图节点。如果两个三角形共享一条边，且这条边不是赛道约束边（即可通行），则在这两个三角形的重心节点间建立一条边，边的权重可以是两重心间的欧氏距离。
2. 以三角形边的中点为节点：将每个三角形非约束边的中点作为图节点。节点之间的连接关系由三角形的邻接关系决定。这种方法生成的路径点更多，可能更平滑。

我们选择了第一种方法，因为它生成的图节点更少，搜索更快。但需要额外处理的是：起点和终点（通常是赛车当前位置和目标点）可能不在任何三角形重心上。我们需要将它们“映射”到图上。方法是找到起点/终点所在的三角形，然后将其与该三角形的重心节点连接起来，作为图的入口和出口。

4. 实操过程与核心环节实现

下面，我结合代码片段和配置，详细说明核心流程的实现。

4.1 环境搭建与依赖库选择

我们整个软件栈基于ROS (Robot Operating System)，这是机器人领域的标准中间件。主要依赖库：

• CGAL (Computational Geometry Algorithms Library)：这是处理Delaunay三角剖分的工业级标准库。它稳定、高效，直接提供了约束Delaunay三角剖分（CDT）的实现。虽然有一定学习曲线，但为了可靠性，强烈建议使用它而非自己从头实现。
• Eigen：用于所有矩阵和几何变换运算。
• nav_msgs / geometry_msgs：ROS标准消息类型，用于发布路径、点云等。

在CMakeLists.txt中，你需要正确链接这些库。使用CGAL时，要注意它通常依赖Boost和GMP/MPFR库。

find_package(CGAL REQUIRED) find_package(Eigen3 REQUIRED) find_package(roscpp REQUIRED) find_package(nav_msgs REQUIRED) include(${CGAL_USE_FILE}) target_link_libraries(your_node ${CGAL_LIBRARIES} Eigen3::Eigen roscpp nav_msgs)

4.2 约束Delaunay三角剖分（CDT）的实现步骤

假设我们已经有了两个向量std::vector<Point> left_boundary和std::vector<Point> right_boundary，分别存储左右侧锥桶的二维坐标（x, y）。

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h> #include <CGAL/Constrained_Delaunay_triangulation_2.h> #include <CGAL/Triangulation_vertex_base_with_info_2.h> // 用于给顶点附加信息（如是否为左侧锥桶） typedef CGAL::Exact_predicates_inexact_constructions_kernel K; typedef CGAL::Triangulation_vertex_base_with_info_2<int, K> Vb; // 信息：0-左侧，1-右侧，2-其他 typedef CGAL::Constrained_triangulation_face_base_2<K> Fb; typedef CGAL::Triangulation_data_structure_2<Vb, Fb> Tds; typedef CGAL::Constrained_Delaunay_triangulation_2<K, Tds> CDT; typedef CDT::Point Point; CDT cdt; // 1. 插入所有顶点，并记录其索引和类型 std::map<CDT::Vertex_handle, int> vertex_type_map; for (const auto& p : left_boundary) { auto vh = cdt.insert(Point(p.x, p.y)); vh->info() = 0; // 标记为左侧边界点 vertex_type_map[vh] = 0; } for (const auto& p : right_boundary) { auto vh = cdt.insert(Point(p.x, p.y)); vh->info() = 1; // 标记为右侧边界点 vertex_type_map[vh] = 1; } // 2. 添加约束边：连接相邻的同侧锥桶，形成赛道边界 for (size_t i = 0; i < left_boundary.size() - 1; ++i) { // 需要根据vertex_type_map找到对应点的Vertex_handle，这里简化为逻辑示意 // 实际中需要维护从Point到Vertex_handle的映射 cdt.insert_constraint(point_to_vh[left_boundary[i]], point_to_vh[left_boundary[i+1]]); } // 同样处理右侧边界...

4.3 构建拓扑图与路径搜索

三角剖分完成后，我们遍历所有三角形，以其重心为图节点建图。

#include <boost/graph/adjacency_list.hpp> #include <boost/graph/astar_search.hpp> typedef boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, double>> Graph; typedef boost::graph_traits<Graph>::vertex_descriptor Vertex; typedef std::pair<int, int> Edge; Graph g; std::vector<Point> node_positions; // 存储每个图节点（三角形重心）的坐标 // 遍历CDT的所有有限面（三角形） for (CDT::Finite_faces_iterator fit = cdt.finite_faces_begin(); fit != cdt.finite_faces_end(); ++fit) { // 计算三角形重心 Point center = CGAL::centroid(fit->vertex(0)->point(), fit->vertex(1)->point(), fit->vertex(2)->point()); node_positions.push_back(center); // 当前三角形的索引就是图节点的索引 } // 添加边：遍历三角形的每条边 for (CDT::Finite_faces_iterator fit = cdt.finite_faces_begin(); fit != cdt.finite_faces_end(); ++fit) { for (int i = 0; i < 3; ++i) { CDT::Edge e = std::make_pair(fit, i); // 检查该边是否为约束边（即赛道边界），如果是，则不能通行，不添加为图边 if (!cdt.is_constrained(e)) { CDT::Face_handle neighbor = fit->neighbor(i); if (cdt.is_infinite(neighbor)) continue; // 忽略无限远面 // 获取当前三角形和邻居三角形在图中的索引（需要自己维护face到graph vertex index的映射） int u = face_to_vertex_index[fit]; int v = face_to_vertex_index[neighbor]; double distance = std::sqrt(CGAL::squared_distance(node_positions[u], node_positions[v])); boost::add_edge(u, v, distance, g); } } } // 使用A*搜索路径 // 需要实现一个启发式函数，例如到终点的欧氏距离 struct Heuristic : public boost::astar_heuristic<Graph, double> { Point goal; const std::vector<Point>& positions; Heuristic(Point g, const std::vector<Point>& pos) : goal(g), positions(pos) {} double operator()(Vertex u) const { return std::sqrt(CGAL::squared_distance(positions[u], goal)); } }; // ... 调用A*搜索算法，获取从起点节点到终点节点的顶点索引序列

4.4 路径后处理与平滑

A*搜索出来的路径是一系列三角形重心的连线，是折线，不满足车辆控制的要求。必须进行平滑。我们采用了梯度下降平滑和二次规划（QP）结合的方法：

1. 梯度下降平滑：将路径点向“空旷”区域移动，同时保持彼此接近。定义一个成本函数，包含两项：与原始点的距离（保持形状）、与障碍物的距离（保证安全）。通过迭代优化使总成本最小。
2. 二次规划平滑：将平滑问题形式化为一个二次优化问题。优化变量是路径点的坐标。目标函数最小化路径点的曲率变化（使路径平滑）和与参考点的偏差，约束条件包括路径点必须在赛道边界内、满足最大曲率约束（对应车辆最小转弯半径）。我们使用了OSQP求解器，它在嵌入式设备上也能高效运行。

平滑后的路径，再通过一个均匀采样或弧长参数化，最终生成一系列等间距或等时距的路径点（nav_msgs::Path），发送给下游的轨迹跟踪控制器。

5. 常见问题与排查技巧实录

在实际测试和比赛中，我们遇到了无数问题。下面这个表格总结了一些典型问题及其解决方案：

一些独家心得：

• 可视化是你的最佳朋友：在ROS中，充分利用RViz可视化每一个中间结果：原始点云、分类后的左右边界、三角网格（用visualization_msgs::Marker画线）、图节点和边、搜索出的粗路径、平滑后的精路径。90%的问题通过看图就能定位。
• 参数化与配置文件：所有阈值（聚类距离、三角化边界、图搜索启发式权重、平滑参数）都必须做成ROS参数，可以在线动态调整。在试车时，准备一个参数调整界面（如rqt_reconfigure）至关重要。
• 记录与回放：务必用rosbag记录所有传感器数据和中间规划结果。在实验室里回放真实赛道数据，是复现和修复赛道特定弯道问题的唯一方法。
• 关注计算耗时：使用ros::Time或std::chrono对每个模块（预处理、三角剖分、建图、搜索、平滑）进行打点计时。在资源紧张的工控机上，你需要精确知道瓶颈在哪里。我们曾发现，80%的时间花在了一个未经优化的几何距离计算循环上。

这个项目让我深刻体会到，将优雅的几何算法应用于狂野的真实世界，是一场充满妥协和打磨的工程之旅。Delaunay三角剖分提供了坚实的骨架，但让赛车流畅跑起来的，是无数针对细节的调整和对边界情况的处理。每一次测试，赛车在赛道上画出的那条平滑曲线，都是对算法鲁棒性和工程完整性的最好证明。