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

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

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

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

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

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

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

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

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

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

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

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

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

1 1點(diǎn)RANSAC單目視覺EKF算法

1.1 預(yù)測(cè)和搜索特征匹配點(diǎn)

在上述方程中，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ù)測(cè)值xk|k-1，通過觀測(cè)模型 hi可以得到觀測(cè)量的預(yù)測(cè)值，如式(3)和式(4)。采用主動(dòng)視覺匹配方法查找匹配點(diǎn)，提高搜索效率。

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

1.2 建立和估計(jì)1點(diǎn)狀態(tài)假設(shè)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖1 針孔攝像機(jī)模型Fig.1 Pinhole camera model

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

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

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

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

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

上述兩節(jié)分別從系統(tǒng)誤差的產(chǎn)生以及EKF濾波發(fā)散的內(nèi)在機(jī)理兩個(gè)方面闡述了濾波發(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，得：

對(duì)于N時(shí)刻的濾波，為了克服濾波發(fā)散，就應(yīng)相應(yīng)地突出KN，而相對(duì)地減少N時(shí)刻以前的 Kk。由式(14)知，Kk與Rk成反比關(guān)系，因此，為了達(dá)到上述目的，可使離N時(shí)刻越來越遠(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時(shí)刻以前觀測(cè)值逐漸衰減。同樣，對(duì)系統(tǒng)干擾噪聲也給以 Sa加權(quán)：

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

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

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

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

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

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

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

3 實(shí)驗(yàn)驗(yàn)證分析

圖 2 實(shí)驗(yàn)所用轉(zhuǎn)臺(tái)及攝像機(jī)Fig.2 Turntable and camera used in the experiment

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

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

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

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

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

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

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

實(shí)驗(yàn)2：驗(yàn)證本文算法的有效性

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

圖6 原算法對(duì)攝像機(jī)姿態(tài)運(yùn)動(dòng)的估計(jì)結(jié)果Fig.6 Estimation results of the original algorithm in attitude angle of the camera

圖7 本文算法對(duì)攝像機(jī)姿態(tài)運(yùn)動(dòng)的估計(jì)結(jié)果Fig.7 Estimation results of the proposed algorithm in attitude angle of the camera

圖8 攝像機(jī)航向角誤差Fig.8 Course angle error of camera

圖9 攝像機(jī)俯仰角誤差Fig.9 Pitch angle error of the camera

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

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

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

圖10 原算法對(duì)攝像機(jī)姿態(tài)運(yùn)動(dòng)的估計(jì)結(jié)果Fig.10 Estimation results of the original algorithm in attitude angle of the camera

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

4 結(jié) 論

本文從單目視覺系統(tǒng)的模型誤差出發(fā)，分析了1點(diǎn)RANSAC單目視覺EKF算法中濾波發(fā)散問題的產(chǎn)生原因，提出了一種基于漸消記憶濾波的 1點(diǎn)RANSAC單目視覺姿態(tài)估計(jì)算法，解決了 1點(diǎn)RANSAC單目視覺EKF算法中的濾波發(fā)散問題，為進(jìn)一步提高單目視覺 EKF算法的估計(jì)精度提供了一種新的思路。由于本文中的加權(quán)因子是通過實(shí)驗(yà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 點(diǎn) RANSAC 的無人直升機(jī)單目視覺 SLAM[J]. 機(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] 李捐. 基于單目視覺的移動(dòng)機(jī)器人 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

國(guó)家自然科學(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