基于激光雷达与定位向量匹配的多无人机协同定位方法与流程

本发明属于多无人机协同定位领域，特别涉及一种基于激光雷达与定位向量匹配的无人机编队飞行协同定位方法。

背景技术：

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

传统的无人机编队定位方式通常通过全球定位系统gps采集位置数据，通过gps数据解析获得当前的固定翼无人机定位信息，然而gps定位精度低，更新频率慢，定位数据容易产生漂移，且易受外接电磁信号的干扰。无人机编队飞行需要机群内部无人机能够提供高精度定位信息从而控制各无人机稳定飞行，只采用gps单一传感器获取的粗略定位信息不能满足多无人机编队飞行对高精度定位的需求。为了获取高精度的定位信息，可以使用激光雷达测量无人机之间的相对位置，但是激光雷达所测量的位置信息不包含所测无人机的编号信息，从而无法实现多无人机协同定位应用。

综上所述，为使多无人机编队具有更强的协同定位能力，需要发展一种适用于无人机编队飞行的高精度协同定位方法。

技术实现要素：

针对现有技术中无人机编队机群内部无人机定位精度低，激光雷达传感器无法获取所测无人机编号的问题，本发明提供了一种基于激光雷达与定位向量匹配的多无人机协同定位技术，可以在无人机编队执行飞行任务时，融合gps和激光雷达传感器的定位数据，提供各编号无人机的高精度定位信息，满足无人机编队飞行对协同定位的要求。

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

基于激光雷达与定位向量匹配的多无人机协同定位方法，包括以下步骤：

步骤1、采用激光雷达测量机群内无人机之间的定位信息，得到激光雷达定位向量；

步骤2：采用gps测得机群内各个无人机的大地坐标信息，并使各个无人机实时获得机群内其它无人机的gps定位信息；

步骤3：根据gps定位信息计算无人机的gps定位向量；

步骤4：对激光雷达定位向量和gps定位向量进行匹配，得到机群内部各无人机的协同定位信息。

优选的，所述激光雷达定位向量为aij＝(xij,yij,zij)；

其中，i为无人机的编号，j为第j组激光雷达定位向量。

优选的，步骤2中，建立无人机编队组网通信系统，通过无人机编队组网通信系统广播各无人机gps定位信息，使得各个无人机获得机群内其它无人机的gps定位信息。

优选的，步骤3中，所述gps定位向量为gij＝(xj-xi,yj-yi,zj-zi)，gij表示j号无人机到i号无人机的gps定位向量。

优选的，步骤4中，使用定位向量匹配算法得到带有编号信息的多无人机协同定位信息。

优选的，所述定位向量匹配方法通过激光雷达定位向量和gps定位向量之间的模值、角度和符号的比较，当激光雷达定位向量和gps定位向量满足匹配条件时，得到该激光雷达定位向量所对应的无人机编号，进而得到无人机之间协同定位信息。

优选的，所述模值匹配方法具体如下：

计算当前无人机的激光雷达定位向量和gps定位向量的模值之差d，当d的值小于预设参数δ1时，激光雷达定位向量和gps定位向量满足模值匹配条件，计算公式如下：

优选的，所述角度值匹配方法具体如下：

计算定位向量在三个平面上投影的角度值之差，当角度之差的绝对值小于预设参数δ2时，激光雷达定位向量和gps定位向量满足角度匹配条件，计算公式如下：

在x-y平面上的投影

在x-z平面上的投影

在y-z平面上的投影

当α+β+γ<δ2时，两向量满足角度匹配。

优选的，所述符号匹配方法通过判定激光雷达定位向量与gps定位向量是否同号；

当xij*(xj-xi)>0，yij*(yj-yi)>0，zij*(zj-zi)>0同时成立，满足两向量符号匹配条件。

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

本发明提供的基于激光雷达与定位向量匹配的多无人机协同定位方法，通过定位向量匹配算法，使用激光雷达测量无人机之间高精度的相对位置，以及使用gps获取各无人机带有编号信息的低精度定位信息，将激光雷达定位信息和gps定位信息融合，通过激光雷达定位向量和gps定位向量匹配，解算出激光雷达所测无人机的编号，得到高精度的无人机协同定位信息，解决了激光雷达无法测得编队机群内部无人机编号的问题，计算出机群内部各无人机之间高精度的协同定位数据，满足无人机编队飞行对协同定位精度的要求。

进一步的，建立无人机编队机间组网通信系统，使得各无人机实时接收机群内部其他无人机的定位信息；

进一步的，提出定位向量匹配算法，将满足匹配算法要求的激光雷达定位向量和gps定位向量所匹配，得到激光雷达所测无人机对应的编号信息。

附图说明

图1是定位向量匹配方法原理图；

图2是基于激光雷达定位向量匹配方法的流程图。

具体实施方式

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

参照图1和图2，基于激光雷达与定位向量匹配的多无人机协同定位方法包括以下步骤：

