CN110068335B - 一种gps拒止环境下无人机集群实时定位方法及系统 - Google Patents

Abstract

本发明属于无人机定位技术领域，公开了一种GPS拒止环境下无人机集群实时定位方法及系统包括：无人机集群首先检测并匹配其所捕捉到的图像彼此之间的显著特征；对共性特征进行三角化，形成稀疏重构，生成所有无人机都可以访问的全局地图；无人机开始移动，每架无人机通过跟踪全局地图中的特征，进行各自的位姿估计。本发明的无人机也可以通过特征匹配和基于多视图几何的相对位姿计算，将相对测量与单个测量融合起来，提高定位的准确性；无人机集群继续飞行，当其跟踪到的特征点的数量低于某一阈值时，则无人机集群重新检测并匹配其所捕捉到的图像的特征点，更新全局地图，以避免因周围环境的变化或无人机快速运动影响定位的效果。

Description

一种GPS拒止环境下无人机集群实时定位方法及系统

技术领域

本发明属于无人机定位技术领域，尤其涉及一种GPS拒止环境下无人机集群实时定位方法及系统。

背景技术

目前，业内常用的现有技术是这样的：无人机在侦查、打击等作战任务中有着很重要的价值，随着无人机的应用场景越来越复杂，对精确定位的需求也变得至关重要。传统的无人机定位方法由全球定位系统(GPS)和惯性测量单元(IMU)组合完成，虽然这种方法实现起来简单明了，但也存在很多局限性，如GPS信号在杂乱的环境或低空飞行时，会受到障碍物的影响，且完全不适用于室内飞行。考虑到GPS/IMU融合导航的这些局限性，大量学者们开始研究如何在GPS拒止环境下实现无人机实时定位。其中主流方案是通过无人机视觉传感器采集的图像序列估计无人机的位姿从而实现定位。基于视觉的定位方案成本低，适用于没有先验环境信息的地区，且无需外部设施辅助，但是基于视觉的方案在昏暗的环境、雨雪天气、快速运动等情况下的效果并不理想甚至失效。为此，有学者提出了一些改进的视觉算法，如利用卡尔曼滤波器将基于视觉得到的位置估计值与IMU数据进行融合，实现更为准确的位置估计，且增加了算法的适用性。然而其计算量大，在单架无人机上运行耗时久，使得定位的效率低，且会导致定位有一定的时间延迟。还有学者提出了动态场景下的视觉协同定位方案，其主要步骤是：1)通过分析特征点的三角剖分一致性将每个特征点分为动态或静态；2)根据特征点的重叠部分，将无人机进行分组；3)各组无人机通过其所捕获的图像构建全局3D地图，并分别在各组内共享；4)在静态场景下，使用无人机个体位姿估计(即通过跟踪全局地图的特征点估计无人机位姿)进行定位，在动态场景下(当静态点很少或固定在某一很小的区域内时)，使用无人机相对位姿估计(通过和另一已知位置的无人机捕获的图像进行特征匹配，估计其相对位姿势)进行定位。多架无人机的协作可帮助提高定位的准确性，且允许将复杂的计算任务分配给领头的无人机或地面站。不过单纯的视觉方案其效果依赖于场景的丰富度，且快速运动时，传感器抖动以及采集图像的速率限制将会导致定位的失败。

综上所述，现有技术存在的问题是：

(1)适用性不强，现有的基于视觉定位方案对于环境要求较高，光线昏暗、场景丰富度较低(如一面白墙)、动态场景等情况下都很难精确定位，在无人机快速运动时定位甚至会失效，因此无法适用于实际应用中的复杂情形。

(2)稳定性差，传感器采集的图像模糊、图传信号不稳定都会极大的影响定位效果。

(3)定位效率低，视觉定位的计算量大，耗时久，而摄像头采集图像的速率较低，这些都会导致定位的效率低。

解决上述技术问题的难度：解决GPS拒止环境下无人机的实时定位问题需要深入了解图像特征点检测、特征点匹配、三维重建地图、基于多视图的位姿估计、卡尔曼滤波器以及IMU模型等相关知识，且需考虑到所设计的方案在实际应用中的各种情况，其具有一定的复杂性。

