基于景象匹配的低精度MEMS 航向修正与组合算法

朱启举，樊振辉，梅春波，杨鹏翔，卢宝峰，杨朝明

（1.西安现代控制技术研究所，西安 710065；2.西北工业大学，电子信息学院，西安 710072）

视觉景象匹配导航系统（Scene Matching Navigation System,SMNS）的研究起源于美国战斧式巡航导弹的末端导航与制导研究，并逐渐发展成为一种自主导航技术[1]。受益于其结构简单、自主、高精度的特性，在卫星拒止的复杂应用环境下，近几年视觉景象匹配导航在国内军事工业领域得到广泛研究[2,3]。

作为一种量测手段，现有的景象匹配导航算法只能输出位置信息，如高动态飞行器常用的捷联惯导系统/合成孔径雷达（Strapdown Inertial Navigation System/Synthetic Aperture Radar,SINS/SAR）紧组合导航系统，其中的SAR 景象匹配模块仅能提供载体位置、载体至回波中心的伪距/伪距率两种量测[4]。此外，基于光学或红外传感器的景象匹配模块仅能提供绝对位置信息[3,5]。受限于成本、体积与功耗等因素，目前低成本、轻量化微机电系统（Micro-Electro-Mechanical System,MEMS）是巡飞弹弹载惯性传感器的主流产品，然而此类MEMS 航向角发散速度较快。在卫星拒止环境下如何充分利用景象匹配结果，设计出一种航向角修正算法，将大大提高巡飞类弹药的自主导航能力，能够为其长时间巡航提供保障。

随着国内外导航技术的快速发展，涌现出诸多航向量测方法，如基于仿生沙蚁导航的偏振光相机能够利用太阳偏振光定向[6]，星敏感器可利用恒星星图匹配实现定位定向，地磁传感器能够提供北向角等[7]。然而此三种传感器有着无法忽视的缺点[8]，偏振光定向功能受限于天气、云雾、与太阳角度等，且目前尚无实际应用的案例，星敏感器一般只能工作于太空，近地空间难以应用，而地磁传感器极易受周边环境的干扰。此外，新增传感器必然给组合导航系统带来冗余，如果能利用景象匹配结果，设计一种航向角修正算法，对导航系统的设计是一种最优的解决方案。

有鉴于此，本文利用景象匹配结果提出了一种航向角测量方法，并根据匹配点数量设计了一种航向角修正与组合算法。为了验证本文算法的有效性，搭建了由惯性/视觉/高度计组合装置组成的实验系统，并进行了挂飞实验，结果表明本文算法的精度与时间均满足实时应用需求。

1 系统架构

视觉景象匹配导航系统的研究起源于导弹的末制导[1]，其流程如图1 所示，可以概括为三步：1）利用相对高度、导航坐标系下的姿态信息，将实时感知图像进行地理信息修正，使其与基准图的像素坐标系平行、空间分辨率相同；2）将地理信息修正后的实时图与基准图进行特征提取与匹配，得到实时图在基准图上的像素坐标；3）根据基准图预存的地理坐标，将匹配所得的像素坐标转化为经纬度，并作为量测与MEMS 导航解算结果进行组合。

图1 视觉景象匹配导航系统流程图Fig.1 Flow chart of scene matching navigation system

定位是视觉景象匹配导航系统的首要功能，本文在视觉景象匹配导航结果的基础上，设计了定向算法与组合算法，如图1 中蓝色部分所示。定向算法利用特征匹配后的匹配点对关系、相对高度两种量测信息，结合基准图空间分辨率、相机内参等参数完成定向。

2 航向误差模型与组合算法

2.1 基于匹配点几何关系的航向误差模型

实时图根据SINS 导航解算得到的姿态进行地理信息修正，即正射投影。如果导航解算生成的航向角存在较大误差（航向角记为φ），利用该姿态投影出的图像必然存在误差，如图2(b)所示。本文设计了一种基于匹配点对的航向角修正算法，通过求取SINS 提供的航向角与航向真值之间的差值间接地计算出航向角，而两者之间的差值通过实时图与基准图之间匹配点的集合关系完成计算。

图2 图像匹配关系Fig.2 Image matching relationship

