CN109634304A - 无人机飞行路径规划方法、装置和存储介质 - Google Patents

Abstract

本申请涉及一种无人机飞行路径规划方法、装置、计算机设备和存储介质，所述方法包括：获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息，获取当前三维环境地图，根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径，判断候选规划路径是否满足安全飞行条件，当满足时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。通过无人机上的拍摄设备拍摄的图像实时生成三维环境地图，根据实时生成的三维环境地图确定无人机周围的环境信息，根据环境信息和无人机定位信息实时规划飞行路径，并根据安全飞行条件保证无人的安全飞行，从而实现无人机在未知环境中的安全飞行。

Description

无人机飞行路径规划方法、装置和存储介质

技术领域

本申请涉及计算机视觉领域，尤其涉及一种无人机飞行路径规划方法、装置和存储介质。

背景技术

近年来，随着无人机市场的飞速增长，其相关技术也正在发生日新月异的变革，以往多用于特种行业甚至军用产品上的技术，如飞行控制、图像传输、目标识别和跟踪等，也越来越多地用于消费级无人机，使其越来越接近于自动化甚至智能化飞行机器人的概念。无人机凭借其良好的飞行性能和灵活的操控性能在国民经济和国防军事的诸多领域中应用广泛。在复杂多变的未知环境中执行飞行任务时，为了避免与障碍物相撞，无人机需要具备自主避障能力，才能保证无人机的安全飞行。由于现有的无人机主要很难在未知环境中快速的实现避障。

发明内容

为了解决上述技术问题，本申请提供了一种无人机飞行路径规划方法、装置、计算机设备和存储介质。

第一方面，本申请提供了一种无人机飞行路径规划方法，包括：

获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息；

获取当前三维环境地图，当前环境三维环境地图为根据无人机的拍摄设备的当前图像进行图像重建得到的；

根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径；

判断候选规划路径是否满足安全飞行条件，当规划路径满足安全飞行条件时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。

第二方面，本申请提供了一种无人机的路径规划装置，包括：

获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息；

获取当前三维环境地图，当前环境三维环境地图为根据无人机的拍摄设备的当前图像进行图像重建得到的；

根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径；

判断候选规划路径是否满足安全飞行条件，当规划路径满足安全飞行条件时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息；

获取当前三维环境地图，当前环境三维环境地图为根据无人机的拍摄设备的当前图像进行图像重建得到的；

根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径；

判断候选规划路径是否满足安全飞行条件，当规划路径满足安全飞行条件时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息；

获取当前三维环境地图，当前环境三维环境地图为根据无人机的拍摄设备的当前图像进行图像重建得到的；

根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径；

判断候选规划路径是否满足安全飞行条件，当规划路径满足安全飞行条件时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。

上述无人机飞行路径规划方法、装置、计算机设备和存储介质，所述方法包括：获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息，获取当前三维环境地图，当前环境三维环境地图为根据无人机的拍摄设备的当前图像进行图像重建得到的，根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径，判断候选规划路径是否满足安全飞行条件，当规划路径满足安全飞行条件时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。通过无人机上的拍摄设备拍摄的图像实时生成三维环境地图，实时生成地图能够得到无人机所处的环境信息，根据生成的三维环境地图和无人机定位信息实时规划无人机的飞行路径，并根据安全飞行条件保证无人的安全飞行，从而实现无人机在未知环境中的安全飞行。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为一个实施例中无人机飞行路径规划方法的应用环境图；

图2为一个实施例中无人机飞行路径规划方法的流程示意图；

图3为一个实施例中A*算法的路径规划流程示意图；

图4为一个具体实施例中，无人机飞路径规划方法的流程示意图；

图5为一个实施例中无人机飞行路径规划装置的结构框图；

图6为一个实施例中计算机设备的内部结构图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

图1为一个实施例中无人机飞行路径规划方法的应用环境图。参照图1，该无人机飞行路径规划方法应用于无人机飞行路径规划系统。该无人机飞行路径规划系统包括终端110和、服务器120。终端110和服务器120通过网络连接。终端或服务器获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息，获取当前三维环境地图，当前环境三维环境地图为根据无人机的拍摄设备的当前图像进行图像重建得到的，根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径，判断候选规划路径是否满足安全飞行条件，当规划路径满足安全飞行条件时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。终端110具体可以是台式终端或移动终端，移动终端具体可以手机、平板电脑、笔记本电脑等中的至少一种。服务器120可以用独立的服务器或者是多个服务器组成的服务器集群来实现。

如图2所示，在一个实施例中，提供了一种无人机飞行路径规划方法。本实施例主要以该方法应用于上述图1中的终端110(或服务器120)来举例说明。参照图2，该无人机飞行路径规划方法具体包括如下步骤：

步骤S201，获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息。

具体地，无人机的当前节点的当前位姿信息是指无人在当前时刻所处的位置和姿态，该位置和姿态可以是指与预设设备之间的位置和姿态，也可以是对某个地点之间的位置和姿态等。无人机的当前节点随着无人机的移动会发生改变，如在A时刻当前节点为无人机所在的位置A，在B时刻当前节点为无人机所在的位置B。目标节点的随着无人机的当前节点的改变发生改变，目标节点与当前节点对应。目标节点的位置信息的表示方式可以自定义，如可以用与当前节点的相对位置信息表示，也可以直接采用GPS等定位系统的定位信息表示，如目标节点的位置信息为东经45度和北纬25度。

