摘要

本文提出了一种未知gps拒绝的室内环境多采用UAVs协同探索方法。该方法的核心是利用Tracking-D*Lite在未知地形下跟踪运动目标的算法Bug算法的WallAround未知室内环境下的算法UA V进行导航。该方法利用了上述两种算法的优点UAV使用w全能算法绕墙飞行，使用w全能算法绕墙飞行Tracking-D*Lite算法实现无人机之间的合作。利用Gazebo结果表明，该方法可以有效地利用多架无人机的优点来探索未知的室内环境。最后，该方法还可以绘制整个环境的边界等高线图。该方法可应用于危险建筑、危险气体工厂、地下矿山或地震后的其他搜救场景。

关键词:多uav协同 目标跟踪 路径规划

一.介绍

在过去的几年里，无人机(UAVs)由于其高成本效益、灵活性和灵活性，耐久性已广泛应用于军事和民用领域。

由于其灵活性和低风险，无人机在探索室内环境[1]方面得到了广泛的发展。UAVs探索室内危险空间有很好的应用场景，如有毒气体泄漏厂、地震后危险建筑、核辐射危险区域等。然而，没有无人机gps在未知的室内环境中自动导航是非常困难的。此外，同步定位和测绘同步定位和测绘(SLAM)[2]探索未知环境的方法会占用大量的计算时间和存储性能。所以，为了减少UAVs通过多种方式计算成本和寻找未知室内环境的时间UAVs采用协同探索策略，可以以较低的成本快速探索整个未知的室内环境，为事故或灾难现场的探索和救援、建筑物和公共设施的检查等后续任务提供及时有效的信息。

多无人机协同探索需要在缺乏地图信息的情况下提供灵活稳定的探索策略。gps在不支持的未知室内环境中，探索策略的设计不仅需要多架无人机的协调，还需要配备自动定位、飞行和避障导航算法。此外，导航策略必须确保每架无人机应尽可能有效地搜索未知区域，覆盖最大范围。

导航策略应避免无人机在搜索过程中的重复探索。最后，该策略应该是鲁棒的。单个无人机的故障不会导致整个搜索过程的失败，从而提高整个策略的稳定性。

在未知的室内环境中，提出了一种基于动态目标跟踪的多无人机协同探测策略。该方法基于两种路径规划算法BugW全能算法[3-5]用于自主绕环境边界飞行；另一种是基于D*Lite[6]和I-ARA*[7]未知地形动态目标跟踪算法(tracking -D*Lite)，用于UAVs中继之间。

综上所述，本文的主要贡献如下：

提出了围绕环境边界飞行的w全能算法。该算法基于Bug算法的理念取消了目标点的存在，使无人机能够按照一定的规则在环境边界周围导航，最终返回到起点。该算法主要用于探索未知空间的边界，为绘制边界等高线图提供位置信息和数据。

针对未知地形下多架无人机之间的中继，提出了动态目标跟踪算法(tracking - d *Lite)。该算法主要基于D*Lite算法和I-ARA*算法。并对I-ARA *该算法用于跟踪运动目标点D * Lite减少重新规划过程中的搜索时间和扩展次数，从而达到快速搜索效率和规划质量。

构建了一个多无人机探测系统，可以在未知的室内环境中独立导航。该系统可以协调多架无人机独立探索未知的室内环境。通过模拟实验验证了探测系统的有效性。实验结果表明，提出的多无人机探测系统可以提高未知室内环境下的探测效率。

第二部分首先介绍了使用无人机探索未知室内环境的方法，然后介绍了工作的任务场景。第三节将详细介绍所使用的方法。第一种方法是W全能算法，用于无人机包围未知环境的边界。第二种方法是基于D*Lite和I-ARA*的Tracking-D*Lite算法，实现无人机之间的中继。在第四部分，我们将介绍我们的实验设置，包括通过和重复d *Lite比较，模拟环境和结果，对Tracking-D*Lite性能实验。第五部分总结了本文，讨论了未来的工作。

二.相关工作及场景说明

2.1相关工作

随着UAV随着技术的快速发展，越来越多的研究人员开始关注使用UAVs.来探索gps否认未知的室内环境。解决方案gps否认未知室内环境自主探索问题的主要方法有三种：SLAM、深入加强学习和传统路径规划算法。

