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

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