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

PX4官方Avoidance-2018论文

PX4团队的规避功能包目前支持两种方案，一种是global_planner，另外一种是所谓的local_planner。并且一直在更新维护，官网上说最近维护的重点是local_planner。2014年的一篇在以前的博客中分享过，然后前段时间发现新的参考文献出来了，就是这篇2018年的硕士论文。之所以目前团队主攻local，我觉得是因为这个算法实在是对计算量要求太小了，连之前的Octomap都没有使用，算法都基于点云。实物飞行实验在Intel RTF这个小飞机上就完全实现了，用过这个飞机的人应该知道它的处理器能力比较弱（相比于NUC、TX2等）。按照老规矩，如果有翻译和理解错误的地方欢迎指正~

原文题目《Obstacle Avoidance for Drones Using a 3DVFH* Algorithm》
PX4官方的Git上可以直接下载查看。

引言（略）

无人机应用多，规避算法有很大的空间去提高。

摘要

这篇文章主要介绍了基于 $3DVFH*$的无人机实时障碍规避算法，该算法结合了前面提出的3DVFH+和 $VFH*$算法的思想，采用了一种新的记忆(memory)策略。 $3DVFH*$算法以纯反应性(purely reactive)的方式计算避障动作，不需要构建环境的全球地图。记忆策略通过将以前的极坐标直方图传播到当前位置来跟踪以前看到的障碍物。为了提高无人机的鲁棒性，并使其在无人机上的应用成为可能，实现了地面探测和安全机制等多种功能。 $3DVFH*$算法已被证明能够有效地避免复杂仿真场景中的障碍物，显示出前瞻性的能力。地面检测算法被展现出能与地面保持最小距离的能力。
简单说就是：基于点云，采取了一种新的记忆策略来表示障碍物。3D、VFH、 $A*$是算法的三个组成部分。然后为了保证飞行安全，算法开发了一些新特性。

简介

障碍规避分两大类：全局障碍规避和局部障碍规避。全局障碍规避需要一个实时环境地图，基于这个地图，使用路径搜寻算法可以找到一条通向终点的路。但是这些算法对于小型无人机的计算能力来说有点难以接受。另一类是局部或反应性规避方法，他们不需要创建地图，而是基于当前传感器得到的数据来进行机动，这样通常难以找到最优路径，然而，这些算法足够快，能在小无人机上运行。VFH是一个常用的局部障碍规避算法， $VFH*$是其变种，它建立了一个前瞻树，将局部算法和 $A*$结合在一起，目前只应用在2D场景中，我们将其扩展到3D环境中。

相关工作

略

全局障碍规避

大多数全局避障算法要么访问环境地图，要么根据获取的传感器数据构建地图。（地图已知或者未知，前者应用就是人开始带着无人机对环境建图，然后自主导航；后者就是执行未知环境的探索任务）。Heng提出了将双目点云构建成Octomap后使用Astar进行搜索。Fraundorfer使用无人机进行了未知环境中地自主建图与探索，基于双目相机实现SLAM，边界算法决定目标，VFH算法进行避障。Astar部分略掉，DWA略掉。
还有一些使用EKF在动态感知下跟踪不同障碍物的研究，最终路径是使用动态规划(DP)算法计算，或者给定障碍物周围一个锥体，限制飞行器的速度矢量在区域外面。还有两步规划法，先规划一个粗糙的路径，然后针对独立的障碍进行安全距离的保持。
另一种建立搜索树的方法是利用概率性质，希望降低算法的计算成本。这部分有点RRT的思路，略掉。
还有一部分基于最优化技术的方法:混合整型规划来计算平滑轨迹，模型预测控制(MPC)。

局部障碍规避

VFH及其变种的历史，略掉。
提到基于势场的方法，指出容易陷入局部最小值的缺陷。
剩下新奇的算法好多我也不了解，略掉。

对本文启发

