孙 伟，苗宏胜，李海军，徐海刚，裴玉锋

（北京自动化控制设备研究所，北京100074）

随着飞行器长时间、远程航行需求的不断增强，对导航系统的精度和自主性要求不断提高。纯惯性导航系统的导航误差随时间积累而不断增大，不能完全满足实际应用的需要。惯性/视觉里程计组合导航可在不提高惯性元件精度的情况下，仅通过增加视觉传感设备，获得载体运动的速度，提升导航系统的导航精度，节约成本。

视觉里程计技术的基本内涵为：单目相机通过前后帧图像的差异，在已知距离的情况下可以测量出载体的速度[1-4]。而双目视觉里程计技术利用双目相机，通过左右两个相机图像差异测出距离，通过前后帧图像的差异测量出载体的速度，但是如果飞行器的高度越高，双目视觉的测距精度越低，因此视觉里程计应用于飞行器上时，需要利用其他手段获得距离信息。

视觉里程计的图像匹配算法分为两大类[5-8]：基于区域的算法和基于特征的算法。基于特征的匹配算法在尺度上具有一定的不变性，对相机的姿态要求不很高。但在实时图与基准图差异较大的情况下会产生大量的误匹配点，或因匹配点数过少而无法对匹配结果进行判断。基于区域的匹配算法包括灰度相关算法、相位相关算法等，在这类算法中，图像的像素点阵直接参与匹配运算，灰度相关算法参与匹配的是图像灰度强度点阵，并可计算出图像匹配的置信度，由置信度来对匹配结果进行判断。

传统图像匹配置信度研究存在缺陷，使用单一特征值来衡量匹配置信度，极有可能出现误报；使用多个特征值来衡量匹配置信度，可能出现相互矛盾的情况。

传统的视觉里程计技术存在缺陷，受相机参数、测量距离的范围等影响，视觉里程计输出的速度信息与真实值将会存在一定的差异；并且，传统视觉里程技术在实时性和精确性方面难以同时满足组合导航的需求。

本文针对视觉里程计的技术特点，在进行图像匹配的过程中，使用衡量匹配置信度的两个特征值，基于证据推理理论，将两个特征值进行融合计算，获得综合置信度，在减少误报概率的同时增强置信度计算的准确性；在惯性/视觉里程计深组合导航过程中，利用惯性信息辅助提升视觉里程计计算的实时性和准确性，并对视觉里程计的误差进行深入建模与分析，利用卡尔曼滤波器对惯性、视觉里程计的误差进行精确估计，实现高精度组合导航。

1 图像匹配算法与特征值分析

1.1 归一化积相关亚像素匹配算法

1.1.1 归一化积相关匹配算法

归一化积相关匹配算法的本质是衡量两幅图像相似程度的度量准则。其过程是，实时图像在基准图中从起始位置（基准图左上角）逐行扫过整个基准图；在每一个位置将实时图像中每个像素点与它所对应基准图区域像素点进行相关性运算，将结果记录在相应的位置处；最后选取相关性最高像素点处，作为匹配结果。

相关性的计算公式为：

式中，(x,y)为实时图像中的坐标点；(u,v)为基准图像中的坐标点；T(x,y)为实时图像在点(x,y)处的灰度值，范围为0 到255；为实时图像在基准图像(u,v)处的均值；I(x+u,y+v)为基准图像点(u,v)处，与实时图像相对应位置处的灰度值；为基准图像(u,v)处与实时图像相同区域的均值。

1.1.2 归一化积相关亚像素匹配

归一化积相关法得到的结果是实时图的整像素位置，实际目标位置不一定在整像素点上，将会导致亚像素级的匹配误差，为降低亚像素级的匹配误差，提出归一化积相关亚像素匹配。

由于相关函数矩阵在以最大值为中心的一个单峰值区域上通过近似满足二次多项式，因此可以通过拟合方法得到该区域相关系数的解析曲面函数，取曲面极值点为匹配结果的亚像素位置。该曲面的方程可由式(2)表达：

