CN106970648A

CN106970648A - 城市低空环境下无人机多目标路径规划联合搜索方法

Info

Publication number: CN106970648A
Application number: CN201710255262.6A
Authority: CN
Inventors: 曹先彬; 肖振宇; 杨朋; 尹超; 席星; 吴大鹏
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2017-04-19
Filing date: 2017-04-19
Publication date: 2017-07-21
Anticipated expiration: 2037-04-19
Also published as: US10706729B2; US20180308371A1; CN106970648B

Abstract

本发明提供了一种城市低空环境下无人机多目标路径规划联合搜索方法，属于无人机路径规划技术领域。本方法步首先以静态已知障碍物为基础构建静态安全指数地图，以无人机在飞行过程中主动探测到的没有标记在地理地图中的障碍物为基础，在机载平台上构建动态安全指数地图；然后基于离线搜索与在线搜索联合求解优化无人机飞行时间和飞行安全的多目标路径规划问题。使用本方法首先离线规划一条从起点到终点的代价最小的路径，然后在检测到未知障碍物时，采用在线搜索为在线规划一段变更的航迹，以避开动态障碍物。本发明的在线搜索具有较小的搜索空间，能够为无人机快速地在线重新规划一段安全的路径，从而满足无人机对路径规划实时性的要求。

Description

城市低空环境下无人机多目标路径规划联合搜索方法

技术领域

本发明属于无人机路径规划技术领域，具体是指一种联合离线和在线搜索的实现城市低空环境下无人机多目标路径规划的方法。

背景技术

自美国联邦航空管理局(Federal Aviation Administration，FAA)发布了至2015年国内无人机(unmanned aerial vehicles，UAVs)起降规划以来，无人机的使用受到了极大关注。可以想象，在未来的智慧城市(Smart Cities)中，无人机将会在物流、应急通信、消防、航拍、遥感、救援等事务中扮演重要角色。为了最大程度地利用无人机的优势，首先需要考虑的是怎样使无人机既快速又安全地到达指定区域，这就是所谓的路径规划问题(PathPlanning Problem)。

路径规划问题指的是：求解无人机当前位置和目的地之间一条或多条无障碍路径的问题。近年来，研究人员一直致力于研究如何在障碍物环境中为车辆或飞行器找出一条无障碍的路径，这一问题的解决对于无人驾驶技术的发展是至关重要的。至今，已有三种类型的路径规划方法被提出，它们是几何法(Geometry Method)、速度空间法(VelocitySpace Method)和图形搜索法(Graph Search Method)。几何法将地图中的障碍物的集合信息作为路径搜索算法的输入，典型的几何法包括势场法(Potential Field Method)、矢量场直方图法(Vector Field Histogram Method)和Voronoi图法(Voronoi DiagramMethod)。速度空间法是综合考虑了车辆或飞行器的形状以及运动学和动力学约束的避免碰撞的方法，包括曲率速度法(Curvature Velocity Method)和动态窗口法(DynamicWindow Method)。图形搜索法旨在找到某个图中两个顶点之间的最短路径，其中A*算法是一种已知的可以有效地找出从起点到目的地之间无障碍路径的图形搜索方法。

然而，上述路径规划方法不能直接用于在动态城市环境中执行低空飞行任务的无人机的路径规划，因为它们需要及时更新的全局地图信息。在动态城市低空环境中，及时更新的全局地图信息对于无人机来说往往是不可知的，因为无人机的带宽是十分有限的。另外，在飞行过程中，无人机可能会遇到一些没有在地图中标记的意外的障碍物，在这种情况下，无人机将有可能与这些障碍物相撞。虽然可以利用重规划(Replanning)来减少飞行期间这种不确定性带来的影响，但仍然至少存在两个显著问题亟待解决：

1)利用有限的机载计算资源实时探测障碍物并更新路径；

2)基于有限的探测范围以及有限的定位精度，如利用全球定位系统(GlobalPositioning System，GPS)进行无人机定位，应对周围环境的变化(与已知地图信息的不匹配)。

实现无人机在城市低空环境下的路径规划，既要保证无人机在飞行期间的安全，又要保证无人机在尽量短的时间内到达目的地。该问题是一个多目标路径规划问题。

近年来，各研究人员已经开发了各种方法来解决无人机的多目标路径规划问题。例如，Roberge等人尝试通过利用遗传算法(GA)和粒子群优化算法(PSO)算法，在复杂的3维环境中为无人机计算可能的近似最优路径。同时，它们采用“单程序，多数据”并行编程范式技术来减少执行时间，使求解过程可以是实时的。Hernndez等人提出了一种多目标A*算法(Multi-Objective A*Algorithm)来找到一条综合考虑飞行时间、转弯角度、燃料消耗和区域偏差的路径。Tao等人提出了一种改进的遗传算法来解决多目标路径规划问题。此外，Niendorf等人提出了一种在敌对环境中处理无人机任务规划的方法，该方法在较短的任务执行时间和较低风险之间进行了权衡。Liu等人提出了一种用于无人机路径规划的协同自适应差分多目标进化算法(Synergistic Self-Adapted Difference Multi-ObjectiveEvolution Algorithm)，以使无人机可以探测并避开障碍物。为了加快运算速度，又引入了自适应调整、交叉和变化策略。

