《Falco: Fast likelihood‐based collision avoidance with extension to human‐guided navigation》

介绍

采用分层方法把规划问题分为两个子问题，第一个问题解决的是全局规划问题，可能会辅以启发式方法，以确保路径不会陷入局部最小值。第二个问题是解决并行运行的局部规划问题，以跟踪全局路径并避开障碍物。在本文中，我们提出了一种可大大降低计算复杂度的方法，从而利用航空飞行器上的轻量级计算来确保安全飞行。

我们从概率的角度来制定规划问题。该方法并不寻求代价最低的路径（通常是最短路径），而是在确定下一步立即执行导航时，最大限度地提高到达目标的概率。

相关工作

搜索算法：

  • 基于图搜索：Dijkstra，A*，D*
  • 基于采样搜索：RRT和变种
  • 基于地图：PRM（Probabilistic Roadmap）,Voronoi graph，vector field
  • 基于置信度： linear‐quadratic controller with Gaussian models，extend RRT with particles on each node to handlethe uncertainty of terrain friction，model the edge costs on a graph with uncertaintiesfor graph search.

问题定义

定义 \(Q\in\mathbb{R}\) 作为车辆的配置空间。使得 \(A\in Q\) 为车辆当前位置同时 \(B\in Q\) 为目标点。车辆上装了感知传感器。定义 \(S\in Q\) 作为传感器感知覆盖，也就是传感器范围。障碍在 \(S\) 中是确定的，在 \(Q \setminus S\) 中可能已知。考虑车辆有多个从 \(A\) 起步开始移动的方向。把当前起步状态定义为 \(x_S\) 。显然不同的 \(x_S\) 选择会导致不同路线。定义 \(P_B(\cdot)\) 为最有可能从给定状态成功到 \(B\) 的概率，和状态 \(x_S\) 关联的概率为 \(P_B(x_S)\) 。规划问题可以定义为：

问题1。给定 \(A,B\in Q,S\subset Q\) ，障碍在 \(Q\) 里面，计算 起始状态 \(x^*_S\) 来最大化概率 \(P_B(x_S)\) ，

\[ x_S^* = \arg \max_{x_S}{P_B(x_S)} \tag{1} \]

以上问题在车辆沿着路径运动每一步都会解，即在导航的每时每刻到达 \(B\) 的最大概率。

方法

概率模型

piBHCYd.png
图2

\(\mathcal{F} \subset S\) 为传感器前沿

提出的方法最大化了从 \(A\) 行走到 \(B\) 的可能性。传感器范围 \(S\) 内的障碍可以通过传感器感知确切的得到。在 \(S\) 之外的如果有先验地图则可能知道。 \(S\) 需要根据车辆的运动模型展开，例如如果车辆可以横移。定义 \(x_f\) 为车辆穿过 \(\mathcal{F}\) 的时候的状态。 \(x_f\) 的条件分布为 \(x_x,p(x_f|x_S)\) ，能从感知传感器的障碍信息被推导出来。车辆从 \(x_f,P_B(x_f)\) 到达 \(B\) 点的概率密度能从先验地图的障碍中获得。我们有

\[ P_B(x_S) = \int P_B(x_f)p(x_f|x_S)\mathrm{d}x_f. \tag{2} \]

公式 \(P_B(x_S)\) 被重写为 \(P_B(x_S)\) 来表示概率密度。考虑 \(n\in \mathbb{Z}^+\) 次抽样 \(\xi_i, i = 1,2,\cdots,n\) ，摘自 \(p(x_f|x_S)\) 。根据蒙特卡洛抽样方法，我们可以得出

\[ \int P_B(x_f)p(x_f|x_S)\mathrm{d}x_f\approx_{n\uparrow\infty}\frac{1}{n}\sum_{i=1}^{n}{P_B(\xi_i)}. \tag{3} \]

结合以上两个公式，\(n\) 为常量：

\[ P_B(x_S)\approx\frac{1}{n}\sum_{i=1}^{n}{P_B(\xi_i)} \tag{4} \]

该公式表明在从 \(x_S,P_B(x_S)\) 导航到 \(B\) 时， 通过 \(n\gg1\) 次从条件分布 \(p(x_f|x_S)\) 中抽样，概率密度得到近似。需要注意的是通过抽样离散问题的方式给分布建模，使用有限的路径数量和体素可以解决该问题。

局部概率