在一个实施例中，获取无人机的当前位姿信息对应的目标位置信息，包括：将当前位姿信息的第一预设距离的位置处的位置信息，作为候选目标位置信息。

具体地，第一预设距离是指预先设置的用于确定候选目标位置的距离，预设距离可以自定义，如可以根据项目需求、环境或无人机的环境地图建模型能力等其中的至少一个确定。将距离当前节点得距离为第一预设距离的位置作为候选目标节点，候选目标节点对应的位置信息作为候选目标位置信息。

判断候选目标位置信息对应的候选环境信息是否满足环境约束条件，当候选环境信息满足所述环境约束条件时，将候选目标位置信息作为目标位置信息。

具体地，环境约束条件是预先设置的用于判断无人机在候选目标节点是否能够安全飞行的临界条件。满足环境约束条件的节点表示在该节点无人机可以安全飞行。环境约束条件的设置与无人机的机型等相关，不同的机型对应的环境约束条件不相同，根据机型设置环境预设条件能够更好的实现无人机的控制，使得无人机能够适应更复杂的飞行环境。如环境约束条件设置为无人机的飞行立体空间，立体空间设置的越大，飞行越安全，但是设置过大会导致增加无人机的飞行代价，增加飞行成本，故环境约束条件需要设置为合理的范围。

当候选环境信息未满足环境约束条件时，更新候选目标位置信息，直至更新后的候选目标位置信息的候选环境信息满足环境约束条件，将更新后的候选目标位置信息作为目标位置信息。

具体地，候选环境信息未满足环境约束条件，表示候选目标节点处无人机无法安全飞行，此时，可以调整候选目标的位置信息，如增大候选目标节点与当前节点之间的距离得到新的候选节点。判断新的候选目标节点对应的候选环境信息是否满足环境约束条件，满足时，将新的候选目标节点作为目标节点，同理不满足时，继续调整候选目标节点与当前节点之间的距离，直至满足时，得到目标节点为止。

步骤S202，获取当前三维环境地图。

具体地，当前环境三维环境地图为根据无人机的拍摄设备的当前图像进行图像重建得到的。无人机上搭载有多个拍摄设备，根据无人机上搭载的拍摄设备在当前时刻拍摄的图像进行三维建模，得到对应的三维环境地图。当前三维环境地图的构建可以根据预设三维模型构建方法进行构建，三维模型的构建方法包括根据无人机与环境中各个物体之间的相对距离建立对应的三维环境地图，如利用无人机上搭载的双目立体视觉测量系统实时获取无人机外部环境的深度信息，将深度信息转化为点云数据，对点云数据并进行体素化描述，结合机载定位装置获取的定位数据和无人机上的惯性导航设备的测量信息实现当前三维环境地图的构建。三维环境地图中包含环境中的各种物品所处的位置，如当前时刻无人机正前方5米处存在一个柱子，则三维环境地图中会按照建图比例在与无人机正前方5米处对应的地方建立一个类似柱子的三维模型。对无人机周围环境进行实时建模，能够实时感知周围环境，根据实时飞行环境确定无人机的飞行路径，提高无人机的飞行安全性。

步骤S203，根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径。

步骤S204，判断候选规划路径是否满足安全飞行条件，当规划路径满足安全飞行条件时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。

具体地，通过当前三维环境地图确定当前节点与目标节点之间存在无人机飞行的障碍物，根据预先设置的路径规划算法规划当前节点与目标节点之间的规划路径，将规划得到的规划路径作为候选路径。安全飞行条件是指预先设置的用于保障无人机安全飞行的条件，安全飞行条件可以是指无人机与各个方向的障碍物之间的安全距离，安全距离可以自定义设置，如不同的无人机的机型的体积大小不一致，可以根据无人机的机型确定安全距离。当规划路径满足安全飞行条件时，说明无人机能够从当前节点安全的抵达目标节点，则可以根据规划路径生成对应的飞行指令，飞行指令是用于指引无人机飞行的计算机指令，生成对应的计算机指令后，使得无人机执行该计算机指令，无人机依照规划路径飞向目标节点。

在一个实施例中，飞行指令包含多个子飞行指令，在执行飞行指令之前之间还包括：判断飞行指令中是否包含同一个方向的多个子飞行指令，当包含多个同一个方向的飞行子指令时叠加成该方向的一个飞行指令。对同一个方向的子飞行指令进行叠加时是指对相邻的两个同一方向的飞行指令的叠加。假设每个子飞行指令的飞行距离为1m，如规划路径为先向正前方飞行5m，向左飞行3m，在向正前方飞行4m，向右飞行3m，最后在向正前方飞行6m，则第一个向正前方飞行5m需要生产5个子飞行指令，则将这5个子飞行指令叠加成一个飞行5m的叠加指令。同理得到一个向左飞行3m的叠加指令，向正前方飞行的4m的叠加指令、向右飞行的3m的叠加指令和向正前方飞行6m的叠加指令，由原来的21个指令，变为5个指令，减少了无人机执行指令的次数，降低了因为位移太小而引起的累积位移误差，提高了执行速率和精确度。

