轨迹优化 | 图解欧氏距离场与梯度场算法(附ROS C++/Python实现)

本文主要是介绍轨迹优化 | 图解欧氏距离场与梯度场算法(附ROS C++/Python实现)，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

0 专栏介绍

🔥课程设计、毕业设计、创新竞赛、学术研究必备！本专栏涉及更高阶的运动规划算法实战：曲线生成与轨迹优化、碰撞模型与检测、多智能体群控、深度强化学习运动规划、社会性导航、全覆盖路径规划等内容，每个模型都包含代码实现加深理解。

🚀详情：运动规划实战进阶

1 什么是距离场？

距离场(Distance Field)也称为距离变换(Distance Transform)，在图像处理和模式识别中是一种重要工具，其核心思想是将图像中每个像素点的值表示为到最近的目标像素的距离度量，更高维的情况依次类推。在计算机视觉领域，距离场是图像分割和配准的基础，在运动规划中则常用于地图建模(广义Voronoi图计算)和轨迹梯度优化。

在这里插入图片描述

给定点集 $\mathcal{G}$ 和其上的目标点集 $Q\subseteq \mathcal{G}$ ，可以定义函数

$\mathcal{D} _I\left( p \right) =\min _{q\in \mathcal{G}}\left( d\left( p,q \right) +I\left( q \right) \right)$

计算任意一点 $p$ 到 $Q$ 的最近距离，其中距离度量 $d\left( \cdot ,\cdot \right)$ 定义了距离场的属性，指示函数

$I_Q\left( q \right) =\begin{cases} 0, \mathrm{if} q\in Q\\ \infty , \mathrm{otherwise}\\\end{cases}$

常用的 $d\left( \cdot ,\cdot \right)$ 为

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

2 欧氏距离场计算原理

$n$ 维距离场可以通过一维距离场迭代计算得到，因此只需要讨论一维EDT的计算即可。如下图左侧所示为初始计算轴的EDT计算，右侧所示为更一般的情况，此时障碍物处的采样函数叠加了前轴计算信息。

在这里插入图片描述

注意到 $\mathcal{O}$ 定义了一系列以障碍物 $q\in \mathcal{O}$ 为顶点的抛物线，而 $\forall p\in \mathcal{G}$ 在系列抛物线形成的下包络的投影组成了距离场 $\mathcal{D} _f\left( p \right)$ ，而下包络的计算与抛物线交点有关。联立两条抛物线 $\left( s-q \right) ^2+f\left( q \right) =\left( s-r \right) ^2+f\left( r \right)$ 可得

$s=\frac{\left( f\left( r \right) +r^2 \right) -\left( f\left( q \right) +q^2 \right)}{2r-2q}$

即任意两条抛物线有且仅有一个交点 $s$ 。设 $\mathcal{K}$ 为实际组成下包络的抛物线集合， $v\left( k \right)$ 表示其中第 $k$ 条抛物线的顶点， $z\left( k \right)$ 表示第 $k$ 条和第 $k - 1$ 条抛物线的交点，区间 $\left[ z\left( k \right) ,z\left( k+1 \right) \right)$ 表示第 $k$ 条抛物线的下包络范围。在遍历求解下包络过程中，对于新的抛物线 $e$ ，其与 $\mathcal{K}$ 中最新的一条抛物线 $k$ 的交点 $s$ 有两种情况：

若 $s>z\left( k \right)$ ，则将 $e$ 添加到 $\mathcal{K}$ 中并更新 $v\left( k \right)$ 、 $z\left( k \right)$ ；
若 $s\leqslant z\left( k \right)$ ，则第 $k$ 条抛物线不参与构成下包络，应从 $\mathcal{K}$ 中删除并重新计算 $e$ 与新的第 $k$ 条抛物线的交点直至 $s$ 在 $z (k)$ 右侧；

算法流程如表所示

在这里插入图片描述

3 双线性插值与欧式梯度场

在求解梯度过程中，需要计算离散距离场的线性插值函数。以二维环境的双线性插值为例，设已知离散空间有四点坐标为 $A\left( x_1,y_1 \right)$ 、 $B\left( x_2,y_1 \right)$ 、 $C\left( x_1,y_2 \right)$ 与 $D\left( x_2,y_2 \right)$ ，对其中任意一点 $P$ ，首先在 $x$ 方向上插值

$\begin{cases} f\left( R_1 \right) =f\left( x,y_1 \right) =\frac{x_2-x}{x_2-x_1}f\left( A \right) +\frac{x-x_1}{x_2-x_1}f\left( B \right)\\ f\left( R_2 \right) =f\left( x,y_2 \right) =\frac{x_2-x}{x_2-x_1}f\left( C \right) +\frac{x-x_1}{x_2-x_1}f\left( D \right)\\\end{cases}$

再基于插值点 $R_1$ 、 $R_2$ 进行 $y$ 方向的插值

$f\left( P \right) =f\left( x,y \right) =\frac{y_2-y}{y_2-y_1}f\left( R_1 \right) +\frac{y-y_1}{y_2-y_1}f\left( R_2 \right)$

展开可得矩阵形式

