齊乃新，張勝修，曹立佳，楊小岡，趙愛罡,

（1. 火箭軍工程大學(xué) 控制工程系，西安 710025；2. 火箭軍工程大學(xué) 士官學(xué)院，青州 262500；3. 四川理工學(xué)院 自動化與電子信息學(xué)院，自貢 643000）

基于漸消記憶濾波的1點RANSAC單目視覺姿態(tài)估計算法

齊乃新1，張勝修1，曹立佳3，楊小岡1，趙愛罡1,2

（1. 火箭軍工程大學(xué) 控制工程系，西安 710025；2. 火箭軍工程大學(xué) 士官學(xué)院，青州 262500；3. 四川理工學(xué)院 自動化與電子信息學(xué)院，自貢 643000）

針對1點RANSAC（Random Sample Consensus）單目視覺EKF（Extended Kalman Filter）算法中的濾波發(fā)散問題，分析了濾波發(fā)散的產(chǎn)生原因，提出了一種基于漸消記憶濾波的1點RANSAC單目視覺姿態(tài)估計算法。該算法通過在EKF濾波方程中引入加權(quán)因子，逐漸加大當(dāng)前數(shù)據(jù)的權(quán)重，相應(yīng)地減少舊數(shù)據(jù)的權(quán)重，有效地扼制了算法中的濾波發(fā)散問題。最后通過兩組驗證性實驗驗證說明了算法的有效性。實驗結(jié)果表明：該算法能夠有效地解決1點RANSAC單目視覺EKF算法中的濾波發(fā)散問題，具有更高的精度。第一組雙軸聯(lián)動實驗，航向角的平均誤差減小2.4158°，俯仰角平均誤差減小0.1782°；第二組偏航軸大角度轉(zhuǎn)動實驗，攝像機航向角的估計誤差一直保持在1.5°以內(nèi)。

1點RANSAC算法；漸消記憶濾波；單目視覺；濾波發(fā)散

隨著計算機視覺的發(fā)展，基于單目視覺的定位定向技術(shù)已經(jīng)逐步地應(yīng)用于移動機器人、無人機平臺等領(lǐng)域，并展現(xiàn)了較為廣闊的研究空間，其中，通過概率濾波器估計特征點和攝像機位置坐標(biāo)的 V-SLAM（Visual Simultaneous Localization And Mapping）方法研究較為成熟[4,10-12]。目前，基于單目視覺的定位定向方法主要分為基于濾波的估計方法和基于光束平差（Bundle Adjustment，BA）法的估計方法兩大類，在文獻(xiàn)[1]中Strasdat H對這兩類方法做了詳細(xì)的分析。

2009年，Civera等人提出了1點RANSAC（RandomSample Consensus）單目視覺EKF算法[6-7]，并成功地擴展到V-SLAM中，實現(xiàn)了實時在線估計，成為濾波類單目視覺定位定向方法的研究熱點。

近年來，以Civera為核心的研究團隊開始著手研究水下環(huán)境的單目視覺定位定向技術(shù)[3]以及基于雙目視覺的定位技術(shù)[2]。

在Civera研究的基礎(chǔ)上，徐偉杰等人提出了2點RANSAC算法[7]，用以解決以無人直升機為載體的單目視覺同步定位與地圖構(gòu)建中存在的濾波發(fā)散的風(fēng)險問題。李娟采用基于擴展卡爾曼濾波器（EKF）的修正協(xié)方差濾波器取代原有算法的EKF濾波器，改善了濾波器的濾波效果[8]。

1點RANSAC單目視覺EKF算法以EKF濾波估計為核心，由于模型的建立以及系統(tǒng)干擾、觀測噪聲等因素引起的模型誤差的存在，使模型不能反映出真實的物理過程，導(dǎo)致觀測值與模型不相對應(yīng)，最終引起濾波發(fā)散。針對上述問題，本文引入漸消記憶濾波方法，通過逐漸減小老觀測數(shù)據(jù)的權(quán)重，相應(yīng)地增加新觀測數(shù)據(jù)的權(quán)重，來逐漸減小過老觀測數(shù)據(jù)對濾波的不良影響，防止濾波發(fā)散問題的產(chǎn)生。

