1. 项目概述从赛道到路径的智能转换在大学生方程式无人驾驶赛车的世界里路径规划是连接感知与控制的“大脑”。想象一下你驾驶一辆赛车眼前是一条由锥桶标记的复杂赛道你的任务不是简单地沿着锥桶走而是规划出一条能让你最快、最稳冲过终点的理想路线。这就是我们项目的核心为无人赛车在动态、未知的赛道上实时找出一条最优行驶路径。我参与过多次相关赛事的软件系统开发深知其中挑战——赛道边界模糊、计算资源有限、实时性要求极高。传统的基于网格或采样的方法在弯道密集的赛道上要么精度不够要么计算负担太重。这时Delaunay三角剖分技术进入了我们的视野。它听起来像是个复杂的数学工具但用赛车手的语言来说它就像一个极其聪明的“赛道理解器”。它能把散乱分布的锥桶赛道边界点瞬间连接成一个没有重叠、没有狭长三角形的三角网这个网完美地勾勒出了赛道的“骨架”和“可通行区域”。基于这个三角网我们就能像在迷宫中寻找最短路径一样为赛车规划出平滑、安全且逼近理论最优的行驶线。这个项目不仅仅是算法的堆砌更是一场对实时性、鲁棒性和工程实践能力的极限考验。接下来我将拆解我们如何将Delaunay三角剖分这一几何工具转化为赛车在赛道上飞驰的智能决策核心。2. 核心思路与方案选型为什么是Delaunay在无人驾驶领域路径规划算法众多如A*、Dijkstra、RRT*等。但在大学生方程式无人车这个特定场景下我们需要进行严格的取舍。赛道由离散的锥桶点定义环境结构化程度相对较高但实时感知的数据是稀疏且带噪声的。我们的核心需求可以归结为三点实时性规划周期需小于50ms、平滑性生成的路径需满足车辆动力学约束避免急转、对未知区域的探索能力能处理部分赛道尚未被完全感知的情况。2.1 传统方法的局限性我们最初尝试过栅格法将赛道区域划分为细密的网格。但问题立刻显现为了精度网格必须足够小这导致状态空间爆炸搜索耗时无法满足实时性。况且锥桶间的空隙赛道宽度是变化的固定大小的网格无法优雅适配。我们也考虑过直接使用样条曲线拟合锥桶中心线但这方法过于“脆弱”——一旦某个锥桶识别错误或丢失整个拟合曲线就会畸变且难以处理岔道或临时障碍。2.2 Delaunay三角剖分的优势Delaunay三角剖分脱颖而出正是因为它完美地匹配了我们的问题特征从点到面构建通行空间输入是一堆无序的锥桶坐标点赛道边界点Delaunay算法能输出一个三角网格。这个网格的妙处在于其三角形的外接圆内不包含任何其他输入点空圆特性这保证了生成的三角形尽可能“胖”避免出现尖锐的角。在赛道语境下这些“胖”三角形更符合车辆的实际可通行区域。天然的路网图三角网格的边和顶点自然构成了一个图Graph。每个三角形重心或边中点可以作为图的节点三角形之间的邻接关系构成了图的边。这样路径规划问题就转化为了在这个图上搜索从起点到目标点的最优路径问题可以直接应用高效的图搜索算法如A*。对噪声和不完整数据的鲁棒性即使个别锥桶漏检或位置有偏差Delaunay三角剖分生成的网格整体结构依然是稳定的不会像直接拟合那样全局崩溃。这对于依赖视觉/激光雷达、难免有感知噪声的赛车环境至关重要。计算效率与可扩展性存在成熟的增量式Delaunay三角剖分算法如Bowyer-Watson算法可以在新车感知到新的锥桶点时高效地更新现有三角网格而不是推倒重算。这为实时在线规划奠定了基础。基于以上考量我们确定了“感知点云 - Delaunay三角剖分 - 构建拓扑图 - 图搜索生成粗路径 - 路径平滑优化”的技术路线。这个方案在理论优雅性和工程可行性之间取得了最佳平衡。3. 核心细节解析与实操要点将理论落地需要攻克一系列工程细节。这里分享几个最关键环节的实现要点和踩过的坑。3.1 感知数据的预处理与清洗来自激光雷达或摄像头的原始锥桶点云是不能直接扔给Delaunay算法的。我们的预处理流水线包括聚类与过滤使用欧几里得聚类如DBSCAN将离散的点云聚合成一个个独立的锥桶对象。这里的关键是设置合适的距离阈值。太大会把不同行的锥桶聚到一起太小则会把一个锥桶因噪声产生的多个点分成不同类。我们的经验值是0.3米到0.5米具体需根据传感器噪声水平调整。地面点移除与高度过滤赛道路面是平坦的但传感器会扫到地面反射点。通过简单的平面拟合或设置Z轴高度阈值可以滤除大部分地面噪声。只保留高度在锥桶典型范围内的点。赛道边界分类可选但推荐如果能够区分左右侧的锥桶例如通过颜色识别或基于赛车位姿的简单逻辑判断将为后续规划带来巨大好处。我们可以构建“左边界点集”和“右边界点集”分别进行三角剖分或者将其合并但打上标签用于在生成图时定义可行区域。注意预处理阶段的参数需要在实际赛场上进行大量调试。静态调试的结果和赛车高速动态运行下的结果可能差异很大。务必录制真实赛道数据包进行回放测试。3.2 Delaunay三角剖分的具体实现与约束我们选择了Bowyer-Watson增量算法因为它适合在线更新。实现时有几个陷阱超级三角形的设置算法需要一个能包含所有点的大三角形作为初始。这个三角形的尺寸要足够大确保所有点都在其内部但也不能太大否则在浮点数计算中可能引入精度问题。通常取点集坐标最大最小值外扩一个比例如20%。局部重三角化的效率当插入新点时需要删除所有外接圆包含该点的三角形形成一个“空洞”然后在空洞内重新三角化。这里的核心是高效地找到受影响的三角形。维护一个良好的邻接关系数据结构至关重要。引入赛道约束标准的Delaunay三角剖分会在所有点之间连边这可能导致三角形穿过赛道即连接了左右边界的点。我们必须将其转化为约束Delaunay三角剖分。我们需要预先定义一组“约束边”例如所有相邻的、同侧的锥桶之间的连线。算法会保证这些约束边出现在最终的三角网格中。这样赛道边界就被强制固定下来三角网只在赛道内部和外部生成不会“穿墙”。3.3 从三角网格到拓扑路网生成三角网格后我们需要从中提取一个用于路径搜索的图。这里有两种主流思路以三角形重心为节点将每个三角形的重心作为图节点。如果两个三角形共享一条边且这条边不是赛道约束边即可通行则在这两个三角形的重心节点间建立一条边边的权重可以是两重心间的欧氏距离。以三角形边的中点为节点将每个三角形非约束边的中点作为图节点。节点之间的连接关系由三角形的邻接关系决定。这种方法生成的路径点更多可能更平滑。我们选择了第一种方法因为它生成的图节点更少搜索更快。但需要额外处理的是起点和终点通常是赛车当前位置和目标点可能不在任何三角形重心上。我们需要将它们“映射”到图上。方法是找到起点/终点所在的三角形然后将其与该三角形的重心节点连接起来作为图的入口和出口。4. 实操过程与核心环节实现下面我结合代码片段和配置详细说明核心流程的实现。4.1 环境搭建与依赖库选择我们整个软件栈基于ROS (Robot Operating System)这是机器人领域的标准中间件。主要依赖库CGAL (Computational Geometry Algorithms Library)这是处理Delaunay三角剖分的工业级标准库。它稳定、高效直接提供了约束Delaunay三角剖分CDT的实现。虽然有一定学习曲线但为了可靠性强烈建议使用它而非自己从头实现。Eigen用于所有矩阵和几何变换运算。nav_msgs / geometry_msgsROS标准消息类型用于发布路径、点云等。在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::vectorPoint left_boundary和std::vectorPoint 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_2int, K Vb; // 信息0-左侧1-右侧2-其他 typedef CGAL::Constrained_triangulation_face_base_2K Fb; typedef CGAL::Triangulation_data_structure_2Vb, Fb Tds; typedef CGAL::Constrained_Delaunay_triangulation_2K, Tds CDT; typedef CDT::Point Point; CDT cdt; // 1. 插入所有顶点并记录其索引和类型 std::mapCDT::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[i1]]); } // 同样处理右侧边界...4.3 构建拓扑图与路径搜索三角剖分完成后我们遍历所有三角形以其重心为图节点建图。#include boost/graph/adjacency_list.hpp #include boost/graph/astar_search.hpp typedef boost::adjacency_listboost::listS, boost::vecS, boost::undirectedS, boost::no_property, boost::propertyboost::edge_weight_t, double Graph; typedef boost::graph_traitsGraph::vertex_descriptor Vertex; typedef std::pairint, int Edge; Graph g; std::vectorPoint 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_heuristicGraph, double { Point goal; const std::vectorPoint positions; Heuristic(Point g, const std::vectorPoint pos) : goal(g), positions(pos) {} double operator()(Vertex u) const { return std::sqrt(CGAL::squared_distance(positions[u], goal)); } }; // ... 调用A*搜索算法获取从起点节点到终点节点的顶点索引序列4.4 路径后处理与平滑A*搜索出来的路径是一系列三角形重心的连线是折线不满足车辆控制的要求。必须进行平滑。我们采用了梯度下降平滑和二次规划QP结合的方法梯度下降平滑将路径点向“空旷”区域移动同时保持彼此接近。定义一个成本函数包含两项与原始点的距离保持形状、与障碍物的距离保证安全。通过迭代优化使总成本最小。二次规划平滑将平滑问题形式化为一个二次优化问题。优化变量是路径点的坐标。目标函数最小化路径点的曲率变化使路径平滑和与参考点的偏差约束条件包括路径点必须在赛道边界内、满足最大曲率约束对应车辆最小转弯半径。我们使用了OSQP求解器它在嵌入式设备上也能高效运行。平滑后的路径再通过一个均匀采样或弧长参数化最终生成一系列等间距或等时距的路径点nav_msgs::Path发送给下游的轨迹跟踪控制器。5. 常见问题与排查技巧实录在实际测试和比赛中我们遇到了无数问题。下面这个表格总结了一些典型问题及其解决方案问题现象可能原因排查步骤与解决方案规划路径突然“穿墙”出界1. 约束边未正确添加。2. 感知分类错误左右边界混淆。3. 三角网格中存在极其狭长的三角形重心落在赛道外。1.可视化调试将三角网格、约束边、原始点云在同一坐标系下用RViz可视化。检查约束边是否完整构成了赛道闭环。2.检查分类逻辑回放数据查看每个锥桶的分类标签是否正确。可以临时将左右边界点用不同颜色发布直观判断。3.后处理过滤在将三角形重心加入图之前判断其是否在赛道多边形内使用射线法。规划延迟高超过100ms1. 点云数量过多三角剖分耗时。2. 图搜索节点过多A*搜索慢。3. 平滑优化求解器迭代次数过多。1.点云降采样在预处理阶段对锥桶点云进行体素滤波或按距离采样减少点数。2.图剪枝只保留赛车前方一定距离如50米内的三角网格来建图。使用“滑动窗口”更新。3.简化平滑在高速直道段可以降低平滑的优化精度或频率。或者使用计算量更小的插值平滑如贝塞尔曲线作为备选。在急弯处路径曲率不连续车辆抖动1. 原始路径点三角形重心分布不均匀在弯道处过于稀疏。2. 平滑算法的曲率约束权重设置不当。3. 路径点间距过大控制器无法跟踪。1.密度自适应三角剖分在曲率大的区域通过历史路径或锥桶密度判断插入虚拟点使三角网格更密。2.调整QP权重增加曲率变化项的权重强制生成更平滑的路径。同时可以加入“曲率导数”约束使曲率变化也平滑。3.路径重采样平滑后以固定弧长如0.1米对路径进行重采样确保送给控制器的点足够密。遇到临时静态障碍如误入赛道的锥桶无法绕行原始设计未考虑动态障碍物。1.障碍物膨胀将障碍物点云加入三角剖分的点集并围绕障碍物添加一圈“虚拟约束边”禁止三角形穿过。这相当于在图中动态挖掉一块区域。2.局部重规划在全局路径的基础上采用动态窗口法DWA或人工势场法进行局部避障。将Delaunay规划作为全局参考线。在起点/终点附近找不到有效路径起点/终点落在了三角网格之外如赛车刚好压线或出界。1.起点/终点投影将起点/终点垂直投影到最近的赛道约束边上以投影点作为搜索的起点/终点。2.松弛约束在搜索开始时临时允许起点/终点所在三角形与外界连接即使边被约束。找到路径后再对起点附近的一小段路径进行后处理使其回到赛道内。一些独家心得可视化是你的最佳朋友在ROS中充分利用RViz可视化每一个中间结果原始点云、分类后的左右边界、三角网格用visualization_msgs::Marker画线、图节点和边、搜索出的粗路径、平滑后的精路径。90%的问题通过看图就能定位。参数化与配置文件所有阈值聚类距离、三角化边界、图搜索启发式权重、平滑参数都必须做成ROS参数可以在线动态调整。在试车时准备一个参数调整界面如rqt_reconfigure至关重要。记录与回放务必用rosbag记录所有传感器数据和中间规划结果。在实验室里回放真实赛道数据是复现和修复赛道特定弯道问题的唯一方法。关注计算耗时使用ros::Time或std::chrono对每个模块预处理、三角剖分、建图、搜索、平滑进行打点计时。在资源紧张的工控机上你需要精确知道瓶颈在哪里。我们曾发现80%的时间花在了一个未经优化的几何距离计算循环上。这个项目让我深刻体会到将优雅的几何算法应用于狂野的真实世界是一场充满妥协和打磨的工程之旅。Delaunay三角剖分提供了坚实的骨架但让赛车流畅跑起来的是无数针对细节的调整和对边界情况的处理。每一次测试赛车在赛道上画出的那条平滑曲线都是对算法鲁棒性和工程完整性的最好证明。