ROS三维A星路径规划：C++实现体素地图与26邻域搜索

简介：面向机器人操作系统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::vector<int8_t>，每个元素对应一个平面栅格的占用概率。这个结构只有 width 和 height 两个轴，info 里有 resolution 和 origin，但整个消息里没有 z 轴。要做三维路径规划，第一件事是把地图从“平面栅格”升级成“体素栅格”。

常见的三种体素表示对比如下：

我做智能小车里的三维规划，一般优先选自定义三维数组。理由很朴素：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_cast<int>((wx - origin_x) / resolution); gy = static_cast<int>((wy - origin_y) / resolution); gz = static_cast<int>((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 的地图在resolution=0.1时对应 5m×5m×2m 的物理空间，对小型智能车仓库或车库场景已经够用。
• 栅格坐标系的origin一般取地图最小角点世界坐标；如果地图来自 octomap_server，消息里自带origin，直接取它，不要自己另算一个。
• 三维数组在堆上分配用std::vector<uint8_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 通常是 map；input.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算，斜向移动的空间距离更大。

对应C++实现里的偏移数组：

// neighbor_offset.h struct Offset { int dx, dy, dz; double cost; }; // 26邻域：6面 + 12边 + 8角，cost是体素边长的倍数 const std::vector<Offset> 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::vector<bool>，命中即O(1)。50×50×20的地图下这个数组只有5万元素，内存开销可忽略。

3.2 用std::priority_queue维护open list：f值排序与重复节点处理

三维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_queue<Node3D*, std::vector<Node3D*>, CompareNode>; std::vector<Node3D> nodes; // 节点池 std::vector<bool> in_closed(size_x * size_y * size_z, false); std::vector<int> 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_cast<int>(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^2+dy^2+dz^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。w>1时搜索更快但可能丢最优；w在1.0到1.5之间通常影响不大；超过2.0后路径会出现明显的“先往终点方向冲、撞到障碍再回头”的锯齿感。另外 h 和 g 的单位必须一致，调试中大约一半的路径异常来自一个乘法用米制、另一个用体素数。

4. ROS节点封装与rviz可视化：把三维A星路径发布成可验证的Marker

4.1 最小可行ROS节点结构：service接收起点终点、返回Path

三维A星作为计算密集的规划器，放进ROS里最合理的形式是 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.advertise<nav_msgs::Path>("path_3d", 1, true); marker_pub_ = nh.advertise<visualization_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::vector<Node3D> 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是否真是 map；z有变化但绕远路，多半是启发权重调得过大。

5.2 三个必调参数：启发权重、最大爬升角、障碍膨胀半径

跑通之后，真正影响三维A星落地效果的是下面三个参数，按顺序调：

最大爬升角要在邻居筛选里实现：计算当前点和邻居的 z 差与水平距离的比值，超过tan(max_pitch)就跳过该邻居。这样A星不会规划出超过小车通过能力的陡坡，比事后修路径省事得多。障碍膨胀半径建议做进地图代价值里，把障碍物周围半径 r 内的体素直接标为占用，A星内部一行不用改。

5.3 调试时最常踩的三个坑

第一个坑是坐标系混用。路径在rviz里显示正常但小车执行时跑到另一侧，几乎都是transform时用了base_link而不是odom或map。第二个坑是重复压入 open list 导致“看似卡死”，现象是内存几十MB还在涨、规划时间指数上升，处理就是前面代码里的best_g判断，同一个栅格只能以更小的 g 值再入堆。第三个坑是不检查z层越界，层数少的地图里频繁访问负下标，进程崩溃得莫名其妙。统一用一个inBounds函数收口，调试打日志也能快速定位。

本文还有配套的精品资源，点击获取