基于准均匀样条曲线的无人机路径修正方法及系统与流程

本发明涉及通用航空管制领域技术，特别涉及一种基于准均匀样条曲线的无人机路径修正方法及系统。

背景技术：

随着各种无人机的普及和小型化，有关无人机管制技术的研究也变得越来越多，研究方向也越来越细致。无人机楼宇穿行除了障碍物不规则、陡变的特征外，还有一个显著的特征就是密集排列。当需要规划的路径非常长的时候，路径也有很大的概率会变得很复杂，如果路径是由控制点所控制的曲线，那远远不是3、5个控制点所能解决的；而密集的楼宇还有一个问题，就是如果某一区域的路径规划没问题的话，我们并不希望它改变，因为改变会引发不确定性。也就是说，当一个区域的路径与障碍冲突需要修正的时候，其它区域的路径能保持不变。而bezier曲线、ph曲线都是全局曲线，改变一处，整条曲线都会跟着改变，这种特性对于复杂的长路径规划和密集排列的楼宇障碍非常不利。

对于无人机的路径规划问题，除了考虑避障，还要考虑的是无人机的跟随问题。如果规划的路径，无人机无法按照路径飞行，那这样的路径规划出来其效果也会打折扣。通常，无人机的跟随性和无人机本身的物理特性有关，不同的无人机跟随性也会不同。但它们基本上的飞行路径都是曲率连续(c2连续)的，所以，对无人机规划出的路径也必须是曲率连续的。

因此，亟待研发一种既能够满足飞行路径全段c2曲率连续性，整条b样条曲线中出现直线段并不会影响到无人机的跟随性，且不需要修正的曲线段尽可能多地保持原样的无人机路径修正方法及系统。

技术实现要素：

有鉴于此，有必要提供一种既能够满足飞行路径全段c2曲率连续性，整条b样条曲线中出现直线段并不会影响到无人机的跟随性，且不需要修正的曲线段尽可能多地保持原样的无人机路径修正方法及系统。

一种基于准均匀样条曲线的无人机路径修正方法，所述基于准均匀样条曲线的无人机路径修正方法包括如下步骤：

s1、建立动态障碍物模型，规划一条连接起点和终点的直线段路径；

s2、以直线段之间的交点为管制点，直线段路径为控制多边形，绘制准均匀三次b样条曲线；

s3、找出与障碍物相交的曲线段以及相交曲线段所对应直线段，在对应直线段上添加控制点，调整曲线使其通过该控制点；

s4、更新管制点组和曲线，并根据无人机的曲率约束和安全约束微调管制点。

一种基于准均匀样条曲线的无人机路径修正系统，所述基于准均匀样条曲线的无人机路径修正系统包括如下功能模块：

初级线路规划模块，用于建立动态障碍物模型，规划一条连接起点和终点的直线段路径；

b样条曲线绘制模块，用于以直线段之间的交点为管制点，直线段路径为控制多边形，绘制准均匀三次b样条曲线；

曲线调整模块，用于找出与障碍物相交的曲线段以及相交曲线段的四个管制点，根据顺序连接四个管制点的形状，对曲线进行相应调整；

线路更新模块，用于更新管制点组和曲线，并根据无人机的曲率约束和安全约束微调管制点。

本发明提供一种基于准均匀样条曲线的无人机路径修正方法及系统，其基于控制点越多，曲线的局部特性越突出的原理，并结合b样条曲线的特性，本发明采用增加控制点的办法来修正b样条曲线，使曲线逐渐靠近由控制点构成的多边形，从而得到一条能够精确避开障碍物的，既满足了飞行路径全段c2曲率连续性，又使不需要修正的曲线段尽可能多地保持原样的无人机飞行路径。

附图说明

图1是本发明所述的基于准均匀样条曲线的无人机路径修正方法的步骤流程图；

图2是本发明所述的基于准均匀样条曲线的无人机路径修正方法流程图；