上述无人机飞行路径规划方法，所述方法包括：获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息，获取当前三维环境地图，根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径，判断候选规划路径是否满足安全飞行条件，当满足时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。通过无人机上的拍摄设备拍摄的图像实时生成三维环境地图，根据实时生成的三维环境地图确定无人机周围的环境信息，根据环境信息和无人机定位信息实时规划飞行路径，并根据安全飞行条件保证无人的安全飞行，从而实现无人机在未知环境中的安全飞行。

在一个实施例中，步骤S201之后，还包括：

步骤S301，判断当前节点的当前环境信息是否满足环境约束条件。

步骤S302，当当前环境信息满足环境约束条件时，计算当前节点在预设空间内的各个候选中间节点的候选代价。

步骤S303，从候选代价中选取满足预设代价条件的目标代价，将目标代价对应的候选节点作为子目标节点。

步骤S304，将子目标节点作为当前节点，进入判断当前节点的当前环境信息是否满足环境约束条件的步骤，直至子目标节点为目标节点。

具体地，首先判断当前节点对应的环境信息是否满足环境约束条件，当不满足时，若是在未起飞时，可以直接调整起始点，若是在飞行过程中，则结束飞行，或者采用人工干预的方式指挥无人机飞行。当满足时，则获取无人机的当前节点的预设空间内的各个候选中间节点。如将当前节点周围的26个节点作为候选中间节点，计算各个候选中间节点的候选代价，候选代价是指通过各个节点进行飞行时的飞行代价，从各个候选代价中选取代价中按照预设代价选取规则选取目标代价，如从各个候选代价中选取代价最小的候选代价作为目标代价，目标代价对应的候选中间节点作为子目标节点。其中子目标节点为当前节点与目标节点之间的中间节点，如当前节点为Start，目标节点为End，当前节点Start和目标节点End之间存在多种路径的对应的中间节点，子目标节点是指满足环境约束条件，且当前节点Start和目标节点End之间的飞行代价最小的路径对应的中间节点。

在得到当前节点的子目标节点后，判断子目标节点对应的环境信息是否满足对应是否满足环境约束条件，满足时，计算子目标节点对应的各个候选中间节点的候选代价，选取其中代价值最小的作为下一个目标子节点，不断重复判断各个子目标节点的环境约束条件和对应的候选代价，直到子目标节点为目标节点。

在一个实施例中，直到子目标节点为目标节点之后，还包括：将目标节点作为当前节点，重复进入获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息，直到无人到达目的地。

在一个实施例中，步骤S302，包括：根据当前节点与各个候选中间节点的位移差，计算当前节点与各个候选中间节点的真实代价和校正代价，根据当前节点与目标节点的位移差，计算当前节点到目标节点的启发代价，对真实代价、校正代价和启发代价进行加权求和，得到各个候选中间节点对应的总代价。

在本实施例中，步骤S303，包括：从各个候选中间节点对应的总代价中选取代价最小的候选中间节点的总代价作为目标代价。

具体地，计算各个当前节点与各个候选中间节点的位移差，位移差包括垂直方向和水平方向的位移差，在三轴坐标系中，可以定义基坐标系，如可以将“北-东-地”坐标系作为基坐标系，“北-东-地”分别对应“X-Y-Z”。计算当前节点与各个候选中间节点的X方向、Y方向和Z方向的位移差，根据三个方向的位移差计算当前节点与各个候选中间节点的真实代价和校正代价。其中校正代价是指定义的用于校正真实代价的代价，校正代价可以自定义用于增加预设方向的代价，如增加Z方向的代价，可以增大Z方向的代价的系数。根据当前节点与目标节点之间的位移差计算启发代价。计算校正代价、真实代价和启发代价的加权结果，加权系数可以自定义设置。根据各个代价对应的加权系数求和，得到候选中间节点的总代价。从候选中间节点的总代价选取代价最小的总代价作为目标代价，将目标代价对应的候选中间节点作为子目标节点。在预设空间内查找子目标节点，需要设置合理的空间范围，若预设空间设置太大，增加数据量，从而导致处理速度低下，难以保证无人机的飞行的实时性。

在一个实施例中，上述无人机飞行路径规划方法，还包括：

步骤S401，根据当前三维环境地图中的环境信息，判断在无人机的预设方向是否存在障碍物。

步骤S402，当在预设方向存在障碍物时，判断当前三维环境地图中与无人机的距离小于或等于第二预设距离内的像素，与大于第二预设距离内的像素的数量比值。

步骤S403，当比值小于或等于预设比值时，进入根据当前三维环境地图、当前位姿信息和目标位置确定候选规划路径。

具体地，环境信息是指地图中的环境信息，环境信息包括障碍物信息，障碍物信息包括但不限于障碍物的位置信息和障碍物的尺寸信息等。预设方向是指预先设置的无人机的任意一个方向，如可以设置预设方向为无人机的前方，当无人机的前方不存在障碍物时沿着原来的航向继续飞行，若存在障碍物时，则判断障碍物是否在上方或下方，在障碍区全部在上方或下方或时，则继续飞行。