1 1點RANSAC單目視覺EKF算法

1.1 預(yù)測和搜索特征匹配點

在上述方程中，uk是系統(tǒng)的控制向量，F(xiàn)k是fk關(guān)于狀態(tài)向量 xk|k-1的雅可比矩陣，Qk-1是系統(tǒng)噪聲的協(xié)方差矩陣，Gk是系統(tǒng)噪聲關(guān)于狀態(tài)向量的雅可比矩陣。

根據(jù)狀態(tài)向量的預(yù)測值xk|k-1，通過觀測模型 hi可以得到觀測量的預(yù)測值，如式(3)和式(4)。采用主動視覺匹配方法查找匹配點，提高搜索效率。

式中，Hi是hi關(guān)于狀態(tài)向量xk|k-1的雅可比矩陣，Ri是觀測模型中高斯噪聲的協(xié)方差矩陣。然后，根據(jù)預(yù)測值用主動匹配方法在特征點以99%概率出現(xiàn)的區(qū)域內(nèi)搜索，得到真實的觀測量，即匹配點zi。

1.2 建立和估計1點狀態(tài)假設(shè)

根據(jù)RANSAC原則，在一定閾值內(nèi)通過對觀測量的計算得到隨機狀態(tài)假設(shè)和相關(guān)的數(shù)據(jù)支撐[13]。在觀測模型噪聲等內(nèi)在不確定因素作用下，通過單匹配點計算狀態(tài)假設(shè)，能夠大大降低觀測量預(yù)測值之間的相關(guān)性。在建立狀態(tài)假設(shè)時所用的閾值可以根據(jù)顯著性參數(shù) α= 0.05的χ2分布得到。

與標(biāo)準(zhǔn)RANSAC算法不同，1點RANSAC算法中的隨機假設(shè)是在和預(yù)測狀態(tài)共同作用下建立的，而不僅僅依賴于。利用這些已知的先驗知識zIC和x能夠縮小樣本大小，使模型的自由度維數(shù)

k|k-1降到能夠使用一個點的數(shù)據(jù)來求解模型參數(shù)的水平。通過RANSAC算法中假設(shè)結(jié)束的判斷式(5)可以知道，假設(shè)的數(shù)量會隨著所需特征點數(shù)量的增加成指數(shù)遞減趨勢，所以采用1點RANSAC算法可以大大減少假設(shè)i的數(shù)量，提高算法的效率。

式中，nhyp是隨機假設(shè)建立的數(shù)量，p是先驗概率，e是外點在點集中的占有比例，m是求解假設(shè)模型參數(shù)所需的最少匹配點數(shù)。

1.3 利用具有小觀測殘差的內(nèi)點進(jìn)行部分更新

對假設(shè)支持性大的特征點稱為具有小觀測殘差的內(nèi)點集，因為這些點靠近假設(shè)的模型，被認(rèn)為是構(gòu)成實際模型的特征點。剩下的距離假設(shè)模型較遠(yuǎn)的點被認(rèn)為是外點，但仍然可能是內(nèi)點。

在建立RANSAC假設(shè)的過程中，距離遠(yuǎn)的特征點能夠?qū)z像機的旋轉(zhuǎn)量建立精度較高的單點假設(shè)，然而此時對位移量估計精度卻不高[6]。在這種情況下，其他距離遠(yuǎn)的外點會呈現(xiàn)出小觀測殘差的特性，并且會支持這個假設(shè)。然而距離較近的內(nèi)點，由于位移量的不準(zhǔn)確估計，也有可能呈現(xiàn)出大觀測殘差的特性。

假設(shè)模型建立以及內(nèi)點確定之后，狀態(tài)矩陣和協(xié)方差矩陣將按式(6)~(8)進(jìn)行部分更新，但此時存在于外點集中的部分內(nèi)點仍然需要拯救。

