基于激光雷达与人工势场法的无人机编队飞行避障方法与流程

本发明属于多无人机协同定位领域，特别涉及基于激光雷达与人工势场法的无人机编队飞行避障方法。

背景技术：

无人机具有效率高、成本低、可重复使用的特点，在工业、商业应用中的需求越来越强烈，应用场景也越来越广泛，而无人机单机自身的性能是有限的，采用固定翼无人机编队可以增加无人机载荷，能够使固定翼无人机应对复杂任务，提高任务执行的可靠性与容错率。实现固定翼无人机编队飞行的一个关键因素在于固定翼无人机的避障飞行问题，在飞行过程中，精准的定位信息可以精确指导固定翼无人机编队飞行，避免固定翼无人机之间发生碰撞，提高任务完成的成功率。固定翼无人机避障飞行可应用于固定翼无人机编队自主飞行任务，具有十分重要的应用价值，因此固定翼无人机协同定位在上世纪八十年代末就成为学术界和应用部门的研究热点。

传统的固定翼无人机避障方式通常通过全球定位系统gps或者单目视觉定位采集位置数据，通过gps数据解析获得当前的固定翼无人机定位信息，然而gps定位精度低，更新频率慢，在强电场信号、密集楼宇建筑环境中易受干扰，会导致测量不准确；单目视觉测量误差大，不适用于高速飞行过程中的固定翼无人机避障。在固定翼无人机飞行过程中由于机体震动使得测量传感器产生量测噪声，也会导致固定翼无人机定位数据出现测量误差。

综上所述，为了使固定翼无人机编队具有更强的飞行避障能力，需要发展一种更加精准、可靠的固定翼无人机定位及避障方法。

技术实现要素：

针对现有技术中固定翼无人机编队避障能力差的问题，本发明提供了一种基于激光雷达与人工势场法的无人机编队飞行避障方法，可以在无人机编队执行飞行任务时，对编队内部无人机和外部障碍物方位进行精准定位，具有高精度和高可靠性等优势。

本发明采用如下技术方案来实现：

基于激光雷达与人工势场法的无人机编队飞行避障方法，包括以下步骤：

步骤1、编队中各个无人机分别获取各自与障碍物的欧氏距离；

步骤2、根据各个无人机与障碍物点的欧氏距离，确定障碍物点与无人机编队的最小距离；

步骤3、根据障碍物点与无人机编队的最小距离，采用人工势场法控制无人机编队的飞行路径，使其避开障碍物。

优选的，步骤1中，无人机通过激光雷达探测获取障碍物的三维点云，并计算无人机至三维点云数据中每一个点的欧氏距离。

优选的，所述欧式距离的计算方法如下：

其中，k为障碍物三维点云数据中某一个点的编号，j为无人机编号，xrj为x方向上编号j号无人机的位置，xk，yk，zk分别为k点的坐标。

优选的，步骤2中确定障碍物点与无人机编队的最小距离的具体方法如下：

所述无人机将各自得到的欧式距离发送给编队中的长机或地面站，由地面站或长机采用快速排序法，对接收到所有欧式距离进行排序，得到最小的欧式距离，该最小的欧氏距离为无人机编队至障碍物的最小距离。

优选的，步骤3中采用人工势场法控制无人机编队飞行路径的方法如下：

1)根据人工势场的势能函数结合无人机编队至障碍物的最小距离，得到吸引力和排斥力；

2)根据吸引力和排斥力，确定吸引力和排斥力的合力；

3)根据合力的大小与方向，控制无人机编队的飞行路径，使无人机编队避开障碍物运动至目标点。

优选的，步骤1)中所述的势能函数如下：

uart(x)＝ugoal(x)+uobs(x)

ugoal(x)＝1/2kp(x-xd)²

其中，ugoal和uobs分别表示为虚拟引力场和虚拟斥力场，x为无人机与目标点的最小欧式距离，xd为常数，kp和n分别为引力增益系数与斥力增益系数，p0为斥力场影响范围的极限距离，p为无人机编队与障碍物的最小距离。

优选的，步骤1)中所述的吸引力fgoal和排斥力fobs的公式如下：

fgoal(x)＝-grad[ugoal(x)]＝-kp(x-xd)

优选的，，步骤2)中合力的公式如下：

本发明具有如下有益的技术效果：

本发明提供的基于激光雷达与人工势场法的无人机编队飞行避障方法，通过扫描障碍物的点云数据，确定障碍物与无人机的欧氏距离，并对所有的欧式距离进行排序，得到无人机编队至障碍物的最小距离，根据最小距离结合人工势场法计算吸引力和排斥力的合力，根据合力的大小确定无人机编队的飞行路径，使其避开障碍物飞行至目标点，从而高效完成飞行任务。

进一步，采用无人机内部的小型激光雷达作为测量传感器，具有定位结果精度高、可靠性高等优势；

进一步地，对小型激光雷达测量数据使用快速排序算法计算出障碍物与无人机编队的最小距离，该方法方便可靠，能够满足无人机编队在飞行中的实时计算要求。