虽然上述多目标路径规划方法可以用于为无人机在复杂环境中找到可行的路径，但是没有一个方法解决了动态避障的问题——它们中的大部分仅仅为静态障碍物构建危险区域或禁区，以引导无人机绕过这些区域。

为了同时避开静态和动态威胁区域，Wen等人提出了一种低空动态环境下无人机在线路径规划的新方法(参考文件1：N.Wen,L.Zhao,X.Su,et al,“UAV online pathplanning algorithm in a low altitude dangerous environment,”IEEE/CAA Journalof Automatica Sinica,vol.2,no.2,2015ISSN 2329-9274,pp.173-185.)。在这种方法中，直觉模糊集(Intuitionistic Fuzzy Set)被用于构建静态威胁模型，并构建了一种可达性集估计器来评估动态威胁。为了降低规划成本并快速获得高安全水平的路径，他们提出了启发式子目标选择器并将其集成到规划系统中。虽然该框架被证明在无人机路径规划任务中表现良好，但它仅集中于解决单目标(即安全水平)路径规划问题，而无法解决所提到的多目标路径优化问题。

为了将动态环境下的单目标解决方案拓展至多目标路径规划问题中，Wu等人提出了一个基于网格的多步A*(Multi-Step A*，MSA*)搜索算法来解决在大型、动态、不确定4维环境中无人机的多目标路径规划问题(参考文件2：P.P.Wu,D.Campbell,and T.Merz,“Multi-objective four-dimensional vehicle motion planning in large dynamicenvironments,”IEEE Transactions on Systems Man&Cybernetics Part B CyberneticsA Publication of the IEEE Systems Man&Cybernetics Society,vol.41,no.3,2011ISSN 1941-0492,pp.621-634.)。MSA*算法利用一种可变的后继运算符(VariableSuccessor Operator)，后者使其可以通过跟踪轨迹段的长度、角度和速度来找到最小代价路径。该后继运算符为不确定性提供了一个固有的容限，它是基于路径间的最小距离和网格序列边界的。此外，通过利用该可变后继运算符来创建多分辨率、高存储效率的网格采样结构，可以使搜索空间大大减少，相应地提高运算效率。另外，Peng等人提出了一种在线路径规划方法(Online Path Planning Scheme)以解决动态多目标优化问题(参考文件3：X.Peng,D.Xu,and F.Zhang,“UAV online path planning based on dynamicmultiobjective evolutionary algorithm,”in Proceeding of Control Conference(CCC),2011 30th Chinese.IEEE,2011ISSN 2161-2927,pp.5424-5429.)，其中机载传感器收集的局部信息不断更新，以重构无人机轨迹。然而，上述方法只解决了在线路径规划问题，它们没有利用离线信息，即地理地图信息，来降低飞行时间。

发明内容

对于现有技术存在的缺陷，本发明的目的是提出了一个有效的路径规划方法，可以保证无人机在动态城市环境中执行低空飞行任务时在保证安全的前提下快速到达目的地。

本发明提供的城市低空环境下无人机多目标路径规划联合搜索方法，实现步骤包括：

步骤1，以地理地图描述的静态已知障碍物为基础，构建了一幅静态安全指数地图，该静态安全指数地图描述了空间图上任意一点的静态安全指数；

步骤2，以无人机在飞行过程中主动探测到的没有标记在地理地图中的障碍物为基础，在机载平台上构建了一幅动态安全指数地图，该动态安全指数地图描述了无人机在飞行过程中探测到的空间中任意一点的动态安全指数；

步骤3，构建联合优化无人机飞行时间和飞行安全的多目标路径规划问题，该问题既要最小化无人机的飞行时间又要最大化无人机的安全飞行；

步骤4，基于离线搜索与在线搜索联合求解多目标路径规划问题，获取最优路径规划方案。

根据无人机的起点、终点和飞行速度，采用离线搜索为无人机离线规划一条从起点到终点的代价最小的路径；无人机在飞行过程中，当检测到未知障碍物时，采用在线搜索为无人机在线规划一段变更的航迹，以避开动态障碍物；所述的在线搜索在当前搜索点和终点之间的连线与标记的未知障碍物之间不相交的时候，搜索过程停止。

本发明的优点与积极效果在于：

(1)本发明提出了静态和动态安全指数地图的概念，并生成了静态安全指数地图与动态安全指数地图，这两种新的安全指数地图的引入不仅为无人机路径规划问题贡献了新的内容。也为其它路径规划问题指明了一个新的研究思路。

(2)尽管地理地图数据是海量的，但是采用本发明方法首先采用离线的方式获得了无人机的起点到目的地之间的路径，这样，无人机在进行路径规划时就无需存储海量的地理地图信息，也无需计算描述该地理地图中每一点的安全性的安全指数地图，从而极大地降低了路径规划问题对无人机存储能力以及计算性能的要求，降低了无人机的成本。

(3)本发明设计的动态安全指数地图的构建方法具有较低的计算复杂度，而且本发明设计的动态搜索方法具有较小的搜索空间，所以，本发明能够为无人机快速地在线重新规划一段安全的路径，从而满足无人机对路径规划实时性的要求。

附图说明

