CN114216454B - 一种gps拒止环境下基于异源图像匹配的无人机自主导航定位方法 - Google Patents

Abstract

本发明涉及一种GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，包括如下步骤：S1、读取无人机上相机、IMU、气压高度计输出的数据，从相机中读取视频流；S2、读取当前帧的地面正射影像，解算出无人机自身经纬度；S3、读取连续两帧地面正射影像；S3、得到无人机任一时刻的速度；S4、得到无人机在地球坐标系下的三维位置信息；S5、根据IMU测量信息进行惯性导航，读取加速度计的加速度值解算出无人机的位置、速度；S6、将步骤S4解算出的无人机三维位置信息、S3解算出的无人机实时速度和S5解算出的无人机位置姿态信息进行扩展卡尔曼滤波融合EKF，得到融合后的高频无人机位置、速度、姿态信息。解决了GPS拒止环境下无人机无法自主导航定位的问题。

Description

一种GPS拒止环境下基于异源图像匹配的无人机自主导航定 位方法

技术领域

本发明属于自主导航技术领域，更具体地，涉及一种GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法。

背景技术

目前无人机的导航方式主要依赖惯性导航和GPS。惯性导航不依赖外部信息、不易受电磁干扰，但是由于导航信息经过积分产生所以定位误差会随时间而增大，长时间飞行时精度较差；GPS导航精度较高，但在遮挡比较严重的野外或者是处于强电磁干扰下，GPS的精度、可用性、连续性和完好性都受到了严重威胁，以至于无人机在GPS拒止环境下无法自主导航定位，导致迫降或坠毁。所以需要一种GPS拒止环境下的高精度无人机自主导航定位方法。

发明内容

针对无人机在GPS拒止环境下无法自主导航定位的问题，以及单一惯性导航中长时间飞行定位精度差的问题，本发明提出了一种GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，包括如下步骤：

S1、读取无人机上相机、IMU、气压高度计输出的数据，调整相机俯仰角使其保持对地面的正射，从相机中读取的是无人机飞行过程中地面正射影像的视频流，从IMU中读取的是无人机自身的角速度和加速度，从气压高度计读取的是飞行高度。

S2、事先根据无人机飞行任务，从网上获取其飞行区域的高分辨率遥感卫星图。读取当前帧的地面正射影像，将其与整片区域的卫星图做异源图像匹配，解算出无人机自身经纬度。

S3、读取连续两帧地面正射影像，利用金字塔LK光流算法得到两帧间的位移，除以时间得到两帧间的速度，由于帧率较快，所以连续两帧间的平均速度可认为是此时刻的瞬时速度。在相机拍摄影像的过程中一直重复步骤S3，直到相机停止拍摄，这样可以得到无人机任一时刻的速度。

S4、计算在步骤S2解算经纬度花费的时间内无人机飞过的位移量，将位移量和S2解算出的无人机经纬度相加得到无人机实时经纬度，读取高度计信息，得到无人机在地球坐标系下的三维位置信息。

S5、根据IMU测量信息进行惯性导航，读取陀螺仪的角速度信息利用四元数法解算出无人机的姿态矩阵读取加速度计的加速度值通过积分解算出无人机的位置、速度。

S6、将步骤S4解算出的无人机三维位置信息、S3解算出的无人机实时速度和S5解算出的无人机位置、速度、姿态信息进行扩展卡尔曼滤波融合EKF，得到融合后的高频无人机位置、速度、姿态信息。

进一步地，步骤S2中对当前帧的地面正射影像与整片区域的卫星图做异源图像匹配，解算出无人机自身经纬度的过程如下：

S21、根据无人机飞行任务，从网上获取其飞行区域的高分辨率遥感卫星图，卫星图上每一个像素都带有准确的经纬度信息，作为图像匹配的卫星底图使用；

S22、无人机搭载可见光相机，在飞行区域内进行地毯式搜索，若发现目标则控制无人机飞至目标中心正上方，调整相机俯仰角使其保持对目标的正射，拍摄目标正射影像；

S23、处理航拍影像，首先缩放航拍图使其空间分辨率与卫星图的空间分辨率保持一致，然后按照航向角旋转航拍图使图像方向保持正北，此时航拍图中所有物体的朝向与卫星图一致；

S24、在卫星底图上进行滑动窗口搜索，窗口大小设置为处理后的航拍图大小，重叠率设置为60％及以上，得到若干底图块，在所有底图块中搜索与航拍图最接近的底图块；

S25、根据航拍图与最接近的底图块经过异源图像匹配后得到的匹配对，利用OpenCV中的findHomography模块计算两幅图的单应性矩阵M，M将航拍图中心点映射到卫星底图上，卫星底图的每一点都带有经纬度信息，由此得到航拍图中心点亦是目标中心的经纬度。

进一步地，步骤S24中具体搜索策略为：将航拍图与任一底图块进行异源图像匹配，每次匹配都会得到两幅图像间的若干匹配对，取匹配对数量最多的底图块即为最接近的底图块；

所述航拍图与任一底图块的异源图像匹配流程为：

S241.使用深度学习模型D2-Net对航拍图进行特征提取、使用D2-Net对所有底图块逐一进行特征提取；