图3是图1中步骤s1的子流程图；

图4是图1中步骤s3的子流程图；

图5为模型中用vorinoi图法和a*搜索算法产生直线段路径的示意图；

图6为模型中初始的b样条曲线的示意图；

图7为在线段中间插入单个控制点的路径调整示意图；

图8为在线段中间插入两个控制点的路径调整示意图；

图9为移动中间控制点的路径调整示意图；

图10为计算插入控制点的曲率和安全距离的示意图；

图11为两条相信曲线段与同一个障碍相交的示意图；

图12为在相领两条控制线段上添加两个控制点的示意图。

具体实施方式

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

如图1所示，本发明实施例提供一种基于准均匀样条曲线的无人机路径修正方法，如图2所示，所述基于准均匀样条曲线的无人机路径修正方法包括如下步骤：

s1、建立动态障碍物模型，规划一条连接起点和终点的直线段路径。

具体如图3所示，所述步骤s1包括以下分步骤：

s11、绘制初始路径周围的障碍，每一个障碍用矩形表示；

s12、用voronoi图法产生连接图，如图5所示；

s13、结合无人机具体执行任务的需求，通过a*搜索算法得到一条连接起点和终点的直线段路径，如图5(b)所示；

s2、以直线段之间的交点为管制点，直线段路径为控制多边形，绘制准均匀三次b样条曲线，如图6所示。

具体的，在得到准均匀三次b样条曲线后，需要对管制点进行微调，使其满足无人机的曲率约束和安全约束。

所述三次b样条曲线是由任意数量的曲线段组成的完全的分段三次多项式。系数变化引起某一区间上的三次片段变化，并从一个区间到另一个区间逐个进行。b样条曲线不通过其控制点。

对于任意一个b样条片段，其公式为：

其中qi为第i个b样条片段，pi是按控制点顺序排列的一个四个点的集合。

在这样的表示形式下，一条b样条曲线是m-2个曲线段的系列。为方便起见，将其标记为q3、q4、q5、……、qm，这些曲线段由m+1个控制点p0、p1、……、pm来定义的，其同m≥3。每一个曲线段都是由四个控制点来定义的，而每一个控制点会影响连续的四条且只有这四条曲线段。这就是b样条曲线的局部控制特性。

准均匀b样条曲线是非均匀b样条曲线的一种特殊形式，它通过在两端插入结点，而在其它地方采用均匀的结点。这样就使起点和终点的连续结点值区间减小到零，从而使b样条曲线与两个端点相交。经常会使用的结点序列为[0,0,0,0,1,2,…,n-2,n-1,n,n,n,n]。

b样条曲线表现出位置连续性(切线连续性(c1)和曲率连续性(c2))、局部性、凸包性、保凸性、对称性、递推性、几何及仿射不变性等性能。其中，连续性是最重要的特征，且曲线上的控制点可以任意移动而不影响连续性；而局部性也是很重要的特征，以三次曲线为例，每一个曲线段都是由四个控制点来定义的，而每一个控制点会影响连续的四条且只有这四条曲线段。

由于所述准均匀三次b样条曲线仅与起点和终点相交，与其它控制点不相交，所以尽管所有的控制点和控制多边形都能避开障碍物，但b样条曲线本身却未必能避开。因此，规划出来的初始路径难免会有与障碍物相交的情况出现，因此，需要进行步骤s3，进行相应的修正措施。

s3、找出与障碍物相交的曲线段以及相交曲线段所对应直线段，在对应直线段上添加控制点，调整曲线使其通过该控制点。

具体的，分段的样条曲线具有如下特性：首先，它是曲线连续的；第二，理论上样条曲线的控制点可以无限增加而不影响它的计算复杂度，因此样条曲线的形式可以很复杂，适合长距离的路径规划；第三，样条曲线是由若干条曲线段构成的，因此它是局部性的，改变一个控制点，只会改变与该控制点有关的相邻几条曲线段，而其它的曲线段可以保持不变，这种性质很适合密集楼宇的路径规划。