式中，x、y为图像平面坐标系中的横、纵坐标；f(x,y)为指定坐标点的匹配相关度。

对式(2)中x、y分别求偏导，且偏导为零时，f(x,y)取得极值，如式(3)所示：

式中，xm、ym即为求得的亚像素匹配结果。

可见，计算亚像素匹配结果转化为求取曲面参数，而曲面参数的求取可通过取以整像素最大值点为中心取3×3 的小窗口，将该窗口中所有数据作为最小二乘拟合方法的输入值，构建拟合方程即可求得曲面参数。

通过上述两个步骤，即可求得两幅图像相关性最高点，但是由于实际图像获取过程中，姿态变化、光照变化等因素，归一化积相关亚像素匹配结果会产生误匹配，需要一些特征值来评估景象匹配结果的准确性及剔除误匹配结果。

1.2 特征值算法

1.2.1 信噪比SNR

实时图和基准图的相似度是体现景象匹配系统性能的重要因素之一。信噪比SNR(Signal Noise Ratio)是用来衡量两图之间的相似程度的重要指标。

实时图是基准图累加一定噪声构成的，将基准图定义成信号，则由天气、图像采集等因素对基准图的影响就是噪声。通过大量试验数据可获得SNR与图像匹配正确率的关系，结果如图1所示。

图1 SNR 与正确率关系图Fig.1 The relationship between SNR and accurate rate

可见SNR与匹配正确率为正相关，与上述分析相符。当SNR大于0.85，匹配正确率基本为100%。

1.2.2 次高峰与最高峰相关性之比SMR

次高峰与最高峰相关性之比(Sub Max Ratio,SMR)定义为：

式中，CCSubMax表示次高峰的相关性，CCMax表示最高峰的相关性。该特征值表征的是基准图中两幅实时图同时存在的概率，越大说明越容易发生误匹配。

通过大量试验数据可获得SMR与图像匹配正确率的关系。以0.01 个SMR作为间隔进行统计，结果如图2所示。

图2 SMR 与正确率关系图Fig.2 The relationship between SMR and accurate rate

可见SMR与匹配正确率是负相关，当SMR小于0.7 时，匹配正确率为100%，说明此时实时图在基准图中是独特的。

用单一特征值评估图像匹配结果存在较大的风险，如图1中SNR大于0.85 时，正确率基本为100%，但是此时仍有可能出现误匹配；而若条件过于苛刻，会将一些正确的匹配结果判定为错误的，即误报概率较大。因此需要将特征值进行融合，以增加特征值评估的可靠性，降低误报概率。因此，本文基于证据推理理论，将两种特征值的图像匹配置信度进行数据融合，求取综合置信度。

2 基于证据推理的综合信度计算方法

证据推理理论将对特征值的单一处理转化为融合处理，它即强调特征值的客观性又称静态性，又强调了特征值对图像匹配正确率估计的主观性又称动态性，一定程度上解决了可能发生的特征值估计图像匹配正确率高度冲突的融合问题。

特征值对图像匹配正确率估计的可靠度主要来自于特征值对应的图像匹配置信度的静态权重及动态权重[9]。

静态权重作为先验信息体现了不同特征值对图像匹配正确率估计具有不同的可靠性和权威性，在本文中体现的是特征值自身的可靠性。一般认为特征值具有相同的静态权重，如有L个特征值时，每个特征值的静态权重ωi=1/L，i∈L。

动态权重指一些特征值应用于图像匹配时所表现出的相互支持度，它以非直接的方式表现了特定特征值对应的图像匹配置信度的可靠性。当某一特征值被其它特征值支持的程度大，说明该特征值具有更大的可靠度，反之亦然。采用经静态权重折扣后的特征值图像匹配置信度与加权平均图像匹配置信度的距离计算相似度、支持度和动态权重。

设SMR 特征值的序号为1，SNR 特征值的序号为2。现给出基于证据推理的融合计算步骤：

1）给定各特征值的静态权重ωi，这里设ω1=0.5,ω2=0.5。

