CN108917753A - 基于从运动恢复结构的飞行器位置确定方法 - Google Patents

Abstract

本发明公开了一种基于从运动恢复结构的飞行器位置确定方法。首先利用所述成像设备对航迹下地形连续成像，以获取运动序列图像；然后基于所述运动序列图像恢复实时三维地形图并获取实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标。其次将所述实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标转化到飞行器平行坐标系下。再次将所述实时三维地形图上各点在飞行器平行坐标系下的空间三维坐标转化到所述地面基准地形坐标系下，最后反向求解飞行器在所述地面基准地形坐标系下的位置。所述基于从运动恢复结构的飞行器位置确定方法可以低成本低能耗精准地测量飞行器的位置。

Description

基于从运动恢复结构的飞行器位置确定方法

技术领域

本发明涉及航天技术领域，特别涉及一种基于从运动恢复结构的飞行器位置确定方法。

背景技术

航天器、无人机等飞行器的导航与控制过程中，飞行器的位置和姿态是安全飞行及执行任务的关键信息。飞行器的飞行位置姿态确定方法中常用的有基于惯性导航、基于GPS导航及基于激光地形测量与景像匹配导航等方法。

惯性导航方法是利用加速度计和陀螺仪等导航传感器对运动信息进行测量，该方法利用加速度计的实时加速度测量并进行积分运算，从而得到飞行过程中的位置与姿态信息。存在如下缺点：采用积分运算进行位姿确定，存在积累误差的问题，尤其在长时间应用时误差积累较大，常常需要采用其他定位准确的方法对惯性导航计算结果进行校正。

GPS导航定位方法利用飞行器安装的GPS组件接收运行于轨道上的GPS导航卫星的信号，根据接收到的多个卫星的信号进行综合计算，获取飞行器的位置信息，是航空飞行器、汽车等运动工具常用的导航方法之一。存在如下缺点：由于GPS卫星信号为国外导航卫星提供，通常无法获取高精度数据，因此计算结果精度较差，而且信号能否正常获取也受制于人；由于接收GPS卫星信号受在轨运行卫星“视线”遮挡的影响，GPS导航方法计算的位置信息精确性和更新频率都会下降。

基于激光地形测量与景象匹配导航方法包括地形匹配定位与景象匹配定位。

地形匹配利用飞行器装载的激光高度计实时测量飞行轨迹的地形高度，将实时测量结果与飞行器预先装载的基准地形图进行匹配计算，确定飞行器的位置从而实现飞行导航。存在如下缺点：激光高度计属于专用设备，能耗较大，并且这类精密仪器价格昂贵，难以在一般飞行器上安装使用，这种一维匹配导航适合于山丘地形的飞行。

景象匹配导航是以地表图像作为特征，采用实时摄像方法录取飞行轨迹周围或目标附近地区地貌，与存储在飞行器上的基准景象图比较，进行匹配定位与导航。该景象匹配属于二维匹配导航,可以确定飞行器两个坐标的偏差,只适合于平坦地区导航。

公开于该背景技术部分的信息仅仅旨在增加对本发明的总体背景的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域一般技术人员所公知的现有技术。

发明内容

本发明的目的在于提供一种基于从运动恢复结构的飞行器位置确定方法，可以低成本低能耗精准地测量飞行器的位置。

为实现上述目的，本发明提供了一种基于从运动恢复结构的飞行器位置确定方法，所述飞行器具有惯性导航设备和成像设备。基于从运动恢复结构的飞行器位置确定方法包括以下步骤：利用所述成像设备对航迹下地形连续成像，以获取运动序列图像；基于所述运动序列图像恢复实时三维地形图并获取实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标，所述飞行器本体姿态坐标系是基于飞行器飞行时的姿态而建立的坐标系；将所述实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标转化到飞行器平行坐标系下，所述飞行器平行坐标系是与地面基准地形坐标系相平行的飞行器原点坐标系；将所述实时三维地形图上各点在飞行器平行坐标系下的空间三维坐标转化到所述地面基准地形坐标系下，以及反向求解飞行器在所述地面基准地形坐标系下的位置。

优选地，上述技术方案中，所述基于运动序列图像恢复实时三维地形结构的包括以下步骤：图像特征点提取；序列图像匹配；根据序列图像视差与飞行速度进行关键帧选择；从关键帧同名特征点解算运动参数；以及实时三维地形重建。

优选地，上述技术方案中，所述序列图像匹配包括以下步骤：进行正向匹配点搜索；进行反向匹配点搜索以及双向最佳匹配从而确定候选的同名特征点；以及进行RANSAC模型约束从而将所述候选的同名特征点中选择出正确的同名特征点。

优选地，上述技术方案中，所述进行正向匹配点搜索包括以下步骤：从所述序列图像的第二帧开始，计算每帧与前一帧的图像匹配结果，采用逐个特征点距离比较的方法，对图像I₁中的每个待匹配特征点f₁(i)，计算其与图像I₂中每个候选特征点f₂(j)的特征距离，将图像I₂中与f₁(i)具有正向最小距离的特征点f₂(k)记为正向匹配的候选同名配准点(f₁(i)，f₂(k))，并记录所述正向最小距离以及记录特征点f₁(i)与I₂中特征点的正向次小距离，所述距离d公式如下：