解决上述技术问题的意义：无人机在军事、农业、物流等领域都有着巨大的发展前景，开发智能化的无人机是未来的发展趋势，而无人机能够实现自主定位是其实现智能化的基础。解决GPS拒止环境下无人机的实时定位问题将有效的扩大无人机的应用场景。

发明内容

针对现有技术存在的问题，本发明提供了一种GPS拒止环境下无人机集群实时定位方法及系统。

本发明是这样实现的，一种GPS拒止环境下基于IMU和视觉的无人机集群实时定位方法，包含以下内容：

本发明所涉及的无人机均配备单目摄像头、视觉处理计算机以及IMU，且无人机之间可以进行通信，共享地图信息，传输和接收数据。所述的GPS拒止环境下实时定位方法具体步骤如下：

第一步，图像采集模块实时采集无人机的视觉传感器捕获的图像信息并对所获的图像信息进行预处理；

第二步，长机通过通信模块实时接收僚机的图像特征信息及位姿信息，进行特征检测、匹配以及3D重建构建周围环境的全局地图，并将得到的全局地图通过通信模块传送给僚机；

第三步，各僚机及长机通过内部定位模块跟踪全局地图的特征点，若跟踪到的特征点数量低于某一阈值，则返回上一步重新构建全局地图，否则根据追踪到的特征点并结合IMU模块的测量值估计自身的位姿，即可得到初步的定位结果；

第四步，僚机还将通过相对定位模块计算其与长机的相对位姿来修正上一步的定位结果；

第五步，输出模块实时输出修正后的定位结果。

进一步，所述第二步无人机集群首先检测并匹配其所捕捉到的图像之间的显著特征，图像特征点是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点；采用了AKAZE算法提取图像特征点；接着采用暴力匹配算法匹配图像特征点，得到匹配的特征点对。

进一步，所述第二步对匹配到的共性特征进行三角化，形成稀疏重构，生成所有无人机共享的全局地图，全局地图的构建过程如下：对所有无人机相机采集的图像进行两两的特征点匹配，选取共性特征点最多的两张图像开始初始的3D重建，生成了初始的地图，其它无人机将根据它们对初始地图特征点的跟踪逐步地被包含在这个3D重建地图中，最终得到关于周围环境的全局地图。

进一步，3D重建的过程是：根据特征匹配点对利用五点算法估计本质矩阵E，两个视图之间的相对旋转R和平移t在与本质矩阵有如下关系：

E＝[t]×R；

由上式，通过SVD分解本质矩阵得到相对位姿；再根据相对位姿，从而恢复两相机的投影矩阵，即可利用三角法及特征点重建出三维点。

进一步，所述第三步随着无人机的移动，每架无人机通过跟踪全局地图中的特征点，并结合IMU的观测值估计各自的位姿；通过基于优化的紧耦合方法融合视觉数据和IMU数据，即通过最小化重投影误差加上IMU的时间误差的和对位姿估计进行改进，其数学表达式为：

其中m_k表示k时刻无人机的位姿估计值，P是相机投影矩阵，P(X_i，m_k)表示将三维点X_i投影到相机图像平面上的位置，而x_i是k时刻图像中实际特征点位置,f(m_k-1，z_k)表示k时刻IMU的测量值为z_k得到的无人机位姿推导值，V_k表示有关IMU的运动方程的协方差矩阵；在内部定位过程中还利用了卡尔曼滤波框架对无人机状态进行预测和更新。

进一步，所述第四步协方差交集算法融合内部定位结果与相对定位结果的具体步骤是：

通过下列公式并使用相对测量数据

无人机i计算出它与长机j的相对位姿和协方差的估计值，记之为

和

然后使用协方差交集算法将这两个估计值进行融合：

进一步，所述GPS拒止环境下无人机集群实时定位方法无人机配备单目摄像头，且与其他无人机进行通信，共享地图信息，传输或接收相对位姿数据。

进一步，所述GPS拒止环境下无人机集群实时定位方法的无人机通过特征匹配和基于多视图几何的相对位姿计算，将相对测量与单个测量融合；无人机集群继续飞行，跟踪到的特征点的数量低于某一阈值时，则无人机集群重新检测并匹配其所捕捉到的图像的特征点，更新全局地图。

本发明的另一目的在于提供一种基于所述GPS拒止环境下无人机集群实时定位方法的GPS拒止环境下无人机集群实时定位系统，所述GPS拒止环境下无人机集群实时定位系统包括：

