PX4飞控Avoidance功能包2016论文分享

原文为《Risk-based Pathfinding for drones》，PX4官网上可以找到，本文对其中部分内容进行翻译，并将原文截图展示，欢迎同行交流。

基于风险的无人机路径寻找

摘要

这篇硕士论文主要研究了在图中同时最小化距离和风险的问题。在基于风险的搜索中，很少有关于使用期望值进行搜索的文章以及如何与传统风险定义比较，但我们将证明，通过使用期望值，可以在不牺牲解决方案质量的情况下，轻松地将风险引入无人机寻路。同时我们讲展示如何使用一个简单的算法保证最大程度的平滑度，然后，这些结果被用来为带有立体摄像机的无人机提供一个最小化时间、能源使用和风险的完整模型，使用OctoMap库来表示环境中的置信度。最后利用Gazebo仿真器对模型进行了仿真，并对不同的仿真策略进行了定量和可视化比较。

简介

路径寻找是一个研究最小化起始顶点到目标定点的路径花费的数学问题。传统意义上，路径寻找分为两个部分，全局规划和局部规划。全局规划寻找一个通往目标的高级路径，而局部规划将这条路径转换成速度指令，发送给机器人的电机。为了机器人寻路，已经开发了许多算法，它们在基本方法上是不同的。有些是在离散网格(discrete grids)上，有些是在连续空间(continuous environment)。本文我们使用基于网格的方法。

相关工作

风险

最安全路径

使用一个对数变换，将最安全路径问题转换成最短距离问题，然后用Dijkstra算法求解。（我的理解就是安全问题往往是很多的概率的加或乘运算，而在对数下可以转换成加法等，后文给出算法详细过程，这里原文一笔带过）。

结合距离和风险

多目标最优路径寻找多目标优化路径是同时优化多个目标函数的路径问题。
最小化成本信任比例最小化成本信任比例(Minimum Cost Reliability Ratio, MCRR)是寻找路径花费与最大通过路径概率之间比例最小值的问题。Greytak和Hoverz给出了A*算法，但是他们的文章中没有给出运行时间的边界，在最坏的情况下，它的时间是指数级的。Katoh给出了一个多项式逼近方案，Aboutahoun给出了伪多项式算法来解决MCRR。
其他代价函数 Jun扩展了最安全路径算法来惩罚距离，同时保持与Dijkstra算法同样的时间复杂度。Kothari给出了快速搜索随机数(RRT)算法，成本函数由期望值组成，而不是纯概率，这使得计算更容易。

风险有界的最短路径

除了同时最小化距离和风险，有约束的最短路径问题(Constrained Shortest Path Problem, CSP)针对最小化距离，并保证整个风险低于一个给定阈值。CSP算法是一个NP-hard问题，Ono和Williams提出了一个算法给出次优解，保证任务以一个高于阈值的概率完成。

在部分可观测的环境中的路径寻找

另外一种风险是图的一部分未知，也就是除了一些已知失败概率的边之外，还有一些边可能存在，他们的情况只有到了其邻接节点才能知道。这是旅行商问题，也是一个NP-hard问题。有人提出了基于启发式来决定何时何地去感知环境来最小化总成本。

路径寻找

最短路径

Dijkstra算法

求图的最短路径是图论的核心问题之一，主要目的是最小化各边的权重。Dijkstra算法的计算复杂度为O(V^2)，经过提升之后可以达到O(nlogn)，该算法的详细介绍网上资源很多，在这不做过多介绍。


3.2中放映了这三种算法的关系，其实A*算法就是Dijkstra和贪婪算法（启发式）的结合。

A*算法

Dijkstra算法在计算路径代价时，没有一个尝试靠近目标点的趋势，而是一圈一圈的算，直到把目标点包括进去为止，这就是为什么图3.2a中的搜索区域为什么远大于3.2c中的A*。A算法在网上教程也很多。

A算法的快速归因于启发函数h(v)，一般来说这个函数设置为当前位置与目标点的距离（欧式或者曼哈顿）。

最安全路径

一条路径的安全程度是关于风险的概率公式，这个概率表示起来就是公式3.2，这和之前使用对数转换有点呼应了。

转换成Dijkstra算法

风险的成本函数是一些边权值的乘积，而不是和，因此不能直接使用Dijkstra算法。我们使用一个对数转换（公式3.3和3.4）。


Log函数单调递增，求解起来意义很明显。

最优路径

图3.4是结合路径和概率最小化的一个实例，代价函数由自己给定，也就是两个部分的权重。每条边有两个权重，长度和风险，我们想同时找到二者的最小值。

选择代价函数