进一步地，采用人工势场法，根据障碍物与无人机的位置信息，能够控制无人机编队进行有效避障，从而高效完成飞行任务。

附图说明

图1是本发明无人机编队飞行避障方法流程图；

图2是本发明无人机编队外部避障方法的原理图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细说明。

参照图1，一种基于激光雷达与人工势场法的无人机编队飞行避障方法，通过设置在编队内部各无人机中的机载多传感器系统、飞行控制计算机、飞行任务计算机来实现的。其中，机载多传感器系统包括小型激光雷达和差分定位系统，飞行任务计算机包括激光雷达点云数据处理器、数传系统和任务管理器。

参照图1和图2，固定翼无人机编队外部避障包括如下步骤：

步骤1：无人机编队中的每个无人机，通过激光雷达探测无人机编队外部的障碍物，确定障碍物点与各自的欧氏距离。

具体为，无人机通过各自内置的激光雷达探测障碍物，激光雷达探测的结果为三维点云，一个障碍物会产生多个点云数据，记为ck＝(xk，yk，zk)，其中(xk，yk，zk)为点的位置坐标，激光雷达点云数据处理器根据计算无人机至每一个点的欧式距离，该障碍物与固定翼无人机的欧氏距离为：

其中，k为障碍物三维点云数据中某一个点的编号，j为无人机编号，xrj为x方向上编号j号无人机的位置。

步骤2：根据各个无人机获取的欧氏距离，确定无人机编队与障碍物的最小距离。

具体为，各个无人机将各自所获取的欧氏距离发送给无人机编队的长机或地面站，长机或地面站对接收到的所有欧氏距离，采用快速排序法进行排序，以获得外部障碍物与无人机编队的最小距离，快速排序法的具体过程为：

根据所有障碍物点与各无人机的欧氏距离，得到距离数组{d11，d12，d1j，...，dkj，...}，随机选取距离数组中某一元素dab为基准元素，从前向后遍历数组，当遇到小于基准元素的元素时，把它放置在基准元素左侧，当遇到大于基准元素的元素时，它放置在基准元素右侧。

然后，将所有小于基准元素的元素作为一个新的距离数组，再次随机选取新的距离数组中某一元素为基准元素，再次遍历整个新的距离数组，将小于基准元素的放置在左侧，将大于基准元素的放置在右侧。

根据上述方法进行迭代，直至得到最小的欧氏距离，即为无人机编队与障碍物之间的最小距离dmin，无人机编队与障碍物最小距离的点的坐标为pobs＝(xo，yo，zo)。

步骤3：根据最小距离并结合人工势场法，控制无人机编队的飞行路径使其避开障碍物。

具体为，设定无人机编队的目标点为pgoal＝(xg，yg，zg)，无人机当前所处位置为pirtk(t)＝(xri，yri，zri)。

根据目标点和无人机的位置确定无人机与目标点的欧式距离。

人工势场中目标点对无人机编队产生牵引力，吸引无人机编队向目标点方向运动，障碍物对无人机编队产生斥力，使得无人机朝着障碍物反方向运动。将虚拟引力场和虚拟斥力场分别用ugoal和uobs表示，对引力场和斥力场分别求负梯度计算出无人机所受的引力fgoal与斥力fobs的大小和方向，则无人机在引力和斥力的合力fart作用下运动至目标点。

人工势场的势能函数如下：

uart(x)＝ugoal(x)+uobs(x)

ugoal(x)＝1/2kp(x-xd)²

其中，x为无人机与目标点的欧式距离，xd为常数，取值范围为2～8，kp和n分别为引力增益系数与斥力增益系数，取值范围为80～150；p0为斥力场影响范围的极限距离，取值范围为20～40，p为无人机编队与障碍物的最小距离。

基于上述势能函数，得到吸引力和排斥力为：

fgoal(x)＝-grad[ugoal(x)]＝-kp(x-xd)

根据吸引力和排斥力得到合力fart为：

各固定翼无人机中的飞行控制计算机根据所得合力的大小与方向，给出飞行指令，改变无人机飞行姿态与航线，使得无人机编队绕开障碍物，抵达目标区域。

相比于传统固定翼无人机编队飞行避障方法，本发明采用小型激光雷达和差分定位系统作为测量传感器，具有定位结果精度高、可靠性高等优势。

进一步地，对小型激光雷达测量数据使用快速排序算法计算出障碍物与固定翼无人机编队的最小距离，该方法方便可靠，能够满足固定翼无人机编队在飞行中的实时计算要求。

进一步地，采用人工势场法，根据障碍物与固定翼无人机的位置信息，能够控制固定翼无人机编队进行有效避障，从而高效完成飞行任务。

由上述内容可知，本发明建立了一种基于激光雷达与人工势场法的固定翼无人机编队飞行避障方法，通过差分定位系统获取各固定翼无人机的精确位置信息，避免无人机编队内部发生碰撞，通过小型激光雷达获得固定翼无人机编队外部障碍物的位置信息，然后利用人工势场法控制固定翼无人机编队避开障碍物，从而高效完成飞行任务。