其中V和V′为待匹配两图中特征点的不变性向量描述，v_i和v_i′是其向量的分量，如果最小距离与次小距离的比值大于设定阈值Rd_th，则确认初步同名特征点(f₁(i)，f₂(k))。

优选地，上述技术方案中，所述进行反向匹配点搜索以及双向最佳匹配包括以下步骤：对图像I₂中的每个待匹配特征点f₂(j)，计算其与图像I₁中每个候选特征点的距离，将图像I₁中与f₂(j)具有反向最小距离的特征点f₁(k)记为反向匹配的候选同名配准点(f₂(j)，f₁(k))，并记录所述反向最小距离以及记录f₂(j)与图像I₁中特征点的反向次小距离，按照所述正向匹配点计算的方法，最终确认反向初步同名匹配(f₂(j)，f₁(k))，然后判断这两个方向匹配结果的一致性，若i＝k，则暂定两个匹配结果为正确同名匹配点，否则判断为错误匹配并放弃。

优选地，上述技术方案中，所述进行RANSAC模型约束包括以下步骤：在保证取点距离大于一定阈值情况下随机选取相邻两帧序列图像中8对参与计算的匹配点；基于8点算法解算的基础矩阵F与匹配点之间关系X′^TFX＝0，其中X为图像I₁中的待匹配点的向量矩阵，X′为图像I₂中的待匹配点的向量矩阵，x、y为X向量的分量，x′、y′为X′向量的分量，用矢量f表示F矩阵的元素，则每一个匹配点对应的求解F的方程写为(x′x,x′y,x′,y′x,y′y,y′,x,y,1)f＝0，由8组匹配点组成线性方程组，估计基础矩阵F；计算所有匹配点与RANSAC模型的基础矩阵F之间的距离d_f，其中d_f＝X′^TFX，距离d_f小于一定阈值的匹配点即作为内点；重复步骤m次，记录每次的解算结果以及相应的内点；以及选择内点数N最多的结果对应的点集合，使用所有内点重新估计F矩阵作为输出结果，所有内点为最终正确匹配的同名特征点。

优选地，上述技术方案中，根据序列图像视差与飞行速度进行关键帧选择包括以下步骤：根据所述特征点最终匹配结果计算平均视差d_m，公式如下：

根据相邻两帧序列图像平均速度v_m和间隔时间t_m计算相邻两帧序列图像的相对距离D_m，公式为D_m＝v_m×t_m，如果d_m大于一定阈值且D_m大于0.08h_m，则判断该图像是关键帧图像，其中h_m是飞行高度，根据前一次位置计算结果得到，而第一次确定方法为，首先在d_m大于一定阈值条件下，由当参与计算的两帧图像的图像中心点进行三维交会，根据该点到相机光心的距离来确定。

优选地，上述技术方案中，从关键帧同名特征点解算运动参数包括以下步骤：根据每两个相邻关键帧图像间的基础矩阵F计算本质矩阵E，计算关系式为E＝K^TFK′，其中K为关键帧K₁中的待匹配点的向量矩阵，K′为关键帧K₂中的待匹配点的向量矩阵；根据本质矩阵E与运动参数的关系式E＝T×R，通过对本质矩阵进行分解得出相邻两个关键帧后一帧相对前一帧的相对运动参数R_x和T_x，其中R表示姿态矩阵，T表示平移向量，R_x表示姿态矩阵相对值，T_x表示平移向量相对值；计算平移距离的绝对值T_a，公式为T_a＝vt，其中v为飞行速度，t为两次成像的时间间隔，两者都是所述成像设备中的参数；以及将T_a带入E＝T×R，确定相邻两个关键帧后者位置姿态相对前者位置姿态的相对运动参数R_a。

优选地，上述技术方案中，基于所述运动序列图像恢复实时三维地形图的包括以下步骤：求解所述每个关键帧中特征点在飞行器本体姿态坐标系下的空间三维坐标；通过所述关键帧中特征点在关键帧图像上重投影误差最小化的方法，对每个关键帧的位置姿态参数进行优化求解，获取最优配置条件下的所有特征点的三维坐标，从而构成重建地形的三维结构；相邻两个关键帧图像之间以第一步的特征点作为种子点，对种子点区域内的点进行仿射变换计算初始匹配作为引导，采用最小二乘方法进行密集点图像匹配；以及通过空间前方交会的方法进行所述密集点三维位置计算，得到表面密集的实时三维地形。

优选地，上述技术方案中，求解所述每个关键帧中特征点在飞行器本体姿态坐标系下的空间三维坐标的公式如下：

其中，Q是关键帧中的成像投影矩阵，f_u、f_v分别为成像设备焦距在像面u、v方向的焦距，u₀、v₀为成像设备主点在像面U、V方向的分量，X为特征点在飞行器本体姿态坐标系下的空间三维坐标矩阵，x_w、y_w、z_w为X的分量，X₁、X₂为相邻两个关键帧中的同名特征点在相应关键帧上的齐次坐标，Q₁、Q₂分别为相邻两个关键帧的投影矩阵，X为特征点在飞行器本体姿态坐标系下的空间三维坐标矩阵。

