简介：这是一套基于C++在ROS中实现A星三维路径规划的完整工程源码，面向机器人导航与路径规划方向的小白和进阶学习者，可直接用于毕业设计、课程设计、工程实训或初期项目立项。整个压缩包包含36个文件，以cpp源码和h头文件为主体，另有xml配置、launch启动文件、rviz可视化配置、README说明及Makefile辅助，整体大小317KB，目录组织清晰，便于按模块查看和扩展。目前已有411人学习下载。工程内设grid_path_searcher与waypoint_generator两大核心模块，支持通过catkin工作区编译并启动演示，在三维栅格环境中观察A星节点的搜索过程与规划路径。对于希望掌握A星算法在三维空间中的实现技巧、ROS节点通信机制以及可视化调试方法的读者，这套工程能提供直接的代码参考和运行范例，是快速上手的实用素材。
1. 三维 A* 的难点从来不在于把二维 A* 加个 z 轴
在二维 costmap 上写 A* 是一回事，把它搬到三维空间里就是另一回事。第一反应往往是给节点加一个z坐标，把邻居从 4/8 个改成 6/18/26 个，然后以为大功告成。真正动手写就会发现，卡住你的不止是算法本身：三维地图在 ROS 里没有统一的占用法则、体素网格的内存会按立方膨胀、用 RViz 调试一条从上方绕过障碍的路径也远比二维横切面别扭。这篇文章会把完整方案拆开讲：从sensor_msgs/PointCloud2点云生成体素地图，用 C++ 实现带启发函数的 A* 核心，再封装成 ROS 节点用MarkerArray可视化。适合已经写过二维路径规划、现在要把无人机或机械臂避障落到 ROS 上的开发者。
2. 三维栅格地图：先在 ROS 里把点云变成 A* 可搜索的体素空间
2.1 为什么不用 nav_msgs/OccupancyGrid，三维地图通常怎么做
nav_msgs/OccupancyGrid的设计目标就是二维栅格，数据是一个一维数组，索引按y * width + x展开，根本没有 z 轴。costmap_2d那一整套黏在nav_msgs/OccupancyGrid接口上的工具链，无法直接推广到三维。
常见做法有三类：
- 直接用
octomap_msgs/Octomap：八叉树结构，稀疏大图下内存表现最好，但每次查邻居都要在树里做搜索，展开 26 个邻居时缓存命中率远不如平铺数组。 - 自研平铺体素数组：把空间按固定分辨率切成
x * y * z个格子，用std::vector<int8_t>存，0 表示自由、1 表示占用、2 表示未知。这种方案代码最短，索引是 O(1)，适合几米到几十米、分辨率不低于 0.05 m 的场景。 - 深度相机视角下的 TSDF/ESDF 体素表示：用于局部视觉规划，但距离普通机器人导航太远。
我一般建议在 ROS 里把“地图维护”和“A* 搜索”拆成两个模块，规划器只接收一个很薄的体素数组接口。OctoMap 在动态地图更新上有优势，但均匀网格上的 A* 用平铺数组性能更稳，调试时也能直接把数组倒出来看障碍分布。
2.2 用 PCL VoxelGrid 处理点云，生成占用体素
输入直接用sensor_msgs/PointCloud2，这是 ROS 里和激光雷达、深度相机对接最通用的消息类型。常见错误是一收到点云就写双重循环，把每个点坐标除以分辨率再取整，然后直接写占用标记。这样做没有去噪，同一个体素内落进几十个点也会反复标记同一位置。正确做法是先过一遍pcl::VoxelGrid降采样。
#include <pcl_conversions/pcl_conversions.h> #include <pcl/point_cloud.h> #include <pcl/point_types.h> #include <pcl/filters/voxel_grid.h> void pointCloudCb(const sensor_msgs::PointCloud2::ConstPtr& msg, std::vector<int8_t>& grid, const Vec3i& dims, double origin_x, double origin_y, double origin_z, double res) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*msg, *cloud); // 先降采样，避免同一个体素被多个点重复标记 pcl::VoxelGrid<pcl::PointXYZ> vg; vg.setInputCloud(cloud); vg.setLeafSize(res, res, res); pcl::PointCloud<pcl::PointXYZ>::Ptr down(new pcl::PointCloud<pcl::PointXYZ>); vg.filter(*down); for (const auto& pt : down->points) { int ix = static_cast<int>(std::floor((pt.x - origin_x) / res)); int iy = static_cast<int>(std::floor((pt.y - origin_y) / res)); int iz = static_cast<int>(std::floor((pt.z - origin_z) / res)); if (ix < 0 || iy < 0 || iz < 0 || ix >= dims.x || iy >= dims.y || iz >= dims.z) { continue; // 超出地图范围，丢弃 } size_t id = (static_cast<size_t>(iz) * dims.y + iy) * dims.x + ix; grid[id] = 1; } }setLeafSize(res, res, res)三个参数分别是 x、y、z 方向的体素边长，室内飞行场景通常设成同一个值。降采样后同一个体素最多保留一个代表点，后续标记grid[id] = 1天然完成了去重。索引计算用std::floor而不是round，因为原点左侧的负坐标也需要稳定映射到正确的体素，round在边界半格处会产生体素漂移。
这里最容易被忽视的是坐标系。如果点云在base_link坐标系下而规划在map坐标系下，所有体素索引都会整体错位。回调里先确认msg->header.frame_id，需要时做一次 TF 变换再交给VoxelGrid。
2.3 用一维索引还是哈希表：三维坐标的状态编码
A* 搜索过程中每个体素都要查 g 值和父节点。三维空间里没有现成的数据结构同时兼顾内存和访问速度，工程上通常按体素规模做两种选择：
- 体素总量在千万量级以下：用
std::vector<double> best_g(size, INFINITY)和std::vector<size_t> parent(size)，搭配一维索引。查询 O(1)，邻居扩展时内存连续，缓存友好。 - 地图稀疏或只规划局部路径：用
std::unordered_map<Vec3i, ...>，内存占用低，但每次访问多一次哈希计算。
推荐第一种。三维 A* 真正危险的是内存而不是哈希开销。一对double + size_t大约是 16 字节，1000 万体素就是 160 MB，这个量级完全可以接受。再往上才需要考虑稀疏表示。
一维索引的编码顺序应当是 x 最内层、z 最外层。
struct Vec3i { int x, y, z; bool operator==(const Vec3i& o) const { return x == o.x && y == o.y && z == o.z; } }; inline size_t encode(const Vec3i& p, const Vec3i& dims) { return (static_cast<size_t>(p.z) * dims.y + p.y) * dims.x + p.x; } inline Vec3i decode(size_t id, const Vec3i& dims) { Vec3i p; p.x = static_cast<int>(id % static_cast<size_t>(dims.x)); id /= static_cast<size_t>(dims.x); p.y = static_cast<int>(id % static_cast<size_t>(dims.y)); p.z = static_cast<int>(id / static_cast<size_t>(dims.y)); return p; }x 做最内层维度后，遍历某个 z 截面时内存地址是连续的。如果 x、y、z 维度不是固定值，每次encode都要把dims传进来，不要图省事存成全局变量。注意dims.x * dims.y * dims.z可能超过 32 位 int，索引相关计算全部用size_t。
如果确实要用哈希表，三维坐标可以这样处理：
struct Vec3iHash { size_t operator()(const Vec3i& p) const { uint64_t h = static_cast<uint64_t>(p.x) * 73856093u ^ static_cast<uint64_t>(p.y) * 19349663u ^ static_cast<uint64_t>(p.z) * 83492791u; return static_cast<size_t>(h); } };这种“质数乘法加异或”的组合在三维坐标上冲突率很低。负坐标会被转成很大的无符号数再参与运算，结果没有问题，但调试时打印 key 要还原成int再读，否则很难跟踪。
3. 用 C++ 实现三维 A* 核心：优先队列、启发函数与路径重建
3.1 状态节点定义和 g、f 的更新策略
三维网格上的 A* 状态由坐标pos、从起点累计的路径代价g、预估总代价f = g + h三部分组成。父节点不需要存完整坐标，只存一维索引，路径重建时再用decode还原，能省一大块内存。
#include <queue> #include <vector> #include <cstdint> #include <cmath> #include <limits> struct PlanNode { Vec3i pos; double g; double f; size_t parent; // 父节点编号，起点的 parent 用 SIZE_MAX }; struct PlanNodeCompare { bool operator()(const PlanNode& a, const PlanNode& b) const { return a.f > b.f; // 小顶堆：f 越小越优先弹出 } };这里有一个 C++ 的经典坑：std::priority_queue默认是最大堆，top()返回的是比较器眼中的“最大”元素，所以比较器必须反过来写，让f小的节点排在堆顶。很多人把return a.f < b.f;抄进去，结果 A* 每次都先展开代价最大的节点。
3.2 26 邻域展开和移动代价
三维网格的邻居展开一般有三档：6 邻域只走面相邻，18 邻域加上边相邻，26 邻域再加上角相邻。无人机在无障碍约束的开放空间运动，26 邻域生成的路径更自然，也不会出现只能沿坐标轴绕行的锯齿路径。机械臂如果考虑关节空间，状态空间不是体素，那是另一套方案，这里不展开。
生成 26 个邻居偏移的方法很直接：
std::vector<Vec3i> offsets; for (int dx = -1; dx <= 1; ++dx) for (int dy = -1; dy <= 1; ++dy) for (int dz = -1; dz <= 1; ++dz) { if (dx == 0 && dy == 0 && dz == 0) continue; offsets.push_back({dx, dy, dz}); }移动代价用几何距离：面邻居为 1.0，边邻居为 √2，体对角邻居为 √3。
inline double moveCost(const Vec3i& a, const Vec3i& b) { int d = std::abs(a.x - b.x) + std::abs(a.y - b.y) + std::abs(a.z - b.z); if (d == 1) return 1.0; if (d == 2) return std::sqrt(2.0); return std::sqrt(3.0); }d == 2对应 (1,1,0) 这类边对角，d == 3对应 (1,1,1) 体对角。如果只做 6 邻域，代价恒为 1.0，但路径只能沿轴走，在三维斜向通道内会明显拉长。
注意：26 邻域的对角线移动可能斜穿障碍体素的角。只检查邻居体素
grid_[nid] == 0时，路径可能“擦着墙边”通过。面邻接只需看目标体素，边邻接建议额外检查共享边的两个相邻面体素，角邻接检查共享边的三个面体素。代价是每次展开多几次数组访问，但能避免路径贴墙穿角。
3.3 开放集实现：priority_queue 加惰性删除
A* 的开放集要反复取出 f 最小的节点，但 C++ 标准库没有“可更新优先级的堆”。常见做法是用std::priority_queue配合惰性删除：不修改堆内已有节点，重复入堆，弹出时检查当前节点是否已经过期。
std::vector<Vec3i> plan(const Vec3i& start, const Vec3i& goal) { std::priority_queue<PlanNode, std::vector<PlanNode>, PlanNodeCompare> open; std::vector<double> best_g(grid_.size(), std::numeric_limits<double>::infinity()); std::vector<size_t> parent(grid_.size(), SIZE_MAX); size_t start_id = encode(start, dims_); best_g[start_id] = 0.0; open.push({start, 0.0, heuristic(start, goal), SIZE_MAX}); int iterations = 0; while (!open.empty()) { PlanNode cur = open.top(); open.pop(); size_t cur_id = encode(cur.pos, dims_); // 过期节点：g 值比记录的最优 g 大，直接丢弃 if (cur.g > best_g[cur_id] + 1e-4) continue; if (cur.pos == goal) { return reconstruct(goal, parent); } if (++iterations > max_iterations_) { ROS_WARN("A* reached max iterations, no path found"); return {}; } for (const Vec3i& off : offsets_) { Vec3i nxt{cur.pos.x + off.x, cur.pos.y + off.y, cur.pos.z + off.z}; size_t nid = encode(nxt, dims_); if (!inGrid(nxt) || grid_[nid] != 0) continue; double ng = cur.g + moveCost(cur.pos, nxt); if (ng < best_g[nid] - 1e-6) { best_g[nid] = ng; parent[nid] = cur_id; open.push({nxt, ng, ng + heuristic(nxt, goal), cur_id}); } } } return {}; }浮点比较必须留容差。cur.g > best_g[cur_id] + 1e-4用来识别来迟的老节点；ng < best_g[nid] - 1e-6用来判断是否找到更优的 g 值。如果没有容差，两个代价理论上相等的路径可能因为舍入误差反复互相覆盖，导致开放集不断膨胀。
3.4 启发函数：三维网格里的欧几里得距离仍然可采纳
三维各向同性网格上，任意两点之间的实际最短路径代价由体对角步长 √3 决定。欧几里得距离永远不超过真实最短路径，所以是可采纳的启发函数：
inline double heuristic(const Vec3i& a, const Vec3i& b) { double dx = std::abs(a.x - b.x); double dy = std::abs(a.y - b.y); double dz = std::abs(a.z - b.z); return std::sqrt(dx * dx + dy * dy + dz * dz); }曼哈顿距离在二维 4 邻域里可采纳，但在 26 邻域三维里会高估斜向路径，不能直接用。如果只做 6 邻域，曼哈顿距离则完全正确。
要注意的是，这里启发函数的单位是体素数。发布路径时把每个体素坐标乘以resolution_转成米，启发函数本身不需要改。若换成真实物理距离，就把启发函数整体乘以resolution_，否则 A* 会过度偏向目标方向的节点，在障碍多的地图里更容易落入局部死胡同。
3.5 路径重建
搜索结束后从目标点沿 parent 链回溯到起点，再反转顺序：
std::vector<Vec3i> reconstruct(const Vec3i& goal, const std::vector<size_t>& parent) { std::vector<Vec3i> path; size_t id = encode(goal, dims_); while (id != SIZE_MAX) { path.emplace_back(decode(id, dims_)); id = parent[id]; } std::reverse(path.begin(), path.end()); return path; }如果重建出来的路径长度明显异常，先检查encode和decode里dims_的三个分量有没有传反。x、y、z 的顺序不统一，是三维 A* 里最常见的隐蔽 bug。
4. 把 A* 封装成 ROS 节点：从 catkin 参数到 RViz 可视化
4.1 包结构和 CMake 配置
一个最小可编译的 ROS 包只需要三个文件：
catkin_ws/src/astar_3d_planner/ ├── CMakeLists.txt ├── package.xml └── src └── astar_3d_node.cppCMakeLists.txt 里的依赖要覆盖点云转换、路径消息和可视化消息：
cmake_minimum_required(VERSION 3.0.2) project(astar_3d_planner) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs nav_msgs visualization_msgs pcl_ros ) catkin_package() add_executable(astar_3d_node src/astar_3d_node.cpp) add_dependencies(astar_3d_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(astar_3d_node ${catkin_LIBRARIES})pcl_ros提供pcl_conversions的头文件以及点云消息转换支持。nav_msgs/Path用于发布路径，visualization_msgs/MarkerArray用于在 RViz 中显示三维折线。机器人上如果还没有点云数据，可以先用 Gazebo 里的 velodyne 插件发布/points，节点无需改动。
package.xml 保持与 CMake 依赖一致：
<package format="2"> <name>astar_3d_planner</name> <version>0.1.0</version> <description>3D A* path planner in ROS</description> <maintainer email="dev@example.com">dev</maintainer> <license>MIT</license> <buildtool_depend>catkin</buildtool_depend> <depend>roscpp</depend> <depend>sensor_msgs</depend> <depend>nav_msgs</depend> <depend>visualization_msgs</depend> <depend>pcl_ros</depend> </package>在catkin_ws下执行catkin_make，再source devel/setup.bash就能rosrun astar_3d_planner astar_3d_node启动。
4.2 节点主循环：订阅点云、定时触发规划、发布 Path
规划核心和 ROS 回调之间要用定时器隔开，不要在点云回调节点里同步跑完整 A*，否则点云频率一高就会堆积延迟。
class AStar3DNode { public: AStar3DNode() : nh_("~") { nh_.param("resolution", resolution_, 0.2); nh_.param("max_iterations", max_iterations_, 1000000); nh_.param("start_x", start_.x, 0); nh_.param("start_y", start_.y, 0); nh_.param("start_z", start_.z, 0); nh_.param("goal_x", goal_.x, 20); nh_.param("goal_y", goal_.y, 20); nh_.param("goal_z", goal_.z, 10); cloud_sub_ = nh_.subscribe("/points", 1, &AStar3DNode::cloudCb, this); path_pub_ = nh_.advertise<nav_msgs::Path>("/astar_3d/path", 1); marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("/astar_3d/markers", 1); timer_ = nh_.createTimer(ros::Duration(1.0), &AStar3DNode::timerCb, this); } private: void timerCb(const ros::TimerEvent&) { if (grid_.empty()) return; AStar3D astar(dims_, grid_, max_iterations_); std::vector<Vec3i> path = astar.plan(start_, goal_); if (path.empty()) { ROS_WARN("No path found"); return; } publishPath(path); publishMarkers(path); } };定时器周期 1.0 秒是保守值，先保证规划完整跑完再缩减周期。start_x、goal_x这些参数的单位是体素索引，不是米；发布消息时再乘resolution_。这样设计可以避免脚本里传浮点坐标、内部又做一次取整的统一性问题，也方便构造已知答案的测试场景。
4.3 用 MarkerArray 在 RViz 里显示三维路径
路径转换成nav_msgs/Path后 RViz 的 Path 显示控件也能画，但 Marker 的 LINE_STRIP 更好用，可以自由控制线宽和颜色。
void publishMarkers(const std::vector<Vec3i>& grid_path) { visualization_msgs::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = ros::Time::now(); marker.ns = "astar_3d_path"; marker.id = 0; marker.type = visualization_msgs::Marker::LINE_STRIP; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.06; // 线宽，单位米 marker.color.a = 0.9; marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; for (const auto& p : grid_path) { geometry_msgs::Point pt; pt.x = p.x * resolution_; pt.y = p.y * resolution_; pt.z = p.z * resolution_; marker.points.push_back(pt); } visualization_msgs::MarkerArray marr; marr.markers.push_back(marker); marker_pub_.publish(marr); }RViz 里把 Fixed Frame 设为map，再添加 MarkerArray 话题astar_3d/markers，就能看到绿色折线。如果路径被遮挡，点开 MarkerArray 控件的 Unary 选项，或者把 Path 控件也加进来，两条线会互相验证。
4.4 必调参数与推荐取值
| 参数 | 默认值 | 建议范围 | 说明 |
|---|---|---|---|
resolution | 0.2 | 0.05 ~ 0.5 | 体素边长，单位米。缩小一格，体素数按三次方增加 |
max_iterations | 1000000 | 1e5 ~ 1e7 | 限制扩展节点数，无解地图里防止死循环 |
start_x/y/z | 0 | 地图范围内 | 起点体素索引，不是米 |
goal_x/y/z | 20/20/10 | 地图范围内 | 目标体素索引 |
inflation_radius | 0 | 0 ~ 0.6 | 对障碍邻域做膨胀，防止路径贴墙飞行 |
resolution是最容易失控的参数。30 m × 30 m × 20 m 的空间，0.05 m 分辨率会产生 1.44 亿个体素，单单best_g和parent两个数组就是 2.3 GB 内存。工程上先以 0.2 m 跑通，确认正确性后再往 0.1 m 收敛。
5. 三维 A* 的验证、排错与内存优化技巧
5.1 用三个已知场景验证算法正确性
没有真实点云时，先构造三类地图验证：
第一，全空地图。从 (0,0,0) 到 (20,20,20)，A* 应当输出一条没有多余转折的直线，路径体素数等于 20 个对角步长。如果路径出现“之”字形，多半是移动代价或启发函数单位不统一。第二，单一障碍块。在路径正中间放一个 5×5×5 的实心方块，检查路径是否绕行，并且不与障碍体素共享任何外表面。第三，无解地图。用一面贯穿整层的大墙堵死空间，确认节点会在max_iterations处退出并返回空路径。
启动命令可以直接传参数做验证：
rosrun astar_3d_planner astar_3d_node \ _resolution:=0.2 _start_x:=0 _start_y:=0 _start_z:=0 \ _goal_x:=10 _goal_y:=10 _goal_z:=10 rosrun rviz rviz5.2 三维 A* 特有的坑：内存、浮点和边界
三维 A* 最常见的三个问题都和维度有关。第一是 z 轴精度选择不当，很多从二维迁移的代码会把 z 方向分辨率设成和 x/y 一样，导致 30 m 范围的平坦场景浪费几十万个体素。无人机场景里通常可在 z 方向单独放大分辨率，比如 x/y 用 0.1 m，z 用 0.2 m，体素数直接减半。第二是std::floor和round混用，点云坐标落在体素边界附近时，不同代码段可能把同一点映射到两个不同格子。第三是开放集里重复节点过多，调试时打印open.size()，如果达到几十万而路径只有一个简单的绕行，优先检查相邻移动代价值是否写反。
5.3 性能优化：从展开顺序到线程分离
26 邻域展开是三维 A* 最热的内层循环。预计算 26 个偏移后，建议把偏移按照“先朝目标方向、再朝其他方向”排序，让更可能的节点先被压入堆，减少堆内无用比较。代价表可以用constexpr double写死，避免每次调用moveCost做两次开方。地图更新时用std::fill复用已有grid_数组，不要反复重新vector扩容。
如果网格很大，第一优先级是把点云处理和 A* 搜索拆到两个线程，用std::mutex保护grid_。单线程里再怎么优化 26 邻域，也比不上减少一次同步阻塞来得直接。想进一步减少展开节点数，可以看 Theta* 或者三维 JPS，前者在路径平滑上更有优势，后者在空旷场景能跳过大量直行体素。这段 A* 核心和 ROS 解耦后，拷到 ROS 2 的rclcpp节点里只需改参数声明和发布订阅接口，路径逻辑一行不用动。
本文还有配套的精品资源，点击获取