给定起始状态 \(x_S\) ，车辆可以跟踪不同的路径达到传感器的前沿 \(\mathcal{F}\) 。把当前这一系列的路径同样叫做 \(x_S\) 。考虑一个 \(x_S\) 的离散模型。如图

piBxlLt.png
图3

最上面一行，有7组路径从上向下的视图呈现，其中 \(x_S\) 在路径起始曲线的左边或者右边。底下一行，有5组路径路径朝上或者朝下。考虑垂直和水平方向，例子中总共有 \(7\times 5=35\) 组路径。所有的路径都在 \(\mathcal{F}\) 结束。

每条路径都以三次样条曲线生成。路径在组里面被分为垂直和水平不同方向。图中，路径首先分割为35个方向(7水平和5垂直)同时每个又被分割为另外的35个方向。结果是每个组有 \(35^2=1225\) 条路径。考虑35个路径组，则总共有 \(35\times 1,225 = 42,875\) 条路径。图4显示了所有路径。

piDP0Og.png
图4

路径组里面的路径都可以认为是从 \(x_S\) 到 \(\mathcal{F}\) 可行的。最后路径的状态可以被视为 \(x_f\) 的抽样 \(\xi_i, i = 1,2,\cdots,n\) ，其中分布是根据 \(p(x_f|x_S)\) 。导航期间，感知传感器检测到的障碍会和确定的路径发生碰撞。图5给出了组内避障后的无碰撞路线。定义一个Boolean函数 \(c(\xi_i)\) 来表示路径净空：

\[ c(\xi_i)=\begin{cases} 1,\quad \xi_i 不被包括,\\ 0,\quad 其他情况 \end{cases} \tag{5} \]

计算 \(P_B(x_S)\) 公式变为：

\[ P_B(x_S)\approx\frac{\sum^{n}_{i=1}c(\xi_i)P_B(\xi_i)}{\sum_{i=1}^{n}c(\xi_i)} \tag{6} \]

等式适用于所有路径组，并选择 \(P_B(x_S)\) 最高的路径组 \(x_S^*\) 供车辆执行。

piDPw6S.png
图5

全局概率

我们的环境用体素表示。与传统的体素表示法不同，我们的体素包含位置和方向信息。如图6a所示，一个体素根据一个恒定的角度间隔被分为多个方向，用 \(\delta\) 表示。定义 \(x_j^k,j,k\in\mathbb{Z}\) 为体素的状态，其中 \(j\) 为体素索引，\(k\) 为方向索引。与 \(x_j^k\) 相关的位置被模拟为在体素内均匀分布，方向被模拟为在 \(x_j^k\) 方向周围 \([-\delta/2, \delta/2]\) 内均匀分布。从 \(x_j^k\) 到达 \(B\) 的概率密度记为 \(P_B(x_j^k)\)。

概率可在相邻体素之间传递。如图6b所示，考虑概率从邻近体素传递到 \(x_j^k\) 的情况，表示为 左下的 \(j_l\) 和右上的 \(j_r\) 。让 \(\theta_k\) 为和 \(x_j^k\) 关联的方向。由于位置被建模为体素内的均匀分布，被转移到 \(x_j^k\) 的概率从 \(j_l\) 和 \(j_r\) 的相对的 \(1-\tan\theta_k/2\) 和 \(\tan\theta_k/2\) 个体素。在这里，灰色区域是通过在 \(j_l\) 和 \(j_r\) 中画一条与 \(x_j^k\) 方向平行的线，连接体素 \(j\) 的左下顶点和右上顶点来确定的。从每个灰色区域开始，概率从三个相邻的方向传输。概率密度传输定义为

\[ P_B(x_j^k)=\left(\left( 1-\frac{\tan\theta_k}{2} \right)\alpha + \frac{\tan\theta_k}{2}b \right)r_j \tag{7} \]

其中

\[ \begin{align} a = w_yP_B\left(x_{j_l}^{k-1}\right) + w_fP_B\left(x_{j_l}^{k}\right) + w_yP_B\left(x_{j_l}^{k+1}\right)\\ b = w_yP_B\left(x_{j_r}^{k-1}\right) + w_fP_B\left(x_{j_r}^{k}\right) + w_yP_B\left(x_{j_r}^{k+1}\right) \end{align} \]