优选地，上述技术方案中，将所述实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标转化到飞行器平行坐标系下包括以下步骤：在所述飞行器平行坐标系下，以XY平面上的等距离位置进行插值，获得XY平面上标准格网分布的三维数据I_pr，所述数据I_pr用高程相对位置的函数值集合h(x_pr,y_pr)来表示。

优选地，上述技术方案中，将所述实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标转化到飞行器平行坐标系下包括以下步骤：根据所述惯性导航设备的信息确定空间点从飞行器本体姿态坐标系到平行坐标系的旋转变换矩阵为R_b-p，飞行器本体姿态坐标系点云P^b变换到与飞行器平行坐标系下的点云P^P的关系为P^P＝R_b-pP^b。

优选地，上述技术方案中，将所述实时三维地形图上各点在飞行器平行坐标系下的空间三维坐标转化到所述地面基准地形坐标系下的包括以下步骤；选择合适的实时三维地形图模板尺度，包括航带方向长度L和宽度W，建立匹配模板点列，确定所述飞行器平行坐标系下的实时三维地形图在所述地面基准地形图的初始位置(x₀,y₀)；计算所述初始位置附近各点与所述飞行器平行坐标系下的实时三维地形图的相关系数，在所述匹配模板点列上，某点(x,y)的相关系数计算公式为：

其中LW表示模板区域，h_t(i,j)、h_m(i,j)分别为所述飞行器平行坐标系下的实时三维地形图和地面地形基准图相对应的LW区域内(i,j)位置的地形高程数据，为所述飞行器平行坐标系下的实时三维地形图和地面地形基准图相对应的LW区域的地形高程均值；确定所述相关系数极大值对应位置；以及将地面地形基准图上的所述相关系数极大值对应位置附近3×3像素单元范围内的每个点的相关系数相对于二维位置用二次曲线拟合，选择二次曲线极值点对应的位置确定为该时刻飞行器平行坐标系下的实时三维地形图模板在地面地形基准图上的精确位置。

优选地，上述技术方案中，所述反向求解飞行器位置的方法为：若所述LW模板中心在飞行器平行坐标系下的空间坐标为P(xt0,yt0,zt0)且所述LW模板中心在所属地面基准地形图上的匹配点的坐标为(xm,ym)，则确定飞行器在所述地面地形基准图上定位结果为(xm-xt0,ym-yt0,-z0)。

与现有技术相比，本发明具有如下有益效果：成本低能耗小、设备便携、不依赖GPS受外界信号干扰小、不受地形限制。

附图说明

图1是根据本发明一实施方式的基于从运动恢复结构的飞行器位置确定方法的流程示意图。

图2是根据本发明一实施方式的特征点邻域内各像素点的梯度坐标系。

图3是根据本发明一实施方式的特征点邻域内各像素点的梯度分布直方图。

图4是根据本发明一实施方式的特征向量描述。

具体实施方式

下面结合附图，对本发明的具体实施方式进行详细描述，但应当理解本发明的保护范围并不受具体实施方式的限制。

除非另有其它明确表示，否则在整个说明书和权利要求书中，术语“包括”或其变换如“包含”或“包括有”等等将被理解为包括所陈述的元件或组成部分，而并未排除其它元件或其它组成部分。

基于从运动恢复结构的飞行器位置确定方法的基本原理是利用飞行器搭载的成像传感器，在飞行过程中对航迹下的地形地貌进行连续成像，对每个区域形成多个位置的多视角成像，根据多视图几何原理，利用计算机视觉中从运动相机恢复场景三维结构的方法解算目标区域三维地形，从而获取航迹区域内的实时地形图，将重建的实时三维地形图与基准地形图进行地形匹配，从而实现飞行器位置计算确定。

图1是根据本发明一实施方式的基于从运动恢复结构的飞行器位置确定方法的流程示意图。

根据本发明一优选的实施方式，基于从运动恢复结构的飞行器位置确定方法分为4个步骤：飞行图像采集11、基于运动序列图像恢复实时三维地形结构12、实时三维地形与基准地形匹配13、反向求解飞行器位置14。

一、飞行图像采集11

飞行图像是在飞行器飞行过程中连续获取的实时成像，在时间上的连续性和成像区域上的重叠性使得相邻图像间同名特征点的视差较小，成像角度、图像特征变化小，适合于序列图像间的图像配准。基于运动序列图像恢复结构的位姿确定方法，以连续图像的不小于60％的重叠率对飞行航线上的地形地貌进行序列成像，在成像的同时记录成像附属信息，附属信息包括每幅图像的成像时间、成像时平台的飞行速度等惯性导航设备信息，作为后续计算的输入。

二、基于运动序列图像恢复三维地形结构12

该步骤以飞行航线内地形地貌的序列成像作为输入，利用计算机视觉方法通过图像处理计算航线覆盖区域的三维地形结构。在该过程中本实施方式提出了基于鲁棒性方法的特征匹配、基于视差估计的关键帧选择、基于分解法的运动与三维解算、密集结构插补等关键过程处理方法。具体包括如下步骤：图像特征点提取、序列图像匹配、关键帧选择、从同名特征点解算运动参数、三维重建。