式中，βij为每个证据ei（i=1…L）在各个命题Hj（j=1…N）下的初始信度。

3）计算动态权重si′：

式中，si=1-di，L为特征值个数。

4）依据飞行试验条件和对特征值的认知确定静态权重比例λ，并计算各特征值对应图像匹配置信度的综合权重αi：

5）用综合权重对各特征值的mass 进行折扣，并计算其加权平均值；

6）则第j次图像匹配的综合信度为βj=，j=1…N。

对图像匹配试验数据进行统计，经过上述步骤的数据融合计算，可得出综合信度与正确率的关系曲线，如图3。

图3 综合信度与正确率的关系曲线Fig.3 The relationship between apprehensive credibility and the accurate rate

可见经过数据融合，综合信度大于0.9 时，匹配正确率高于95%。

3 基于惯性信息的视觉里程计技术

3.1 基本原理

基于惯性信息的视觉里程计的基本原理是视觉里程计通过惯导获取载体姿态角、载体速度等信息，利用惯导的速度、姿态信息预测（计算）实时图在基准图中的坐标，将该坐标作为搜索区域的中心点，搜索区域的大小则由惯导的速度、姿态误差决定；利用水平姿态角信息修正由于飞行姿态问题带来的视觉里程计测速误差，提升视觉里程计测速精度；利用惯导输出的当前姿态角、速度信息，将速度信息进行积分，即可得到在两次采集图像的时间间隔内载体运动的位移，将运动位移结合姿态角信息经过坐标变换转换到图像坐标系下的像素值，利用计算出来的像素值，对下一时刻的匹配区位置进行预测，提高匹配搜索的速度及精度，增强视觉里程计的实时性和精确性。

3.2 视觉里程计实时图截取

设图像采样时间间隔为dt，惯导输出的水平载体坐标系下的dt时间内平均速度变量为θΔ，相机焦距（以像素为单位）为Fu，无线电高度表输出高度为Hw，图像沿OU 轴移动的像素值为du，图像沿OV轴移动的像素值为dv，则利用惯导输出的载体坐标系下速度信息可预测图像移动的像素值，如式(8)所示。

式中，duimu、dvimu为惯导速度信息计算得到的移动像素值；、为惯导输出的水平载体坐标系下，X、Z 轴dt时间的平均速度。

为了保证视觉里程计能够测量的速度达到最大，希望截取的实时图像在当前时刻图像中的位置坐标点与在上一时刻图像中的位置坐标点的连线的中点位于基准图的中心位置坐标。

3.3 视觉里程计搜索区预测

传统视觉里程计在图像匹配的过程中，往往将整张基准图作为匹配搜索的区域，造成系统资源的浪费以及实时性的降低，本文利用惯性信息对上一帧图像中的匹配区位置进行预测，提高匹配搜索的速度及精度，增强视觉里程计的实时性和精确性。

本方法主要给出利用惯性速度信息来确定搜索区中心坐标，其中 (us_m,vs_m)表示搜索区的中心坐标。考虑两帧图像之间的姿态角变化分别为Δφ、Δγ、Δθ，进一步推导可得搜索区中心坐标如下：

式中，（ureal_m,vreal_m）为当前帧图像中的中心坐标。

3.4 视觉里程计输出速度校正

传统的视觉里程计在计算速度时，若载体姿态角发生变化，将影响视觉里程计测速精度，且影响程度与姿态角变化幅度、相机到特征点的距离成正比。本文利用惯性的姿态角信息，对视觉里程计输出的速度信息进行高精度补偿，能够极大提升视觉里程计的测速精度。

式中，uodo_real_m,vodo_real_m为视觉里程计中实时图在当前时刻图像的中心坐标；duodo、dvodo为视觉里程计得到的在图像平面坐标系下沿OU、OV轴的像素偏移值。

获得惯性导航系统姿态角信息后，通过分析可以对视觉里程计速度进行补偿，进一步提高视觉里程计测速精度，其计算如式(11)所示：