基准图与实时图在经过匹配及筛选流程后，能够得到匹配点对，如图2 所示。基准图与实时图上的红色方框表示一对匹配点，蓝色方框表示另一对匹配点。通过求解匹配点对横纵像素距离的反正切值，能够分别计算出基准图与实时图中两个像素连线与正上方的角度，如图2 中的角a与角b。

根据图2 所示的几何关系可以看出，角b减去角a即为实时图与基准图之间航向角差值，记为角c。考虑到基准图横纵方向为正东正北，角c即为航向真值与导航解算航向角φ之间的差值。角c加上φ就等于航向角真值。角a，b的计算方法为：

其中，Ib表示基准图；In表示地理信息修正后的实时图；上标中的u、v分别表示匹配点的横纵像素坐标，数字表示匹配点的序号。航向角真值的计算公式为：

为了提高航向真值的计算精度，采用多对点计算并取均值的方式计算角c，如图3 所示，此时航向角真值的计算公式为：

图3 多对点对的航向角计算关系Fig.3 Course angle calculation of multiple matching points

式(3)即可独立完成航向角计算。考虑到投影过程引入了水平两角度的误差，导致式(1)计算的差值里包含了水平两角度误差所引起的投影误差。由于三个姿态角的耦合关系，无法将航向角单独分离出来，即当水平姿态角误差较大的情况下，地理信息修正后的图像In特征点位置存在误差，从而引起式(1)与式(3)计算出的航向角存在误差。为解决该问题，本文设计了基于匹配点集透视变换关系的航向误差模型，并采用优化的方法求解出高精度航向角。首先给出基准图In与原始图Ic的关系为：

其中，K表示相机内参；(u,v) 表示二维像素坐标；表示相机坐标系（c系）至导航坐标系（n系）之间的旋转矩阵，可通过惯性导航解算的航向角φ、俯仰角γ、滚转角θ计算得到。图4 为坐标系示意图，导航坐标系On-xn yn zn近似以相机光心为中心，xn yn zn三轴指向北天东；相机坐标系Oc-xc yc zc以相机光心为中心，xc yc zc三轴指向相机右上前（左手坐标系）；以相机光心为中心构建局部坐标系Ow-xw yw zw，其为正下视坐标系，xw yw zw分别指向南东地（左手坐标系）。

图4 坐标系示意图Fig.4 Diagram of the coordinates

由于在长航时过程中，惯性导航系统提供的姿态角存在较大误差，可根据式(1)与式(4)构建出航向角误差模型[9]：

式(5)右侧ai、ib的求解包含了式(4)的模型，即包含了航向角这一待优化变量，其优化求解过程见2.3节。

2.2 基于匹配点重投影的航向误差模型

景象匹配完成后，能够得到基准图与地理信息修正后实时图的匹配关系，此时二者仅存在平移关系（姿态误差将作为优化变量）。根据基准图预存的地理信息，可以计算出相机光心的绝对坐标。令高度计提供的相对高度∆H为基准图匹配点在局部坐标系下zw向坐标，并通过基准图空间分辨率计算出基准图上匹配点在局部坐标系下的xw、yw向坐标，最终可得到基准图匹配点在局部坐标系下的三维坐标P(xw,yw,zw)。

根据相机成像模型，可知局部坐标系至像素坐标系的计算公式为：

其中，t表示局部坐标系与相机坐标系之间的平移，由图4 可看出平移量为0；Ie为成像模型计算出匹配点的像素坐标。局部坐标系的三轴与导航坐标系的三轴相平行，轴间对应关系见图4。根据MEMS 导航解算提供的姿态角，计算公式为：

需要注意的是由于局部坐标系为左手坐标系，按照航向、滚转、俯仰的旋转顺序构建旋转矩阵时，需要将滚转角与航向角取为负值，则根据模型预测Ie与原始值Ic（真值）可以构建重投影误差模型：

根据相对高度与相机视场角的几何关系[10]，得到修正后的相对高度∆H*为：

其中，FOV表示水平视场角度；Lirt表示投影后图像水平方向尺寸；εbenc表示基准图空间分辨率。

2.3 航向修正与组合算法