图像采集模块，用于将无人机的视觉传感器获得的图像实时传入系统中，并对所获的图像信息进行预处理；

通信模块，用于实现无人机集群间图像信号的传送和接收；

地图构建模块，用于构建可在所有无人机间共享的全局地图，用于每架无人机的内部定位；

IMU模块，用于根据IMU的测量值推导无人机的位姿并提供给内部定位模块；

内部定位模块，用于通过跟踪全局地图的特征并结合IMU模块提供的数据，从而对无人机进行定位；

相对定位模块，用于通过估计无人机间相对位姿从而修正无人机定位，提高定位精度；

输出模块，用于实时输出定位结果。

本发明的另一目的在于提供一种应用所述GPS拒止环境下无人机集群实时定位方法的无人机控制平台。

综上所述，本发明的优点及积极效果为：本发明提出的定位方法可以显著提高集群定位的精度、稳定性和适用性。

与单架无人机的定位方案相比，本发明中多架无人机的协作增加了信息量，可帮助提高定位的准确性，且允许将复杂的计算任务分配给领头的无人机或地面站，提高了定位的效率。

与纯视觉的协同定位相比，本发明融合了IMU的数据，避免了视觉方案过于依赖场景的丰富度，对环境要求较高等问题。在通讯暂时中断或光线较弱的情形下，本发明由于融合了IMU数据，因此能在短时间内继续实现自主定位。本发明提出的定位方法可以显著提高集群定位的精度、稳定性和适用性。

附图说明

图1是本发明实施例提供的GPS拒止环境下无人机集群实时定位方法流程图。

图2是本发明实施例提供的GPS拒止环境下无人机集群实时定位系统的结构示意图。

具体实施方式

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

本发明为了保证无人机集群在GPS拒止环境下能够自主定位，提出GPS拒止环境下无人机集群实时定位方法，帮助无人机集群实现高效、稳定、精确的定位。

下面结合附图对本发明的应用原理作详细的描述。

如图1所示，本发明实施例提供的GPS拒止环境下无人机集群实时定位方法包括以下步骤：

S101：图像采集模块实时采集无人机的视觉传感器捕获的图像信息并对所获的图像信息进行预处理；

S102：长机通过通信模块实时接收僚机的图像特征信息及位姿信息，进行特征检测、匹配以及3D重建从而构建周围环境的全局地图，并将得到的全局地图通过通信模块传送给僚机；

S103：各僚机及长机通过内部定位模块跟踪全局地图的特征点，若跟踪到的特征点数量低于某一阈值，则返回上一步重新构建全局地图，否则将结合IMU模块的测量值计算实时位姿；

S104：与此同时，僚机还将通过相对定位模块计算其与长机的相对位姿从而修正无人机定位，提高定位精度；

S105：输出模块实时输出修正后的定位结果。

如图2所示，本发明实施例提供的GPS拒止环境下无人机集群实时定位系统包括：图像采集模块、通信模块、地图构建模块、IMU模块、内部定位模块、相对定位模块以及输出模块组成。

图像采集模块，用于将无人机的视觉传感器获得的图像实时传入系统中，并对所获的图像信息进行预处理。

通信模块，用于实现无人机集群间图像信号的传送和接收。

地图构建模块，用于构建可在所有无人机间共享的全局地图，用于每架无人机的内部定位。

IMU模块，用于根据IMU的测量值推导无人机的位姿并提供给内部定位模块。

内部定位模块，用于通过跟踪全局地图的特征并结合IMU模块提供的数据，从而对无人机进行定位。

相对定位模块，用于通过估计无人机间相对位姿从而修正无人机定位，提高定位精度。

输出模块，用于实时输出定位结果。

本发明实施例提供的GPS拒止环境下无人机集群实时定位方法具体包括以下步骤：

(定位问题描述)设有N架无人机，其编号为m＝1，2，3......N，时间由离散时刻K＝0，1，2，......表示，每一架无人机使用单目相机采集周围环境的图像。在K＝0时刻，选择其中的一架无人机为长机，以其初始位置作为全局的笛卡尔坐标系的原点，长机在每一时刻的位置可以作为其它无人机定位的依据。本发明对无人机集群定位问题作出了如下假设：所有无人机的相机均经过校正且相机的内参已知；任意两架无人机开始的距离已知；无人机之间能够互相传输特征点和姿态数据等信息，忽视通讯中产生的延迟。