1，图像特征点提取包括特征点检测和特征点向量描述。

特征点检测原理：特征点是连续图像中存在的特殊图像点，通过特征点可以将序列图像关联起来，根据不同图像同名特征点内在的约束关系可实现运动成像的几何关系解算。

特征点检测方法：逐帧进行特征点检测提取，对每帧中每个像素点进行考察，选择满足判定条件的像素点为特征点，其判定标准为像素点的周围邻域内是否存在足够多的像素与该像素点处于不同的灰度区间，计算公式如下：

其中N_P为以候选点为中心，以特定尺度半径的圆形区域的综合兴趣值函数，I(x)为圆周上像素点的图像灰度强度值，I(p)为候选点位置的图像灰度强度值，若候选点P的兴趣值N_P大于给定阈值(通常选50)则选择P为特征点。

特征点向量描述：为了实现不同图像上的特征点之间的同名配准，需要建立每个特征点的特征描述，本实施方式中的特征描述采用了具有旋转不变性的描述方法。该方法可以适应图像旋转，特征矢量时使用积分图像，可以提高计算图像梯度的效率。建立特征向量描述主要过程为统计所在区域的主方向，之后将邻域所有像素点相对于主方向的图像强度的分布多维向量组合起来获得。具体步骤包括：特征点区域梯度大小和主方向计算、消除旋转差异、建立特征向量描述。

特征点区域梯度大小和主方向计算：

图2是根据本发明一实施方式的特征点邻域内各像素点的梯度坐标系。图中每个小箭头表示该像素点的梯度大小和方向。

图3是根据本发明一实施方式的特征点邻域内各像素点的梯度分布直方图。

主方向是特征点邻域内各点的梯度方向直方图中最大值所对应的方向。基于主方向的特征配准可以消除图像旋转对匹配造成的干扰。特征点的主方向根据一定范围邻域内所有像素的梯度方向和强度分布的统计特性来计算。将0～360°方向划分成r份(通常取36)，以r个梯度方向作为自变量，其中每个梯度方向上所有像素点的梯度大小之和作为变量形成梯度分布直方图，选择直方图的最大值对应的梯度方向θ₀作为特征点的主方向。

选择特征点邻近区域为主方向计算的统计范围，其中每个像素点(u，v)的梯度大小m和方向θ的公式如下。

θ＝arctan((I_u,v+1-I_u,v-1)/(I_u+1,v-I_u-1,v))

消除旋转差异：以特征点为中心，将特征点邻近区域内所有像素点整体进行旋转，使旋转后的主方向与图1所示的梯度坐标系的x方向坐标轴方向平行，以主方向为参考方向，将每个像素点的梯度方向与主方向做差作为该像素点新的梯度方向。

建立特征向量描述：以旋转后的特征点的邻近区域为对象，将特征点附近区域划分为n×n个子区域(建议n＝4)。对每个子区域的像素进行统计，以像素的梯度方向为参考变量，建立子区域的梯度强度直方图，每个子区域梯度分为8个方向，因此直方图可以表示为一个含8元素的向量。将所有子区域的所有向量组合，得到该特征点n×n×8维的向量描述，构成特征向量。为了能够抵抗图像亮度的变化，将特征向量归一化处理，方法是将向量中各分量进行相同比例的缩放使最大的分量的大小值归一化为1。图4是根据本发明一实施方式的特征向量描述。

2，序列图像匹配

序列图像匹配过程是对飞行过程实时成像序列中每相邻两帧图像进行匹配，每次当前成像完成即可开始与上一帧图像进行匹配。图像匹配在特征提取的基础上进行，通过相邻两帧图像上的特征点距离计算与比较进行初步匹配，在此基础上使用基于RANSAC的基础矩阵模型约束进行匹配验证，实现最终可靠匹配。

序列图像匹配方法的步骤包括：正向匹配点搜索、反向匹配点搜索以及双向最佳匹配确认、RANSAC模型约束。

正向匹配点搜索：从序列图像的第二帧开始，计算每帧与前一帧的图像匹配结果。匹配采用逐个特征点距离比较的方法，对图像I₁中的每个待匹配特征点f₁(i)，计算其与图像I₂中每个候选特征点f₂(j)的特征距离，将图像I₂中与f₁(i)具有最小距离的特征点f₂(k)记为正向匹配的候选同名配准点(f₁(i)，f₂(k))，并记录该最小距离，并记录特征点f₁(i)与I₂中特征点的次小距离。

特征距离比较的方法使用欧氏距离进行计算，设V和V′为待匹配两图中特征点的不变性向量描述，v_i和v_i′是其向量的分量，则两者的距离d公式如下：

如果最小距离与次小距离的比值大于设定阈值Rd_th(一般取0.5)，则确认初步同名匹配(f₁(i)，f₂(k))，公式如下：

反向匹配点搜索以及双向最佳匹配确认：