b样条曲线具有如下特性：当三个控制点在同一条直线上时，那么b样条曲线就经过中点的那个控制点；而当四个控制点在同一条直线上时，那么b样条曲线在中间两个控制点之前是一条直线段，对于无人机来说，其采纳路径优先度为直线>c2曲线>c1曲线>c0曲线，只要满足全段c2曲率连续性，整条b样条曲线中出现直线段并不会影响到无人机的跟随性。

结合分段的样条曲线以及b样条曲线的特性，所述步骤s3的具体修正措施如下。

如图4所示，所述步骤s3包括以下分步骤：

s31、找出与障碍物相交的曲线段，以及相交曲线段所对应直线段的四个管制点，

s32、根据顺序连接四个管制点的形状，确定在对应直线段上添加控制点以及添加控制点的数量；

s33、根据控制点对曲线进行调整，使曲线通过添加的控制点。

其中，步骤s32包括下列情况；

情况一：如果四个管制点的连接形状为四边形，则在对应直线段上添加一个控制点，使曲线通过该控制点；如果四个管制点的连接形状为四边形，且在对应直线段上添加一个控制点之后仍无法避开障碍，则取消该控制点，并在对应直线段上添加两个控制点，将修正的曲线的中间段变成直线段，使曲线通过该直线段。

如图7(a)所示，假设四个管制点分别为p0、p1、p2、p3，如果p0、p1、p2、p3之间的连线构成四边形，根据b样条曲线凸包特性，在p1和p2之间的线段上添加一个控制点p12，曲线段将变成如图7(b)所示，则曲线避开障碍物，满足无人机路径规划要求。

如果进行上述调整后还不能避开障碍物，如图8(a)所示，则表示两个障碍物之间通道很窄，对于这种情况，以直线通过这个通道则更安全。因此，取消p12，在p1和p2之间添加两个控制点p12和p21，将修正的曲线的中间段变成直线，如图8(b)所示，避开障碍物，满足无人机路径规划要求。

情况二：如果四个管制点的连接形状为闪电形，则将位于四个管制点中间的两个管制点中的其中一个管制点向相邻两个管制点的方向进行调整，使闪电形调整为四边形，之后再根据四边形的规则进行调整。

如图9(a)所示，如果p0、p1、p2、p3之间的连线构成闪电形，即线段p0p3和p1p2相交，则首先要将闪电形调整为四边形，即将p1向线段p0p2之间移动，或p2向p1p3之间移动，本实施例中是将p2向p1p3之间移动；之后在p1和p2之间的线段上添加一个控制点p12，在p2和p3之间的线段上添加一个控制点p23，使曲线段将变成如图9(b)所示，则曲线避开障碍物，满足无人机路径规划要求。

上述路径规划中，如图10所示，最佳控制点的求取步骤如下：

1)在对应直线段上选择多个点，分别以每一个点为添加控制点来修正路径；

2)判断每一条修正路径是否还与障碍物相交，剔除相交的路径，保留不相交路径所对应的控制点；

3)根据控制点的曲率和安全距离的权值，计算出每一个控制点修正后曲线的最大曲率和最短安全距离，代入到下列公式中：

式中，k为修正曲线在p12处的曲率；l为p12到障碍物最短安全距离；k越小越能保障无人机的通行能力，而l越大越能保障无人机的飞行安全，w1和w2分别为k和的权值，不同的使用者可以设定不同的权值，当无人机对最大曲率的要求严格时，可提高w1的比重；当无人机大小比较大时，则可提高w2的比重。

选择的最小值，其对应的控制点即是最佳添加控制点。

步骤s32还包括情况三；

情况三：如果四个管制点的连接形状为四边形，且同一个障碍物与连续两个曲线段相交，则取两条对应直线段的四个管制点的合集得到五个管制点，在两条对应直线段上分别添加一个控制点，使曲线同时通过这两个控制点。

