基于渐消记忆滤波的1点RANSAC单目视觉姿态估计算法

齐乃新，张胜修，曹立佳，杨小冈，赵爱罡,

（1. 火箭军工程大学 控制工程系，西安 710025；2. 火箭军工程大学 士官学院，青州 262500；3. 四川理工学院 自动化与电子信息学院，自贡 643000）

基于渐消记忆滤波的1点RANSAC单目视觉姿态估计算法

齐乃新1，张胜修1，曹立佳3，杨小冈1，赵爱罡1,2

（1. 火箭军工程大学 控制工程系，西安 710025；2. 火箭军工程大学 士官学院，青州 262500；3. 四川理工学院 自动化与电子信息学院，自贡 643000）

针对1点RANSAC（Random Sample Consensus）单目视觉EKF（Extended Kalman Filter）算法中的滤波发散问题，分析了滤波发散的产生原因，提出了一种基于渐消记忆滤波的1点RANSAC单目视觉姿态估计算法。该算法通过在EKF滤波方程中引入加权因子，逐渐加大当前数据的权重，相应地减少旧数据的权重，有效地扼制了算法中的滤波发散问题。最后通过两组验证性实验验证说明了算法的有效性。实验结果表明：该算法能够有效地解决1点RANSAC单目视觉EKF算法中的滤波发散问题，具有更高的精度。第一组双轴联动实验，航向角的平均误差减小2.4158°，俯仰角平均误差减小0.1782°；第二组偏航轴大角度转动实验，摄像机航向角的估计误差一直保持在1.5°以内。

1点RANSAC算法；渐消记忆滤波；单目视觉；滤波发散

随着计算机视觉的发展，基于单目视觉的定位定向技术已经逐步地应用于移动机器人、无人机平台等领域，并展现了较为广阔的研究空间，其中，通过概率滤波器估计特征点和摄像机位置坐标的 V-SLAM（Visual Simultaneous Localization And Mapping）方法研究较为成熟[4,10-12]。目前，基于单目视觉的定位定向方法主要分为基于滤波的估计方法和基于光束平差（Bundle Adjustment，BA）法的估计方法两大类，在文献[1]中Strasdat H对这两类方法做了详细的分析。

2009年，Civera等人提出了1点RANSAC（RandomSample Consensus）单目视觉EKF算法[6-7]，并成功地扩展到V-SLAM中，实现了实时在线估计，成为滤波类单目视觉定位定向方法的研究热点。

近年来，以Civera为核心的研究团队开始着手研究水下环境的单目视觉定位定向技术[3]以及基于双目视觉的定位技术[2]。

在Civera研究的基础上，徐伟杰等人提出了2点RANSAC算法[7]，用以解决以无人直升机为载体的单目视觉同步定位与地图构建中存在的滤波发散的风险问题。李娟采用基于扩展卡尔曼滤波器（EKF）的修正协方差滤波器取代原有算法的EKF滤波器，改善了滤波器的滤波效果[8]。

1点RANSAC单目视觉EKF算法以EKF滤波估计为核心，由于模型的建立以及系统干扰、观测噪声等因素引起的模型误差的存在，使模型不能反映出真实的物理过程，导致观测值与模型不相对应，最终引起滤波发散。针对上述问题，本文引入渐消记忆滤波方法，通过逐渐减小老观测数据的权重，相应地增加新观测数据的权重，来逐渐减小过老观测数据对滤波的不良影响，防止滤波发散问题的产生。

1 1点RANSAC单目视觉EKF算法

1.1 预测和搜索特征匹配点

在上述方程中，uk是系统的控制向量，Fk是fk关于状态向量 xk|k-1的雅可比矩阵，Qk-1是系统噪声的协方差矩阵，Gk是系统噪声关于状态向量的雅可比矩阵。

根据状态向量的预测值xk|k-1，通过观测模型 hi可以得到观测量的预测值，如式(3)和式(4)。采用主动视觉匹配方法查找匹配点，提高搜索效率。