进行反向匹配点搜索：对图像I₂中的每个待匹配特征点f₂(j)，计算其与图像I₁中每个候选特征点的距离，将图像I₁中与f₂(j)具有最小距离的特征点f₁(k)记为反向匹配的候选同名配准点(f₂(j)，f₁(k))，并记录该最小距离，并记录f₂(j)与图像I₁中特征点的次小距离。按照正向匹配点计算的方法，最终确认反向初步同名匹配(f₂(j)，f₁(k))。

在正向初步匹配结果与反向初步的初步匹配完成后，判断这两个方向匹配结果的一致性，即考察正向匹配结果(f₁(i)，f₂(k))和反向匹配结果(f₂(j)，f₁(k))的对应序号关系，若i＝k，则暂定两个匹配结果为正确同名配准点，否则判断为错误匹配并放弃。

RANSAC模型约束匹配过滤：采用相邻两帧图像之间的基础矩阵F作为约束模型，对上述初步匹配的同名特征点进行模型约束，剔除其中的错误匹配结果。使用基础矩阵F约束模型的RANSAC过滤过程通过迭代匹配点求解F矩阵，并考虑所有匹配点对F矩阵的支持程度来完成，具体步骤如下：

在保证取点距离大于一定阈值(通常取20像素)情况下随机选取8对参与计算的匹配点；

基于8点算法解算基础矩阵F，根据匹配点之间关系X′^TFX＝0，用矢量f表示F矩阵的元素，则每一个匹配点对应的求解F的方程写为(x′x,x′y,x′,y′x,y′y,y′,x,y,1)f＝0，由8组以上匹配点组成线性方程组，估计基础矩阵F；

计算所有匹配点与模型之间的距离d_f，其中d_f＝X′^TFX，距离d_f小于距离阈值的匹配点即作为内点；

重复步骤m次，记录每次的解算结果以及相应的内点；

选择内点数最多的结果对应的点集合，使用所有内点重新估计F矩阵作为输出结果，所有内点为最终匹配结果。

3，关键帧选择

根据相邻两帧图像间特征点最终匹配结果计算平均视差d_m，公式如下：

根据相邻两帧序列图像平均速度v_m和间隔时间t_m计算相邻两帧序列图像的相对距离D_m，公式为D_m＝v_m×t_m，如果d_m大于一定阈值且D_m大于0.08h_m，则判断该图像是关键帧图像，参与三维结构的求解计算。其中h_m是飞行高度。

4，从同名特征点解算运动参数

根据每两个关键帧图像间的基础矩阵F计算本质矩阵E，计算关系式为E＝K^TFK′，再根据本质矩阵与运动参数的关系式E＝T×R，通过分解方法对本质矩阵进行SVD分解，分解结果提供了用于计算第二个关键帧成像位置姿态相对第一个关键帧位置姿态的相对运动参数R和T，其中R表示第二次成像相对于第一次成像的旋转矩阵，T＝[t₁，t₂，t₃]表示第二次成像相对于第一次成像的平移向量。在该分解结果的运动参数中位移量T是相对值，其实际值与真实平移值相差一个比例系数。

通过下述方法计算位移量T的绝对值。记飞行速度v和两次成像的时间间隔t，则运动距离的绝对值为T_a＝vt，将上述T用T_a代替，即可获得第二关健帧相对第一关键帧的运动参数。其中第一关键帧的相对运动参数R₁为单位矩阵，T₁为0矩阵。

使用上述方法，从第二个序列关键帧图像开始，解算每个关键帧之间的相对运动参数。

5，三维重建

三维重建原理：飞行过程成像覆盖范围的地形地貌在相邻的关键帧图像中具有足够的重叠度，将连续的关键帧图像中相邻的两两关键帧构成立体的图像对，进行整个地形的三维重建。

对任意两个连续关键帧图像组成的立体图像对，计算每幅图像的成像投影矩阵Q，公式如下：

基于上述成像关系，则结合同名特征点在连续两关键帧图像上的成像，可求解得到同名特征点的空间三维坐标，公式如下：

其中，Q是关键帧中的成像投影矩阵，f_u、f_v分别为成像设备焦距在像面u、v方向的焦距，u₀、v₀为成像设备主点在像面U、V方向的分量，X为特征点在飞行器本体姿态坐标系下的空间三维坐标矩阵，x_w、y_w、z_w为X的分量，X₁、X₂为相邻两个关键帧中的同名特征点在相应关键帧上的齐次坐标，Q₁、Q₂分别为相邻两个关键帧的投影矩阵，X为特征点在飞行器本体姿态坐标系下的空间三维坐标矩阵。

基于上述原理，对整个序列图像关键帧的空间点三维重建过程步骤如下：

对第1、2关键帧构成的立体图像对进行特征点三维重建，首先计算第2关键帧相对于第1关键帧的运动参数R_2,1和T_2,1。以第1关键帧作为参考帧，其姿态矩阵为R₁单位矩阵，位置矩阵T₁为零矩阵。第2关键帧的位置姿态参数为R₂＝R₁×R_2,1，T₂＝R₁×T_2,1+T₁，将这两帧图像的位置姿态参数带入上述三维重建步骤所述的计算公式中可求解同名特征点的空间三维坐标；