式(3)对匹配点之间的距离存在要求，当匹配点之间的距离较近时，会带来较大的航向角误差。理想情况下，实时图像四个角附近特征点的精度最高。式(5)与式(8)需要较多的匹配点，匹配点数量较少时使用优化算法会导致较大误差。因此，考虑匹配点之间的距离与匹配点数量，本文设计的航向角修正与组合算法如下：

1）当匹配点对数小于等于15 时：

Step.1 实时图与基准图经过匹配流程后，得到匹配点对；

Step.2 以第1 对匹配点为基准，遍历剩余匹配点，当距离大于30 像素时，分别计算ai、bi、ci，如图3所示，否则不进行运算。以剩余匹配点为基准，重复上述与第1 对匹配点同样的流程，最终所有ci取平均即可得到c。

Step.3 将c作为航向角修正值，与导航解算的航向角φ叠加即为修正后的航向角。

2）当匹配点对数大于15 时：

Step.1 计算出航向角，使用式(5)构建误差模型；

Step.2 根据匹配点对计算尺度比S，进一步修正∆H，在此基础上计算三维坐标P(xw,yw,zw)，并根据匹配关系构建重投影误差模型；

Step.3 联合优化，如式(11)所示。

由于优化算法不同，式(11)所述的联合优化分两步进行。首先，根据式(7)利用导航解算姿态角计算出旋转矩阵。对于式(8)，本文采用左乘扰动模型计算三维姿态角的优化增量，该过程使用惯性导航系统提供的姿态角作为初值计算初始矩阵。其次，由于式(5)所示的待优化函数涉及旋转矩阵内变量的反正切值，如果考虑欧拉角间的耦合关系，则对于角φ的导函数没有显式表达式。同时考虑到图1 所示的视觉景象匹配导航系统一般应用在水平姿态角变化较小的场景，本文忽略耦合关系，直接将角φ视为待优化变量，并将式(8)优化后的水平姿态角作为常值，通过式(7)代入式(4)，即可得到包含唯一变量的优化函数，采用高斯牛顿法即可完成优化求解。

本文仅更新航向量测，以误差作为状态量，并采用常规的松组合卡尔曼滤波完成航向角量测与惯导系统的组合。首先，构建相对旋转矩阵：

然后，计算航向误差量测量：

由于存在量测滞后，需通过线性外推完成量测更新，然后对外推量测更新结果得到的状态量回推，得到姿态更新结果[11]。

3 实验分析与结果

3.1 实验过程与结果

本文搭建的挂飞实验平台如图5所示，其中图5(a)为挂飞试验平台，图5(b)为各传感器安装位置，图5(c)-(g)分别为GNSS、MEMS、处理器1、相机、处理器2。各传感器配置为：相机（1920×1080，30 Hz）、MEMS（30 Hz，陀螺仪零偏：10°/h，随机噪声：）、GNSS（10 Hz）、处理器1（SK1207B）和处理器2（Nvidia NX,Ubuntu-18.04）。处理器1 用于数据对齐，处理器2 用于运行本文算法。GNSS 信息经过利用载波相位差分技术（Real time kinematic,RTK）处理后可提供高精度位置信息。如图1 所示的系统架构，地理信息修正需要输入相机相对于地面的高度，本文RTK 测量绝对高度，并使用无人机飞行过程中的绝对高度与起飞前的绝对高度之差作为相对高度。考虑到在有限的高度下，相机所感知的图像对应实际区域较小，且相机的分辨率达到2 K，地形高度引起的误差可以忽略。同时RTK 定位精度为厘米级，使用双卫星接收机提供精确的航向作为对比参考值。

图5 实验平台实物图Fig.5 The experimental platform

惯性/视觉景象匹配导航装置如图6 所示，其中OI-xI yI zI表示惯性坐标系（载体坐标系），Oc-xc yc zc表示相机坐标系。两种坐标系在实验前经过单独标定与联合标定，温度对标定内参的影响可以忽略。

图6 惯性/视觉景象匹配导航装置Fig.6 Inertial/vision scene matching navigation device