式中，Hi是hi关于状态向量xk|k-1的雅可比矩阵，Ri是观测模型中高斯噪声的协方差矩阵。然后，根据预测值用主动匹配方法在特征点以99%概率出现的区域内搜索，得到真实的观测量，即匹配点zi。

1.2 建立和估计1点状态假设

根据RANSAC原则，在一定阈值内通过对观测量的计算得到随机状态假设和相关的数据支撑[13]。在观测模型噪声等内在不确定因素作用下，通过单匹配点计算状态假设，能够大大降低观测量预测值之间的相关性。在建立状态假设时所用的阈值可以根据显著性参数 α= 0.05的χ2分布得到。

与标准RANSAC算法不同，1点RANSAC算法中的随机假设是在和预测状态共同作用下建立的，而不仅仅依赖于。利用这些已知的先验知识zIC和x能够缩小样本大小，使模型的自由度维数

k|k-1降到能够使用一个点的数据来求解模型参数的水平。通过RANSAC算法中假设结束的判断式(5)可以知道，假设的数量会随着所需特征点数量的增加成指数递减趋势，所以采用1点RANSAC算法可以大大减少假设i的数量，提高算法的效率。

式中，nhyp是随机假设建立的数量，p是先验概率，e是外点在点集中的占有比例，m是求解假设模型参数所需的最少匹配点数。

1.3 利用具有小观测残差的内点进行部分更新

对假设支持性大的特征点称为具有小观测残差的内点集，因为这些点靠近假设的模型，被认为是构成实际模型的特征点。剩下的距离假设模型较远的点被认为是外点，但仍然可能是内点。

在建立RANSAC假设的过程中，距离远的特征点能够对摄像机的旋转量建立精度较高的单点假设，然而此时对位移量估计精度却不高[6]。在这种情况下，其他距离远的外点会呈现出小观测残差的特性，并且会支持这个假设。然而距离较近的内点，由于位移量的不准确估计，也有可能呈现出大观测残差的特性。

假设模型建立以及内点确定之后，状态矩阵和协方差矩阵将按式(6)~(8)进行部分更新，但此时存在于外点集中的部分内点仍然需要拯救。

1.4 利用具有大观测残差的内点进行部分更新

上述部分更新完成之后，EKF预测中大部分的相关误差被修正，协方差中的误差大大减小。此时，就可以有效地恢复出具有大观测残差的内点。

利用第一次部分状态更新得到的xk|k计算出每一个具有大观测残差的匹配点zj的独立高斯预测值。如果匹配点zj的观测值有 99%的概率落在高斯预测的区域内，就被认为是内点拯救出来。这一步拯救出的具有大观测残差的内点标记为zhi_inliers。当检测出所有的zhi_inliers后，完成EKF的第二次部分更新。

2 加权渐消记忆滤波

2.1 滤波发散的产生原因

1点RANSAC单目视觉EKF算法采用的是针孔摄像机模型[9]，如图1所示。

图1 针孔摄像机模型Fig.1 Pinhole camera model

由光的直线传播条件可知，物体表面的反射光P经过一个针孔O投影到像平面上，对应地形成一个虚像平面，PI为光线与虚像平面的交点。针孔O即为光心，过投影中心O且垂直于摄像机平面的射线称为光轴或主轴，主轴与虚像平面的交点称为摄像机的主点，光心与主点之间的距离定义为摄像机的焦距f。由此可得到摄像机模型的描述如式(9)所示：

式中，fx、fy、u0、v0,为摄像机的内部参数，是由摄像机的内部结构决定的，需要通过特定的标定方法得到。在标定过程中必然存在一定的误差，从而使摄像机模型存在一定的模型误差。

估计的地图参数 yw是由n个特征点的特征参数组成：，这些特征参数 yw在逆深度表i述形式中被确定为参数由此可得系统的维数为13+6× n，其中n是特征点的个数。特征点的数目较大时，系统会具有较高的维数。同时，1点RANSAC单目视觉EKF算法的系统模型是非线性的，在简化处理过程中难免会带来模型的不准确问题，造成模型误差的产生。

