專利名稱：一種雙目視覺里程計實現(xiàn)方法
技術(shù)領(lǐng)域：
本發(fā)明涉及移動機器人自主導(dǎo)航技術(shù)領(lǐng)域，尤其是一種應(yīng)用于室內(nèi)環(huán)境中輪驅(qū)式移動機器人自主導(dǎo)航技術(shù)中的雙目視覺里程計實現(xiàn)方法。
背景技術(shù)：
移動機器人研究的目標是在沒有人干預(yù)且無需對環(huán)境作任何規(guī)定和改變的情況下，有目的地移動和完成相應(yīng)任務(wù)。在實際應(yīng)用中，當機器人要自主的完成一項任務(wù)時，最重要的是進行準確的自定位，即機器人需要知道自己現(xiàn)在所處的位置和姿態(tài)。通常輪驅(qū)式移動機器人自定位采用航跡推算的方法利用兩個輪子加裝的里程計測出的微小位移，計算出移動機器人的位置和姿態(tài)，通過累加實現(xiàn)移動機器人的自定位。這種自定位方法的最大問題是驅(qū)動輪空轉(zhuǎn)或打滑情況下累積誤差增大。此外，傳感器精度降低、輪子半徑的變化等情況下也會引起里程計誤差增大。為降低自定位誤差并提高定位對環(huán)境的魯棒性，計算機視覺被應(yīng)用于機器人自定位中，構(gòu)成視覺里程計。視覺里程計利用機器人攝像機采集的序列圖像，通過特征跟蹤和相對運動估計得出機器人自身的運動參數(shù)。與傳統(tǒng)定位方式相比，視覺里程計采用被動視覺傳感器，結(jié)構(gòu)簡單、信息量大，只依靠視覺輸入信息即可完成定位，無需場景及運動的先驗知識，且不存在傳感器精度降低導(dǎo)致的誤差，適用于各種環(huán)境。
發(fā)明內(nèi)容
本發(fā)明的目的在于實現(xiàn)一種視覺里程計，該里程計可實現(xiàn)高精度、實時里程信息測量，并避免了機器人驅(qū)動輪半徑變化及輪子空轉(zhuǎn)、打滑等情況下里程信息累計誤差的增大。本發(fā)明所提出的一種雙目視覺里程計實現(xiàn)方法，其特征在于，該方法包括以下步驟步驟1，使用雙目攝像機采集室內(nèi)天花板圖像對，并對兩幅圖像分別提取連接點特征；步驟2，采用點模式匹配方法建立雙目攝像機采集到的圖像對的特征匹配關(guān)系，進而根據(jù)立體視覺算法計算匹配點對對應(yīng)的三維坐標；步驟3，使用步驟2的方法得到攝像機采集到的圖像序列中的各相鄰圖像之間的匹配點對的三維對應(yīng)關(guān)系；步驟4，根據(jù)步驟3得到的匹配點對的三維對應(yīng)關(guān)系計算攝像機的相對運動參數(shù)， 并采用分段優(yōu)化算法對其進行優(yōu)化；步驟5，根據(jù)攝像機與機器人的相對位置關(guān)系將攝像機的相對運動參數(shù)轉(zhuǎn)化為機器人運動參數(shù)，并更新機器人運動的里程信息。本發(fā)明能產(chǎn)生如下有益效果利用攝像頭對構(gòu)成立體視覺傳感器，安裝于移動機器人頂部，拍攝室內(nèi)天花板場景，通過對相鄰連續(xù)幀的處理，以分段優(yōu)化算法估計攝像機運動參數(shù)，實現(xiàn)移動機器人里程信息的推算。視覺里程計不依賴于對輪軸旋轉(zhuǎn)的計數(shù)，從而避免了車輪半徑變化、空轉(zhuǎn)、打滑等對里程計測量精度的影響，又通過對圖像降采樣提取連接點及采用點模式方法進行特征匹配跟蹤，在提高測量精度的同時保證了視覺處理過程的實時性。另外，雙目視覺里程計安裝簡單，維護方便。
圖1是根據(jù)本發(fā)明實施例的方法流程圖。圖2是提取連接點特征實例示意圖。圖3是點模式匹配方法流程圖。圖4是圖像序列分段優(yōu)化示意圖。
具體實施例方式為使本發(fā)明的目的、技術(shù)方案和優(yōu)點更加清楚明白，以下結(jié)合具體實施例，并參照附圖，對本發(fā)明進一步詳細說明。本發(fā)明中，雙目攝像機安裝于移動機器人的頂部，隨著機器人的運動，攝像機采集室內(nèi)天花板圖像得到序列圖像，通過圖像特征提取、特征匹配與跟蹤和分段優(yōu)化算法計算各幀圖像對應(yīng)的攝像機相對運動參數(shù)，進而根據(jù)攝像機與機器人的相對位置關(guān)系將攝像機運動參數(shù)轉(zhuǎn)化為機器人的相對運動參數(shù)，這一相對運動參數(shù)的累計就構(gòu)成了機器人運動的里程信息。圖1是根據(jù)本發(fā)明實施例的方法流程圖。如圖1所示，本發(fā)明提出的雙目視覺里程計實現(xiàn)方法包括以下步驟步驟1，使用雙目攝像機采集室內(nèi)天花板圖像對，并對兩幅圖像分別提取連接點特征。本發(fā)明采用雙目攝像機采集室內(nèi)天花板圖像對，其中，一臺攝像機作為主攝像機， 其采集的圖像序列用于特征匹配；另一臺攝像機作為輔助攝像機，與主攝像機采集同一幀圖像，用于計算每一幀圖像上的特征的三維坐標。本實例中，移動機器人在室內(nèi)運動，針對室內(nèi)環(huán)境紋理簡單且邊緣直線較多這一特點，以圖像邊緣直線連接點作為實現(xiàn)視覺里程計的主要視覺特征。與其他常用的圖像特征如角點和團塊特征相比，連接點特征在環(huán)境光照條件變化情況下檢出率穩(wěn)定，且定位精
度更高。連接點特征提取的實例示意圖如圖2所示，其中，圖2(a)為攝像機獲取的原始圖像首先，采用Carmy算子提取圖像邊緣得到邊緣圖像，如圖2 (b)所示；然后，利用Hough變換從邊緣圖像中提取直線，提取出的直線如圖2(c)所示；由于圖像模糊和邊緣寬度的影響，Hough變換得到的是若干組相互平行的直線，通過最小二乘法擬合直線，將相互平行且距離較近的直線組合并為單一直線，通過最小二乘法擬合后得到的直線如圖2(d)所示；最后，計算多條直線的交點獲得連接點特征，如圖2(e)所示。步驟2，采用點模式匹配方法建立雙目攝像機采集到的圖像對的特征匹配關(guān)系，進而根據(jù)立體視覺算法計算匹配點對對應(yīng)的三維坐標。采用雙目攝像機采集同一場景的圖像，如果兩幅圖像中對應(yīng)同一空間點的兩個像素點之間的對應(yīng)關(guān)系已知，結(jié)合攝像機內(nèi)部參數(shù)及相對位置關(guān)系，則可根據(jù)立體視覺算法由采集得到的圖像恢復(fù)場景空間的三維信息。立體視覺算法的核心在于圖像對之間像素點的匹配，一旦建立起精確的對應(yīng)關(guān)系，就可由簡單的三角計算得到三維坐標。常用查找像素匹配點的算法為基于面積的匹配點搜尋方法，其基本思路是計算欲匹配點周圍區(qū)域與另一幅圖像上同樣大小區(qū)域的灰度相似性。如果兩幅圖像局部結(jié)構(gòu)相似性較高，基于面積的匹配方法易得出錯誤的匹配關(guān)系，并且匹配計算量較高。為提高系統(tǒng)的實時性，并考慮到同一場景對應(yīng)的兩幅圖像及序列圖像中相鄰幀之間結(jié)構(gòu)偏移量較小，本發(fā)明采用點模式匹配方法進行特征點的匹配。所述點模式匹配方法，是將圖像匹配問題轉(zhuǎn)化為從兩個滿足一定映射關(guān)系的點集中找出最優(yōu)匹配點對的問題。本發(fā)明中，攝像機采集天花板圖像，當攝像機與天花板的距離遠大于天花板上物體高度時，可將天花板視為平面。已知由同一空間平面映射而成的兩幅圖像上對應(yīng)點之間滿足單應(yīng)映射關(guān)系=H艮，其中，艮、分別為兩幅圖像對應(yīng)點的齊次坐標，H為3 X 3矩陣，稱為單應(yīng)映射矩陣，Si為常系數(shù)，因此，本發(fā)明中可通過迭代求解特征點集之間的最優(yōu)單應(yīng)映射關(guān)系，同時獲得匹配對應(yīng)關(guān)系。圖3所示為點模式匹配方法的具體實現(xiàn)步驟步驟2. 1，將圖像對中分別所含的特征點組成特征點集P和Q ；步驟2. 2，采用最近距離準則建立待匹配特征點集P與Q之間的一一對應(yīng)關(guān)系；考慮到P與Q為不完全匹配情況，且兩者都存在出格點，于是當P中可匹配點在最近距離準則下都找到Q中相應(yīng)的對應(yīng)點時，P中出格點由于不存在可匹配點而會尋找到Q中出格點，其對應(yīng)距離大于可匹配點的最近距離。因此，可設(shè)定距離閾值σ，當P中的某一點與Q中對應(yīng)的最近點之間的距離大于σ時，則將P和Q中的對應(yīng)點均作為出格點處理。步驟2. 3，估計單應(yīng)映射矩陣H的初值；將特征點集P和Q中最近點距離小于距離閾值σ的對應(yīng)點作為初始匹配子集，對兩幅圖像中產(chǎn)生的兩個初始匹配子集的坐標分別進行平移，使得特征點集P和Q的質(zhì)心移至坐標原點，然后選擇合適的縮放因子使特征點集P和Q中的各點到原點的平均距離為1， 此時特征點集P和Q在坐標系的4個象限都有點分布，于是可以選擇每個象限中特征點集 P和Q的質(zhì)心計算單應(yīng)映射矩陣(維數(shù)為4*4)，作為初始匹配點單應(yīng)映射矩陣H的估計初值。步驟2. 4，計算特征點集P與Q之間的映射距離ε，并根據(jù)計算出的映射距離ε 判斷H是否為最優(yōu)映射關(guān)系。計算映射距離時，除計算匹配子集之間的距離外，還需考慮出格點的數(shù)量對映射關(guān)系的影響。設(shè)點集P與Q的點數(shù)分別為η和m，η彡m，P與Q有k對匹配點，映射距離
化幻一。當ε小于預(yù)設(shè)的最小映射距離￡min時，表明已經(jīng)得到最優(yōu)的
i
H估計，結(jié)束迭代過程跳轉(zhuǎn)到步驟2. 5 ；否則，將P經(jīng)H映射為中間點集P'與Q組成待匹配點集，跳轉(zhuǎn)到步驟2. 2 (即，將P'作為P)，繼續(xù)迭代過程。步驟2. 5，累計相乘迭代過程中估計的各映射矩陣H構(gòu)成原特征點集P與Q的最優(yōu)
6映射關(guān)系矩陣H，得到特征點集P與Q之間的匹配關(guān)系。在得到圖像對特征點集之間的匹配關(guān)系后，可采用立體視覺算法計算各特征點對應(yīng)的三維坐標。本發(fā)明中，攝像機對之間的相對位置固定，通過校準可使攝像機對的內(nèi)部參數(shù)相同，且圖像各點對應(yīng)極線相互平行，又稱為外極線配置。在外極線配置下，兩幅圖像
的匹配點對(x，y)、(x'，y')對應(yīng)的三維點坐標為義=^: ),Y = d(°-5(y+y')-vo)，
權(quán)利要求
1.一種雙目視覺里程計實現(xiàn)方法，其特征在于，該方法包括以下步驟步驟1，使用雙目攝像機采集室內(nèi)天花板圖像對，并對兩幅圖像分別提取連接點特征； 步驟2，采用點模式匹配方法建立雙目攝像機采集到的圖像對的特征匹配關(guān)系，進而根據(jù)立體視覺算法計算匹配點對對應(yīng)的三維坐標；步驟3，使用步驟2的方法得到攝像機采集到的圖像序列中的各相鄰圖像之間的匹配點對的三維對應(yīng)關(guān)系；步驟4，根據(jù)步驟3得到的匹配點對的三維對應(yīng)關(guān)系計算攝像機的相對運動參數(shù)，并采用分段優(yōu)化算法對其進行優(yōu)化；步驟5，根據(jù)攝像機與機器人的相對位置關(guān)系將攝像機的相對運動參數(shù)轉(zhuǎn)化為機器人運動參數(shù)，并更新機器人運動的里程信息。
2.根據(jù)權(quán)利要求1所述的方法，其特征在于，連接點特征的提取進一步包括以下步驟 首先，采用Carmy算子提取圖像邊緣得到邊緣圖像；然后，利用Hough變換從邊緣圖像中提取直線； 最后，計算多條直線的交點獲得連接點特征。
3.根據(jù)權(quán)利要求2所述的方法，其特征在于，利用Hough變換從邊緣圖像中提取直線時，由于圖像模糊和邊緣寬度的影響，Hough變換得到的是若干組相互平行的直線，通過最小二乘法擬合直線，將相互平行且距離較近的直線組合并為單一直線。
4.根據(jù)權(quán)利要求1所述的方法，其特征在于，采用點模式匹配方法建立雙目攝像機采集到的圖像對的特征匹配關(guān)系進一步包括以下步驟步驟2. 1，將圖像對中分別所含的特征點組成特征點集P和Q ；步驟2. 2，采用最近距離準則建立待匹配特征點集P與Q之間的一一對應(yīng)關(guān)系；步驟2. 3，估計單應(yīng)映射矩陣H的初值；步驟2. 4，計算特征點集P與Q之間的映射距離ε，當ε小于預(yù)設(shè)的最小映射距離ε min 時，表明已經(jīng)得到最優(yōu)的H估計，結(jié)束迭代過程跳轉(zhuǎn)到步驟2. 5 ；否則，將特征點集P經(jīng)單應(yīng)映射矩陣H映射為中間點集P'與Q組成待匹配點集，跳轉(zhuǎn)到步驟2. 2，繼續(xù)迭代過程；步驟2. 5，累計迭代過程中估計的各映射矩陣H構(gòu)成原特征點集P與Q的最優(yōu)映射關(guān)系矩陣H，得到特征點集P與Q之間的匹配關(guān)系。
5.根據(jù)權(quán)利要求4所述的方法，其特征在于，所述步驟2.2中，當特征點集P中的某一點與特征點集Q中對應(yīng)的最近點之間的距離大于ο時，則將特征點集P和Q中的對應(yīng)點均作為出格點處理。
6.根據(jù)權(quán)利要求4所述的方法，其特征在于，所述步驟2.3中，將特征點集P和Q中最近點距離小于距離閾值σ的對應(yīng)點作為初始匹配子集，對兩幅圖像中產(chǎn)生的兩個初始匹配子集的坐標分別進行平移，使得特征點集P和Q的質(zhì)心移至坐標原點，然后選擇合適的縮放因子使特征點集P和Q中的各點到原點的平均距離為1，此時特征點集P和Q在坐標系的4個象限都有點分布，于是可以選擇每個象限中特征點集P和Q的質(zhì)心作為初始匹配點計算單應(yīng)映射矩陣，作為單應(yīng)映射矩陣H的估計初值。
7.根據(jù)權(quán)利要求4所述的方法，其特征在于，所述步驟2.4中，特征點集P與Q之間的映射距離s = Ι化-HpiI2，其中，k為特征點集P與Q的匹配點對數(shù)，η和m分別為特征點集P與Q的點數(shù)，η ( Π ,ρ,、化分別為兩幅圖像對應(yīng)點的齊次坐標，O為預(yù)設(shè)的距離閾值。
8.根據(jù)權(quán)利要求1所述的方法，其特征在于，根據(jù)立體視覺算法計算匹配點對(X，y)、 (χ'，y')對應(yīng)的三維點坐標為X = d(x-u0) γ = d(0.5(y +y')-V0) z= dfX X ，X X， X X，其中，f為攝像機焦距，d為攝像機對光心之間的相對距離，(Utl，Y0)為主點坐標，即特征點集P中光心的圖像坐標。
9.根據(jù)權(quán)利要求1所述的方法，其特征在于，假設(shè)所述三維對應(yīng)關(guān)系確定的三維對應(yīng)點為Pi、P‘ i，那么攝像機的相對運動參數(shù)旋轉(zhuǎn)矩陣R和三維平移矢量t根據(jù)其與三維對應(yīng)點的關(guān)系式p' i = RPi+t來計算。
10.根據(jù)權(quán)利要求9所述的方法，其特征在于，所述分段優(yōu)化算法進一步包括以下步驟步驟4. 1，計算第1幀圖像各個特征點的三維坐標Qli = Ixli, ，ζπ}，其中，i = 1， 2，. . .，η，η為第1幀圖像特征點的個數(shù)；步驟4. 2，獲取第2幀圖像，根據(jù)點模式匹配方法將第2幀圖像與第1幀圖像進行匹配， 得到與第1幀圖像點匹配的第2幀圖像中的特征點P2i，其中，i = 1，2，. . .，m，m為第2幀圖像特征點的個數(shù)；計算第2幀圖像中各個特征點的三維坐標q2i = {x2i, y2i，Z2J ；計算攝像機的相對運動參數(shù)1 21，t21 ；并以重投影誤差最小min^>2 (A,M2A)為準則優(yōu)化參數(shù)集 (R21, t21，Qli)，其中， 分別為圖像點A和空間點知對應(yīng)的齊次坐標，M為投影矩陣；步驟4.3，繼續(xù)獲取圖像，得到第k幀圖像，k>= 3，計算其與第1幀圖像匹配的特征點Pki及其三維坐標qki = {xki，ykidki}，其中，i = 1，2，...，h，h為第k幀圖像特征點的個數(shù)，如果第k幀圖像與第1幀圖像滿足重疊條件其匹配對數(shù)目大于等于某一閾值，則計算攝像機的相對運動參數(shù)I kl，tkl，以重投影誤差的和禮，MH禮)+…+ f (瓦，似21禮》 最小為準則優(yōu)化參數(shù)集(Rkl，tkl，-,R21,t21,qii)；否則以第k-1幀為第1幀返回步驟4. 1， 劃分新的圖像段進行優(yōu)化過程，直至最后一幅圖像，結(jié)束。
11.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟5進一步包括以下步驟 根據(jù)攝像機相對運動參數(shù)R。。和t。。，計算對應(yīng)的機器人運動參數(shù)Ree = RecRccRL，tee =Re。t。。-RM、，其中，旋轉(zhuǎn)矩陣民。和平移矢量、為攝像機相對于機器人本體的位置關(guān)系的描述參數(shù)；更新運動后機器人的里程信息為Rtl,k = R0,^1Ree' t0,k = t^+R。^、，其中k為匹配點的個數(shù)，R0, k-!和t^i為運動前機器人相對起始點的里程信息。
全文摘要
本發(fā)明公開了一種移動機器人雙目視覺里程計實現(xiàn)方法。該方法包括以下步驟使用雙目攝像機采集室內(nèi)天花板圖像對，對兩幅圖像分別提取連接點特征；采用點模式匹配方法建立圖像對的特征匹配關(guān)系，進而根據(jù)立體視覺算法計算匹配點對對應(yīng)的三維坐標；得到采集到的圖像序列中的各相鄰圖像之間的匹配點對的三維對應(yīng)關(guān)系；根據(jù)匹配點對的三維對應(yīng)關(guān)系計算攝像機的相對運動參數(shù)，采用分段優(yōu)化算法對其進行優(yōu)化；根據(jù)攝像機與機器人的相對位置關(guān)系將攝像機的相對運動參數(shù)轉(zhuǎn)化為機器人運動參數(shù)，并更新機器人運動的里程信息。本發(fā)明的雙目視覺里程計不依賴于對輪軸旋轉(zhuǎn)的計數(shù)，從而避免了車輪半徑變化、空轉(zhuǎn)、打滑等對里程計測量精度的影響。
文檔編號G01C22/00GK102519481SQ20111044992
公開日2012年6月27日 申請日期2011年12月29日 優(yōu)先權(quán)日2011年12月29日
發(fā)明者劉宏林, 李成榮, 羅楊宇 申請人:中國科學(xué)院自動化研究所