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

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

1 欧氏符号距离场的应用

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

$$d(p)=\{\begin{array}{ll}{\min }_{q\in \partial O}||p-q||& \text{若 }p\notin O,\\ -{\min }_{q\in \partial O}||p-q||& \text{若 }p\in O,\end{array}$$

其中$O$表示障碍物集合，$\partial O$是障碍物的表面（边界），$||\cdot ||$ 是距离范数，常用的距离度量有：

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

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

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

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

轨迹优化的首要任务是确保路径不与障碍物发生碰撞。对于轨迹上的任意点$p(t)$（$t$为时间参数），其ESDF值$d(p(t))$需满足$d(p(t))\ge r$，其中$r$是机器人的安全半径（考虑机器人自身尺寸）。若$d(p(t))<r$，则路径与障碍物发生碰撞。 这一约束可被形式化为优化问题的硬约束
$$d(p(t))\ge r,\forall t\in [t_0,t_f]$$
在动态窗口DWA算法、人工势场APF算法中ESDF还可用于生成避障力，引导轨迹远离障碍物

案例二： 机器人轨迹优化软约束

轨迹优化的目标通常是多目标的，既希望路径尽可能短（或能耗低），又希望足够安全。ESDF的数值可直接作为安全性代价的度量：路径上各点的ESDF值越小（或越负），安全性越低。因此，可将ESDF的倒数或平方作为代价权重，融入总代价函数
$$J={\int }_{t_0}^{t_f}\left(w_1\cdot p(t)+w_2\cdot \frac{1}{d(p(t))+ϵ}\right)dt$$
其中 $w_1$, $w_2$ 是权重系数，$ϵ$ 是避免分母为零的小常数，第二项惩罚靠近障碍物的行为（鼓励远离障碍物）。 这种设计使得梯度下降规划等优化算法能够自动权衡路径长度与安全性，生成保持安全的最优轨迹。

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

2 基于激光雷达的ESDF构建

激光雷达获取的点云数据本质上是连续三维空间中的离散采样点集 $\mathcal{P}={\mathbf{p}}_1,{\mathbf{p}}_2,...,{\mathbf{p}}_n$，其中 ${\mathbf{p}}_i=(x_i,y_i,z_i)\in {\mathbb{R}}^3$。构建三维栅格地图的首要任务是将这些连续坐标映射到离散的体素空间：

$$\Phi :{\mathbb{R}}^3\to {\mathbb{Z}}^3,\Phi (\mathbf{p})=\left(\lfloor \frac{x}{\delta }\rfloor ,\lfloor \frac{y}{\delta }\rfloor ,\lfloor \frac{z}{\delta }\rfloor \right)=v_{ijk}$$

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

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

3 ROS算法仿真

核心代码如下所示

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]));
  }
}

和上面的算法流程对应。

仿真参数设置如下：

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切片，其中红色到蓝色表示距离障碍物的距离由近到远

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

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …