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

RC-ESDF-Lazy Theta* 算法技术文档

📑 目录

项目背景
ESDF核心特性
ESDF可视化效果
ESDF快速开始
[Lazy Theta* 算法原理](#Lazy Theta* 算法原理)
代价函数
算法流程
视线检测算法实现
[与 A* 的详细对比](#与 A* 的详细对比)
梯度下降路径优化
项目代码结构
运行步骤
算法性能分析
[RC-ESDF 与 MPPI 局部规划器的结合](#RC-ESDF 与 MPPI 局部规划器的结合)
ESDF应用场景
技术要点总结
参考资源

📖 项目背景 {#project-background}

本项目基于 RC-ESDF （Robo-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 < 0 dist<0)。
🟢 绿色区域 : 机器人外部安全区 ( d i s t > 0 dist > 0 dist>0)。
🟨 黄色轮廓: 输入的原始多边形物理边界。
⚪ 白色箭头 : 解析梯度向量 ∇ 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 Sight）

Theta* 的核心创新是引入了视线检测机制：在更新节点代价时，检查当前节点与其"祖先"节点之间是否有直接通路（无障碍物遮挡）。

复制代码

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 < 0 dist<0 的区域（障碍物内部）被视为不可通行， d i s t ≥ 0 dist \geq 0 dist≥0 的区域（边界及外部）为可通行区域。

3.2 RC-ESDF 相比 Costmap 的优势

对比维度	Costmap	RC-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 数据结构

cpp 复制代码

struct PriorityNode {
    Eigen::Vector2d pos;    // 节点位置
    double f;               // 总代价
    double g;               // 实际代价
    Eigen::Vector2d parent;  // 父节点
};

std::priority_queue<PriorityNode> open_set;    // 开放列表（优先队列）
std::vector<Eigen::Vector2d> closed_set;        // 关闭列表
std::map<Eigen::Vector2d, std::pair<double, Eigen::Vector2d>> g_values;  // g值和parent记录
std::map<Eigen::Vector2d, Eigen::Vector2d> came_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 true

5.2 代码实现

cpp 复制代码

bool RcEsdfMap::hasLineOfSight(const Eigen::Vector2d& a, const Eigen::Vector2d& b) {
    // 1. 将世界坐标转换为栅格坐标
    double gx_a, gy_a, gx_b, gy_b;
    posToGrid(a, gx_a, gy_a);
    posToGrid(b, gx_b, gy_b);

    int x0 = static_cast<int>(gx_a);
    int y0 = static_cast<int>(gy_a);
    int x1 = static_cast<int>(gx_b);
    int y1 = static_cast<int>(gy_b);

    // 2. 初始化 Bresenham 参数
    int dx = abs(x1 - x0);
    int dy = abs(y1 - y0);
    int sx = x0 < x1 ? 1 : -1;
    int sy = y0 < y1 ? 1 : -1;
    int err = dx - dy;

    int cx = x0;
    int cy = y0;

    // 3. 沿着直线前进，检查每个栅格
    while (cx != x1 || cy != y1) {
        // 如果当前栅格是障碍物，视线被遮挡
        if (getRaw(cx, cy) < 0) {
            return false;
        }

        int e2 = 2 * err;
        if (e2 > -dy) {
            err -= dy;
            cx += sx;
        }
        if (e2 < dx) {
            err += dx;
            cy += sy;
        }
    }

    // 4. 检查终点栅格
    return getRaw(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)^2 mini∑D(pi)2

7.2 更新规则

cpp 复制代码

// 普通情况：沿梯度下降
p_i_new = p_i_old - alpha * grad(D(p_i))

// 安全余量不足时：使用斥力
if D(p_i) < safe_margin:
    p_i_new = p_i_old + beta * (-grad(D(p_i)))

📁 项目代码结构

核心文件

文件	位置	功能
`rc_esdf.h`	include/	类声明、Lazy Theta* 函数声明
`rc_esdf.cpp`	src/	ESDF 实现、Lazy Theta* 算法实现
`rc_esdf_planner_node.cpp`	src/	ROS 节点，调度路径规划

新增函数

函数名	功能
`hasLineOfSight()`	Bresenham 视线检测
`planPathLazyThetaStar()`	Lazy Theta* 主算法
`planPathLazyThetaStarWithGradientOpt()`	Lazy Theta* + 梯度优化

🚀 运行步骤

1. 编译项目

bash 复制代码

cd ~/RC_ESDF
catkin_make
source devel/setup.bash

2. 运行节点

bash 复制代码

# 使用 Lazy Theta* 算法
roslaunch rc_esdf rc_esdf_planner.launch use_lazy_theta_star:=true

# 使用原始 A* 算法
roslaunch rc_esdf rc_esdf_planner.launch use_astar:=true

3. 关键参数说明

参数	类型	默认值	说明
`use_lazy_theta_star`	bool	false	启用 Lazy Theta*（优先级高于 use_astar）
`use_astar`	bool	true	使用 A* 算法
`opt_iterations`	int	50	梯度优化迭代次数
`safe_margin`	double	1.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_values	O(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 < 0 dist<0 即碰撞
梯度信息	解析梯度 ∇ 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 + MPPI	RC-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.pdf
Nav2 Theta Star Planner：

https://github.com/ros-planning/navigation2/tree/main/nav2_theta_star_planner
Bresenham 算法：

Bresenham, J. E. (1965). "Algorithm for computer control of a digital plotter"

文档更新时间：2026-04-12