当在预设方向存在障碍物时，获取当前三维环境地图中与无人机之间的距离，即与当前节点之间的距离小于或等于第二预设距离内的像素的数量，和获取与大于第二预设距离内的像素的数量，计算在第二预设距离内的数量与第二预设距离外的数量的比值。其中第二预设距离自定义设置，如可以根据三维环境地图的建图规则确定。如设置在深度信息在10米内的为障碍物，在10米外的为空旷点，则可以设置第二预设距离为10米。

判断比值与预设比值之间的大小，当比值小于或等于预设比值时，表示有较大的概率能够使得飞机安全飞行，进入根据当前三维环境地图、当前位姿信息和目标位置确定候选规划路径的步骤，通过当前三维环境地图、当前位姿信息和目标位置规划路径。

在一个实施例中，当比值大于预设比值时，还包括：

步骤S501，获取目标节点与无人机的相对位置，根据相对位置确定第一方向。

具体地，获取目标节点与无人机的相对位置，相对位置是根据获取的当前节点与目标节点的位置信息确定的，根据相对位置确定目标节点在无人机的哪个方位，如在左前方，则将左边作为作为第一方向。

在一个实施例中，当目标节点在无人机的正前方时，可以预先设置一个方向为第一方向，如设置为无人机的左边或右边为第一方向。

步骤S502，规划无人机在第一方向的第一路径，判断第一路径是否满足安全飞行条件。

步骤S503，当第一路径满足安全飞行条件时，生成与第一路径对应的飞行指令。

具体地，采用路径规划算法规划从第一方向飞行的第一路径，判断规划的第一路径是否满足安全飞行条件，当满足时，生成与第一路径的对应的飞行指令，执行该飞行指令，以使无人机按照第一路径飞行。第一路径是否满足安全飞行条件的判断过程与步骤S204中的判断过程一致，在此不再赘述。

步骤S504，当第一路径未满足安全飞行条件时，规划无人机在第二方向的第二路径，判断第二路径是否满足安全飞行条件。

步骤S505，当第二路径满足安全飞行条件时，生成与第二路径对应的飞行指令。

具体地，当第一路径未满足安全飞行条件时，规定第二方向的第二路径，其中第一路径和第二路径为不同方向的路径，如第一方向为左边，第二方向可以设置为右边。规划从第二方向飞行的路径，作为第二路径，同理对第二路径是否满足安全飞行条件进行判断，当满足时，生成与第二路径对应的飞行指令，无人机执行该飞行指令，以使无人机按照第二路径飞行。

在一个实施例中，当第二路径不满足安全飞行条件时，可以采用人工干预的方式调整无人机的飞行路径。

在一个具体的实施例中，上述无人机飞行路径规划方法，包括：

构建体素化环境地图。获取无人机上的至少两个摄像机拍摄的图像，当两个摄像机在同一时刻观测到空间物体的同一特征点P(x_c，y_c，z_c)，分别在第一相机拍摄的第一图像和第二相机拍摄的第二图像上P点的坐标，假设上P点在第一图像的位置坐标为P₁(x₁，y₁，z₁)，第二图像上的坐标为P₂(x₂，y₂，z₂)，双目相机依据视差原理测量其与P点之间的距离，设B为两摄像机投影中心的连线距离，第一相机和第二相机的焦距分别为f1，f2，由三角几何关系可以得到 其中D＝x₁-x₂。以无人机在初始位置处的“北-东-地”坐标系作为基坐标系。“北-东-地”分别对应图中“X-Y-Z”坐标轴，结合GPS和机载惯导系统得到的无人机的当前位姿信息。根据无人机的当前位姿信息和基坐标系中各个图像中观测到的点与无人机的相对位置信息，即深度信息，将深度信息转换为点云数据。

利用坐标系的旋转平移变换可以将每帧的点云数据转换到基坐标系，从而构建出三维环境地图。其中的旋转矩阵如式(1)所示：

式中：θ为俯仰角/rad，为偏航角/rad，φ为横滚角/rad，s表示正弦函数sin，c表示余弦函数cos。

根据第一图像和第二图像计算得到的三维点云数据构建体素化环境地图。三维点云数据是一组离散化的数据，采用立体几何的方式描述三维电云数据，得到体素化环境地图。

在一个实施例中，典型的体素化描述过程中，每个体素立方体具备二值性，分别用1和0表示是否为障碍物，可以通过设定每个体素所代表的实际空间尺寸来调节对障碍物描述的精细程度。

在一个实施例中，采用形态学算法对二值化的体素化环境地图进行去噪。如采用“膨胀腐蚀”算法进行去噪。

如图3所示为A*算法的路径规划流程示意图。A*算法规划路径。动态给定起始点及目标节点坐标。将无人机的当前位姿信息对应的节点作为起始点a₀，在无人机航向前方d米处设置候选目标节点，判断候选目标节点是否满足环境约束条件，当满足时，将候选目标节点作为目标节点a_n。当不满足时，调整d的取值，如在(d+1)米处设置候选目标节点，重复判断过程，直到候选目标节点满足环境约束条件，得到目标节点a_n。

