CN107632616B

CN107632616B - 一种基于三维空间曲线的无人机协同路径规划方法

Info

Publication number: CN107632616B
Application number: CN201710789366.5A
Authority: CN
Inventors: 高亮; 邬嘉扬; 李新宇; 易进
Original assignee: Huazhong University of Science and Technology
Current assignee: Huazhong University of Science and Technology
Priority date: 2017-09-05
Filing date: 2017-09-05
Publication date: 2019-08-09
Anticipated expiration: 2037-09-05
Also published as: CN107632616A

Abstract

本发明属于无人机控制相关技术领域，并公开了一种基于三维空间曲线的无人机协同路径规划方法，包括：对无人机运动参数执行初始化，并构建飞行地理模型；生成多条三维空间曲线作为可选的飞行路径，并获得任一时刻所对应的位置点三维坐标及速度矢量，然后基于毕达哥拉夫条件等式来筛选合适的三维空间曲线；设定五阶曲线的控制点，并基于这些控制点构建算法来获取最优化飞行路径，由此完成整体的路径规划过程。通过本发明，不仅可获得更高精度和更高效率的三维空间规划路径，而且可有效避开各种飞行过程中的障碍物，同时具备无需再次光滑处理规划路径、满足无人机运动学约束等特点。

Description

一种基于三维空间曲线的无人机协同路径规划方法

技术领域

本发明涉及无人机控制相关技术领域，更具体地，涉及一种基于三维空间曲线的无人机协同路径规划方法。

背景技术

无人机(Unmanned Aerial Vehicle)是指利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机。与载人飞机相比，无人机具有灵活性高、成本低、安全性高以及隐蔽性强等特点，这些特有的优越性能使无人机技术快速发展，逐渐成为世界前沿科技的代表性技术，被广泛应用于各种军事侦察、城市快递、地形勘探、环境监测等领域。

无人机路径规划的目的在于满足无人机及环境的约束条件下，寻找初始位置和目标位置之间的最优或接近最优的路径。作为无人机自主飞行的核心技术之一，无人机路径规划方案根据无人机对环境地图掌握程度不同，可以分为全局路径规划和局部路径规划等基本类型。

现有技术中已经提出了一些用于无人机的路径规划方法。例如，CN104808688A公开了一种无人机曲率连续可调路径规划方法，并包括以下步骤：首先执行路径曲率连续化，利用参数化Catmull-Rom曲线连接路标点，通过优化插值方法保证连接点处曲率连续；接着计算路径曲线各点曲率值，查找并标注曲率值超过规定阈值的起点和终点；最后采用最小圆曲率转移法，利用曲率值单调变化的Bezier曲线连接起点和终点。又如，CN103697896A公开了一种无人机路径规划方法，并包括以下步骤：首先，初始化任务地图，标注起点、终点和障碍物的坐标，计算出无人机的最小安全转向半径；接着设定起终点为节点，查找其他节点坐标，以结构点建立二叉树；接着，以深度优先左值遍历方法读取二叉树，依次记录节点顺序，将相邻节点间的连线及采用了Dijkstra算法生成的路径存入矩阵之中。

然而，进一步的研究表明，上述现有无人机路径规划方法仍然具备以下的缺陷或不足：首先，这类规划方式通常是针对二维平面路径来进行规划的，在实际运用时往往不能适应无人机在复杂三维空间中的运动控制；其次，现有技术中的路径规划方法很少针对障碍物的规避问题进行研究，相应导致无法精确避开飞行过程中的障碍物的问题；最后，现有算法还存在路径规划效率偏低、操控复杂、运算量大等情况。相应地，本领域亟需作出进一步的完善，以便更好地符合更高标准的无人机路径规划要求。

发明内容

针对现有技术的以上缺陷或不足，本发明的目的在于提供一种基于三维空间曲线的无人机协同路径规划方法，其中通过结合无人机自主飞行的自身特点和应用需求，对其规划算法和关键工艺步骤作出针对性设计，相应不仅可获得更高精度和更高效率的三维空间规划路径，而且可有效避开各种飞行过程中的障碍物，同时具备无需再次光滑处理规划鼓励、可满足无人机运动学约束等特点，因而尤其适用于各类复杂飞行环境的应用场合。

为解决上述目的，按照本发明，提供了一种基于三维空间曲线的无人机协同路径规划方法，其特征在于，该方法包括下列步骤：