在探索未知的室内环境时，SLAM是一种全面准确的方法。SLAM准确定位、避障、导航和实时可视化飞行机器人。[8]将适用于地面机器人导航和测绘的方法扩展到UA Vs.人工构建了一种带有处理器和各种雷达的特殊无人机，可以在未知环境中实时构建和导航获得的空间信息。在UA v它配有激光雷达和深度摄像头。它用激光雷达感知周围的障碍物，深度摄像头避开障碍物。基于slam方法是未知室内环境探测领域最经典、最准确的方法。该方法要求无人机配备高性能处理单元和存储设备，实现复杂场景的精确建模和无人机的独立控制。

但该方法不适用于计算能力弱、存储性能低的无人机，尤其是一些微型无人机。

基于深度强化学习的方法，重点研究无人机是否存在gps学习室内环境中的定位、导航和避障策略。[10]采用深度学习和强化学习方法识别无人机拍摄的视频图像，并在室内环境中设计搜救应用系统。[11]通过机器人操作系统主要用于机载传感器(ROS)马尔可夫决策过程的上部可以观察到(POMDP)在gps在被拒绝的障碍环境中定位。[12]提出了一种利用PID q -学习算法训练UA V学习如何在未知环境中导航到目标点。[13]提出了基于深度强化学习方法的建议UAV室内环境探索框架。该框架分为马尔可夫决策过程(MDP)部分可以观察马尔可夫的决策过程(POMDP)。以传统为基础，提出了一个目标发现框架pomdp规划与深度强化学习相结合。不同于[13]，[14]使用多个UAVs探索相同的室内环境。基于深度强化学习的方法UA Vs探索复杂未知的环境可以取得一定的效果，但这种方法往往不能在地图构建、运动决策和规划等方面取得理想的效果。此外，该方法的研究主要是控制单个无人机V进行探测，缺乏多架无人机的有效利用。

一些作品使用传统的路径规划算法来探索未知的室内环境。[15]针对探索微型飞行机器人未知环境的问题，提出了最小化导航方法——群梯度bug算法(SGBA)。这项工作使一群微型飞行机器人向不同的优先方向前进Bug算法导航，最后所有飞行机器人返回起始位置。然而，在探索过程中，多个飞行机器人可能会反复搜索同一区域的问题。由于需要返回，每个飞行机器人的搜索范围无法最大化。

为了在gps无人机在不支持的未知室内环境中得到有效的探索和导航，本文提出了一种结合w的全方位算法TrackingD*Lite多无人机协调探索算法。该方法可应用于计算能力弱、内存小的无人机（特别是微型无人机），对低成本的室内环境探索具有一定的实用性。

2.2场景描述

在本文中，我们考虑使用多个UA Vs来探索一个gps拒绝未知的室内环境，并绘制整个室内环境的边界图，如图1所示。在这种情况下，每一个UA V从相同的起始位置开始。首架无人机采用w全能算法绕环境边界飞行，红线表示首架无人机的飞行轨迹。当第一架无人机低功率飞行或因事故停止工作时，第二架无人机将开始中继。跟踪中继将在此过程中使用tracking - d *Lite如黄线所示。跟踪完成后，第二架UA V在返回起始位置之前，使用w全能算法继续绕环境边界飞行。在整个探索过程中，当探索完成时，实时绘制环境边界周围的无人机路径。

图1所示。两架无人机探索中没有一架gps未知的室内环境示意图。红线表示第一架无人机绕墙飞行的轨迹，黄线表示第一架无人机在接力过程中跟踪的轨迹，蓝色虚线表示第二架无人机跟踪第一架无人机。两架无人机聚集在五角星上，绿色轨迹表示第二架无人机绕墙飞行的轨迹。（在线颜色图）

三.方法