在确定目标节点后，判断当前节点是否满足环境约束条件，满足时，在当前节点的预设空间内查找下一个子目标节点。如在a₀的26领域内查找飞行代价最小的节点作为下一个子目标节点，判断子目标节点的是否满足环境约束条件，满足时，在下一个目标节点的26领域内查找飞行代价最小的节点作为下一个子目标节点的下一个子目标节点，重复上述环境约束条件判断过程和查找过程，直到抵达目标节点。当无人在各个节点的飞行代价f的计算方法如公式(2)所示：

f_i(x，y，z)＝g′_i(x，y，z)+h_i(x，y，z) (2)

其中，i无人机当前位姿对应的当前节点，j为当前节点的下一子目标节点，n为目标节点位置，g′_i(x，y，z)为代价调整后当前节点到下一子目标节点之间的真实代价，h_i(x，y，z)为当前节点到目标节点的启发代价。g′_i(x，y，z)＝kΔzi_j+g_i(x，y，z)，k取值可以自定义，如取80、90或100等，Δz_ij为当前节点与下一子目标节点之间在z方向的距离差，g_i(x，y，z)为代价调整前当前节点到下一子目标节点之间的真实代价，g_i(x，y，z)＝k₁(|Δx_ij|+|Δy_ij|+|Δz_ij|)，启发代价h_i(x，y，z)＝k₂Δl_in，Δx_in为当前节点到目标节点在x方向的位移差，Δy_in为当前节点到目标节点在y方向的位移差，Δz_in为当前节点到目标节点在z方向的位移差。其中k₁和k₁可以自定义，如取值为5、或10等。

利用A*算法规划过程中，给定每一个待选节点都要检验其水平方向3m、垂直方向1m邻域内是否存在障碍物体素，如果存在，则选取其它待选节点，直至规划结束，得到对应的规划路径。如旋翼无人机自身具有一定的几何尺寸，相对于A*算法使用的空间栅格地图的最小栅格0.4m*0.4m*0.4m，为确保无人机安全飞行，该尺寸大小不能忽略，需要在路径规划过程中考虑安全飞行约束。每当无人机利用A*规划算法搜索到一个扩展路径点时，都需要先判别以该点为中心的水平3m*3m、垂直1m邻域内是否存在障碍物，该参数可以根据无人机实际的“长*宽*高”几何参数进行调整。若存在，则该点不安全。

如图4所示为一个具体实施例中，无人机飞路径规划方法的流程示意图，智能决策，包括：智能决策系统基于环境感知构建的三维体素化环境地图中障碍物的形态设定不同的自主避障模式，人机从当前节点飞向目标节点，若前方空域不存在障碍物，或者障碍物都在无人机下方，则继续沿既定航向飞行，若无人机前方为典型障碍物如柱状物、锥形物和线状物等，则调用改进的A*算法进行规划路径，安全避开障碍物并回到既定航向，直至到达目标节点，若无人机前方存在大型障碍物，如墙壁等，完全遮断当前航迹，则开启左路或者右路双目相机准备向侧面飞行，若两侧均不存在安全飞行通道，则选择降落或者改为人工干预模式。以上过程中如果算法规划失败，不存在安全通道，则无人机均转入人工干预模式。

空域模式：无人机的摄像机采集到每一帧图像后，都要预先判断无人机前方是否存在障碍物。当场景中不存在障碍物或者障碍物都处于无人机下方，障碍物体素最小z值大于0.4m时，无需再调用改进的A*算法进行规划以节省时间，此时决策结果为：直接控制无人机沿当前航向直线飞行3～5m。受制于无人机的摄像机的视觉测距范围的局限，考虑到无人机具有一定飞行速度，因此设定了3～5m距离，采用3-5米是一个相对安全的位移增量。

墙壁模式：当依据前路双目摄像机感知构建的三维体素环境地图，判断前方存在墙壁类障碍物时，智能决策系统首先判断目标终点相对于无人机的位置，若在无人机左侧前方，则开启左路双目相机，反之，开启右路双目相机。当无人机在左侧寻找能够绕过墙壁的安全路径时，若左路双目相机构建的当前帧地图中可以规划出可行路径，则将其转化为飞行指令发送给无人机飞行控制系统执行。指令执行完毕后，无人机开启前路双目相机，进一步判断新位置的前方是否依旧是墙壁。若前方仍然是墙壁，则再次开启左路相机，继续在无人机左侧寻找路径点的相关操作，直到从左侧绕开墙壁，找到到达目标节点的路径。如果最终无人机左侧不存在可行通道，则开启右路双目相机，按照与左路双目相机的避障过程类似的步骤执行操作。如果无人机左右两侧均不存在躲开墙壁的可行路径，则给出降落指令，或者转入人工干预模式。

基于双目摄像机视觉的无人机自主避障方法能够通过视觉环境感知建立三维栅格地图，并根据环境中障碍物形态自主进入相应的避障模式，给出相应的避障规划结果，转化为位移控制指令发送给无人机飞控系统执行，使得无人机作业前环境数据部分未知或者整体未知情况下能够实现自主壁障，采用改进的A*算法可进行实时壁障决策，可以高效的完成避障，保证无人机的飞行。

图2为一个实施例中无人机飞行路径规划方法的流程示意图。应该理解的是，虽然图2的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

在一个实施例中，如图5所示，提供了一种无人机飞行路径规划装置200，包括：

位置信息获取模块201，用于获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息。