图1是无人机位置不确定性分布模型示意图；

图2是无人机危险区域示意图；

图3是本发明构建静态代价分布地图的示意图；

图4是本发明进行在线搜索的过程示意图；

图5是本发明的离线搜索和在线搜索联合求解多目标路径规划问题的过程示意图；

图6是用于仿真的原始地图；

图7是预计飞行时间-路径安全指数曲线；

图8是一条预计飞行时间为296.5秒、安全指数为38的路径；

图9是当探测范围为10时无人机的飞行轨迹；

图10是当探测范围为30时无人机的飞行轨迹。

具体实施方式

下面将结合附图和实例对本发明作进一步的说明。

本发明所要求解的多目标路径规划问题是：为无人机飞过动态城市低空环境找到帕累托最优路径——既能保证无人机在飞行期间的安全，又能使无人机在尽量短的时间内到达目的地。为了这个目标，本发明提出了两种类型的安全指数地图，即静态安全指数地图和动态安全指数地图，它们分别以地理地图(如百度地图)中标记出的障碍物和无人机飞行过程中探测到的未被标记的未知障碍物为基础构建。以此为基础，分别生成了静态代价分布地图和动态代价分布地图，并提出了一个基于这两种代价分布地图的离线搜索和在线搜索相联合的方法来解决该多目标路径规划问题，其中多目标包括飞行时间和安全水平。

本发明要解决的多路径规划问题，通过一个空间图(Space Graph)来描述。设多路径规划问题用一个三元组<G,S_start,S_target>表示，其中G＝(N,A,c)表示一个空间图，N表示一个点集，A是一个参数集，c:A→R^K表示一个路径代价函数，R^K表示K维向量空间，K是优化目标的数目，K维代价向量c(m,n)表示从起点到终点规划出的路径的总代价。S_start、S_target分别表示无人机的起点和目标终点。

对于一个多目标路径规划问题，经常会求解其帕累托最优解，下面定义帕累托最优路径：

定义1.给定向量和当且仅当且其中x_1i和x_2i分别表示和的第i个元素，称强支配(strongly dominate)记作

定义2.给定向量集X，当且仅当满足那么称为非支配的(non-dominant)。

定义3.令P_mn为两点之间的路径的集合，当且仅当满足p₂＜p₁，即在集合P_mn中没有一条路径在所有优化目标上比p₁更好，那么p₁∈P_mn可以被认为是一条非支配的路径。

为了求解本发明的多目标路径规划问题，需要解决静态路径预规划问题(StaticPath Planning Problem)和动态路径重规划问题(Dynamic Path Replanning Problem)。本发明方法进行了如下改进和创新：

(1)引入一个安全指数(Safety Index)的概念，利用它来建立一个城市低空环境的安全指数地图(Safety Index Map，SIM)。本发明设计了两种安全指数地图，一种是包含相应地理地图中主要障碍物的静态安全指数地图，一种是包含在飞行期间在地理地图中没有标注的未知障碍物的动态安全指数地图。其中，静态安全指数地图是基于定位误差离线构造的，而动态安全指数地图是在飞行过程中，通过当无人机探测到地理地图上未被标记的未知障碍物时，通过在线生成动态构造的。

(2)利用上述两种安全指数地图分别构建了两种代价分布地图，分别是静态代价分布地图和动态代价分布地图。基于这两种代价分布地图，利用离线搜索和在线搜索的综合方法，来解决本发明的多目标路径规划问题。其中，离线搜索(Offline SearchAlgorithm，OFSA)是指基于静态代价分布地图，找到起点与目的地之间一系列可以避开地图上已被标记的障碍物的帕累托最优路径。而在无人机飞行的过程中，一旦探测到地图中未标记的障碍物，启动在线搜索(Online Search Algorithm，ONSA)。在线搜索是指基于具有低计算复杂度的动态代价分布地图，同时使用低计算复杂度的动态搜索算法，来重新规划路径，以避开地理地图上未标注的未知障碍物。

本发明提供的城市低空环境下无人机多目标路径规划方法，实现步骤主要包括五部分。

第一步，以已经被地理地图标记的静态已知障碍物为基础，构建静态安全指数地图。

无人机飞行过程中的不确定性可以利用概率方法来建模，本发明当前只考虑定位误差导致的位置不确定性的情况。在民用领域，定位系统具有5至10米的精度、95％的置信度。虽然单次定位误差相对较小，但这些较小的误差在飞行过程中会不断累积，导致不确定性增大。因此，本发明考虑最简单的误差分布情况——二维高斯分布，来描述这种各向同性的误差，记为p表示二维高斯分布函数，其中μ_x、μ_y是无人机中心位置的坐标，是坐标轴方向上的方差，因为两个方向上的误差分布是相互独立的，所以相关系数为0。该分布如图1所示。

在上述假设下，可以通过以下方式来构建静态安全指数地图：

定义1.风险概率Pr。给定一个二维不确定性模型P(x_c,y_c)，其中(x_c,y_c)表示无人机中心点的坐标，无人机的风险概率Pr(x_c,y_c)是指由于定位误差而可能与障碍物相撞的概率，因此可以表示为无人机的位置概率密度P(x_c,y_c)在障碍物区域上的积分，即位置概率密度在图2所示的危险区域(risk area)上的积分。