针对图1所示的未知室内环境探测问题，我们提出了一种基于动态目标跟踪的未知室内环境ua V协同探测方法。该方法包括三个机构：w全能机构、接力跟踪机构和边界轮廓构建机构。如图2所示，三种机制的具体算法调度框架主要包括Input、wworlds算法、Tracking-D*Lite算法和Output四部分。输入处于初始状态UA Vs的位置。输入后直接调用Wall Around算法绕墙飞行，实时绘制无人机的飞行轨迹。w全能算法主要包括绘制轨迹和绕墙飞行。这两项工作是同步的。当无人机停止工作（能量耗尽或事故）时，系统触发下一架无人机的中继电话Tracking-D*Lite。Tracking-D*Lite主要包括六个部分。当赶上最后一架无人机时，Tracking-D*Lite算法将返回Bug算法。该框架的输出是多架无人机绕墙飞行后绘制的边界图。

图2显示。系统算法框架示意图。主要包括WallAround算法和tracking - d *Lite算法的两部分，以及两种算法之间的转换过程和调度过程。

3.1Wall-Around算法

Wall-Around算法的设计是为了让UAV沿着室内环境的边界，即沿着墙壁飞行。传统Bug在了解目标点位置的基础上，算法[3-5]UAV向目标点前进。遇到障碍物时，UA V围绕障碍物不断靠近目标点，避开障碍物。Wall-Around算法和Bug算法的区别在于没有明确的目标点，无人机只需要围绕地图的边界，最终回到起点。

通过类似，无人机可以使用TangenBug[16]的方式进行导航，通过视觉或激光雷达等传感器感知周围一定距离内的障碍物，从而决定向某个方向移动。

该算法将整个未知空间分为前、后、左、右四个方向。这四个方向是固定的。但对于无人机来说，它需要不断选择正确的方向绕墙飞行。因此，无人机本身有四个方向的状态，包括MD(主方向)，ND(下一个方向)，PD(前一个方向)和OD(反方向)。MD最重要的是，它表示当前无人机指向墙壁的方向。其他三个方向MD方向顺时针旋转计算，如MD方向右，ND方向后，PD方向前，OD方向左。四个方向可以通过类比随时计算。

3.2Tracking-D*Lite

为了使UA Vs中继平滑，需要在未知环境下设计目标跟踪算法。传统路径规划算法中使用的网格地图，A* [17] A n d J P S[18]等主流路径规划算法是基于已知地图信息的两个固定点之间的路径规划。对于未知空间路径规划，例如D*[19]和D*Lite[6]算法只能规划固定点之间的路径。

在跟踪目标方面，I-ARA*[7]可以快速重新规划运动目标点的路径，但它是以完全知道地图信息为前提的。因此，对于未知环境中两个不断移动的UA Vs，在位置坐标已知的前提下，需要设计一种目标跟踪算法来完成UA Vs之间的中继任务。本文提出的未知地形动态目标跟踪(tracking -D*Lite)算法主要是在D*Lite的基础上进行改进。由于D*Lite是未知地形下的路径规划算法，不能直接用于跟踪，所以结合I-ARA*的思想可以用于跟踪未知地形目标。

在介绍Tracking-D*Lite之前，有必要解释一下算法中使用的符号:S表示网格地图中的顶点集合。sstart∈S和sgoal∈S表示起始顶点和目标顶点。Succ(s)⊆s表示s∈s的子顶点集合。同理，P re d(s)⊆s表示s∈s的父顶点集合。0 < c(s, s‘)≤∞表示将顶点s移动到顶点s?的代价。

