简介面向机器人操作系统ROS开发者与路径规划学习者这份源码工程基于C实现A星三维路径规划算法并融合JPS跳点搜索优化策略能够在三维栅格地图中为智能小车规划出可行路径。压缩包共包含74个文件整体容量约859KB核心代码由22个cpp源文件与20个h头文件构成另有8个xml配置、4个rviz可视化文件、2个launch启动脚本、若干txt和markdown说明文档目录划分清晰。工程主体划分为路径搜索、RViz显示插件、航点生成三个部分分别对应A星搜索、跳点加速、三维地图显示与路径点生成等环节覆盖从算法实现到可视化验证的完整链路。已有349人学习浏览适合希望深入阅读源码并上手实践的ROS学习者也可用于智能小车导航二次开发或课程设计参考。1. 为什么ROS智能小车要专门做三维A星路径规划二维栅格上的A星在小车平地上跑得很好可场景换成地下车库坡道、跨线桥、多层货架或带高程的野外地形基于nav_msgs::OccupancyGrid的平面规划就会把路线“钉”在地面层明明上层有空道也只能绕远路。标题里这个基于C实现的三位三维A星路径规划源码要解决的不是“给坐标加一个z”而是从栅格索引、邻居扩展、启发函数到ROS话题通信全链路一起改。这篇文章把三维A星完整拆一遍地图怎么表示、邻居怎么扩、open list怎么维护、rviz里怎么验证。新手能照着跑通第一个三维规划节点写过二维A星的老手重点看26邻域代价设计和重复节点处理。2. 三维栅格地图与坐标变换A星算法在ROS里跑起来的三个前置选择2.1 为什么nav_msgs::OccupancyGrid不够用三维体素地图的表示选型在ROS里做二维路径规划最常见的数据结构是nav_msgs::OccupancyGrid它内部是一个std::vectorint8_t每个元素对应一个平面栅格的占用概率。这个结构只有 width 和 height 两个轴info 里有 resolution 和 origin但整个消息里没有 z 轴。要做三维路径规划第一件事是把地图从“平面栅格”升级成“体素栅格”。常见的三种体素表示对比如下方案存储方式内存随分辨率增长随机访问开销ROS集成便利度自定义三维 uint8 数组线性堆叠三次方增长O(1)数组下标需自己写发布逻辑octomap_msgs::Octomap八叉树稀疏场景增长平缓O(log n)树遍历有现成rviz插件grid_map 多层高程多层栅格按层线性增长O(1)但层间要查适合2.5D坡道我做智能小车里的三维规划一般优先选自定义三维数组。理由很朴素A星扩展节点时要反复读取当前点周围26个邻居的占用状态数组下标访问是严格O(1)而 octomap 的树遍历在高频访问下会明显拖慢规划周期。如果地图规模控制在50×50×20个体素以内一个uint8_t数组只占50KB完全不用为内存牺牲速度。选型之后要处理地图坐标与栅格索引的换算。三维数组在C里常用一维存储索引公式是idx (z * size_y y) * size_x x。每个体素的世界坐标减去地图原点再除以分辨率就能算出它落在哪一层哪个格子上。这个换算关系是算法里最容易被忽略、也最容易出错的地方。2.2 C定义A星三维节点结构体、代价字段与栅格索引换算先贴一个最小可用的三维节点结构体这是整个源码里最基础的数据单元// node3d.h #pragma once #include cstdint #include vector struct Node3D { int x, y, z; // 体素栅格坐标不是世界坐标 double g; // 从起点到当前节点的实际代价 double h; // 启发式估计代价 double f; // 总代价 f g h int parent_idx; // 父节点在节点池中的索引-1表示起点 Node3D(int x_, int y_, int z_, double g_, double h_, int parent_) : x(x_), y(y_), z(z_), g(g_), h(h_), f(g_ h_), parent_idx(parent_) {} }; // 把世界坐标映射到栅格索引 inline bool worldToGrid(double wx, double wy, double wz, double origin_x, double origin_y, double origin_z, double resolution, int size_x, int size_y, int size_z, int gx, int gy, int gz) { gx static_castint((wx - origin_x) / resolution); gy static_castint((wy - origin_y) / resolution); gz static_castint((wz - origin_z) / resolution); return gx 0 gx size_x gy 0 gy size_y gz 0 gz size_z; }parent_idx存节点池索引而不是父节点的x/y/z目的是避免路径回溯时反复构造对象。g从起点累加h由启发函数算出f在构造函数里一次算好。worldToGrid返回 bool调用方在起点落在障碍物里或越界时直接报错而不是拿负下标访问数组。三个参数说明resolution越小地图越精细但体素总量按三次方膨胀。50×50×20 的地图在resolution0.1时对应 5m×5m×2m 的物理空间对小型智能车仓库或车库场景已经够用。栅格坐标系的origin一般取地图最小角点世界坐标如果地图来自 octomap_server消息里自带origin直接取它不要自己另算一个。三维数组在堆上分配用std::vectoruint8_t map(size_x * size_y * size_z, 0)值0表示空闲、100表示占用、255表示未知和nav_msgs::OccupancyGrid的约定保持一致。2.3 TF坐标转栅格索引把odom和map系下的位姿对齐到体素层在ROS里小车的位姿通常来自 map - odom - base_footprint 这条TF链而三维规划用的地图挂在 map 系下。最常见的坑是直接把 base_link 坐标当成地图坐标去查体素结果路径整体漂移。我一般先用tf2把起点和终点的位姿变换到地图坐标系再做worldToGrid// tf_to_grid.cpp 核心片段 #include tf2_geometry_msgs/tf2_geometry_msgs.h #include tf2_ros/transform_listener.h bool poseToGrid(tf2_ros::Buffer tf_buffer, const std::string target_frame, const geometry_msgs::PoseStamped input, double origin_x, double origin_y, double origin_z, double resolution, int size_x, int size_y, int size_z, int gx, int gy, int gz) { geometry_msgs::PoseStamped transformed; try { // target_frame 通常是 mapinput.frame_id_ 是 base_link 或 odom transformed tf_buffer.transform(input, target_frame, ros::Duration(0.5)); } catch (tf2::TransformException ex) { ROS_WARN_STREAM(Transform failed: ex.what()); return false; } return worldToGrid(transformed.pose.position.x, transformed.pose.position.y, transformed.pose.position.z, origin_x, origin_y, origin_z, resolution, size_x, size_y, size_z, gx, gy, gz); }调用tf_buffer.transform时输入PoseStamped的header.frame_id必须能在TF树里查到链路否则抛ExtrapolationException。第二个参数ros::Duration(0.5)是等待变换的超时时间实际小车上如果TF延迟高可以放宽到1秒代价是规划节点可能阻塞更久。把位姿先变换到 map 系再做栅格换算看起来多了两步但能省掉整个调试阶段“路径奇怪偏了半米”的排查。二维A星里这个对齐可做可不做到三维之后多了z层误差会在层间被放大必须在入口统一。3. 三维A星核心改写26邻域扩展、open list与启发函数设计3.1 从8邻域到26邻域邻居偏移量与移动代价表二维A星扩展用4邻域或8邻域三维A星自然扩展到26个6个面邻居、12个边邻居、8个角邻居。26邻域保证路径不会出现“穿墙角”的假象但也要求代价不能统一按1算斜向移动的空间距离更大。邻居类型方向数移动代价因子示例偏移实际含义面邻居61.0(1,0,0)沿坐标轴直走一格边邻居121.414(1,1,0)同一层走对角线角邻居81.732(1,1,1)跨层对角对应C实现里的偏移数组// neighbor_offset.h struct Offset { int dx, dy, dz; double cost; }; // 26邻域6面 12边 8角cost是体素边长的倍数 const std::vectorOffset kNeighbor26 { // 6 个面邻居 { 1, 0, 0, 1.0}, {-1, 0, 0, 1.0}, { 0, 1, 0, 1.0}, { 0,-1, 0, 1.0}, { 0, 0, 1, 1.0}, { 0, 0,-1, 1.0}, // 12 个边邻居 { 1, 1, 0, 1.414}, { 1,-1, 0, 1.414}, {-1, 1, 0, 1.414}, {-1,-1, 0, 1.414}, { 1, 0, 1, 1.414}, { 1, 0,-1, 1.414}, {-1, 0, 1, 1.414}, {-1, 0,-1, 1.414}, { 0, 1, 1, 1.414}, { 0, 1,-1, 1.414}, { 0,-1, 1, 1.414}, { 0,-1,-1, 1.414}, // 8 个角邻居 { 1, 1, 1, 1.732}, { 1, 1,-1, 1.732}, { 1,-1, 1, 1.732}, { 1,-1,-1, 1.732}, {-1, 1, 1, 1.732}, {-1, 1,-1, 1.732}, {-1,-1, 1, 1.732}, {-1,-1,-1, 1.732}, };代价单位是“体素边长倍数”真正累加到 g 时再乘上resolution得到米制代价。如果地图的 z 分辨率与 x/y 不一致三个方向要分别用各自分辨率折算否则规划出的路径会整体偏向更高或更矮的层。扩展时对每个邻居要检查三件事越界、障碍物、是否已在 closed list。检查 closed 一种做法是用std::unordered_set存 x/y/z 拼成的 key体素规模小也够用更快是开一个和地图同尺寸的std::vectorbool命中即O(1)。50×50×20的地图下这个数组只有5万元素内存开销可忽略。3.2 用std::priority_queue维护open listf值排序与重复节点处理三维A星性能瓶颈在两点open list的插入弹出复杂度以及重复节点的去重。std::priority_queue基于堆实现插入和弹出都是O(log n)比 vector 配合线性扫描快很多。// astar3d_core.cpp 主循环片段 #include queue #include vector struct CompareNode { bool operator()(const Node3D* a, const Node3D* b) const { return a-f b-f; // 小顶堆f值最小的节点在堆顶 } }; using OpenList std::priority_queueNode3D*, std::vectorNode3D*, CompareNode; std::vectorNode3D nodes; // 节点池 std::vectorbool in_closed(size_x * size_y * size_z, false); std::vectorint best_g(size_x * size_y * size_z, -1); // -1表示未访问过 OpenList open; nodes.reserve(size_x * size_y * size_z); // 防止扩容导致指针失效 nodes.emplace_back(sx, sy, sz, 0.0, h_start, -1); open.push(nodes.back()); while (!open.empty()) { Node3D* current open.top(); open.pop(); int cur_idx toIndex(current-x, current-y, current-z); if (in_closed[cur_idx]) continue; // 延迟删除跳过已闭合节点 in_closed[cur_idx] true; if (current-x gx current-y gy current-z gz) { break; // 回溯 parent_idx 链得到路径 } for (const auto off : kNeighbor26) { int nx current-x off.dx; int ny current-y off.dy; int nz current-z off.dz; if (!inBounds(nx, ny, nz)) continue; int nidx toIndex(nx, ny, nz); if (map[nidx] 90 || in_closed[nidx]) continue; double new_g current-g off.cost * resolution; if (best_g[nidx] 0 || new_g best_g[nidx]) { best_g[nidx] new_g; double h_new heuristic(nx, ny, nz, gx, gy, gz); nodes.emplace_back(nx, ny, nz, new_g, h_new, static_castint(current - nodes[0])); open.push(nodes.back()); } } }两个关键细节第一是best_g数组。它记录每个栅格当前找到的最小 g 值只有new_g更小才更新并压入新节点。同一个栅格可能被压入多次但堆的特性决定了较差的节点要么永远到不了堆顶要么弹出时被in_closed跳过。这是标准的延迟删除手法比查open list是否存在快得多。第二是current - nodes[0]。std::vector在push_back扩容时会搬移元素如果不提前 reserve指针会失效。所以循环开始前先nodes.reserve(size_x * size_y * size_z)把最大容量占好。g 值更新逻辑对应A星的一致性要求当网格代价满足三角不等式时节点第一次被弹出 closed 就是最优路径。26邻域的代价表基于空间距离设计天然满足但如果在图里加了地形惩罚比如陡坡邻居代价翻倍就要接受结果可能是次优。这是“性能换语义”的取舍。3.3 三维启发函数欧几里得距离与对角线距离怎么选三维栅格上启发函数两个常用选择三维欧几里得距离sqrt(dx^2dy^2dz^2)和对角线距离max (sqrt2-1)*mid (sqrt3-sqrt2)*min。前者的优点是简单、永远不大于真实代价保证最优性缺点是26邻域单位移动实际代价最低是1欧几里得估计偏小会让A星多扩展不少节点。后者精确反映26邻域最小代价扩展数量少但前提是邻居代价表必须严格对应这个公式。我一般先写成欧几里得距离跑通后再考虑换// heuristic.h inline double heuristic(int dx, int dy, int dz, double resolution) { // dx/dy/dz 是栅格距离差乘分辨率换算成米制 return std::sqrt((double)dx * dx dy * dy dz * dz) * resolution; }如果要在速度和最优性之间调参给 h 乘一个权重 w。w1时搜索更快但可能丢最优w在1.0到1.5之间通常影响不大超过2.0后路径会出现明显的“先往终点方向冲、撞到障碍再回头”的锯齿感。另外 h 和 g 的单位必须一致调试中大约一半的路径异常来自一个乘法用米制、另一个用体素数。4. ROS节点封装与rviz可视化把三维A星路径发布成可验证的Marker4.1 最小可行ROS节点结构service接收起点终点、返回Path三维A星作为计算密集的规划器放进ROS里最合理的形式是 service 或 action。两种方式的选择可以先看这张对比通信方式适用场景取消/反馈实现成本service一次查询一次响应不支持低action耗时任务、记录进度支持高以最少代码跑通我习惯先写成 service// astar3d_server.cpp 核心片段 #include ros/ros.h #include nav_msgs/Path.h #include geometry_msgs/PoseStamped.h #include path_service/PlanPath.h // 自定义srv: start/goal - path class AStar3DServer { public: explicit AStar3DServer(ros::NodeHandle nh) { server_ nh.advertiseService(plan_path_3d, AStar3DServer::onPlan, this); path_pub_ nh.advertisenav_msgs::Path(path_3d, 1, true); marker_pub_ nh.advertisevisualization_msgs::MarkerArray(path_markers, 1); } bool onPlan(path_service::PlanPath::Request req, path_service::PlanPath::Response res) { // 1. 用 tf 把 req.start 和 req.goal 变换到 map 系 // 2. worldToGrid 得到起点/终点栅格 // 3. 读取地图参数运行 AStar3D::plan // 4. 把栅格路径反算回世界坐标填入 res.path std::vectorNode3D final_path planner_.plan( req.start, req.goal, map_data_, params_); res.path.header.frame_id map; res.path.header.stamp ros::Time::now(); for (const auto n : final_path) { geometry_msgs::PoseStamped pose; pose.pose.position.x origin_x_ n.x * resolution_; pose.pose.position.y origin_y_ n.y * resolution_; pose.pose.position.z origin_z_ n.z * resolution_; pose.pose.orientation.w 1.0; res.path.poses.push_back(pose); } return true; } private: ros::ServiceServer server_; ros::Publisher path_pub_; ros::Publisher marker_pub_; // planner_、map_data_、params_ 由构造函数注入便于单测 };Service定义里注意 response 的 Path 中每个PoseStamped的frame_id要统一写map不要沿用小车的base_link。路径在rviz里显示错位九成是这个字段写错。发布path_3d时第三个参数true表示 latched晚订阅的rviz也能看到最近一次规划结果。4.2 用MarkerArray可视化体素地图和规划出来的三维路径rviz 里nav_msgs::Path只能显示线看不出三维地图的形状和障碍物分布。更直观的是用visualization_msgs::MarkerArray把路径画成粗线把起点终点画成圆球必要时把障碍物边界渲染成半透明方块。// marker_pub.cpp —— 路径用 LINE_STRIP 画成黄色线 visualization_msgs::Marker line_marker; line_marker.header.frame_id map; line_marker.ns planned_path; line_marker.id 0; line_marker.type visualization_msgs::Marker::LINE_STRIP; line_marker.action visualization_msgs::Marker::ADD; line_marker.scale.x 0.06; // 线宽单位米 line_marker.color.r 1.0; line_marker.color.g 0.8; line_marker.color.b 0.1; line_marker.color.a 1.0; for (const auto p : res.path.poses) { geometry_msgs::Point pt; pt.x p.pose.position.x; pt.y p.pose.position.y; pt.z p.pose.position.z; line_marker.points.push_back(pt); } marker_pub_.publish(line_marker);障碍物体素全画时同一帧内每个方块要放进同一个 MarkerArray 且 id 各不相同rviz 才能正常刷新否则旧方块残留路径看久了全是拖影。体素超过一万个就不建议全画改成画边界层或按高度分层染色调试效果其实更好。4.3 路径平滑与动态避障的衔接三维全局路径如何交棒给局部规划三维A星规划的原始路径在拐角处常有折线尤其斜向邻居扩展会走出45度转向直接喂给底盘会导致速度突变。常见做法是加一个平滑器对路径点做移动平均再用曲率约束剔除过密的点。theta星算法把这个思路提前到搜索阶段在扩展时穿插视线检查能显著减少拐点但它是另一套实现不要和现有A星主循环混在一起改。平滑之后全局路径要交给局部规划器执行。三维场景下局部规划器不再只是DWA那种二维速度采样还要加入爬坡速度限制和越障检查。动态避障小车路径规划里常见的架构是全局走三维A星给出走廊局部用带代价地图的DWA或TEB做实时障碍物规避两层通过costmap_3d中间层衔接。如果项目里已有混合A星Hybrid A*做带运动学约束的全局规划那三维A星更适合做它的前置通道搜索两者职责不冲突。5. 验证与调参三维A星在小车仿真里落地要过的三关5.1 在Gazebo仿真里快速验证三维路径算法写完后的第一步验证不一定要真车。在开始之前先用鱼香ROS一键安装或手动安装把 ros-desktop-full 备好确保 gazebo、rviz 和 tf2 都在。然后起一个带坡度或两层平台的gazebo仿真手动请求一次规划看路径是否符合预期# 终端1启动仿真和地图 roslaunch astar3d_demo gazebo_ramp.launch # 终端2启动规划服务 roslaunch astar3d_demo astar3d_server.launch # 终端3请求一次路径规划 rosservice call /plan_path_3d \ {start: {header: {frame_id: map}, pose: {position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}, goal: {header: {frame_id: map}, pose: {position: {x: 2.0, y: 1.0, z: 0.5}, orientation: {w: 1.0}}}}返回后rviz里添加Path和MarkerArray显示框架选map。路径完全贴地且z变化为0说明目标点的z没在worldToGrid里生效路径整体偏移优先查TF变换后的frame_id是否真是 mapz有变化但绕远路多半是启发权重调得过大。5.2 三个必调参数启发权重、最大爬升角、障碍膨胀半径跑通之后真正影响三维A星落地效果的是下面三个参数按顺序调参数推荐初始值调大后的现象调小后的现象启发权重 w1.0搜索快但路径贴障碍超过2.0出现锯齿接近最优但扩展节点多、变慢最大爬升角30度能过更陡坡但易打滑或穿模路径保守绕行距离变长障碍膨胀半径0.2m安全但窄通道被堵死路径贴墙转弯刮蹭最大爬升角要在邻居筛选里实现计算当前点和邻居的 z 差与水平距离的比值超过tan(max_pitch)就跳过该邻居。这样A星不会规划出超过小车通过能力的陡坡比事后修路径省事得多。障碍膨胀半径建议做进地图代价值里把障碍物周围半径 r 内的体素直接标为占用A星内部一行不用改。5.3 调试时最常踩的三个坑第一个坑是坐标系混用。路径在rviz里显示正常但小车执行时跑到另一侧几乎都是transform时用了base_link而不是odom或map。第二个坑是重复压入 open list 导致“看似卡死”现象是内存几十MB还在涨、规划时间指数上升处理就是前面代码里的best_g判断同一个栅格只能以更小的 g 值再入堆。第三个坑是不检查z层越界层数少的地图里频繁访问负下标进程崩溃得莫名其妙。统一用一个inBounds函数收口调试打日志也能快速定位。本文还有配套的精品资源点击获取