地图获取模块202，用于获取当前三维环境地图，当前环境三维环境地图为根据无人机的拍摄设备的当前图像进行图像重建得到的。

候选路径规划模块203，用于根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径。

飞行模块204，用于判断候选规划路径是否满足安全飞行条件，当规划路径满足安全飞行条件时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。

在一个实施例中，位置信息获取模块，包括：

候选位置信息确定单元，用于将当前位姿信息的第一预设距离的位置处的位置信息，作为候选目标位置信息。

候选节点环境约束单元，用于判断候选目标位置信息对应的候选环境信息是否满足环境约束条件。

目标位置信息确定单元，用于当候选环境信息满足环境约束条件时，将候选目标位置信息作为目标位置信息。

目标位置信息确定单元还用于当候选环境信息未满足环境约束条件时，更新候选目标位置信息，直至更新后的候选目标位置信息的候选环境信息满足环境约束条件，将更新后的候选目标位置信息作为目标位置信息。

在一个实施例中，上述无人机飞行路径规划装置，还包括：

当前节点环境预设模块，用于判断当前节点的当前环境信息是否满足环境约束条件。

候选代价计算模块，用于当当前环境信息满足环境约束条件时，计算当前节点在预设空间内的各个候选中间节点的候选代价。

子目标节点确定模块，用于从候选代价中选取满足预设代价条件的目标代价，将目标代价对应的候选中间节点作为子目标节点，其中子目标节点为当前节点与目标节点之间的中间节点。

在一个实施例中，子目标节点确定模块还用于将子目标节点作为当前节点，判断当前节点的当前环境信息是否满足环境约束条件的，直至子目标节点为目标节点。

在一个实施例中，候选代价计算模块还用于根据当前节点与各个候选中间节点的位移差，计算当前节点与各个候选中间节点的真实代价和校正代价，根据当前节点与目标节点的位移差，计算当前节点到目标节点的启发代价，对真实代价、校正代价和启发代价进行加权求和，得到各个候选中间节点对应的总代价。

子目标节点确定模块还用于从各个候选中间节点对应的总代价中选取代价最小的候选中间节点的总代价作为目标代价。

在一个实施例中，上述无人机飞行路径规划装置200，还包括：

障碍物判断模块，用于根据当前三维环境地图中的环境信息，判断在无人机的预设方向是否存在障碍物。

比值计算模块，用于当在预设方向存在障碍物时，判断当前三维环境地图中与无人机的距离小于或等于第二预设距离内的像素，与大于第二预设距离内的像素的数量比值。

飞行模块还用于当比值小于或等于预设比值时，进入根据当前三维环境地图、当前位姿信息和目标位置确定候选规划路径。

在一个实施例中，上述无人机飞行路径规划装置200，还包括：

方向确定模块，用于获取目标节点与所述无人机的相对位置，根据相对位置确定第一方向。

第一飞行模块，用于规划无人机在所述第一方向的第一路径，判断第一路径是否满足安全飞行条件，当第一路径满足安全飞行条件时，生成与第一路径对应的飞行指令。

第二飞行模块，用于当第一路径未满足所述安全飞行条件时，规划无人机在第二方向的第二路径，判断第二路径是否满足安全飞行条件，当第二路径满足安全飞行条件时，生成与第二路径对应的飞行指令。

图6示出了一个实施例中计算机设备的内部结构图。该计算机设备具体可以是图1中的终端110(或服务器120)。如图6所示，该计算机设备包括该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、输入装置和显示屏。其中，存储器包括非易失性存储介质和内存储器。该计算机设备的非易失性存储介质存储有操作系统，还可存储有计算机程序，该计算机程序被处理器执行时，可使得处理器实现无人机飞行路径规划方法。该内存储器中也可储存有计算机程序，该计算机程序被处理器执行时，可使得处理器执行无人机飞行路径规划方法。计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。

本领域技术人员可以理解，图6中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，本申请提供的无人机飞行路径规划装置可以实现为一种计算机程序的形式，计算机程序可在如图6所示的计算机设备上运行。计算机设备的存储器中可存储组成该无人机飞行路径规划装置的各个程序模块，比如，图5所示的位置信息获取模块201、地图获取模块202、候选路径规划模块203和飞行模块204。各个程序模块构成的计算机程序使得处理器执行本说明书中描述的本申请各个实施例的无人机飞行路径规划方法中的步骤。

例如，图6所示的计算机设备可以通过如图5所示的无人机飞行路径规划装置中的位置信息获取模块201执行获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息。计算机设备可通过地图获取模块202执行获取当前三维环境地图，当前环境三维环境地图为根据无人机的拍摄设备的图像进行图像重建得到的。计算机设备可通过候选路径规划模块203执行根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径。计算机设备可通过飞行模块204执行判断候选规划路径是否满足安全飞行条件，当规划路径满足安全飞行条件时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。

在一个实施例中，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现以下步骤：获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息，获取当前三维环境地图，根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径，判断候选规划路径是否满足安全飞行条件，当满足时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。

在一个实施例中，获取无人机的当前位姿信息对应的目标位置信息，包括：将当前位姿信息的第一预设距离的位置处的位置信息，作为候选目标位置信息，判断候选目标位置信息对应的候选环境信息是否满足环境约束条件，当候选环境信息满足环境约束条件时，将候选目标位置信息作为目标位置信息，当候选环境信息未满足环境约束条件时，更新候选目标位置信息，直至更新后的候选目标位置信息的候选环境信息满足环境约束条件，将更新后的候选目标位置信息作为目标位置信息。