根据定义1，基于无人机中心位置的安全概率可以记为

定义2.静态安全指数地图。给定一个空间图G＝(N,A,c)，那么静态安全指数地图就描述了无人机飞行过程中空间图上任意一点的安全指数，它可以表示为：

其中，表示无人机在位置的安全指数。

一条路径的安全指数是指该路径上所有位置点的安全指数之和，并且安全指数越高意味着该路径越危险，亦即安全指数与安全水平成负相关。

下面是构建静态安全指数地图的过程：

首先，给定一幅地理地图MAP，设置无人机GPS定位误差的二维高斯分布方差σ_x、σ_y，设置高斯分布函数置信半径R＝3max{σ_x,σ_y}和协方差矩阵初始化静态安全指数地图S-SIMap，S-SIMap是与Map具有相同维度的零矩阵。

其次，对MAP中的任意一点point，迭代判断point是否在障碍物区域内，若不在，则设置该点处的位置概率为1，并记为P[point]＝1。进一步计算将点ds定位到点point的GPS定位误差的概率P(ds)，其中，点ds是以点point为圆心，R为半径的圆中的任意一点。更新位置概率P[point]的值，即，P[point]＝P[point]-P(ds)。接下来，根据最终的P[point]值计算point点的安全指数，并更新静态安全指数地图S-SIMap中point点对应位置的值。

第二步，构建动态安全指数地图。

生成静态安全指数地图的计算复杂度相对较高，不适合在机载平台上动态生成。因此，本发明提出了一个动态的安全指数地图来弥补这一缺陷，用它来描述地理地图中未被标记的未知障碍物和危险区域。

首先设定感知范围(Perception Range)。无人机在飞行过程中会用传感器探测周围环境以获取周围环境的信息，本发明将无人机的探测范围建模为一个以无人机当前位置为圆心，半径为传感器感知距离R的圆。

如果一架无人机探测到了一个在地理地图中未被标记的未知障碍物，那么它将这些未知障碍物标记为“新障碍物”，并使用下述机制来构建动态安全指数地图。

首先，说明一个定义——安全裕度(Safety Margin)。当无人机在其探测范围内发现未知障碍物时，无人机采取的规避措施是紧急制动，它与无人机的飞行速度相关，将该制动距离作为安全裕度距离，以d_sm表示。

然后，用下式构建动态安全指数地图，表示地图中无人机在位置的安全指数：

其中，d_c表示无人机与该未知障碍物之间的垂直距离。

下面是构建动态安全指数地图的过程：

首先，给定一幅地理地图MAP，设置无人机上机载传感器的感知半径R，以及安全裕度距离d_sm，并初始化动态安全指数地图D-SIMap，D-SIMap是与Map具有相同维度的零矩阵。

其次，对MAP中的任意一点point，迭代判断point是否在已知障碍物区域内，若不在，则设置该点处的安全指数为0，记为I₂[point]＝0。进一步计算点point到无人机探测到的未知障碍物之间的垂直距离d_c，并对比d_c与安全裕度距离d_sm，如果d_c≤d_sm，那么更新点point处的安全指数I₂[point]为+∞，并对应更新到动态安全指数地图D-SIMap中；反之，不更新I₂[point]。

第三步，构建多目标代价函数。

在城市低空复杂环境中的无人机路径规划问题受到一组内部约束和一组外部约束的约束，内部约束如飞行时间、能量消耗、转弯半径等，外部约束如其他无人机、高层建筑、鸟类、高压线等障碍。本发明主要考虑其中两个决策目标，即优化目标，包括安全水平和飞行时间。

1)安全性：安全性目标基于避免碰撞来建模。由于传感器误差，无人机自身的位置、速度和方向信息有一定的不确定性，因此需要与障碍物保持一定安全距离才能保证自身安全，一般而言该距离越大安全水平越高。

2)飞行时间：对于大部分无人机任务来说，飞行时间都是一个重要的优化目标。飞行时间通常由无人机巡航速度、飞行路径等限制。

因此，本发明构建了一个由上述两个优化目标构成的多目标路径规划问题。该问题的解决方案是在空间图G＝(N,A,c)中寻找一条最优路径。其中集合A中的每个参数i都有两个非负代价，即飞行时间和安全水平，分别由c_i,1和c_i,2表示。令g_j(p)是某一条路径p的第j个优化目标的总代价，并令是路径p的代价向量，那么，当该路径与另一条路径q相比较时，所谓最优路径p应当满足下述条件：

且

没有被任何其他方案支配的解即是帕累托最优解，用帕累托集合P表示，P中包含若干条帕累托路径。定义具有最小代价W的一条路径p为帕累托最优路径：

其中权重系数α可以根据实际需求而调节。

第四步，实现离线搜索。

离线搜索方法与Osher和Sethian设计的一类水平集方法具有类似的思想：移动波前来匹配单个参数族。对于一段路径l∈p，令两端点之间的代价w＝αI(l)+(1-α)t，其中t是飞行时间，I(l)是路径段l的安全指数。本发明所使用的离线搜索方法，设计了一个新的波速函数，该函数不仅与当前点处无人机的速度有关还与无人机的安全指数有关。定义新的波速函数为：