1.4 利用具有大觀測殘差的內(nèi)點進(jìn)行部分更新

上述部分更新完成之后，EKF預(yù)測中大部分的相關(guān)誤差被修正，協(xié)方差中的誤差大大減小。此時，就可以有效地恢復(fù)出具有大觀測殘差的內(nèi)點。

利用第一次部分狀態(tài)更新得到的xk|k計算出每一個具有大觀測殘差的匹配點zj的獨立高斯預(yù)測值。如果匹配點zj的觀測值有 99%的概率落在高斯預(yù)測的區(qū)域內(nèi)，就被認(rèn)為是內(nèi)點拯救出來。這一步拯救出的具有大觀測殘差的內(nèi)點標(biāo)記為zhi_inliers。當(dāng)檢測出所有的zhi_inliers后，完成EKF的第二次部分更新。

2 加權(quán)漸消記憶濾波

2.1 濾波發(fā)散的產(chǎn)生原因

1點RANSAC單目視覺EKF算法采用的是針孔攝像機模型[9]，如圖1所示。

圖1 針孔攝像機模型Fig.1 Pinhole camera model

由光的直線傳播條件可知，物體表面的反射光P經(jīng)過一個針孔O投影到像平面上，對應(yīng)地形成一個虛像平面，PI為光線與虛像平面的交點。針孔O即為光心，過投影中心O且垂直于攝像機平面的射線稱為光軸或主軸，主軸與虛像平面的交點稱為攝像機的主點，光心與主點之間的距離定義為攝像機的焦距f。由此可得到攝像機模型的描述如式(9)所示：

式中，fx、fy、u0、v0,為攝像機的內(nèi)部參數(shù)，是由攝像機的內(nèi)部結(jié)構(gòu)決定的，需要通過特定的標(biāo)定方法得到。在標(biāo)定過程中必然存在一定的誤差，從而使攝像機模型存在一定的模型誤差。

估計的地圖參數(shù) yw是由n個特征點的特征參數(shù)組成：，這些特征參數(shù) yw在逆深度表i述形式中被確定為參數(shù)由此可得系統(tǒng)的維數(shù)為13+6× n，其中n是特征點的個數(shù)。特征點的數(shù)目較大時，系統(tǒng)會具有較高的維數(shù)。同時，1點RANSAC單目視覺EKF算法的系統(tǒng)模型是非線性的，在簡化處理過程中難免會帶來模型的不準(zhǔn)確問題，造成模型誤差的產(chǎn)生。

在單目視覺系統(tǒng)采集圖像序列以及圖像序列的存儲、傳輸以及處理過程中都存在干擾和噪聲的影響，由于對這些干擾和噪聲的統(tǒng)計特性缺乏全面的了解，僅僅采用高斯白噪聲統(tǒng)計特性來進(jìn)行統(tǒng)計分析往往因為統(tǒng)計參數(shù)取得不合理，使得噪聲模型不準(zhǔn)確，這樣就又引入了一種模型誤差。由于上述三種模型誤差的存在，使模型不能反映出真實的物理過程，使得觀測值與模型不相對應(yīng)，從而會引起濾波的發(fā)散。

2.2 抑制濾波發(fā)散問題的方法

上述兩節(jié)分別從系統(tǒng)誤差的產(chǎn)生以及EKF濾波發(fā)散的內(nèi)在機理兩個方面闡述了濾波發(fā)散問題。研究表明，可以通過加大當(dāng)前數(shù)據(jù)的權(quán)重，相應(yīng)地減少舊數(shù)據(jù)的權(quán)重來防止濾波發(fā)散。因此，本文引入了 Sa加權(quán)漸消記憶濾波方法，具體過程闡述如下。

設(shè)系統(tǒng)模型為：

式中：Wk和 Vk都是零均值的白噪聲序列，E｛Wk｝=0，；初始值Vk和x0不相關(guān)。

