專利名稱：一種動(dòng)態(tài)環(huán)境中的視覺(jué)定位方法
技術(shù)領(lǐng)域：
:本發(fā)明是涉及圖像處理及機(jī)器視覺(jué)的移動(dòng)機(jī)器人自定位技術(shù)，該技術(shù)可以在復(fù)雜的動(dòng)態(tài)場(chǎng)景中以視覺(jué)信息作為輸入實(shí)現(xiàn)移動(dòng)機(jī)器人自定位。
背景技術(shù)：
:在智能機(jī)器人領(lǐng)域，移動(dòng)機(jī)器人定位與導(dǎo)航技術(shù)一直是研究的關(guān)鍵問(wèn)題。定位的精度及速度直接影響機(jī)器人完成既定任務(wù)的效率和準(zhǔn)確度。之前的定位方法，如:編碼器或輪式里程計(jì)等無(wú)法克服由摩擦、滑動(dòng)等非線性因素引起的定位誤差，近幾年興起的視覺(jué)定位方法可以很好地解決非線性誤差，但現(xiàn)有的視覺(jué)定位方法存在一定缺陷:1:當(dāng)環(huán)境中存在紋理相似區(qū)域，及大量相似的重復(fù)性結(jié)構(gòu)時(shí)，容易引起特征點(diǎn)的誤匹配，導(dǎo)致定位錯(cuò)誤。2:環(huán)境中存在大量獨(dú)立運(yùn)動(dòng)物體時(shí)，位于動(dòng)態(tài)物體上的特征點(diǎn)會(huì)干擾里程計(jì)結(jié)果，導(dǎo)致定位精度降低。為了使視覺(jué)定位技術(shù)可以在復(fù)雜的動(dòng)態(tài)場(chǎng)景中得以應(yīng)用，需要采用一定的方法來(lái)剔除位于動(dòng)態(tài)物體上的特征點(diǎn)，然而目前的算法，如離群度測(cè)量算法，圖像理解算法等均不能同時(shí)滿足定位的實(shí)時(shí)性及有效性要求。如何讓視覺(jué)里程計(jì)在動(dòng)態(tài)環(huán)境中實(shí)時(shí)有效地工作成為本專利申請(qǐng)人關(guān)注和研究的新課題
發(fā)明內(nèi)容
:本發(fā)明的目的在于解決現(xiàn)有技術(shù)中存在的問(wèn)題，提供一種可以在復(fù)雜動(dòng)態(tài)環(huán)境中應(yīng)用的視覺(jué)定位算法，使得移動(dòng)機(jī)器人可以依靠視覺(jué)信息作為輸入，完成在復(fù)雜環(huán)境中的自定位任務(wù)。本發(fā)明的技術(shù)方案包括以下步驟:1:用已標(biāo)定好的雙目相機(jī)，進(jìn)行左右圖像連續(xù)采集。2:采集當(dāng)前時(shí)刻左右兩幅圖像，使用sobel算子提取圖像中的特征點(diǎn)，并利用SIFT描述符進(jìn)行描述，通過(guò)計(jì)算左右圖像各點(diǎn)間特征描述符的歐式距離進(jìn)行匹配，使用三維重建算法將特征點(diǎn)二維圖像坐標(biāo)轉(zhuǎn)換為三維空間坐標(biāo)，初始化位姿變換矩陣為單位矩陣，并設(shè)當(dāng)前位置為原點(diǎn)。如果圖像是第一幀，從新進(jìn)入步驟2 ;如果圖像不是第一幀進(jìn)入步驟3。3:將前時(shí)刻獲取的左右兩幀圖像中的特征點(diǎn)，與前一時(shí)刻的兩幀圖像一起進(jìn)行特征點(diǎn)的立體匹配，并通過(guò)回環(huán)匹配增加匹配準(zhǔn)確度，得到各幀圖像之間特征點(diǎn)的對(duì)應(yīng)關(guān)系，利用雙視幾何原理求解出特征點(diǎn)在兩幀圖像中相對(duì)于攝像機(jī)坐標(biāo)系的坐標(biāo)。4:使用場(chǎng)流法對(duì)匹配后的特征點(diǎn)集進(jìn)行計(jì)算，求出該點(diǎn)在t-ι時(shí)刻的位置經(jīng)過(guò)位姿變換矩陣RT投影以后的三維坐標(biāo)與該點(diǎn)t時(shí)刻三維坐標(biāo)的差值，差值大于閾值的點(diǎn)認(rèn)為是位于動(dòng)態(tài)物體上的特征點(diǎn)，將其從特征點(diǎn)集中刪除。其詳細(xì)步驟如下:4-1.由于機(jī)器人的運(yùn)動(dòng)過(guò)程是連續(xù)的，所以認(rèn)為機(jī)器人在t時(shí)刻的位姿變化與t-ι時(shí)刻位姿變化近似相等，以t-ι時(shí)刻的RT作為t時(shí)刻RT的估計(jì)值。
4-2.利用4-1的結(jié)果，考慮相機(jī)成像時(shí)的像素位置誤差，計(jì)算估計(jì)RT的不準(zhǔn)確度，求出場(chǎng)流法的誤差模型。4-3.計(jì)算每個(gè)點(diǎn)對(duì)應(yīng)于該模型的馬氏距離值，值越大說(shuō)明其離群度較高，將大于一定閾值的點(diǎn)從點(diǎn)集中刪除。5:使用高斯-牛頓迭代算法，以當(dāng)前時(shí)刻和前一時(shí)刻的特征點(diǎn)集作為輸入量求解機(jī)器人當(dāng)前時(shí)刻相對(duì)前一時(shí)刻的位姿變化，同時(shí)使用RANSIC算法進(jìn)一步提高定位準(zhǔn)確度。5-1.在特征點(diǎn)集中隨機(jī)選擇3對(duì)特征點(diǎn)，通過(guò)迭代算法，進(jìn)行一次運(yùn)動(dòng)估計(jì)，得到一個(gè) RT。5-2.利用當(dāng)前的RT，對(duì)所有前一時(shí)刻特征點(diǎn)進(jìn)行投影，對(duì)比投影結(jié)果與當(dāng)前觀測(cè)位置的差，將差值小于閾值的點(diǎn)記為局內(nèi)點(diǎn)。5-3.重復(fù)5-1、5_2，進(jìn)行N次迭代，比較每次局內(nèi)點(diǎn)數(shù)目，將局內(nèi)點(diǎn)數(shù)目最大的一組局內(nèi)點(diǎn)重新構(gòu)成特征點(diǎn)集。5-4.利用新的特征點(diǎn)集，使用高斯-牛頓迭代進(jìn)行運(yùn)動(dòng)估計(jì)，得到更優(yōu)的R和T。6:使用步驟5的結(jié)果對(duì)全局的RT進(jìn)行更新，得到機(jī)器人相對(duì)于初始坐標(biāo)系的位置和姿態(tài)。本發(fā)明的優(yōu)點(diǎn)在于:I)本發(fā)明通過(guò)回環(huán)匹配來(lái)增加匹配穩(wěn)定性，減小了因誤匹配引入的誤差。2)本發(fā)明使用場(chǎng)流法對(duì)特征點(diǎn)進(jìn)行處理，可以將處于運(yùn)動(dòng)物體上的特征點(diǎn)剔除出去，使視覺(jué)定位可以在復(fù)雜的動(dòng)態(tài)場(chǎng)景中實(shí)時(shí)有效的工作。3)本發(fā)明在使用場(chǎng)流法時(shí)，考慮了可能產(chǎn)生誤差的因素，求解其誤差模型，計(jì)算各點(diǎn)對(duì)應(yīng)該模型的馬氏距離，使得對(duì)局外點(diǎn)的判斷可以通過(guò)統(tǒng)一的閾值來(lái)決定。4)在視覺(jué)定位過(guò)程中使用了 RANSIC算法，該方法與場(chǎng)流法結(jié)合使用，提高了視覺(jué)定位方法的實(shí)時(shí)性與有效性