(1)针对作为规划对象的无人机，对其相关的多个运动参数进行初始化，包括无人机起点的三维空间坐标、无人机终点三维空间坐标，飞行曲率和飞行安全间距等，同时构建反映无人机飞行环境特征的无人机飞行地理模型，其中包括该飞行环境中各类障碍物的三维空间坐标；

(2)利用步骤(1)所获得的飞行地理模型、无人机起点及终点三维空间坐标，生成相应的多条三维空间曲线作为可选的飞行路径，同时针对各个飞行路径也即各条三维空间曲线获得任一时刻t所对应的位置点的三维坐标(x_m(t),y_m(t),z_m(t)),以及各位置点所对应的速度矢量(x'_m(t),y'_m(t),z'_m(t)),其中m表示对各个飞行路径依次赋予的路径序号且m＝1,2,3,…；接着，判断各条三维空间曲线上的各位置点所对应的速度矢量是否满足毕达哥拉夫条件等式，并且进一步删除其中不满足该条件等式的三维空间曲线，同时保留满足该条件等式的其余三维空间曲线；

(3)针对步骤(2)所保留的其余三维空间曲线，继续根据以下表达式组来设定五阶曲线的控制点及其对应的三维坐标值，其具体过程如下：

其中，p₀-p₅分别表示所设定的6个控制点，并且它们各自具有对应的三维坐标值；A₀-A₂依次表示根据需要所预设的四元数元素多项式，依次表示所述A₀-A₂各自所对应的共轭四元数，i则表示纯向量四元数；

(4)基于步骤(3)所设定的五阶控制点，相应构建如下表达式组所示且呈现为三维空间曲线形式的最优化飞行路径r(t)，由此完成整体的无人机协同路径规划过程：

其中，p_k表示上述各个控制点各自所对应的三维坐标值，并且k值依次赋值为0-5；n表示所设定的所有控制点的总数量。

作为进一步优选地，在步骤(1)中，优选采用三维插值法来构建反映无人机飞行环境特征的无人机飞行地理模型。

作为进一步优选地，在步骤(3)中，优选采用和声搜索算法来预设所需的四元数等式。

作为进一步优选地，在步骤(4)中，优选积分方法来计算得出无人机飞行路径的长度。

作为进一步优选地，在步骤(4)中，优选还可针对所述四元数执行角变量调整，以便更好执行对障碍物的规避。

总体而言，按照本发明的以上技术构思与现有技术相比，通过结合复杂空间环境的无人机避障飞行的具体需求进行综合考虑，并针对性提出采用毕达哥拉夫空间曲线进行三维无人机路径的规划，同时采用四元数法和插值运算，相应不仅可高效确定路径形状，避免对飞行路径的二次光滑，而且还可以通过调整系数来各控制点之间的矢量长度，相应在有效满足无人机飞行曲率的情况下更好地实现规避障碍物的效果，因而尤其适用于各类复杂地理环境的无人机飞行及避障应用场合。

附图说明

图1为按照本发明所构建的基于三维空间曲线的无人机协同路径规划方法的整体流程示意图；

图2是用于示范性现实按照本发明某应用实例而生成的环境任务地图；

图3是按照本发明一个优选实施例、利用和声搜索算法执行的迭代搜索的流程示意图；

图4是结合图2所示的应用实例，按照本发明所获得路径优化结果示意图；

图5是结合图2所示的应用实例，按照本发明对无人机飞行曲率进行判别的示意图；

图6是结合图2所示的应用实例，按照本发明对无人机距离约束进行判别的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

如图1所示，按照本发明所构建的无人机协同路径规划方法主要包括以下步骤：

首先，针对作为规划对象的无人机，对其相关的多个运动参数进行初始化，包括无人机起点的三维空间坐标、无人机终点三维空间坐标，飞行曲率和飞行安全间距等，同时构建如图2所示地用于反映无人机飞行环境特征的无人机飞行地理模型，其中包括该飞行环境中各类障碍物的三维空间坐标；

接着，利用以上步骤所获得的飞行地理模型、无人机起点及终点三维空间坐标，生成相应的多条三维空间曲线作为可选的飞行路径，同时针对各个飞行路径也即各条三维空间曲线获得任一时刻t所对应的位置点的三维坐标(x_m(t),y_m(t),z_m(t)),以及各位置点所对应的速度矢量(x'_m(t),y'_m(t),z'_m(t)),其中m表示对各个飞行路径依次赋予的路径序号且m＝1,2,3,…；在此基础上，作为本发明的关键改进之一，可判断各条三维空间曲线上的各位置点所对应的速度矢量是否满足毕达哥拉夫条件等式，譬如可构造多项式等式并将多项式表示成伯恩斯坦形式，然后进一步删除其中不满足该条件等式的三维空间曲线，同时保留满足该条件等式的其余三维空间曲线；

接着，作为本发明的另一关键改进，可针对以上步骤所保留的其余三维空间曲线，继续根据以下表达式组来设定五阶曲线的控制点及其对应的三维坐标值，其具体过程如下：

其中，p₀-p₅分别表示所设定的6个控制点，并且它们各自具有对应的三维坐标值；A₀-A₂依次表示根据需要所预设的四元数元素多项式，依次表示所述A₀-A₂各自所对应的共轭四元数，i则表示纯向量四元数。换而言之，也即在本发明中将空间曲线定义为含有空间拐点的五阶曲线进行安全规避，并譬如可采用四元数法来计算路径控制点的位置参数。可参见图3，在此过程中优选可利用和声搜索算法来进行反复的迭代搜索，由此寻找最优或接近最优的角变量，进而找到最优的位置参数。和声搜索算法的原理和操作过程属于本领域技术人员所熟知的技术，因此在此不再赘述。

最后，基于以上步骤所设定的五阶控制点，相应构建如下表达式组所示且呈现为三维空间曲线形式的最优化飞行路径r(t)，由此完成整体的无人机协同路径规划过程：

下面将结合一个具体实例来进一步解释说明按照本发明的无人机协同路径规划方法。

在本实例中，假设两架执行任务的无人机的起始点坐标分别为(10,10,20)，(10,25,20)；终止点坐标分别为(90,80,600)，(90,95,600)；起始点方向角均为终止点方向角均为环境地图为图2所示，其中山峰状建筑代表障碍物，圆柱状建筑表示禁飞区域，禁飞区高度为300。

参看图4，图中的左侧曲线和右侧曲线分别为计算得出的无人机飞行路径，这两条路径均避开了环境中的障碍物，其中左侧曲线表示的路径从禁飞区飞过，但是路径在禁飞区上空的最低高度为412.8，高于禁飞区高度，因此满足要求。进一步参看图5，两路径曲率均不大于约束曲率极值0.3，故路径满足无人机飞行曲率要求，且路径平滑不需要进行再光滑处理。

进一步参看图6所示，通过计算两无人机在飞行时的距离可知，两无人机之间的距离始终大于最小安全距离(5km)，因此无人机相互之间不会出现干扰、碰撞的情况。

综上，本发明中通过针对性选择毕达哥拉夫空间曲线进行三维无人机路径的规划，同时采用四元数法和插值运算，相应不仅可高效确定路径形状，避免对飞行路径的二次光滑处理，而且还可以通过调整系数来各控制点之间的矢量长度，相应在有效满足无人机飞行曲率的情况下更好地实现规避障碍物的效果，因而尤其适用于各类复杂地理环境的无人机飞行及避障应用场合。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims

1.一种基于三维空间曲线的无人机协同路径规划方法，其特征在于，该方法包括下列步骤：

(1)针对作为规划对象的无人机，对其相关的多个运动参数进行初始化，包括无人机起点的三维空间坐标、无人机终点三维空间坐标，飞行曲率和飞行安全间距，同时构建反映无人机飞行环境特征的无人机飞行地理模型，其中包括该飞行环境中各类障碍物的三维空间坐标；

其中，p₀-p₅分别表示所设定的6个控制点，并且它们各自具有对应的三维坐标值；A₀-A₂依次表示根据需要所预设的四元数多项式，依次表示所述A₀-A₂各自所对应的共轭四元数，i则表示纯向量四元数；

p_k＝(x_k,y_k,z_k),(k＝0,1,2,…,n)，

2.如权利要求1所述的方法，其特征在于，在步骤(1)中，采用三维插值法来构建反映无人机飞行环境特征的无人机飞行地理模型。

3.如权利要求1或2所述的方法，其特征在于，在步骤(3)中，采用和声搜索算法来预设所需的四元数多项式。

4.如权利要求1或2所述的方法，其特征在于，在步骤(4)中，采用积分方法来计算得出无人机飞行路径的长度。

5.如权利要求1或2所述的方法，其特征在于，在步骤(4)中，还针对所述四元数多项式执行角变量调整，以便更好执行对障碍物的规避。