对第2、3关键帧构成的立体图像对进行特征点三维重建，第3关键帧相对于第2关键帧的运动参数R_3,2和T_3,2。以第2关键帧作为参考帧，其姿态矩阵为R₂，位置矩阵为T₂。第3关键帧的位置姿态参数为R₃＝R₂×R_3,2，T₃＝R₂×T_3,2+T₂将这两帧图像的位置姿态参数带入三维重建步骤所述的计算公式中可求解同名特征点的空间三维坐标；

从第3关键帧开始的持续对后续第N关键帧和N+1关键帧图像构成的立体图像对进行特征点三维重建，其中第N关键帧图像位置姿态参数可根据上次计算获得，第N+1关键帧图像的位姿参数为R_N+1＝R_N×R_N+1,N，T_N+1＝R_N×T_N+1,N+T_N。将这两帧图像的位置姿态参数带入三维重建步骤所述的计算公式中可求解同名特征点的空间三维坐标；

以关键帧图像成像的位资参数、同名特征点的空间三维坐标作为输入，通过特征点在关键帧图像上重投影误差最小化的方法，对每个关键帧的位置姿态参数进行优化求解，同时获取最优配置条件下的所有特征点的三维坐标，从而构成重建地形的三维结构；

在每两个关键帧图像之间以第一步的特征点作为种子点，对种子点区域内的点进行仿射变换计算初始匹配作为引导，采用最小二乘方法进行密集点图像匹配；

基于密集匹配结果，利用优化后的相机运动位置姿态参数和相机内参数，通过空间前方交会的方法进行密集点三维位置计算，得到表面密集的三维地形。

三、三维地形与基准地形匹配13

1，实时三维地形生成

经过上述三维重建可以得到在飞行器本体姿态坐标系下的密集点云三维数据，为实现飞行器定位还需要得到与参考坐标系相平行的飞行器原点坐标系的数据，称为飞行器平行坐标系。根据飞行的惯性导航信息，设空间点从飞行器本体姿态坐标系到平行坐标系旋转转换矩阵为R_b-p，即飞行器本体坐标系与参考坐标系之间的转换关系，该条件下将本体姿态坐标系点云P^b变换到与基准地形平行的飞行器平行坐标系下的点云P^P，转换关系为P^P＝R_b-pP^b。

在此飞行器平行坐标系下，以坐标系的XY平面上的等距离位置为基准，对三维点云进行插值，获得XY平面上标准格网分布的三维数据表示的重建实时地形数据I^pr，数据可以表示为高程相对位置的函数值集合h(x^pr,y^pr)。

2，根据三维地形的匹配定位

在飞行器平行坐标系下的航迹实时地形的三维重建结果h(x^pr,y^pr)可直接与基准地形图h_m(x^b,y^b)进行匹配，提供导航定位信息。基准地形图是航迹区域各地面点地形高度h关于其绝对位置(x,y)的函数，以离散点的形式存储。

采用归一化互相关(COR)方法进行的地形匹配，在基准图初始位置附近各点计算与实时地形图的相关系数，并将相关系数做拟合精确定位。在实时地形图上匹配模板取沿航迹方向长L，宽W的范围。在(x,y)位置的相关系数计算公式为：

式中LW表示模板区域，h_t(i,j)、h_m(i,j)分别为实时图和基准图相对应的LW区域内(i,j)位置的地形高程数据，为实时图和基准图相对应的LW区域的地形高程均值。根据机载惯性导航系统的误差范围确定匹配搜索区域。

基准地形匹配具体实现分为以下三个步骤：

初始化：选择合适的实时图模板尺度，包括航带方向长度L和宽度W，建立匹配模板点列，根据前次匹配定位提供的引导信息(第一次匹配定位引导信息可以通过初始化装配参数获得)，确定实时地形图在基准图的初始位置(x₀,y₀)；

搜索：在初始位置附近搜索，计算各点与实时图的COR相关系数，确定相关系数极大值对应位置。

拟合精确定位：在基准图上相关系数极大值对应位置附近3×3像素范围内每个点计算实时图与及基准图的匹配相关系数，将相关系数相对于二维位置用二次曲线拟合，选择二次曲线极值点对应的位置(x_m,y_m)确定为该时刻实时图模板在基准图上的精确位置。

四、反向求解飞行器位置14

设在上述实时图匹配的模板中心位置在飞行器平行坐标系下的空间坐标为P(x_t0,y_t0,z_t0)，该点在基准图上的精确匹配位置为(x_m,y_m)，则可确定飞行器的在基准图上定位结果为(x_m-x_t0,y_m-y_t0,-z₀)。

本发明提供的基于从运动恢复结构的飞行器位置确定方法可以低成本低能耗精准地测量飞行器的位置。具有成本低能耗小、设备便携、不依赖GPS受外界信号干扰小、不受地形限制等优点。

前述对本发明的具体示例性实施方案的描述是为了说明和例证的目的。这些描述并非想将本发明限定为所公开的精确形式，并且很显然，根据上述教导，可以进行很多改变和变化。对示例性实施例进行选择和描述的目的在于解释本发明的特定原理及其实际应用，从而使得本领域的技术人员能够实现并利用本发明的各种不同的示例性实施方案以及各种不同的选择和改变。本发明的范围意在由权利要求书及其等同形式所限定。

