專利名稱：一種利用視差空間集束調(diào)整的高精度實時立體視覺定位方法
技術(shù)領(lǐng)域：
本發(fā)明屬于計算機視覺自主導(dǎo)航技術(shù)領(lǐng)域，涉及一種基于視差空間集束調(diào)整的高精度實時立體視覺定位方法，可以應(yīng)用于自主移動機器人、智能車輛等領(lǐng)域。
背景技術(shù)：
在未知環(huán)境中自主導(dǎo)航是各種移動機器人或智能車應(yīng)用的關(guān)鍵任務(wù)之一。機器人在其長距離漫游過程中需要實時地獲得自身的定位信息。目前，由于視覺傳感器的性價比日益提高以及視覺計算的發(fā)展，基于視覺的導(dǎo)航定位方法正開始廣泛運用于各個相關(guān)領(lǐng)域。視覺導(dǎo)航定位方法依賴硬件設(shè)備少、對體積、功耗、載荷等要求低，適用于各種復(fù)雜未知環(huán)境，并能夠填補GPS衛(wèi)星定位的盲區(qū)，有效糾正輪式里程計和慣性導(dǎo)航的累計誤差。因此，采用視覺定位方法已經(jīng)成為機器人自主導(dǎo)航的必然趨勢。實時立體視覺定位算法一般利用二幀之間的特征匹配信息進行運動估計，對運動估計參數(shù)進行級聯(lián)計算出攝像機定位信息，這種方法使小的運動估計誤差不斷積累，從而導(dǎo)致長距離行進下定位精度不高。經(jīng)對現(xiàn)有技術(shù)的文獻檢索發(fā)現(xiàn)，E. Mouragnon等人于2006年《2006 Proceedings of theIEEE Conference on Computer Vision and Pattern Recognition))上發(fā)表的論文 ^Real-TimeLocalization and 3D Reconstruction，，以及 N. Sunderhauf 等人于 2006 年 《2006 Proceedings ofTowards Autonomous Robotic Systems》上發(fā)表的論文 “Towards Using Bundle Adjustment forRobust Stereo Odometry in Outdoor Terrain，，提出了利用集束調(diào)整進行實時視覺定位。集束調(diào)整采用公共坐標系統(tǒng)同時優(yōu)化3D點和攝像機位姿參數(shù)，由于其可以利用多幀之間的特征匹配信息，所以作為最后一步優(yōu)化步驟可大大提高立體視覺定位的精度。但集束調(diào)整優(yōu)化方法對初始值依賴太大，由于遠距離特征點或長的運動序列引起的弱結(jié)構(gòu)會造成收斂速度慢或收斂發(fā)散等數(shù)值穩(wěn)定性低的缺點。