式中，Vodo_x、Vodo_z为视觉里程计输出的水平载体坐标系下沿Ob′Xb′、Ob′Zb′速度；Fu为相机焦距（像素）；为无线电高度输出的上一滤波周期时间更新时备份的dt时间内原始高度平均值并经过惯导信息补偿后的值；为无线电高度输出的上一滤波周期时间更新时备份的dt时间内原始高度平均值；、为惯导输出的上一滤波周期时间更新时备份的滚转、俯仰角；、为惯导输出的当前时刻滤波周期时间更新时备份的滚转、俯仰角；dt为图像采样时间间隔。

3.5 视觉里程计有效性判断

本文通过计算视觉里程计卡方和视觉里程计的综合信度来对视觉里程计有效性进行判断，下面介绍这两个指标的计算方法。

3.5.1 视觉里程计置信度计算算法

经过图像匹配后，计算图像匹配的特征值——信噪比SNR、次高峰与最高峰相关性之比SMR，使用基于证据推理的综合图像匹配置信度算法计算出视觉里程计的综合信度，当综合置信度大于一定阈值后，认为图像匹配正确。

该算法仅通过图像信息来对视觉里程计有效性进行判断，进一步表明视觉里程计测速的自主性，为视觉里程计有效性判断提供了强有力的依据。

3.5.2 视觉里程计卡方计算算法

先获得组合滤波的新息值：

式中相关变量均与组合滤波相关，Z(k+1)为量测量，H(k+1)为量测矩阵，X(k+1,k)为一步状态预测值，具体的计算方法将会在后续章节进行描述。

因γ(k)包含其它观测量，故将γ(k)中与视觉里程计无关的值置零，得到γOdo(k)，将此作为视觉里程计的新息值。

则视觉里程计卡方为：

式中，PBak(k+1,k)为一步预测误差方差矩阵在时间更新时进行的备份。

当视觉里程计卡方小于某一阈值时，认为视觉里程计输出的速度可用于组合导航。该算法依赖于惯性导航信息，将惯性导航信息作为视觉里程计有效性判断的依据之一，进一步提高了视觉里程计有效性判断的可靠性。

4 惯性/视觉里程计深组合导航

4.1 惯性/视觉里程计深组合导航基本原理

本文描述的深组合导航算法如图4所示。

深组合设计思想主要体现在以下两方面：

（1）视觉里程计中，使用惯导速度和姿态信息进行实时图截取；惯导速度和姿态信息预测搜索区位置；参考惯导速度信息进行视觉里程计有效性判断；惯导姿态信息参与视觉里程计运动状态估计；

（2）惯导、视觉里程计通过组合滤波实现对惯导误差校正，且在组合滤波器中引入视觉导航误差量，实现惯性导航与视觉导航在导航状态估计的深度组合。

图4 惯性/视觉里程计深组合算法原理Fig.4 The algorithm principle of INS/ visual odometry deeply integrated navigation

4.2 视觉里程计系统误差模型

惯导系统误差模型可参考相应书籍与文献，这里不再赘述，本节主要对视觉里程计系统误差进行建模，为深组合方案的提出打下基础。

本文中，视觉里程计与惯性导航系统固联在同一基座上，而且在视觉里程计计算过程中会使用惯性导航系统与无线电高度表的信息。经分析，视觉里程计主要包括以下误差源：

（1）视觉里程计与惯性导航系统的安装误差；

（2）无线电高度表测高误差；

（3）相机焦距误差；

（4）视觉里程计图像匹配像素误差。

视觉里程计误差源较多，现从两个方面考虑，对视觉里程计误差进行建模：

（1）视觉里程计输出速度公式

首先分析视觉里程计输出速度公式。由式(11)可知，当图像匹配、无线电高度表测高、相机焦距出现误差时会导致视觉里程计测速误差，且认为惯性导航系统的姿态精度很高，故可得视觉里程计测速误差的方程：