Claims (10)

1.一种基于从运动恢复结构的飞行器位置确定方法，所述飞行器具有惯性导航设备和成像设备，其特征在于，基于从运动恢复结构的飞行器位置确定方法包括以下步骤：

利用所述成像设备对航迹下地形连续成像，以获取运动序列图像；

基于所述运动序列图像恢复实时三维地形图并获取实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标，所述飞行器本体姿态坐标系是基于飞行器飞行时的姿态而建立的坐标系；

将所述实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标转化到飞行器平行坐标系下，所述飞行器平行坐标系是与地面基准地形坐标系相平行的飞行器原点坐标系；

将所述实时三维地形图上各点在飞行器平行坐标系下的空间三维坐标转化到所述地面基准地形坐标系下，以及

反向求解飞行器在所述地面基准地形坐标系下的位置。

2.根据权利要求1所述的基于从运动恢复结构的飞行器位置确定方法，其特征在于，所述基于运动序列图像恢复实时三维地形结构的包括以下步骤：

图像特征点提取；

序列图像匹配；

根据序列图像视差与飞行速度进行关键帧选择；

从关键帧同名特征点解算运动参数；以及

实时三维地形重建。

3.根据权利要求2所述的基于从运动恢复结构的飞行器位置确定方法，其特征在于，所述序列图像匹配包括以下步骤：

进行正向匹配点搜索，包括以下内容：从所述序列图像的第二帧开始，计算每帧与前一帧的图像匹配结果，采用逐个特征点距离比较的方法，对图像I₁中的每个待匹配特征点f₁(i)，计算其与图像I₂中每个候选特征点f₂(j)的特征距离，将图像I₂中与f₁(i)具有正向最小距离的特征点f₂(k)记为正向匹配的候选同名配准点(f₁(i)，f₂(k))，并记录所述正向最小距离以及记录特征点f₁(i)与I₂中特征点的正向次小距离，所述距离d公式如下：

其中V和V′为待匹配两图中特征点的不变性向量描述，v_i和v_i′是其向量的分量，如果最小距离与次小距离的比值大于设定阈值Rd_th，则确认初步同名特征点(f₁(i)，f₂(k))；

进行反向匹配点搜索以及双向最佳匹配从而确定候选的同名特征点，包括以下内容：对图像I₂中的每个待匹配特征点f₂(j)，计算其与图像I₁中每个候选特征点的距离，将图像I₁中与f₂(j)具有反向最小距离的特征点f₁(k)记为反向匹配的候选同名配准点(f₂(j)，f₁(k))，并记录所述反向最小距离以及记录f₂(j)与图像I₁中特征点的反向次小距离，按照所述正向匹配点计算的方法，最终确认反向初步同名匹配(f₂(j)，f₁(k))，然后判断这两个方向匹配结果的一致性，若i＝k，则暂定两个匹配结果为正确同名匹配点，否则判断为错误匹配并放弃；以及

进行RANSAC模型约束从而将所述候选的同名特征点中选择出正确的同名特征点，包括以下内容：在保证取点距离大于一定阈值情况下随机选取相邻两帧序列图像中8对参与计算的匹配点；基于8点算法解算的基础矩阵F与匹配点之间关系X′^TFX＝0，其中X为图像I₁中的待匹配点的向量矩阵，X′为图像I₂中的待匹配点的向量矩阵，x、y为X向量的分量，x′、y′为X′向量的分量，用矢量f表示F矩阵的元素，则每一个匹配点对应的求解F的方程写为(x′x,x′y,x′,y′x,y′y,y′,x,y,1)f＝0，由8组匹配点组成线性方程组，估计基础矩阵F；计算所有匹配点与RANSAC模型的基础矩阵F之间的距离d_f，其中d_f＝X′^TFX，距离d_f小于一定阈值的匹配点即作为内点；重复步骤m次，记录每次的解算结果以及相应的内点；以及选择内点数N最多的结果对应的点集合，使用所有内点重新估计F矩阵作为输出结果，所有内点为最终正确匹配的同名特征点。

4.根据权利要求3所述的基于从运动恢复结构的飞行器位置确定方法，其特征在于，根据序列图像视差与飞行速度进行关键帧选择包括以下步骤：

根据所述特征点最终匹配结果计算平均视差d_m，公式如下：

以及

根据相邻两帧序列图像平均速度v_m和间隔时间t_m计算相邻两帧序列图像的相对距离D_m，公式为D_m＝v_m×t_m，如果d_m大于一定阈值且D_m大于0.08h_m，则判断该图像是关键帧图像，其中h_m是飞行高度，根据前一次位置计算结果得到，而第一次确定方法为，首先在d_m大于一定阈值条件下，由当参与计算的两帧图像的图像中心点进行三维交会，根据该点到相机光心的距离来确定。

5.根据权利要求2所述的基于从运动恢复结构的飞行器位置确定方法，其特征在于，从关键帧同名特征点解算运动参数包括以下步骤：