由EKF的最優(yōu)增益矩陣公式：

取k=N，得：

對于N時刻的濾波，為了克服濾波發(fā)散，就應(yīng)相應(yīng)地突出KN，而相對地減少N時刻以前的 Kk。由式(14)知，Kk與Rk成反比關(guān)系，因此，為了達(dá)到上述目的，可使離N時刻越來越遠(yuǎn)的 Rk逐漸變大。采取Sa（a＞0，S＞1）加權(quán)的辦法，可將P0、R1，R2，…，RN分別變?yōu)橄铝芯仃嚕?/p>

式中的S是適當(dāng)選取的大于1的數(shù)。

采用這種加權(quán)方法，就是把N時刻以前觀測值逐漸衰減。同樣，對系統(tǒng)干擾噪聲也給以 Sa加權(quán)：

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

應(yīng)用Sa加權(quán)，就等價于對狀態(tài)xk求最優(yōu)濾波時，把P0和N時刻以前的噪聲方差陣 Q0，Q1，…，QN-1、R1，R2，…，RN分別用式(16)和(17)來代替，這又相當(dāng)于提出了一個新的模型：

模型式(12)和(13)在N時刻的漸消記憶濾波，就是模型式(18)和(19)在N時刻的最優(yōu)濾波。濾波方程組為:

于是，式(21)、(22)和(23)分別變?yōu)椋?/p>

就可得到一組漸消記憶濾波方程如下：

由以上漸消記憶濾波公式可以看出，對于k≤N的每一步計算，由于表示式(31)中的第一項乘上|k-1了一個大于1的因子S，故有：

可見，漸消記憶濾波加大了當(dāng)前觀測數(shù)據(jù) hi在濾波方程中的權(quán)，相應(yīng)地又減少了的權(quán)，即相應(yīng)地|k-1降低了老觀測數(shù)據(jù)對的不良影響。|k

3 實驗驗證分析

圖 2 實驗所用轉(zhuǎn)臺及攝像機Fig.2 Turntable and camera used in the experiment

實驗分為兩部分：第一部分通過轉(zhuǎn)臺偏航軸單軸轉(zhuǎn)動 10°來確定加權(quán)系數(shù)S的值；第二部分在確定了加權(quán)系數(shù)S的前提下，分別通過轉(zhuǎn)臺的雙軸聯(lián)動和單軸大角度運動來驗證算法的有效性。如圖2所示，實驗所用的轉(zhuǎn)臺為雙軸轉(zhuǎn)臺，偏航軸和俯仰軸都能夠自由地轉(zhuǎn)動，攝像機為事先標(biāo)定好的可見光針孔攝像機，采集的圖像大小為240360× ，幀頻為25。實驗系統(tǒng)中單目攝像機與轉(zhuǎn)臺固連，轉(zhuǎn)臺控制器可以精確地控制轉(zhuǎn)臺兩個軸的轉(zhuǎn)動度數(shù)。

實驗1：確定加權(quán)因子S的值

實驗中控制轉(zhuǎn)臺的偏航軸轉(zhuǎn)動10°，俯仰軸不動，此時采集一組圖像序列，以加權(quán)因子S的取值為變量對攝像機的航向角進(jìn)行濾波估計，然后分別采用“最大絕對誤差”、“絕對誤差”和“均方誤差”對實驗所得的航向角估計值進(jìn)行統(tǒng)計分析，結(jié)果如圖3~5所示。

分析圖 3~5誤差結(jié)果可以得到，加權(quán)因子S在1.010附近時姿態(tài)角的“最大絕對誤差”、“絕對誤差”和“均方誤差”能夠穩(wěn)定地保持在較低水平，并且當(dāng)加權(quán)因子S取1.009時，實驗結(jié)果最為理想。

圖 3 不同加權(quán)因子下攝像機航向角的最大絕對誤差Fig.3 Maximum absolute error of course angle in different weighted factors