在K时刻，每一架无人机根据其单目相机采集的图像进行六自由度的姿态估计，第i架无人机在K时刻的姿态即为该无人机相机矩阵的一部分：

其中R∈SO(3)，

K表示相机的内参矩阵。通过相机矩阵估计出的无人机m在k时刻的位姿可以写成(以欧拉角度表示旋转)：

第一步，无人机集群首先检测并匹配其所捕捉到的图像之间的显著特征。图像特征点指的是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点，其反映了图像的本质特征。本发明采用了AKAZE算法提取图像特征点，与SIFT/SURF算法相比它的计算速度更快，且具有更好的准确性。接着采用暴力匹配算法匹配图像特征点，得到匹配的特征点对；

第二步，对上一步中匹配到的共性特征进行三角化，形成稀疏重构，生成所有无人机共享的全局地图。全局地图的构建过程如下：

对所有无人机相机采集的图像进行两两的特征点匹配，选取共性特征点最多的两张图像开始初始的3D重建，一旦这个重建生成了初始的地图，其它无人机将根据它们对初始地图特征点的跟踪逐步地被包含在这个3D重建地图中，最终得到关于周围环境的全局地图。

3D重建的过程是：根据特征匹配点对利用五点算法估计本质矩阵E，本质矩阵是指归一化图像坐标下的基本矩阵(对极几何中的知识，这里不做过多介绍)的特殊情况，而两个视图之间的相对旋转R和平移t在与本质矩阵有如下关系：

E＝[t]×R；

由上式，即可通过SVD分解本质矩阵得到相对位姿；再根据相对位姿，从而恢复两相机的投影矩阵，即可利用三角法及特征点重建出三维点。这里涉及的算法均是常用算法。

第三步，随着无人机的移动，每架无人机通过跟踪全局地图中的特征点，并结合IMU的观测值估计各自的位姿，此过程称之为内部定位。此处的位姿估计是一个PnP问题，其求解方法已经很成熟，不再展开说明；本发明通过基于优化的紧耦合方法融合了视觉数据和IMU数据，即通过最小化重投影误差加上IMU的时间误差的和对位姿估计进行改进，其数学表达式为：

其中m_k表示k时刻无人机的位姿估计值，P是相机投影矩阵，P(X_i，m_k)表示将三维点X_i投影到相机图像平面上的位置，而x_i是k时刻图像中实际特征点位置,f(m_k-1，z_k)表示k时刻IMU的测量值为z_k得到的无人机位姿推导值，V_k表示有关IMU的运动方程的协方差矩阵；本发明在内部定位过程中还利用了卡尔曼滤波框架对无人机状态进行预测和更新。

第四步，僚机通过与长机的图像进行特征点匹配，基于多视图几何估计出其与长机的相对位姿，再采用协方差交集算法将相对位姿与第三步中估计的无人机位姿融合，从而修正第三步中的内部定位结果，此过程称之为相对定位。此过程中相对位姿的估计方法与步骤二中相对位姿估计方法是一致的。所谓的协方差交集算法融合内部定位结果与相对定位结果的具体步骤是：

通过下列公式并使用相对测量数据

无人机i计算出它与长机j的相对位姿和协方差的估计值(记之为

和

)：

与此同时，假设无人机j已经执行了MAV内部定位步骤，因此会有一个状态协方差阵存在。然后使用协方差交集算法将这两个估计值进行融合，如下所示：

第五步，当无人机集群跟踪到全局地图的特征点的数量低于某一阈值时，则无人机集群重新检测并匹配其所捕捉到的图像的特征点，更新全局地图。更新全局地图的过程和步骤二中构建全局地图的过程是一样的。

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

Claims (5)

1.一种GPS拒止环境下无人机集群实时定位方法，其特征在于，所述GPS拒止环境下无人机集群实时定位方法包括：

第一步，图像采集模块实时采集无人机的视觉传感器捕获的图像信息并对所获的图像信息进行预处理；

第二步，长机通过通信模块实时接收僚机的图像特征信息及位姿信息，进行特征检测、匹配以及3D重建构建周围环境的全局地图，并将得到的全局地图通过通信模块传送给僚机；