根据每两个相邻关键帧图像间的基础矩阵F计算本质矩阵E，计算关系式为E＝K^TFK′，其中K为关键帧K₁中的待匹配点的向量矩阵，K′为关键帧K₂中的待匹配点的向量矩阵；

根据本质矩阵E与运动参数的关系式E＝T×R，通过对本质矩阵进行分解得出相邻两个关键帧后一帧相对前一帧的相对运动参数R_x和T_x，其中R表示姿态矩阵，T表示平移向量，R_x表示姿态矩阵相对值，T_x表示平移向量相对值；

计算平移距离的绝对值T_a，公式为T_a＝vt，其中v为飞行速度，t为两次成像的时间间隔，两者都是所述成像设备中的参数；以及

将T_a带入E＝T×R，确定相邻两个关键帧后者位置姿态相对前者位置姿态的相对运动参数R_a。

6.根据权利要求2所述的基于从运动恢复结构的飞行器位置确定方法，其特征在于，基于所述运动序列图像恢复实时三维地形图的包括以下步骤：

求解所述每个关键帧中特征点在飞行器本体姿态坐标系下的空间三维坐标；

通过所述关键帧中特征点在关键帧图像上重投影误差最小化的方法，对每个关键帧的位置姿态参数进行优化求解，获取最优配置条件下的所有特征点的三维坐标，从而构成重建地形的三维结构；

相邻两个关键帧图像之间以第一步的特征点作为种子点，对种子点区域内的点进行仿射变换计算初始匹配作为引导，采用最小二乘方法进行密集点图像匹配；以及

通过空间前方交会的方法进行所述密集点三维位置计算，得到表面密集的实时三维地形，

其中，求解所述每个关键帧中特征点在飞行器本体姿态坐标系下的空间三维坐标的公式如下：

其中，Q是关键帧中的成像投影矩阵，f_u、f_v分别为成像设备焦距在像面u、v方向的焦距，u₀、v₀为成像设备主点在像面U、V方向的分量，X为特征点在飞行器本体姿态坐标系下的空间三维坐标矩阵，x_w、y_w、z_w为X的分量，X₁、X₂为相邻两个关键帧中的同名特征点在相应关键帧上的齐次坐标，Q₁、Q₂分别为相邻两个关键帧的投影矩阵，X为特征点在飞行器本体姿态坐标系下的空间三维坐标矩阵。

7.根据权利要求1所述的基于从运动恢复结构的飞行器位置确定方法，其特征在于，将所述实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标转化到飞行器平行坐标系下包括以下步骤：

在所述飞行器平行坐标系下，以XY平面上的等距离位置进行插值，获得XY平面上标准格网分布的三维数据I_pr，所述数据I_pr用高程相对位置的函数值集合h(x_pr,y_pr)来表示。

8.根据权利要求1所述的基于从运动恢复结构的飞行器位置确定方法，其特征在于，将所述实时三维地形图上各点在飞行器本体姿态坐标系下的空间三维坐标转化到飞行器平行坐标系下包括以下步骤：

根据所述惯性导航设备的信息确定空间点从飞行器本体姿态坐标系到平行坐标系的旋转变换矩阵为R_b-p，飞行器本体姿态坐标系点云P^b变换到与飞行器平行坐标系下的点云P^P的关系为P^P＝R_b-pP^b。

9.根据权利要求1所述的基于从运动恢复结构的飞行器位置确定方法，其特征在于，将所述实时三维地形图上各点在飞行器平行坐标系下的空间三维坐标转化到所述地面基准地形坐标系下的包括以下步骤；

选择合适的实时三维地形图模板尺度，包括航带方向长度L和宽度W，建立匹配模板点列，确定所述飞行器平行坐标系下的实时三维地形图在所述地面基准地形图的初始位置(x₀,y₀)；

计算所述初始位置附近各点与所述飞行器平行坐标系下的实时三维地形图的相关系数，在所述匹配模板点列上，某点(x,y)的相关系数计算公式为：

其中LW表示模板区域，h_t(i,j)、h_m(i,j)分别为所述飞行器平行坐标系下的实时三维地形图和地面地形基准图相对应的LW区域内(i,j)位置的地形高程数据，为所述飞行器平行坐标系下的实时三维地形图和地面地形基准图相对应的LW区域的地形高程均值；

确定所述相关系数极大值对应位置；以及

将地面地形基准图上的所述相关系数极大值对应位置附近3×3像素单元范围内的每个点的相关系数相对于二维位置用二次曲线拟合，选择二次曲线极值点对应的位置确定为该时刻飞行器平行坐标系下的实时三维地形图模板在地面地形基准图上的精确位置。

10.根据权利要求9所述的基于从运动恢复结构的飞行器位置确定方法，其特征在于，所述反向求解飞行器位置的方法为：若所述LW模板中心在飞行器平行坐标系下的空间坐标为P(x_t0,y_t0,z_t0)且所述LW模板中心在所属地面基准地形图上的匹配点的坐标为(x_m,y_m)，则确定飞行器在所述地面地形基准图上定位结果为(x_m-x_t0,y_m-y_t0,-z₀)。