S242.使用K近邻搜索算法将航拍图与任一底图块的特征进行粗匹配得到若干匹配对；

S243.使用动态自适应约束条件提纯匹配对；

S244.使用RANSAC算法进一步剔除误匹配对；

S245.最终取匹配对最多的底图块即为最接近的底图块。

进一步地，所述步骤S242中粗匹配的具体过程为：使用K近邻算法将两张图片的特征向量进行粗匹配，令K＝2，得到N个匹配对，这些匹配对每一个都包含欧氏距离最近的第1匹配点dis_j和次近的第2匹配点dis′_j。

进一步地，所述步骤S243中提纯匹配对的具体过程为：用动态自适应欧氏距离约束条件对匹配对进行提纯，统计所有匹配对中第1匹配点和第2匹配点距离差的均值：

对于每一个待筛选匹配对,提纯的条件为第1距离小于第2距离与距离差均值avgdis之差,公式为：

dis_j＜dis′_j-avgdis

dis_j表示距离最近的第1匹配点的距离值，dis′_j表示次近的第二匹配点的距离值，不满足此公式的匹配对删去，留下满足公式的匹配对。

进一步地，所述步骤S244中剔除误匹配对的具体过程为：

S2441.从提纯后的匹配对中随机抽出若干对匹配对样本，用这若干对匹配对样本拟合一个模型P；

S2442.计算其余匹配对与模型P的误差，若误差小于阈值则认为是局内点，大于阈值则认为是局外点；

S2443.上述过程被称为一次迭代，迭代r次后局内点数量最多的某次结果即为最终的结果，此时计算出的所有局外点即为误匹配对，直接剔除即可。

进一步地，步骤S3中使用金字塔LK光流算法解算无人机实时速度的过程为：

S31、将连续两帧影像转换为灰度图

S32、将两帧影像按照航向角(也叫偏航角)进行旋转，使图像坐标系的横向、纵向与地球坐标系的经纬度方向重合；

S33、在前一帧中使用OpenCV模块中的Shi-Tomasi角点检测函数提取特征点。

S34、采用金字塔LK算法检测出前一帧提取出的特征点在后一帧中的位置并确定坐标。

S35、将两帧的特征点坐标相减，得到地球坐标系下经度和纬度方向上的像素位移Flow_lon、Flow_lat，将像素位移与图像的空间分辨率相乘，得到了以米为单位的两帧图像间的位移Flow_lon_m、Flow_lat_m

S36、如公式(1)、(2)所示，将两帧图像间的位移除以采样时间，得到了两帧间的平均速度，由于采样时间较短，平均速度可以认为是此时的瞬时速度

Vel_lon_m＝Flow_lon_m/(t2-t1) (1)

Vel_lat_m＝Flow_lat_m/(t2-t1) (2)

式(1)(2)中，Vel_lon_m表示经度方向上的速度，Vel_lat_m表示纬度方向上的速度，t2-t1表示两帧图像间的时间差。

进一步地，步骤S4中解算无人机实时位置的过程为：

若步骤S2解算经纬度需要t秒，在这t秒内无人机继续飞行，t秒内相机共有n帧正射影像，步骤S3已经解算出这t秒内每两帧间的位移，累加n-1个位移即为t秒内总位移Δs，按照公式(3)、(4)计算当前时刻无人机的经纬度(lon_now,lat_now)

lon_now＝lon_pre+Δs_lon*degree_lon (3)

lat_now＝lat_pre+Δs_lat*degree_lat (4)

式中，(lon_pre,lat_pre)表示步骤S2解算出的t秒前航拍图与卫星图匹配的无人机经纬度，(Δs_lon,Δs_lat)表示这t秒内经度、纬度方向上的总位移(以米为单位)，degree_lon表示经度方向上1米实际度，degree_lat表示纬度方向上1米实际度。

读取此时的高度计数据h。无人机在地球坐标系下的三维位置即为(lon_now，lat_now，h)。

进一步地，步骤S5中根据IMU测量信息进行无人机位姿信息解算的方法为：

IMU中加速度计测得的加速度值是在载体坐标系下的，步骤S4解算出的无人机位置是在地球坐标下的，为了后续的融合，需要将加速度值转变为地球坐标系，转换公式如(5)。对加速度V_en在采样周期t内进行积分得到速度，再对速度在采样周期t内进行积分得到位移。

f_b为加速度计测量值，为四元数法解算出的载体坐标系到地球坐标系的姿态矩阵，/>表示地球引力加速度矢量。

进一步地，步骤S6中将S4解算出的无人机三维位置信息、S3解算出的无人机实时速度和S5解算出的无人机位置、速度、姿态信息进行扩展卡尔曼滤波融合EKF的过程为：

在无人机飞行的信息融合定位过程中首先建立两个子模型，以步骤S5IMU解算的位姿信息建立***模型，以步骤S4解算出的无人机三维信息建立观测模型；

S61、IMU***模型可描述为

x(k)＝f(x_k-1，u_k-1)+W_k-1 (6)

式(6)中，x＝[s_ix s_iy s_iz v_ix v_iy v_iz]^T表示无人机在k时刻位置s和速度v的6维***状态向量，x、y、z表示地球坐标系的三轴方向，f表示在k-1时刻与状态向量x_k-1，***控制输入量u_k-1有关的非线性函数，W_k-1表示在k-1时刻***高斯白噪声，呈正态分布。

***模型的状态向量预测方程为：

式(7)中，表示在k时刻由步骤S5解算得到无人机位置和速度的先验估计值，/>表示在k-1时刻无人机状态向量输出的最优估计值，/>分别表示先验估计姿态角，Δa表示k-1时刻到k时刻的加速度差。

***状态向量先验估计误差协方差矩阵计算公式如下

式(8)(9)中，表示在k-1时刻无人机最优估计误差协方差矩阵，_A表示函数f的雅克比矩阵，Q_k-1表示在k-1时刻***噪声协方差矩阵。式(9)中x＝x_k-1是x＝x_k-1。

S62、观测模型可描述为

z(k)＝h(x_k)+V_k (10)

式中，z＝(lon_now，lat_now，h，Vel_lon_m，Vel_lat_m，vel_h)，z是6维观测向量，(lon_now，lat_now，h)是步骤S4解算出的无人机三维位置信息，Vel_lon_m，Vel_lat_m是步骤S3光流法计算出的无人机实时速度，vel_h是高度方向上的速度，通过高度计差值除以时间间隔得到。h表示k时刻关于***状态向量x_k的非线性函数，h的雅克比矩阵为H_k，V_k表示在k时刻的观测噪声，呈高斯状态分布，其协方差矩阵为R_k。

计算卡尔曼增益K：

最后更新***状态向量与误差协方差矩阵：

P_k＝(I-K_kH_k)P_k ^- (13)

S63、由于IMU的更新频率大于图像匹配的更新频率，所以当观测模型中没有新的图像匹配经纬度到来时，跳过S62的观测更新步骤，仅使用S61的惯性导航方式进行导航。最终得到了EKF融合后的高频无人机位置、速度、姿态数据。

总体而言，通过本发明所构思的以上技术方案能够取得下列有益效果：

本发明的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，无人机上搭载相机、惯性测量单元IMU、气压高度计三种传感器，相机用于拍摄连续的地面正射影像；IMU主要包括陀螺仪和加速度计，陀螺仪用于测量无人机的角速度，解算其姿态信息；加速度计用于测量无人机的加速度，气压高度计用于测量无人机的飞行高度。然后，基于异源图像匹配算法和卫星底图解算出无人机经纬度，基于LK光流算法解算出无人机实时速度和位移，可代替GPS；与惯性测量单元IMU数据融合后得到了高频的无人机导航数据，解决了GPS拒止环境下无人机无法自主导航定位的问题。基于扩展卡尔曼滤波融合EKF算法将惯组信息和上述经纬度、速度信息融合得到高频无人机位姿信息；又因为数据融合后图像匹配解算出的经纬度和光流法解算出的速度能以一定频率纠正惯导的累积误差，所以本发明还解决了单一惯性导航中长时间飞行定位精度差的问题。

附图说明

图1为本发明一种GPS拒止环境的无人机自主导航定位方法的原理图；

图2为航拍图在整个区域的完整卫星底图上匹配定位的流程图；

图3为本发明基于异源图像匹配的无人机地面目标定位方法的原理流程图；(为航拍图与任一底图块异源图像匹配算法原理图；)

图4为匹配对最少的航拍图和底图块；

图5为匹配对最多的航拍图和底图块；

图6为两帧影像按照航向角(也叫偏航角)进行旋转示意图；

图7为角点检测的示例图，图中黑点为检测出的角点。

图中，1-航拍图，11-航拍图1(与航拍图为同一幅图，方向不同)，2-卫星底图块，21-底图块1，22-底图块2，3-线条的两端点互为匹配对。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施方式，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施方式仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，无人机搭载相机、惯性测量单元IMU、气压高度计三种传感器，相机用于拍摄连续的地面正射影像；IMU主要包括陀螺仪和加速度计，陀螺仪用于测量无人机的角速度，推算其姿态变化；加速度计用于测量无人机的加速度，气压高度计用于测量无人机的飞行高度。

本发明的一种GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于，包括如下步骤：

S1、读取无人机上相机、IMU、气压高度计输出的数据，调整相机俯仰角使其保持对地面的正射，从相机中读取的是无人机飞行过程中地面正射影像的视频流，从IMU中读取的是无人机自身的角速度和加速度，从气压高度计读取的是飞行高度。

S2、事先根据无人机飞行任务，从网上获取其飞行区域的高分辨率遥感卫星图。读取当前帧的地面正射影像，将其与卫星图做异源图像匹配，得到一个单应性矩阵M可将当前帧的航拍图上任一点映射到卫星图上，从而得到该点的经纬度，由于航拍图为正射，可认为航拍图中心点的经纬度即为无人机自身经纬度。如图2所示，航拍图在整个区域的完整卫星底图上进行匹配定位得到无人机自身经纬度的具体过程为：

S21、根据无人机飞行任务，从网上获取其飞行区域的高分辨率遥感卫星图，卫星图上每一个像素都带有准确的经纬度信息，作为图像匹配的卫星底图使用；遥感地图有1-18级，来源于谷歌地图、百度地图等地图软件，选择最高级别18级的卫星底图，空间分辨率0.5米以上；

S22、无人机搭载可见光相机，在飞行区域内进行地毯式搜索，若发现目标则控制无人机飞至目标中心正上方，调整相机俯仰角使其保持对目标的正射，拍摄目标正射影像；

S23、处理航拍影像，首先缩放航拍图使其空间分辨率与卫星图的空间分辨率保持一致，然后按照航向角旋转航拍图使图像方向保持正北，此时航拍图中所有物体的朝向与卫星图一致；

S24、在卫星底图上进行滑动窗口搜索，窗口大小设置为处理后的航拍图大小，重叠率设置为60％及以上，得到若干底图块，在所有底图块中搜索与航拍图最接近的底图块；

步骤S24中具体搜索策略为：将航拍图与任一底图块进行异源图像匹配，每次匹配都会得到两幅图像间的若干匹配对，取匹配对数量最多的底图块即为最接近的底图块；由于卫星底图涉及的地面范围远大于航拍图像的地面范围，且处理后的航拍图尺寸往往远小于卫星图，二者直接匹配误差较大。滑动窗口法使航拍图每次只需与同等地面范围的底图块做匹配，有效提高了匹配的准确性。

所述航拍图与任一底图块的异源图像匹配流程为：

S241.使用深度学习模型D2-Net对航拍图进行特征提取、使用D2-Net对所有底图块逐一进行特征提取；

详细地，本专利中特征提取方法使用深度学习模型D2-Net。传统的特征提取方法是先检测关键点(keypointdetection)再对关键点进行描述子提取(featuredescription)，也就是detect-then-describe模式。最终一副图像可得到若干个关键点及其对应的描述子，也就是关键点及其对应的n维特征向量。常用的SIFT、SURF算法都属于先检测再描述一类。传统方法提取出的特征虽然具有尺度不变性，但面对光照、季节、波段差别较大的异源图像时其表达能力有限，D2-Net是一种学习型关键点检测和描述算法，它通过训练一个CNN网络能够端到端地同时提取关键点和描述子，这种同时提取关键点和描述子的方法称为describe-and-detect。D2-Net首先使用CNN计算输入图像的特征图，然后通过将特征图进行切片的方式来计算描述子，并且选择特征图中的局部最大值作为关键点。D2-Net中关键点和描述子都是由CNN提取出的，带有高层语义信息，在差别较大的异源图像上可以获得更好的效果。在使用D2-Net分别对航拍图和底图块进行特征提取后，得到两张图各自的关键点，每个关键点都用n维特征向量表示。

S242.使用K近邻搜索算法将航拍图与任一底图块的特征进行粗匹配得到若干匹配对；

所述步骤S242中粗匹配的具体过程为：使用K近邻算法将两张图片的特征向量进行粗匹配，令K＝2，得到N个匹配对，这些匹配对每一个都包含欧氏距离最近的第1匹配点dis_j和次近的第2匹配点dis′_j。

理论上，任意K>2的数都可以取，但是一般取K＝2，因为只用到最近的两个点。

本实施方式中，首先航拍图提取了n个特征向量(n1，n2……nn)，底图块提取了m个特征向量(m1，m2，……mm)，每个特征向量都代表原图中的一个像素点。对于n1会从m个特征向量中找出K个与n1向量欧氏距离最近的向量，K＝2，说明每次只需要找到与n1向量最近的第1匹配点和次近的第2匹配点；n1……nn中的每个特征向量都能在(m1、m2……mm)中找到与之对应的点，这样就有n对匹配对；比如n1向量的第1匹配点是m3，第2匹配点是m9，那么就认为(n1，m3)是一个匹配对。

使用K近邻后(n1……nn)中的每个点都能找到匹配对，这些匹配对并不都是好的，所以后续还使用了动态自适应欧氏距离约束条件来提纯匹配对，因为使用的约束条件涉及到第1匹配点和次近的第2匹配点，所以令K＝2。

S243.使用动态自适应约束条件提纯匹配对；

所述步骤S243中提纯匹配对的具体过程为：用动态自适应欧氏距离约束条件对匹配对进行提纯，统计所有匹配对中第1匹配点和第2匹配点距离差的均值：

对于每一个待筛选匹配对，提纯的条件为第1距离小于第2距离与距离差均值avgdis之差，公式为：

dis_j＜dis′_j-avgdis

dis_j表示距离最近的第1匹配点的距离值，dis′_j表示次近的第二匹配点的距离值，例如航拍图的n1特征向量，它的第1匹配点是底图块的m3向量，n1与m3之间的欧式距离值即为dis_j；它的第2匹配点是m9向量，则n1与m9之间的欧氏距离值即为dis′_j，不满足此公式的匹配对删去，留下满足公式的匹配对；对提纯后的匹配对进行RANSAC算法剔除误匹配对，得到更高质量的匹配对，解算经纬度会更精准。

S244.使用RANSAC算法进一步剔除误匹配对；

RANSAC算法是计算机视觉领域一种常用算法。假设一组数据有“局内点”和“局外点”，局内点的分布符合某个数学模型，局外点是不能适应该模型的数据。本具体实施方式中，所述步骤S244中RANSAC算法剔除误匹配对的具体过程为：

S2441.从提纯后的匹配对中随机抽出若干对匹配对样本，用这若干对匹配对样本拟合一个模型P，具体为3×3的单应性矩阵；

S2442.计算其余匹配对与模型P的误差，若误差小于阈值则认为是局内点，大于阈值则认为是局外点；

S2443.上述过程被称为一次迭代，迭代r次后局内点数量最多的某次结果即为最终的结果，此时计算出的所有局外点即为误匹配对，直接剔除即可。

所述步骤S441中的若干对匹配对样本为小于10对，本实施例中取4对匹配对样本。

一般一张航拍图和一个卫星底图块做完初次匹配也就是k近邻后，可能会有几十到几百个匹配对，然后用动态条件剔除一部分、再用ransac剔除一部分留下可能就十几个到几十个匹配对，拿这最后剩下的匹配对算单应性矩阵。具体再说明下，假设使用RANSAC算法之前一张航拍图和一个底图块有100个匹配对，那RANSAC算法就会在这100个匹配对中随机取几个匹配对拟合一个模型P，然后剩下的匹配对和P模型算误差，分出局内点和局外点，假设这样重复n次后算出了30对局外点，那就剔除，剩下的70个匹配对就认为是高质量的匹配对，这样算出来经纬度精度更准。

S245.最终取匹配对最多的底图块即为最接近的底图块；

如图3所示，这两张图左边是旋转缩放后的航拍图，右边是一个底图块，中间的每条连线就表示一个匹配对，这条线将左图中的一个点和右图中的一个点连接起来，表明这两个点在实际环境中是同一个位置。匹配对数量越多显然两个图片越相似，目的就是想从若干个底图块中，找到跟航拍图最相似的底图块；就是直接比数量，一张航拍图会跟若干个底图块都进行匹配，比如航拍图1跟“底图块11”匹配得到10个匹配对、跟“底图块31”(没有图示出)匹配得到20个匹配对，跟“底图块21”匹配得到25个匹配对，那底图块21的匹配对数量就是最多的。

S25、根据航拍图与最接近的底图块经过异源图像匹配后得到的匹配对，利用OpenCV中的findHomography模块计算两幅图的单应性矩阵M，M将航拍图中心点映射到卫星底图上，卫星底图的每一点都带有经纬度信息，由此得到航拍图中心点亦是目标中心的经纬度。

S3、读取连续两帧地面正射影像，利用金字塔LK光流算法得到两帧间的位移，除以时间得到两帧间的速度，由于帧率较快，所以连续两帧间的平均速度可认为是此时刻的瞬时速度。在相机拍摄影像的过程中一直重复步骤S3，直到相机停止拍摄，这样可以得到无人机任一时刻的速度。

步骤S3中使用金字塔LK光流算法解算无人机实时速度的过程为：

S31、将连续两帧影像转换为灰度图

S32、将两帧影像按照航向角(也叫偏航角)进行旋转，使图像坐标系的横向、纵向与地球坐标系的经纬度方向重合；/>能从惯性单元中读到的，如图6，左边幅图是原始航拍图，此时无人机偏航角为-125.2°，按照偏航角旋转后得到第二幅图，第二幅图的物体朝向和地球坐标系经纬度方向一致；

S33、在前一帧中使用OpenCV模块中的Shi-Tomasi角点检测函数提取特征点。因为角点检测在opencv里都集成好了，直接调用即可。如图7所示，图中黑点为检测出的角点；

S34、采用金字塔LK算法检测出前一帧提取出的特征点在后一帧中的位置并确定坐标。

S35、将两帧的特征点坐标相减，得到地球坐标系下经度和纬度方向上的像素位移Flow_lon、Flow_lat，将像素位移与图像的空间分辨率相乘，得到了以米为单位的两帧图像间的位移Flow_lon_m、Flow_lat_m

S36、如公式(1)、(2)所示，将两帧图像间的位移除以采样时间，得到了两帧间的平均速度，由于采样时间较短，平均速度可以认为是此时的瞬时速度

Vel_lon_m＝Flow_lon_m/(t2-t1) (1)

Vel_lat_m＝Flow_lat_m/(t2-t1) (2)

式(1)(2)中，Vel_lon_m表示经度方向上的速度，Vel_lat_m表示纬度方向上的速度，t2-t1表示两帧图像间的时间差。

S4、计算在步骤S2解算经纬度花费的时间内无人机飞过的位移量，将位移量和S2解算出的无人机经纬度相加得到无人机实时经纬度，读取高度计信息，得到无人机在地球坐标系下的三维位置信息；

解算无人机实时三维位置的过程为：步骤S2通过异源图像匹配来解算无人机经纬度的时间比较长，做不到实时解算，若解算经纬度需要t秒，在这t秒内无人机继续飞行，t秒内相机共有n帧正射影像，步骤S3已经解算出这t秒内每两帧间的位移，累加n-1个位移即为t秒内总位移Δs，按照公式(3)、(4)计算当前时刻无人机的经纬度(lon_now,lat_now)

lon_now＝lon_pre+Δs_lon*degree_lon (3)

lat_now＝lat_pre+Δs_lat*degree_lat (4)

式中，(lon_pre,lat_pre)表示步骤S2解算出的t秒前航拍图与卫星图匹配的无人机经纬度，(Δs_lon,Δs_lat)表示这t秒内经度、纬度方向上的总位移(以米为单位)，degree_lon表示经度方向上1米实际度，degree_lat表示纬度方向上1米实际度。

读取此时的高度计数据h。无人机在地球坐标系下的三维位置即为(lon_now，lat_now，h)。

S5、根据IMU测量信息进行惯性导航，读取陀螺仪的角速度信息利用四元数法解算出无人机的姿态矩阵读取加速度计的加速度值通过积分解算出无人机的位置、速度。

根据IMU测量信息进行无人机位姿信息解算的方法为：

IMU中加速度计测得的加速度值是在载体坐标系下的，步骤S4解算出的无人机位置是在地球坐标下的，为了后续的融合，需要将加速度值转变为地球坐标系，转换公式如(5)。对加速度V_en在采样周期t内进行积分得到速度，再对速度在采样周期t内进行积分得到位移。

f_b为加速度计测量值，为四元数法解算出的载体坐标系到地球坐标系的姿态矩阵，/>表示地球引力加速度矢量。

对加速度在采样周期t内进行积分得到速度，再对速度在采样周期t内进行积分得到位移。

S6、将步骤S4解算出的无人机三维位置信息、S3解算出的无人机实时速度和S5解算出的无人机位置、速度、姿态信息进行扩展卡尔曼滤波融合EKF，得到融合后的高频无人机位置、速度、姿态信息。一般的EKF是把GPS和IMU的信息融合起来，算法输入是GPS提供的经纬度、速度+IMU信息；本发明相比一般的EKF只是输入不同，本发明就是把GPS信息换成了用匹配算出的经纬度+光流法算出的速度。

具体地，扩展卡尔曼滤波融合EKF的过程为：

在无人机飞行的信息融合定位过程中首先建立两个子模型，以步骤S5IMU解算的位姿信息建立***模型，以步骤S4解算出的无人机三维信息建立观测模型；

S61、IMU***模型可描述为

x(k)＝f(x_k-1，u_k-1)+W_k-1 (6)

式(6)中，x＝[s_ix s_iy s_iz v_ix v_ty v_iz]^T表示无人机在k时刻位置s和速度v的6维***状态向量，x、y、z表示地球坐标系的三轴方向，f表示在k-1时刻与状态向量x_k-1，***控制输入量u_k-1有关的非线性函数，W_k-1表示在k-1时刻***高斯白噪声，呈正态分布。

***模型的状态向量预测方程为：

式(7)中，表示在k时刻由步骤S5解算得到无人机位置和速度的先验估计值，/>表示在k-1时刻无人机状态向量输出的最优估计值，/>分别表示先验估计姿态角，Δa表示k-1时刻到k时刻的加速度差。

***状态向量先验估计误差协方差矩阵计算公式如下

/>

式(8)(9)中，表示在k-1时刻无人机最优估计误差协方差矩阵，A表示函数f的雅克比矩阵，Q_k-1表示在k-1时刻***噪声协方差矩阵。式(9)中x＝x_k-1是x＝x_k-1。

S62、观测模型可描述为

z(k)＝h(x_k)+V_k (10)

式中，z＝(lon_now，lat_now，h，Vel_lon_m，Vel_lat_m，vel_h)，z是6维观测向量，(lon_now，lat_now，h)是步骤S4解算出的无人机三维位置信息，Vel_lon_m，Vel_lat_m是步骤S3光流法计算出的无人机实时速度，vel_h是高度方向上的速度，通过高度计差值除以时间间隔得到。h表示k时刻关于***状态向量x_k的非线性函数，h的雅克比矩阵为H_k，V_k表示在k时刻的观测噪声，呈高斯状态分布，其协方差矩阵为R_k。

计算卡尔曼增益K：

最后更新***状态向量与误差协方差矩阵：

S63、由于IMU的更新频率大于图像匹配的更新频率，所以当观测模型中没有新的图像匹配经纬度到来时，跳过步骤S62的观测更新步骤，仅使用S61的惯性导航方式进行导航。最终得到了EKF融合后的高频无人机位置、速度、姿态数据。

Claims (10)

1.一种GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于，包括如下步骤：

S1、读取无人机上相机、IMU、气压高度计输出的数据，调整相机俯仰角使其保持对地面的正射，从相机中读取的是无人机飞行过程中地面正射影像的视频流，从IMU中读取的是无人机自身的角速度和加速度，从气压高度计读取的是飞行高度；

S2、事先根据无人机飞行任务，从网上获取其飞行区域的高分辨率遥感卫星图；读取当前帧的地面正射影像，将其与整片区域的卫星图做异源图像匹配，解算出无人机自身经纬度；