1. 总结前人工作，能成功实现的算法有：MPC，octomap+Astar，机器学习，势场法。多数应用环境为2D，构建一个前瞻树，还没有扩展到3D环境中。因此，直方图方法在无人机飞行中具有巨大的潜力。
2. 本文的研究目的是将VFH*算法与3DVFH算法相结合，并对其进行了扩展，增加了一些额外的功能，以实现具有可靠避障能力。

方法

$3DVFH$算法

算法基础部分不赘述，开始的时候作者一直在强调全局有其优点，但是计算开销一直很大，因此本文直接基于点云来实现算法。

接下来文章简单介绍了3D的直方图相关信息，计算点云中每个3D点与当前无人机位置的俯仰角和航向角（蓝黄交点）。然后将直方图二值化来代表前方环境信息，然后在给定的无人机动力学约束下来选择合适路径。选择路径过程中使用了代价函数，常规代价函数由三部分组成（反正就是对路径一个综合刻画），无人机每次选择代价函数最低的路径进行机动。如果在没有障碍物存在的情况下，直方图仅由未占用的单元组成，此时无人机的速度可以变快，同时将航路点指向目标，不需要经过直方图进行计算。

建立记忆

写在前面：其实我觉得全局和局部路径规划算法可以理解成为算法记忆能力的区分，全局算法的记忆时间长，经过地方的路况一直都可以调用；局部算法记忆时间短，我仅考虑当前某段时间内的环境状况。
回到原文，所实现的3DVFH算法是纯局部和反应性的，这意味着该算法只能访问当前时间步长的数据，并且是无人机附近的。局部回避策略的最大缺点之一是，它们不考虑来自以前时间步骤的任何数据或操作。这常常导致不稳定的行为和局部极小值。一个方向的移动会导致另一个方向的移动是有利的，因为之前的数据会被丢弃，而无人机对之前看到的障碍物一无所知。特别是沿着更大的障碍物，如墙壁，因此不可能使用局部VFH方法。这种记忆是根据3DVFH算法的极坐标直方图原理构建的，它允许记忆障碍物而不需要存储大量的数据集。方法为以下四步：

1. 上一时刻的（组合）直方图被重投影为3D点云；
2. 将1中的点云以原分辨率的一半建立出一个记忆直方图。然后将这个记忆直方图上采样变成原分辨率；
3. 根据当前的深度数据计算当前时刻的直方图；
4. 最后，将新的直方图和记忆直方图融合成当前时间的组合直方图。
   这个组合直方图将被用来确定可能的正确方向，并在障碍物周围导航。在下面的段落中，将更详细地解释组合直方图的各个步骤。

将直方图重投影到3D点

记忆方法使用最近一个时刻的数据来增强当前时刻构建的直方图。被存储下来的，旧的数据，是直方图格式。旧直方图是之前时刻障碍的极（坐标）表示。由于障碍物的角度方向取决于无人机的位置，所以旧的直方图与创建它的旧位置有关。为了在当前时刻使用旧直方图的数据来计算，我们需要将旧直方图转换成一个在当前位置的直方图。（同样的障碍物信息在前后时刻两帧直方图中的表示。）这可以通过将旧直方图中被占用的细胞重新投影到3D点上，然后从这些3D点上构建一个记忆直方图来实现。为了得到重投影的点，将扫描旧的直方图以寻找被占用的单元格。每一个被占用的格子通过俯仰角和航向角来表示。除了这两个角度之外，距离信息被储存在距离层，年龄存储在年龄层。如果被占用的格子的年龄没有超过预设限制，这个单元格被重投影到四个与单元格角对应的3D点，图3.3。

每个角点的俯仰角和航向角可以通过分辨率 $\alpha$来计算。

从3D点建立直方图

为了使用一个直方图表征环境，3D点首先需要转换成一个2D原始极坐标直方图，其中每个cell保存了在对应扇形区域内的点数。然后是一些直角坐标和极坐标的转换，不赘述，贴图。

“从3D点建立直方图”这个技术一共使用了两次：

