简介这是一套基于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::vectorint8_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::vectorint8_t grid, const Vec3i dims, double origin_x, double origin_y, double origin_z, double res) { pcl::PointCloudpcl::PointXYZ::Ptr cloud(new pcl::PointCloudpcl::PointXYZ); pcl::fromROSMsg(*msg, *cloud); // 先降采样避免同一个体素被多个点重复标记 pcl::VoxelGridpcl::PointXYZ vg; vg.setInputCloud(cloud); vg.setLeafSize(res, res, res); pcl::PointCloudpcl::PointXYZ::Ptr down(new pcl::PointCloudpcl::PointXYZ); vg.filter(*down); for (const auto pt : down-points) { int ix static_castint(std::floor((pt.x - origin_x) / res)); int iy static_castint(std::floor((pt.y - origin_y) / res)); int iz static_castint(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_castsize_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::vectordouble best_g(size, INFINITY)和std::vectorsize_t parent(size)搭配一维索引。查询 O(1)邻居扩展时内存连续缓存友好。地图稀疏或只规划局部路径用std::unordered_mapVec3i, ...内存占用低但每次访问多一次哈希计算。推荐第一种。三维 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_castsize_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_castint(id % static_castsize_t(dims.x)); id / static_castsize_t(dims.x); p.y static_castint(id % static_castsize_t(dims.y)); p.z static_castint(id / static_castsize_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_castuint64_t(p.x) * 73856093u ^ static_castuint64_t(p.y) * 19349663u ^ static_castuint64_t(p.z) * 83492791u; return static_castsize_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::vectorVec3i 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::vectorVec3i plan(const Vec3i start, const Vec3i goal) { std::priority_queuePlanNode, std::vectorPlanNode, PlanNodeCompare open; std::vectordouble best_g(grid_.size(), std::numeric_limitsdouble::infinity()); std::vectorsize_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::vectorVec3i reconstruct(const Vec3i goal, const std::vectorsize_t parent) { std::vectorVec3i 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 format2 nameastar_3d_planner/name version0.1.0/version description3D A* path planner in ROS/description maintainer emaildevexample.comdev/maintainer licenseMIT/license buildtool_dependcatkin/buildtool_depend dependroscpp/depend dependsensor_msgs/depend dependnav_msgs/depend dependvisualization_msgs/depend dependpcl_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_.advertisenav_msgs::Path(/astar_3d/path, 1); marker_pub_ nh_.advertisevisualization_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::vectorVec3i 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::vectorVec3i 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 必调参数与推荐取值参数默认值建议范围说明resolution0.20.05 ~ 0.5体素边长单位米。缩小一格体素数按三次方增加max_iterations10000001e5 ~ 1e7限制扩展节点数无解地图里防止死循环start_x/y/z0地图范围内起点体素索引不是米goal_x/y/z20/20/10地图范围内目标体素索引inflation_radius00 ~ 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 mz 用 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节点里只需改参数声明和发布订阅接口路径逻辑一行不用动。本文还有配套的精品资源点击获取