轨迹优化 | 基于激光雷达的欧氏距离场ESDF地图构建(附ROS C++仿真)

目录

  • [1 欧氏符号距离场的应用](#1 欧氏符号距离场的应用)
  • [2 基于激光雷达的ESDF构建](#2 基于激光雷达的ESDF构建)
  • [3 ROS算法仿真](#3 ROS算法仿真)

在机器人路径规划与自主导航领域,**欧氏符号距离场(Euclidean Signed Distance Field, ESDF)**是一种用于高效表示环境几何信息的关键数据结构。它通过量化空间中每个点到最近障碍物的有符号欧氏距离,将复杂的障碍物分布转化为连续的数值场,为轨迹优化、碰撞检测等任务提供了强大的数学工具。本文将从数学定义出发,结合轨迹优化的具体场景,阐述ESDF的核心思想与应用


1 欧氏符号距离场的应用

ESDF的核心是有符号的欧氏距离 。对于三维空间中的任意一点 p = ( x , y , z ) p = (x, y, z) p=(x,y,z),其ESDF值 d ( p ) d(p) d(p)定义为:

d ( p ) = { min ⁡ q ∈ ∂ O ∣ ∣ p − q ∣ ∣ 若 p ∉ O , − min ⁡ q ∈ ∂ O ∣ ∣ p − q ∣ ∣ 若 p ∈ O , d(p) = \begin{cases} \min_{q \in \partial O} ||p - q|| \ & \text{若 } p \notin O, \\ -\min_{q \in \partial O} ||p - q|| \ & \text{若 } p \in O, \end{cases} d(p)={minq∈∂O∣∣p−q∣∣ −minq∈∂O∣∣p−q∣∣ 若 p∈/O,若 p∈O,

其中 O O O表示障碍物集合, ∂ O \partial O ∂O是障碍物的表面(边界), ∣ ∣ ⋅ ∣ ∣ ||\cdot || ∣∣⋅∣∣ 是距离范数,常用的距离度量有:

  • 欧氏距离,此时称为欧氏距离变换(Euclidean Distance Transform, EDT)
  • 曼哈顿距离,此时称为曼哈顿距离变换(Manhattan Distance Transform, MDT)
  • 切比雪夫距离,此时称为切比雪夫距离变换(Chebyshev Distance Transform, CDT)
  • ...

当采用欧氏距离定义距离范数,即三维空间中两点间的直线距离时, d ( p ) d(p) d(p)组成ESDP。从物理意义的角度理解:当 p p p位于障碍物外部( p ∉ O p \notin O p∈/O)时, d ( p ) d(p) d(p)为正,表示 p p p到最近障碍物表面的距离;当 p p p位于障碍物内部( p ∈ O p \in O p∈O)时, d ( p ) d(p) d(p)为负,其绝对值仍表示到表面的距离。这种符号设计使得ESDF不仅能判断是否碰撞,还能隐含碰撞方向的信息(如负值表示点在障碍物内部,需远离;正值表示点在自由空间,可接近),这在距离梯度的建模方面非常有效。

接下来用两个简单的例子说明ESDF的应用方法:

案例一 : 机器人碰撞检测硬约束

轨迹优化的首要任务是确保路径不与障碍物发生碰撞。对于轨迹上的任意点 p ( t ) p(t) p(t)( t t t为时间参数),其ESDF值 d ( p ( t ) ) d(p(t)) d(p(t))需满足 d ( p ( t ) ) ≥ r d(p(t)) \geq r d(p(t))≥r,其中 r r r是机器人的安全半径(考虑机器人自身尺寸)。若 d ( p ( t ) ) < r d(p(t)) < r d(p(t))<r,则路径与障碍物发生碰撞。 这一约束可被形式化为优化问题的硬约束
d ( p ( t ) ) ≥ r , ∀ t ∈ [ t 0 , t f ] d(p(t)) \geq r, \quad \forall t \in [t_0, t_f] d(p(t))≥r,∀t∈[t0,tf]

动态窗口DWA算法人工势场APF算法中ESDF还可用于生成避障力,引导轨迹远离障碍物
案例二 : 机器人轨迹优化软约束

轨迹优化的目标通常是多目标的,既希望路径尽可能短(或能耗低),又希望足够安全。ESDF的数值可直接作为安全性代价的度量:路径上各点的ESDF值越小(或越负),安全性越低。因此,可将ESDF的倒数或平方作为代价权重,融入总代价函数
J = ∫ t 0 t f ( w 1 ⋅ p ( t ) + w 2 ⋅ 1 d ( p ( t ) ) + ϵ ) d t J = \int_{t_0}^{t_f} \left( w_1 \cdot{p}(t) \ \ + w_2 \cdot \frac{1}{d(p(t)) + \epsilon} \right) dt J=∫t0tf(w1⋅p(t) +w2⋅d(p(t))+ϵ1)dt

其中 w 1 w_1 w1, w 2 w_2 w2 是权重系数, ϵ \epsilon ϵ 是避免分母为零的小常数,第二项惩罚靠近障碍物的行为(鼓励远离障碍物)。 这种设计使得梯度下降规划等优化算法能够自动权衡路径长度与安全性,生成保持安全的最优轨迹。

ESDF的计算原理是多维度的动态规划,如下图所示,具体可以看推导文章轨迹优化 | 图解欧氏距离场与梯度场算法(附ROS C++/Python实现)

2 基于激光雷达的ESDF构建

激光雷达获取的点云数据本质上是连续三维空间中的离散采样点集 P = p 1 , p 2 , . . . , p n \mathcal{P} = {\mathbf{p}_1, \mathbf{p}_2, ..., \mathbf{p}_n} P=p1,p2,...,pn,其中 p i = ( x i , y i , z i ) ∈ R 3 \mathbf{p}_i = (x_i, y_i, z_i) \in \mathbb{R}^3 pi=(xi,yi,zi)∈R3。构建三维栅格地图的首要任务是将这些连续坐标映射到离散的体素空间:

Φ : R 3 → Z 3 , Φ ( p ) = ( ⌊ x δ ⌋ , ⌊ y δ ⌋ , ⌊ z δ ⌋ ) = v i j k \Phi:\mathbb{R}^{3}\rightarrow\mathbb{Z}^{3},\quad\Phi(\mathbf{p})=\left(\left\lfloor\frac{x}{\delta}\right\rfloor,\left\lfloor\frac{y}{\delta}\right\rfloor,\left\lfloor\frac{z}{\delta}\right\rfloor\right)=v_{ijk} Φ:R3→Z3,Φ(p)=(⌊δx⌋,⌊δy⌋,⌊δz⌋)=vijk

其中 δ \delta δ 是体素分辨率。这个过程称为体素化,它将每个点分配到唯一的体素单元 v i j k v_{ijk} vijk。体素化的目的是将连续的三维空间离散化,降低计算复杂度。体素大小的选择需平衡精度与计算量: δ \delta δ过小会导致体素数量爆炸, δ \delta δ 过大则会丢失细节(如无法区分狭窄通道)。实际应用中, δ \delta δ 通常根据机器人尺寸(如AGV的宽度)或任务需求(如导航精度)设定为 0.05m ~0.2m。将点云转化为栅格地图的过程可以参考障碍感知 | 基于3D激光雷达的三维膨胀栅格地图构建(附ROS C++仿真)

接着可以直接运行3D ESDF构建算法;也可以选取一个水平切片高度,运行2D ESDF构建算法,算法流程如表所示

3 ROS算法仿真

核心代码如下所示

cpp 复制代码
template <typename F_get_val, typename F_set_val>
void compute(F_get_val f_get, F_set_val f_set, int dim)
{
  // initialzation
  int k = 0;
  int n = dim == 0 ? ny_ : nx_;
  std::vector<int> v(n);
  std::vector<double> z(n + 1);
  z[0] = -std::numeric_limits<double>::max();
  z[1] = std::numeric_limits<double>::max();

  // envelope
  for (int q = 1; q < n; q++)
  {
    double s =
        ((f_get(v[k]) + std::pow(static_cast<double>(v[k]), 2)) - (f_get(q) + std::pow(static_cast<double>(q), 2))) /
        (2 * (v[k] - q));
    while (s <= z[k])
    {
      k -= 1;
      s = ((f_get(v[k]) + std::pow(static_cast<double>(v[k]), 2)) -
           (f_get(q) + std::pow(static_cast<double>(q), 2))) /
          (2 * (v[k] - q));
    }
    k += 1;
    v[k] = q;
    z[k] = s;
    z[k + 1] = std::numeric_limits<double>::max();
  }

  // distance calculation
  k = 0;
  for (int q = 0; q < n; q++)
  {
    while (z[k + 1] < q)
      k += 1;
    f_set(q, std::pow(static_cast<double>(q - v[k]), 2) + f_get(v[k]));
  }
}

和上面的算法流程对应。

仿真参数设置如下:

yaml 复制代码
sdf_map:
  resolution: 0.15
  map_x_size: 30.0
  map_y_size: 30.0
  map_z_size: 3.0
  local_update_range_x: 6.0
  local_update_range_y: 6.0
  local_update_range_z: 1.5
  inflation_radius: 0.15
  slice_height: 0.3
  esdf_max_distance_vis: 3.0
  esdf_min_distance_vis: 0.0

启动仿真后,如下图灰色体素所示为激光雷达点云转换的栅格

可视化0.2m的水平ESDF切片,其中红色到蓝色表示距离障碍物的距离由近到远

完整工程代码请联系下方博主名片获取


🔥 更多精彩专栏

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇

相关推荐
蹦蹦跳跳真可爱5891 分钟前
Python----OpenCV(图像増强——高通滤波(索贝尔算子、沙尔算子、拉普拉斯算子),图像浮雕与特效处理)
人工智能·python·opencv·计算机视觉
雷羿 LexChien12 分钟前
从 Prompt 管理到人格稳定:探索 Cursor AI 编辑器如何赋能 Prompt 工程与人格风格设计(上)
人工智能·python·llm·编辑器·prompt
无小道18 分钟前
c++-引用(包括完美转发,移动构造,万能引用)
c语言·开发语言·汇编·c++
两棵雪松1 小时前
如何通过向量化技术比较两段文本是否相似?
人工智能
heart000_11 小时前
128K 长文本处理实战:腾讯混元 + 云函数 SCF 构建 PDF 摘要生成器
人工智能·自然语言处理·pdf
敲键盘的小夜猫1 小时前
LLM复杂记忆存储-多会话隔离案例实战
人工智能·python·langchain
开开心心_Every1 小时前
便捷的Office批量转PDF工具
开发语言·人工智能·r语言·pdf·c#·音视频·symfony
cooldream20091 小时前
「源力觉醒 创作者计划」_基于 PaddlePaddle 部署 ERNIE-4.5-0.3B 轻量级大模型实战指南
人工智能·paddlepaddle·文心大模型
亚里随笔2 小时前
L0:让大模型成为通用智能体的强化学习新范式
人工智能·llm·大语言模型·rlhf
白杆杆红伞伞2 小时前
T01_神经网络
人工智能·深度学习·神经网络