\(r_j\) 代表由于障碍体素 \(j\) 的可穿越性，其中 \(r_j = 1\) 意思是完全净空， \(r_j=0\) 代表完全碰撞。 \(w_f\) 和 \(w_y\) 相对地决定了超前运动运动和yaw转向的可能的分布。我们规定

\[ w_f + 2w_y = 1 \tag{8} \]

piDiHbQ.png
图6

三维情况是二维的扩展，在图6a中有多层，每层都和一个俯仰角关联。设 \(\alpha_l\) 为层的俯仰角 \(l,l\in\mathbb{Z}\) 。3D情况的体素状态为 \(x_j^{l,k}\) 。从邻近的体素概率密度的传输和2D是一样的。考虑俯仰角概率为

\[ P_B(x_j^{l,k}) = \left(\left( 1-\frac{|\tan\alpha_l|}{2}\left( \sin\theta_k + \cos\theta_k \right) \right) \left( \left ( 1-\frac{\tan\theta_k}{2} \right)c + \frac{\tan\theta_k}{2}d\right)+\frac{|\tan\alpha_l|}{2} \left( \sin\theta_k+\cos\theta_k\right)e\right)w_j^{l,k} \tag{9} \]

其中

\[ \begin{aligned} c = \space &w_{py}P_B\left(x_{jb}^{l-1,k-1}\right) + w_pP_B\left(x_{jb}^{l-1,k}\right) + w_{py}P_B\left(x_{jb}^{l-1,k+1}\right) \\ & + w_yP_B\left(x_{jb}^{l,k-1}\right) + w_fP_B\left(x_{jb}^{l,k}\right) + w_yP_B\left(x_{jb}^{l,k+1}\right) \\ & + w_{py}P_B\left(x_{jb}^{l+1,k-1}\right) + w_{p}P_B\left(x_{jb}^{l+1,k}\right) + w_{py}P_B\left(x_{jb}^{l+1,k+1}\right), \\ d = \space &w_{py}P_B\left(x_{jl}^{l-1,k-1}\right) + w_pP_B\left(x_{jl}^{l-1,k}\right) + w_{py}P_B\left(x_{jl}^{l-1,k+1}\right) \\ & + w_yP_B\left(x_{jl}^{l,k-1}\right) + w_fP_B\left(x_{jl}^{l,k}\right) + w_yP_B\left(x_{jl}^{l,k+1}\right) \\ & + w_{py}P_B\left(x_{jl}^{l+1,k-1}\right) + w_{p}P_B\left(x_{jl}^{l+1,k}\right) + w_{py}P_B\left(x_{jl}^{l+1,k+1}\right), \\ e = \space &w_{py}P_B\left(x_{jr}^{l-1,k-1}\right) + w_pP_B\left(x_{jr}^{l-1,k}\right) + w_{py}P_B\left(x_{jr}^{l-1,k+1}\right) \\ & + w_yP_B\left(x_{jr}^{l,k-1}\right) + w_fP_B\left(x_{jr}^{l,k}\right) + w_yP_B\left(x_{jr}^{l,k+1}\right) \\ & + w_{py}P_B\left(x_{jr}^{l+1,k-1}\right) + w_{p}P_B\left(x_{jr}^{l+1,k}\right) + w_{py}P_B\left(x_{jr}^{l+1,k+1}\right), \end{aligned} \]

这里，\(w_j^{lk}\) 表示 \(x_j^{lk}\) 因障碍物而产生的可穿越性。\(w_p\) 是与俯仰转弯相对应的权重，用于从 \(P_B(x_{j_*}^{l-1,k})\) 和 \(P_B(x_{j_*}^{l+1,k})\) 到 \(P_B(x^{l,k})\) 的概率传输，其中 \(*\) 代表 \(b,l\) 或 \(r\) 。 \(w_{py}\) 是与俯仰和偏航转向相对应的权重，用于 \(P_B(x_{j_*}^{l-1,k-1}),P_B(x_{j_*}^{l-1,k+1}),P_B(x_{j_*}^{l+1,k-1})和P_B(x_{j_*}^{l+1,k+1})\) 到 \(P_B(x^{l,k})\) 的概率传输。同样，我们要求在传输过程中没有概率损失，即

\[ w_f + 2w_p + 2w_y +4w_{pw}=1 \tag{10} \]

