基于序列图像和基准图匹配的飞行器全参数导航方法

摘要

本发明涉及基于序列图像和基准图匹配的飞行器全参数导航方法。该方法在飞行器上安装摄像机、惯导系统、数据处理器件和存储器件，且数据存储器上存储有预定区域的地面基准图数据，构成基于景象匹配的飞行器全参数导航系统。通过实时图像与基准图的匹配，并与惯导进行组合导航的卡尔曼滤波，即可实现飞行器导航的高精度全参数测量，具有广泛、重要的应用前景。

说明书

技术领域

本发明涉及飞行器自主导航以及摄像测量技术领域，进一步指利用飞 行器自身携带摄像机、惯导系统和数据处理器件，对自身的位置、姿 态、速度和姿态角速度等进行全参数测量。

背景技术

飞行器导航为飞行器提供实时位置与姿态等信息，对执行飞行任务至 关重要。飞行器导航通常要求导航系统实时给出飞行器的12个运动参 数，包括飞行器运动的6个零次量（三维位置、姿态）、6个一次量（ 三个方向速度分量和姿态角速度分量）。目前能够提供飞行器全参数 测量的方法有GPS、惯性导航等。GPS信号易受干扰和钳制；惯性导航 短期精度较高，但导航误差随时间累积。在这样的背景下，研究一种 独立工作、高可靠性、高精度、体积小、重量轻、基于视觉的飞行器 全参数导航系统对无人机等小型航空飞行器发展具有重要意义。

景象匹配导航技术通过将采集到的实时图与飞行器上预存带有大地坐 标的地面基准图进行匹配，仅能得到导航需要的水平位置信息。目前 飞行器视觉导航主要用于相对位姿测量以及二维定位，未见国内外有 利用实时图与基准图匹配得到大地绝对坐标系下飞行器导航全参数的 视觉导航研究。本发明实现了一种基于单目摄像机采集序列实时图与 基准图匹配的飞行器全参数导航方法，通过实时图上多个特征点与基 准图匹配，得到特征点地面三维坐标，并利用摄像机内参求解成像模 型，得到飞行器在大地绝对坐标系下的位置和姿态等参数，进一步与 惯导进行组合导航得到精确的飞行器速度和姿态角速度等参数。方法 所需设备简单，完全依赖飞行器上现有装备得到飞行器全参数测量结 果，测量实时性好，精度高，具有很好的应用前景。

发明内容

本发明是：在飞行器（航空器）上装备一台摄像机、惯导系统、数据 处理器件（微机、DSP）和存储器件（硬盘等存储介质）（已存储基准 图数据），构成基于序列图像和基准图匹配的飞行器全参数视觉导航 系统。上述硬件若飞行器上已装备，则无需重新安装。该系统组成如 附图1所示。系统开始工作时，摄像机实时对地面进行成像，之后采用 本发明的基于景象匹配的飞行器全参数导航方法完成飞行器全参数的 测量。

1 本发明的基于景象匹配的飞行器全参数导航的实施过程是：

1) 在飞行器上安装摄像机、惯导系统、数据处理器件和存储器件等 ，且数据存储器上存储有预定区域的地面基准图数据（通常为卫星图 片），构成基于景象匹配的飞行器全参数导航系统。

2) 飞行器进入预定区域后，开启基于景象匹配的飞行器全参数导航 系统，摄像机对地面进行连续拍摄，采用本发明基于景象匹配的飞行 器全参数导航方法，即飞行器位姿参数估计方法或飞行器全参数测量 的组合导航方法，完成飞行器全参数的测量，具体按以下步骤进行：

2.1) 在每帧实时图像上选取多个特征点，与带有大地水平坐标和高 程的基准图进行匹配，得到特征点在大地绝对坐标系下的三维坐标。

2.2) 利用匹配得到特征点图像坐标和三维大地绝对坐标，以及已知 的摄像机内参数（像主点、等效焦距）和安装角，求解摄像机成像模 型，得到飞行器三维位置和姿态参数。

2.3) 将解算出的飞行器位姿参数拟合出飞行器的速度、加速度、角 速度角加速度等导航参数，或者将解算出的飞行器位姿参数和惯导进 行组合导航，得到高精度的飞行器全参数导航测量结果。

2 本发明的基于景象匹配的飞行器全参数导航方法

上述过程步骤中，图像特征点选取以及与基准图的匹配，有多种公知 的方法，具体可参照已有技术的摄像测量、摄影测量、计算机视觉的 相关内容。而根据序列实时图多点匹配结果进行飞行器导航的全参数 测量是本发明的发明点，其基本原理和方法是：

1) 基于景象匹配的飞行器全参数导航基本原理