Pareto-optimality
简单说就是找长度和风险二者的最优解，就是找到一条路同时满足二者最优，这同样是个NP-hard问题。
最小化“风险信任”比

公式3.9表示的思路就是花小成本干成功可能性大的事情。虽然这也是个NP问题，但是在多项式时间内近似它是可能的，并且对于整形输入，一个伪多项式算法存在。接下来文章说到MCRR问题多用于处理网络的通讯问题而不是无人机路径，之后举例二者的不同之处。
Tailor Fitted Cost Function
将3.9的分子对数转换来方便求解。

概率 vs 期望值

考虑风险和距离的另一种方法是使用失败的期望次数而不是一次或者多次失败的概率。期望值的定义为一种加法，


然后将代价函数表示成为一个距离和风险线性组合，之后同样可以引入Dijkstra算法。

联合边界、期望作为近似

这段说明使用期望值来代表风险和使用概率代表风险的区别和联系，原文公式3.17-3.19给出二者的定义，之后推算了之间的关系，看公式不懂可以举个例子，三个节点A，B，C，两条路AB，BC，再给一个风险，按照概率和期望值分别计算一些二者的风险，会发现在风险小的时候，二者差值不大，并且期望值得到的结果更加保守，也就是期望值给出的风险结果大于基于概率的，虽然在风险大的时候，二者差有点大，但是基于期望值的方法仍是单调递增的，也能反映出趋势，同时风险值在这个值的时候，路径基本就被淘汰了，因为无人机是不敢冒着这么大的风险来机动。

探索的风险

之前说到，环境只有无人机感知过的地方才能认为是已知的，也就是边有权重，知道顶点的位置。目前，我们定义的风险是由于机动和感知的不完善导致的坠机。也就是说一些边有固定已知的风险，另外一些被阻塞的边也存在风险。

由于vt之间路径不可知，因此我们有两种策略从s到t，期望花费为

这个问题是旅行商问题的变种，和NP-hard问题一样很难有找到最优解。

最平滑路径

代价函数除了长度和风险之外，拥有一个平滑路径对无人机也是重要的。不仅好的路径可以减少时间和碰撞的风险，同时便于直观的理解。文中提到转弯的代价函数设置时可以认为是非线性的，也就是转90度的代价大于两倍转45度的代价。
传感器可能因为急转弯进入一个未知环境，比如对一个FOV为90度（正负45度）的相机来说，在这个范围内做机动会很快，但是目标点设置在这个范围之外，就要采取减速或者停止来重新规划。

设置转弯的代价函数时，我们可以将转弯离散成45,90,135,180来分别给定代价函数（这个东西其实不好量化，在设置的时候反映出代价不同就可以了）。
在Dijkstra算法中寻找平滑路径
Dijkstra算法的一个性质：从s到t的路径经过顶点w的代价，等于从s到w和从w到t的代价之和。当然这是将飞行器看做一个质点时。不过在无人机飞行过程中会有旋转(rotation)代价，图3.12给出一个示例。

同样是s到w的两条路，红线不仅转弯总和少于蓝色，并且路径总和也小，但是在s到t的路径规划中，算法还是优先选择了蓝色。
定义路径P的平滑度$S(P)$，从s到t的平滑度可以写成公式3.28.

但是在Dijkstra算法中很难表示在顶点w处的转动代价，我们需要一个类似3.29形式的公式才能套Dijkstra算法，也就是想办法令$angle(p_{s,w},p_{w,t})=0$。所以给出一个方法让一个顶点(vertex)来表示一个位置(position)和方向(direction)的元组(tuple)。定义从$G$衍生出来的一个$G^{\prime }$，后者每个顶点有8个方向，效果如图3.13.

这种方法带来的问题是每个节点的信息量相较于2D平面是原来的十倍，计算量的增长远不止这些，因此Dijkstra算法的计算复杂度在实际应用中要有所考量了。此时A*算法启发式的优点得以体现。

动态搜索算法

无人机在飞行过程中多次反复执行路径搜寻算法，当机器人靠近目标时，它会获得周围环境的信息，而这些信息可能会使它当前的路径失效。动态算法可以避免在多个图差异不大的时候反复计算最短路径。传感器倾向于测量无人机位置周围的区域，因此算法可以在靠近终点s时，图有改变的情况下使用，也就是从t到s建立的搜索树，多数时候有效，而在s附近时可能会出现一些修改。图3.14。

左图表示在当前位置计算出的两条路径中，上面的路径代价函数小，因此优先选择上面，但是在行进过程中发现了一个新的障碍，因此搜索树变更为之前下面的路径去抵达目标点。但是动态搜索有明显缺点，经过之前的图转换，一个单量测，要十个更新。并且代价函数要经常调优参数，参数的更改会影响整个搜索树。为了将其合并到动态算法中，需要跟踪多个树，并再次乘以每次测量所需的更新数量。