1. 从重投影的3D点中得到记忆直方图；
2. 从双目相机得到的当前时刻点云数据计算新的直方图。

然而，转换为二进制直方图的阈值是不同的，因为总的三维点计数在很大程度上是不同的。记忆直方图的cell尺寸较大是因为它是按照一半分辨率构建的。对于一个新的直方图，如果至少有一个3D点落在二进制层中的一个bin中，则将其设置为已占用。在distance层对应的值就是落在这个bin的点云距离长度的均值。年龄层中的值总是设置为零，因为这个直方图是由新数据生成的。另一方面，记忆直方图以所有其他直方图的一半分辨率生成，然后向上采样得到。如果至少有6个点落在二进制层中的单元格中，则认为该单元格已被占用。这个阈值需要谨慎地选择。
接下来文章说明记忆直方图的这个阈值会产生什么影响，我的理解是这个直方图相当于对环境记忆时间长度的表示，尤其是age层。
如果将旧直方图的每个细胞分割成4个3D点，那么只有6个是可接受的值。如果阈值小于6，则记忆直方图比旧直方图拥有更多的已占用单元。这将导致合并后的直方图收敛到一个完全被占用的直方图。但是，如果阈值选择得太大，内存直方图将比旧直方图占用更少的单元格。记忆效果会被最小化，尤其是障碍物的边界会被忽略。因此，阈值的最佳选择是在不导致收敛到一个完整的直方图的前提下，尽可能地选最大的数。对于distance层，使用与新直方图相同的方法。年龄值也确定为bin中所有点的平均年龄。在以半分辨率创建记忆直方图之后，对其进行上采样以匹配新直方图的完整分辨率。分辨率的变化使得该方法不易出现离散化误差。

结合记忆直方图和新直方图

在上一步中，需要将记忆直方图(memory histogram)和新直方图(new histogram)组合成最终直方图(final histogram)。 最终直方图用于导航并存储用于下一次计算。为了结合两种直方图，我们要区分两种区域： 落在无人机当前FOV里面的cell，和与之相反，落在FOV外面的cell。新直方图有可能占据了FOV之外的cell，因为FOV的选择是非常保守的。它标记了新数据可信的区域，不仅是包含障碍的被占用单元，还有真实空闲的空单元。另一方面，FOV之外的未占用单元格（空闲单元格）通常是由于缺乏数据而不是关于空单元的知识造成的。
对于在FOV里面的直方图cell，新直方图被复制，因此给一个高于记忆直方图的优先级。对于位于FOV外部的所有cell，二进制层使用OR（或）操作。如果相应的单元格标被记为已占用，则从新的直方图中提取年龄和距离层。只有当单元格只被占用在记忆直方图中时，才会从那里复制年龄和距离层值。图3.5给出一个示例。

我觉得这个规则无非就是：

1. 无条件相信FOV内部的新直方图信息，给予最高优先级，就是图3.5中的黄色格子，但是他们只是最终直方图的一部分。
2. 对于FOV之外的，更愿意相信“被占用的”，而不愿意相信“空闲的”，原因在上文黑体字，就是空闲的判断没有占用充分。另外，OR操作也算是为了保险起见，就是当前和记忆中任有一个认为这是个障碍（1），那么最终直方图就会标为1。对于一个格子来说，如果当前和记忆均为0时， $0||0=0$，剩下的如果new=1，那么所有信息根据new来更新；如果new=0，memory=1，则使用memory的信息。

适应安全和拐角情况

3DVFH算法在实际应用中要加入一些适应和安全考虑，所有直方图方法的一个固有缺点是它们的方向性。直方图给出了障碍物方向的信息，但它不知道障碍物的距离。如第3.2章所述，记忆3DVFH算法的直方图还具有一个距离层。这一层存储距离是必要的，以便能够重投影直方图单元格到正确的三维位置。

在FOV内部方向运动