如果 \(\alpha_l<0\) ，则体素 \(j_b\) 将被体素 \(j\) 上方的体素（即体素 \(j_a\) ）取代。有两种特殊情况。首先，如果 \(\alpha_l=0\) ，则车辆水平移动。在三维情况下，概率会从体素 \(j_l\) 和 \(j_r\) 传递到体素 \(j\) ，但不会从体素 \(j_a\) 或 \(j_b\) 传递到体素 \(j\) 。其次，如果 \(\theta_k=0\) ，车辆平行移动到体素 \(j_r\) 。在二维和三维情况下，概率从体素 \(j_l\) 而不是体素 \(j_r\) 传播到体素 \(j\) 。在初始化过程中，概率密度均匀分布在包含 \(B\) 的体素的所有方向上。图7给出了在二维环境中传播概率密度的示例。

piDruZV.png
图7

方法实现

路径组是离线生成的。我们使用体素网格和传感器范围 \(S\) 来做碰撞检测。体素和路径之间的对应关系预先建立并存储在邻接列表中。在邻接列表中，每一行都与一个体素相关联，并由被位于体素中心的障碍物遮挡的路径索引组成。系统启动时，路径和邻接列表会被加载到车辆计算机内存中。在线碰撞检查会处理所有感知传感器数据点，并根据邻接表标出相应的闭塞路径。然后，算法遍历每组中的所有路径，根据公式6计算 \(P_B(x_S)\) ，并选择 \(P_B(x_S)\) 最高的路径组。算法返回 \(x_S^*\) 为

\[ x_S^*=\arg \max P_B(x_S) \tag{11} \]

\(n\) 是路径组中的路径数。设 \(h\in\mathbb{Z}^+\) 为路径组的个数，\(m\in\mathbb{Z}^+\) 为感知传感器数据点的个数。在线处理算法如算法1所述

\[ \begin{aligned} \hline \\ &\large{\textbf{算法3}：计算探索路径} \\ \hline \\ &\textbf{输入：} 路径组，邻接列表，\text{2D}的P_B(x_j^k)或\text{3D}的P_B(x_j^{l,k}),i,k,l\in\mathbb{Z} \\ &\textbf{输出：} x_s^*或者没找到路径 \\ &\textbf{begin} \\ &\qquad 初始化所有路径为未碰撞 &//\space\mathcal{O}(nh) \\ &\qquad \textbf{for}\space \text{each} \space 感知到的传感器数据点 \space \textbf{do} &//\space\mathcal{O}(mnh) \\ &\qquad \qquad 使用邻接列表把响应的路径标记为碰撞 &//\space\mathcal{O}(nh) \\ &\qquad \textbf{end} \\ &\qquad \textbf{if}\space 不是所有的路径都碰撞\space \textbf{then} &//\space\mathcal{O}(1) \\ &\qquad \qquad \textbf{for}\space \text{each} \space 路径组 \space \textbf{do} &//\space\mathcal{O}(nh) \\ &\qquad \qquad \qquad 根据公式6计算P_B(x_S) &//\space\mathcal{O}(n) \\ &\qquad \qquad \textbf{end} \\ &\qquad \qquad 找到最大P_B(x_S)的路径组并返回x_S^* &//\space\mathcal{O}(h) \\ &\qquad \textbf{end} \\ &\qquad \textbf{else} \\ &\qquad 返回未找到路径 &//\space\mathcal{O}(1) \\ &\qquad \textbf{end} \\ &\textbf{end} \\ \hline \\ \end{aligned} \]

定理1. 在线处理算法的计算复杂度为 \(\mathcal{O}(mnh)\)

定理1分析了最坏情况下的计算复杂度，即每个感知传感器数据点都会阻塞每组中的所有路径。在实践中，一个数据点可能只阻塞几条路径，因此计算量要小得多。全局范围内的概率传播使用覆盖环境的第二个体素网格，只在导航前运行一次。该算法实现和A*算法有些相似，其中只更新与开放集中相邻体素的概率密度。如果包含 \(A\) 的体素中概率密度的变化小于阈值，则该过程终止。

在没有先验地图的情况下，在公式6中我们也可以使用启发式函数来计算 \(P_B(\xi_i)\)：

\[ P_B(\xi_i)=\begin{cases} -|\Delta y_i|, &\text{2D条件},\\ -|\Delta p_i\Delta y_i|, &\text{3D条件}, \tag{12} \\ \end{cases} \]

其中 \(\Delta p_i\) 和 \(\Delta y_i\) 为 \(\xi_i\) 和目标方向的相对的俯仰角和方位角