其中，表示当前无人机的位置，表示处无人机的速度函数，表示地图中无人机在位置处的安全指数。

这意味着，当安全指数越大(即安全水平越低)，飞行速度越小时，波的传播速度相应的就越低。

下面首先描述构建无人机路径规划静态代价分布地图的方法，其具体的实现过程如下，结合图3来进行说明。

首先，给定无人机的起点START和终点GOAL，给定无人机的飞行速度VELOCITY，以及一幅静态安全指数地图S-SIMap。

其次，令A表示无人机的终点GOAL的集合，集合A包含无人机的全部目的地点，初始化代价cost[START]为0，并令NarrowBand表示无人机的START的集合，集合NarrowBand包含无人机的全部起始点。对NarrowBand中的任意一点node，迭代判断node是否在禁止区域内，若不在，则更新node点处的代价cost[node]为1/V^*(node)。若node在禁止区域内，则对该点不进行计算。此处的禁止区域是指已知的静态障碍物范围。

node点处的代价还可以表示为cost[node]＝(αS-SIMap[node]*VELOCITY+(1-α))/VELOCITY；S-SIMap[node]为静态安全指数地图中node点处的安全指数。与上面计算公式对应，此处的node相当于当前无人机的位置S-SIMap[node]相当于VELOCITY相当于

接下来，令FarAway表示其他节点的集合，集合FarAway中包含地图中除了加到集合A和集合NarrowBand中的节点之外的全部节点，对FarAway中的任意一点node，令cost[node]＝+∞。再判断NarrowBand和FarAway是否为空，如果NarrowBand和FarAway都不为空，那么将NarrowBand中的代价最小的节点记为best_neighbor，并将该best_neighbor加入集合A，同时，迭代判断best_neighbor的任意一个邻居节点node是否在禁止区域内，如果不在，那么，将邻居节点node的代价cost[node]更新为1/V^*(node)，再次判断邻居节点node是否在FarAway中，如果在，那么将node点从FarAway中移除，并将node点添加到NarrowBand中。如此，反复迭代判断NarrowBand和FarAway是否为空，直到条件终止。最终生成的集合A就是静态代价分布地图COST。

需要指出的是，按照相同的方法，当将上述过程中的静态安全指数地图替换成动态安全指数地图后，就得到了动态代价分布地图。

本发明的离线搜索过程，是依据静态代价分布地图，为无人机离线规划一条完整的从起始地到目的地的代价最小的航迹。按照这条航迹飞行，无人机不仅可以安全地避开静态障碍物而且能够快速地抵达目的地。离线搜索包括如下步骤：

步骤1，给定无人机的起点START和终点GOAL，输入一幅静态代价分布地图COST。

步骤2，令当前点current为无人机的起点START，记path为包含START的路径。迭代判断当前current是否等于GOAL，如果不等于，那么判断current是否有邻居节点，若没有邻居节点，则返回失败(failure)，若有邻居节点，则更新current为current的邻居中代价最小的节点，并将更新后的current加入到path中。反之，如果current等于GOAL，那么直接返回path。

第五步，实现在线搜索。在线搜索是根据动态代价分布地图，为无人机在线规划一段变更的航迹，按照这段航迹飞行，无人机能够快速安全地避开动态障碍物。

本发明在线搜索方法使用与A*算法相似的思想，同时与之存在两处显著差别：(1)利用上述离线搜索方法得到的结果，即代价分布，来指导其搜索方向而不是传统的欧氏距离或曼哈顿距离；(2)利用限制搜索空间的机制来降低搜索时间。

本发明的在线搜索方法利用代价启发函数来引导其搜索，表示形式如下：

其中是从起点到当前位置的路径的总代价，表示从当前位置到目的地的估计的路径代价，表示当前无人机的位置。对于A*算法，一般使用欧氏距离或曼哈顿距离作为其因为它是理论上的两点之间的最小距离代价。但是，在前述离线搜索方法中，已经计算出了空间图中任意一点到终点的帕累托最优路径及其对应的最小代价，因此，此处可以直接使用这个代价作为该点的

A*算法的计算复杂度是O(NlogN)，其中N是当前地图中的点的数量。因此，当地图较大或分辨率较高时，N就相应地变得很大，并且若当前位置与终点距离较远时，A*算法的运算时间将会很长，无法满足在线方法的实时性要求。

为此，本发明提出了一种限制搜索空间的机制，即当前搜索点和终点之间的连线与标记的未知障碍物之间不相交的时候，搜索过程即停止。这相当于此时从搜索终点可以“看到”目的地点，那么从该点开始一定可以重新根据离线搜索规划出一条帕累托最优路径来。并且，在当前位置与终点距离较远时，该机制可以有效地降低算法的搜索空间，使在线搜索方法的运行时间得到显著的下降。

在线搜索算法的一个具体实现过程可以归纳为如下几个步骤，结合图4说明如下：

(4.1)给定无人机的起点START和终点GOAL，输入一幅动态代价分布地图COST。

(4.2)初始化集合CLOSED为空集，令OPEN表示包含START的优先队列，同时，令came_from为空字典，初始化路径代价g[start]为0，并令代价启发函数的初始值f[start]等于COST[START]。