当存在两种代价函数相近似的选择时，算法会在两种选择中多次权衡。这种情况经常出现在一个障碍物竖直隔开飞机与目标。在这种情况下，避开右边的障碍物可能和避开左边的障碍物一样好。如果无人机选择向右飞行，它将同时向前移动和向右偏航。由于偏航运动导致FOV的变化，同时右侧的障碍物作为新的障碍物区域变得可见。这使得左边的方向更具吸引力，无人机也会改变向左的方向。由于FOV的改变，障碍物的一个新部分再次变得可见，这也导致无人机改变方向。如果算法在这两个选项之间切换太频繁，无人机就会越来越靠近障碍物，最终会坠毁。

一种减少此类风险的措施是，移动仅能选择在FOV内部的方向。如果下一个航路点位于FOV外，无人机必须悬停并偏航，直到航路点进入FOV。针对图3.6的情况，无人机将一直盘旋并向右转，同时从右侧（以前未知的）障碍物中获取数据。如果右侧障碍物包含的信息给出“左侧更有吸引力”，则无人机将继续盘旋并向左侧偏航。在这种情况下，障碍物的所有部分（从无人机的当前位置可见）已经被感知并存储在存储器中。 因此，无人机将从这些信息中选择更好的方向，转动和移动。 在实际测试中，由于点云中的噪声，无人机可能在每个方向上转动不止一次。 此功能无法解决在可能性之间切换的问题，但可以消除发生故障的风险。

安全措施：后退

在一些情况中，飞机由于FOV受限制，可能非常接近障碍物。Realsense相机的最小感知距离为20cm。在真实的场景中，最小距离可能更大，根据噪音和光照条件，距离小于50cm的障碍物相当不可靠。图3.7显示了最常见的危急情况之一。

开始检测到墙，准备向左规避障碍，然而沿着墙飞行一段时间之后，由于移动方向的前向分量，无人机已经非常靠近障碍物。如果左边的墙很长，根据之前的规则，向右转来规避障碍的策略会更有吸引力，因此它会悬停并向右偏航。如果这个飞机与墙的距离小于最小感知距离，飞机会发生危险。这是传感器固有的性质，我们无法避免其出现。然而，通过跟踪现有点云的最近部分，可以显着降低崩溃的风险。盲点通常伴随着障碍物的其他部分，其位于传感器的稍远处并且可以在点云中看到。点云的那些部分通常距离接近dmin。如果检测到这样的接近点，则无人机需要向后移动，从而可以减少点云中的孔的可能性。 向后移动不是最理想的，因为无人机后面没有传感器。 但如果无人机继续向前运动，那么崩溃的风险要高得多。 点云中的洞通常位于非常有吸引力的方向上，因此选择去那里的可能性非常高。 另一方面，无人机后面的空间很可能是自由的，因为无人机来自那个方向。 因此，如果点云中的某些点比特定距离更近，则无人机将在x-y平面中从该点后退1米。

直方图中的安全裕度取决于障碍距离

膨胀被占用的栅格来留有安全裕度。安全裕度太大会导致飞机过于保守，绕远或者难以到达目标，太小容易出现飞行事故。在接近障碍物时，安全高于进度，因此要增加安全裕度。当飞机远离障碍物时，一个小的安全裕度更有益处。考虑到这一点，已经实施了两个不同的裕度。 最近障碍物的距离决定了安全边际的大小。 边缘尺寸的每次改变通常引入期望方向的改变，因为通常期望的方向位于被阻挡的细胞旁边。 为了避免振荡，实施滞后方法，以在不同尺寸的裕度之间切换。

取决于进程的代价参数自适应

建立直方图之后可以得到候选方向，每个方向的代价都使用代价函数进行评估，并为移动选择最佳候选方向。代价函数没什么大的变动，直接上图。