第三步，各僚机及长机通过内部定位模块跟踪全局地图的特征点，若跟踪到的特征点数量低于某一阈值，则返回上一步重新构建全局地图，否则根据追踪到的特征点并结合IMU模块的测量值估计自身的位姿，即可得到初步的定位结果；

第四步，僚机还将通过相对定位模块计算其与长机的相对位姿修正上一步的定位结果；

第五步，输出模块实时输出修正后的定位结果；

所述第一步无人机集群首先检测并匹配其所捕捉到的图像之间的显著特征，图像特征点是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点；采用了AKAZE算法提取图像特征点；接着采用暴力匹配算法匹配图像特征点，得到匹配的特征点对；

所述第二步对匹配到的共性特征进行三角化，形成稀疏重构，生成所有无人机共享的全局地图，全局地图的构建过程如下：对所有无人机相机采集的图像进行两两的特征点匹配，选取共性特征点最多的两张图像开始初始的3D重建，生成了初始的地图，其它无人机将根据它们对初始地图特征点的跟踪逐步地被包含在这个3D重建地图中，最终得到关于周围环境的全局地图；

3D重建的过程是：根据特征匹配点对利用五点算法估计本质矩阵E，两个视图之间的相对旋转R和平移t在与本质矩阵有如下关系：

E＝[t]×R；

由上式，通过SVD分解本质矩阵得到相对位姿；再根据相对位姿，从而恢复两相机的投影矩阵，即可利用三角法及特征点重建出三维点；

所述第三步随着无人机的移动，每架无人机通过跟踪全局地图中的特征点，并结合IMU的观测值估计各自的位姿；通过基于优化的紧耦合方法融合视觉数据和IMU数据，即通过最小化重投影误差加上IMU的时间误差的和对位姿估计进行改进，其数学表达式为：

其中m_k表示k时刻无人机的位姿估计值，P是相机投影矩阵，P(X_i，m_k)表示将三维点x_i投影到相机图像平面上的位置，而x_i是k时刻图像中实际特征点位置，f(m_k-1，z_k)表示k时刻IMU的测量值为z_k得到的无人机位姿推导值，V_k表示有关IMU的运动方程的协方差矩阵；在内部定位过程中还利用了卡尔曼滤波框架对无人机状态进行预测和更新；

所述第四步协方差交集算法融合内部定位结果与相对定位结果的具体步骤是：

通过下列公式并使用相对测量数据

无人机i计算出它与长机j的相对位姿和协方差的估计值，记之为

和

然后使用协方差交集算法将这两个估计值进行融合：

2.如权利要求1所述的GPS拒止环境下无人机集群实时定位方法，其特征在于，所述GPS拒止环境下无人机集群实时定位方法无人机配备单目摄像头，且与其他无人机进行通信，共享地图信息，传输或接收相对位姿数据。

3.如权利要求1所述的GPS拒止环境下无人机集群实时定位方法，其特征在于，所述GPS拒止环境下无人机集群实时定位方法的无人机通过特征匹配和基于多视图几何的相对位姿计算，将相对测量与单个测量融合；无人机集群继续飞行，跟踪到的特征点的数量低于某一阈值时，则无人机集群重新检测并匹配其所捕捉到的图像的特征点，更新全局地图。

4.一种基于权利要求1所述GPS拒止环境下无人机集群实时定位方法的GPS拒止环境下无人机集群实时定位系统，其特征在于，所述GPS拒止环境下无人机集群实时定位系统包括：

图像采集模块，用于将无人机的视觉传感器获得的图像实时传入系统中，并对所获的图像信息进行预处理；

通信模块，用于实现无人机集群间图像信号的传送和接收；

地图构建模块，用于构建可在所有无人机间共享的全局地图，用于每架无人机的内部定位；

IMU模块，用于根据IMU的测量值推导无人机的位姿并提供给内部定位模块；

内部定位模块，用于通过跟踪全局地图的特征并结合IMU模块提供的数据，从而对无人机进行定位；

相对定位模块，用于通过估计无人机间相对位姿从而修正无人机定位，提高定位精度；

输出模块，用于实时输出定位结果。

5.一种应用权利要求1～2任意一项所述GPS拒止环境下无人机集群实时定位方法的无人机控制平台。

Images