双向搜索

在s到t搜索时，同时进行t到s的搜索。将一个长搜索变为两个短搜索，图3.15展示一个例子，每个搜索从根开始直至两树汇合。

顶点与距离呈二次方关系，对于四维空间，Dijkstra算法的双向版本可以达到原来的8倍之快。不过值得注意的是，其中两个维度(高度和方向)是有限的，这降低了潜在的加速速度。虽然Dijkstra算法倾向于在s周围建立一个扩展顶点的球体，但对于A*却不是这样。通过使用高估的启发式来加速搜索，很难说双向搜索是否会显著提高性能。

仿真环境

软件组成

ROS

略

Gazebo

配合ROS使用的3D仿真环境，它可以使用多种传感器，并且将量测信息通过点云的方式发布。

PX4

一个low-level控制器，通过mavros和mavlink与ROS连接。

OctoMap

octomap建立了一个3D概率占有格地图，ros节点名字为octomap_server。数据结构是一个八叉树octree，根据占有概率进行生长，网上有很多资料，数学原理略（反正就是根据点云对每块区域是否被占用有一个概率，然后来表征周围环境）。

交互

ROS中的交互是通过节点(Node)，贴张图，不多解释。

应用

代价函数

我们准备使用三个标准进行最小化，时间、能量、风险。但是上文只提到最小化路径长度，曲折度，风险。但是穿越时间、能量消耗与路径长度、曲折度等价表示。

距离

路径长度以相似的方式同时时间和能量，我们假设水平面单位距离的移动花费相同的代价，升降代价大于水平移动代价。

水平移动一米的时间和能量，以及时间和能量的单位消耗。至此，距离的代价转换成为时间和能量。

平滑度

曲折度定义成为相邻节点的角度代价之和，先确定每两个顶点之间的夹角，然后对应找到这个夹角的时间和能量代价，然后加和得到总代价。为了惩罚急转弯，我们可以将90度转弯的代价设置成为45度转弯代价的4倍，这样就可以将一个90度转弯转换成两个45的转弯。

风险

风险是代价函数中最难定义的部分，因为octomap只能给出每个栅格被占用的概率，而不能给出一条路径安全通过的概率。风险要通过环境来分析并且根据实际情况进行设置。如果之前用过movebase功能包的话，里面是对占用栅格地图进行一些膨胀操作，来为基于航路点的导航留出充分的安全空间。

octomap占用概率
地图提供了一个体素被障碍物占用的概率，为了安全起见，被占用的栅格在代价函数中被定义为坠毁。
octomap邻居风险
传感器的精度影响着无人机对周围环境的判断，邻居风险定义为某个体素的相邻体素的风险和。
未探索区域的风险
根据之前的讨论，未探索的区域和已知区域的风险不同。所以，穿越一个未探索的区域来规划一个新路径的代价会很高。为了对这个风险有一个实用的估计，我们根据高度来设置一个先验知识。在不用的高度被占据的风险不同，如图5.1.

启发式

A*算法优势在于启发函数的设置，启发函数也应该反应出从当前点到目标点的距离、曲折度、风险。

距离

“直线距离”是最流行的寻路启发式，但有几种方法可以定义距离的下界。欧几里得距离可以使用，但它不能反映网格上的路径距离，而且由于它的平方根，计算起来比较困难。曼哈顿距离很容易计算，但它不是下界，因为它不考虑对角线移动。要得到对角线移动路径的紧下界，可以使用以下公式5.5

平滑度

以45度作为一个单位来衡量旋转量，计算方式和之前差不多，简略。

风险

在分析了风险启发函数之后，原文采用了一种最常见情况的启发函数，选择一条路通过完全未知的空间。原文是"a path through completely unknown space, that is, the heuristics for the cost of risk will be the cost of risk for the shortest path where every voxel is unknown."，因此，某一顶点的风险启发可以定义成式5.7。

在5.7中，前一项为这个顶点所在高度被占用的先验概率，后一项为距离。举个例子，如果前面被挡住，你可以选择向上飞过，或者向下飞过，可能有大风险短距离和小风险长距离的权衡。正如原文所说“因为目标倾向于在低空环境，因此算法会寻找其他方案直到无法避开这个高风险”。然后，启发函数将t的风险加入，对比A*算法中的$H=F+G$，risk是基于之前代价函数算出的运行t的风险，就是从起点到当前评估点的代价，这个代价相比于从评估点到顶点u的风险来说，是更加准确的，往往在传感器的感知范围内，在感知范围外也没什么意义去计算它。