在单目视觉系统采集图像序列以及图像序列的存储、传输以及处理过程中都存在干扰和噪声的影响，由于对这些干扰和噪声的统计特性缺乏全面的了解，仅仅采用高斯白噪声统计特性来进行统计分析往往因为统计参数取得不合理，使得噪声模型不准确，这样就又引入了一种模型误差。由于上述三种模型误差的存在，使模型不能反映出真实的物理过程，使得观测值与模型不相对应，从而会引起滤波的发散。

2.2 抑制滤波发散问题的方法

上述两节分别从系统误差的产生以及EKF滤波发散的内在机理两个方面阐述了滤波发散问题。研究表明，可以通过加大当前数据的权重，相应地减少旧数据的权重来防止滤波发散。因此，本文引入了 Sa加权渐消记忆滤波方法，具体过程阐述如下。

设系统模型为：

式中：Wk和 Vk都是零均值的白噪声序列，E｛Wk｝=0，；初始值Vk和x0不相关。

由EKF的最优增益矩阵公式：

取k=N，得：

对于N时刻的滤波，为了克服滤波发散，就应相应地突出KN，而相对地减少N时刻以前的 Kk。由式(14)知，Kk与Rk成反比关系，因此，为了达到上述目的，可使离N时刻越来越远的 Rk逐渐变大。采取Sa（a＞0，S＞1）加权的办法，可将P0、R1，R2，…，RN分别变为下列矩阵：

式中的S是适当选取的大于1的数。

采用这种加权方法，就是把N时刻以前观测值逐渐衰减。同样，对系统干扰噪声也给以 Sa加权：

式中的S与式(16)中的S相同。

应用Sa加权，就等价于对状态xk求最优滤波时，把P0和N时刻以前的噪声方差阵 Q0，Q1，…，QN-1、R1，R2，…，RN分别用式(16)和(17)来代替，这又相当于提出了一个新的模型：

模型式(12)和(13)在N时刻的渐消记忆滤波，就是模型式(18)和(19)在N时刻的最优滤波。滤波方程组为:

于是，式(21)、(22)和(23)分别变为：

就可得到一组渐消记忆滤波方程如下：

由以上渐消记忆滤波公式可以看出，对于k≤N的每一步计算，由于表示式(31)中的第一项乘上|k-1了一个大于1的因子S，故有：

可见，渐消记忆滤波加大了当前观测数据 hi在滤波方程中的权，相应地又减少了的权，即相应地|k-1降低了老观测数据对的不良影响。|k

3 实验验证分析

图 2 实验所用转台及摄像机Fig.2 Turntable and camera used in the experiment

实验分为两部分：第一部分通过转台偏航轴单轴转动 10°来确定加权系数S的值；第二部分在确定了加权系数S的前提下，分别通过转台的双轴联动和单轴大角度运动来验证算法的有效性。如图2所示，实验所用的转台为双轴转台，偏航轴和俯仰轴都能够自由地转动，摄像机为事先标定好的可见光针孔摄像机，采集的图像大小为240360× ，帧频为25。实验系统中单目摄像机与转台固连，转台控制器可以精确地控制转台两个轴的转动度数。

实验1：确定加权因子S的值

实验中控制转台的偏航轴转动10°，俯仰轴不动，此时采集一组图像序列，以加权因子S的取值为变量对摄像机的航向角进行滤波估计，然后分别采用“最大绝对误差”、“绝对误差”和“均方误差”对实验所得的航向角估计值进行统计分析，结果如图3~5所示。

分析图 3~5误差结果可以得到，加权因子S在1.010附近时姿态角的“最大绝对误差”、“绝对误差”和“均方误差”能够稳定地保持在较低水平，并且当加权因子S取1.009时，实验结果最为理想。

图 3 不同加权因子下摄像机航向角的最大绝对误差Fig.3 Maximum absolute error of course angle in different weighted factors