式中，、为视觉里程计输出速度公式中相关误差源导致的水平载体坐标系下沿x、z轴的视觉里程计测速误差；δduodo、δdvodo为视觉里程计在图像平面坐标系下沿OU、OV轴图像匹配像素误差；为视觉里程计高度误差；δFu为相机焦距误差；为无线电高度表测高误差。

经过整理，去掉二阶小量，且考虑到δFu远小于Fu，故可略去δFu，可得下式：

将式(15)与视觉里程计输出速度公式相结合，可得出视觉里程计测速误差为：

（2）安装误差

在安装的过程中，应该使得视觉里程计与惯性导航系统尽量重合，但由于安装工艺的问题，不可避免地存在安装误差，这个安装误差虽然对视觉里程计测速不会造成影响，但是当需要将视觉里程计与惯性导航系统进行组合时，会导致一定的误差，故需要进一步分析由于安装误差导致的视觉里程计测速误差。

设机器视觉与惯性导航系统在载体坐标系下沿x、y、z轴的安装误差角为：

已知δaΦ为小角度向量，故含安装误差的惯导对应水平载体坐标系到视觉里程计对应水平载体坐标系的变换矩阵Codo可表示为：

式中：

由安装误差角导致的视觉里程计测速误差计算公式为：

整理可得：

经过上述分析，可得出视觉里程计测速误差为：

4.3 惯性/视觉里程计深组合滤波器设计

惯性/视觉里程计深组合导航系统采用速度匹配的组合滤波方案，组合滤波方程如下：

4.3.1 状态方程建模

在常规卡尔曼滤波中加入视觉里程计对应的误差状态量，共选取21 个系统状态：δVn、δVu、δVe、δL、δh、δλ、φn、φu、φe、∇x、∇y、∇z、εx、εy、εz、φax、φay、φaz、δH0、δduodo、δdvodo。

其中，δVn、δVu、δVe分别表示捷联惯导系统北向、天向、东向的速度误差；δL、δh、δλ分别表示微惯导系统的纬度误差、高度误差、经度误差；φn、φu、φe分别表示微惯导系统导航坐标系内北、天、东三个方向的失准角；∇x、∇y、∇z分别表示惯导系统载体坐标系内X、Y、Z 三个方向的加速度计零偏；εx、εy、εz分别表示微惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移；φax、φay、φaz惯导与视觉里程计的x、y、z轴的安装误差；δH0表示无线电高度表测高误差；δduodo、δdvodo分别表示视觉里程计图像匹配在OU 和OV 轴的像素误差。

系统矩阵为：

式中，Fbase为9 个基本导航参数的系统矩阵；FD为惯性器件的误差项；FM为加速度计和陀螺的一阶马尔可夫漂移误差项。

Fbase由惯性导航系统误差方程构成，而惯性器件的一阶马尔可夫漂移误差相关系统矩阵可表示为：

式中，Trx、Try、Trz为加速度计在x、y、z三个方向的一阶马尔柯夫过程的相关时间；TGx、TGy、TGz为陀螺仪在x、y、z三个方向的一阶马尔柯夫过程的相关时间。

4.3.2 观测方程建模

选择视觉里程计输出的速度信息作为观测，因此观测量为：

确定好观测量后，组合滤波器观测矩阵的求取直接决定了组合滤波最优估计的准确性。因为本文使用的是间接法的组合滤波，组合滤波器的输入是惯性导航系统和机器视觉导航系统的误差量，而惯性导航系统、机器视觉导航系统输出的信息可看作真值与误差量之和，故式(26)可表示为：

式中，、为惯导输出前向和侧向的速度误差；δvx_ov、δvz_ov为视觉里程计输出前向和侧向的速度误差。

组合滤波器观测矩阵反应了观测量与系统状态变量的关系，因为间接法组合滤波中，系统状态变量与误差量相关，故通过惯性导航系统和视觉里程计系统的误差模型，由式(19)(24)即可求取出组合滤波器观测矩阵。同时，在进行组合导航时，需要利用基于证据推理和卡方对视觉里程计有效性进行判断，如果视觉里程计无效，则观测矩阵置零，卡尔曼滤波仅进行时间更新，不进行量程更新。

