CN114115363A - 一种基于动态目标追踪的多无人机未知室内空间探索方法 - Google Patents
一种基于动态目标追踪的多无人机未知室内空间探索方法 Download PDFInfo
- Publication number
- CN114115363A CN114115363A CN202111468016.1A CN202111468016A CN114115363A CN 114115363 A CN114115363 A CN 114115363A CN 202111468016 A CN202111468016 A CN 202111468016A CN 114115363 A CN114115363 A CN 114115363A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- module
- numbered
- boundary
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/12—Target-seeking control
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Abstract
本发明公开了一种基于动态目标追踪的多无人机未知区域协同探索方法。目的是提高无人机探索无GPS信号未知空间区域效率。技术方案是构建由基站、无人机组成的多无人机协同探索系统；无人机包括硬件层和软件层，软件层包括感知定位模块、地图边界环绕模块、接力追踪模块、地图边界绘制模块和电量控制模块，基站上安装总控制软件；采用多无人机协同探索系统对未知室内空间进行探索：地图边界环绕模块采用Wall‑Around算法对未知区域边界进行探索；接力追踪模块采用Tracking‑D*Lite算法控制无人机之间接力；将所有无人机绘制的轨迹图拼接得到边界轮廓图。本发明能有效完成未知空间探索，绘制边界轮廓图既准确又快速。
Description
技术领域
本发明涉及多机器人协作系统领域，具体涉及一种基于动态目标追踪的多无人机在无GPS信号的未知室内区域协同探索方法。
背景技术
随着机器人技术的不断发展和进步，单个移动机器人已经难以通过自身完成复杂繁琐的工作任务，难以完成生产实践的工作指标，人们开始迫切需要研究新的方向来满足机械领域中的实际需要，于是多机器人协作系统得到了越来越多的重视，成为了各国研究人员的重点研究内容。围绕着任务分配、规划以及控制这一领域展开，对于多机器人系统的主要研究内容包括多机器人系统的体系结构、控制、规划、导航定位和通讯交流等。
相较于单机器人而言，多机器人组成的协作系统体现了一定的优越性。首先，多机器人系统对环境表现较强的适应能力，在工作任务中表现出较强的灵活性和变通能力，在功能上和空间上的分布性优于单机器人。其次，多机器人系统的承载能力强，多机器人系统作为一个群体，每个机器人各自工作的同时协调配合其他机器人的工作，使得工作时间大大缩短，有效提高生产效率，工作承载能力较强。最后，多机器人系统的鲁棒性高，任务的完成需要每个机器人的参与，而不是完全取决于单个机器人，一旦某一个机器人出错或者发生故障，可以通过控制调度系统交给其它的机器人完成相应的任务，因此具有较强的容错性和鲁棒性。
无人机是机器人系统中应用最为典型且广泛的，其不仅潜移默化的改变着人们的生活方式，还将在未来的产业和商业乃至民用、军用领域引起了一场重大的变革。无人机因其性价比高、灵活性强、经得起损耗等优势，不仅能够取代人完成一些简单的重复性劳动，如农业灌溉、定点巡逻、快递投放等，还能够在未知的复杂环境或危险环境中替代人完成具有危险性或者特殊难度的任务，如地形测绘、事故灾害现场勘探即救援等。但由于任务需求的不断增加，多变繁杂的应用场景对无人机提出了更多的要求和挑战。因此，由于单无人机数据处理能力低、应用任务场景少等限制，多无人机组成一套多机器人协同系统是一种必然趋势。
无人机由于其灵活性和低危险性等特点在探索无GPS信号未知室内空间方面得到了大量发展，特别是对于危险区域的探索，比如有毒气体泄露的工厂、地震之后的危楼、具有危险核辐射的地区等。因此，对无GPS信号的未知室内空间的探索任务是多无人机协同系统的应用场景之一。针对无GPS信号的复杂未知室内空间场景，可将该多无人机协同系统划分为控制结构、协作定位、路径规划和通讯四个子任务模块。
传统的多机器人系统控制结构可分为集中式结构和分布式结构，集中式结构有一个负责全局规划的控制中心，这个中心可以由群体中的某个机器人担任，也可以由群体以外的设备担任。分布式控制中每一个机器人都是平等、自主的智能体，可以根据自己的目标以及当前环境进行决策。虽然集中式结构拥有全局信息，可以得到规划、协调的优化解，但是分布式结构在鲁棒性、灵活性、可扩展性等方面优于集中式结构。
多机器人的协作定位是指依靠机器人自身携带的传感器、无线通信设备，通过分布式感知、信息共享、相对观测等方式实现整个机器人群体的定位。在信息获取、环境的感知等方面单个机器人的能力就非常有限，尤其在复杂环境定位是移动机器人系统需解决的关键之一，精确的定位能力是移动机器人完成指定任务的前提。
路径规划是由指定的起点到终点在空间中找到一组避开障碍物并且有先后顺序的空间位置点，路径规划问题是机器人研究中最基本的研究问题，具有很多不同类型的算法。
按照不同的算法思想可将路径规划算法分为基于图的搜索算法和基于采样的搜索算法。基于图的搜索算法依靠已知的环境地图以及地图中的障碍物信息构造从起点到终点的可行路径，主要分成深度优先和广度优先两个方向，深度优先算法优先扩展搜索深度大的结点，可以快速的得到一条可行路径，但是深度优先算法得到的第一条路径往往是较长的路径；广度优先算法优先扩展深度小的结点，呈波状的搜索方式，广度优先算法搜索到的路径就是最短路径，比较典型的基于图的搜索算法包括Dijkstra算法、A*算法、D*算法等。基于采样的搜索算法主要基于快速搜索树(RRT)算法提出的，该方法是一种增量式采样的搜索方法，具备良好的使用性能，它利用增量式方法构建搜索树，以逐渐提高分辨能力，而无须设置任何分辨率参数。在极限情况，该搜索树将稠密的布满整个空间，此时搜索树由很多较短曲线或路经构成，以实现充满整个空间的目的。
按照对地图信息的了解程度可将路径规划算法分为完全已知信息算法和部分已知或者全局未知信息算法。完全已知信息路径规划算法是指智能体在规划时掌握整个地图信息，包括障碍物、目标点、自由空间等的具体位置，这类算法更有利于获得到达目标点的最优路径，代表性的算法有Dijkstra算法、A*算法等。部分已知或者全局未知信息的路径规划算法是指智能体事先对整个地图信息并不掌握，依靠智能体的感知，通过不断地重规划，实时地更新地图信息，最后到达目标点，这类算法所规划的路径一般都不具有最优性，D*算法、D*Lite算法都属于此类算法。
多机器人系统通讯的基本要求是保证通讯的有效性和实时性，可分为直接通讯和间接通讯两种。直接通讯是每个机器人以通讯网络(如无线以太网)为媒介，主动发送和接收信息。间接通讯是通过机器人观察环境及其他机器人动作来实现的，这是对每个机器人感知和推理能力的反映。
目前有关使用多无人机探索无GPS信号的未知室内空间的研究工作有许多。2012年IEEE机器人技术与自动化国际会议(International Conference on Robotics andAutomation，ICRA)收录的文章《Indoor Navigation with a Swarm of Flying Robots》提出了使用群体无人机对一个封闭的无GPS信号的未知室内空间进行探索，该文章将无人机分为信标无人机和探索无人机，信标无人机处于静止状态，被固定在天花板上，以节约能源并形成无人机传感器网络。探索无人机在信标无人机的指引下不断朝未知空间前进，当到达探索边界时，探索又会转化为信标无人机为其他探索无人机指引方向。该工作中使用了多架信标无人机组成一个巨大的指引网络为探索无人机指引正确的前进方向，同时该指引网络的存在成功解决了无人机的导航定位问题。然而，该方法为了构造指引网络需要用到大量信标无人机，因为在整个多无人机系统中，大部分无人机都用于定位和指引，只有很少一部分无人机用于真正的探索任务，使得成本很高。除此之外，该系统属于集中式控制系统，如果某一架无人机出现错误或发生故障，可能会导致整个探索任务的失败。
2019年科学杂志子刊机器人(Science Robotics)收录的文章《Minimalnavigation solution for a swarm of tiny flying robots to explore an unknownenvironment》提出了一种名为群体梯度错误算法(Swarm Gradient Bug Algorithm，SGBA)，该方法使用群体微型无人机探索无GPS信号的未知室内空间。微型无人机相比于普通无人机具有更小的体型和更轻的重量，该文章中所使用的微型无人机重量只有27克，机身上搭载了一款STM32处理器和一颗内存大小为192kB的存储器。该微型无人机机身上配备了六个激光雷达用于测量前后左右上下六个方向上到障碍物的距离，从而起到避障的效果。同时，该微型机器人的底部还具有一块基于光流的视觉里程计，用于计算微型无人机在飞行过程中移动的距离和方向，可以达到辅助定位的效果。该文章使用这款微型无人机的硬件支持再搭配适当的调度算法和路径规划算法提出了SGBA方法，SGBA方法派出多架微型无人机朝不同的优先方向前进，利用简单的Bug算法进行导航和返回，最后绘制出所有无人机围绕障碍物所飞行的路线，从而得到整个未知室内空间的边界轮廓图。SGBA方法避免了在微型无人机上使用过于复杂的方法(如SLAM)，能够有效地在无GPS信号的未知室内空间中进行搜索和导航。但是，该方法对每架微型无人机所分配的任务可能会产生重合，比如多架微型无人机可能对同一片区域进行重复搜索，导致效率上的浪费。同时，该方法未将微型无人机的探索效率发挥到最大，因为该方法没有充分利用微型无人机的经得起损耗的特性(该特性使得微型无人机可以将全部能源用于探索新的边界，进而探索更大的空间)。
因此，为最大化利用每一架无人机的探索性能并绘制出整个未知室内空间的边界轮廓图，如何为多无人机系统设计一个探索无GPS信号未知室内空间的协同探索策略成为了本研究领域的热点问题。
发明内容
本发明要解决的技术问题是提供一种使用多无人机系统在无GPS信号的未知室内空间中协同探索的方法。此方法利用单架无人机对环境的感知，基于对不同路径规划算法的合理调度作为多无人机系统在未知室内空间中的导航策略，提高无人机在探索未知室内空间区域的效率和安全性，并绘制出整个未知室内空间的边界轮廓图。
为解决上述技术问题，本发明的技术方案是：构建由基站、I架无人机组成的多无人机协同探索系统，每架无人机实现探索的部件包括硬件层和软件层，硬件层包括激光雷达、单目相机、无线网络传感器和电池，软件层包括感知定位模块、地图边界环绕模块、接力追踪模块、地图边界绘制模块和电量控制模块，基站上安装有总控制软件。采用多无人机协同探索系统对未知室内空间进行探索：多无人机群处于未知室内空间的同一边界位置，基站上的总控制软件依次派出无人机对未知空间的边界进行环绕，地图边界环绕模块控制无人机对未知室内区域的边界进行探索，使无人机在地图边界进行环绕导航最终回到出发点，同时正在运行地图边界绘制模块的无人机对其飞行轨迹进行实时绘制；接力追踪模块在未知空间控制多架无人机之间接力，当前一架无人机能源不足时，总控制软件立即派出下一架无人机接力前一架无人机，当接力成功时，后一架无人机继续对边界进行环绕；多架无人机按追踪接力的方式对边界环绕获得多幅轨迹图，当最后一架无人机返回到起始点时停止对未知室内空间探索；最后，总控制软件将所有无人机绘制出的轨迹图进行拼接得到整个地图的边界轮廓图，完成对整个未知室内环境的探索任务。
本发明包括以下步骤：
第一步，构建多无人机协同探索系统。多无人机协同探索系统由I架无人机、一个基站组成，无人机编号为1,2,3,…,i,…,I，I为正整数，要求I足够大，使得I架无人机保证能完成未知室内空间探索，1≤i≤I，基站和无人机都部署于未知室内空间的同一边界位置。
基站上安装有总控制软件，与I架无人机通过无线信号进行通信，完成无人机调度、实时无人机轨迹显示以及地图轨迹拼接的功能。总控制软件发出指令启动无人机，命令编号为i的无人机进行边界环绕；从编号为i的无人机接收编号为i的无人机的位置坐标信息、剩余电量。若编号为i的无人机电量小于等于低电量阈值A(A一般为无人机初始总电量的30％)，则总控制软件命令编号为i+1的无人机对第i架无人机进行接力。
每一架无人机均由硬件层和软件层组成。硬件层搭载有六个激光雷达、一台单目相机、无线信号传感器、电池，以提供飞行、感知、测距、通讯等基础功能，硬件层与软件层的感知定位模块和电量控制模块相连，每一架无人机能够朝前、后、左、右四个方向飞行。六个激光雷达用于感知周围的环境信息，获取障碍物和室内空间边界的位置，与软件层的感知定位模块相连，将探测到的距离障碍物的距离传递给感知定位模块。单目相机通过不断拍摄视频以基于光流的视觉里程计方法计算无人机的移动位置和方向，与软件层的感知定位模块相连，将无人机的移动方向和移动距离传输给感知定位模块。每一架无人机在初始时都有100％电量，其电量会随着无人机飞行而不断减少，无人机每飞行1m消耗1％的电量；当电量控制模块发现无人机电池电量仅剩余A时向基站的总控制软件传递接力信号。无线网络传感器用于基站与无人机之间的通讯，基站将将要移动的方向和距离发送给无人机，无人机的无线网络传感器将移动的轨迹和周围障碍物信息发送给基站。
每台无人机的软件层包括感知定位模块、地图边界环绕模块、接力追踪模块、地图边界绘制模块、电量控制模块。
感知定位模块与激光雷达、单目相机、地图边界环绕模块、接力追踪模块相连，在未知空间中进行障碍物感知、障碍物测距和定位。感知定位模块从激光雷达获得前后左右上下六个方向上无人机到障碍物的距离，若得到的某一方向上的无人机到障碍物距离小于等于无人机可视距离α(α设为1米)，则判定该方向上存在障碍物且无人机处于该障碍物附近。感知定位模块从单目相机获取视频图像，利用基于光流的视觉里程计方法实现定位功能，即计算出无人机相对于起点移动的距离和方向，得到无人机的坐标位置，将无人机的坐标位置和周围环境信息发送给地图边界环绕模块、接力追踪模块。
地图边界环绕模块与感知定位模块、接力追踪模块、地图边界绘制模块、电量控制模块和总控制软件相连。地图边界环绕模块从感知定位模块接收无人机的坐标位置，控制无人机围绕未知室内空间的边界进行飞行，获得无人机探索到的未知室内空间的边界信息，并将未知室内空间的边界信息发送给地图边界绘制模块，以便地图边界绘制模块因此实时绘制无人机轨迹。当无人机的电量小于等于A的时候，地图边界环绕模块接收来自电量控制模块的低电量信息，将无人机当前位置信息发送给基站的总控制软件，表明当前无人机需要接力。当后续无人机对当前无人机接力成功时，地图边界环绕模块停止工作。
接力追踪模块与感知定位模块、地图边界环绕模块和总控制软件相连，从感知定位模块接收无人机的坐标位置，采用Tracking-D*Lite算法控制两架无人机之间的接力，该算法将D*Lite算法(见文献“Koenig S,Likhachev M.D*lite[J].Aaai/iaai,2002,15.”,Koenig S等人的论文：D*lite)和I-ARA*算法(见文献“Sun X,Uras T,Koenig S,etal.Incremental ARA*：an incremental anytime search algorithm for moving-targetsearch[C]//Proceedings of the Twenty-Second International Conference onInternational Conference on Automated Planning and Scheduling.2012:243-251.”，Koenig S等人的论文：增量ARA*：一种用于移动目标搜索的增量任意时间搜索算法)相结合。采用D*Lite算法在未知环境中对固定目标点进行路径规划，采用I-ARA*算法在已知环境中对移动目标点进行追踪。接力追踪模块在已知起点和目标点位置的情况下，对未知空间中移动目标点(即移动的无人机)进行追踪。当编号为i+1的无人机的接力追踪模块从总控制软件接收到信号时，使用Tracking-D*Lite算法对编号为i的无人机进行接力，当接力成功时，第i+1架无人机的地图边界环绕模块开始探索边界。
地图边界绘制模块与地图边界环绕模块相连，与地图边界环绕模块同时运行，从地图边界环绕模块接收每架无人机的实时位置，记录下每架无人机在边界探索过程中的位置信息，实时绘制出每架无人机的飞行轨迹。
电量控制模块与地图边界环绕模块、总控制软件相连，该模块负责无人机剩余电量的实时监控，监控硬件层的电池电量信息。电量控制模块将无人机的电量实时传送给地图边界环绕模块；若正在进行边界探索的无人机的电量控制模块发现剩余电量小于等于A，则向基站的总控制软件传递接力信号，表明需要下一架无人机进行接力。
第二步，采用多无人机协同探索系统对未知室内空间进行探索，方法是：
2.1初始化无人机编号i＝1。
2.2编号为i的无人机从其激光雷达获得无人机前后左右四个方向无人机到障碍物的距离，若得到的某一方向上的障碍物与无人机距离小于等于无人机可视距离α，则判定该方向存在障碍物且无人机处于该障碍物附近，以此方法获得无人机前后左右四个方向上无人机到障碍物的距离，将无人机到障碍物的距离发送给感知定位模块，编号为i的无人机的感知定位模块将前、后、左、右四个方向无人机到障碍物的距离发送给地图边界环绕模块。
2.3编号为i的无人机的电量控制模块实时监控编号为i的无人机电量，若编号为i的无人机电量小于等于A，电量控制模块向基站的总控制软件发出接力信号，转2.4；若编号为i的无人机电量大于A，转2.5，同时编号为i的无人机的地图边界绘制模块按2.8的流程对地图边界进行实时的绘制。
2.4当前步骤表明编号为i+1的无人机需要对编号为i的无人机进行接力，编号为i的无人机启动地图边界环绕模块，编号为i+1的无人机启动接力追踪模块。同时，编号为i的无人机的地图边界环绕模块、总控制软件、编号为i+1的无人机的接力追踪模块、编号为i的无人机的地图边界绘制模块并行工作，完成无人机对未知室内空间的边界信息的探索，无人机的接力、无人机轨迹的绘制。具体为：
编号为i的无人机的地图边界环绕模块按2.5的流程围绕未知室内空间的边界进行飞行，获得无人机探索到的未知室内空间的边界信息；编号为i的无人机的地图边界环绕模块运行的同时，总控制软件按2.6的流程将编号为i的无人机的实时位置发送给编号为i+1的无人机的接力追踪模块；同时编号为i+1的无人机的接力追踪模块按2.7的流程对编号为i的无人机进行接力追踪；同时编号为i的无人机的地图边界绘制模块按2.8的流程对地图边界进行实时的绘制。
2.5编号为i的无人机的地图边界环绕模块采用Wall-Around算法围绕未知室内空间的边界进行飞行，获得无人机探索到的未知室内空间的边界信息：
2.5.1由于无人机飞行于固定高度，可以对未知室内空间在二维平面上及逆行场景建立平面直角坐标系，方法是：编号为i的无人机的地图边界环绕模块从感知定位模块获取前、后、左、右四个方向无人机到障碍物的距离，得到以无人机起点为原点的平面直角坐标系，平面直角坐标系中每一个坐标值交点称为网格地图结点，并以平面直角坐标系原点为中心分为前、后、左、右四个方向，其中x正方向为右，x负方向为左，y正方向为前，y的负方向为后。单架无人机在某一时刻的运动方式只能朝前、后、左、右四个方向的某一方向运动。
2.5.2地图边界环绕模块根据前、后、左、右四个方向无人机到障碍物的距离，计算编号为i的无人机的MD、ND、OD和PD四个方向状态量，控制编号为i的无人机运动方向的切换，以使编号为i的无人机进行边界环绕，MD(Main Direction)为主方向，ND(NextDirection)为下一运动方向，OD(Opposite Direction)为反方向，PD(PreviousDirection)为上一运动方向。MD表示当前无人机在环绕边界过程中指向边界的方向，ND表示当前无人机在环绕边界过程中下一步将要前进的方向，OD表示与MD相反的方向，PD表示与ND相反的方向。
控制方法如下：
2.5.2.1地图边界环绕模块从激光雷达得到前后左右四个方向无人机到障碍物的距离，找到距离障碍物最近的方向，将此方向设为MD，通过逆时针计算规则得到无人机环绕边界飞行的其余三个运动方向状态量。逆时针计算规则是：MD逆时针旋转90度得到ND，ND逆时针旋转90度得到OD，OD逆时针旋转90度得到PD，PD逆时针旋转90度得到MD。例如，当MD为右时，则ND为后，OD为左，PD为前。以此类推，当无人机任意运动方向状态量已知时，可以计算出所有其它状态量。
2.5.2.2若地图边界环绕模块从激光雷达获知无人机MD方向有障碍物且ND方向无障碍物，则所有运动方向状态量保持不变，控制无人机朝ND方向前进1米，转2.5.3。
2.5.2.3若地图边界环绕模块从激光雷达获知无人机MD方向上无障碍物，则将ND设为MD方向，通过新的ND按照逆时针计算规则重新计算MD、OD和PD，并控制无人机朝ND方向前进1米，转2.5.3。
2.5.2.4若地图边界环绕模块从激光雷达获知无人机MD和ND方向均有障碍物且OD方向无障碍物，则将ND设为OD方向，通过新的ND按照逆时针计算规则重新计算MD、OD和PD，并控制无人机朝ND方向前进1米，转2.5.3。
2.5.2.5若地图边界环绕模块从激光雷达获知无人机MD、ND、OD方向均有障碍物且PD方向无障碍物，则将ND设为PD方向，通过新的ND按照逆时针计算规则重新计算MD、OD和PD，并控制无人机朝ND方向前进1米，转2.5.3。
2.5.3若编号为i的无人机的感知定位模块获取编号为i的无人机的当前坐标位置不为基站坐标，表明整个未知室内空间还未被全部探索，则转2.5.2继续进行边界环绕；若编号为i的无人机的感知定位模块获取编号为i的无人机的当前坐标位置为基站坐标，表明整个未知室内空间的封闭边界已被全部探索，探索完成，转2.8；若编号为i的无人机的电量感知模块获得编号为i的无人机电量等于A，则电量控制模块向基站总控制软件发出接力信号，并且编号为i的无人机的地图边界环绕模块将编号为i的无人机的当前坐标发送给基站总控制软件，总控制软件转2.6，编号为i的无人机转2.5.2继续进行边界环绕；若编号为i的无人机的电量感知模块获得编号为i的无人机电量大于0且小于A，则编号为i的无人机转2.5.2继续进行边界环绕；若编号为i的无人机的电量感知模块获得编号为i的无人机电量等于0，则编号为i的无人机转2.5.4；
2.5.4编号为i的无人机停止飞行，编号为i的无人机的边界环绕模块和地图边界绘制模块停止运行，令i＝i+1，若编号为i的无人机未接力成功，则转2.7；若编号为i的无人机接力成功，则转2.5；
2.6总控制软件判定是否从编号为i的无人机接收到编号为i的无人机电量不足信号，若接收到电量不足信号，令i＝i+1，转2.7；若未接收到电量不足信号，转2.6继续等待。
2.7编号为i的无人机的接力追踪模块从编号为i的无人机的感知定位模块获取无人机前后左右四个方向上无人机到障碍物的距离，从基站总控制软件获取编号为i-1的无人机坐标位置，采用Tracking-D*Lite算法接力追踪编号为i-1的无人机：
2.7.1编号为i的无人机的接力追踪模块通过2.5.1建立的平面直角坐标系，通过感知定位模块不断感知并获取周围环境，编号为i的无人机接力追踪模块对未知室内空间在二维平面上进行建模并设置Tracking-D*Lite算法所需要使用的变量，方法如下：
2.7.1.1将整个未知室内空间抽象为一个二维平面网格图，设位于出发点的编号为i+1的无人机的位置为二维平面网格图的原点，单位距离为1米，二维平面网格图中的每一个网格表示未知室内空间中对应的位置。
2.7.1.2令集合S表示整个未知室内空间的二维网格地图结点集合，后继结点集合Succ(s)表示所有从网格地图结点s的扩展出子结点组成的集合，即从s扩展的结点，
s∈S；前继结点集合Pred(s)表示s的父结点组成的集合，
c(s,s′)表示从s到邻接结点s′的单步代价值，因为无人机每次只能向前后左右四个方向移动1米的距离，所以c(s,s′)＝1；s′∈S,s′是s的前继邻接结点。
2.7.2编号为i的无人机对编号为i-1的无人机进行追踪，在此接力过程中需要不断地进行路径规划，方法如下：
2.7.2.1对所需状态量进行初始化。
2.7.2.1.1令编号为i的无人机初始坐标为(x0,y0)，并初始化编号为i的无人机当前位置snow为(x0,y0)。
2.7.2.1.2编号为i的无人机从基站获取编号为i-1的无人机位置(xg,yg)，并初始化编号为i-1的无人机当前目标点gnow为(xg,yg)。
2.7.2.1.3对于S中的任意一个结点s，令rhs(s)＝g(s)＝∞，
其中g(s)表示从gnow到s的最短距离；rhs(s)表示基于s的父结点parent(s)计算出的g(s)的预测值，目的是为了寻找更小代价路径结点，rhs(s)按公式(一)计算，其中spred为s的前继邻接结点，c(s,spred)表示从s到邻接结点spred的单步代价值。
2.7.2.1.4初始化可进行搜索结点的优先队列
U用于记录地图中可进行搜索的结点；初始化需要删除的结点队列
D用于存储每次重新搜索过程中需要删除的结点；U中的结点s的优先级k(s)计算方式如公式(二)所示。
其中，k(s)由两部分构成，分别是k1(s)＝min(g(s),rhs(s))+h(s,gnow)+km，k2(s)＝min(g(s),rhs(s))；h(s,gnow)为启发式函数表示从结点s到gnow的最小代价评估值，该代价评估值使用曼哈顿距离计算；km表示每一次起点改变之后的启发式补偿值，以保持后续搜索中优先队列U中键值的严格升序，初始值为1。
k(s)与结点s′的优先级k(s′)的比较方法如公式(三)所示，k(s)值越小表示s优先级越高，k(s)值越大表示s优先级越低。
其中，s′表示与s进行优先级比较的结点，k(s′)为s′的优先级，若k1(s)≤k1(s′)，则k(s)≤k(s′)，表明s的优先级高于s′的优先级；若k1(s)＝k1(s′)并且k2(s)≤k2(s′)，则k(s)≤k(s′)，表明s的优先级高于s′的优先级；其余情况都表示k(s)>k(s′)，表明s的优先级低于s′的优先级。公式(二)用于U中元素优先级的计算，公式(三)用于U中元素优先级的比较。
2.7.2.2令rhs(gnow)＝0，并将gnow加入优先队列U中；采用反向探索，即从目标点gnow扩展到起点snow，构建以目标点gnow为根结点的搜索树，计算编号为i的无人机下一步将要移动到的位置snext。令其他可行结点集合Sother表示中存储集合S中的除snow之外的其他可行结点，即Sother＝{s1,s2,…,sother}；采用启发式函数公式(四)计算从snow出发到各可行结点的曼哈顿距离h(snow,sother)，其中sother∈Sother，snow.x表示snow的横坐标，snow.y表示snow的纵坐标，sother.x表示sother的横坐标，sother.y表示sother的纵坐标。
h(snow,sother)＝|snow.x-sother.x|+|snow.y-sother.y| 公式（四）
构建搜索树的具体步骤如下：
2.7.2.2.1从优先队列U中取出优先级最高的可探索结点，令为u；若结点u＝snow，则表明已探索出以gnow为根结点并包含snow的一棵搜索树，转2.7.2.2.3；若结点u≠snow，则表明当前探索过程中还未探索snow，需要继续进行结点扩展，转2.7.2.2.2。
2.7.2.2.2如果g(u)>rhs(u)，则令g(u)＝rhs(u)，将u从U中删除，同时将结点u可被扩展的任意邻居结点u′(即满足g(u′)≠rhs(u′))加入到队列U；如果g(u)≤rhs(u)，则令g(u)＝∞，同时将结点u和其所有可被扩展的邻居结点u′(即g(u′)与rhs(u′)不相等的结点)加入到队列U；将从结点u扩展的所有邻居结点u′的父结点设为结点u，即令parent(u′)＝u，最后转2.7.2.2.1。
2.7.2.2.3通过2.7.2.2.1和2.7.2.2.2的步骤，构建了以结点gnow为根结点的一棵搜索树，令搜索树为tree，tree中包含从snow到gnow的路径，令为从snow到gnow的路径为path。
2.7.2.2.4令snext为path中取出的snow下一结点。
2.7.2.3编号为i的无人机从snow移动到snext，即令snow＝snext；更新Sother，令其他可行结点集合Sother表示中存储集合S中的除snow之外的其他可行结点，即Sother＝{s1,s2,…,sother}；同时令km＝km+1。
2.7.2.4编号为i的无人机通过总控制软件获取编号为i-1的无人机当前位置gnow′，令gnow＝gnow′；
2.7.2.5编号为i的无人机的感知定位模块判断前后左右四个方向上的障碍物信息，更新编号为i的无人机到障碍物的距离。
2.7.2.6根据公式(四)重新计算更新后的snow位置和集合Sother中各个结点的曼哈顿距离h(snow,sother)，其中sother∈Sother。
2.7.2.7初始化用于记录重新搜索过程中删除的结点队列
对于tree中的任意一个dv，若结点dv不属于snow的子结点，则将结点dv从tree删除，令g(dv)＝rhs(dv)＝∞，并将结点dv加入D中，则D＝{d1,d2,d3,…,dv,…,,dV}，V为队列D中结点的个数；
2.7.2.8重新使用D中的部分结点以加快下一次规划速度，对于D中任意结点dv的任意邻居结点nneighbor，若nneighbor以snow为起点并且nneighbor不属于队列U，则将dv加入队列U中；
2.7.2.9若编号为i的无人机的snow＝gnow，表明编号为i的无人机对编号为i-1的无人机接力成功，则编号为i的无人机运行地图边界环绕模块，转2.5；否则表示未接力成功，转2.7.2.2步；
2.8编号为i的无人机的地图边界绘制模块在编号为i的无人机的地图边界环绕模块对未知室内空间进行边界环绕的同时，进行地图边界绘制，若编号为i的无人机返回无人机出发的起点，则完成对整个位置空间的遍历，转2.9；若编号为i的无人机没有返回无人机出发的起点，则编号为i的无人机的地图边界绘制模块根据计算出的坐标位置实时以10次每秒的频率绘制编号为i的无人机的飞行轨迹，每次绘制飞行轨迹的方法是：
2.8.1令时间变量t1＝编号为i的无人机的系统时间，编号为i的无人机的地图边界绘制模块从编号为i的无人机的边界环绕模块获取地图边界信息和无人机实时位置；
2.8.2令编号为i的无人机在x轴方向上移动的绝对距离为Δx，在y轴方向上移动的绝对距离为Δy，根据Δx,Δy计算编号为i的无人机的当前坐标，具体方法是：如果编号为i的无人机朝x轴正方向移动，朝y轴正方向移动，则编号为i的无人机当前坐标为(Δx,Δy)；如果编号为i的无人机朝x轴正方向移动，朝y轴负方向移动，则编号为i的无人机当前坐标为(Δx,-Δy)；如果编号为i的无人机朝x轴负方向移动，朝y轴正方向移动，则编号为i的无人机当前坐标为(-Δx,Δy)；如果编号为i的无人机朝x轴负方向移动，朝y轴负方向移动，则编号为i的无人机当前坐标为(-Δx,-Δy)；
2.8.3将当前坐标标注为航行轨迹点，并将当前坐标与上一次的标注航行轨迹点连线，依此绘制航行轨迹；
2.8.4若编号为i的无人机的边界环绕模块仍在运行，转2.8.5；若编号为i的无人机的边界环绕模块没在运行，则编号为i的无人机的地图边界绘制模块停止运行，令i＝i+1，若编号为i的无人机未接力成功，转2.7；若编号为i的无人机以接力成功，则转2.5；
2.8.5令时间变量t2＝编号为i的无人机的系统时间，若t2-t1<0.1秒，转2.8.5；若t2-t1＝0.1秒，转2.8.1；
2.9令参与未知区域协同探索的无人机总数II＝i；总控制软件对从II架无人机处获取的每架无人机的轨迹进行拼接，得到最终的未知室内空间边界轮廓图，方法如下：
2.9.1.1将无人机轨迹根据无人机编号分配为1,2,3,…,ii,…,II，II≤I；
2.9.1.2令ii＝1；
2.9.1.3从第ii段轨迹的终点连接到第ii+1段轨迹的起点；
2.9.1.4令ii＝ii+1，若ii＝II，则完成所有轨迹的拼接，得到最终的未知室内空间边界轮廓图，转第三步；若ii<II，则转2.9.1.3。
第三步，结束。
采用本发明可以达到以下技术效果：
1.本发明第二步中地图边界环绕模块采用Wall-Around算法控制无人机对未知室内区域的边界进行探索，该算法使无人机按照一定规则在地图边界进行环绕导航最终回到出发点。该算法为绘制边界轮廓图提供准确的位置信息和数据信息，算法过程较为简单，易于在无人机上部署。
2.本发明第二步中接力追踪模块采用Tracking-D*Lite算法在未知空间控制多架无人机之间接力，该算法主要完成未知空间目标追踪的功能，通过对每一次规划路径的搜索树进行修剪并重用修剪之后的部分结点，减少每次重规划过程中搜索时间和扩展结点数量，以加快搜索效率和路径规划质量，追踪未知环境中的移动目标点时既准确又快速。
附图说明
图1为本发明整体流程图。
图2为本发明第一步构建的目标跟踪系统逻辑结构图。
图3为本发明第二步中构建的二维网格地图。其中图3(a)展示初始时网格地图环境、起点和终点的位置以及障碍物位置；图3(b)展示处于起点位置无人机可观察到的环境信息以及到达每个结点的曼哈顿距离。
图4为本发明第二步中使用Tracking-D*Lite算法进行第一次路径规划的过程图。其中图4(a)至图4(h)展示Tracking-D*Lite算法第一次路径规划的结点扩展示意图；图4(i)表示第一次路径规划的结果。
图5为本发明第二步中使用Tracking-D*Lite算法在更新起点和目标点之后的二维网格地图。其中图5(a)表示第一次路径规划之后移动起点和目标点之后的网格地图环境、起点和终点的位置以及障碍物位置；图5(b)展示处于起点位置无人机可观察到的环境信息以及到达每个结点的曼哈顿距离。
图6为本发明第二步中使用Tracking-D*Lite算法修剪搜索树和第二次路径规划的过程图。其中图6(a)展示了对搜索树修剪的过程，图6(b)展示了重用部分被删除结点的过程；图6(c)至图6(h)展示Tracking-D*Lite算法第二次路径规划的结点扩展示意图；图6(i)表示第一次路径规划的结果。
图7为本发明在仿真空间中最终绘制的未知室内空间边界轮廓图；
图8为本发明与SGBA方法在探索同一未知室内场景时单架无人机探索覆盖区域的百分比对比柱状图；
图9为本发明与SGBA方法在探索同一未知室内场景时所有无人机探索总覆盖区域的百分比对比柱状图；
具体实施方式
为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。
如图1所示，本发明包括以下步骤：
第一步，构建多无人机协同探索系统。多无人机协同探索系统由I架无人机、一个基站组成，无人机编号为1,2,3,…,i,…,I，I为正整数，要求I足够大，使得I架无人机保证能完成未知室内空间探索，1≤i≤I，基站和无人机都部署于未知室内空间的同一边界位置。
基站上安装有总控制软件，与I架无人机通过无线信号进行通信，完成无人机调度、实时无人机轨迹显示以及地图轨迹拼接的功能。总控制软件发出指令启动无人机，命令编号为i的无人机进行边界环绕；从编号为i的无人机接收编号为i的无人机的位置坐标信息、剩余电量。若编号为i的无人机电量小于等于低电量阈值A(A一般为无人机初始总电量的30％)，则总控制软件命令编号为i+1的无人机对第i架无人机进行接力。
每一架无人机均由硬件层和软件层组成。硬件层搭载有六个激光雷达、一台单目相机、无线信号传感器、电池，以提供飞行、感知、测距、通讯等基础功能，硬件层与软件层的感知定位模块和电量控制模块相连，每一架无人机能够朝前、后、左、右四个方向飞行。六个激光雷达用于感知周围的环境信息，获取障碍物和室内空间边界的位置，与软件层的感知定位模块相连，将探测到的距离障碍物的距离传递给感知定位模块。单目相机通过不断拍摄视频以基于光流的视觉里程计方法计算无人机的移动位置和方向，与软件层的感知定位模块相连，将无人机的移动方向和移动距离传输给感知定位模块。每一架无人机在初始时都有100％电量，其电量会随着无人机飞行而不断减少，无人机每飞行1m消耗1％的电量；当电量控制模块发现无人机电池电量仅剩余A时向基站的总控制软件传递接力信号。无线网络传感器用于基站与无人机之间的通讯，基站将将要移动的方向和距离发送给无人机，无人机的无线网络传感器将移动的轨迹和周围障碍物信息发送给基站。
每台无人机的软件层包括感知定位模块、地图边界环绕模块、接力追踪模块、地图边界绘制模块、电量控制模块。
感知定位模块与激光雷达、单目相机、地图边界环绕模块、接力追踪模块相连，在未知空间中进行障碍物感知、障碍物测距和定位。感知定位模块从激光雷达获得前后左右上下六个方向上无人机到障碍物的距离，若得到的某一方向上的无人机到障碍物距离小于等于无人机可视距离α(α设为1米)，则判定该方向上存在障碍物且无人机处于该障碍物附近。感知定位模块从单目相机获取视频图像，利用基于光流的视觉里程计方法实现定位功能，即计算出无人机相对于起点移动的距离和方向，得到无人机的坐标位置，将无人机的坐标位置和周围环境信息发送给地图边界环绕模块、接力追踪模块。
地图边界环绕模块与感知定位模块、接力追踪模块、地图边界绘制模块、电量控制模块和总控制软件相连。地图边界环绕模块从感知定位模块接收无人机的坐标位置，控制无人机围绕未知室内空间的边界进行飞行，获得无人机探索到的未知室内空间的边界信息，并将未知室内空间的边界信息发送给地图边界绘制模块，以便地图边界绘制模块因此实时绘制无人机轨迹。当无人机的电量小于等于A的时候，地图边界环绕模块接收来自电量控制模块的低电量信息，将无人机当前位置信息发送给基站的总控制软件，表明当前无人机需要接力。当后续无人机对当前无人机接力成功时，地图边界环绕模块停止工作。
接力追踪模块与感知定位模块、地图边界环绕模块和总控制软件相连，从感知定位模块接收无人机的坐标位置，采用Tracking-D*Lite算法控制两架无人机之间的接力，该算法将D*Lite算法(见文献“Koenig S,Likhachev M.D*lite[J].Aaai/iaai,2002,15.”,Koenig S等人的论文：D*lite)和I-ARA*算法(见文献“Sun X,Uras T,Koenig S,etal.Incremental ARA*：an incremental anytime search algorithm for moving-targetsearch[C]//Proceedings of the Twenty-Second International Conference onInternational Conference on Automated Planning and Scheduling.2012:243-251.”，Koenig S等人的论文：增量ARA*：一种用于移动目标搜索的增量任意时间搜索算法)相结合。采用D*Lite算法在未知环境中对固定目标点进行路径规划，采用I-ARA*算法在已知环境中对移动目标点进行追踪。接力追踪模块在已知起点和目标点位置的情况下，对未知空间中移动目标点(即移动的无人机)进行追踪。当编号为i+1的无人机的接力追踪模块从总控制软件接收到信号时，使用Tracking-D*Lite算法对编号为i的无人机进行接力，当接力成功时，第i+1架无人机的地图边界环绕模块开始探索边界。
地图边界绘制模块与地图边界环绕模块相连，与地图边界环绕模块同时运行，从地图边界环绕模块接收每架无人机的实时位置，记录下每架无人机在边界探索过程中的位置信息，实时绘制出每架无人机的飞行轨迹。
电量控制模块与地图边界环绕模块、总控制软件相连，该模块负责无人机剩余电量的实时监控，监控硬件层的电池电量信息。电量控制模块将无人机的电量实时传送给地图边界环绕模块；若正在进行边界探索的无人机的电量控制模块发现剩余电量小于等于A，则向基站的总控制软件传递接力信号，表明需要下一架无人机进行接力。
第二步，采用多无人机协同探索系统对未知室内空间进行探索，方法是：
2.1初始化无人机编号i＝1。
2.2编号为i的无人机从其激光雷达获得无人机前后左右四个方向无人机到障碍物的距离，若得到的某一方向上的障碍物与无人机距离小于等于无人机可视距离α，则判定该方向存在障碍物且无人机处于该障碍物附近，以此方法获得无人机前后左右四个方向上无人机到障碍物的距离，将无人机到障碍物的距离发送给感知定位模块，编号为i的无人机的感知定位模块将前、后、左、右四个方向无人机到障碍物的距离发送给地图边界环绕模块。
2.3编号为i的无人机的电量控制模块实时监控编号为i的无人机电量，若编号为i的无人机电量小于等于A，电量控制模块向基站的总控制软件发出接力信号，转2.4；若编号为i的无人机电量大于A，转2.5，同时编号为i的无人机的地图边界绘制模块按2.8的流程对地图边界进行实时的绘制。
2.4当前步骤表明编号为i+1的无人机需要对编号为i的无人机进行接力，编号为i的无人机启动地图边界环绕模块，编号为i+1的无人机启动接力追踪模块。同时，编号为i的无人机的地图边界环绕模块、总控制软件、编号为i+1的无人机的接力追踪模块、编号为i的无人机的地图边界绘制模块并行工作，完成无人机对未知室内空间的边界信息的探索，无人机的接力、无人机轨迹的绘制。具体为：
编号为i的无人机的地图边界环绕模块按2.5的流程围绕未知室内空间的边界进行飞行，获得无人机探索到的未知室内空间的边界信息；编号为i的无人机的地图边界环绕模块运行的同时，总控制软件按2.6的流程将编号为i的无人机的实时位置发送给编号为i+1的无人机的接力追踪模块；同时编号为i+1的无人机的接力追踪模块按2.7的流程对编号为i的无人机进行接力追踪；同时编号为i的无人机的地图边界绘制模块按2.8的流程对地图边界进行实时的绘制。
2.5编号为i的无人机的地图边界环绕模块采用Wall-Around算法围绕未知室内空间的边界进行飞行，获得无人机探索到的未知室内空间的边界信息：
2.5.1由于无人机飞行于固定高度，可以对未知室内空间在二维平面上及逆行场景建立平面直角坐标系，方法是：编号为i的无人机的地图边界环绕模块从感知定位模块获取前、后、左、右四个方向无人机到障碍物的距离，得到以无人机起点为原点的平面直角坐标系，平面直角坐标系中每一个坐标值交点称为网格地图结点，并以平面直角坐标系原点为中心分为前、后、左、右四个方向，其中x正方向为右，x负方向为左，y正方向为前，y的负方向为后。单架无人机在某一时刻的运动方式只能朝前、后、左、右四个方向的某一方向运动。
2.5.2地图边界环绕模块根据前、后、左、右四个方向无人机到障碍物的距离，计算编号为i的无人机的MD、ND、OD和PD四个方向状态量，控制编号为i的无人机运动方向的切换，以使编号为i的无人机进行边界环绕，MD(Main Direction)为主方向，ND(NextDirection)为下一运动方向，OD(Opposite Direction)为反方向，PD(PreviousDirection)为上一运动方向。MD表示当前无人机在环绕边界过程中指向边界的方向，ND表示当前无人机在环绕边界过程中下一步将要前进的方向，OD表示与MD相反的方向，PD表示与ND相反的方向。
控制方法如下：
2.5.2.1地图边界环绕模块从激光雷达得到前后左右四个方向无人机到障碍物的距离，找到距离障碍物最近的方向，将此方向设为MD，通过逆时针计算规则得到无人机环绕边界飞行的其余三个运动方向状态量。逆时针计算规则是：MD逆时针旋转90度得到ND，ND逆时针旋转90度得到OD，OD逆时针旋转90度得到PD，PD逆时针旋转90度得到MD。例如，当MD为右时，则ND为后，OD为左，PD为前。以此类推，当无人机任意运动方向状态量已知时，可以计算出所有其它状态量。
2.5.2.2若地图边界环绕模块从激光雷达获知无人机MD方向有障碍物且ND方向无障碍物，则所有运动方向状态量保持不变，控制无人机朝ND方向前进1米，转2.5.3。
2.5.2.3若地图边界环绕模块从激光雷达获知无人机MD方向上无障碍物，则将ND设为MD方向，通过新的ND按照逆时针计算规则重新计算MD、OD和PD，并控制无人机朝ND方向前进1米，转2.5.3。
2.5.2.4若地图边界环绕模块从激光雷达获知无人机MD和ND方向均有障碍物且OD方向无障碍物，则将ND设为OD方向，通过新的ND按照逆时针计算规则重新计算MD、OD和PD，并控制无人机朝ND方向前进1米，转2.5.3。
2.5.2.5若地图边界环绕模块从激光雷达获知无人机MD、ND、OD方向均有障碍物且PD方向无障碍物，则将ND设为PD方向，通过新的ND按照逆时针计算规则重新计算MD、OD和PD，并控制无人机朝ND方向前进1米，转2.5.3。
2.5.3若编号为i的无人机的感知定位模块获取编号为i的无人机的当前坐标位置不为基站坐标，表明整个未知室内空间还未被全部探索，则转2.5.2继续进行边界环绕；若编号为i的无人机的感知定位模块获取编号为i的无人机的当前坐标位置为基站坐标，表明整个未知室内空间的封闭边界已被全部探索，探索完成，转2.8；若编号为i的无人机的电量感知模块获得编号为i的无人机电量等于A，则电量控制模块向基站总控制软件发出接力信号，并且编号为i的无人机的地图边界环绕模块将编号为i的无人机的当前坐标发送给基站总控制软件，总控制软件转2.6，编号为i的无人机转2.5.2继续进行边界环绕；若编号为i的无人机的电量感知模块获得编号为i的无人机电量大于0且小于A，则编号为i的无人机转2.5.2继续进行边界环绕；若编号为i的无人机的电量感知模块获得编号为i的无人机电量等于0，则编号为i的无人机转2.5.4；
2.5.4编号为i的无人机停止飞行，编号为i的无人机的边界环绕模块和地图边界绘制模块停止运行，令i＝i+1，若编号为i的无人机未接力成功，则转2.7；若编号为i的无人机接力成功，则转2.5；
2.6总控制软件判定是否从编号为i的无人机接收到编号为i的无人机电量不足信号，若接收到电量不足信号，令i＝i+1，转2.7；若未接收到电量不足信号，转2.6继续等待。
2.7编号为i的无人机的接力追踪模块从编号为i的无人机的感知定位模块获取无人机前后左右四个方向上无人机到障碍物的距离，从基站总控制软件获取编号为i-1的无人机坐标位置，采用Tracking-D*Lite算法接力追踪编号为i-1的无人机：
2.7.1编号为i的无人机的接力追踪模块通过2.5.1建立的平面直角坐标系，通过感知定位模块不断感知并获取周围环境，编号为i的无人机接力追踪模块对未知室内空间在二维平面上进行建模并设置Tracking-D*Lite算法所需要使用的变量，方法如下：
2.7.1.1将整个未知室内空间抽象为一个二维平面网格图，设位于出发点的编号为i+1的无人机的位置为二维平面网格图的原点，单位距离为1米，二维平面网格图中的每一个网格表示未知室内空间中对应的位置。
2.7.1.2令集合S表示整个未知室内空间的二维网格地图结点集合，后继结点集合Succ(s)表示所有从网格地图结点s的扩展出子结点组成的集合，即从s扩展的结点，
s∈S；前继结点集合Pred(s)表示s的父结点组成的集合，
c(s,s′)表示从s到邻接结点s′的单步代价值，因为无人机每次只能向前后左右四个方向移动1米的距离，所以c(s,s′)＝1；s′∈S,s′是s的前继邻接结点。
2.7.2编号为i的无人机对编号为i-1的无人机进行追踪，在此接力过程中需要不断地进行路径规划，方法如下：
2.7.2.1对所需状态量进行初始化。
2.7.2.1.1令编号为i的无人机初始坐标为(x0,y0)，并初始化编号为i的无人机当前位置snow为(x0,y0)，即为图3(a)中的B1(第B行第1个网格)。
2.7.2.1.2编号为i的无人机从基站获取编号为i-1的无人机位置(xg,yg)，并初始化编号为i-1的无人机当前目标点gnow为(xg,yg)，即为图3(a)中的D3(第D行第3个网格)。
2.7.2.1.3对于S中的任意一个结点s，令rhs(s)＝g(s)＝∞，
其中g(s)表示从gnow到s的最短距离；rhs(s)表示基于s的父结点parent(s)计算出的g(s)的预测值，目的是为了寻找更小代价路径结点，rhs(s)按公式(一)计算，其中spred为s的前继邻接结点，c(s,spred)表示从s到邻接结点spred的单步代价值。
2.7.2.1.4初始化可进行搜索结点的优先队列
U用于记录地图中可进行搜索的结点；初始化需要删除的结点队列
D用于存储每次重新搜索过程中需要删除的结点；U中的结点s的优先级k(s)计算方式如公式(二)所示。
其中，k(s)由两部分构成，分别是k1(s)＝min(g(s),rhs(s))+h(s,gnow)+km，k2(s)＝min(g(s),rhs(s))；h(s,gnow)为启发式函数表示从结点s到gnow的最小代价评估值，该代价评估值使用曼哈顿距离计算；km表示每一次起点改变之后的启发式补偿值，以保持后续搜索中优先队列U中键值的严格升序，初始值为1。
k(s)与结点s′的优先级k(s′)的比较方法如公式(三)所示，k(s)值越小表示s优先级越高，k(s)值越大表示s优先级越低。
其中，s′表示与s进行优先级比较的结点，k(s′)为s′的优先级，若k1(s)≤k1(s′)，则k(s)≤k(s′)，表明s的优先级高于s′的优先级；若k1(s)＝k1(s′)并且k2(s)≤k2(s′)，则k(s)≤k(s′)，表明s的优先级高于s′的优先级；其余情况都表示k(s)>k(s′)，表明s的优先级低于s′的优先级。公式(二)用于U中元素优先级的计算，公式(三)用于U中元素优先级的比较。
2.7.2.2令rhs(gnow)＝0，并将gnow加入优先队列U中；采用反向探索，即从目标点gnow扩展到起点snow，构建以目标点gnow为根结点的搜索树，计算编号为i的无人机下一步将要移动到的位置snext。令其他可行结点集合Sother表示中存储集合S中的除snow之外的其他可行结点，即Sother＝{s1,s2,…,sother}；采用启发式函数公式(四)计算从snow出发到各可行结点的曼哈顿距离h(snow,sother)，其中sother∈Sother，snow.x表示snow的横坐标，snow.y表示snow的纵坐标，sother.x表示sother的横坐标，sother.y表示sother的纵坐标，计算结果如图3(b)所示。
h(snow,sother)＝|snow.x-sother.x|+|snow.y-sother.y| 公式(四)
算法过程如图4所示，图4中阴影结点代表处于优先队列中的结点，黑色结点表示障碍物，结点之间的箭头指向表示父结点指向子结点，每个结点v都有g(v)、rhs(v)以及优先队列键值，g(v)位于网格结点v的左上角，rhs(v)位于网格结点v的右上角，结点v的优先级位于网格的下半部分。过程如图4(a)至图4(i)和图6(c)至图6(i)所示，构建搜索树的具体步骤如下：
2.7.2.2.1从优先队列U中取出优先级最高的可探索结点，令为u；若结点u＝snow，则表明已探索出以gnow为根结点并包含snow的一棵搜索树，转2.7.2.2.3；若结点u≠snow，则表明当前探索过程中还未探索snow，需要继续进行结点扩展，转2.7.2.2.2。
2.7.2.2.2如果g(u)>rhs(u)，则令g(u)＝rhs(u)，将u从U中删除，同时将结点u可被扩展的任意邻居结点u′(即满足g(u′)≠rhs(u′))加入到队列U；如果g(u)≤rhs(u)，则令g(u)＝∞，同时将结点u和其所有可被扩展的邻居结点u′(即g(u′)与rhs(u′)不相等的结点)加入到队列U；将从结点u扩展的所有邻居结点u′的父结点设为结点u，即令parent(u′)＝u，最后转2.7.2.2.1。如图4(a)所示，从当前优先队列U取出优先级最高的可探索结点D3，对D3的邻居结点C3、D2和E3按照上述规则进行扩展，计算他们的g(C3)、rhs(C3)、g(D2)、rhs(D2)和g(E3)、rhs(E3)值，加入到队列U，最后将结点C3、D2和E3的父结点设为D3；图4(b)至图4(h)与图4(a)类似，都展示了扩展新结点的过程。
2.7.2.2.3通过2.7.2.2.1和2.7.2.2.2的步骤，构建了以结点gnow为根结点的一棵搜索树，令搜索树为tree，tree中包含从snow到gnow的路径，令为从snow到gnow的路径为path，如图4(i)所示最后规划出一条从B1到D3的路径。
2.7.2.2.4令snext为path中取出的snow下一结点。
2.7.2.3编号为i的无人机从snow移动到snext，即令snow＝snext；更新Sother，令其他可行结点集合Sother表示中存储集合S中的除snow之外的其他可行结点，即Sother＝{s1,s2,…,sother}；同时令km＝km+1。如图5(a)所示，编号为i的无人机的起点位置移动到C1。
2.7.2.4编号为i的无人机通过总控制软件获取编号为i-1的无人机当前位置gnow′，令gnow＝gnow′；如图5(a)所示，编号为i-1的无人机位置移动到C3。
2.7.2.5编号为i的无人机的感知定位模块判断前后左右四个方向上的障碍物信息，更新编号为i的无人机到障碍物的距离。
2.7.2.6根据公式(四)重新计算更新后的snow位置和集合Sother中各个结点的曼哈顿距离h(snow,sother)，其中sother∈Sother，如图5(b)所示为从当前的C1到网格中其他各位置的曼哈顿距离。
2.7.2.7初始化用于记录重新搜索过程中删除的结点队列
对于tree中的任意一个dv，若结点dv不属于snow的子结点，则将结点dv从tree删除，令g(dv)＝rhs(dv)＝∞，并将结点dv加入D中，则D＝{d1,d2,d3,…,dv,…,,dV}，V为队列D中结点的个数；如图6(a)所示A1、B1、C1、D1、E1、E2、E3、D3为加入队列D的结点。
2.7.2.8重新使用D中的部分结点以加快下一次规划速度，对于D中任意结点dv的任意邻居结点nneighbor，若nneighbor以snow为起点并且nneighbor不属于队列U，则将dv加入队列U中；如图6(b)所示，将D3加入队列U中，并重新计算g(D3)和rhs(D3)。
2.7.2.9若编号为i的无人机的snow＝gnow，表明编号为i的无人机对编号为i-1的无人机接力成功，则编号为i的无人机运行地图边界环绕模块，转2.5；否则表示未接力成功，转2.7.2.2步；
2.8编号为i的无人机的地图边界绘制模块在编号为i的无人机的地图边界环绕模块对未知室内空间进行边界环绕的同时，进行地图边界绘制，若编号为i的无人机返回无人机出发的起点，则完成对整个位置空间的遍历，转2.9；若编号为i的无人机没有返回无人机出发的起点，则编号为i的无人机的地图边界绘制模块根据计算出的坐标位置实时以10次每秒的频率绘制编号为i的无人机的飞行轨迹，每次绘制飞行轨迹的方法是：
2.8.1令时间变量t1＝编号为i的无人机的系统时间，编号为i的无人机的地图边界绘制模块从编号为i的无人机的边界环绕模块获取地图边界信息和无人机实时位置；
2.8.2令编号为i的无人机在x轴方向上移动的绝对距离为Δx，在y轴方向上移动的绝对距离为Δy，根据Δx,Δy计算编号为i的无人机的当前坐标，具体方法是：如果编号为i的无人机朝x轴正方向移动，朝y轴正方向移动，则编号为i的无人机当前坐标为(Δx,Δy)；如果编号为i的无人机朝x轴正方向移动，朝y轴负方向移动，则编号为i的无人机当前坐标为(Δx,-Δy)；如果编号为i的无人机朝x轴负方向移动，朝y轴正方向移动，则编号为i的无人机当前坐标为(-Δx,Δy)；如果编号为i的无人机朝x轴负方向移动，朝y轴负方向移动，则编号为i的无人机当前坐标为(-Δx,-Δy)；
2.8.3将当前坐标标注为航行轨迹点，并将当前坐标与上一次的标注航行轨迹点连线，依此绘制航行轨迹；
2.8.4若编号为i的无人机的边界环绕模块仍在运行，转2.8.5；若编号为i的无人机的边界环绕模块没在运行，则编号为i的无人机的地图边界绘制模块停止运行，令i＝i+1，若编号为i的无人机未接力成功，转2.7；若编号为i的无人机以接力成功，则转2.5；
2.8.5令时间变量t2＝编号为i的无人机的系统时间，若t2-t1<0.1秒，转2.8.5；若t2-t1＝0.1秒，转2.8.1；
2.9令参与未知区域协同探索的无人机总数II＝i；总控制软件对从II架无人机处获取的每架无人机的轨迹进行拼接，得到最终的未知室内空间边界轮廓图，方法如下：
2.9.1.1将无人机轨迹根据无人机编号分配为1,2,3,…,ii,…,II，II≤I；
2.9.1.2令ii＝1；
2.9.1.3从第ii段轨迹的终点连接到第ii+1段轨迹的起点；
2.9.1.4令ii＝ii+1，若ii＝II，则完成所有轨迹的拼接，得到最终的未知室内空间边界轮廓图，转第三步；若ii<II，则转2.9.1.3。
第三步，结束。采用以下实验验证本发明的效果：
1.加载无人机模型：仿真场景中会预先分配M架微型无人机，基站和所有无人机初始位置都处于同一地图边界区域。其中，每一架无人机搭载了激光雷达、视觉里程计、电池等硬件支撑模块以提供飞行、感知、测距等基础功能，并且每一架无人机能够朝前、后、左、右四个方向飞行。每一架微型无人机在初始时都有100％电量，其电量会随着无人机飞行而不断减少，无人机每飞行1m消耗1％的电量；当无人机电量剩余30％时发出提醒并警告电量不足；
2.搭建室内空间场景：该场景中包含整个室内空间的仿真场景，每个室内空间场景大小都在400平方米左右，同时包含足够多且随机生成的房间。为了更具体地模拟实际情况，不同的室内空间场景的形状也会不同，可模拟的室内环境包括办公楼层、工厂车间、居民房屋等。同时，室内空间场景的大小保证单架微型无人机能从任意一点飞行到距离无人机出发点最远的位置。
3.增加空间障碍物模型：在每一个室内空间场景中都会随机生成许多不同形状的障碍物，数量在10-30个，每个障碍物占地面积为1m2-5m2之间，障碍物的形状可为圆柱、长方体、正方体、多变体以及球体等。
4.运行基于动态目标追踪的多无人机协同探索系统，具体步骤如下：
4.1对所有无人机进行编号为1,2,3,…,M；
4.2启动编号为1的无人机，令其使用Wall-Around算法对未知室内空间进行边界环绕探索；当第1架无人机电量小于等于30％时，从基站派出第2架无人机使用Tracking-D*Lite算法进行追踪接力；
4.3启动编号为无人机，若存在编号为i-1的无人机处于低电量状态或者出现故障，则使用Tracking-D*Lite算法进行接力追踪；当第i的架无人机接力成功时，继续使用Wall-Around算法进行边界环绕；若第i的架无人机返回初始地点，则探索任务完成；
4.4将4.2和4.3中处于边界环绕状态的无人机实时位置发送给基站总控制软件，基站总控制软件进行实时轨迹绘制；当探索任务完成时，将所有无人机轨迹进行拼接，得到整个未知室内空间的边界轮廓图。
为对比本发明提出的基于动态目标追踪的多无人机未知室内空间探索方法与SGBA方法在探索同一未知室内空间的性能差异，在实验设计上，设置了10个不同大小、边界不同的室内空间，分别使用2、3、4、6、8架无人机对未知室内空间各进行了10次探索，实验对比指标包括单架无人机的探索覆盖区域百分比和所有无人机总共覆盖区域百分比。本发明中实验设备为台式电脑，搭载因特尔i7-9700处理器，处理器频率为3.0GHz，RAM大小为16GB，显卡为Nvidia GTX 1060Ti大小为4GB，实验系统为Windows 10版本。
如图7所示为本发明的实验结果图，为四架无人机沿墙飞行对整个未知室内空间探索完毕之后绘制的边界轮廓图。图7中线条表示不同的无人机分别绘制出的对应的航行轨迹，线条边的矩形为墙，通过总控制软件进行拼接组合最后形成地图边界轮廓图。
如图8所示为本发明和SGBA方法在无人机数量不同时单架无人机的探索覆盖区域百分比变化情况，可以看到SGBA算法单架无人机覆盖百分比在不同无人机数量的情况下都处于10％以下，而本发明中单架无人机探索覆盖百分比随无人机数量的增加而减少，但是总体都高于SGBA方法，因为单架无人机探索区域会因总无人机数量的增多而减小，另外由于SGBA方法由于需要进行返航，所以它的单架无人机探索覆盖百分比较小。
如图9所示为本发明和SGBA方法在无人机数量不同时所有无人机总探索覆盖区域百分比变化情况，可以看出随着无人机数量的增多，两种方法的覆盖面积百分比都逐渐增大，但是本发明总是优于SGBA方法，能够在相同的无人机数量上探索更多的未知空间。
以上对本发明所提供的一种基于动态目标追踪的多微型无人机未知室内区域协同探索方法进行了详细介绍。本文对本发明的原理及实施方式进行了阐述，以上说明用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通研究人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。
Claims (8)
1.一种基于动态目标追踪的多无人机未知室内空间探索方法，其特征在于技术方案是
第一步，构建由基站、I架无人机组成的多无人机协同探索系统，每架无人机实现探索的部件包括硬件层和软件层，硬件层搭载有六个激光雷达、一台单目相机、无线信号传感器、电池，提供飞行、感知、测距、通讯功能；软件层包括感知定位模块、地图边界环绕模块、接力追踪模块、地图边界绘制模块和电量控制模块，基站上安装有总控制软件；I是正整数；
第二步，采用多无人机协同探索系统对未知室内空间进行探索：多无人机群处于未知室内空间的同一边界位置，基站上的总控制软件依次派出无人机对未知空间的边界进行环绕，地图边界环绕模块控制无人机对未知室内区域的边界进行探索，使无人机在地图边界进行环绕导航最终回到出发点，同时正在运行地图边界绘制模块的无人机对其飞行轨迹进行实时绘制；接力追踪模块在未知空间控制多架无人机之间接力，当前一架无人机能源不足时，总控制软件立即派出下一架无人机接力前一架无人机，当接力成功时，后一架无人机继续对边界进行环绕；多架无人机按追踪接力的方式对边界环绕获得多幅轨迹图，当最后一架无人机返回到起始点时停止对未知室内空间探索；最后，总控制软件将所有无人机绘制出的轨迹图进行拼接得到整个地图的边界轮廓图，完成对整个未知室内环境的探索任务。
第三步，结束。
2.如权利要求1所述的一种基于动态目标追踪的多无人机未知室内空间探索方法，其特征在于第一步所述构建多无人机协同探索系统的方法是：多无人机协同探索系统由I架无人机、一个基站组成，无人机编号为1，2，3，...，i，...，I，I为正整数，1≤i≤I，基站和无人机都部署于未知室内空间的同一边界位置；
基站上安装有总控制软件，与I架无人机通过无线信号进行通信，完成无人机调度、实时无人机轨迹显示以及地图轨迹拼接的功能；总控制软件发出指令启动无人机，命令编号为i的无人机进行边界环绕；从编号为i的无人机接收编号为i的无人机的位置坐标信息、剩余电量；若编号为i的无人机电量小于等于低电量阈值A，则总控制软件命令编号为i+1的无人机对第i架无人机进行接力；
每一架无人机均由硬件层和软件层组成；硬件层与软件层的感知定位模块和电量控制模块相连，每一架无人机能够朝前、后、左、右四个方向飞行；六个激光雷达用于感知周围的环境信息，获取障碍物和室内空间边界的位置，与软件层的感知定位模块相连，将探测到的距离障碍物的距离传递给感知定位模块；单目相机通过不断拍摄视频以基于光流的视觉里程计方法计算无人机的移动位置和方向，与软件层的感知定位模块相连，将无人机的移动方向和移动距离传输给感知定位模块；当电量控制模块发现无人机电池电量剩余A时向基站的总控制软件传递接力信号；无线网络传感器用于基站与无人机之间的通讯，基站将将要移动的方向和距离发送给无人机，无人机的无线网络传感器将移动的轨迹和周围障碍物信息发送给基站；
感知定位模块与激光雷达、单目相机、地图边界环绕模块、接力追踪模块相连，在未知空间中进行障碍物感知、障碍物测距和定位；感知定位模块从激光雷达获得前后左右四个方向上无人机到障碍物的距离，若得到的某一方向上的无人机到障碍物距离小于等于无人机可视距离α，则判定该方向上存在障碍物且无人机处于该障碍物附近；感知定位模块从单目相机获取视频图像，利用基于光流的视觉里程计方法实现定位功能，即计算出无人机相对于起点移动的距离和方向，得到无人机的坐标位置，将无人机的坐标位置和周围环境信息发送给地图边界环绕模块、接力追踪模块；
地图边界环绕模块与感知定位模块、接力追踪模块、地图边界绘制模块、电量控制模块和总控制软件相连；地图边界环绕模块从感知定位模块接收无人机的坐标位置，控制无人机围绕未知室内空间的边界进行飞行，获得无人机探索到的未知室内空间的边界信息，并将未知室内空间的边界信息发送给地图边界绘制模块，以便地图边界绘制模块因此实时绘制无人机轨迹；当无人机的电量小于等于A的时候，地图边界环绕模块接收来自电量控制模块的低电量信息，将无人机当前位置信息发送给基站的总控制软件，表明当前无人机需要接力；当后续无人机对当前无人机接力成功时，地图边界环绕模块停止工作；
接力追踪模块与感知定位模块、地图边界环绕模块和总控制软件相连，从感知定位模块接收无人机的坐标位置，控制两架无人机之间的接力，一方面在未知环境中对固定目标点进行路径规划，一方面在已知起点和目标点位置的情况下，对未知空间中移动目标点即移动的无人机进行追踪；当编号为i+1的无人机的接力追踪模块从总控制软件接收到信号时，对编号为i的无人机进行接力，当接力成功时，第i+1架无人机的地图边界环绕模块开始探索边界；
地图边界绘制模块与地图边界环绕模块相连，与地图边界环绕模块同时运行，从地图边界环绕模块接收每架无人机的实时位置，记录下每架无人机在边界探索过程中的位置信息，实时绘制出每架无人机的飞行轨迹；
电量控制模块与地图边界环绕模块、总控制软件相连，该模块负责无人机剩余电量的实时监控，监控硬件层的电池电量信息；电量控制模块将无人机的电量实时传送给地图边界环绕模块；若正在进行边界探索的无人机的电量控制模块发现剩余电量小于等于A，则向基站的总控制软件传递接力信号。
3.如权利要求2所述的一种基于动态目标追踪的多无人机未知室内空间探索方法，其特征在于所述低电量阈值A为无人机初始总电量的30％，所述无人机可视距离α为1米。
4.如权利要求2所述的一种基于动态目标追踪的多无人机未知室内空间探索方法，其特征在于所述接力追踪模块采用Tracking-D*Lite算法控制两架无人机之间的接力，Tracking-D*Lite算法将D*Lite算法和I-ARA*算法相结合，采用D*Lite算法在未知环境中对固定目标点进行路径规划，采用I-ARA*算法在已知环境中对移动目标点进行追踪。
5.如权利要求1所述的一种基于动态目标追踪的多无人机未知室内空间探索方法，其特征在于第二步采用多无人机协同探索系统对未知室内空间进行探索的具体方法是：
2.1初始化无人机编号i＝1；
2.2编号为i的无人机从其激光雷达获得无人机前后左右四个方向无人机到障碍物的距离，若得到的某一方向上的障碍物与无人机距离小于等于α，则判定该方向存在障碍物且无人机处于该障碍物附近，以此方法获得无人机前后左右四个方向上无人机到障碍物的距离，将无人机到障碍物的距离发送给感知定位模块，编号为i的无人机的感知定位模块将前、后、左、右四个方向无人机到障碍物的距离发送给地图边界环绕模块；
2.3编号为i的无人机的电量控制模块实时监控编号为i的无人机电量，若编号为i的无人机电量小于等于A，电量控制模块向基站的总控制软件发出接力信号，转2.4；若编号为i的无人机电量大于A，转2.5，同时编号为i的无人机的地图边界绘制模块按2.8的流程对地图边界进行实时的绘制；
2.4当前步骤表明编号为i+1的无人机需要对编号为i的无人机进行接力，编号为i的无人机启动地图边界环绕模块，编号为i+1的无人机启动接力追踪模块；同时，编号为i的无人机的地图边界环绕模块、总控制软件、编号为i+1的无人机的接力追踪模块、编号为i的无人机的地图边界绘制模块并行工作，完成无人机对未知室内空间的边界信息的探索，无人机的接力、无人机轨迹的绘制；具体为：
编号为i的无人机的地图边界环绕模块按2.5的流程围绕未知室内空间的边界进行飞行，获得无人机探索到的未知室内空间的边界信息；编号为i的无人机的地图边界环绕模块运行的同时，总控制软件按2.6的流程将编号为i的无人机的实时位置发送给编号为i+1的无人机的接力追踪模块；同时编号为i+1的无人机的接力追踪模块按2.7的流程对编号为i的无人机进行接力追踪；同时编号为i的无人机的地图边界绘制模块按2.8的流程对地图边界进行实时的绘制；
2.5编号为i的无人机的地图边界环绕模块采用Wall-Around算法围绕未知室内空间的边界进行飞行，获得无人机探索到的未知室内空间的边界信息：
2.5.1对未知室内空间在二维平面上及逆行场景建立平面直角坐标系，方法是：编号为i的无人机的地图边界环绕模块从感知定位模块获取前、后、左、右四个方向无人机到障碍物的距离，得到以无人机起点为原点的平面直角坐标系，平面直角坐标系中每一个坐标值交点称为网格地图结点，并以平面直角坐标系原点为中心分为前、后、左、右四个方向，其中x正方向为右，x负方向为左，y正方向为前，y的负方向为后；单架无人机在某一时刻的运动方式只能朝前、后、左、右四个方向的某一方向运动；
2.5.2地图边界环绕模块根据前、后、左、右四个方向无人机到障碍物的距离，计算编号为i的无人机的MD、ND、OD和PD四个方向状态量，控制编号为i的无人机运动方向的切换，使编号为i的无人机进行边界环绕，MD为主方向，ND为下一运动方向，OD为反方向，PD为上一运动方向；MD表示当前无人机在环绕边界过程中指向边界的方向，ND表示当前无人机在环绕边界过程中下一步将要前进的方向，OD表示与MD相反的方向，PD表示与ND相反的方向；
2.5.3若编号为i的无人机的感知定位模块获取编号为i的无人机的当前坐标位置不为基站坐标，表明整个未知室内空间还未被全部探索，则转2.5.2继续进行边界环绕；若编号为i的无人机的感知定位模块获取编号为i的无人机的当前坐标位置为基站坐标，表明整个未知室内空间的封闭边界已被全部探索，探索完成，转2.8；若编号为i的无人机的电量感知模块获得编号为i的无人机电量等于A，则电量控制模块向基站总控制软件发出接力信号，并且编号为i的无人机的地图边界环绕模块将编号为i的无人机的当前坐标发送给基站总控制软件，总控制软件转2.6，编号为i的无人机转2.5.2继续进行边界环绕；若编号为i的无人机的电量感知模块获得编号为i的无人机电量大于0且小于A，则编号为i的无人机转2.5.2继续进行边界环绕；若编号为i的无人机的电量感知模块获得编号为i的无人机电量等于0，则编号为i的无人机转2.5.4；
2.5.4编号为i的无人机停止飞行，编号为i的无人机的边界环绕模块和地图边界绘制模块停止运行，令i＝i+1，若编号为i的无人机未接力成功，转2.7；若编号为i的无人机以接力成功，则转2.5；
2.6总控制软件判定是否从编号为i的无人机接收到编号为i的无人机电量不足信号，若接收到电量不足信号，令i＝i+1，转2.7；若未接收到电量不足信号，转2.6继续等待；
2.7编号为i的无人机的接力追踪模块从编号为i的无人机的感知定位模块获取无人机前后左右四个方向上无人机到障碍物的距离，从基站总控制软件获取编号为i-1的无人机坐标位置，采用Tracking-D*Lite算法接力追踪编号为i-1的无人机：
2.7.1编号为i的无人机的接力追踪模块通过2.5.1建立的平面直角坐标系，通过感知定位模块不断感知并获取周围环境，编号为i的无人机接力追踪模块对未知室内空间在二维平面上进行建模并设置Tracking-D*Lite算法所需要使用的变量，方法如下：
2.7.1.1将整个未知室内空间抽象为一个二维平面网格图，设位于出发点的编号为i+1的无人机的位置为二维平面网格图的原点，单位距离为1米，二维平面网格图中的每一个网格表示未知室内空间中对应的位置；
2.7.1.2令集合S表示整个未知室内空间的二维网格地图结点集合，后继结点集合Succ(s)表示所有从网格地图结点s的扩展出子结点组成的集合，即从s扩展的结点，
前继结点集合Pred(s)表示s的父结点组成的集合，
c(s，s′)表示从s到邻接结点s′的单步代价值，c(s，s′)＝1；s′∈S，s′是s的前继邻接结点；
2.7.2编号为i的无人机对编号为i-1的无人机进行追踪，在此接力过程中需要不断地进行路径规划，方法如下：
2.7.2.1对所需状态量进行初始化；
2.7.2.1.1令编号为i的无人机初始坐标为(x0，y0)，并初始化编号为i的无人机当前位置snow为(x0，y0)，即为图；
2.7.2.1.2编号为i的无人机从基站获取编号为i-1的无人机位置(xg，yg)，并初始化编号为i-1的无人机当前目标点gnow为(xg，yg)，即为；
2.7.2.1.3对于S中的任意一个结点s，令rhs(s)＝g(s)＝∞，
其中g(s)表示从gnow到s的最短距离；rhs(s)表示基于s的父结点parent(s)计算出的g(s)的预测值，目的是为了寻找更小代价路径结点，rhs(s)按公式(一)计算，其中spred为s的前继邻接结点，c(s，spred)表示从s到邻接结点spred的单步代价值；
2.7.2.1.4初始化可进行搜索结点的优先队列
U用于记录地图中可进行搜索的结点；初始化需要删除的结点队列
D用于存储每次重新搜索过程中需要删除的结点；U中的结点s的优先级k(s)计算方式如公式(二)所示；
其中，k(s)由两部分构成，分别是k1(s)＝min(g(s)，rhs(s))+h(s，gnow)+km，k2(s)＝min(g(s)，rhs(s))；h(s，gnow)为启发式函数表示从结点s到gnow的最小代价评估值，该代价评估值使用曼哈顿距离计算；km表示每一次起点改变之后的启发式补偿值，令km初始值为1；
k(s)与结点s′的优先级k(s′)的比较方法如公式(三)所示，k(s)值越小表示s优先级越高，k(s)值越大表示s优先级越低；
其中，s′表示与s进行优先级比较的结点，k(s′)为s′的优先级，若k1(s)≤k1(s′)，则k(s)≤k(s′)，表明s的优先级高于s′的优先级；若k1(s)＝k1(s′)并且k2(s)≤k2(s′)，则k(s)≤k(s′)，表明s的优先级高于s′的优先级；其余情况都表示k(s)＞k(s′)，表明s的优先级低于s′的优先级；公式(二)用于U中元素优先级的计算，公式(三)用于U中元素优先级的比较；
2.7.2.2令rhs(gnow)＝0，并将gnow加入优先队列U中；采用反向探索，即从目标点gnow扩展到起点snow，构建以目标点gnow为根结点的搜索树，计算编号为i的无人机下一步将要移动到的位置snext；令其他可行结点集合Sother中存储集合S中的除snow之外的其他可行结点，即Sother＝{s1，s2，...，sother}；采用启发式函数公式(四)计算从snow出发到各可行结点的曼哈顿距离h(snow，sother)，其中sother∈Sother，snow.x表示snow的横坐标，snow.y表示snow的纵坐标，sother.x表示sother的横坐标，sother.y表示sother的纵坐标；
h(snow，sother)＝|snow.x-sother.x|+|snow·y-sother.y| 公式(四)
构造搜索树的具体步骤如下：
2.7.2.2.1从优先队列U中取出优先级最高的可探索结点，令为u；若结点u＝snow，则表明已探索出以gnow为根结点并包含snow的一棵搜索树，转2.7.2.2.3；若结点u≠snow，则表明当前探索过程中还未探索snow，需要继续进行结点扩展，转2.7.2.2.2；
2.7.2.2.2如果g(u)＞rhs(u)，则令g(u)＝rhs(u)，将u从U中删除，同时将结点u可被扩展的任意邻居结点u′(即满足g(u′)≠rhs(u′))加入到队列U；如果g(u)≤rhs(u)，则令g(u)＝∞，同时将结点u和其所有可被扩展的邻居结点u′(即g(u′)与rhs(u′)不相等的结点)加入到队列U；将从结点u扩展的所有邻居结点u′的父结点设为结点u，即令parent(u′)＝u，转2.7.2.2.1；
2.7.2.2.3通过2.7.2.2.1和2.7.2.2.2的步骤，构建了以结点gnow为根结点的一棵搜索树，令搜索树为tree，tree中包含从snow到gnow的路径，令从snow到gnow的路径为path；
2.7.2.2.4令snext为path中取出的snow下一结点；
2.7.2.3编号为i的无人机从snow移动到snext，即令snow＝snext；更新Sother，令其他可行结点集合Sother中存储集合S中的除snow之外的其他可行结点，即Sother＝{s1，s2，...，sother}；同时令km＝km+1；
2.7.2.4编号为i的无人机通过总控制软件获取编号为i-1的无人机当前位置gnow′，令gnow＝gnow′；
2.7.2.5编号为i的无人机的感知定位模块判断前后左右四个方向上的障碍物信息，更新编号为i的无人机到障碍物的距离；
2.7.2.6根据公式(四)重新计算更新后的snow位置和集合Sother中各个结点的曼哈顿距离h(snow，sother)，其中sother∈Sother；
2.7.2.7初始化用于记录重新搜索过程中删除的结点队列
对于tree中的任意一个dv，若结点dv不属于snow的子结点，则将结点dv从tree删除，令g(dv)＝rhs(dv)＝∞，并将结点dv加入D中，则D＝{d1，d2，d3，...，dv，...，，dV}，V为队列D中结点的个数；
2.7.2.8重新使用D中的部分结点以加快下一次规划速度，对于D中任意结点dv的任意邻居结点nneighbor，若nneighbor以snow为起点并且nneighbor不属于队列U，则将dv加入队列U中；
2.7.2.9若编号为i的无人机的snow＝gnow，表明编号为i的无人机对编号为i-1的无人机接力成功，则编号为i的无人机运行地图边界环绕模块，转2.5；否则表示未接力成功，转2.7.2.2步；
2.8编号为i的无人机的地图边界绘制模块在编号为i的无人机的地图边界环绕模块对未知室内空间进行边界环绕的同时，进行地图边界绘制，若编号为i的无人机返回无人机出发的起点，则完成对整个位置空间的遍历，转2.9；若编号为i的无人机没有返回无人机出发的起点，则编号为i的无人机的地图边界绘制模块根据计算出的坐标位置实时以10次每秒的频率绘制编号为i的无人机的飞行轨迹，每次绘制飞行轨迹的方法是：
2.8.1令时间变量t1＝编号为i的无人机的系统时间，编号为i的无人机的地图边界绘制模块从编号为i的无人机的边界环绕模块获取地图边界信息和无人机实时位置；
2.8.2令编号为i的无人机在x轴方向上移动的绝对距离为Δx，在y轴方向上移动的绝对距离为Δy，根据Δx，Δy计算编号为i的无人机的当前坐标；
2.8.3将当前坐标标注为航行轨迹点，并将当前坐标与上一次的标注航行轨迹点连线，依此绘制航行轨迹；
2.8.4若编号为i的无人机的边界环绕模块仍在运行，转2.8.5；若编号为i的无人机的边界环绕模块没在运行，则编号为i的无人机的地图边界绘制模块停止运行，令i＝i+1，若编号为i的无人机未接力成功，转2.7；若编号为i的无人机以接力成功，则转2.5；
2.8.5令时间变量t2＝编号为i的无人机的系统时间，若t2-t1＜0.1秒，转2.8.5；若t2-t1＝0.1秒，转2.8.1；
2.9令参与未知区域协同探索的无人机总数II＝i；总控制软件对从II架无人机处获取的每架无人机的轨迹进行拼接，得到最终的未知室内空间边界轮廓图，方法如下：
2.9.1.1将无人机轨迹根据无人机编号分配为1，2，3，...，ii，...，II，II≤I；
2.9.1.2令ii＝1；
2.9.1.3从第ii段轨迹的终点连接到第ii+1段轨迹的起点；
2.9.1.4令ii＝ii+1，若ii＝II，则完成所有轨迹的拼接，得到最终的未知室内空间边界轮廓图，结束；若ii＜II，则转2.9.1.3。
6.如权利要求5所述的一种基于动态目标追踪的多无人机未知室内空间探索方法，其特征在于2.5.2步所述地图边界环绕模块控制编号为i的无人机运动方向的切换的方法是：
2.5.2.1地图边界环绕模块从激光雷达得到前后左右四个方向无人机到障碍物的距离，找到距离障碍物最近的方向，将此方向设为MD，通过逆时针计算规则得到无人机环绕边界飞行的其余三个运动方向状态量；
2.5.2.2若地图边界环绕模块从激光雷达获知无人机MD方向有障碍物且ND方向无障碍物，则所有运动方向状态量保持不变，控制无人机朝ND方向前进1米，结束；
2.5.2.3若地图边界环绕模块从激光雷达获知无人机MD方向上无障碍物，则将ND设为MD方向，通过新的ND按照逆时针计算规则重新计算MD、OD和PD，并控制无人机朝ND方向前进1米，结束；
2.5.2.4若地图边界环绕模块从激光雷达获知无人机MD和ND方向均有障碍物且OD方向无障碍物，则将ND设为OD方向，通过新的ND按照逆时针计算规则重新计算MD、OD和PD，并控制无人机朝ND方向前进1米，结束；
2.5.2.5若地图边界环绕模块从激光雷达获知无人机MD、ND、OD方向均有障碍物且PD方向无障碍物，则将ND设为PD方向，通过新的ND按照逆时针计算规则重新计算MD、OD和PD，并控制无人机朝ND方向前进1米，结束。
7.如权利要求6所述的一种基于动态目标追踪的多无人机未知室内空间探索方法，其特征在于所述逆时针计算规则是：MD逆时针旋转90度得到ND，ND逆时针旋转90度得到OD，OD逆时针旋转90度得到PD，PD逆时针旋转90度得到MD。
8.如权利要求5所述的一种基于动态目标追踪的多无人机未知室内空间探索方法，其特征在于2.8.2步所述计算编号为i的无人机的坐标的方法是：如果编号为i的无人机朝x轴正方向移动，朝y轴正方向移动，则编号为i的无人机当前坐标为(Δx，Δy)；如果编号为i的无人机朝x轴正方向移动，朝y轴负方向移动，则编号为i的无人机当前坐标为(Δx，-Δy)；如果编号为i的无人机朝x轴负方向移动，朝y轴正方向移动，则编号为i的无人机当前坐标为(-Δx，Δy)；如果编号为i的无人机朝x轴负方向移动，朝y轴负方向移动，则编号为i的无人机当前坐标为(-Δx，-Δy)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111468016.1A CN114115363B (zh) | 2021-12-04 | 2021-12-04 | 一种基于动态目标追踪的多无人机未知室内空间探索方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111468016.1A CN114115363B (zh) | 2021-12-04 | 2021-12-04 | 一种基于动态目标追踪的多无人机未知室内空间探索方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114115363A true CN114115363A (zh) | 2022-03-01 |
| CN114115363B CN114115363B (zh) | 2023-07-18 |
Family
ID=80366343
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111468016.1A Active CN114115363B (zh) | 2021-12-04 | 2021-12-04 | 一种基于动态目标追踪的多无人机未知室内空间探索方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114115363B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105717946A (zh) * | 2016-03-22 | 2016-06-29 | 临沂高新区翔鸿电子科技有限公司 | 多基站无人机连续跟踪系统及方法 |
| CN107466469A (zh) * | 2017-02-17 | 2017-12-12 | 深圳前海达闼云端智能科技有限公司 | 地图绘制方法、其云端平台及服务器 |
| CN110658828A (zh) * | 2019-10-25 | 2020-01-07 | 桂林电子科技大学 | 一种地貌自主探测方法及无人机 |
| KR102154950B1 (ko) * | 2020-01-30 | 2020-09-10 | 주식회사 디지털커브 | 무인 비행체에 의해 촬영된 이미지를 지도, 도면 또는 위성 사진에 대해 매칭시키는 방법 및 장치 |
| CN112578779A (zh) * | 2019-09-29 | 2021-03-30 | 苏州宝时得电动工具有限公司 | 地图建立方法、自移动设备、自动工作系统 |
| CN113625721A (zh) * | 2021-08-19 | 2021-11-09 | 东北大学 | 一种未知空间自主探索规划方法 |
| CN113741523A (zh) * | 2021-09-08 | 2021-12-03 | 北京航空航天大学 | 一种基于边界和采样的混合无人机自主探测方法 |
-
2021
- 2021-12-04 CN CN202111468016.1A patent/CN114115363B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105717946A (zh) * | 2016-03-22 | 2016-06-29 | 临沂高新区翔鸿电子科技有限公司 | 多基站无人机连续跟踪系统及方法 |
| CN107466469A (zh) * | 2017-02-17 | 2017-12-12 | 深圳前海达闼云端智能科技有限公司 | 地图绘制方法、其云端平台及服务器 |
| CN112578779A (zh) * | 2019-09-29 | 2021-03-30 | 苏州宝时得电动工具有限公司 | 地图建立方法、自移动设备、自动工作系统 |
| CN110658828A (zh) * | 2019-10-25 | 2020-01-07 | 桂林电子科技大学 | 一种地貌自主探测方法及无人机 |
| KR102154950B1 (ko) * | 2020-01-30 | 2020-09-10 | 주식회사 디지털커브 | 무인 비행체에 의해 촬영된 이미지를 지도, 도면 또는 위성 사진에 대해 매칭시키는 방법 및 장치 |
| CN113625721A (zh) * | 2021-08-19 | 2021-11-09 | 东北大学 | 一种未知空间自主探索规划方法 |
| CN113741523A (zh) * | 2021-09-08 | 2021-12-03 | 北京航空航天大学 | 一种基于边界和采样的混合无人机自主探测方法 |
Non-Patent Citations (6)
| Title |
|---|
| HAN QIE: "Joint_Optimization_of_Multi-UAV_Target_Assignment_and_Path_Planning_Based_on_Multi-Agent_Reinforcement_Learning", IEEE ACCESS * |
| TAO HAN: "An effective approach to unmanned aerial vehicle navigation using visual topological map in outdoor and indoor environments", COMPUTER COMMUNICATIONS, vol. 150 * |
| 余小欢: "基于双目视觉的微型无人机室内3维地图构建", 信息与控制, vol. 43, no. 4 * |
| 刘瑞雪;曾碧;汪明慧;卢智亮;: "一种基于高效边界探索的机器人自主建图方法", 广东工业大学学报, no. 05 * |
| 史殿习: "基于行为树调度的多无人机未知室内空间探索方法", 基旋即科学, vol. 49, no. 11 * |
| 史殿习: "基于行为树调度的多无人机未知室内空间探索方法", 计算机科学, vol. 49, no. 11 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114115363B (zh) | 2023-07-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110428111B (zh) | Uav/ugv协同长时多任务作业轨迹规划方法 | |
| Liu et al. | Path planning for smart car based on Dijkstra algorithm and dynamic window approach | |
| Zhou et al. | Survey on path and view planning for UAVs | |
| CN110488859B (zh) | 一种基于改进Q-learning算法的无人机航路规划方法 | |
| Maza et al. | Classification of multi-UAV architectures | |
| CN109613931A (zh) | 基于生物社会力的异构无人机集群目标跟踪系统及方法 | |
| CN105492985A (zh) | 多传感器环境地图构建 | |
| Arul et al. | LSwarm: Efficient collision avoidance for large swarms with coverage constraints in complex urban scenes | |
| CN110471426A (zh) | 基于量子狼群算法的无人驾驶智能车自动避碰方法 | |
| Petrlík et al. | Coverage optimization in the cooperative surveillance task using multiple micro aerial vehicles | |
| CN112083727B (zh) | 基于速度障碍物的多自主体系统分布式避碰编队控制方法 | |
| CN114355981A (zh) | 一种四旋翼无人机自主探索建图的方法和系统 | |
| Choi et al. | Improved CNN-based path planning for stairs climbing in autonomous UAV with LiDAR sensor | |
| Chen et al. | Multilayer mapping kit for autonomous UAV navigation | |
| Rocha et al. | Plannie: A benchmark framework for autonomous robots path planning algorithms integrated to simulated and real environments | |
| Xuexi et al. | Survey on key technology of robocup rescue robot | |
| Tang et al. | Learning to coordinate for a worker-station multi-robot system in planar coverage tasks | |
| CN114115363B (zh) | 一种基于动态目标追踪的多无人机未知室内空间探索方法 | |
| CN114596360B (zh) | 一种基于图拓扑的双阶段主动即时定位与建图算法 | |
| CN115857372A (zh) | 分布式多无人机集群协同空间搜索调度的控制仿真系统 | |
| CN115755975A (zh) | 多无人机协同的分布式空间搜索及轨迹规划方法及装置 | |
| Du | A precision spraying mission assignment and path planning performed by multi-quadcopters | |
| CN114706406A (zh) | 一种基于人机交互的多智能体协同环境探索系统及装置 | |
| Wu et al. | UAV formation control based on consistency | |
| Taha et al. | Optimal 3D path planning for the delivery quadcopters operating in a city airspace |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |