CN103954283A - 基于景象匹配/视觉里程的惯性组合导航方法 - Google Patents

Abstract

本发明涉及一种基于景象匹配/视觉里程的惯性组合导航方法，根据视觉里程原理，计算无人机航拍实时图像序列的单应矩阵，通过累积连续两帧实时图之间的相对位移，递推计算出无人机的当前位置；由于视觉里程导航随时间的增加会产生累积误差，因而引入基于FREAK特征的景象匹配算法进行辅助修正，景象匹配具有定位精度高、自主性强、抗电磁干扰等优点，在适配区可以进行高精度定位，有效补偿视觉里程导航长时间工作产生的累积误差；建立惯性导航***的误差模型以及视觉数据的量测模型，通过卡尔曼滤波得出最优估计结果，并对惯性导航***进行校正。本发明有效改善了导航精度，有助于提高无人机自主飞行能力。

Description

基于景象匹配/视觉里程的惯性组合导航方法

技术领域

本发明属于无人机导航定位技术领域，涉及一种基于景象匹配/视觉里程的惯性组合导航方法。 

背景技术

高精度、高动态及高可靠的自主导航是保证无人机顺利完成各种任务的关键技术之一，对于增强无人机自主行为能力，提高作战效能具有十分重要的意义。导航方式分为卫星导航、无线电导航、惯性导航等，其中惯性导航(INS)以其高度自主的突出优点，在导航技术中占有特殊的位置，现有无人机导航***都是以惯性导航为核心构成组合导航***，完成复杂环境下无人机的自主导航。 

目前，无人机导航的最主要方式是INS/GPS组合导航***，但在未知的、动态变化的复杂环境下，GPS信号功率弱，易受到电磁干扰，在信号盲区甚至会停止工作，导致组合导航***出现极大错误，产生不可预估的后果，而我国的北斗卫星导航***处于不断发展阶段，其可靠性和精确性尚难满足诸如军事应用等高精度导航要求。 

发明内容

要解决的技术问题 

为了避免现有技术的不足之处，本发明提出一种基于景象匹配/视觉里程的惯性组合导航方法，实现无人机复杂环境下全自主导航。 

技术方案 

一种基于景象匹配/视觉里程的惯性组合导航方法，其特征在于步骤如下： 

步骤1：在无人机飞行过程中，机载下视摄像机实时获取地面图像a； 

步骤2：利用图像a与前一帧图像a′，确定无人机的视觉里程，步骤如下： 

A、使用Harris角点检测算法分别在连续两帧实时图像a与a′中提取特征点； 

B、在图像a中以(x,y)^T为中心的方形区域中搜索与图像a′中的每个特征点(x,y)^T具有最高邻域互相关的匹配点；同时，在图像a′中以(x,y)^T为中心的方形区域中搜索 与图像a中的每个特征点(x,y)^T具有最高邻域互相关的匹配点； 

C、运用RANSAC鲁棒估计方法得到最大的一致点集和单应矩阵H的估计，过程为首先随机抽取4组匹配点对组成一个随机样本，并计算单应矩阵H；然后对步骤B中的每个匹配点对，计算距离dis；再设定阈值t，若dis<t，则此匹配点对为内点，否则剔除，并统计内点数目；重复上述过程k次，选择H； 

D、由划定为内点的所有匹配点对具有最大内点数的H重新估计使用重新获得的H计算出图像a′中与图像a中每个特征点(x,y)^T相对应的点(x₁,y₁)^T。并使用步骤B中方法分别在图像a中以(x,y)^T为中心的方形区域中搜索与图像a′中的每个特征点(x₁,y₁)^T具有最高邻域互相关的匹配点；同时，在图像a′中以(x₁,y₁)^T为中心的方形区域中搜索与图像a中的每个特征点(x,y)^T具有最高邻域互相关的匹配点； 

E、重复步骤B到步骤D，直到匹配点对的数目稳定为止； 

步骤3：当无人机进入适配区时，根据惯性导航***对无人机进行粗定位，在机载存储设备中找到与粗定位结果对应的基准图像b，并与实时图像a进行景象匹配，确定无人机的位置； 

步骤4：在适配区中利用步骤3得到的无人机位置校正视觉里程产生的误差，得到无人机较精确地位置； 

步骤5：利用惯性导航***给出无人机当前的位置和姿态； 

步骤6：使用惯性导航***的误差方程作为组合导航***的状态方程，导航坐标系选择为东北天坐标系，将步骤4中得出位置与步骤5中得出位置的差值作为量测。用卡尔曼滤波器估计出惯性***的漂移误差，并使用该漂移误差校正惯性导航***，得到融合后的导航参数。 

所述步骤3的过程如下：首先无人机机载下视摄像机实时获取地面图像a，对图像a进行预处理，得到图像c；然后提取图像b与图像c的FAST特征；再使用FREAK特征描述符对提取的FAST特征进行描述；利用最近汉明距离的相似性准则进行特征匹配，得到匹配位置，即当前无人机的位置。 

所述地面图像a为光学图像或红外图像。 

有益效果 

本发明提出的一种基于景象匹配/视觉里程的惯性组合导航方法，根据视觉里程原理，计算无人机航拍实时图像序列的单应矩阵，通过累积连续两帧实时图之间的相对位移，递推计算出无人机的当前位置；由于视觉里程导航随时间的增加会产生累积误差，因而引入基于FREAK特征的景象匹配算法进行辅助修正，景象匹配具有定位精度高、自主性强、抗电磁干扰等优点，在适配区可以进行高精度定位，有效补偿视觉里程导航长时间工作产生的累积误差；建立惯性导航***的误差模型以及视觉数据的量测模型，通过卡尔曼滤波得出最优估计结果，并对惯性导航***进行校正。本发明有效改善了导航精度，有助于提高无人机自主飞行能力。 

但是，景象匹配适配区分度的研究表明，只有在适配区的匹配才能够为无人机提供较为可靠的位置信息，而在非适配区如沙漠、海面等，景象匹配是无法正常工作的。 

视觉里程是通过处理连续两帧图像估计运动信息的技术，该技术作为一种新的导航定位方式，已成功应用于自主移动机器人中。在无人机飞行过程中，由于连续两帧图像均为无人机平台下同一传感器、同一时间段、同一条件拍摄的图像，具有相同的噪声分布和成像误差，且不存在自然条件变化造成的成像差异，因此能够在非适配区提供较精确的定位信息；同时，连续两帧图像尺寸相比于景象匹配中基准图尺寸要小得多，因此匹配时间较少，从而提高导航***的实时性。 

因此，本发明提出一种基于景象匹配/视觉里程的无人机惯性组合导航方法，该方法具有自主性强、载荷轻、设备成本低以及抗干扰性强等优点，为无人机导航***的工程应用提供一种可行的技术方案。是一种基于计算机视觉的导航方式，具有定位精度高、抗电子干扰能力强及机载设备尺寸小、成本低等显著优点，可消除惯性导航***长时间工作的累积误差，大幅度提高惯性导航***的定位精度，成为GPS失效、故障或精度下降等情况下的备用导航发法。 

附图说明

图1是本发明的框架流程 

图2是两视图单应变换关系图 

图3是FREAK描述子采样模式 

具体实施方式

现结合实施例、附图对本发明作进一步描述： 

本发明中基于景象匹配/视觉里程的惯性组合导航方法，利用侦察手段获取无人机预定飞行区域的地物景象作为基准图，当携带视觉传感器的无人机飞过预定的区域时，获取当地图像并按像素点分辨率、飞行高度和视场等参数生成一定大小的地物景象作为实时图，通过实时图与基准图的匹配，找出实时图在基准图中的位置，进而确定出无人机当前的准确位置。景象匹配导航精度不受导航时间影响，具备抗电磁干扰能力强、自主性好、精度高、成本低、体积小、信息丰富等优点，将其与惯性导航进行组合能够大大提高导航***的整体性能，因此在无人机导航中开展惯性/景象匹配自主组合导航研究，有利于摆脱外界辅助***，增强无人机的可靠性、机动性、隐蔽性、抗干扰性和生存能力。主要包括惯性导航、景象匹配、视觉里程、融合校正与组合导航卡尔曼滤波五个部分，包括以下步骤： 

第一步，机载下视摄像机实时获取地面图像a，通过估计实时图像a与前一帧实时图像a′的单应矩阵，确定无人机的视觉里程。 

具体实施步骤如下： 

1、运用Harris角点检测算法分别在连续两帧实时图像a与a′中提取特征点； 

2、对a′中的每个特征点(x,y)^T，在a中以(x,y)^T为中心的方形区域中搜索具有最高邻域互相关的匹配点。同理，对a中的每个特征点在a′中搜索其匹配点，最终确定匹配特征点对； 

3、运用RANSAC鲁棒估计方法得到最大的一致点集并且计算连续两帧实时图像a与a′之间的单应矩阵H。具体流程包括：(1)随机抽取4组匹配点对组成一个随机样本，并计算单应矩阵H；(2)对步骤2中的每个匹配点对，计算距离dis；(3)设定阈值t(t小于实时图边长的一半)，若dis<t，则此匹配点对为内点，否则剔除，并统计内点数目；(4)重复上述过程k次，选择具有最大内点数的H； 

4、由划定为内点的所有匹配点重新计算单应矩阵H，并由H决定步骤2中的搜索区域的位置，从而确定更准确的匹配点对； 

5、重复步骤2到步骤4，直到匹配点对的数目稳定为止，并且使用最终确定的稳定匹配点对计算出单应矩阵H。 

6、使用步骤5得到的单应矩阵H，并且根据惯性元件提供的姿态信息与气压高度计提供的高度信息，求出摄像机在拍摄连续两帧图像时的相对位移，通过累积摄像机的位移，利用之前估计的位置，循环地计算出无人机当前的位置。 

第二步，当无人机进入适配区时，根据惯性导航***对无人机进行粗定位，根据粗定位结果，在预先存储的无人机飞行区域航拍基准图中裁剪圆形基准子图像b，基准子图半径需大于惯性元件单位时间平均漂移距离与无人机在两个适配区之间飞行时间乘积的1.5倍。将实时图像a与基准子图像b进行景象匹配，确定无人机的位置。景象匹配具体步骤如下： 

1、分别对实时图像a与基准子图像b进行灰度化，并且提取实时图像a与基准子图像b的FAST特征点。 

2、对每一个FAST特征点求出其FREAK特征描述算子，其步骤如下： 

如图3所示，FREAK以每个特征点为中心构建圆形采样，中心密集，四周稀疏，采样数量以指数形式递减。使用不同的高斯核预平滑每个采样圆，高斯核的标准差与圆的大小成正比。 

FREAK描述子由差分高斯的比特串构成，定义采样圆的准则T为 

P_a代表采样对，I(·)代表平滑后的采样圆强度值。选择N个采样对，定义二进制准则 

$F=\underset{0\&le;a<N}{\mathrm{\&Sigma;}}{2}^{a}T\left(P_a\right)$

即可得到N维的二进制比特串。 

采样对的选取是一个由粗到精的过程，准则如下： 

a)创建一个能包含5万个特征点的大矩阵D，每行代表一个特征点，通过对每个特征点的43个采样圆强度进行两两比较，得到1000多维描述子； 

b)计算矩阵D每一列的均值与方差，当均值为0.5时方差最大，可以保证特征描述符的独特性。 

c)根据方差的大小对每一列进行排序，方差最大的位于第一列。 

d)保留前N列，对每个特征点进行描述，得到N维二进制比特串。本发明选择N＝256。 

如图3，在所有采样圆中选择M(M＝45)个对称采样对，计算局部梯度为 

$O=\frac{1}{M}\underset{P_O\&Element;G}{\mathrm{\&Sigma;}}(I\left(P_O^{r1}\right)-I\left(P_O^{r2}\right))\frac{P_O^{r1}-P_O^{r2}}{\left|\right|P_O^{r1}-P_O^{r2}\left|\right|}$

对应采样圆的二维向量，I(·)代表平滑后的采样圆强度值。 

3、计算进行匹配的两个特征点FREAK描述符的最近汉明距离。为了滤除误匹配，本发明设置一个阈值10，高于该则直接滤除，低于该阈值的点被认为是相互匹配的两点。 

4、根据步骤2和步骤3中的方法进行特征匹配，通过确定实时图像a中特征点在基准子图像b中的位置来确定实时图像a在基准子图像b中的位置，从而确定飞机当前位置。 

第三步，利用第二步得到的无人机位置校正视觉里程产生的误差，得到无人机较精确的位置P_vision，并通过惯性导航***给出无人机当前的位置P_ins与姿态A_ins。 

假设在适配区通过景象匹配得到的无人机位置误差很小，在此步骤中直接使用景象匹配的结果对视觉里程计算出的位置进行重置。 

第四步，使用卡尔曼滤波器对P_vision、P_ins和A_ins进行估计，得到最优导航信息。 

具体实施例如下： 

1、在无人机飞行过程中，机载下视摄像机实时获取地面图像a。 

利用无人机机载的下视光学摄像机或者红外摄像机实时获取地面图像序列，但只需保存当前帧与前一帧图像即可。 

2、利用图像a与前一帧图像a′，通过估计a与a′的单应矩阵，确定无人机的视觉里程。 

如图2所示，无人机在飞行状态下，机载摄像机在不同位姿下连续拍摄两帧图像I₁和I₂，对应的摄像机坐标系为F和F′，假定平面π上的点P映射为图像I₁中的点p和I₂中的点p′，对应于F和F′中的向量为和则存在 

${\stackrel{\&RightArrow;}{p}}^{\&prime;}=R_{c1}^{c2}\stackrel{\&RightArrow;}{p}+t$

和t分别代表两次拍摄间无人机的旋转和平移运动。 

由图2，n是c1处相对于平面π的法向量，d是c1到平面π的距离，有 

$n^T\stackrel{\&RightArrow;}{p}=d$

因此， 

${\stackrel{\&RightArrow;}{p}}^{\&prime;}=R\stackrel{\&RightArrow;}{p}+t\frac{n^T\stackrel{\&RightArrow;}{p}}{d}=H_c\stackrel{\&RightArrow;}{p}$

其中， 

$H_c=R_{c1}^{c2}+\frac{{\mathrm{tn}}^T}{d}$

称为平面π关于摄像机的单应矩阵。 

考虑摄像机内参数，则 

$p=K\stackrel{\&RightArrow;}{p},p^{\&prime;}=K{\stackrel{\&RightArrow;}{p}}^{\&prime;}$

因此， 

p′＝KH_cK^-1p＝Hp 

其中， 

$H=K(R_{c1}^{c2}+\frac{{\mathrm{tn}}^T}{d})K^{-1}$

称为平面π关于两帧图像的单应矩阵。 

根据文献可知，单应矩阵是自由度为8的3×3矩阵，因此需要知道两幅图像之间4组匹配点对来计算，为了防止出现退化，这4组匹配点对所在平面要求不能通过摄像机光心，且空间4个点任意3个点不共线。 

另外，由于摄像机固连在无人机上，可以认为摄像机的姿态与无人机的姿态一致，由机载的惯导器件提供，分别为偏航角ψ、俯仰角θ、横滚角γ，因此有 

$R_{c1}^{c2}=R_n^{c2}{R}_{c1}^n$

其中是c2处摄像机坐标系到导航坐标系的旋转矩阵，为 

$R_n^{c2}=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\\ -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}\end{array}\right]$

同理可得

如果假设地面为平面，则n＝(0,0,1)^T，当计算出单应矩阵后，根据惯导提供的姿态信息与气压高度计提供的高度信息，使用上述公式求出摄像机在拍摄连续两帧图像时的相对位移，通过累积摄像机的位移，利用之前估计的位置，循环地计算出无人机当前的位置。具体实施步骤如下： 

2.1、运用Harris角点检测算法分别在连续两帧实时图像a与a′中提取特征点； 

2.2、对a′中的每个特征点(x,y)^T，在a中以(x,y)^T为中心的方形区域中搜索具有最高邻域互相关的匹配，对称的，对a中的每个特征点在a′中搜索其匹配，最终确定匹配特征点对； 

2.3、运用RANSAC鲁棒估计方法得到最大的一致点集和单应矩阵H的估计。具体流程包括：(1)随机抽取4组匹配点对组成一个随机样本，并计算单应矩阵H；(2)对步骤B中的每个匹配点对，计算距离dis；(3)设定阈值t，若dis<t，则此匹配点对为内点，否则剔除，并统计内点数目；(4)重复上述过程k次，选择具有最大内点数的H； 

2.4、由划定为内点的所有匹配点对H重新估计，并由H决定步骤2.2中的搜索区域，更准确地确定匹配点对； 

2.5、重复步骤2.2到步骤2.4，直到匹配点对的数目稳定为止。 

3、当无人机进入适配区时，根据惯性导航***对无人机进行粗定位，在机载存储 设备中找到与粗定位结果对应的基准图像b，并与实时图像a进行景象匹配，确定无人机的位置。 

本发明采用基于FREAK描述符的景象匹配算法对实时图与基准图进行匹配，如图3所示，FREAK描述子采用类似视网膜采样模式，以每个特征点为中心构建圆形采样，中心密集，四周稀疏，采样数量以指数形式递减。为了增强采样圆的鲁棒性，提高描述符的稳定性与独特性，使用不同的高斯核预平滑每个采样圆，高斯核的标准差与圆的大小成正比。 

FREAK描述子由差分高斯的比特串构成，定义采样圆的准则T为 

P_a代表采样对，I(·)代表平滑后的采样圆强度值。选择N个采样对，定义二进制准则 

$F=\underset{0\&le;a<N}{\mathrm{\&Sigma;}}{2}^{a}T\left(P_a\right)$

即可得到N维的二进制比特串。 

采样对的选取是一个由粗到精的过程，准则如下： 

a)创建一个能包含5万个特征点的大矩阵D，每行代表一个特征点，通过对每个特征点的43个采样圆强度进行两两比较，得到1000多维描述子； 

b)计算矩阵D每一列的均值与方差，当均值为0.5时方差最大，可以保证特征描述符的独特性。 

c)根据方差的大小对每一列进行排序，方差最大的位于第一列。 

d)保留前N列，对每个特征点进行描述，得到N维二进制比特串。本文选择N＝256。 

采样对的选取准则保证了描述符的灰度不变性。 

如图3，在所有采样圆中选择M(M＝45)个对称采样对，计算局部梯度为 

$O=\frac{1}{M}\underset{P_O\&Element;G}{\mathrm{\&Sigma;}}(I\left(P_O^{r1}\right)-I\left(P_O^{r2}\right))\frac{P_O^{r1}-P_O^{r2}}{\left|\right|P_O^{r1}-P_O^{r2}\left|\right|}$

对应采样圆的二维向量，I(·)代表平滑后的采样圆强度值。采样对的对称性保证了描述子的旋转不变性。 

FREAK描述符是1和0组成的二进制比特串，因此相似性准则采用最近汉明距离方法，即对进行匹配的两个描述符按位进行异或操作。针对512维FREAK描述符，最大汉明距离为512，最小为0，为了滤除误匹配，设置一个阈值T,高于T则直接滤除，本文中阈值设为10。 

4、在适配区中利用景象匹配得到的无人机位置校正视觉里程产生的误差，得到无人机较精确地位置。 

由于在适配区中景象匹配的定位结果是可靠的，因此直接利用景象匹配得到的无人机位置对视觉里程计算得到的位置进行重置，消除视觉里程长时间工作产生的累积误差。 

5、利用惯性导航***给出无人机当前的位置和姿态。 

6、利用卡尔曼滤波算法估计无人机当前的精确位置和姿态。 

使用惯性导航***的误差方程作为组合导航***的状态方程，导航坐标系选择为东北天坐标系，将4中位置与5中位置的差值作为量测值，用卡尔曼滤波器估计出惯性***的漂移误差，然后校正惯性导航***，得到融合后的导航参数。 

Claims (3)

1.一种基于景象匹配/视觉里程的惯性组合导航方法，其特征在于步骤如下：

步骤1：在无人机飞行过程中，机载下视摄像机实时获取地面图像a；

步骤2：利用图像a与前一帧图像a′，确定无人机的视觉里程，步骤如下：

A、使用Harris角点检测算法分别在连续两帧实时图a与a′中提取特征点；

B、在图像a中以(x,y)^T为中心的方形区域中搜索与图像a′中的每个特征点(x,y)T具有最高邻域互相关的匹配点；同时，在图像a′中以(x,y)^T为中心的方形区域中搜索与图像a中的每个特征点(x,y)^T具有最高邻域互相关的匹配点；

C、运用RANSAC鲁棒估计方法得到最大的一致点集和单应矩阵H的估计，过程为首先随机抽取4组匹配点对组成一个随机样本，并计算单应矩阵H；然后对步骤B中的每个匹配点对，计算距离dis；再设定阈值t，若dis<t，则此匹配点对为内点，否则剔除，并统计内点数目；重复上述过程k次，选择H；

D、由划定为内点的所有匹配点对具有最大内点数的H重新估计，使用重新获得的H计算出图像a′中与图像a中每个特征点(x,y)^T相对应的点(x₁,y₁)^T。并使用步骤B中方法分别在图像a中以(x,y)^T为中心的方形区域中搜索与图像a′中的每个特征点(x₁,y₁)^T具有最高邻域互相关的匹配点；同时，在图像a′中以(x₁,y₁)^T为中心的方形区域中搜索与图像a中的每个特征点(x,y)^T具有最高邻域互相关的匹配点；

E、重复步骤B到步骤D，直到匹配点对的数目稳定为止；

步骤3：当无人机进入适配区时，根据惯性导航***对无人机进行粗定位，在机载存储设备中找到与粗定位结果对应的基准图b，并与实时图a进行景象匹配，确定无人机的位置；

步骤4：在适配区中利用步骤3得到的无人机位置校正视觉里程产生的误差，得到无人机较精确地位置；

步骤5：利用惯性导航***给出无人机当前的位置和姿态；

步骤6：使用惯性导航***的误差方程作为组合导航***的状态方程，导航坐标系选择为东北天坐标系，将步骤4中得出位置与步骤5中得出位置的差值作为量测。用卡尔曼滤波器估计出惯性***的漂移误差，并使用该漂移误差校正惯性导航***，得到融合后的导航参数。

2.根据权利要求1所述基于景象匹配/视觉里程的惯性组合导航方法，其特征在于：所述地面图像a为光学图像或红外图像。

3.根据权利要求1所述基于景象匹配/视觉里程的惯性组合导航方法，其特征在于：所述步骤3的过程如下：首先无人机机载下视摄像机实时获取地面图像a，对图像a进行预处理，得到图像c；然后提取图像b与图像c的FAST特征；再使用FREAK特征描述符对提取的FAST特征进行描述；利用最近汉明距离的相似性准则进行特征匹配，得到匹配位置，即当前无人机的位置。