简单说，代价函数分为两部分，目标导向和平滑度。
权重因子决定其选择路径的倾向，比如向上或下规避。如果up和down的权重过大，说明更倾向于在当前2D平面选择绕过障碍物。这个设置和实际飞行环境有很大关系，结合之前的例子，如果飞机前面有一堵墙，那么up权重很大时，飞机很难做“向上飞跃”的决定。
同时，自适应调整参数的方法显出优势。飞向目标的进展应当被监视，如果对飞向目标没有进展，我们应该适当降低上升代价。**为了跟踪朝向目标的进度速率，监视距离d随时间的目标的趋势。**计算这个“函数”的斜率，如果是一个大的负斜率值，表示为一个接近目标的好趋势，如果是零，那么没有进展，试想绕着目标绕圈，如果是个大正数，说明飞机在远离目标的路径上。
斜率s与参数相关，具体调整引入“增量式”思想。在给定的上下限区间内，对当前状态与期望状态进行“做差”，然后给一个增量进行调整，直到满足要求或者达到上下限。

球体避障-离障碍的最小距离

VFH算法不能保证无人机与障碍物有一个最小距离，它确实改变方向以避开障碍物但它继续靠近，除非直方图的前半部分中的所有单元都被占用。即便是点云信息有噪声或者不稳定，这种行为会更加显著。为了防止过于接近障碍物，3DVFH算法增加了一个额外的特性：到任何障碍的最小距离 $r_{sphere}$。

定义一个比 $r_{sphere}$更大的 $r_{sphere-cloud}$来决定点云中哪些点该被考虑。无人机的位置为重心， $r_{sphere-cloud}$作为安全包络的半径，在安全包络内的所有障碍物点云位置平均数作为障碍中心 $c_{sphere}$，以 $c_{sphere}$为球心， $r_{sphere}$为半径作球形，相当于将障碍物膨胀，留出飞行安全裕度。该球体被用作附加约束，无人机无法进入。

球的避碰约束只在算法流程的末端产生影响（航路点按照之前套路正常算，只是在最后一步输出到控制器的时候，用这个约束判断一下这个点是不是安全）。

在之前的障碍物安全预警区域之外，我们设置了一个迟滞因素（我更倾向理解为一个过度缓冲区）。当航路点被设置在这个区域内的时候，会做一些修正，首先修正z，保持xy平面不动，直到z和 $c_{sphere}$对齐。这样会在离障碍物最近，并且观测最理想的地方决定如何规避。

根据航路点的原始计算值和安全包络分析，生成最终的发给控制器的航路点。三种情况对应不同的方式，特别近的，就设置在包络上；在迟滞区，给一个加权；在安全环境中不做改变。

地面检测

无人机在飞行过程中不能看到正下方的情况，无人机通过前方视场来对环境建立一个内部高度图，当飞机飞到这部分区域时可以被使用。这使得无人机能够与地面保持一定的距离，甚至在向更高的地面移动时，还能向前看并提前增加高度。

建立内部高度图

在ground box中定义一个最小飞行高度，


RANSAC是一个算法，可以拟合直线、平面。具体过程可百度，基于最小二乘思想。

三位空间中，四个参数表示一个确定平面（地面）。

然后将图3.11右侧阴影patch的二维坐标x，y的min和max以及hmin记下为当前的“高度信息”。

从所有这些保存的patch中，构建了环境的高度记忆（类比之前的记忆直方图）。当new patch被构建时，先搜索高度记忆来寻找相似的patch，如果它和新patch重叠，则它们被假定为具有相同表面，并被保存为具有相同边界值的一个条目（entry）。

无外部障碍的高度控制

视场中没有障碍物的时候，无人机直接飞向目标。为了规避地面上的障碍，内部高度图的知识可以被使用来修正飞行高度。
当无人机接近存储在内部高度图的某一块patch时，它要尽早上升使得在这块patch正上方时达到所需高度。