由于该算法采用四连接展开法，因此相邻两个顶点的代价为1。启发式函数为曼哈顿函数，表示为h(sstart, s)，满足h(sstart, s) = 0，对于所有的顶点s∈s, s?符合三角形不等式h(sstart, s)≤c(s, s') + h(sstart, s’)∈Succ(s)。

Tracking-D*Lite与D*Lite类似，它也从目标开始，延伸到起点，最后给出一个路径。g值和rhs值的计算算法中也涉及到g(s)表示sgoal到当前顶点s∈s的最短距离。rhs(s)是基于g(s)计算的，这个变量的主要作用是寻找一个代价更小的路径顶点。如果当前顶点s = sstart, rhs(s) = 0。相反，如果当前顶点s不是sstart，则s rhs(s) t为所有父顶点中一步代价加g(s)的最小值，如式(1)所示。

对于顶点s∈s，当g(s) = rhs(s)时，认为顶点s处于局部一致状态，即能够找到从sgoal到s的最短路径。如果图中所有顶点都是局部一致的，则可以找到从sgoal到任意可达顶点的最短路径。当g(s) > r h s(s)时，认为顶点s是局部过一致的。这意味着从sgoal到s有一条更短的路径，主要表现在从障碍物区域到可通行区域的一定区域。当g(s) < r h s(s)时，认为顶点s是局部一致的。这意味着之前从sgoal到s找到的最短路径代价变大，需要重新计算最短路径，主要表现在某一区域从可通行区域到障碍物区域。

Tracking-D*Lite需要维护一个Frontier优先级队列来存储本地不一致的顶点。这些顶点需要按照一定的规则进行排序，然后选择一些顶点进行展开，然后将这些选择的顶点转化为局部一致的。Frontier优先级队列的排序依据如式(2)提供的键值k(s)所示。k(s)有两部分:k1(s) = min(g(s)， r h s(s)) + h(s, sgoal) + km, k2(s) = min(g(s)， r h s(s))。km表示每一步开始后的启发式补偿值，以保持后续搜索中键值的严格升序。

Frontier队列使用键值k(s)进行排序。k(s)的排序规则为字典排序，即k1的值越小优先级越高;如果k1相同，则k2值越小优先级越高。

 

Tracking-D*Lite在D*Lite的基础上，在I-ARA*中加入了删除队列的思想。

删除队列用于存储当前搜索过程中不属于当前搜索根顶点的搜索树，该顶点将在下一次搜索中重复使用。trace - d *Lite的执行过程如图3所示。

 图3所示。Tracking-D*Lite算法流程图。

 

Tracking-D*Lite算法的步骤如下:初始化。初始化移动目标和起始位置。

2. 计算最短路径。在起始的视觉范围内检测环境信息。，第一次计算从起点到目标的最短路径。

3.更新位置和判断。更新开始和目标位置。如果开始赶上了目标，算法就结束;否则，更新环境信息。

4. 构建删除队列。从删除搜索树中不属于这个根顶点的顶点开始，并将它们放入删除队列。

5. 重用和更新搜索树。对于所有的Delete顶点，如果它们的邻居顶点属于当前搜索树(不是Frontier中的顶点)，那么该顶点将被重新展开并添加到Frontier中。最后，通过以上步骤清空Delete队列。

6. 重复上述步骤2到5，直到开始追赶目标。

Tracking-D*Lite算法的伪代码主要如下图所示。在算法2的第2 - 3行，算法记录了开始和目标的位置，用于后续计算。然后，Main()调用Initialize()来初始化第4行中的搜索问题。在此过程中，根据公式(1)设置所有顶点的g值和rhs值，并将每个顶点的父顶点在搜索树中设为空。在Initialize()的最后一步中，只有目标的顶点是局部不一致的，所以它被添加到Frontier。

Initialize()执行后，Main()调用ComputeShortestPath()(参见算法3)来搜索从目标到起点的最短路径。在展开搜索树顶点的过程中，对于局部过一致的顶点(算法3第5行)，将其g值设为rhs-value，展开它周围的相邻顶点。对于局部一致的顶点(算法3的第7行)，将其g值设为无穷大，重新加入优先级队列Frontier。此步骤相当于将其设置为本地超一致。

 

图4所示。以《Tracking-D*Lite》(Part 1)为例，如左图所示，机器人位于B1处为起点，D3处的目标为目标。在右图中，机器人只能感知到一个单位距离的周围环境信息，所以B2和B3是障碍物。右图中的h值代表启发式值，它使用了从每个节点到起点的曼哈顿距离。

下一步，Main()更新start和目标的位置(算法2的第7-11行)，并将目标的父节点设为空值(算法2的第13行)，并特别注意计算不同起始位置之间的启发式差值km(算法2的第9行)，以保证Frontier的关键值的一致性。当目标位置发生变化时，Main()(算法2的第16-22行)

图5所示。以track - d *Lite为例(第二部分)，Step1到Step7代表了第一次展开过程中Tracking-D*Lite的过程，最后得到first path指向的路径。(颜色图在线)

删除在搜索树中根顶点不是当前搜索开始点的成员，并将它们放入Delete队列，然后重用一些被删除的顶点(参见算法4)。更新网格地图中改变的顶点的边开销，更新Frontier和每个顶点的g值。

图6所示。以Tracking-D*Lite为例(第三部分)，当Robot根据第一次搜索得到的路径向C1移动一个单位距离时，Goal也会随机移动到C3。此时，Robot感知到新的障碍物D2，并重新计算整个地图中每个节点的启发式值。

下面是一个Tracking-D*Lite的例子。图4中有一个5*3的网格地图。机器人的位置(开始)在B1，目标在D3。在图4中，M ap是整个地图的真实环境。机器人可以感知周围单位距离的地图。图4中的启发式为机器人在可达格点处的启发式值，启发式函数为曼哈顿函数。

图5显示了Tracking-D*Lite第一次执行ComputeShortestPath()函数的情况。该算法的初始搜索迭代步骤与D*Lite算法相同，都是从目标位置开始搜索和扩展。

如图5所示，黄色网格表示优先队列中的顶点，红色方框顶点表示下次展开的顶点，顶点之间的箭头表示父顶点指向子顶点。

每个顶点都有一个g值(在网格的左上角)、rhs值(在网格的右上角)和键值(如果存在则在网格的下方)。

第一次搜索是在初始化阶段从目标展开的。根据Eq.(1)将目标的rhs值设为无穷大和零，根据Eq.(2)计算顶点的优先键值。在本算法的算例中，采用四连接展开法。通过选择优先级队列中优先级最高的顶点(即Key值最小的顶点)，将该顶点的邻居展开并放入Frontier。然后将扩展后的顶点从Frontier队列中删除，最后将这些顶点的g值设为rhs值。第一次执行ComputeShortestPath()函数后的路径如图5中的第一个路径所示。

图7所示。以track - d *Lite为例(第4部分)，该部分为重新规划过程中Delete节点复用部分的具体流程。删除图是将不以第二次搜索的起点(C3)为根节点的搜索树删除，存储在删除队列(灰色网格)中。DeleteNode表示Delete队列中靠近新搜索树的节点(D3)的展开，该节点之前没有被放在优先级队列中。Step1到Step6表示第二次展开过程中Tracking-D*Lite的过程，最后得到second path指向的路径。

在搜索完第一条路径后，如图6所示，机器人按照规划的路径移动一个单位距离到达C1，目标随机到达C3一个单位距离。机器人感知到周围的环境信息，发现顶点D2是一个新的障碍物，因此算法更新地图信息并设置新的启发式值，同时设置km为1。

在第二次搜索中，需要删除与这次搜索无关的顶点。因此，Tracking-D*lite从最后一个目标顶点开始，通过深度优先搜索，删除搜索树中根顶点不是当前目标顶点的成员，并将其放入删除队列中。

为了在每次搜索后有效地利用信息。图5中的步骤7是第一次搜索后的最后一个状态，其中包含了一个以D3为根顶点的搜索树(因为D3是第一次搜索的目标)。C3是第二次搜索的目标位置，所以我们需要砍掉搜索树。如图7的Delete图所示，灰色网格表示已放入Delete队列的顶点。这些顶点由D3作为第一次搜索的根顶点的子顶点组成。我们可以发现，在删除图中，保留了以C3为根顶点的子树，因此在删除顶点的过程中保留了上次搜索结果的部分状态信息。然后我们需要重用Delete队列中的一些顶点。

对于删除队列中的所有顶点，如果它们的邻居顶点都属于当前搜索树(而不是优先队列中的顶点)，那么这些顶点将被重新展开并添加到Frontier中。如图7所示(重用DeleteNode)，顶点D3被添加到Frontier。

如图7步骤1-6所示，第二次搜索过程与第一次搜索过程类似。调用ComputeShortestPath()来搜索第二个路径。

本文提出的Tracking-D*Lite算法能够在未知地形下跟踪运动目标。该算法的主要思想是每次移动后重新规划目标的路径。在重新规划的过程中，会重复使用上次搜索结果的部分顶点信息。其主要过程是删除不属于当前搜索树的顶点，并在当前搜索中保留根顶点所在的子树。然后再利用删除中的部分顶点，减少每次搜索过程中顶点展开的次数，从而加快搜索速度和效率，达到跟踪运动目标的效果。

四.实验

4.1Tracking-D * Lite算法实验

本实验的目的是验证TrackingD*Lite算法的性能，主要比较跟踪时间、跟踪移动距离、扩展次数和成功率(目标不停止时)这四个指标。本实验比较了TrackingD*Lite算法和repeat -D*Lite算法(重复调用D*Lite)。操作平台为Windows 10，实验采用c++语言进行开发。

本实验使用60*60的复杂网格地图进行100组实验。每组实验随机分配一对不同位置的追逐器和目标。每组实验的目标运动路径也是不同的。在Tracking-D*Lite和repeat - d *Lite的对比测试中，目标的移动速度和路径相同。如表1所示，在agent的视图游侠为2、4、6时，实验对两种算法进行了测试。从平均跟踪时间上看，tracking - d *Lite跟踪目标的时间比repeat - d *Lite要短，并且随着agent视图游侠的增加，两种算法所消耗的时间都会增加。然而，Tracking-D*Lite比repeat - d *Lite花费更少的时间，并且增加的时间更少。对于平均移动距离，相同算法下不同视场游行者的结果相差不大，但TrackingD*Lite的移动距离比repeat - d *Lite要短，这也是Tracking-D*Lite的优势所在。与平均移动距离的结果相似，Tracking-D*Lite的平均顶点扩展结果要比repeat - d *Lite小很多。而不同游景器下的顶点扩展数量差异不大。最后，求agent跟踪不停止目标的成功率。两种算法的跟踪成功率随着视场游行者的增加而增加，但tracking - d *Lite要比repeat - d *Lite好得多。

表1。Tracking-D*Lite与Repeated-D*Lite的比较。

4.2仿真实验

仿真实验的目的是验证基于动态目标跟踪的未知室内环境下多ua V协同探索方法的合理性。实验的内容是建立一个模拟室内环境。6个四旋翼UA V模型使用本文提出的方法探索整个环境，并绘制边界等高线地图。

图8所示。仿真实验的场景。在这个场景中使用6 UA Vs来探索一个未知的室内环境。未知的室内环境包括障碍物、小房间等。

仿真验证实验是在Ubuntu16.04下的ROS、Gazebo、Rviz三个平台上，主要使用hector四旋翼[20]模型工具箱，使用c++语言进行仿真和开发。具体实验场景如图8所示。

图9所示。实时跟踪轨迹。绿色轨迹为实时绘制的边界等高线图，黄色轨迹为后一个UAV中继前一个UAV的轨迹(彩色图在线)

仿真实验首先集成了w全能算法。为了让无人机在地图的边界上飞行，每架无人机可以感知1米左右的环境信息。然后实验将Tracking-D*Lite算法集成到仿真环境中。当最后一架无人机功率不足时，下一架无人机将被派去使用Tracking-D*Lite在未知地形跟踪最后一架无人机进行中继。当中继成功后，下一架无人机继续使用w全能算法绕墙飞行。最后，在实验中绘制每架无人机的实时轨迹，得到边界等高线图。

 图10所示。边界等值线图。绿色轨迹是整个室内环境的最终等高线图。(颜色图在线)

本实验使用RVIZ实时绘制各UAV的轨迹，包括边界等高线图和跟踪过程中规划的路径。如图9所示，绿色轨迹为所有UA Vs绕墙飞行的轨迹，黄色轨迹为接力过程中的跟踪轨迹。图10是探索整个未知室内空间后绘制的边界等高线图。

五.结论

为了提高gps阻断环境下的室内探测效率，提出了一种基于动态目标跟踪的多无人机协同探测方法。该方法利用WallAround算法和Tracking-D*Lite算法的优势，利用w全方位算法探索未知室内环境的边界，利用Tracking-D*Lite算法实现无人机之间的协作。最后，完成对整个未知室内环境的探索任务。本文提出的方法能够有效地跟踪未知地形下的运动目标，在仿真实验中取得了较好的效果。