在一个实施例中，获取无人机的当前位姿信息和对应的目标节点的位置信息之后，计算机程序被处理器执行时还实现以下步骤：判断当前节点的当前环境信息是否满足环境约束条件，当当前环境信息满足环境约束条件时，计算当前节点在预设空间内的各个候选中间节点的候选代价，从候选代价中选取满足预设代价条件的目标代价，将目标代价对应的候选中间节点作为子目标节点，其中子目标节点为当前节点与目标节点之间的中间节点。

在一个实施例中，计算机程序被处理器执行时还实现以下步骤：将子目标节点作为所述当前节点，执行判断当前节点的当前环境信息是否满足环境约束条件的步骤，直至子目标节点为所述目标节点。

在一个实施例中，计算当前节点在预设空间内的各个候选中间节点的候选代价，包括：根据当前节点与各个候选中间节点的位移差，计算当前节点与各个候选中间节点的真实代价和校正代价，根据当前节点与目标节点的位移差，计算当前节点到目标节点的启发代价，对真实代价、校正代价和启发代价进行加权求和，得到各个候选中间节点对应的总代价，从候选代价中选取满足预设代价条件的目标代价，包括：从各个候选中间节点对应的总代价中选取代价最小的候选中间节点的总代价作为目标代价。

在一个实施例中，计算机程序被处理器执行时还实现以下步骤：根据当前三维环境地图中的环境信息，判断在无人机的预设方向是否存在障碍物，当在预设方向存在障碍物时，判断当前三维环境地图中与无人机的距离小于或等于第二预设距离内的像素，与大于第二预设距离内的像素的数量比值，当比值小于或等于预设比值时，进入根据当前三维环境地图、当前位姿信息和目标位置确定候选规划路径。

在一个实施例中，当比值大于预设比值时，计算机程序被处理器执行时还实现以下步骤：获取目标节点与无人机的相对位置，根据相对位置确定第一方向，规划无人机在第一方向的第一路径，判断第一路径是否满足安全飞行条件，当第一路径满足安全飞行条件时，生成与第一路径对应的飞行指令，当第一路径未满足安全飞行条件时，规划无人机在第二方向的第二路径，判断第二路径是否满足安全飞行条件，当第二路径满足安全飞行条件时，生成与第二路径对应的飞行指令。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：获取无人机的当前节点的当前位姿信息和对应的目标节点的位置信息，获取当前三维环境地图，根据当前三维环境地图、当前位姿信息和目标节点的位置信息确定候选规划路径，判断候选规划路径是否满足安全飞行条件，当满足时，根据候选规划路径生成对应的飞行指令，执行飞行指令，以使无人机根据飞行指令飞向目标节点。

在一个实施例中，获取无人机的当前位姿信息对应的目标位置信息，包括：将当前位姿信息的第一预设距离的位置处的位置信息，作为候选目标位置信息，判断候选目标位置信息对应的候选环境信息是否满足环境约束条件，当候选环境信息满足环境约束条件时，将候选目标位置信息作为目标位置信息，当候选环境信息未满足环境约束条件时，更新候选目标位置信息，直至更新后的候选目标位置信息的候选环境信息满足环境约束条件，将更新后的候选目标位置信息作为目标位置信息。

在一个实施例中，获取无人机的当前位姿信息和对应的目标节点的位置信息之后，计算机程序被处理器执行时还实现以下步骤：判断当前节点的当前环境信息是否满足环境约束条件，当当前环境信息满足环境约束条件时，计算当前节点在预设空间内的各个候选中间节点的候选代价，从候选代价中选取满足预设代价条件的目标代价，将目标代价对应的候选中间节点作为子目标节点，其中子目标节点为当前节点与目标节点之间的中间节点。

在一个实施例中，计算机程序被处理器执行时还实现以下步骤：将子目标节点作为所述当前节点，执行判断当前节点的当前环境信息是否满足环境约束条件的步骤，直至子目标节点为所述目标节点。

在一个实施例中，计算当前节点在预设空间内的各个候选中间节点的候选代价，包括：根据当前节点与各个候选中间节点的位移差，计算当前节点与各个候选中间节点的真实代价和校正代价，根据当前节点与目标节点的位移差，计算当前节点到目标节点的启发代价，对真实代价、校正代价和启发代价进行加权求和，得到各个候选中间节点对应的总代价，从候选代价中选取满足预设代价条件的目标代价，包括：从各个候选中间节点对应的总代价中选取代价最小的候选中间节点的总代价作为目标代价。

在一个实施例中，计算机程序被处理器执行时还实现以下步骤：根据当前三维环境地图中的环境信息，判断在无人机的预设方向是否存在障碍物，当在预设方向存在障碍物时，判断当前三维环境地图中与无人机的距离小于或等于第二预设距离内的像素，与大于第二预设距离内的像素的数量比值，当比值小于或等于预设比值时，进入根据当前三维环境地图、当前位姿信息和目标位置确定候选规划路径。