4.4 组合导航步骤

基于证据推理的惯性/视觉里程计深组合导航的基本步骤如下：

1）相关变量初始化，卡尔曼滤波器初始化；

2）开始导航解算，并计算系统状态转移矩阵

3）到达卡尔曼滤波周期时，进行时间更新：

4）进入视觉里程计匹配区域，将当前帧图像作为实时图，上一帧图像作为基准图，利用惯性信息辅助视觉里程计计算，获得实时图在基准图中的位置，并计算当次匹配的特征值SMR和SNR。

5）由试验数据统计得到的特征值与视觉里程计匹配置信度关系并计算两个特征值所表征的视觉里程计匹配置信度Bpasmr和Bpasnr。计算公式如下：

式中，a1、b1、c1、a2、b2、c2为多项式拟合参数。

6）计算经静态权重折扣后的证据与其加权平均证据的距离di，计算公式见式(5)。

7)计算各证据的综合权重ai，使用综合权重对各证据源的mass 进行折扣，并计算其加权平均值。加权平均值表示的是第j次视觉里程计匹配经证据推理后得到的综合信度。当综合信度表征的视觉里程计匹配正确率低于95%时，不进行量测更新，即Kk+1=0；否则进行量测更新：

8）求取最优估计状态向量和最优估计协方差矩阵Pk+1：

9）若进行量测更新，对估计出的系统误差进行闭环修正，转入步骤（2）；若未进行量测更新，直接转入步骤（2）。

5 惯性/视觉里程计深组合导航

5.1 试验条件

本文针对惯性/视觉里程计深组合导航，研制了演示样机，并安装于运12 飞机内部开展了机载试验，安装情况如图5所示。

试验设备：

1）惯性导航系统，采用光纤捷联惯导系统，陀螺漂移0.05 °/h，加表零偏：100μg；

2）可见光相机：ICL-B2520 相机（2456×2058）；

3）距离获取设备：卫星高度通道。

在机载试验中，可见光相机拍摄运12 飞机下方的景象，利用视觉里程计测量载体的前向、侧向速度，并与惯导系统进行深度组合，实现高精度导航定位。

图5 机载试验安装示意图Fig.5 The installation of airborne test

飞行轨迹如图6所示。

图6 飞行轨迹Fig.6 Flight path

飞行高度如图7所示。

图7 飞行高度Fig.7 Flight altitude

5.2 试验结果

图8(a)表示试验结果，分别使用传统的惯性/视觉里程计组合导航和本文提出的基于证据推理的惯性/视觉里程计深组合导航。图8(b)为图8(a)速度误差曲线的局部放大图。

由图8(b)可知，在视觉里程计匹配过程中，传统方法对视觉里程计匹配结果的正确率判断不准确，导致速度误差变大。基于证据推理的惯性/视觉里程计深组合导航，首先对视觉里程计匹配结果进行更加可靠的正确率判断，认为视觉里程计匹配正确后，才进行惯性/视觉里程计深组合导航，组合导航结果更为平滑、准确。

图8(a) 速度误差曲线Fig.8(a) The curve of velocity error

图8(b) 速度误差曲线局部放大图Fig.8(b) The partial enlarged curve of velocity error

径向位置误差对比如图9所示。经计算，采用新方法后组合导航精度由0.4%D提升到0.3%D，进一步验证了新方法的有效性。

图9 径向位置误差对比Fig.9 The comparison of radial position

6 结 论

本文基于证据推理理论，将与视觉里程计匹配置信度相关的特征量进行数据融合，获得综合信度与匹配正确率的关系曲线。使用综合信度作为组合导航量测更新的依据，提高了卡尔曼滤波器的误差估计精度以及组合导航精度。

通过机载试验表明，基于证据推理的惯性/视觉里程计深组合导航相比传统惯性/视觉里程计组合导航，导航精度提升到了0.3%D，且计算复杂度低，易于工程实现。