调优

$\alpha$是启发函数中的过高估计因素(overestimation factor)。

具体解释没太看懂，不过我的理解就是在不同的顶点u，对距离和曲折度的权重不同，可能比较远的地方我就不关心曲折度（也许都不知道是什么情况，也没法估计），在相对比较近的地方就降低前者权重来考量当前顶点的代价。（这个理解是结合下小节得出的）。

流程

因为整个算法需要多次迭代，所以重要的是要有某种方法来调整算法，使其能够快速找到路径，即使它可能远远不是最优的。为了做到这一点，该算法运行搜索几次，并降低了高估因子，但总的迭代次数有限。由于过高估计只是过高估计了距离启发式，较大的$\alpha$通常忽略了弯曲度的代价，在这种情况下，做平滑变换（第三章）是没有意义的，因为它只会减慢搜索速度。前几个搜索使用一个名为search3D()的函数，该函数只考虑距离和风险。每次搜索，$\alpha$都会变小，得到更好的解。当$\alpha <2$时，函数search4D()被调用，它也考虑平滑度。如果在找到任何解之前时间已经耗尽，则在最大高度调用二维搜索。这是因为二维搜索要快得多，而且只有在极少数情况下，在高海拔地区是不可能找到路径的。下面是伪代码。

结果

这部分可以亲自在仿真中尝试，配置起来也不是很麻烦，按照步骤做就可以。不得不说，PX4团队还是很良心的。

测试设置

略

不同策略的对比

二维与三维搜索对比

2D搜索只在2米高度，3D在10米高度以内。

有风险的搜索

在成本函数中添加风险后，立即将失败率降低到原始3D算法的四分之一左右。从图中可以看出，遍历时间略有增加，但这是意料之中的，因为算法不再寻找最短路径。

飞行高度

在我们的例子中，先验的飞行速度随着高度的增加而降低，因此在高海拔飞行被认为比接近地面飞行更安全。但是由于上升的高度需要消耗大量的能量，所以在通过增加高度来降低风险和通过保持当前的高度来减少能量消耗之间存在权衡。是否值得把精力花在提高海拔上取决于目标有多远。这是假设无人机从地面以上1米开始飞行，没有关于世界的信息。这使得无人机在执行任务的早期就能花能量越过障碍物，因为它知道从长远来看，更高的海拔是有益的。高度先验降低了遍历时间和故障率，同时略微增加了能量消耗，从而整体降低了成本。这是因为先验提供了关于哪些高度更可能有好的路径的信息，并且早期到达高海拔可以节省大量时间。

平滑度

通过增加路径的平滑度，降低了遍历时间和总成本。正如预期的那样，优化平滑度也证明可以显著增加迭代次数。

不同策略的视觉对比

略。

启发

过估计的启发

回想一下，A可以被看作是Dijkstra算法和Best- irst search的混合，并且只要启发式不被高估，就可以保证得到最优解。由于启发式估计过高，A更像Best-first搜索，贪婪地扩展t附近的顶点。这消除了最优性的保证，可以大大减少扩展顶点的数量，从而加快算法的速度。

风险启发

不同的过估计因子的代价。

双向搜索

略。

讨论

未来工作

1. 受特定环境的影响，风险建模的方式很多。例如，只有一个前置摄像头的无人机可能会发现水平移动比垂直移动更安全。探索的成本和收益也是一个悬而未决的问题。高度先验提供了一种很好的方法来估计获得高度的收益，但是需要提前知道高度先验。一个评估在当前环境中找到路径有多难的自适应模型可能是一个有趣的扩展。
2. 基于之前搜索过的信息的动态搜索也可能是有益的。目前，由于需要大量的计算，一种始终跟踪最短路径树的完全动态算法是不可行的，但是通过以定期更新或使用旧路径作为启发式，可以减少总工作量。通过连续运行新路径请求和新解决方案之间的延迟可以减少，从而允许算法在同一时间内找到更好的路径。

总结

我们提出了一种有效地在三维空间中寻找路径的算法，该算法在最小化距离和风险的同时，最大限度地提高了路径的平稳性。使用预期的失败数量被证明是将风险纳入寻路的好方法，而不会使问题变得难以计算。通过选择路径长度和期望故障次数为代价函数，可以用Dijkstra算法或$A\ast$求解。这也使得我们可以在不牺牲算法效率的前提下，将平滑性引入到代价函数中。我们还展示了如何将时间和能量方面的成本函数转换成距离和平滑度，然后用$A\ast$对其进行优化，使用线性成本函数可以将不同的标准联系在一起。风险是根据从传感器获得的占用率和依赖于飞行高度的先验概率来定义的。