其基本原理如附图2、3所示。通过在实时图上选取多个特征点，与基 准图匹配得到特征点对应的三维大地绝对坐标，并利用已知摄像机内 参（像主点、等效焦距）和安装角，求解摄像机成像模型，获得飞行 器三维位置和姿态参数。然后将飞行器位姿参数测量结果与惯导进行 组合导航，得到高精度的飞行器导航的全参数测量结果。组合导航有 两种方法，一种假设惯导速度误差和角速度误差在短时间内为常值， 利用解算出的飞行器位姿参数构建误差修正方程进行组合导航；一种 利用惯导误差状态方程，与解算出的飞行器位姿参数进行卡尔曼滤波 完成组合导航。

2) 基于景象匹配的飞行器全参数导航方法

2.1) 飞行器位姿参数估计方法

在实时图上选取多个特征点与基准图匹配，得到特征点图像坐标以及 对应的三维大 地绝对坐标。根据摄像测量学知识，设地面上有n个点，其大地绝对坐 标为(X_i,Y_i,Z_i)，图像坐标为(x_i,y_i)，F_x、F_y为摄像机等效焦距，C _x、C_y为摄像机像主点坐标，(X_c,Y_c,Z_c)为摄像机光心在大地绝对坐标 系中的位置坐标。由中心投影成像模型，建立以下方程：

$\left(\begin{array}{c}\frac{{\tilde{x}}_i-C_x}{F_x}=-\frac{a_1(X_i-X_c)+b_1(Y_i-Y_c)+c_1(Z_i-Z_c)}{a_3(X_i-X_c)+b_3(Y_i-Y_c)+c_3(Z_i-Z_c)}\\ \frac{{\tilde{y}}_i-C_y}{F_y}=-\frac{a_2(X_i-X_c)+b_2(Y_i-Y_c)+c_2(Z_i-Z_c)}{a_3(X_i-X_c)+b_3(Y_i-Y_c)+c_3(Z_i-Z_c)}\end{array}\right)---\left(1\right)$

其中，大地绝对坐标系到摄像机坐标系的旋转矩阵为为R，则R可由飞 行器姿态角以及摄像机安装角表示，如下：

$R=\left(\begin{array}{ccc}{a}_1& b_1& c_1\\ a_2& b_2& c_2\\ a_3& b_3& c_3\end{array}\right)---\left(2\right)$

当有至少三个对应特征点对时，根据以上成像方程可以利用非线性迭 代最优化的方法求解得到飞行器三个位置量和三个姿态角。

2.2) 飞行器全参数测量的组合导航方法

方法一：

假设短时间内惯导速度误差为恒定值，并以X方向为例说明组合导航方 法。设在t₁到t₂的一小段时间段内任意时刻t_k，惯导位置、姿态和飞 行器位姿参数解算出的位置、姿态分别为、和、，惯导速度误差 和角速度误差为常值，分别用δv和δω表示，并设和，那么

$\left(\begin{array}{c}{\mathrm{\Delta x}}_k+v_k={\mathrm{\Delta x}}_1+\mathrm{\delta v}(t_k-t_1)\\ {\mathrm{\Delta \phi }}_k+w_k={\mathrm{\Delta \phi }}_1+\mathrm{\delta \omega }(t_k-t_1)\end{array}\right)---\left(3\right)$

其中v和w表示飞行器位姿估计中的位置和姿态解算误差。

根据某一小段时间内由序列图像解算出的飞行器位姿参数结果以及惯 导输出，联立式（3）所示的线性方程组，使用最小二乘法对该时间段 内的初始位置误差Δx₁、初始姿态误差Δφ₁、速度误差δv和角速度 误差δω进行估计。并利用下式完成组合导航修正，得到高精度飞行 器全参数导航结果。

$\left(\begin{array}{c}{x}_k=x_k^I+{\mathrm{\Delta x}}_1+\mathrm{\delta v}(t_k-t_1)\\ v_k=v_k^I+\mathrm{\delta v}\\ {\phi }_k={\phi }_k^I+{\mathrm{\Delta \phi }}_1+\mathrm{\delta \omega }(t_k-t_1)\\ {\omega }_k={\omega }_k^I+\mathrm{\delta \omega }\end{array}\right)---\left(4\right)$

方法二：

考虑姿态角误差、速度误差、位置误差、陀螺仪测量误差、加速度计 测量误差，由惯导误差方程得到组合导航的误差状态方程如下：

$X_k=F_{k-1}{X}_{k-1}+G_{k-1}{W}_{k-1}---\left(5\right)$

其中：