图 4 不同加权因子下摄像机航向角的绝对误差Fig.4 Absolute error of course angle in different weighted factors

图5 不同加权因子下摄像机航向角的均方误差Fig.5 Mean square error of course angles in different weighted factors

实验2：验证本文算法的有效性

实验 2设计了两组实验：第一组实验控制转台的偏航轴和俯仰轴进行双轴联动，两个轴各自转动的角度是20°，采集一组图像序列进行分析验证；第二组实验控制转台的偏航轴单轴转动70°的大角度，采集一组图像序列进行分析验证。两组实验都分别采用1点RANSAC单目视觉EKF算法以及本文算法对摄像机的运动姿态进行估计，实验中加权因子S取理想值1.009。

图6 原算法对摄像机姿态运动的估计结果Fig.6 Estimation results of the original algorithm in attitude angle of the camera

图7 本文算法对摄像机姿态运动的估计结果Fig.7 Estimation results of the proposed algorithm in attitude angle of the camera

图8 摄像机航向角误差Fig.8 Course angle error of camera

图9 摄像机俯仰角误差Fig.9 Pitch angle error of the camera

实验2的第一组实验结果如图6~9所示。从图6和图7可以看出，由于模型误差的存在，导致EKF算法对摄像机姿态运动的估计结果出现了较大幅度的滤波发散。本文算法有效地抑制了滤波发散，在很大程度上减小了发散的幅度。分析图8和图9可知，相比于原算法，本文算法估计出的摄像机航向角平均误差减小2.4158°，俯仰角平均误差减小0.1782°。

实验2的第二组实验结果如图10和图11所示。从图10中可以看出，本文算法能够更好地跟踪摄像机的航向角真值。分析图11可知，本文算法对摄像机姿态角的估计误差能够稳定地保持在1.5°以内，而1点RANSAC单目视觉EKF算法出现了一定程度的滤波发散现象，最终的累积误差超过了4°。

从实验2的两组实验可以得出，本文通过引入aS加权渐消记忆滤波方法，加大了当前数据的权重，相应地减小了旧数据的权重，有效地解决1点RANSAC单目视觉 EKF算法中因为模型误差引起的滤波发散问题，提高了EKF对摄像机姿态角的估计精度。

图10 原算法对摄像机姿态运动的估计结果Fig.10 Estimation results of the original algorithm in attitude angle of the camera

图11 摄像机航向角误差Fig.11 Course angle error of camera

4 结 论

本文从单目视觉系统的模型误差出发，分析了1点RANSAC单目视觉EKF算法中滤波发散问题的产生原因，提出了一种基于渐消记忆滤波的 1点RANSAC单目视觉姿态估计算法，解决了 1点RANSAC单目视觉EKF算法中的滤波发散问题，为进一步提高单目视觉 EKF算法的估计精度提供了一种新的思路。由于本文中的加权因子是通过实验所得，在不同的系统环境下具有差异性。因此，寻求一种加权因子自适应的滤波方法是今后进一步研究的方向。

（References）：

[1] Strasdat H, Montiel J M M, Davison A J. Visual SLAM: why filter?[J]. Image and Vision Computing, 2012, 30(2): 65-77.

[2] Pire T, Fischer T, Civera J, et al. Stereo parallel tracking and mapping for robot localization[C]//2015 IEEE/RSJ International Conference on Intelligent Robots and Systems. 2015: 1373-1378.

[3] Concha A, Drews-Jr P, Campos M, et al. Real-time localization and dense mapping in underwater environments from a monocular sequence[C]//OCEANS 2015-Genova. IEEE, 2015: 1-5.

