RC-ESDF与Lazy Theta* 算法结合进行离线全局路径的生成

RC-ESDF-Lazy Theta* 算法技术文档 目录项目背景ESDF核心特性ESDF可视化效果ESDF快速开始Lazy Theta* 算法原理代价函数算法流程视线检测算法实现与 A* 的详细对比梯度下降路径优化项目代码结构运行步骤算法性能分析RC-ESDF 与 MPPI 局部规划器的结合ESDF应用场景技术要点总结参考资源 项目背景 {#project-background}本项目基于RC-ESDFRobo-Centric 2D Signed Distance Field地图采用Lazy Theta* 算法进行全局路径规划生成参考路径。RC-ESDF 原本只提供 ESDF 地图生成和查询功能路径规划部分使用的是传统 A* 算法。本项目参考 Nav2 Theta Star Planner 的 Theta* 算法框架核心改进是将 Nav2 中基于 Costmap 的代价函数替换为 RC-ESDF 的欧几里得距离代价函数从而在 RC-ESDF 地图上实现任意角度的平滑路径规划。PS经过实验发现RC-ESDF地图用于全局路径的一个代价项不是最优选择应当与局部规划器或者局部的轨迹进行结合。复现论文Robo-centric ESDF: A Fast and Accurate Whole-body Collision Evaluation Tool for Any-shape Robotic Planning,Weijia Zhou, Fei Gao, et al.参考项目Nav2 Theta Star Planner: https://github.com/ros-planning/navigation2/tree/main/nav2_theta_star_planner✨ ESDF核心特性 {#esdf-features}论文算法复现: 复现了论文中提出的机器人中心 ESDF 构建逻辑适用于任意形状的多边形足迹Any-shape Footprint。机器人中心坐标系 (Robo-Centric): 所有计算均在 Body Frame 下实时生成无需全局地图天然适配动态环境避障。高速O ( 1 ) O(1)O(1)查询: 基于双线性插值Bilinear Interpolation单次查询耗时仅约2.4 μs测试环境普通移动端 CPU满足极致的实时性需求。解析梯度 (Analytic Gradient): 提供连续、平滑的一阶解析梯度确保基于梯度的优化器如 g2o, Ceres, NLopt能够快速且稳定地收敛。可视化辅助: 内置基于 OpenCV 的诊断工具可直观对比物理轮廓Yellow Box与离散场SDF Field的对齐准确度。轻量化设计: 仅依赖 Eigen3 核心库易于集成到现有的 ROS 或嵌入式导航系统中。 ESDF可视化效果 {#esdf-visualization}通过内置的visualizeEsdf()函数您可以清晰地观察红色区域: 机器人内部 (d i s t 0 dist 0dist0)。绿色区域: 机器人外部安全区 (d i s t 0 dist 0dist0)。黄色轮廓: 输入的原始多边形物理边界。⚪白色箭头: 解析梯度向量∇ D \nabla D∇D始终指向最短路径脱离碰撞的方向。 ESDF快速开始 {#esdf-quick-start}依赖 (Dependencies)Eigen3 (核心计算)OpenCV (可选仅用于可视化调试)CMake ( 3.10) Lazy Theta* 算法原理 {#lazy-theta-algorithm-principles}1. 问题背景为什么需要 Theta*1.1 传统 A* 的局限性传统的 A* 算法在栅格地图上只能产生沿栅格方向的动作4方向或8方向这导致生成的路径呈现明显的锯齿状A* 路径示意8方向 ●●●● ● ● ●●●●这种路径虽然能到达目标点但存在以下问题路径不自然机器人在实际运动中需要频繁转向路径长度欠优锯齿路径通常比理论最短路径长转向代价高对差速驱动机器人尤其不友好1.2 任意角度路径的需求在真实机器人应用中我们希望路径能够直接从起点指向目标点任意角度尽可能短且平滑避免不必要的转向Theta* 算法正是为了解决这一问题而提出的。2. Theta* 算法核心思想2.1 视线检测Line of SightTheta* 的核心创新是引入了视线检测机制在更新节点代价时检查当前节点与其祖先节点之间是否有直接通路无障碍物遮挡。A* 的 parent 更新 start → A → B → C → current必须沿栅格方向 Theta* 的 parent 更新 start ──────→ current如果有视线可直接连接 ↑ A → B → C绕路节点被跳过2.2 Lazy 策略Lazy Theta* 是 Theta* 的优化变体其核心思想是延迟计算标准 Theta*在节点加入开放列表时就执行视线检测Lazy Theta*只在节点从优先队列中弹出被扩展时才执行视线检测这种懒惰策略的好处是如果节点在弹出前被其他更优路径更新则之前的视线检测计算就浪费了Lazy 策略避免了这种浪费提高了计算效率3. 代价函数 {#-代价函数}g(n)- 从起点到节点 n 的实际代价g ( n ) g ( p a r e n t ( n ) ) w e u c ⋅ d e u c l i d e a n ( p a r e n t ( n ) , n ) g(n) g(parent(n)) w_{euc} \cdot d_{euclidean}(parent(n), n)g(n)g(parent(n))weuc​⋅deuclidean​(parent(n),n)其中d e u c l i d e a n d_{euclidean}deuclidean​是两点间的欧几里得距离w e u c w_{euc}weuc​是欧几里得距离的权重参数核心改进说明与 Nav2 Theta Star Planner 使用 Costmap 代价函数不同本项目基于 RC-ESDF 地图的特性采用了更简洁的欧几里得距离作为遍历代价。RC-ESDF 地图中d i s t 0 dist 0dist0的区域障碍物内部被视为不可通行d i s t ≥ 0 dist \geq 0dist≥0的区域边界及外部为可通行区域。3.2 RC-ESDF 相比 Costmap 的优势对比维度CostmapRC-ESDF距离信息仅代价值0-255需推算距离精确欧几里得距离O ( 1 ) O(1)O(1)查询梯度信息无解析梯度提供连续解析梯度障碍物表示代价膨胀后的栅格原始几何距离任意形状需栅格化近似原生支持任意多边形查询速度代价查表双线性插值 ~2.4μs碰撞检测栅格碰撞检测精确距离比较RC-ESDF 的核心优势精确距离感知RC-ESDF 直接存储到障碍物的欧几里得距离而非代价值。在路径规划中这意味着代价计算更加精确路径更贴近实际最短距离。连续解析梯度ESDF 提供解析梯度∇ D \nabla D∇D可用于势场法避障或路径优化。Costmap 无法提供这种连续梯度信息。原生支持任意形状机器人足迹定义为多边形顶点无需栅格化近似。这对非圆形机器人如人形机器人、复杂形状 AGV尤为重要。查询速度极快双线性插值实现O ( 1 ) O(1)O(1)查询约 2.4μs适合高实时性要求的动态避障场景。Robo-Centric 特性坐标系以机器人为中心天然适配局部规划无需全局地图更新。3.3 启发式函数h(n)- 从节点 n 到目标的估计代价使用欧几里得距离h ( n ) w h e u r i s t i c ⋅ d e u c l i d e a n ( n , g o a l ) h(n) w_{heuristic} \cdot d_{euclidean}(n, goal)h(n)wheuristic​⋅deuclidean​(n,goal)3.4 总代价f(n)- 节点 n 的综合代价f ( n ) g ( n ) h ( n ) f(n) g(n) h(n)f(n)g(n)h(n) 算法流程 {#-算法流程}4.1 数据结构structPriorityNode{Eigen::Vector2d pos;// 节点位置doublef;// 总代价doubleg;// 实际代价Eigen::Vector2d parent;// 父节点};std::priority_queuePriorityNodeopen_set;// 开放列表优先队列std::vectorEigen::Vector2dclosed_set;// 关闭列表std::mapEigen::Vector2d,std::pairdouble,Eigen::Vector2dg_values;// g值和parent记录std::mapEigen::Vector2d,Eigen::Vector2dcame_from;// 路径回溯4.2 主循环流程1. 初始化 g(start) 0 将 start 加入开放列表 2. 主循环 while open_set 非空 a. 弹出代价最小的节点 n b. 如果 n 是目标点 通过 came_from 回溯得到路径 return SUCCESS c. 如果 n 在关闭列表中 continue跳过 d. 将 n 加入关闭列表 e. 【Lazy Theta* 关键步骤】更新 n 的 g 值和 parent if n 不是起点 if hasLineOfSight(parent(parent(n)), n) g(n) g(parent(parent(n))) d(parent(parent(n)), n) parent(n) parent(parent(n)) f. 扩展邻居节点 for each neighbor m of n // 计算到达 m 的代价 if hasLineOfSight(parent(n), m) tentative_g g(parent(n)) d(parent(n), m) m_parent parent(n) else tentative_g g(n) d(n, m) m_parent n if m 未被访问 or tentative_g g(m) g(m) tentative_g parent(m) m_parent 将 m 加入开放列表 3. return FAILURE未找到路径5. 视线检测算法实现视线检测使用Bresenham 算法这是一种绘制两点之间直线的经典算法我们用它来检查直线是否穿过障碍物。5.1 Bresenham 算法原理从 (x0, y0) 到 (x1, y1) dx |x1 - x0| dy |y1 - y0| sx (x0 x1) ? 1 : -1 sy (y0 y1) ? 1 : -1 err dx - dy while cx ! x1 or cy ! y1: if grid[cy][cx] 是障碍物: return false e2 2 * err if e2 -dy: err - dy cx sx if e2 dx: err dx cy sy return true5.2 代码实现boolRcEsdfMap::hasLineOfSight(constEigen::Vector2da,constEigen::Vector2db){// 1. 将世界坐标转换为栅格坐标doublegx_a,gy_a,gx_b,gy_b;posToGrid(a,gx_a,gy_a);posToGrid(b,gx_b,gy_b);intx0static_castint(gx_a);inty0static_castint(gy_a);intx1static_castint(gx_b);inty1static_castint(gy_b);// 2. 初始化 Bresenham 参数intdxabs(x1-x0);intdyabs(y1-y0);intsxx0x1?1:-1;intsyy0y1?1:-1;interrdx-dy;intcxx0;intcyy0;// 3. 沿着直线前进检查每个栅格while(cx!x1||cy!y1){// 如果当前栅格是障碍物视线被遮挡if(getRaw(cx,cy)0){returnfalse;}inte22*err;if(e2-dy){err-dy;cxsx;}if(e2dx){errdx;cysy;}}// 4. 检查终点栅格returngetRaw(x1,y1)0;}6. 与 A* 的详细对比 {#-与-a-的详细对比}对比维度A*Lazy Theta*路径形态锯齿状沿栅格方向任意角度平滑路径路径长度通常比最短路径长 5-15%接近理论最短路径计算开销O(1) 邻居扩展需要额外的视线检测邻居扩展直接从当前节点可能从祖先节点跳过一个或多个节点转向次数频繁明显减少适用场景计算资源受限环境路径质量要求高的场景7. 梯度下降路径优化 {#-梯度下降路径优化}Lazy Theta* 生成的路径虽然比 A* 平滑但在某些情况下仍可能不够理想。为此项目实现了基于 ESDF 梯度下降的路径优化7.1 优化目标最小化路径点到障碍物的距离min ⁡ ∑ i D ( p i ) 2 \min \sum_{i} D(p_i)^2mini∑​D(pi​)27.2 更新规则// 普通情况沿梯度下降p_i_newp_i_old-alpha*grad(D(p_i))// 安全余量不足时使用斥力ifD(p_i)safe_margin:p_i_newp_i_oldbeta*(-grad(D(p_i))) 项目代码结构核心文件文件位置功能rc_esdf.hinclude/类声明、Lazy Theta* 函数声明rc_esdf.cppsrc/ESDF 实现、Lazy Theta* 算法实现rc_esdf_planner_node.cppsrc/ROS 节点调度路径规划新增函数函数名功能hasLineOfSight()Bresenham 视线检测planPathLazyThetaStar()Lazy Theta* 主算法planPathLazyThetaStarWithGradientOpt()Lazy Theta* 梯度优化 运行步骤1. 编译项目cd~/RC_ESDF catkin_makesourcedevel/setup.bash2. 运行节点# 使用 Lazy Theta* 算法roslaunch rc_esdf rc_esdf_planner.launch use_lazy_theta_star:true# 使用原始 A* 算法roslaunch rc_esdf rc_esdf_planner.launch use_astar:true3. 关键参数说明参数类型默认值说明use_lazy_theta_starboolfalse启用 Lazy Theta*优先级高于 use_astaruse_astarbooltrue使用 A* 算法opt_iterationsint50梯度优化迭代次数safe_margindouble1.0安全余量米 算法性能分析时间复杂度 {#-算法性能分析}操作复杂度地图初始化O(W × H)ESDF 查询O(1)视线检测O(L)L 为两点间栅格距离Lazy Theta*O(N log N)N 为扩展节点数空间复杂度数据结构空间ESDF 地图O(W × H)开放列表O(N)关闭列表O(N)g_valuesO(N) RC-ESDF 与 MPPI 局部规划器的结合 {#rc-esdf-with-mppi}MPPI 简介MPPI (Model Predictive Path Integral Control)是一种基于采样的局部轨迹优化算法其核心流程采样生成多条候选控制序列/轨迹评估计算每条轨迹的代价含碰撞代价、 smoothness 代价等聚合按代价加权平均输出最优控制RC-ESDF 在 MPPI 中的作用MPPI 需求RC-ESDF 能提供的支持碰撞检测O ( 1 ) O(1)O(1)精确距离查询d i s t 0 dist 0dist0即碰撞梯度信息解析梯度∇ D \nabla D∇D引导轨迹远离障碍安全约束安全余量判断动态调整斥力强度计算效率~2.4μs 单次查询支持高频采样评估结合方案示意MPPI 采样轨迹评估流程 ┌─────────────────────────────────┐ │ for each sampled trajectory: │ │ for each waypoint p: │ │ dist, grad esdf.query(p) │ │ if dist 0: │ │ cost collision_penalty │ │ else: │ │ cost - dist * safety_weight │ │ │ │ apply gradient to trajectory │ └─────────────────────────────────┘相比 Costmap 的优势场景Costmap MPPIRC-ESDF MPPI梯度引导只能用有限差分估计解析梯度精确高效安全走廊膨胀代价不稳定精确距离平滑势场非圆形机器人需复杂膨胀计算原生多边形支持实时性差分计算慢~2.4μs 查询RC-ESDF → MPPI 数据流┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │ 激光雷达 │ ──► │ RC-ESDF │ ──► │ MPPI │ │ /点云数据 │ │ 局部距离场 │ │ 轨迹优化 │ └──────────────┘ └──────────────┘ └──────────────┘ │ │ │ dist, grad 查询 │ └─────────────────────┘ │ ┌──────┴──────┐ │ 输出平滑安全 │ │ 的局部轨迹 │ └─────────────┘总结RC-ESDF 与 MPPI 结合在以下场景尤为有效动态环境避障实时构建距离场快速响应移动障碍物复杂形状机器人原生支持任意多边形无需近似膨胀高精度需求解析梯度提供精确的势场引导轨迹更平滑 ESDF应用场景 {#esdf-applications}TEB Local Planner: 增强碰撞检测逻辑为非圆形状机器人提供更精确的代价约束。轨迹优化 (Trajectory Optimization): 在 MPC 或 EGO-Planner 框架中作为硬约束或惩罚项。势场法导航: 生成高质量、无震荡的斥力场。 技术要点总结 {#technical-summary}Lazy Theta* 的三大关键创新视线检测允许节点直接连接祖先节点生成任意角度路径Lazy 策略延迟视线检测到节点扩展时避免不必要的计算代价传播通过 parent 指针链式传播最优代价工程实现注意事项坐标转换世界坐标与栅格坐标的转换要确保一致性边界检查视线检测和邻居扩展都要检查地图边界障碍物定义ESDF 中 dist 0 的区域视为障碍物数值精度使用 epsilon (1e-6) 处理浮点数比较 参考资源Lazy Theta原始论文*Nash A, Koenig S, Felner C. “Theta*: Any-Angle Path Planning on Grids”http://idm-lab.org/bib/abstracts/papers/aaai10b.pdfNav2 Theta Star Plannerhttps://github.com/ros-planning/navigation2/tree/main/nav2_theta_star_plannerBresenham 算法Bresenham, J. E. (1965). “Algorithm for computer control of a digital plotter”文档更新时间2026-04-12