$f\left( x,y \right) =\frac{1}{\left( x_2-x_1 \right) \left( y_2-y_1 \right)}\left[ \begin{array}{c} x_2-x\\ x-x_1\\\end{array} \right] ^T\left[ \begin{matrix} f\left( A \right)& f\left( C \right)\\ f\left( B \right)& f\left( D \right)\\\end{matrix} \right] \left[ \begin{array}{c} y_2-y\\ y-y_1\\\end{array} \right]$

在这里插入图片描述

对于离散栅格坐标而言， $x_2-x_1=y_2-y_1=1$ ，设 $\varDelta x=x-x_1$ 与 $\varDelta y=y-y_1$ ，则插值函数简化为

$f\left( \varDelta x,\varDelta y \right) =\left[ \begin{array}{c} 1-\varDelta x\\ \varDelta x\\\end{array} \right] ^T\left[ \begin{matrix} f\left( A \right)& f\left( C \right)\\ f\left( B \right)& f\left( D \right)\\\end{matrix} \right] \left[ \begin{array}{c} 1-\varDelta y\\ \varDelta y\\\end{array} \right]$

则函数 $f$ 在任意一点的梯度为

$\nabla f\left( \varDelta x,\varDelta y \right) =\left[ \begin{array}{c} \frac{\partial f\left( \varDelta x,\varDelta y \right)}{\partial \varDelta x}\\ \frac{\partial f\left( \varDelta x,\varDelta y \right)}{\partial \varDelta y}\\\end{array} \right] =\left[ \begin{array}{c} \left[ \begin{array}{c} -1\\ 1\\\end{array} \right] ^T\left[ \begin{matrix} f\left( A \right)& f\left( C \right)\\ f\left( B \right)& f\left( D \right)\\\end{matrix} \right] \left[ \begin{array}{c} 1-\varDelta y\\ \varDelta y\\\end{array} \right]\\ \left[ \begin{array}{c} 1-\varDelta x\\ \varDelta x\\\end{array} \right] ^T\left[ \begin{matrix} f\left( A \right)& f\left( C \right)\\ f\left( B \right)& f\left( D \right)\\\end{matrix} \right] \left[ \begin{array}{c} -1\\ 1\\\end{array} \right]\\\end{array} \right]$

4 仿真实现

4.1 ROS C++实现

核心代码如下所示

GradientLayer::updateCosts(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,int max_i,int max_j)
{if (!enabled_) {return;}unsigned char * master_array = master_grid.getCharMap();unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();// Fixing window coordinates with map size if necessary.min_i = std::max(0, min_i);min_j = std::max(0, min_j);max_i = std::min(static_cast<int>(size_x), max_i);max_j = std::min(static_cast<int>(size_y), max_j);// Simply computing one-by-one cost per each cellint gradient_index;for (int j = min_j; j < max_j; j++) {// Reset gradient_index each time when reaching the end of re-calculated window// by OY axis.gradient_index = 0;for (int i = min_i; i < max_i; i++) {int index = master_grid.getIndex(i, j);// setting the gradient costunsigned char cost = (LETHAL_OBSTACLE - gradient_index*GRADIENT_FACTOR)%255;if (gradient_index <= GRADIENT_SIZE) {gradient_index++;} else {gradient_index = 0;}master_array[index] = cost;}}
}

4.2 Python实现

欧氏距离场核心代码

def compute(self, f_get, mat: np.ndarray, dim: int) -> np.array:"""Compute distance field along one-dimension baseon sample funciton f_get.Parameters:f_get (function): sample functionmat (np.ndarray): the matrix to transformdim (int): the dimension to transformReturns:df (np.array): the distance field along dim-dimension"""# initialzationk = 0n = mat.shape[dim]v, z = [0 for _ in range(n)], [0 for _ in range(n + 1)]z[0], z[1] = -self.INF, self.INF# envelopefor q in range(1, n):s = ((f_get(v[k]) + v[k] ** 2) - (f_get(q) + q ** 2)) / (2 * (v[k] - q))while s <= z[k]:k -= 1s = ((f_get(v[k]) + v[k] ** 2) - (f_get(q) + q ** 2)) / (2 * (v[k] - q))k += 1v[k] = qz[k], z[k + 1] = s, self.INF# distance calculationk = 0edf = np.zeros((n, ))for q in range(n):while z[k + 1] < q:k += 1edf[q] = (q - v[k]) ** 2 + f_get(v[k])return edf

欧氏梯度场核心代码：

def gradient(self, df: np.ndarray, x: float, y: float) -> np.array:"""To obtain the gradient at (x, y) in the distance field through bilinear interpolation.Parameters:df (np.ndarray): the distance fieldx/y (float): the query coordinateReturns:g(x, y): the gradient at (x, y)"""m, n = df.shapex, y = max(min(n - 1, x), 0), max(min(m - 1, y), 0)xi, yi = int(x), int(y)dx, dy = x - xi, y - yixi, yi = max(min(n - 1, xi), 0), max(min(m - 1, yi), 0)xp, yp = max(min(n - 1, xi + 1), 0), max(min(m - 1, yi - 1), 0)bl, br = df[yi, xi], df[yi, xp]tl, tr = df[yp, xi], df[yp, xp]return np.array([(1 - dy) * (br - bl) + dy * (tr + tl),-((1 - dx) * (tl - bl) + dx * (tr - br))])