最大上升角为24度，如果以更加陡峭的角度上升，则不能保证安全路径的生成。因此，无人机需要在进入patch前面的裕度m时开始调整其高度。公式3.19给出m的计算公式。
一旦飞机在飞行方向进入了被裕度m扩大后的patch区域，裕度被设置为固定，并且飞机被认为在相关patch上面。连续的裕度调整会导致飞机振荡，因为裕度随着飞机的高度上升而减少。
当无人机飞过障碍物时，最小飞行高度通过将最小飞行高度和障碍物高度求和的方式计算得到。（裕度m计算也是这么得到的。）文中给出三种情况对应的高度调整策略。

情况1：飞机高度太低。
解决1：将新的航路点设置在目标的x-y平面方向，同时以最大上升角度抬高无人机。
情况2：飞机高度够高，但是接近最小飞行高度（在允许飞行高度内遇到高障碍物）。
解决2：此时飞机不能再接受到下降机动的指令，因此，引导飞机下降的航路点要被修改。
情况3：飞机高度够高，但是远离最小飞行高度（近似于安全情况）。
解决3：保持原始指向目标的航路点。

为了避免震荡，在三种情况中均使用了迟滞。

结合直方图的高度控制

与没有障碍的情况类似，首先判断无人机是否在一个patch的上面。计算方法同公式3.19，但是要考虑在期望上升角的离散直方图。

与上小节的三种情况相对应：

1. 当无人机飞的很低，位置低于emax的仰角的所有直方图单元被阻挡，使得它们不再是有效候选者。这意味着无人机除了在相应的直方图单元空闲的方向上以角度emax上升之外没有其他可能性。
2. 在低于零的仰角处的所有候选cell被丢弃，使得无人机不能选择降低高度。
3. 在不介入高度控制器下，根据直方图结果来选定方向。

其实有“优先调高度，其次调平面”的感觉。

前瞻性：有记忆的 $3DVFH*$

基本原则是在未来可能存在的位置上重复“直方图步骤”来建立一个搜索树。

算法结构

3.12是流程图，综合上文提到的各个模块。

1. 算法首先接收相机的点云，然后根据飞行进程更新代价参数。
2. 重投影旧直方图，融合新直方图得到导航直方图。
3. 建立搜索树。

树的根（root）是当前位置，将该位置作为第一个原点输入树中，并设置树的成本函数以及第一个节点的启发式。从这开始，新的节点开始加入树结构中，直到扩展了指定数量N的节点。（这里树结构的深度和广度都需要一些限制。）
要扩展节点，首先要裁剪当前位置周围的完整点云。由于当前节点只是一个可能移动的提议，如果太靠近障碍物，则不需要做出反应。但是如果节点太接近障碍物，那么它的代价是无穷的，我们不会扩展它，而是寻求下一个最佳节点。
为了展开节点，计算节点所在位置的FOV。直方图是由裁剪后的点云直方图和重投影后的点的直方图组合而成。要将这两个直方图结合起来，需要无人机在节点位置的FOV。直方图被下采样到分辨率的一半，以减少候选方向的数量并使它们更加清晰。候选方面的评估和之前的3DVFH中的方法相同，为了减少搜索树的分支因子，并不是所有候选节点都作为节点添加。按照代价成本添加最优的作为节点。只有当来自相同源的节点之前没有添加临近节点时，才会添加候选节点（就是在广度上要保证相邻的两个node之间有一定间隔？）。最大分枝数量为bmax（每层新节点的个数最大值），对每个新添加的方向产生一个相距Lnodes距离的新节点（步长）。然后整个树搜索最低总代价的节点，继而修枝，重复以上步骤，直到满足节点数量为N。当整个树构造完成之后，采取“倒寻”找到路径。

值得注意的是，tree在构建过程中是寻找一层，修剪一层，因此每层过后仅剩下这层及之前的（局部）最优解。（Astar基本原理就是如此。）

图3.13给出一个循环中的逻辑，注意，如果当前时刻中前方没有障碍物，则利用之前计算的路径。先前计算的路径是否仍然有效，取决于该路径的年龄（可信度的一种反映）以及与建议的树路径相比当前无人机的位置（路况是否符合预期）。如果无人机似乎在之前计算过的树状路径上（没什么突发情况），那么路径将被跟踪到最后，而不是直接指向目标。在点云包含大量噪声，甚至可能在一段时间内是空的情况下，无人机将停留在计算的路径上，不会转向目标。计算精度和时间花费需要一个折中。