S3、读取连续两帧地面正射影像，利用金字塔LK光流算法得到两帧间的位移，除以时间得到两帧间的速度，由于帧率较快，所以连续两帧间的平均速度可认为是此时刻的瞬时速度；在相机拍摄影像的过程中一直重复步骤S3，直到相机停止拍摄，这样可以得到无人机任一时刻的速度；

S4、计算在步骤S2解算经纬度花费的时间内无人机飞过的位移量，将位移量和S2解算出的无人机经纬度相加得到无人机实时经纬度，读取高度计信息，得到无人机在地球坐标系下的三维位置信息；

S5、根据IMU测量信息进行惯性导航，读取陀螺仪的角速度信息利用四元数法解算出无人机的姿态矩阵读取加速度计的加速度值通过积分解算出无人机的位置、速度；

S6、将步骤S4解算出的无人机三维位置信息、S3解算出的无人机实时速度和S5解算出的无人机位置、速度、姿态信息进行扩展卡尔曼滤波融合EKF，得到融合后的高频无人机位置、速度、姿态信息。

2.如权利要求1所述的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于：步骤S2中对当前帧的地面正射影像与整片区域的卫星图做异源图像匹配，解算出无人机自身经纬度的过程如下：

S21、根据无人机飞行任务，从网上获取其飞行区域的高分辨率遥感卫星图，卫星图上每一个像素都带有准确的经纬度信息，作为图像匹配的卫星底图使用；

S22、无人机搭载可见光相机，在飞行区域内进行地毯式搜索，若发现目标则控制无人机飞至目标中心正上方，调整相机俯仰角使其保持对目标的正射，拍摄目标正射影像；

S23、处理航拍影像，首先缩放航拍图使其空间分辨率与卫星图的空间分辨率保持一致，然后按照航向角旋转航拍图使图像方向保持正北，此时航拍图中所有物体的朝向与卫星图一致；

S24、在卫星底图上进行滑动窗口搜索，窗口大小设置为处理后的航拍图大小，重叠率设置为60％及以上，得到若干底图块，在所有底图块中搜索与航拍图最接近的底图块；

S25、根据航拍图与最接近的底图块经过异源图像匹配后得到的匹配对，利用OpenCV中的findHomography模块计算两幅图的单应性矩阵M，M将航拍图中心点映射到卫星底图上，卫星底图的每一点都带有经纬度信息，由此得到航拍图中心点亦是目标中心的经纬度。

3.根据权利要求2所述的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于，步骤S24中具体搜索策略为：将航拍图与任一底图块进行异源图像匹配，每次匹配都会得到两幅图像间的若干匹配对，取匹配对数量最多的底图块即为最接近的底图块；

所述航拍图与任一底图块的异源图像匹配流程为：

S241.使用深度学习模型D2-Net对航拍图进行特征提取、使用D2-Net对所有底图块逐一进行特征提取；

S242.使用K近邻搜索算法将航拍图与任一底图块的特征进行粗匹配得到若干匹配对；

S243.使用动态自适应约束条件提纯匹配对；

S244.使用RANSAC算法进一步剔除误匹配对；

S245.最终取匹配对最多的底图块即为最接近的底图块。

4.根据权利要求3所述的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于，所述步骤S242中粗匹配的具体过程为：使用K近邻算法将两张图片的特征向量进行粗匹配，令K＝2，得到N个匹配对，这些匹配对每一个都包含欧氏距离最近的第1匹配点dis_j和次近的第2匹配点dis′_j。

5.根据权利要求3所述的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于，所述步骤S243中提纯匹配对的具体过程为：用动态自适应欧氏距离约束条件对匹配对进行提纯，统计所有匹配对中第1匹配点和第2匹配点距离差的均值：

对于每一个待筛选匹配对,提纯的条件为第1距离小于第2距离与距离差均值avgdis之差,公式为：

dis_j＜dis′_j-avgdis

dis_j表示距离最近的第1匹配点的距离值，dis′_j表示次近的第二匹配点的距离值，不满足此公式的匹配对删去，留下满足公式的匹配对。

6.根据权利要求3所述的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于，所述步骤S244中剔除误匹配对的具体过程为：

S2441.从提纯后的匹配对中随机抽出若干对匹配对样本，用这若干对匹配对样本拟合一个模型P；

S2442.计算其余匹配对与模型P的误差，若误差小于阈值则认为是局内点，大于阈值则认为是局外点；

S2443.上述过程被称为一次迭代，迭代r次后局内点数量最多的某次结果即为最终的结果，此时计算出的所有局外点即为误匹配对，直接剔除即可。

7.如权利要求1所述的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于：步骤S3中使用金字塔LK光流算法解算无人机实时速度的过程为：

S31、将连续两帧影像转换为灰度图；

S32、将两帧影像按照航向角进行旋转，使图像坐标系的横向、纵向与地球坐标系的经纬度方向重合；

S33、在前一帧中使用OpenCV模块中的Shi-Tomasi角点检测函数提取特征点；

S34、采用金字塔LK算法检测出前一帧提取出的特征点在后一帧中的位置并确定坐标；

