[运动规划算法]基于似然场的快速避障算法

---

一、简介

这是一种在复杂环境中实现快速自主飞行的规划方法。通常，在复杂环境中进行自主导航需要在栅格地图上进行连续搜索，但是导航过程中通过传感器数据来对图进行更新的计算代价是昂贵的，路径规划也是一样，特别是要求搜索出的路径必须是运动学上可行的。我们建议尽量避免在线搜索来减少计算的复杂度。

首先，要对环境进行建模，使障碍物在传感器探测范围内确定性已知，传感器范围以外概率已知，该方法并非寻找代价最低的路径，而是最大化确定能成功导航到下一个目标的可能性。

该方法支持使用或者不使用先验地图的两种配置。两种配置均可用于运动规划。后者可以允许设定特定目标点或者人工指引来进行导航。

在实验中，它使轻型无人机能够在复杂的森林环境中以10m/s的速度飞行。

---

二、原理

1. 问题描述

定义: $Q\subset R$表示为车辆的配置空间，$A\subset Q$表示车辆的当前位置，$B\subset Q$表示目标点的位置，起始状态表示$X_s$，到达目标点B的概率表示为$P_B(.)$。

整个规划问题可以描述为：
给定$A、B\in Q$，$S\subset Q$及在$Q$中的障碍物，通过初始状态$X_s$来最大化概率$P_B(X_s)$。

---

2. 概率模型

根据问题定义中所述，传感器范围$S$内的障碍物被确定认为是已知的。如果有地图可用，则S以外的障碍物被认为是概率已知的。否则，则认为是没有先验障碍。

将$S$表示为灰色区域。$F\subset S$为传感器边界（红色实线表示）。给定初始状态$X_s$，A和B点连接的路径为黑色曲线。对于所有可能的路径，它们必定与$F$相交。

定义车辆通过F时的状态为$X_f$。给定为$X_f$和$P(X_F|X_s)$的条件分布可以从传感器提供的障碍信息中得出。此外，可以从先验地图上的障碍物获得车辆从$X_f$到达B的概率密度$\rho B(X_f)$。

车辆从$X_f$到达B的概率密度$\rho B(X_f)$可以表示为：

根据蒙特卡洛抽样理论，可以建立：

合并公式（2）和（3）以及假定n是一个常数可得：

---

3. 局部概率

给定了初始状态$X_s$，车辆可以沿着不同的路径到达传感器边界$F$。

图中所有路径均由三次样条曲线生成，路径首先在35个方向（水平7个，垂直5个方向）上分割，每个路径又在35个方向上分割。这导致一组中的$35^2$= 1225条路径。考虑35个路径组，总共有35×1225 = 42,875条路径。

所有路径是根据车辆运动的约束生成的，组中的所有路径都可视为从$X_s$到$F$的可行路径。
可以将路径末端的状态视为$X_f$的样本${\xi }_i$, i =1、2、…n ，其中的$X_f$分布是从$P(X_f|X_s)$得出的。在导航过程中，障碍物会被传感器遮挡住某些路径。图中给出了一个障碍物示例以及组中相应的无碰撞路径。 定义一个布尔函数表示路径的间隙：

基于公式（4）可以计算出$P_B(X_s)$:

将公式（6）应用于所有的路径组，并选择最高$P_B(X_s)$的$X_s^{\ast }$来执行控制车辆。

---

4. 全局概率

环境是以体素网格来表示。与传统体素不同的是，我们的体素同时包含位置和方向信息。

如图(a)所示，基于恒定的角度间隔将体素分为多个方向，以$\delta$表示。角度间隔设置了车辆穿过体素的方向。 用$X_j^k,j，k$∈Z 表示体素的状态，其中$j$是体素指数，$k$是方向指数。 与$X_j^k$相关的位置被建模在体素内，并且将该方向建模为围绕$X_j^k$方向均匀分布在[− $\delta /2\delta /2$]内。 从$X_j^k$到达B的概率密度表示为$\rho B(X_j^k)$。

概率可在相邻体素之间传输。考虑概率是从相邻体素传输到$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的左下和右上顶点。 从每个灰色区域，从三个相邻方向传输概率概率密度传输定义为：


$r_j$表示由于障碍而引起的体素$j$的可穿越性，其中$r_j$ = 1表示完全清除，$r_j$ = 0表示完全闭塞。

$w_f$和$w_y$分别确定对应于向后运动和偏航调整的概率分布。

三维情况的扩展，与二维情况类似。
概率密度计算为:

其中：

$w_j^{l,k}$表示$x_j^{l,k}$障碍物的可遍历性。

$w_p$表示概率传输${\rho }_B(x_{j\ast }^{l-1,k})$和${\rho }_B(x_{j\ast }^{l+1,k})$到${\rho }_B(x^{l,k})$的转弯权重。

要求在传输过程中不损失任何概率：

初始化期间，概率密度是均匀分布在包含B的体素中的所有方向上。概率的传播是通过迭代进行的。如图给出了2D环境中传播的概率密度演示。体素越亮表示概率密度越高。

---

5. 方法实现

路径组是离线生成的。我们使用传感器覆盖范围$S$生成体素网格来做碰撞检测。

体素和路径之间的对应关系是预先建立，并存储在邻接表中。在邻接表中，每一行都与一个体素相关联，并由路径的索引组成，这些路径被放置在体素中心的障碍物所遮挡。接着，要考虑车辆半径来计算遮挡。

系统启动后，路径和邻接表将加载到机载电脑中。在线碰撞检测会处理所有感知数据，并根据邻接表标记要遮挡的相应路径。然后，该算法会遍历每组中的所有路径，并基于（6）计来算$P_B(X_s)$，并选择最高$P_B(X_s)$值的路径组。 该算法返回$x_{\ast }^s$：

---

三、项目演示

差速机器人模型的仿真

[运动规划算法]基于似然场的快速避障算法（1）

加入A*算法作全局规划

[运动规划算法]基于似然场的快速避障算法（2）

---

参考

【1】Ji Zhang Chen Hu Rushat Gupta Chadha Sanjiv Singh，Falco: Fast likelihood‐based collision avoidance with extension to human‐guided navigation.

---