独立 State Lattice 规划器从 Nav2 Smac Planner 解耦的纯 C 路径规划库项目链接https://github.com/Robot-Nav/standalone_lattice_planner说明本项目为从 ROSnav2_smac_planner解耦出来的独立 State Lattice 规划器当前仍处于待测试阶段。有兴趣的同学可以先拉取项目进行编译和场景测试如果发现问题也欢迎提交 issue方便后续继续完善。1. 项目简介standalone_lattice_planner是一个独立的 State Lattice 路径规划器。项目从 ROS Navigation2 中的nav2_smac_planner解耦而来保留了 State Lattice 规划器的核心算法逻辑同时去除了 ROS、Nav2、TF、消息类型和参数服务器等依赖形成了一个纯 C17 实现的路径规划库。该项目主要面向 Ackermann 车辆、小型无人车、自动驾驶底盘、移动机器人等平台能够在二维栅格地图中生成满足车辆运动学约束的可跟踪路径。与普通 A* 或 Hybrid A* 不同State Lattice Planner 不是简单地在二维栅格上进行八邻域搜索而是通过预先生成的运动基元进行状态扩展使生成的路径天然具备车辆可执行性尤其适合具有最小转弯半径约束的非完整约束车辆。图1 State Lattice Planner 整体架构图图1展示了standalone_lattice_planner的整体架构。系统首先加载地图、起终点、规划配置和运动基元然后基于 State Lattice A* 进行搜索并结合碰撞检测、双启发式和解析扩展生成初始路径最后通过路径平滑模块输出可供控制器跟踪的路径点序列。2. 项目特点该项目的核心特点可以概括为ROS-Free、State Lattice、运动基元、双启发式、解析扩展、路径平滑。2.1 纯 C17 实现项目完全采用 C17 编写不依赖 ROS 相关组件例如不依赖rclcpp不依赖nav2_core不依赖nav2_costmap_2d不依赖tf2不依赖geometry_msgs不依赖 ROS 参数服务器因此该规划器可以直接集成到普通 C 工程、嵌入式工程、仿真系统、自动驾驶平台或自研机器人系统中。2.2 State Lattice A* 搜索项目采用 State Lattice A* 作为核心搜索框架。普通 A* 一般只考虑二维平面位置(x, y)而 State Lattice 会将车辆状态扩展为(x, y, theta)其中x表示车辆在地图中的横向位置y表示车辆在地图中的纵向位置theta表示车辆航向角。项目中默认采用 72 个方向进行航向角量化也就是将 360° 离散为 72 个方向每个方向间隔约 5°。这种方式可以让规划器在搜索过程中同时考虑车辆位置和朝向从而生成更加符合车辆运动学约束的路径。图2 State Lattice A* 状态搜索原理图图2展示了 State Lattice A* 的状态扩展过程。与普通二维 A* 不同State Lattice 的节点不仅包含位置坐标还包含车辆航向角即(x, y, theta)。每次扩展时规划器会根据当前航向角选择对应的运动基元生成满足车辆运动学约束的候选子节点再通过碰撞检测和代价计算筛选可行节点。2.3 Ackermann 运动基元该项目支持基于 Ackermann 车辆模型的离散运动基元。运动基元可以理解为车辆在某个状态下允许执行的一组短距离运动动作例如向前直行向前左转向前右转小曲率转弯大曲率转弯可选的倒车扩展。与普通栅格搜索相比基于运动基元的搜索结果更加平滑也更容易被后续的路径跟踪控制器执行。项目中的运动基元通过 JSON 文件加载例如examples/data/lattice/output.json这样可以方便地根据不同车辆底盘参数、最小转弯半径和运动约束生成不同的运动基元文件。图3 Ackermann 运动基元扩展示意图图3展示了 Ackermann 车辆在 State Lattice 中的运动基元扩展方式。每个运动基元都可以看作车辆在短时间或短距离内能够执行的一段可行运动。由于这些基元提前考虑了车辆最小转弯半径和非完整约束因此搜索得到的路径比普通栅格路径更适合车辆底盘直接跟踪。2.4 双启发式搜索为了提升搜索效率项目使用了双启发式机制2D 障碍物波前启发式SE2 距离启发式其中2D 障碍物波前启发式类似于在二维代价地图上进行 Dijkstra 或 BFS 搜索可以考虑障碍物对路径代价的影响。SE2 距离启发式则考虑车辆在平面中的位姿状态即SE2 (x, y, theta)项目通过 Dubins 或 Reeds-Shepp 距离估计车辆从当前位姿到目标位姿的运动学可达代价从而提升 A* 搜索的方向性。这种双启发式设计兼顾了障碍物环境和车辆运动学约束比单纯的欧式距离启发式更加适合车辆路径规划。图4 双启发式代价融合原理图图4展示了规划器中的双启发式机制。2D 障碍物启发式主要用于感知地图中障碍物对路径的影响而 SE2 运动学启发式则用于估计车辆从当前位姿到目标位姿的可达代价。二者结合后可以同时提高搜索效率和路径可执行性。A* 的总代价一般可以写成f(n) g(n) h(n)其中g(n)表示从起点到当前节点的累计代价h(n)表示从当前节点到目标节点的启发式估计代价f(n)表示用于节点排序的总代价。2.5 Dubins / Reeds-Shepp 解析扩展项目支持周期性尝试从当前搜索节点直接连接到目标点这一过程称为解析扩展。支持的连接方式包括Dubins 曲线Reeds-Shepp 曲线。其中Dubins 曲线适用于只能前进的车辆Reeds-Shepp 曲线适用于支持前进和倒车的车辆。解析扩展的作用是减少不必要的搜索节点加快规划速度并在接近目标时生成更加连续、可执行的路径。图5 Dubins / Reeds-Shepp 解析扩展示意图图5展示了解析扩展的基本思想。规划器并不是一直通过离散运动基元一步一步搜索到目标点而是在搜索过程中周期性尝试使用 Dubins 或 Reeds-Shepp 曲线直接连接目标。当该连接路径满足碰撞检测和运动学约束时就可以提前结束搜索提高规划效率。2.6 多边形足迹碰撞检测项目支持车辆多边形足迹碰撞检测。相比简单的圆形碰撞模型多边形足迹能够更准确地描述车辆底盘外形例如矩形小车、Ackermann 车辆、清洁机器人底盘等。项目会预先计算不同航向角下的足迹形状默认支持 72 个旋转角度从而减少规划过程中的重复计算提高碰撞检测效率。配置示例footprint:[[0.1,0.1],[-0.1,0.1],[-0.1,-0.1],[0.1,-0.1]],footprint_is_radius:false如果只想使用圆形足迹也可以将footprint_is_radius设置为true。图6 多边形足迹碰撞检测流程图图6展示了多边形足迹碰撞检测流程。规划器会根据当前航向角选择预计算好的旋转足迹并将其转换到地图坐标系下随后遍历足迹覆盖的栅格。如果足迹区域超出地图边界或者与障碍物栅格发生重叠则该候选节点会被直接丢弃。2.7 共轭梯度路径平滑State Lattice 搜索得到的是一条离散路径虽然已经满足运动基元约束但仍然可能存在局部锯齿或不够平滑的问题。因此项目内置了路径平滑模块采用共轭梯度法对搜索结果进行优化。路径平滑主要目标包括减少路径局部抖动提高路径连续性保持起点和终点约束尽量避免路径靠近障碍物提高路径跟踪控制器的可执行性。如果不需要平滑可以通过命令行参数关闭--no-smooth图7 共轭梯度路径平滑示意图图7展示了搜索原始路径和平滑后路径的区别。原始路径可能受离散栅格和运动基元影响存在局部折线和曲率变化平滑后路径更加连续更适合后端 Pure Pursuit、Stanley、MPC、MPPI 等控制器进行跟踪。3. 项目依赖3.1 必需依赖依赖版本要求作用C 编译器C17编译项目CMake≥ 3.14构建系统OMPL≥ 1.5Dubins / Reeds-Shepp / SE2 状态空间Eigen3≥ 3.3数值计算依赖nlohmann/json≥ 3.0JSON 配置与运动基元解析3.2 可选依赖依赖作用GoogleTest单元测试项目中还内置了thirdparty/robin_hood.h用于高性能哈希表。4. 安装依赖4.1 Ubuntu / Debian 安装sudoaptinstalllibompl-devsudoaptinstalllibeigen3-devsudoaptinstallnlohmann-json3-devsudoaptinstalllibgtest-devsudoaptinstallcmake4.2 Conda 安装如果系统环境中的 OMPL 版本不合适也可以使用 Conda 安装依赖condainstall-cconda-forge ompl eigen nlohmann_json gtest cmake4.3 源码安装 OMPLgitclone https://github.com/ompl/ompl.gitcdomplmkdirbuildcdbuild cmake..make-j$(nproc)sudomakeinstall5. 编译项目进入项目目录cdstandalone_lattice_plannermkdirbuildcdbuild cmake..make-j$(nproc)如果 OMPL 安装在非标准路径例如 Conda 环境可以指定 OMPL 路径cmake..-DOMPL_ROOT/home/user/miniconda3Release 模式编译cmake..-DCMAKE_BUILD_TYPEReleasemake-j$(nproc)Debug 模式编译cmake..-DCMAKE_BUILD_TYPEDebugmake-j$(nproc)关闭测试编译cmake..-DBUILD_TESTINGOFFmake-j$(nproc)编译完成后主要产物如下文件说明liblattice_planner.so规划器共享库lattice_planner_example命令行示例程序test_*单元测试可执行文件6. 快速运行示例项目提供了一个命令行示例程序可以直接加载运动基元和配置文件进行路径规划。cdbuild ./lattice_planner_example\--lattice../examples/data/lattice/output.json\--config../examples/data/default_config.json\--start1.0,1.0,0.0\--goal5.0,5.0,1.57运行后会输出规划信息例如[INFO] No map specified, generating test map (200x200, 0.05m/cell) [INFO] Map size: 200x200, resolution0.05m [INFO] Loading config from: ../examples/data/default_config.json [INFO] Lattice file: ../examples/data/lattice/output.json [INFO] Start: (1, 1, 0) [INFO] Goal: (5, 5, 1.57) [INFO] Planning... [INFO] Planning completed in 2.09 ms [INFO] Path summary: 176 points, total length 9.99696 m输出路径点格式大致如下[0] x1.050000, y1.050000, theta0.000000 [1] x1.099976, y1.050043, theta0.002510 ...可以看到最终输出的是路径点序列每个路径点包含x, y, theta这类输出非常适合作为后续路径跟踪控制器的参考路径例如 Pure Pursuit、Stanley、MPC、MPPI 等控制器。7. 加载 PGM 地图规划除了默认生成测试地图外项目也支持加载 PGM 地图文件./lattice_planner_example\--map/path/to/map.pgm\--resolution0.05\--origin0,0\--lattice../examples/data/lattice/output.json\--start1.0,1.0,0.0\--goal10.0,10.0,1.57\--output/path/to/output.csv其中--map指定 PGM 地图路径--resolution指定地图分辨率--origin指定地图原点--start指定起点位姿--goal指定终点位姿--output指定 CSV 输出文件。项目支持 PGM 的 P5 和 P2 格式。8. 路径输出8.1 输出 CSV./lattice_planner_example...--outputpath.csvCSV 文件中一般保存路径点序列便于后续用 Python、MATLAB、C 或控制器模块读取。8.2 输出 JSON./lattice_planner_example... --output-json path.jsonJSON 格式更适合在工程项目中保存完整路径数据方便与其他模块进行数据交互。9. 常用命令行参数参数格式说明--mappathPGM 地图文件--startx,y,theta起点位姿--goalx,y,theta目标位姿--latticepath运动基元 JSON 文件--configpath配置 JSON 文件--outputpath输出 CSV 路径--output-jsonpath输出 JSON 路径--resolutionr地图分辨率--originx,y地图原点--no-smoothflag禁用路径平滑--helpflag查看帮助10. C 编程接口除了命令行使用该项目也可以作为 C 库集成到自己的工程中。图8 LatticePlanner 完整运行流程图图8展示了standalone_lattice_planner的完整运行流程。用户只需要提供地图、配置文件、运动基元、起点和终点规划器便可以完成可行性检查、State Lattice A* 搜索、解析扩展、路径平滑和最终路径输出。整个过程不依赖 ROS可以直接嵌入普通 C 工程中。一个最小使用示例如下#includelattice_planner/lattice_planner.hpp#includelattice_planner/io/map_loader.hppusingnamespacelattice_planner;intmain(){// 1. 创建空地图autocostmapMapLoader::createEmptyMap(200,200,0.05,0.0,0.0);// 2. 添加一个矩形障碍物MapLoader::addRectangle(costmap.get(),5.0,3.0,0.2,2.0);// 3. 加载配置LatticePlannerConfig configLatticePlannerConfig::loadFromFile(examples/data/default_config.json);// 4. 创建并配置规划器LatticePlanner planner;planner.configure(config,costmap.get());// 5. 设置起点和终点Pose2D start{1.0,1.0,0.0};Pose2D goal{5.0,5.0,1.57};// 6. 执行路径规划Path pathplanner.createPlan(start,goal);// 7. 输出路径点数量std::coutPath: path.poses.size() pointsstd::endl;// 8. 清理资源planner.cleanup();return0;}整体使用流程非常清晰创建地图 - 加载配置 - 配置规划器 - 输入起终点 - 执行规划 - 输出路径11. 异常处理路径规划过程中可能出现起点越界、目标点越界、起点被障碍物占据、无法找到路径、规划超时等情况。项目提供了对应的异常类型方便上层系统进行处理try{Path pathplanner.createPlan(start,goal);}catch(constStartOutsideMapBoundse){// 起点在地图外}catch(constGoalOutsideMapBoundse){// 目标在地图外}catch(constStartOccupiede){// 起点被障碍物占据}catch(constNoValidPathCouldBeFounde){// 找不到路径}catch(constPlannerTimedOute){// 规划超时}catch(constPlannerExceptione){// 其他规划错误}这种异常机制比简单返回空路径更加清晰也方便工程中对不同错误情况进行日志记录或恢复处理。12. 配置文件说明项目采用 JSON 作为配置文件不再依赖 ROS 参数服务器。配置文件示例如下{motion_model:STATE_LATTICE,lattice_filepath:examples/data/lattice/output.json,search_info:{minimum_turning_radius:0.5,non_straight_penalty:1.05,change_penalty:0.05,reverse_penalty:2.0,cost_penalty:2.0,allow_reverse_expansion:false,downsample_obstacle_heuristic:true},max_planning_time:5.0,tolerance:0.25,smooth_path:true,footprint:[[0.1,0.1],[-0.1,0.1],[-0.1,-0.1],[0.1,-0.1]],footprint_is_radius:false}几个关键参数说明如下。12.1minimum_turning_radius车辆最小转弯半径。该参数需要根据实际底盘运动学能力设置。如果设置得过小规划路径可能在实际车辆上无法执行如果设置得过大规划器会更保守可能导致狭窄空间中规划失败。12.2non_straight_penalty非直线运动惩罚。该参数用于对转弯动作增加额外代价使规划器在条件允许时优先选择更直、更短的路径。12.3change_penalty方向变化惩罚。该参数用于抑制频繁改变运动方向或运动基元类型减少路径抖动。12.4reverse_penalty倒车惩罚。当允许 Reeds-Shepp 或倒车扩展时该参数可以控制倒车动作的使用频率。12.5cost_penalty地图代价惩罚。该参数会让规划器尽量远离高代价区域或障碍物区域提高路径安全性。12.6allow_reverse_expansion是否允许倒车扩展。如果车辆支持倒车可以设置为true如果车辆只允许前进则设置为false。12.7smooth_path是否启用路径平滑。smooth_path:true开启后规划器会在搜索完成后对路径进行平滑处理。13. 项目目录结构项目整体目录结构如下standalone_lattice_planner/ ├── include/lattice_planner/ # 公共头文件 ├── src/ # 实现文件 ├── examples/ # CLI 示例和测试数据 ├── test/ # 单元测试 ├── thirdparty/ # 第三方头文件 ├── scripts/ # 测试地图生成脚本 ├── docs/ # 项目文档 │ ├── API.md # API 参考手册 │ ├── ARCHITECTURE.md # 架构与算法原理 │ └── PERFORMANCE.md # 性能测试报告 ├── CMakeLists.txt # CMake 构建文件 └── README.md # 项目说明文档其中比较重要的目录包括include/lattice_planner/对外暴露的头文件src/核心算法实现examples/命令行示例和示例数据test/GoogleTest 单元测试docs/API、架构和性能说明文档。14. 单元测试项目提供了比较完整的单元测试覆盖代价地图、碰撞检测、运动基元、节点结构、A* 搜索和平滑器等模块。运行测试cdbuild cmake..make-j$(nproc)ctest --output-on-failure测试模块包括测试文件覆盖内容test_costmap.cppCostmap2D 构造、坐标变换、代价读写、膨胀参数test_collision_checker.cpp圆形/多边形足迹碰撞、预计算角度、越界检测test_motion_primitives.cppJSON 加载、元数据一致性、方位 bin 转换test_node_lattice.cpp节点构造、重置、代价、索引、运动模型初始化test_a_star.cpp端到端规划、避障、异常场景、配置状态test_smoother.cpp平滑器参数、空路径、端点保持、锯齿消除完整测试可以帮助验证规划器在不同模块下的稳定性也便于后续二次开发。15. 与 nav2_smac_planner 的关系该项目来自 Navigation2 中的nav2_smac_planner但并不是简单复制而是进行了解耦和工程化重构。主要变化包括原 Nav2 版本standalone_lattice_planner依赖 ROS2不依赖 ROS使用 ROS 参数服务器使用 JSON 配置使用 ROS 消息类型使用纯 C 数据结构使用 nav2_costmap_2d使用独立 Costmap2D使用 rclcpp 日志使用LP_LOG_*日志宏作为 Nav2 插件运行可作为普通 C 库使用依赖 Nav2 生命周期使用LatticePlanner门面类保留下来的核心算法包括A* 搜索State Lattice 节点扩展Ackermann 运动基元Dubins / Reeds-Shepp 解析扩展2D 障碍物启发式SE2 距离启发式碰撞检测路径平滑。因此该项目更适合希望单独使用 State Lattice 规划器但又不想引入整个 ROS2 / Nav2 系统的场景。16. 适用场景该项目比较适合以下场景。16.1 Ackermann 小车路径规划例如自动驾驶小车线控底盘低速无人车清洁机器人园区物流车农业机器人巡检机器人。这类车辆通常具有最小转弯半径约束不能像差速机器人一样原地旋转因此 State Lattice 比普通二维 A* 更适合。16.2 非 ROS 工程集成如果项目本身不是 ROS 架构但又需要一个成熟的路径规划器可以直接将该项目作为 C 库集成进去。适合以下系统自研自动驾驶系统嵌入式导航系统C 仿真平台机器人竞赛平台教学实验平台自研局部或全局规划框架。16.3 路径跟踪控制前端该规划器输出的是路径点x, y, theta不强制输出速度、加速度或时间戳因此更适合作为路径跟踪控制器的前端输入。后端可以接入Pure PursuitStanleyLQRMPCMPPITEBDWA自研局部规划器。16.4 Nav2 State Lattice 算法学习如果想学习 Nav2 Smac Planner 中 State Lattice 的核心算法但又不想被 ROS2 工程结构干扰该项目也比较适合作为学习版本。因为它去掉了大量 ROS 插件、生命周期、消息转换和参数服务器逻辑更容易直接阅读核心算法。17. 与 Hybrid A* 的区别State Lattice 和 Hybrid A* 都常用于车辆路径规划两者都可以考虑车辆朝向和运动学约束。但是二者仍然有一定区别。Hybrid A* 通常在搜索过程中根据车辆模型实时生成连续扩展动作而 State Lattice 更强调预先离散化好的运动基元。简单理解方法特点Hybrid A*在线根据车辆模型扩展灵活性较强State Lattice预先生成运动基元搜索更规则路径可执行性较好State Lattice 的优点是运动基元可控搜索扩展稳定适合固定底盘参数输出路径更符合车辆可执行动作便于工程部署。Hybrid A* 的优点是扩展方式更灵活对不同场景适应性较强在自动泊车等任务中使用广泛。如果车辆底盘结构明确并且希望规划结果更加稳定State Lattice 是一个非常合适的选择。18. 总结standalone_lattice_planner是一个从 ROS Navigation2nav2_smac_planner中解耦出来的独立 State Lattice 路径规划器。它的主要价值在于保留 Nav2 Smac Planner 中成熟的 State Lattice 核心算法去除 ROS / Nav2 依赖使用纯 C17 实现支持 Ackermann 运动基元支持 Dubins / Reeds-Shepp 解析扩展支持双启发式搜索支持多边形足迹碰撞检测支持路径平滑支持 JSON 配置和 PGM 地图加载适合直接集成到非 ROS 工程中。对于需要车辆运动学约束路径规划但又不想依赖完整 ROS2 / Nav2 系统的项目来说这是一个比较实用的独立规划器实现。项目仍处于待测试阶段有兴趣的可以先拉取项目进行编译测试。如果在使用过程中发现问题也欢迎提交 issue方便后续继续完善。项目地址https://github.com/Robot-Nav/standalone_lattice_planner