$X={\left(\begin{array}{ccccccccccccccc}{\mathrm{\delta \phi }}_E& {\mathrm{\delta \phi }}_N& {\mathrm{\delta \phi }}_U& {\mathrm{\delta v}}_E& {\mathrm{\delta v}}_N& {\mathrm{\delta v}}_U& {\mathrm{\delta x}}_E& {\mathrm{\delta x}}_N& {\mathrm{\delta x}}_U& {ϵ}_x& {ϵ}_y& {ϵ}_z& {▿}_x& {▿}_y& {▿}_z\end{array}\right)}^T$

$W={\left(\begin{array}{cccccc}{w}_{\mathrm{gx}}& w_{\mathrm{gy}}& w_{\mathrm{gz}}& w_{\mathrm{ax}}& w_{\mathrm{ay}}& w_{\mathrm{az}}\end{array}\right)}^T$

式中φ表示姿态角；v表示飞行器速度；x表示飞行器位置；ε表示陀 螺仪测量误差；表示加速度计测量误差；下标E、N、U分别表示直角 坐标系的东向、北向和天向，下标g表示陀螺仪，下标a表示加速度计 。

以解算出的飞行器位姿参数与惯导输出的位姿参数之差作为量测值， 构建组合导航的量测方程。

$Z_k=H_k{X}_k+v_k---\left(6\right)$

其中：

$Z={\left(\begin{array}{cccccc}{\phi }_{\mathrm{IE}}-{\phi }_{\mathrm{VE}}& {\phi }_{\mathrm{IN}}-{\phi }_{\mathrm{VN}}& {\phi }_{\mathrm{IU}}-{\phi }_{\mathrm{VU}}& x_{\mathrm{IE}}-x_{\mathrm{VE}}& x_{\mathrm{IN}}-x_{\mathrm{VN}}& x_{\mathrm{IU}}-x_{\mathrm{VU}}\end{array}\right)}^T$

式中下标I表示惯性导航输出；下标V表示基于景象匹配的视觉导航输 出。基于式（5）、（6）的组合导航卡尔曼滤波为公知方法，可参考 相关文献。根据组合导航的滤波输出，得到高精度的飞行器导航的全 参数测量结果如下：

$Y=\left(\begin{array}{c}{\omega }_I-{ϵ}_g\\ {\phi }_I-\mathrm{\delta \phi }\\ v_I-\mathrm{\delta v}\\ x_I-\mathrm{\delta x}\end{array}\right)---\left(7\right)$

其中：

$Y=\left(\begin{array}{cccc}\omega & \phi & v& x\end{array}\right)$

式中ω表示飞行器姿态角速度。

3 本发明可以达到以下的技术效果：

本发明仅利用飞行器上常规装备的摄像机（可见光、红外摄像机）、 惯导系统、数据处理器件（微机、DSP）和存储器件（硬盘等存储介质 ），不需要额外增加载荷，通过实时图像与基准图的匹配，并与惯导 进行组合导航的卡尔曼滤波，即可实现飞行器导航的高精度全参数测 量，具有广泛、重要的应用前景。

附图说明

图1 基于序列图像和基准图匹配的飞行器全参数导航系统组成图；

图2 基于序列图像和基准图匹配飞行器位姿解算方法基本原理图；

图3 飞行器全参数导航的组合导航原理框图。

具体实施方式

如图1，基于景象匹配的飞行器全参数导航系统由安装在飞行器上的一 台摄像机、惯导系统、数据处理器件（微机、DSP）和存储器件（硬盘 等存储介质）构成。其中摄像机用于对地面实时成像，惯导系统用于 提供匹配预测以及组合导航的输入，数据处理器件用于图像匹配、飞 行器位姿解算以及组合导航的卡尔曼滤波计算，存储器件用于存储图 像和计算结果数据，包括预先存储的带有大地水平坐标和高程的基准 图。

如图2，在实时图上选取多个特征点，与基准图匹配得到特征点的三维 大地绝对坐标，利用已知的摄像机内参和安装角，求解成像模型，即 可得到飞行器位置和姿态。

如图3，方法一：假设某一小段时间内惯导速度误差和角速度误差为常 值，并据此构建线性误差方程，利用飞行器位姿估计的测量值，解出 惯导位置、速度误差和姿态、角速度误差，并进一步进行误差补偿完 成组合导航。

方法二：以惯导输出的飞行器位姿参数与解算出的飞行器位姿参数之 差作为量测值，构建组合导航量测方程；以姿态角误差、速度误差、 位置误差、陀螺仪测量误差、加速度计测量误差作为状态量，构建组 合导航误差状态方程，并进行卡尔曼滤波完成误差估计，进一步得到 高精度的飞行器导航的全参数测量结果。