圖 4 不同加權(quán)因子下攝像機航向角的絕對誤差Fig.4 Absolute error of course angle in different weighted factors

圖5 不同加權(quán)因子下攝像機航向角的均方誤差Fig.5 Mean square error of course angles in different weighted factors

實驗2：驗證本文算法的有效性

實驗 2設(shè)計了兩組實驗：第一組實驗控制轉(zhuǎn)臺的偏航軸和俯仰軸進(jìn)行雙軸聯(lián)動，兩個軸各自轉(zhuǎn)動的角度是20°，采集一組圖像序列進(jìn)行分析驗證；第二組實驗控制轉(zhuǎn)臺的偏航軸單軸轉(zhuǎn)動70°的大角度，采集一組圖像序列進(jìn)行分析驗證。兩組實驗都分別采用1點RANSAC單目視覺EKF算法以及本文算法對攝像機的運動姿態(tài)進(jìn)行估計，實驗中加權(quán)因子S取理想值1.009。

圖6 原算法對攝像機姿態(tài)運動的估計結(jié)果Fig.6 Estimation results of the original algorithm in attitude angle of the camera

圖7 本文算法對攝像機姿態(tài)運動的估計結(jié)果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的第一組實驗結(jié)果如圖6~9所示。從圖6和圖7可以看出，由于模型誤差的存在，導(dǎo)致EKF算法對攝像機姿態(tài)運動的估計結(jié)果出現(xiàn)了較大幅度的濾波發(fā)散。本文算法有效地抑制了濾波發(fā)散，在很大程度上減小了發(fā)散的幅度。分析圖8和圖9可知，相比于原算法，本文算法估計出的攝像機航向角平均誤差減小2.4158°，俯仰角平均誤差減小0.1782°。

實驗2的第二組實驗結(jié)果如圖10和圖11所示。從圖10中可以看出，本文算法能夠更好地跟蹤攝像機的航向角真值。分析圖11可知，本文算法對攝像機姿態(tài)角的估計誤差能夠穩(wěn)定地保持在1.5°以內(nèi)，而1點RANSAC單目視覺EKF算法出現(xiàn)了一定程度的濾波發(fā)散現(xiàn)象，最終的累積誤差超過了4°。

從實驗2的兩組實驗可以得出，本文通過引入aS加權(quán)漸消記憶濾波方法，加大了當(dāng)前數(shù)據(jù)的權(quán)重，相應(yīng)地減小了舊數(shù)據(jù)的權(quán)重，有效地解決1點RANSAC單目視覺 EKF算法中因為模型誤差引起的濾波發(fā)散問題，提高了EKF對攝像機姿態(tài)角的估計精度。

圖10 原算法對攝像機姿態(tài)運動的估計結(jié)果Fig.10 Estimation results of the original algorithm in attitude angle of the camera

圖11 攝像機航向角誤差Fig.11 Course angle error of camera

4 結(jié) 論

本文從單目視覺系統(tǒng)的模型誤差出發(fā)，分析了1點RANSAC單目視覺EKF算法中濾波發(fā)散問題的產(chǎn)生原因，提出了一種基于漸消記憶濾波的 1點RANSAC單目視覺姿態(tài)估計算法，解決了 1點RANSAC單目視覺EKF算法中的濾波發(fā)散問題，為進(jìn)一步提高單目視覺 EKF算法的估計精度提供了一種新的思路。由于本文中的加權(quán)因子是通過實驗所得，在不同的系統(tǒng)環(huán)境下具有差異性。因此，尋求一種加權(quán)因子自適應(yīng)的濾波方法是今后進(jìn)一步研究的方向。

（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]. 哈爾濱, 哈爾濱工業(yè)大學(xué), 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

國家自然科學(xué)基金（61203189）；陜西省自然科學(xué)基金（2015JQ6226）

齊乃新（1989—），男，博士研究生，從事視覺導(dǎo)航方面的研究。E-mail: qinaixin2015@sina.com

1005-6734(2016)03-0366-06

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