为了验证本文算法的有效性，在陕西省西安市锦湖街道进行了无人机载飞行实验。实验经度范围约为108.8942°～108.9101°，纬度范围约为34.1224°～34.1385°。无人机飞行期间，高度约为150～500 m，速度约为12 m/s，水平姿态变化范围为-30°～-90°，航向变化范围为-180°～180°，飞行时间约30 min。作为参考的卫星图像来自于Googlemap，分辨率为645.8 mm。特征提取与匹配算法使用点特征，实验匹配效果如图7 所示。

图7 匹配效果Fig.7 Matching results

数据分析过程使用21 min 1 s 至27 min 37 s 之间的数据，此阶段导航系统解算的航向误差为2.5°～4.5°。相比前期数据，此阶段的航向误差较大，具备对本文算法进行验证的条件。采集不同角度、不同高度的实时图像共397 张，采集频率为1 Hz。以惯性/RTK 松组合提供的航向角为基准（双卫星接收机提供航向量测），最终计算出的航向误差如图8 所示。在处理过程中，考虑到匹配点误差较大时必然导致航向角误差大，本文将误差绝对值大于15°的结果舍弃。

图8 航向误差结果Fig.8 Results of the course error

在所有图像中，满足2.3 节所述规则的图像有392张，即满足至少有两对匹配点距离大于30 像素，有效率为98.74%。在有效图片中，满足2.3 节条件（1）的数量共计为303 张，满足条件（2）的数量为89 张。基于有效图像，得到航向角的均方根误差（Root Mean Square Error,RMSE）为0.7423°；最大误差绝对值为7.591°，这是由于匹配点误差较大，导致计算出的航向角误差也较大；最小误差绝对值为0.002°。

3.2 实验分析

实验结果表明，本文算法解算出的航向角具备一定精度，且有效图像解算出的航向角相比导航解算航向角有较大提高，验证了本文算法的有效性。

在对原始实时图像进行地理信息修正的过程中，会利用惯性导航解算结果进行投影处理，使实时图与基准图处于相同的坐标系[12]。但随着时间累积，惯性导航系统提供的航向角必然存在较大误差，投影之后的图像与基准图存在误差，而本文算法利用匹配点的几何关系计算出航向角误差，与导航系统解算的航向角相叠加即可计算出修正后的航向角。由于姿态角存在耦合关系，本文设计了两种误差模型来优化航向角，考虑到优化模型需要大量的样本才能保证精度，因此基于匹配点数量设计了一种匹配规则，确保了航向角的精度。

同时，需要指出的是，本文算法的适用性受限于前端“特征匹配”的结果。首先，需满足在景象匹配步骤后得到匹配点对，即本文算法不适用于传统的线特征匹配结果。同时，匹配成功是本文算法的前提，匹配不成功无法应用本文算法。因季节、气候、光照、雨雪雾霾等因素导致基准图与实时图存在较大差异，亦或沙漠、海洋、戈壁等少纹理特征的区域导致的匹配失败，本文亦无法解决。本文仅基于“特征匹配”结果设计了一种航向角修正与融合算法。此外，本文算法的精度取决于匹配误差，应确保匹配像素误差低于1.5 像素，考虑到常规的SURF（Speeded Up Robust Feature,加速稳健特征）家族点特征匹配算法、方向梯度特征通道（Chanel Feature of Orientated Gradient,CFOG）区域特征算法等均能满足该要求[13]，可以认为本文所设计的算法具备一定的通用性。

4 结论

本文针对低精度MEMS 在长航时过程中航向角易发散的问题，提出了一种利用景象匹配结果修正航向角的算法。首先，基于匹配点几何关系设计了航向角误差模型，通过匹配点对与图像纵轴的正切值之差计算航向修正值。然后，基于匹配点重投影误差设计了航向角误差模型，通过实时图与基准图的匹配关系构建了重投影误差，通过优化计算航向角的最优值。基于上述两种模型，本文根据匹配点数量设计了一种航向角修正与组合算法。飞行实验结果表明，长航时过程中修正后的航向角误差为0.7423°（RMSE），更新频率优于1 Hz。满足条件的有效图像数量比例为98.74%，验证了本文算法的有效性。同时，与传统仅能定位的景象匹配算法相比，本文设计了一种航向计算算法，且该算法对传感器精度依赖性较低，具备一定的通用性。