在某种极端情况下，可能会出现同一个障碍物与连续两个曲线段相交的情况，如图11所示，那么令这两个曲线段的管制点分别为p0、p1、p2、p3和p1、p2、p3、p4，可以认为两个曲线段的管制点为p0、p1、p2、p3、p4五个点。这种极端情况很大的可能性是该障碍物与它周围的障碍物之间的距离较近，因此控制多边形离该障碍物较近所致(按voronoi法，控制多边形不会与障碍相交)，如图9所示。

出现这样的情况时，用移动管制点的方法会十分受限，因此还是采用添加控制点的方法，在线段p1p2和p2p3上分别添加两个控制点p12和p23，则修正后的路径如图12所示。

情况三中，所述两条对应直线段上添加的两个控制点组成一个控制点组，其中最佳的控制点组的求取步骤如下：

1)在两条对应直线段上分别选择m和n个点构成m*n个点组，分别以每一个点组为添加控制点组来修正路径；

2)判断每一条修正路径是否还与障碍物相交，剔除相交的路径，保留不相交路径所对应的控制点组；

3)根据控制点的曲率和安全距离的权值，计算出每一控制点组修正后曲线的最大曲率和最短安全距离，代入到下列公式中：

选择的最小值，其对应的控制点即是最佳添加控制点组。

s4、更新管制点组和曲线，并根据无人机的曲率约束和安全约束微调管制点。

根据上述基于准均匀样条曲线的无人机路径修正方法，本发明还提供一种基于准均匀样条曲线的无人机路径修正系统，所述基于准均匀样条曲线的无人机路径修正系统包括如下功能模块：

初级线路规划模块，用于建立动态障碍物模型，规划一条连接起点和终点的直线段路径；

b样条曲线绘制模块，用于以直线段之间的交点为管制点，直线段路径为控制多边形，绘制准均匀三次b样条曲线；

曲线调整模块，用于找出与障碍物相交的曲线段以及相交曲线段的四个管制点，根据顺序连接四个管制点的形状，对曲线进行相应调整；

线路更新模块，用于更新管制点组和曲线，并根据无人机的曲率约束和安全约束微调管制点。

其中，所述初级线路规划模块包括如下功能单元：

模型建立单元，用于绘制初始路径周围的障碍，每一个障碍用矩形表示；

图形生成单元，用于用voronoi图法产生连接图；

路径计算单元，用于结合无人机具体执行任务的需求，通过a*搜索算法得到一条连接起点和终点的直线段路径。

其中，所述曲线调整模块包括如下功能单元：

管制点提取单元、用于找出与障碍物相交的曲线段，以及相交曲线段所对应直线段的四个管制点，

控制点确定单元、用于根据顺序连接四个管制点的形状，确定在对应直线段上添加控制点以及添加控制点的数量；

路径调整单元、用于根据控制点对曲线进行调整，使曲线通过添加的控制点。

本发明提供了一种基于准均匀样条曲线的无人机路径修正方法及系统，其基于控制点越多，曲线的局部特性越突出的原理，并结合b样条曲线的特性，本发明采用增加控制点的办法来修正b样条曲线，使曲线逐渐靠近由控制点构成的多边形，从而得到一条能够精确避开障碍物的，既满足了飞行路径全段c2曲率连续性，又使不需要修正的曲线段尽可能多地保持原样的无人机飞行路径。

本发明应用在低空无人机飞跃山地和城市楼宇的情况下，能够具有较高的避障效率，能够提升能充分提高无人机飞行的效能，能够规划出复杂的路径而不显著增加运算量。因此，可广泛应用于无人机路径规划的实际任务应用。

以上装置实施例与方法实施例是一一对应的，装置实施例简略之处，参见方法实施例即可。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能性一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应超过本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机储存器、内存、只读存储器、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd-rom、或技术领域内所公知的任意其他形式的存储介质中。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。