自动驾驶路径规划算法C++源码深度解析：从A*到Hybrid A*

简介基于C实现的自动驾驶常用路径规划算法源码包来源于实际项目内含A*、Dijkstra、RRT、RRT_Connect、RRT_Star、Bezier、B-Spline等经典与进阶算法每个模块均配有可编译运行的demo及代码注释适用于计算机、人工智能、数据科学等相关专业的课程设计、毕业设计、课程大作业或初期项目演示。资源包共36个文件包含14个cpp源码、8个h头文件、12个gif演示动图以及使用说明md和txt文档整个压缩包仅18.67MB目录按算法模块划分便于检索和针对性学习。目前已有327人学习下载代码均经过功能验证稳定可靠可直接运行体验。gif动图清晰展示了各算法在路径搜索中的实时效果配合注释能直观理解从图搜索到随机采样等不同规划思路适合作为入门进阶或二次开发的基础。1. 基于 C 的自动驾驶路径规划算法源码包拆包后的第一眼拿到一份基于 C 的自动驾驶常用路径规划算法源码包第一件事不是急着建工程、点编译而是先打开使用说明把算法模块和道路结构的对应关系梳理清楚。路径规划处在自动驾驶软件栈的中间层上游输入栅格地图、障碍物列表和自车位姿下游输出一条可被横纵向控制器跟踪的点序列。这个环节做得不扎实感知再准、底盘执行再快车照样会在原地打转。常见实现选择 C 写核心规划器原因很直接规划循环频率通常要求 10Hz 到 50Hz一次规划要在几十毫秒内完成碰撞查询和路径更新。Python 原型能验证思路但实车条件下内存拷贝和解释器开销很快就会成为瓶颈。源码包里能看到的常用算法一般绕不开 Dijkstra、A*、RRT*、DWA以及带车辆运动学约束的 Hybrid A* 或 Lattice 规划器各有各的适用场景。这套代码对两类读者最有价值一类是刚转自动驾驶规划的工程师想脱离公式看真实工程怎么组织另一类是已经跑过单个算法 Demo、但没见过项目级代码的同学。接下来按算法选型、源码实现、编译调优、场景验证的顺序把这类包里的关键结构、参数和踩坑点逐一展开。2. 路径规划算法选型A*、RRT*、Hybrid A* 与 DWA 的分层取舍在实车项目里没有哪个路径规划算法能包打全场。源码包通常会按功能拆成全局路径规划和局部路径规划两层全局层负责在高精地图或栅格地图上找到从起点到终点的粗略走廊局部层再根据当前感知结果在走廊内生成符合车辆运动学约束的平滑轨迹。选错算法的问题不会立刻暴露——低速园区里怎么跑都行一进车流密集的十字路口规划耗时、轨迹平滑度、避障反应速度的短板会被同时放大。2.1 分层决策全局规划与局部规划怎么配合全局规划回答“走哪条路”输入是高精地图 lanelet、栅格地图或从语义地图抽取的可行驶区域输出是带方向属性的途经点序列。局部规划回答“当前这一秒怎么走”输入是感知融合后的障碍物列表、预测轨迹和定位位姿输出是每隔几十毫秒刷新一次的带时间戳轨迹点。源码里这两层一般不会揉进同一个类常见做法是定义 Planner 抽象基类把全局路径和局部轨迹作为两个独立数据类型局部规划器每次开始前从全局路径中截取当前位置之后的一段作为参考线参考线长度通常设定在 20 到 50 米。看这类规划源码时我习惯先找两层之间的数据接口而不是先看某个算法的内部细节。如果接口上直接传vectorcommon::Pose说明团队对规划结果的表达还停留在直线段拼接阶段如果传的是带横向偏差和曲率约束的 FrenetFrame 数据工程化程度就明显更高下游控制模块可以少做两次坐标变换也更容易处理换道和弯道减速。2.2 网格搜索A* 与 Dijkstra 的代价函数设计A* 在网格地图上做启发式搜索代价函数 f g h 中g 是起点到当前节点的实际代价h 是对剩余距离的估计。Dijkstra 是它的特例把启发式权重设为零时自然退化。源码编写上的差别只在于是否在节点扩展循环里计算 h但工程上两者完全可以用同一套框架运行前用一个开关切换。启发式函数的选择要跟邻居扩展方式匹配四方向扩展配曼哈顿距离八方向扩展配切比雪夫距离如果地图按真实尺度建立、对角线方向的移动代价等于 √2 倍直线代价就要配欧氏距离。更常见的情况是代价地图带不同权重比如车道区域 cost 低、路肩 cost 高此时 h 可以适度放大乘 1.05 到 1.2 左右能明显减少扩展节点数代价是路径不再保证全局最优。源码里如果看到启发式权重系数被往上调得很大基本可以判断是性能调优时留下的临时改动接手后要注意还原。2.3 RRT* 与 Hybrid A*采样搜索下的车辆运动学约束RRT* 的核心差异在于重连步骤。基础 RRT 贪心地向随机采样点生长找到第一条可行路径就返回RRT* 在新节点加入后会检查周围近邻半径内的已有节点如果经由新节点到达这些节点的代价更小就改写它们的父指针让路径代价随迭代次数增加逐步逼近最优。工程实现中重点在近邻半径的计算常用公式是 r γ * sqrt(log(n) / n)其中 γ 与地图维度相关n 是当前节点数。但 RRT* 生成的是状态空间里的几何路径不保证车辆能沿路径转向。Hybrid A* 把节点的后继扩展限定为一组离散的前轮转角组合用圆弧积分推出车辆新位姿因此每个新节点天然满足最小转弯半径约束。实际泊车场景里我一般把 Reeds-Shepp 曲线作为搜索接近目标时的终止条件当某个节点与目标位姿误差小于 0.3 米和 10 度时直接用短曲线补足最后一段避免采样在狭小空间里无限膨胀。泊车路径规划算法里 Hybrid A* 出现频率很高就是因为低速场景对终点姿态要求严格靠纯栅格搜索很难在合理时间内收敛。2.4 DWA 与 Lattice局部实时避障的两条技术路线DWA动态窗口法在速度空间 (v, ω) 内做采样先根据当前速度和加速度限制生成一个动态窗口窗口内每个 (v, ω) 对应用一条预测圆弧再按障碍物距离、目标朝向、速度大小加权评分选最高分对应的速度去执行。评价权重一般写成 cost_params 结构体暴露在配置文件中便于路试时改。这里给一组可用的初值障碍物距离权重 0.25、朝向权重 0.2、速度权重 0.1后续根据路径连续性和绕障半径去调。如果项目跑在 ROS 系框架里局部规划器还可以换装 TEB 或 SMAC但评估函数和碰撞检查的结构是一样的换算法不换骨架。Lattice 规划器把轨迹按纵向位移和横向偏移采样为每个采样点求解五次多项式或一组满足终端约束的曲线族再统一做碰撞检查和代价评估。它是 Frenet 坐标系的典型应用横向和纵向维度可以独立设置速度、加速度约束车道保持和换道可以共用一套逻辑只是采样范围不同。缺点是实现量大一次规划的候选轨迹经常在 500 到 2000 条之间C 代码里计算曲线系数的部分要靠预计算表或查表法加速。算法搜索空间车辆运动学约束实时性典型用途A* / Dijkstra栅格不支持好全局寻路RRT*连续状态空间不支持中越野与结构化程度低的场景Hybrid A*连续状态加控制量支持较差泊车、园区低速DWA速度空间隐含在窗口内好局部实时避障Lattice轨迹集合支持中高速换道、弯道规划读源码时发现同一个算法出现多份实现不用觉得重复。很多项目是不同场景各自演进的结果保留旧版本就是为了回归对比删代码前先看看 config 里有没有对应的开关。3. C 源码核心拆解GridMap、碰撞检测与规划循环实现路径规划源码包的目录结构有很强的一致性。压缩包里通常会先看到 include、src、config、data、tools 这几个目录认清目录再动手比逐行读代码快得多。这一章按实际项目最常见的组织方式拆开讲方便你拿到新包后对照着看。3.1 目录与类依赖实际项目怎么组织源码include 下放所有公共头文件按模块拆成 grid_map.h、collision_checker.h、planner_base.h、astar_planner.h、rrt_star_planner.h、dwa_planner.h 等。src 下每个头文件对应一个 .cppconfig 放 yaml 或 json 参数文件data 放测试地图和录制好的场景数据tools 里一般是可视化脚本或日志转换程序。编译依赖上实际项目很少用纯标准库硬啃。常见依赖是 Eigen 做矩阵运算、yaml-cpp 读配置、OpenCV 做地图读取和可视化如果目标平台没有图像库会用自写的 PGM 解析器替代 OpenCV。这里有一个值得注意的工程细节planner_base.h 里通常会定义两个纯虚接口形如virtual bool plan(const PlanningRequest, PlanningResponse) 0;和virtual bool reset() 0;。规划失败不会抛出异常而是通过 response.status 传递错误码。看到这种设计排查“路径规划失败”时就该先看错误码而不是一头扎进算法内部打日志。3.2 GridMap 与碰撞检测两个最值得先读的类网格地图实现上惯用一维 vector 存储代价而不是二维 vector。看下面这个头文件class GridMap { public: GridMap(int rows, int cols, float resolution) : rows_(rows), cols_(cols), resolution_(resolution), data_(rows * cols, 0.0f) {} bool inBounds(int x, int y) const { return x 0 x cols_ y 0 y rows_; } float costAt(int x, int y) const { if (!inBounds(x, y)) { return kLethalCost; // 地图边界一律视为致命障碍 } return data_[y * cols_ x]; // 行优先索引一次乘加 } void setCost(int x, int y, float c) { if (inBounds(x, y)) { data_[y * cols_ x] c; } } bool isTraversable(int x, int y) const { return costAt(x, y) lethal_threshold_; } void inflateObstacle(float radius_meter); int width() const { return cols_; } int height() const { return rows_; } private: int rows_, cols_; float resolution_; // 米/像素 float lethal_threshold_; std::vectorfloat data_; };data_ 用一维数组而不是 vectorvector 原因是规划循环每秒要调用数万次 costAt二维向量的每次访问都要两次指针跳转而一维索引 y * cols_ x 只做一次乘加更重要的是整行拷贝、缓存遍历和后续做距离变换时一维布局的 cache 命中率明显更好。碰撞检测器围绕 GridMap 来做路径合法性判断。核心逻辑是沿线段步进采样步长取栅格分辨率的一半class CollisionChecker { public: explicit CollisionChecker(const GridMap map) : map_(map) {} bool isPathFree(const std::vectorEigen::Vector2d path, double footprint_radius) const { for (size_t i 1; i path.size(); i) { if (!segmentFree(path[i - 1], path[i], footprint_radius)) { return false; } } return true; } private: bool segmentFree(const Eigen::Vector2d a, const Eigen::Vector2d b, double radius) const { double dist (b - a).norm(); double step map_.resolution() * 0.5; int n static_castint(std::ceil(dist / step)); for (int k 0; k n; k) { double t dist 1e-6 ? 0.0 : double(k) / double(n); Eigen::Vector2d p a (b - a) * t; int px static_castint(p.x() / map_.resolution()); int py static_castint(p.y() / map_.resolution()); if (!map_.isTraversable(px, py)) { return false; } } return true; } };footprint_radius 参数是车辆外接圆半径的简化表达。实际项目里会用多边形包围盒做更精确的检查但核心仍然是“沿线段离散采样、逐点查询”。步长取分辨率的一半是个好习惯太粗会漏检细杆和护栏间隙太细则白白浪费 CPU。碰撞检查是规划器里的热点函数后续想提升性能优先优化这里而不是优化搜索循环。3.3 核心规划循环以 A* 的 Open 表实现为例A* 的工程实现重点在 open 表的数据结构和过时节点处理。下面是一段常见的核心循环结构struct AStarNode { int x, y; float g; float f; int parent_idx; }; bool AStarPlanner::plan(const GridMap map, const Eigen::Vector2d start, const Eigen::Vector2d goal, std::vectorEigen::Vector2d* path) { std::priority_queuestd::pairfloat, int, std::vectorstd::pairfloat, int, std::greater open; std::vectorfloat g_score(map.width() * map.height(), kInfty); std::vectorint came_from(map.width() * map.height(), -1); int sid toIndex(start); g_score[sid] 0.0f; open.emplace(heuristic(start, goal), sid); int iter 0; while (!open.empty() iter kMaxIterations) { auto [f, id] open.top(); open.pop(); if (f g_score[id] 1e-6) continue; // 过时节点直接跳过 if (isGoal(id, goal)) { reconstructPath(id, came_from, path); return true; } for (const auto nb : neighbors(id)) { float new_g g_score[id] moveCost(id, nb); if (new_g 1e-6 g_score[nb] map.isTraversable(nb.x, nb.y)) { g_score[nb] new_g; came_from[nb] id; open.emplace(new_g heuristic(nb, goal), nb); } } } return false; // 迭代上限耗尽需要降级策略 }priority_queue 配合过时节点判断是实际项目中最常用的写法复杂度对几百乘几百的栅格地图已经足够。如果地图扩大到 2000 x 2000 以上会换用分桶队列进一步降低常数。kMaxIterations 是防止异常地图导致规划卡死的保险丝这个上限必须在构造函数里和 yaml 配置对应起来。值得多留意的不是 open 表本身而是那些没写注释的分支迭代上限耗尽后返回 false任务调度层要做什么降级动作目标点本身处于膨胀区内时是报错还是退回最近可达点这些隐藏状态往往决定了一个规划器在实车上的表现也是源码包里“代码注释”最有价值的部分。读这类源码时我会先把所有 return false 的路径标记出来再对照使用说明看错误码定义比逐行读懂整个算法更快。3.4 使用说明与代码注释工程交付的两个加分项压缩包名里的“使用说明”和“代码注释”在实际交付时是分开写的。使用说明至少要覆盖三件事依赖环境怎么搭、哪些参数不能乱动、输出格式是什么。我见过不少源码包README 写了详细的编译步骤却漏了最关键的一句地图坐标系原点是车体后轴中心还是图像左上角。坐标系不一致会让规划结果在仿真里看着正常一上车就偏出车道。代码注释方面实用主义风格只注释“为什么”不注释“是什么”。比如h * 1.2; // 牺牲少量最优性换取更少的扩展节点是有价值的int i 0; // 计数器是纯噪音。接手团队代码时最怕看到一行“这里很关键”却不写关键在哪这类注释不如删掉。理想情况是算法核心处注释讲清楚约束条件参数所在处注释讲清楚单位降级分支处注释讲清楚触发前提。4. 编译运行与参数调优把规划器从 Demo 变成可交付源码包里的代码注释做得再好编译不过或者跑不出预期轨迹价值都大打折扣。这一章讲依赖搭建、命令行启动和参数调优的顺序。重点不是背命令而是理解每个参数在规划链路里的位置。4.1 先装好 vscode 的 C 环境再用 CMake 构建在 vscode 里配置 C/C 环境实质是让编辑器能找到编译器和头文件路径。Ubuntu 下一般用 gcc-11 或 clang-14Windows 下用 Visual Studio 2022 的 clmacOS 下用 clang。配好后用 CMake 组织工程一个最小可用的 CMakeLists.txt 大致如下cmake_minimum_required(VERSION 3.16) project(autopilot_planner LANGUAGES CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(Eigen3 REQUIRED) find_package(yaml-cpp REQUIRED) find_package(OpenCV QUIET COMPONENTS core imgcodecs highgui) add_executable(planner_main src/main.cpp src/grid_map.cpp src/collision_checker.cpp src/astar_planner.cpp src/rrt_star_planner.cpp src/dwa_planner.cpp ) target_include_directories(planner_main PRIVATE include) target_link_libraries(planner_main PRIVATE Eigen3::Eigen yaml-cpp OpenCV::core )find_package 拆分写的好处是缺依赖时CMake 的报错能直接指出缺哪一个而不是等编译到一半才暴露。OpenCV 用 QUIET 关键字没装也能编译只影响可视化工具。Eigen 是 header-only 库库里大量使用模板表达式编译选项最好开 Release否则 Eigen 的性能会比 Debug 慢一个数量级。构建命令如下mkdir -p build cd build cmake .. -DCMAKE_BUILD_TYPERelease cmake --build . -j$(nproc)Windows 下把最后一行的-j$(nproc)去掉用cmake --build . --config Release。构建出错时优先看两个位置一是 find_package 报错说明系统环境缺包二是模板编译错误说明 Eigen 版本和代码里某些 API 不匹配常见于用了较新的 segment 相关接口但环境里是旧版 Eigen。4.2 命令行启动与地图数据准备编译通过后先用自带测试数据跑冒烟测试。常见的数据准备流程是把网上下载的自动驾驶数据集转成灰度图或者直接用 PGM 地图。灰度图映射规则一般是0 为自由空间255 为致命障碍中间灰度作为软约束。转换时要注意像素分辨率和实际地图比例一致否则 0.1 米分辨率的图在 0.05 米分辨率配置下膨胀半径会差一倍。运行示例./planner_main \ --map ../data/garage.png \ --resolution 0.1 \ --start 2.0,3.0,0.0 \ --goal 18.0,9.0,3.14 \ --planner hybrid_astar \ --max-iterations 60000 \ --config ../config/hybrid_astar.yaml--start 和 --goal 的顺序是 x, y, thetatheta 单位是弧度。--planner 参数一般支持 astar、rrt_star、hybrid_astar、dwa 等值内部用工厂方法根据字符串创建对应实例。--max-iterations 是搜索保险丝出现“规划失败”时先把它调大再看日志。--config 指定的 yaml 文件存放权重与运动学参数不要在 main 里硬编码数值。首次运行如果地图没有加载出来先检查图像路径是否相对于 build 目录定位正确如果路径规划直接返回失败优先打印起点和终点对应的栅格 cost常见问题是起点落在膨胀后的障碍物区域内规划器一开始就找不到可行邻居。4.3 关键参数怎么调膨胀半径、启发式权重与控制频率参数之间不是独立的。调整时遵循一个固定顺序先固定运动学参数比如最小转弯半径、最大加速度再调搜索参数比如迭代次数、启发式权重最后才调代价权重比如障碍物距离、路径长度、横向偏移。下面是一组可以直接用于园区场景的初值参数推荐初值作用调大后的风险inflation_radius0.4 米障碍物扩张范围路径过于保守窄路无解heuristic_weight1.05搜索效率与最优性平衡路径明显绕远max_iterations50000搜索迭代上限内存占用上升规划变慢control_frequency20 Hz规划刷新频率计算超时丢失控制周期footprint_radius0.45 米碰撞检测边界实际车体蹭碰障碍物宏观看流程路径规划算法的迭代节奏和软件开发不太一样。每改一个参数记录场景编号、参数值和规划结果形成一张可回放的对照表。我遇到过一种典型问题是膨胀半径调大后A* 路径变长但耗时下降因为窄通道被提前判死搜索分支减少表面看性能变好实际上地图里所有 3 米以下的通道都被堵死。这就要回头对比栅格地图的可通行区域比例而不只看单条路径。5. 场景回放验证把规划回归测试变成源码包自带的技能路径规划调试和普通单元测试最大的差别在于输入输出都是高维连续量无法简单断言相等。最实用的做法是给每个场景建立一条可回放的数据记录让每次代码改动都能和基准轨迹对比。这个习惯比任何参数表都值钱。5.1 每跑一个场景存一份输入输出快照规划器每次跑完后把规划请求地图裁剪区域、起点终点、障碍物列表和规划结果路径点序列、耗时、返回码按场景号存成一块快照。格式用 CSV 就够结构类似下面这样scene_id,map,start,goal,planner,result,plan_time_ms,path scn_001,garage.png,2.0,3.0,0.0,18.0,9.0,3.14,hybrid_astar,SUCCESS,85.2,[[2.0,3.0],[2.1,3.0],...]地图本身不需要反复拷贝存一个文件名加裁剪区域即可。这样本地积累几十个场景后任何一次代码改动都可以做批量回放而不是重新开仿真。5.2 回归回放脚本固定随机种子后逐场景对比回放脚本做的事情很简单读 CSV 里的场景重新调用 planner_main再用同一份地图计算新路径与基准路径的偏差。核心逻辑可以写成一个几十行的 Python 脚本import subprocess import math import json def replay_and_compare(scene, threshold_m0.5): cmd [ ./planner_main, --map, scene[map], --start, scene[start], --goal, scene[goal], --planner, scene[planner], --config, scene.get(config, config/planner.yaml), ] r subprocess.run(cmd, capture_outputTrue, textTrue, timeout10) if r.returncode ! 0: return False, fscene {scene[id]} failed: {r.stderr[:200]} result json.loads(r.stdout) # 规划器以 json 形式输出路径 baseline json.loads(scene[path]) max_dev max( math.dist(p, q) for p, q in zip(result[path], baseline) ) return max_dev threshold_m, fmax deviation: {max_dev:.3f} m注意一点所有基于随机采样的算法RRT* 和 Hybrid A* 都在内回归对比前必须固定随机种子。否则两次规划天然不同统计出来的偏差没有意义。固定方式很简单在规划器构造函数里直接用场景 ID 派生种子例如std::mt19937 rng(static_castunsigned(std::hashstd::string()(scene_id)))保证同一个场景每次运行得到同样的采样序列。最后一件事把规划耗时、返回码写进结构化日志行和场景 CSV 放在同一层目录。下次遇到“偶尔规划失败”时用grep -l FAIL scene_*.csv过滤出全部失败场景再对失败场景单独回放。路径规划的问题几乎都是场景特异性的可复现性藏在输入与参数组合里不在海量日志里。本文还有配套的精品资源点击获取