:圖1是本發(fā)明提出的動(dòng)態(tài)環(huán)境中視覺(jué)定位方法總體流程圖。圖2是本發(fā)明中定義的各個(gè)坐標(biāo)系。圖3是本發(fā)明中使用的回環(huán)匹配算法示意圖。圖4是本發(fā)明中場(chǎng)流法的計(jì)算流程圖。圖5是本發(fā)明中RANSIC算法流程圖。
具體實(shí)施方式
:下面結(jié)合說(shuō)明書(shū)附圖對(duì)本發(fā)明方法做詳細(xì)說(shuō)明:步驟1:對(duì)雙目攝像機(jī)進(jìn)行標(biāo)定，獲取相機(jī)內(nèi)外參數(shù)，包括:焦距f，基線長(zhǎng)度b，圖像中心像素位置Utl, Vtl，整幅圖像的校正矩陣等。步驟2:開(kāi)啟雙目相機(jī)，進(jìn)行左右圖像連續(xù)采集，利用步驟I獲取的相機(jī)參數(shù)對(duì)圖像進(jìn)行校正。步驟3:如果圖像是第一幀，對(duì)采集的左右圖像使用sobel算子提取特征，并利用SIFT描述符進(jìn)行描述，在左右圖像之間進(jìn)行特正匹配，得到特征點(diǎn)對(duì)應(yīng)關(guān)系。如果圖像不是第一幀，則將前后兩個(gè)時(shí)刻共四幅圖像進(jìn)行特征匹配，匹配通過(guò)回環(huán)檢測(cè)的方式提高準(zhǔn)確度，如說(shuō)明書(shū)附圖3:從當(dāng)前時(shí)刻左圖像開(kāi)始，按照順序進(jìn)行立體匹配，如果最后通過(guò)匹配可以找到該特征點(diǎn)本身，則將特征點(diǎn)信息存儲(chǔ)于點(diǎn)集中，否則予以舍棄。步驟4:利用步驟3的結(jié)果，使用場(chǎng)流法對(duì)特征點(diǎn)進(jìn)行篩選，使用立體幾何原理求
得像素點(diǎn)對(duì)應(yīng)的三維坐標(biāo)值
權(quán)利要求
1.一種可以在復(fù)雜動(dòng)態(tài)場(chǎng)景中應(yīng)用的視覺(jué)定位方法，其特征在于，包括以下步驟: 步驟一:對(duì)雙目攝像機(jī)進(jìn)行標(biāo)定。
步驟二:開(kāi)啟相機(jī)，進(jìn)行左右圖像的連續(xù)采集。
步驟三:如果圖像是第一幀，SObel算子提取圖像中的特征點(diǎn)位置，并利用SIFT描述符進(jìn)行描述，在左右圖像之間進(jìn)行特正匹配，得到特征點(diǎn)對(duì)應(yīng)關(guān)系。如果圖像不是第一幀，則在前后兩個(gè)時(shí)刻共四幅圖像間進(jìn)行特征匹配，匹配通過(guò)回環(huán)檢測(cè)的方式提高準(zhǔn)確度。
步驟四:通過(guò)雙視幾何的原理計(jì)算特征像素點(diǎn)對(duì)應(yīng)的三維空間位置，使用場(chǎng)流法對(duì)特征點(diǎn)進(jìn)行篩選，考慮計(jì)算場(chǎng)流過(guò)程中可能引入的誤差，通過(guò)計(jì)算每個(gè)點(diǎn)對(duì)應(yīng)的馬氏距離，將離群度大于一定閾值的點(diǎn)從特征點(diǎn)集中刪除。
步驟五:使用RANSIC算法進(jìn)行迭代，每次迭代時(shí)利用求得的姿態(tài)R和位置T計(jì)算滿足該變換的特征點(diǎn)數(shù)目，求取包含局內(nèi)點(diǎn)最多一組結(jié)果，并利用該組中所有局內(nèi)點(diǎn)求解更準(zhǔn)確的R和T。
步驟六:將步驟五求得的結(jié)果進(jìn)行累乘，得到機(jī)器人在當(dāng)前時(shí)刻相對(duì)于初始時(shí)刻的位姿變換矩陣。
2.權(quán)利要求1所述的一種動(dòng)態(tài)場(chǎng)景中視覺(jué)定位方法，其特征在于，所述步驟三中采用回環(huán)匹配的方法減小誤匹配概率。
3.權(quán)利要求1所述的一種動(dòng)態(tài)場(chǎng)景中視覺(jué)定位方法，其特征在于，所述步驟四中考慮了場(chǎng)流法的誤差模型，通過(guò)每一組特征點(diǎn)對(duì)應(yīng)的馬氏距離，將離群度大于閾值的特征點(diǎn)對(duì)從點(diǎn)集中刪除，達(dá)到排除位于動(dòng)態(tài)物體上的特征點(diǎn)的效果。
步驟4-1計(jì)算場(chǎng)流法誤差模型，最初的像素點(diǎn)誤差成高斯分布，計(jì)算定位空間點(diǎn)三維位置的誤差。
步驟4-2利用4-1求得的誤差模型以及視覺(jué)定位模型的雅克比矩陣，求得估計(jì)出的RT的誤差模型。
步驟4-3利用4-2的結(jié)果以及和場(chǎng)流法對(duì)應(yīng)的雅克比矩陣，求得場(chǎng)流法的誤差模型。
步驟4-4利用4-3求得的場(chǎng)流誤差模型計(jì)算每一特征點(diǎn)對(duì)應(yīng)的馬氏距離。
4.權(quán)利要求1所述的一種動(dòng)態(tài)場(chǎng)景中視覺(jué)定位方法，其特征在于，使用RANSIC算法，每次選取三個(gè)特征點(diǎn)對(duì)計(jì)算RT，對(duì)所有特征點(diǎn)對(duì)按照計(jì)算出的RT進(jìn)行投影，統(tǒng)計(jì)局內(nèi)點(diǎn)數(shù)目，迭代N次后，以局內(nèi)點(diǎn)數(shù)目最大的一組特征點(diǎn)對(duì)為輸入，估計(jì)得到更精確的R和T。
全文摘要
本發(fā)明公開(kāi)了一種可以在動(dòng)態(tài)環(huán)境中實(shí)現(xiàn)精確定位的立體視覺(jué)定位方法。通過(guò)雙目攝像機(jī)實(shí)時(shí)采集環(huán)境圖像。通過(guò)特征提取算法提取圖像中的特征點(diǎn)。對(duì)雙目攝像機(jī)前后兩個(gè)時(shí)刻采集的四幅圖像進(jìn)行立體匹配。利用雙視幾何原理恢復(fù)特征點(diǎn)三維信息。以回環(huán)匹配的方式提高匹配準(zhǔn)確度。使用場(chǎng)流法剔除位于動(dòng)態(tài)物體上的特征點(diǎn)?？紤]可能引起誤差的因素并通過(guò)計(jì)算協(xié)方差矩陣改善場(chǎng)流法效果。使用高斯牛頓迭代的方法由特征點(diǎn)位置信息求得機(jī)器人運(yùn)動(dòng)參數(shù)。使用RANSIC算法進(jìn)一步提高視覺(jué)定位準(zhǔn)確度。整個(gè)過(guò)程不斷迭代，實(shí)現(xiàn)對(duì)機(jī)器人姿態(tài)和位置的實(shí)時(shí)計(jì)算。
文檔編號(hào)G06T7/00GK103150728SQ20131006747
公開(kāi)日2013年6月12日 申請(qǐng)日期2013年3月4日 優(yōu)先權(quán)日2013年3月4日
發(fā)明者賈慶軒, 葉平, 王軒, 孫漢旭, 張?zhí)焓? 竇仁銀 申請(qǐng)人:北京郵電大學(xué)