(4.3)判断OPEN是否为空，当OPEN非空时，将OPEN中弹出的优先度最高的点作为当前点current，然后执行下面(4.4)～(4.8)；如果OPEN为空时，返回失败(failure)，停止执行。

(4.4)如果current等于GOAL或者从current到GOAL的连线与被标记为NEW_FORBIDDEN的区域没有交点，那么返回从came_from中重构出的路径队列，并终止程序。否则，直接继续执行(4.5)。被标记为NEW_FORBIDDEN的区域是指检测到的未知障碍物区域。

(4.5)将current加入到CLOSED中。

(4.6)判断current的任意一个邻居neighbor是否在禁止区域或者NEW_FORBIDDEN中，如果不在，那么，更新估计的路径代价tentative_g为g[current]与从current到neighbor的代价之和，继续执行(4.7)。

(4.7)判断neighbor是否在OPEN中，且tentative_g是否小于g[neighbor]，如果两个条件同时满足，那么，将neighbor从OPEN中移除。否则，继续执行(4.8)。

(4.8)判断neighbor是否在CLOSED中，且tentative_g是否小于g[neighbor]，如果两个条件同时满足，那么，将neighbor从CLOSED中移除。否则，继续执行(4.9)。

(4.9)判断neighbor是否在OPEN中，且neighbor是否在CLOSED中，如果都不在，那么，将tentative_g赋值给g[neighbor]，并更新f[neighbor]为g[neighbor]与neighbor处的代价的和，同时将neighbor加入OPEN中，且令came_from[neighbor]等于current。

(4.10)继续判断current是否还有新的邻居，如果有，转(4.6)继续执行，否则，转(4.3)执行，继续判断OPEN是否为空。

需要说明的是，在线搜索实现步骤并不局限于上述的步骤(4.1)～(4.10)，只要符合在线搜索的要求，在搜索过程中，采用所述的限制搜索空间的机制，并以所述的代价启发函数来引导搜索方向，则采用其他不需要付出脑力劳动而实现的步骤也属于本申请的实现范畴。

第六步，基于离线搜索与在线搜索联合求解多目标路径规划问题，获取最优路径规划方案。根据无人机的起点、终点和飞行速度，采用离线搜索为无人机离线规划一条从起点到终点的代价最小的路径；无人机在飞行过程中，当检测到未知障碍物时，采用在线搜索为无人机在线规划一段变更的航迹，以避开动态障碍物。

本发明设计的多目标路径规划方法的一个具体实现过程，结合图5，各步骤说明如下：

(5.1)给定无人机的起点START和终点GOAL，输入一幅地理地图MAP，并设置无人机GPS定位误差的二维高斯分布方差σ_x、σ_y，设置无人机的飞行速度为VELOCITY，机载传感器的探测范围为MARGIN。

(5.2)调用离线搜索方法生成起点START到终点GOAL的路径plan_path。

(5.3)初始化空队列replan_path，设置包含START的队列actual_path，同时，令当前节点current等于START，设标识位is_replanning等于false。

(5.4)判断current是否是GOAL，若不是，执行(5.5)，若current是GOAL，则执行(5.10)。

(5.5)判断is_replanning是否是false，若是，则将plan_path中弹出的节点设置为current，然后执行(5.7)；若不是，则执行(5.6)；

(5.6)判断replan_path是否为空，若不是，则将replan_path中弹出的节点设置为current，然后执行(5.7)；若是，则令is_replanning等于false，且调用离线搜索方法生成current到GOAL的路径，同时返回到(5.4)，继续判断current是否是GOAL。

(5.7)接下来将current加入到actual_path的队尾，且设标志位is_detected_new_obstacles等于false，初始化空集new_obstacles。

(5.8)再迭代判断任意一个在无人机探测范围内的点node是否为未被检测的障碍物UNDETECETED_OBSTACLE，若是，则将node标记为已知障碍物DETECETED_OBSTACLE，且将node加入new_obstacles，并令is_detected_new_obstacles等于true。如此遍历所有无人机探测范围内的点。

(5.9)此后，令集合extended_new_obstacles表示包含了全部到集合new_obstacles所代表的区域的距离小于或者等于MARGIN的点的集合，并标记extended_new_obstacles内的全部点为NEW_FORBIDDEN，接下来，判断is_detected_new_obstacles是否为true，如果是，那么，调用在线搜索方法生成从current到终点GOAL的路径replan_path，同时，令is_replanning等于true。

(5.10)迭代直到current是GOAL，此时，返回actual_path。

需要说明的是，基于离线搜索和在线搜索实现多目标路径规划的步骤并不局限于上述的步骤(5.1)～(5.10)，若仅是采用其他不需要付出脑力劳动而改动的步骤也属于本申请的实现范畴。

下面是利用本发明方法实现路径规划的一个实例。

图6给出了原始地图，其中黑色部分为地理地图上标记的已知障碍物，灰色部分为地理地图上未标记的未知障碍物，白色部分为自由空域。

图7给出了在图6所示地图下的多目标路径规划的帕累托最优解的集合，其中黑色点表示找到的一条可行路径，黑圈点表示找到的帕累托最优路径。由图7可知：

●较短的飞行时间对应较大的安全指数，即较低的安全水平，这两个目标之间是相互矛盾的；