重利用旧路径

如果满足某些需求，在之前时刻，从搜索树计算出来的路径可以被重新利用（reuse）。如果路径的年龄超过限制，则被丢弃。否则，计算路径上两个最近节点的距离。如果最近的节点就是路径的末端，该路径被丢弃。（如果无人机距离最近的节点过远，也被丢弃？）

reuse之前计算的路径如图3.14所示。如果旧的路径可以被reuse，那么角度 $\delta$可以用公式3.20计算。两个节点之间的距离已知，根据余弦定理能求出角度 $\delta$。引入p来判断无人机在节点i-1和i之间的进程，以此来决定下一时刻的航路点设置在什么地方（待选区间是节点i至i+1的整个闭区间内）。
不过…我觉得原文公式3.21，22，23中的某几个好像有点小问题，不过策略的思路还是描述得很清晰。
根据图和公式3.20，我们姑且认为它们没有问题，合并22和23两式，发现p是靠 $cos\delta$衡量，如果 $\delta=0$，这时无人机靠近node i-1，此时p=1，下个航路点为node i；如果 $\delta=1$，这时无人机靠近node i，此时p=0，下个航路点为node i+1。对比文中公式3.23和p的分类讨论正好相反，因此，文中p的分类可能笔误了。原文的效果解算出来是靠近n3时，下一个航路点为n5，而靠近n4时，下个航路点还是n4。
但是根据图标注的话，3.21应该写成 $l_i=d_4 \times cos\delta$，并且光一个 $cos\delta$也没办法真正刻画无人机靠近 $n_3$还是 $n_4$，所以觉得公式3.21有点问题（ $L_nodes$不合适），并且公式3.23的权重系数应该换位置。

代价和启发函数

Astar搜索需要一个代价和启发函数来评估每个节点，这两个函数都被设计成使用角度差异（angular differences）而不是比较位置（comparing positions）。代价函数由四个不同的部分组成：目标代价，偏航代价，路径平滑代价，树平滑代价。目标代价描述节点的方向是否与目标的方向一致。换句话说，它是由当前节点引出的树枝和目标方向的角度差计算出来的。偏航代价为无人机当前偏航与树枝到节点的角距离。路径平滑代价驱使树内的路径平滑。由当前树枝和上一时刻的树枝夹角表示，其实就是两个树枝间的角增量。树平滑代价不是一个与角度有关的代价准则，而是对不同时间步长的树的比较。它将当前节点与最后选择路径中相同深度的节点进行比较。这个代价需要一个量级来适应和角度相关的代价计算方式。

不同的代价项的权重因子 $k$表示了每项的重要程度。深度大的地方受代价的惩罚程度较轻，折扣因子小于1。折扣因子越小，长远策略的惩罚越小，算法更注重当前几步的代价。Astar需要一个启发函数来评估从一个节点n到目标最便宜路径的代价，对于一个可用的启发函数，它不应该过分估计到达目标的代价，为了简单起见，我们使用当前节点的目标代价（target cost）作为启发式（启发函数本来就是一个估计，不是很准，并且目标周围环境不好评估，只使用target cost比较客观）。
因此Astar算法的代价函数（四个部分）和启发函数（一个部分）都已经得到。

结果

实验设置

ROS和四个节点的功能。

仿真设置

Gazebo+ROS

实验平台

Intel Aero Ready to Fly + Realsense 200 camera

局部路径规划节点

输入：无人机状态、3D点云数据
输出：下个航路点
双线程工作，一个线程运行规划算法，另一个线程与其他节点进行交互并且保持时间跟踪。只要有新的点云被接受到，都会执行planner算法。通信线程以10赫兹的速率运行，以确保满足最小的设定值速率。