在一个实施例中，当比值大于预设比值时，计算机程序被处理器执行时还实现以下步骤：获取目标节点与无人机的相对位置，根据相对位置确定第一方向，规划无人机在第一方向的第一路径，判断第一路径是否满足安全飞行条件，当第一路径满足安全飞行条件时，生成与第一路径对应的飞行指令，当第一路径未满足安全飞行条件时，规划无人机在第二方向的第二路径，判断第二路径是否满足安全飞行条件，当第二路径满足安全飞行条件时，生成与第二路径对应的飞行指令。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种无人机飞行路径规划方法，所述方法包括：

获取所述无人机的当前节点的当前位姿信息和对应的目标节点的位置信息；

获取当前三维环境地图，所述当前环境三维环境地图为根据所述无人机的拍摄设备的当前图像进行图像重建得到的；

根据所述当前三维环境地图、所述当前位姿信息和所述目标节点的位置信息确定候选规划路径；

判断所述候选规划路径是否满足安全飞行条件，当所述规划路径满足所述安全飞行条件时，根据所述候选规划路径生成对应的飞行指令，执行所述飞行指令，以使所述无人机根据所述飞行指令飞向所述目标节点。

2.根据权利要求1所述的方法，其特征在于，所述获取所述无人机的当前位姿信息对应的目标位置信息，包括：

将所述当前位姿信息的第一预设距离的位置处的位置信息，作为候选目标位置信息；

判断所述候选目标位置信息对应的候选环境信息是否满足环境约束条件；

当所述候选环境信息满足所述环境约束条件时，将所述候选目标位置信息作为所述目标位置信息；

当所述候选环境信息未满足所述环境约束条件时，更新所述候选目标位置信息，直至更新后的所述候选目标位置信息的候选环境信息满足所述环境约束条件，将所述更新后的所述候选目标位置信息作为所述目标位置信息。

3.根据权利要求1所述的方法，其特征在于，所述获取所述无人机的当前位姿信息和对应的目标节点的位置信息之后，还包括：

判断所述当前节点的当前环境信息是否满足所述环境约束条件；

当所述当前环境信息满足环境约束条件时，计算所述当前节点在预设空间内的各个候选中间节点的候选代价；

从所述候选代价中选取满足预设代价条件的目标代价，将所述目标代价对应的候选中间节点作为子目标节点，其中所述子目标节点为所述当前节点与所述目标节点之间的中间节点。

4.根据权利要求3所述的方法，其特征在于，所述方法还包括：

将所述子目标节点作为所述当前节点，进入判断所述当前节点的当前环境信息是否满足所述环境约束条件的步骤，直至所述子目标节点为所述目标节点。

5.根据权利要求3所述的方法，其特征在于，所述计算所述当前节点在预设空间内的各个候选中间节点的候选代价，包括：

根据所述当前节点与各个所述候选中间节点的位移差，计算所述当前节点与各个所述候选中间节点的真实代价和校正代价；

根据所述当前节点与所述目标节点的位移差，计算所述当前节点到所述目标节点的启发代价；

对所述真实代价、所述校正代价和所述启发代价进行加权求和，得到各个所述候选中间节点对应的总代价；

所述从所述候选代价中选取满足预设代价条件的目标代价，包括：

从各个所述候选中间节点对应的总代价中选取代价最小的候选中间节点的总代价作为所述目标代价。

6.根据权利要求1所述的方法，其特征在于，所述方法还包括：

根据所述当前三维环境地图中的环境信息，判断在所述无人机的预设方向是否存在障碍物；

当在所述预设方向存在所述障碍物时，判断所述当前三维环境地图中与所述无人机的距离小于或等于第二预设距离内的像素，与大于所述第二预设距离内的像素的数量比值；

当所述比值小于或等于预设比值时，进入根据所述当前三维环境地图、所述当前位姿信息和所述目标位置确定候选规划路径。

7.根据权利要求6所述的方法，其特征在于，当所述比值大于所述预设比值时，所述方法还包括：

获取所述目标节点与所述无人机的相对位置，根据所述相对位置确定第一方向；

规划所述无人机在所述第一方向的第一路径，判断所述第一路径是否满足所述安全飞行条件；

当所述第一路径满足所述安全飞行条件时，生成与所述第一路径对应的飞行指令；

当所述第一路径未满足所述安全飞行条件时，规划所述无人机在第二方向的第二路径，判断所述第二路径是否满足所述安全飞行条件；

当所述第二路径满足所述安全飞行条件时，生成与所述第二路径对应的飞行指令。

8.一种无人机飞行路径规划装置，其特征在于，所述装置包括：

位置信息获取模块，用于获取所述无人机的当前节点的当前位姿信息和对应的目标节点的位置信息；

地图获取模块，用于获取当前三维环境地图，所述当前环境三维环境地图为根据所述无人机的拍摄设备的图像进行图像重建得到的；

候选路径规划模块，用于根据所述当前三维环境地图、所述当前位姿信息和所述目标节点的位置信息确定候选规划路径；

飞行模块，用于判断所述候选规划路径是否满足安全飞行条件，当所述规划路径满足所述安全飞行条件时，根据所述候选规划路径生成对应的飞行指令，执行所述飞行指令，以使所述无人机根据所述飞行指令飞向所述目标节点。

9.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。