●结果中不存在同时具有最小飞行时间和最小安全指数(最高安全水平)的路径；

●用户可以根据自己的实际需求，从帕累托最优路径集合中(黑圈点对应的路径)选择适合自己的路径。

图8给出了路径安全指数为38、预计飞行时间为296.5秒的一条路径。图中灰色区域中的颜色的深浅代表了安全指数的大小，灰色越深代表安全指数越大，亦即安全水平越低；图中“O”为起点，“X”为终点，其中连接的曲线是规划出的路径。

图9、图10分别给出了在不同探测范围条件下在线搜索的运行结果。图中灰色区域中的颜色的深浅代表了安全指数的大小，灰色越深代表安全指数越大，亦即安全水平越低；“O”为起点，“X”为终点，黑色曲线是规划出的路径；黑色圆点表示无人机当前位置，在它周围有一个浅灰色圆盘，表示无人机的探测范围；在(e)、(f)、(g)中的虚线表示在线搜索重规划出的路径；深褐色表示已被探测到的未知障碍物，浅褐色表示为被探测到的未知障碍物添加的安全裕度区域。由这些结果可以得出：

●更大的探测范围可以帮助无人机更好地避开未知的障碍物；

●图中的虚线都很短，说明在线搜索的搜索范围很小，其算法运行时间很短。表1给出了两种探测范围下在线搜索运行的平均耗时和最大耗时。

表1：在线搜索算法的运行时间

探测范围	平均耗时	最长耗时
			10m	0.1588us	0.5998us
30m	0.1333us	0.6227us

从表1中可以看出，无人机的探测范围越大，在线搜索所需要的平均耗时越少。

Claims

1.一种城市低空环境下无人机多目标路径规划联合搜索方法，其特征在于，包括如下步骤：

步骤1，以地理地图中标记的静态已知障碍物为基础，构建静态安全指数地图；所述的静态安全指数地图描述无人机飞行过程中空间图上任意一点的静态安全指数；

步骤2，以无人机在飞行过程中主动探测到的没有标记在地理地图中的障碍物为基础，在机载平台上构建动态安全指数地图；所述的动态安全指数地图描述无人机在飞行过程中探测到的空间中任意一点的动态安全指数；

步骤3，构建联合优化无人机飞行时间和飞行安全的多目标路径规划问题；

步骤4，基于离线搜索与在线搜索联合求解多目标路径规划问题，获取最优路径规划方案；

根据无人机的起点、终点和飞行速度，采用离线搜索为无人机离线规划一条从起点到终点的代价最小的路径；无人机在飞行过程中，当检测到未知障碍物时，采用在线搜索为无人机在线规划一段变更的航迹，以避开动态障碍物；所述的在线搜索在当前搜索点和终点之间的连线与检测到的未知障碍物之间不相交的时候，搜索过程停止。

2.根据权利要求1所述的方法，其特征在于，所述的步骤1中构建静态安全指数地图时，设Pr(x_c,y_c)表示无人机的风险概率，通过无人机的位置概率密度在障碍物区域上的积分得到，其中(x_c,y_c)表示无人机中心点的坐标；无人机中心位置的安全概率静态安全指数地图中，无人机在位置的安全指数

3.根据权利要求1所述的方法，其特征在于，所述的步骤2中构建动态安全指数地图时，设(x_c,y_c)表示无人机中心点的坐标，首先将无人机的探测范围建模为一个以无人机当前位置为圆心，半径为传感器感知距离的圆，然后判断探测范围内是否存在与所探测到的障碍物之间的垂直距离小于等于安全裕度距离的点，如果存在，则设置无人机在位置(x_c,y_c)的安全指数为+∞，否则设置为0。

4.根据权利要求1所述的方法，其特征在于，所述的步骤3中构建的多目标路径规划问题，定义具有最小代价W的一条路径p为帕累托最优路径；

W = \underset{p &Element; P}{m i n} {{αg}_{1} (p) + (1 - α) g_{2} (p)}

其中，α为权重系数，g_j(p)是路径p的第j个优化目标的总代价，j∈{1,2}，集合P是求解多目标路径规划问题的帕累托路径集合。

5.根据权利要求1所述的方法，其特征在于，步骤4中所述的离线搜索中，采用如下波速函数来计算：

V^{*} (\overset{&RightArrow;}{s}) = \frac{V (\overset{&RightArrow;}{s})}{α I (\overset{&RightArrow;}{s}) \cdot V (\overset{&RightArrow;}{s}) + (1 - α)}

其中，表示当前无人机的位置，表示处无人机的速度函数，表示地图中无人机在位置处的安全指数，α为权重系数。

6.根据权利要求1或5所述的方法，其特征在于，步骤4中所述的离线搜索，实现步骤包括：

(1)根据无人机的起点、终点和飞行速度，基于静态安全指数地图生成静态代价分布地图；

设集合A包含无人机的全部目的地点，集合NarrowBand包含无人机的全部起始点，集合FarAway包含地图中除了集合A和NarrowBand中的节点之外的全部节点；

对NarrowBand中不在禁止区域内的任意一点node，更新node点处的代价cost[node]；

cost[node]＝(αS-SIMap[node]*VELOCITY+(1-α))/VELOCITY，