步骤1：无人机编队机群内部各无人机搭载激光雷达飞行，无人机编队机群共有n架无人机，对应无人机编号为1至n，无人机的激光雷达获取机群内其它无人机与其的相对位置信息，需要说明的是，激光雷达只能得到其它无人机的相对位置信息，不能获得相对位置信息对应的无人机的编号。

具体的，机群内部编号为i的无人机使用激光雷达测得m个周围无人机相对自身的位置数据，记为di(ai1,ai2,...,aim)，n架无人机共可得到n组测量数据，记为(d1,d2,...,dn)。

具体的，aij＝(xij,yij,zij)是i号无人机使用激光雷达测得机群内部某无人机得到的第j组定位坐标向量，表示所测到的无人机距离i号无人机相对位置的定位向量。

具体的，以三架无人机编队为例，无人机编号为无人机1号、2号、3号，1号无人机可以测得另外两架无人机相对自身的位置数据，记为d1(a11,a12)，3架无人机共可得到3组测量数据，记为(d1,d2,d3)。

步骤2:gps测得机群内部n架无人机各自大地坐标系的大地坐标信息，记为(xk,yk,zk)，k＝1,2,3...,n为对应无人机编号。

进一步的，建立无人机编队机间组网通信系统，机群内部各无人机将当前gps定位信息通过机间组网通信系统广播，各架无人机实时获得机群内其他无人机的gps定位数据。

优选的，机间组网通信系统由小型数传模块组成，数传模块工作频率433mhz,无人机基于mavlink协议进行数据通信。

具体的，以三架无人机编队为例，gps可以测得3组无人机定位信息，记为(x1,y1,z1)，(x2,y2,z2)，(x3,y3,z3)，分别表示1、2、3号无人机gps定位信息。

步骤3：计算各无人机的gps定位向量组，通过机群内其他无人机gps定位坐标减去i号无人机定位坐标，得到gps定位向量组，记为gi(gi1,gi2,...,gin)。

进一步的，gij＝(xj-xi,yj-yi,zj-zi)，表示j号无人机到i号无人机的gps定位向量。

步骤4：gps定位信息的位置偏差为r，无人机实际位置即在以预设位置为圆心，位置偏差r为半径的圆内，由此可以计算无人机在大地坐标系下的相对位置向量(由搭载激光雷达无人机的位置指向被激光雷达探测到无人机的位置)；另一方面，激光雷达可以测得无人机之间的相对位置，得到激光雷达定位向量，通过两个向量之间模值、角度、方向的比较，选择出最为匹配的一对向量，即可知该激光雷达定位向量所对应的无人机编号，定位向量匹配方法使用如下步骤进行检测。

优选的，r,δ1和δ2为常数，是算法预设参数，取值如下：

r＝5,δ1＝1.5,δ2＝0.4

步骤4a：模值匹配，计算当前无人机的激光雷达定位向量和gps定位向量的模值之差d，设定算法预设参数δ1，当d的值小于δ1时，激光雷达定位向量和gps定位向量满足模值匹配条件。

步骤4b：角度值匹配，计算当前无人机的激光雷达定位向量和gps定位向量在x-y平面、x-z平面、y-z平面上投影的角度值之差，设定算法预设参数δ2，当角度之差的绝对值小于δ2时，激光雷达定位向量和gps定位向量满足角度匹配条件。

在x-y平面上的投影

在x-z平面上的投影

在y-z平面上的投影

当α+β+γ<δ2时，两向量满足角度匹配。

步骤4c：符号同值匹配，安装时候设置激光雷达大地坐标系轴角方向相同，计算激光雷达定位向量与gps定位向量是否同号，即：xij*(xj-xi)>0，yij*(yj-yi)>0，zij*(zj-zi)>0同时成立，则满足两向量符号同值匹配。

步骤4d：当激光雷达定位向量和gps定位向量同时满足模值匹配、角度值匹配和符号同值匹配，则该激光雷达定位向量和gps定位向量匹配，激光雷达所测无人机编号为gps定位向量对应的机群内部另一无人机编号，使用激光雷达定位信息表示满足匹配要求的两无人机协同定位结果，将满足匹配要求的定位结果通过机间组网通信系统广播，各无人机实时接收协同定位信息。

重复步骤4，遍历所有激光雷达定位向量和gps定位向量，完成所有无人机定位向量匹配，激光雷达所测无人机编号为gps定位向量对应的机群内部另一无人机编号，两无人机之间协同定位信息由激光雷达定位数据表示，得到整个机群n架无人机之间的协同定位信息。

具体的，对于三架无人机编队，通过以上步骤可以得到1号无人机与2号无人机，1号无人机与3号无人机，2号无人机与3号无人机之间的精确定位信息，作为协同定位方法的输出结果。

由上述内容可知，本发明建立了一种基于激光雷达与定位向量匹配的多无人机协同定位方法，通过定位向量匹配算法，融合激光雷达定位信息和gps定位信息，解决了激光雷达无法测得编队机群内部无人机编号的问题，计算出机群内部各无人机之间高精度的协同定位数据，满足无人机编队飞行对协同定位精度的要求。