S35、将两帧的特征点坐标相减，得到地球坐标系下经度和纬度方向上的像素位移Flow_lon、Flow_lat，将像素位移与图像的空间分辨率相乘，得到了以米为单位的两帧图像间的位移Flow_lon_m、Flow_lat_m；

S36、如公式(1)、(2)所示，将两帧图像间的位移除以采样时间，得到了两帧间的平均速度，由于采样时间较短，平均速度可以认为是此时的瞬时速度

Vel_lon_m＝Flow_lon_m/(t2-t1) (1)

Vel_lat_m＝Flow_lat_m/(t2-t1) (2)

式(1)(2)中，Vel_lon_m表示经度方向上的速度，Vel_lat_m表示纬度方向上的速度，t2-t1表示两帧图像间的时间差。

8.如权利要求1所述的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于：步骤S4中解算无人机实时位置的过程为：

若步骤S2解算经纬度需要t秒，在这t秒内无人机继续飞行，t秒内相机共有n帧正射影像，步骤S3已经解算出这t秒内每两帧间的位移，累加n-1个位移即为t秒内总位移Δs，按照公式(3)、(4)计算当前时刻无人机的经纬度(lon_now,lat_now)

lon_now＝lon_pre+Δs_lon*degree_lon (3)

lat_now＝lat_pre+Δs_lat*degree_lat (4)

式中，(lon_pre,lat_pre)表示步骤S2解算出的t秒前航拍图与卫星图匹配的无人机经纬度，(Δs_lon,Δs_lat)表示这t秒内经度、纬度方向上的总位移，degree_lon表示经度方向上1米实际度，degree_lat表示纬度方向上1米实际度；

读取此时的高度计数据h，无人机在地球坐标系下的三维位置即为(lon_now，lat_now，h)。

9.如权利要求1所述的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于：步骤S5中根据IMU测量信息进行无人机位姿信息解算的方法为：

IMU中加速度计测得的加速度值是在载体坐标系下的，步骤S4解算出的无人机位置是在地球坐标下的，为了后续的融合，需要将加速度值转变为地球坐标系，转换公式如(5)；对加速度V_en在采样周期t内进行积分得到速度，再对速度在采样周期t内进行积分得到位移；

f_b为加速度计测量值，为四元数法解算出的载体坐标系到地球坐标系的姿态矩阵，表示地球引力加速度矢量。

10.如权利要求1所述的GPS拒止环境下基于异源图像匹配的无人机自主导航定位方法，其特征在于：步骤S6中将S4解算出的无人机三维位置信息、S3解算出的无人机实时速度和S5解算出的无人机位置、速度、姿态信息进行扩展卡尔曼滤波融合EKF的过程为：

在无人机飞行的信息融合定位过程中首先建立两个子模型，以步骤S5 IMU解算的位姿信息建立***模型，以步骤S4解算出的无人机三维信息建立观测模型；

S61、IMU***模型可描述为

x(k)＝f(x_k-1，u_k-1)+W_k-1 (6)

式(6)中，x＝[s_ix s_iy s_iz v_ix v_iy v_iz]^T表示无人机在k时刻位置s和速度v的6维***状态向量，x、y、z表示地球坐标系的三轴方向，f表示在k-1时刻与状态向量x_k-1，***控制输入量u_k-1有关的非线性函数，W_k-1表示在k-1时刻***高斯白噪声，呈正态分布；

***模型的状态向量预测方程为：

式(7)中，表示在k时刻由步骤S5解算得到无人机位置和速度的先验估计值，/>表示在k-1时刻无人机状态向量输出的最优估计值，/>分别表示先验估计姿态角，Δa表示k-1时刻到k时刻的加速度差；

***状态向量先验估计误差协方差矩阵计算公式如下

式(8)(9)中，表示在k-1时刻无人机最优估计误差协方差矩阵，A表示函数f的雅克比矩阵，Q_k-1表示在k-1时刻***噪声协方差矩阵；式(9)中x＝x_k-1是x＝x_k-1；

S62、观测模型可描述为

z(k)＝h(x_k)+V_k (10)

式中，z＝(lon_now，lat_now，h，Vel_lon_m，Vel_lat_m，vel_h)，z是6维观测向量，(lon_now，lat_now，h)是步骤S4解算出的无人机三维位置信息，Vel_lon_m，Vel_lat_m是步骤S3光流法计算出的无人机实时速度，vel_h是高度方向上的速度，通过高度计差值除以时间间隔得到；h表示k时刻关于***状态向量x_k的非线性函数，h的雅克比矩阵为H_k，V_k表示在k时刻的观测噪声，呈高斯状态分布，其协方差矩阵为R_k；

计算卡尔曼增益K：

最后更新***状态向量与误差协方差矩阵：

P_k＝(I-K_kH_k)P_k ^- (13)

S63、由于IMU的更新频率大于图像匹配的更新频率，所以当观测模型中没有新的图像匹配经纬度到来时，跳过S62的观测更新步骤，仅使用S61的惯性导航方式进行导航；最终得到了EKF融合后的高频无人机位置、速度、姿态数据。