其中，α为权重系数，S-SIMap[node]为静态安全指数地图中node点处的安全指数，VELOCITY为无人机的飞行速度；

对FarAway中的任意一点node，令cost[node]＝+∞；

迭代判断NarrowBand和FarAway是否为空，如果NarrowBand和FarAway都不为空，执行(1.1)，否则终止迭代，最终生成的集合A就是静态代价分布地图；

(1.1)将NarrowBand中的代价最小的节点记为best_neighbor，并将该best_neighbor加入集合A，同时，对best_neighbor的任意一个不在禁止区域内的邻居节点node，更新该node点处的代价cost[node]；再判断该node点是否在FarAway中，如果在，将其从FarAway中移除，并添加到NarrowBand中；

(2)依据静态代价分布地图，为无人机离线规划一条从起点到终点的路径。

7.根据权利要求1所述的方法，其特征在于，步骤4中所述的在线搜索中，利用代价启发函数来引导搜索；

f (\overset{&RightArrow;}{s}) = g (\overset{&RightArrow;}{s}) + h (\overset{&RightArrow;}{s})

其中，表示当前无人机的位置，是从起点到当前位置的路径的总代价，表示从当前位置到目的地估计的路径代价，根据离线搜索得到。

8.根据权利要求1或7所述的方法，其特征在于，步骤4中所述的在线搜索，实现步骤包括：

(1)输入无人机的起点START、终点GOAL和动态代价分布地图COST；初始化空集集合CLOSED，令OPEN表示包含START的优先队列，设置空字典came_from，并令路径代价g[start]为0，令代价启发函数的初始值f[start]等于COST[START]；

(2)判断OPEN是否为空，若OPEN非空，则将OPEN中弹出的优先度最高的点作为当前点current，然后执行(2.1)～(2.3)；如果OPEN为空，停止执行；

(2.1)如果current等于GOAL或者从current到GOAL的连线与标记为NEW_FORBIDDEN的区域没有交点，那么返回从came_from中重构出的路径队列，并结束执行过程；否则，将current加入到CLOSED中，继续执行；标记为NEW_FORBIDDEN的区域是指检测到的未知障碍物区域；

(2.2)判断current的任意一个邻居neighbor是否在禁止区域或者NEW_FORBIDDEN中，如果不在，更新估计的路径代价tentative_g为g[current]与从current到neighbor的代价之和，继续执行(2.3)；

(2.3)判断neighbor是否在OPEN中，且tentative_g是否小于g[neighbor]，如果两个条件同时满足，那么，将neighbor从OPEN中移除；否则，继续执行(2.4)；

(2.4)判断neighbor是否在CLOSED中，且tentative_g是否小于g[neighbor]，如果两个条件同时满足，那么，将neighbor从CLOSED中移除；否则，继续执行(2.5)；

(2.5)判断neighbor是否在OPEN中，且neighbor是否在CLOSED中，如果都不在，那么，将tentative_g赋值给g[neighbor]，并更新f[neighbor]为g[neighbor]与neighbor处的代价的和，同时将neighbor加入OPEN中，且令came_from[neighbor]等于current；

(2.6)继续判断current是否还有新的邻居，如果有，转(2.2)继续执行，否则，转(2)执行，继续判断OPEN是否为空。

9.根据权利要求1所述的方法，其特征在于，所述的步骤4，基于离线搜索与在线搜索联合求解多目标路径规划问题的实现步骤包括：

(1)输入无人机的起点START和终点GOAL，以及地理地图MAP；设置无人机GPS定位误差的二维高斯分布方差，设置无人机的飞行速度VELOCITY，确定机载传感器的探测范围MARGIN；

调用离线搜索方法生成起点START到终点GOAL的路径plan_path；

设置空队列replan_path，设置包含START的队列actual_path，令当前节点current等于START，设标识位is_replanning等于false；

(2)判断current是否是GOAL，若不是，执行(2.1)～(2.3)；若current是GOAL，则返回actual_path，并终止执行；

(2.1)判断is_replanning是否是false，若是，则将plan_path中弹出的节点设置为current，然后转(2.2)继续执行；若不是，则判断replan_path是否为空，若不是，则将replan_path中弹出的节点设置为current，然后转(2.2)继续执行，若是，则令is_replanning等于false，且调用离线搜索方法生成current到GOAL的路径，然后，返回到(2)继续执行；

(2.2)将current加入到actual_path的队尾，设标志位is_detected_new_obstacles等于false，初始化空集new_obstacles；继续执行(2.3)；

(2.3)判断任意一个在无人机探测范围内的点node是否为未被检测的障碍物UNDETECETED_OBSTACLE，若是，则将node标记为已知障碍物DETECETED_OBSTACLE，且将node加入new_obstacles，并令is_detected_new_obstacles等于true；如此遍历所有无人机探测范围内的点；接下来，令集合extended_new_obstacles表示到集合new_obstacles所代表的区域的距离小于或者等于MARGIN的点的集合，并标记extended_new_obstacles内的全部点为NEW_FORBIDDEN，然后，判断is_detected_new_obstacles是否为true，如果是，那么，调用在线搜索方法生成从current到终点GOAL的路径replan_path，同时，令is_replanning等于true；然后，返回到(2)继续执行。