發(fā)明內(nèi)容
本發(fā)明的目的在于克服現(xiàn)有技術(shù)中的不足和缺點，提出一種利用視差空間集束調(diào)整的高精度實時立體視覺定位方法。相對于集束調(diào)整，由于視差空間集束調(diào)整采用視差空間特征點投影約束和相對坐標表達，所以其參數(shù)優(yōu)化的數(shù)值穩(wěn)定性高，收斂速度快，對初始值依賴小，魯棒性高，從而使自主導(dǎo)航車在長距離行進下也能夠進行精度實時的定位。為此，本發(fā)明采用以下技術(shù)方案，其步驟如下步驟一、對視頻流當(dāng)前幀的立體圖像對進行極線校正，視頻流的第一幀選為起點幀；視頻流由立體相機的攝像頭拍攝形成；步驟二、提取第一圖像的CenSurE特征和U-SURF描述符，所述第一圖像為所述立體圖像對中的左圖像和右圖像之一；步驟三、采用SAD區(qū)域匹配方法沿第二圖像水平方向極線進行一維搜索得到第一圖像CenSurE特征對應(yīng)的圖像坐標，從而得到特征點的3D坐標；所述第二圖像為所述立體圖像對中的左圖像和右圖像之另一；所述特征點為攝像頭拍攝的同一場景圖像關(guān)鍵點，具體反映在立體圖像第一圖像CenSurE特征和第二圖像對應(yīng)圖像坐標；若當(dāng)前幀為起點幀，則將當(dāng)前幀的下一幀作為當(dāng)前幀，重復(fù)步驟一至三的處理步驟；否則，對所述當(dāng)前幀再進行步驟四和五；步驟四、對當(dāng)前幀和前一幀立體圖像的特征點集進行一一匹配；步驟五、采用RANSAC算法和2D-3D位姿估計方法得到匹配內(nèi)點和拍攝當(dāng)前幀時的所述攝像頭運動參數(shù)，包括相對位置、相對姿態(tài)；所述相對位置為平移參數(shù)t，所述相對姿態(tài)為旋轉(zhuǎn)參數(shù)R ；步驟六、如步驟五所述的當(dāng)前巾貞與起點巾貞之間的巾貞數(shù)、起點巾貞和當(dāng)前巾貞的總和小于N，將步驟五所述的當(dāng)前幀的下一幀作為當(dāng)前幀，對所述當(dāng)前幀進行步驟一至五的處理， 否者，進行步驟七和步驟八的處理；N = 2*n+l，n為大于等于I的自然數(shù)；步驟七、利用當(dāng)前幀前的N幀中的兩兩相鄰幀的匹配內(nèi)點之間的視差空間投影約束，采用視差空間集束調(diào)整對拍攝當(dāng)前幀的前n幀攝像頭的運動參數(shù)進行迭代優(yōu)化；如步驟七所述的當(dāng)前幀與起點幀之間的幀數(shù)等于N，則所述迭代優(yōu)化的初值為當(dāng)前幀前的n幀的攝像頭運動參數(shù)；否則，所述迭代優(yōu)化的初值為當(dāng)前幀前的n-1幀在視差空間集束調(diào)整后的攝像頭運動參數(shù)和當(dāng)前幀運動參數(shù)；步驟八、利用拍攝所述當(dāng)前幀前第I幀時攝像頭的全局位置姿態(tài)參數(shù)和拍攝所述當(dāng)前幀時攝像頭運動參數(shù)的迭代優(yōu)化值，得到拍攝當(dāng)前幀時攝像頭的全局位置姿態(tài)參數(shù)作為拍攝實時幀時攝像頭的全局位置姿態(tài)參數(shù)，以對攝像頭實時立體視覺定位。在采用上述技術(shù)方案的基礎(chǔ)上，本發(fā)明還可采用以下進一步的技術(shù)方案在所述步驟一中，采用以下步驟利用已標定立體相機的參數(shù)，采用極線校正方法對立體圖像對進行校正，使立體圖像對的左右圖像對應(yīng)的特征點位于同一水平掃描線上，同時得到校正后立體相機基線長度B和焦距f。在所述步驟二中，采用以下步驟(I)、計算得到第一圖像的積分圖像；(2)、利用積分圖像在每個像素上計算六個尺度的盒子核近似高斯拉普拉斯濾波器響應(yīng)；(3)、采用非極大值抑制檢測局部極大值得到CenSurE特征,所述CenSurE特征包括其圖像坐標(u，v)、尺度s及極大值；(4)、利用積分圖像和CenSurE特征的圖像坐標(u, v)和尺度s計算對應(yīng)的U-SURF 描述符。在所述步驟三中，采用以下步驟對第一圖像的每個CenSurE特征在第二圖像采用簡單的SAD立體匹配方法沿水平方向極線進行一維搜索匹配，從而得到第一圖像CenSurE特征在第二圖像上的對應(yīng)位置坐標(u2，V)，則得到整數(shù)視差d = u2-u ;采用如下二次函數(shù)對視差值進行擬合，從而達到亞象素級視差
權(quán)利要求
1.一種利用視差空間集束調(diào)整的高精確實時立體視覺定位方法，其特征在于它包括以下步驟步驟一、對視頻流當(dāng)前幀的立體圖像對進行極線校正，視頻流的第一幀選為起點幀；視頻流由立體相機的攝像頭拍攝形成；步驟二、提取第一圖像的CenSurE特征和U-SURF描述符，所述第一圖像為所述立體圖像對中的左圖像和右圖像之一；步驟三、采用SAD區(qū)域匹配方法沿第二圖像水平方向極線進行一維搜索得到第一圖像 CenSurE特征對應(yīng)的圖像坐標，從而得到特征點的3D坐標；所述第二圖像為所述立體圖像對中的左圖像和右圖像之另一；所述特征點為攝像頭拍攝的同一場景圖像關(guān)鍵點，具體反映在立體圖像第一圖像CenSurE特征和第二圖像對應(yīng)圖像坐標；若當(dāng)前幀為起點幀，則將當(dāng)前幀的下一幀作為當(dāng)前幀，重復(fù)步驟一至三的處理步驟；否則，對所述當(dāng)前幀再進行步驟四和五；步驟四、對當(dāng)前幀和前一幀立體圖像的特征點集進行一一匹配；步驟五、采用RANSAC算法和2D-3D位姿估計方法得到匹配內(nèi)點和拍攝當(dāng)前幀時的所述攝像頭運動參數(shù)，包括相對位置、相對姿態(tài)；所述相對位置為平移參數(shù)t，所述相對姿態(tài)為旋轉(zhuǎn)參數(shù)R ；步驟六、如步驟五所述的當(dāng)前幀與起點幀之間的幀數(shù)、起點幀和當(dāng)前幀的總和小于N， 將步驟五所述的當(dāng)前幀的下一幀作為當(dāng)前幀，對所述當(dāng)前幀進行步驟一至五的處理，否者， 進行步驟七和步驟八的處理；N = 2*n+l，n為大于等于I的自然數(shù)；步驟七、利用當(dāng)前幀前的N幀中的兩兩相鄰幀的匹配內(nèi)點之間的視差空間投影約束， 采用視差空間集束調(diào)整對拍攝當(dāng)前幀的前n幀攝像頭的運動參數(shù)進行迭代優(yōu)化；如步驟七所述的當(dāng)前幀與起點幀之間的幀數(shù)等于N，則所述迭代優(yōu)化的初值為當(dāng)前幀前的n幀的攝像頭運動參數(shù)；否則，所述迭代優(yōu)化的初值為當(dāng)前幀前的n-1幀在視差空間集束調(diào)整后的攝像頭運動參數(shù)和當(dāng)如巾貞運動參數(shù)；步驟八、利用拍攝所述當(dāng)前幀前第I幀時攝像頭的全局位置姿態(tài)參數(shù)和拍攝所述當(dāng)前幀時攝像頭運動參數(shù)的迭代優(yōu)化值，得到拍攝當(dāng)前幀時攝像頭的全局位置姿態(tài)參數(shù)作為拍攝實時幀時攝像頭的全局位置姿態(tài)參數(shù)，以對攝像頭實時立體視覺定位。
2.根據(jù)權(quán)利要求I所述的利用視差空間集束調(diào)整的高精度實時立體視覺定位方法,其特征在于在所述步驟一中，采用以下步驟利用已標定立體相機的參數(shù)，采用極線校正方法對立體圖像對進行校正，使立體圖像對的左右圖像對應(yīng)的特征點位于同一水平掃描線上，同時得到校正后立體相機基線長度B 和焦距f。
3.根據(jù)權(quán)利要求I所述的利用視差空間集束調(diào)整的高精度實時立體視覺定位方法,其特征在于在所述步驟二中，采用以下步驟(1)、計算得到第一圖像的積分圖像；(2)、利用積分圖像在每個像素上計算六個尺度的盒子核近似高斯拉普拉斯濾波器響(3)、采用非極大值抑制檢測局部極大值得到CenSurE特征，所述CenSurE特征包括其圖像坐標(u, V)、尺度s及極大值；(4)、利用積分圖像和CenSurE特征的圖像坐標(u, V)和尺度s計算對應(yīng)的U-SURF描述符。
4.根據(jù)權(quán)利要求I或3所述的利用視差空間集束調(diào)整的高精度實時立體視覺定位方法，其特征在于在所述步驟三中，采用以下步驟對第一圖像的每個CenSurE特征在第二圖像采用簡單的SAD立體匹配方法沿水平方向極線進行一維搜索匹配，從而得到第一圖像CenSurE特征在第二圖像上的對應(yīng)位置坐標 (u2, V)，則得到整數(shù)視差d = u2-u ;采用如下二次函數(shù)對視差值進行擬合，從而達到亞象素級視差
5.根據(jù)權(quán)利要求I或3所述的利用視差空間集束調(diào)整的高精度實時立體視覺定位方法，其特征在于在所述步驟四中，采用以下步驟進行匹配取當(dāng)前幀第一圖像中的某個特征點U-SURF描述符，與前一幀第一圖像中在某個視差范圍(-L，L)內(nèi)的特征點U-SURF描述符計算二者之間的歐式距離，如果最近的歐式距離與次近的歐式距離的比值少于某個比例閥值并且最近鄰的距離小于某個閥值時則接受這一對特征點為匹配點，否則拒絕；在匹配過程中只對極大值同為正值或同為負值的CenSurE 特征的U-SURF描述符進行匹配；重復(fù)以上步驟，得到至少3對匹配點。
6.根據(jù)權(quán)利要求5所述的利用視差空間集束調(diào)整的高精度實時立體視覺定位方法，其特征在于在所述步驟五中，采用以下步驟(1)、在步驟四得到的匹配點集中隨機采樣3對匹配點；(2)、利用2D-3D位姿估計算法和3對匹配點計算拍攝當(dāng)前幀時攝像頭的運動參數(shù)R和t ；(3)、利用該運動參數(shù)R和t計算所有匹配點在立體相機上的再投影誤差
7.根據(jù)權(quán)利要求I所述的利用視差空間集束調(diào)整的高精度實時立體視覺定位方法,其特征在于在所述步驟七中，采用以下步驟(I)、把旋轉(zhuǎn)矩陣 R 轉(zhuǎn)化為四元數(shù)(qp q2，q3，Q4)，其中W + cIl +cIs =1，貝1J Qi Q2^ Q3 為獨立變量，加上平移參數(shù)t = [X y z]T,則相鄰二巾貞之間的運動參數(shù)Θ t = Ilq1 q2 q3 X y ζ]τ為6維向量；(2)、利用相鄰二幀之間的匹配內(nèi)點得到最近N幀圖像任意幀之間的匹配點，利用Pk,j 表示幀j到幀k之間的匹配點數(shù)；(3)、通過拍攝相鄰兩幀的攝像頭之間的的運動參數(shù)級聯(lián)表達得到拍攝任意非相鄰幀攝像頭之間的運動參數(shù)；相鄰二幀攝像頭之間的運動參數(shù)即為拍攝各幀時的攝像頭的運動參數(shù)；(4)、采用增量式局部參數(shù)更新優(yōu)化方式對運動參數(shù)進行優(yōu)化，本次優(yōu)化得到的參數(shù)作為下一輪優(yōu)化的初始值；在當(dāng)前幀t，僅優(yōu)化其之前的n幀的運動參數(shù)，但包含其之前的N 幀的視差空間特征點投影約束，設(shè)局部視差空間集束調(diào)整的目標函數(shù)為F( 0 )，待優(yōu)化的運動參數(shù)為0 = { 0 t_n+1, ... 0 J ,則目標函數(shù)表達如下
8.根據(jù)權(quán)利要求7所述的利用視差空間集束調(diào)整的高精度實時立體視覺定位方法，其特征在于在所述步驟八中，采用以下步驟(1)、利用拍攝當(dāng)前幀前第n+1幀時攝像頭相對于世界坐標系的旋轉(zhuǎn)參數(shù)R和平移參數(shù) t、拍攝當(dāng)前幀前第n幀時攝像頭的運動參數(shù)0 t_n+1計算得到拍攝當(dāng)前幀前第n幀時攝像頭相對于世界坐標系的旋轉(zhuǎn)參數(shù)和平移參數(shù)；(2)、利用拍攝當(dāng)前幀前第n+1幀時攝像頭相對于世界坐標系的各個旋轉(zhuǎn)參數(shù)R和平移參數(shù)t、拍攝當(dāng)前巾貞前n巾貞的攝像頭運動參數(shù)0 = {0t_n+1,... Q J計算得到拍攝當(dāng)前巾貞時攝像頭相對于世界坐標系的旋轉(zhuǎn)參數(shù)R和平移參數(shù)t ;拍攝當(dāng)前幀時攝像頭相對于世界坐標系的旋轉(zhuǎn)參數(shù)和平移參數(shù)即為立體相機6個自由度的3D定位信息。
全文摘要
本發(fā)明提出了一種利用視差空間集束調(diào)整的高精度實時立體視覺定位方法。本發(fā)明僅利用立體相機得到的視頻流進行移動機器人的導(dǎo)航定位，具有完全的自主性；本發(fā)明利用U-SURF描述符進行特征匹配，對運動過程中出現(xiàn)的光照變化、運動模糊、大的旋轉(zhuǎn)和尺度變換具有較強的魯棒性；本發(fā)明利用視差空間集束調(diào)整優(yōu)化方法使自主導(dǎo)航車在長距離行進下也能夠進行精度實時的定位，其具有對優(yōu)化初始值依賴程度低，收斂速度快，數(shù)值穩(wěn)定性高等優(yōu)點，與利用集束調(diào)整的立體視覺定位方法相比，定位精度更高，速度更快。本發(fā)明整個立體視覺定位方法計算速度快，可在線實時運行。
文檔編號H04N13/00GK102607535SQ20121002646
公開日2012年7月25日 申請日期2012年2月7日 優(yōu)先權(quán)日2012年2月7日
發(fā)明者劉勇, 蔣云良, 許允喜 申請人:湖州師范學(xué)院