[4] Salas-Moreno R, Newcombe R, Strasdat H, et al. Slam++: Simultaneous localisation and mapping at the level of objects[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2013: 1352-1359.

[5] Civera J, Grasa O G, Davison A J, et al. 1-point RANSAC for EKF-based structure from motion[C]// IEEE/RSJ International Conference on Intelligent Robots and Systems. 2009: 3498-3504.

[6] Civera J, Grasa O G, Davison A J, et al. 1-Point RANSAC for extended Kalman filtering: Application to real-time structure from motion and visual odometry[J]. Journal of Field Robotics, 2010, 27(5): 609-631.

[7] 徐伟杰, 李平, 韩波. 基于 2 点 RANSAC 的无人直升机单目视觉 SLAM[J]. 机器人, 2012, 34(1): 65-71. Xu W J, Li P, Han B. Monocular Visual SLAM of Unmanned Helicopter Based on 2-point RANSAC[J]. Robot, 2012, 34 (1): 65-71.

[8] 李捐. 基于单目视觉的移动机器人 SLAM 问题的研究[D]. 哈尔滨, 哈尔滨工业大学, 2013. Li J. Reserch on SLAM problem of mobile robot with monocular vision[D]. Harbin Institute of Technology, 2013.

[9] Schmitt R, Cai Y. Single camera-based synchronisation within a concept of robotic assembly in motion[J]. Assembly Automation, 2015, 34(2): 160-168.

[10] Taguchi Y, Jian Y D, Ramalingam S, et al. Point-plane SLAM for hand-held 3D sensors[C]//2013 IEEE International Conference on Robotics and Automation. 2013: 5182-5189.

[11] Tan W, Liu H, Dong Z, et al. Robust monocular SLAM in dynamic environments[C]//2013 IEEE International Symposium on Mixed and Augmented Reality. 2013: 209-218.

[12] Grasa O G, Bernal E, Casado S, et al. Visual SLAM for handheld monocular endoscope[J]. IEEE Transactions on Medical Imaging, 2014, 33(1): 135-146.

[13] Burguera A, González Y, Oliver G. RANSAC based data association for underwater visual SLAM[C]//ROBOT2013: First Iberian Robotics Conference. Springer International Publishing, 2014: 3-16.

[14] Sengupta A, Elanattil S. New feature detection mechanism for extended Kalman filter based monocular SLAM with 1-point RANSAC[M]//Mining Intelligence and Knowledge Exploration. Springer International Publishing, 2015: 29-36.

1-point random sample consensus based on fading memory filtering for attitude estimation with monocular vision

QI Nai-xin1, ZHANG Sheng-xiu1, CAO Li-jia3, YANG Xiao-gang1, ZHAO Ai-gang1,2
(1. Department of Control Engineering, Rocket Force University of Engineering, Xi’an 710025, China; 2. Petty Officer Academy, Rocket Force University of Engineering, Qingzhou 262500, China; 3. College of Automation and Electronic Information, Sichuan University of Science& Engineering, Zigong 64300, China)

In view that 1-point random sample consensus (RANSAC) for extended Kalman filter (EKF) filtering with monocular visual has filtering divergence problem, a 1-point RANSAC based on fading memory filtering algorithm is proposed with analyzing the causes of the filtering divergence. The proposed method effectively solves the filtering divergence problem by increasing the weights of the current data and accordingly reducing the old data, with the weighted factor added in the EKF. Finally, two groups of confirmatory experiments verify the effectiveness of the method. Experiment results show that the proposed method can effectively solve the problem of filtering divergence and has higher estimation accuracy. In the first experiment, the estimation mean errors of the proposed method are decreased by 2.4158° in camera’s course angle and 0.1782° in camera’s pitch angle. In the second experiment, the estimation errors of the camera’s course angle can be kept to within 1.5°.

1-point RANSAC algorithm; fading memory filtering; monocular visual; filtering divergence

TP391.4

：A

2016-03-01；

：2016-05-26

国家自然科学基金（61203189）；陕西省自然科学基金（2015JQ6226）

齐乃新（1989—），男，博士研究生，从事视觉导航方面的研究。E-mail: qinaixin2015@sina.com

1005-6734(2016)03-0366-06

10.13695/j.cnki.12-1222/o3.2016.03.016