基于天花板視覺的同步定位與地圖繪制的方法
【專利摘要】本發(fā)明屬機(jī)器人同步定位與地圖創(chuàng)建領(lǐng)域，涉及基于天花板的角點(diǎn)信息實(shí)現(xiàn)機(jī)器人定位及地圖繪制的方法，該方法采用實(shí)時(shí)高效的角點(diǎn)選取算法，角點(diǎn)位于天花板上，由安裝在機(jī)器人上且光軸與天花板垂直的攝像機(jī)進(jìn)行拍攝，并從拍攝的圖片中提取出角點(diǎn)；使用光流跟蹤法跟蹤連續(xù)兩張圖片中匹配的角點(diǎn)對(duì)，利用匹配的角點(diǎn)對(duì)來實(shí)現(xiàn)角點(diǎn)的三維重構(gòu)；配合基于擴(kuò)展卡爾曼濾波的同步定位與地圖創(chuàng)建算法確定機(jī)器人每次拍攝時(shí)的位置和航向角，結(jié)合重構(gòu)的路標(biāo)信息不斷完善地圖。本發(fā)明實(shí)現(xiàn)了基于天花板視覺的同步定位與地圖繪制，能較精確地描述機(jī)器人與周圍環(huán)境的關(guān)系。
【專利說明】基于天花板視覺的同步定位與地圖繪制的方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于機(jī)器人同步定位與地圖創(chuàng)建(SLAM)領(lǐng)域，涉及基于天花板的角點(diǎn)信息實(shí)現(xiàn)機(jī)器人定位及地圖繪制的方法。本方法中結(jié)合了數(shù)字圖像處理技術(shù)、計(jì)算機(jī)視覺和擴(kuò)展卡爾曼濾波技術(shù)等等。
【背景技術(shù)】
[0002]同時(shí)定位與地圖創(chuàng)建(SLAM)的技術(shù)是機(jī)器人領(lǐng)域比較經(jīng)典的問題，通常SLAM問題可以描述為:機(jī)器人在未知環(huán)境中從一個(gè)未知位置開始移動(dòng)，在移動(dòng)過程中根據(jù)位置估計(jì)和地圖進(jìn)行自身定位，同時(shí)在自身定位的基礎(chǔ)上建造增量式地圖，實(shí)現(xiàn)機(jī)器人的自主定位和導(dǎo)航。影響SLAM問題的比較重要的因?yàn)榄h(huán)境噪聲以及觀測數(shù)據(jù)的相關(guān)聯(lián)性，如果能夠得到較高的數(shù)據(jù)相關(guān)聯(lián)性將決定了對(duì)周圍環(huán)境觀測的正確性，進(jìn)而影響到整個(gè)環(huán)境地圖的創(chuàng)建。
[0003]SIAM問題依賴于機(jī)器人對(duì)環(huán)境信息的感知，目前可以用于環(huán)境信息感知的傳感器種類多樣，主要有碼盤、攝像機(jī)、激光雷達(dá)等等，其中視覺傳感器價(jià)格低廉、信息量豐富，隨著圖像處理和機(jī)器視覺技術(shù)的發(fā)展，將越來越多地應(yīng)用于機(jī)器人領(lǐng)域，基于視覺傳感器的移動(dòng)機(jī)器人同時(shí)定位與建圖已經(jīng)成為本領(lǐng)域比較熱門的研究課題。
[0004]本發(fā)明擬采用室內(nèi)天花板的視覺信息作為研究對(duì)象，基于室內(nèi)天花板興趣點(diǎn)的獲取與跟蹤，以此得到的信息構(gòu)建場景地圖，用于機(jī)器人的定位與路徑規(guī)劃。

【發(fā)明內(nèi)容】

[0005]本發(fā)明的目的是提供一種同時(shí)定位與地圖創(chuàng)建(SLAM)的解決方案，涉及基于天花板視頻信息的機(jī)器人定位與地圖構(gòu)建系統(tǒng)，尤其是基于天花板的角點(diǎn)信息實(shí)現(xiàn)機(jī)器人定位及地圖繪制的方法。本系統(tǒng)中從特征點(diǎn)采集與匹配到基于擴(kuò)展卡爾曼濾波的EKF-SLAM算法，由于特征點(diǎn)采集來自于圖像角點(diǎn)，可以確保較高的數(shù)據(jù)相關(guān)性。
[0006]本發(fā)明方法中，采用實(shí)時(shí)高效的角點(diǎn)選取算法，角點(diǎn)位于天花板上，由安裝在機(jī)器人上且光軸與天花板垂直的攝像機(jī)進(jìn)行拍攝，并從拍攝的圖片中提取出角點(diǎn)；使用光流跟蹤法跟蹤連續(xù)兩張圖片中匹配的角點(diǎn)對(duì)，利用匹配的角點(diǎn)對(duì)實(shí)現(xiàn)角點(diǎn)的三維重構(gòu)；配合上基于擴(kuò)展卡爾曼濾波的同步定位與地圖創(chuàng)建算法確定機(jī)器人每次拍攝時(shí)的位置和航向角，結(jié)合重構(gòu)的路標(biāo)信息不斷完善地圖。
[0007]本發(fā)明提供了基于天花板視覺的同步定位與地圖繪制的方法，其特征在于，通過安裝在機(jī)器人上垂直于天花板的攝像頭，由攝像頭對(duì)天花板進(jìn)行拍攝，將照片作為輸入傳給系統(tǒng)，經(jīng)系統(tǒng)處理后輸出當(dāng)前更新的地圖；
[0008]所述的系統(tǒng)包括:機(jī)器人運(yùn)動(dòng)模塊(I)、圖像處理模塊(2)、視覺信息處理模塊
(3)、基于擴(kuò)展卡爾曼濾波的slam模塊(4),其中，
[0009]所述機(jī)器人運(yùn)動(dòng)模塊(I)包括運(yùn)動(dòng)模塊(1.1)和攝像模塊(1.2)，運(yùn)動(dòng)模塊(1.1)控制機(jī)器人的運(yùn)動(dòng)，機(jī)器人的每次運(yùn)動(dòng)在接收速度和運(yùn)動(dòng)方向信號(hào)(即控制矩陣)后，經(jīng)過分析后完成運(yùn)動(dòng)，并且給出運(yùn)動(dòng)的距離作為輸入量傳給視覺處理模塊；攝像模塊(1.3)由內(nèi)嵌于機(jī)器人內(nèi)部垂直向上的攝像頭來完成對(duì)天花板的拍攝任務(wù)，并把圖片傳輸?shù)綀D像處理模塊。
[0010]所述圖像處理模塊(2)包括角點(diǎn)特征提取子模塊(2.1)、通過光流跟蹤子模塊(2.2);角點(diǎn)特征提取子模塊(2.1)接收機(jī)器人每步中拍攝的照片，使用Harris角點(diǎn)檢測法提取其中的角點(diǎn)；角點(diǎn)特征提取模塊(2.1)中輸出的角點(diǎn)像素信息作為輸入傳給光流跟蹤子模塊(2.2)，經(jīng)處理后輸出連續(xù)圖片中匹配的角點(diǎn)對(duì)。
[0011]所述視覺信息處理模塊(3)包括深度信息重構(gòu)子模塊(3.1)和三維重構(gòu)子模塊(3.2);圖像處理模塊(2)得到的角點(diǎn)對(duì)做為輸入傳到深度信息子模塊(3.1)中去，使用三角測量法進(jìn)行深度信息重構(gòu)；將得到的深度信息作為輸入傳給三維重構(gòu)子模塊(3.2)，根據(jù)光學(xué)相關(guān)知識(shí)進(jìn)行三維坐標(biāo)的重構(gòu)。
[0012]所述基于擴(kuò)展卡爾曼濾波的SLAM模塊(4)包括預(yù)測子模塊(4.1)、觀測子模塊(4.2)、更新子模塊(4.3)和地圖管理子模塊(4.4);預(yù)測子模塊(4.1)根據(jù)預(yù)測模型以及先前系統(tǒng)狀態(tài)推測出當(dāng)前系統(tǒng)的狀態(tài)；觀測子模塊(4.2)接收三維重構(gòu)的數(shù)據(jù)，如果符合觀測數(shù)據(jù)的要求，則將其作為觀測數(shù)據(jù)處理；更新子模塊(4.3)，根據(jù)擴(kuò)展卡爾曼濾波的更新公式進(jìn)行矩陣運(yùn)算；地圖管理子模塊(4.4)，根據(jù)當(dāng)前的狀態(tài)以及觀測數(shù)據(jù)，對(duì)地圖實(shí)施添加、修改和刪除等管理操作。
[0013]本發(fā)明中:
[0014]所述機(jī)器人運(yùn)動(dòng)模塊(1.1 )，控制機(jī)器人的運(yùn)動(dòng)，機(jī)器人的每次運(yùn)動(dòng)在接收速度和運(yùn)動(dòng)方向信號(hào)(即控制矩陣)后，經(jīng)過分析后完成運(yùn)動(dòng)，并且給出運(yùn)動(dòng)的距離作為輸入量傳給視覺處理模塊。
[0015]所述攝像模塊(1.2)由內(nèi)嵌于機(jī)器人內(nèi)部垂直向上的攝像頭來完成對(duì)天花板的拍攝任務(wù)，并把圖片傳輸?shù)綀D像處理模塊。
[0016]所述的角點(diǎn)特征提取子模塊(2.1)，根據(jù)Harris角點(diǎn)檢測法提取角點(diǎn)，并存儲(chǔ)角點(diǎn)像素坐標(biāo)，本發(fā)明的實(shí)施例中，依據(jù):第一，Harris角點(diǎn)相對(duì)于其他角點(diǎn)檢測法速度較快而且準(zhǔn)確率較高；第二，對(duì)光的敏感度較低等的原因，選取Harris角點(diǎn)檢測法作為角點(diǎn)選取的方法。
[0017]所述光流跟蹤子模塊(2.2)，在前一幀獲取的特征點(diǎn)周圍基于光流金字塔，進(jìn)行特征點(diǎn)匹配，如果匹配成功的特征點(diǎn)數(shù)目少于指定數(shù)目，則重新檢測特征點(diǎn)；否則，將獲取的特征點(diǎn)的中位數(shù)作為輸出；
[0018]所述深度信息重構(gòu)子模塊(3.1)，采用三角測量法以及連續(xù)兩禎圖片中匹配的角點(diǎn)提取深度信息；其中的三角測量法，使用連續(xù)圖片中匹配角點(diǎn)在運(yùn)動(dòng)方向的像素移動(dòng)配合上標(biāo)定出來的攝像頭焦距能夠得到有一定誤差的深度信息，推導(dǎo)公式如下:
[0019]
【權(quán)利要求】
1.基于天花板視覺的同步定位與地圖繪制的方法，其特征在于，通過安裝在機(jī)器人上垂直于天花板的攝像頭，由攝像頭對(duì)天花板進(jìn)行拍攝，經(jīng)系統(tǒng)處理后輸出當(dāng)前更新的地圖； 所述系統(tǒng)包括:機(jī)器人運(yùn)動(dòng)模塊(I)、圖像處理模塊(2)、視覺信息處理模塊(3)、基于擴(kuò)展卡爾曼濾波的SLAM模塊(4)； 其中， 所述機(jī)器人運(yùn)動(dòng)模塊(I)包括運(yùn)動(dòng)模塊(1.1)和攝像模塊(1.2)，運(yùn)動(dòng)模塊(1.1)控制機(jī)器人的運(yùn)動(dòng)，機(jī)器人的每次運(yùn)動(dòng)在接收速度和運(yùn)動(dòng)方向信號(hào)后，經(jīng)過分析完成運(yùn)動(dòng)，并且給出運(yùn)動(dòng)的距離作為輸入量傳給視覺處理模塊；攝像模塊(1.2)由內(nèi)嵌于機(jī)器人內(nèi)部垂直向上的攝像頭完成對(duì)天花板的拍攝任務(wù)，并將圖片傳輸?shù)綀D像處理模塊； 所述圖像處理模塊(2)包括角點(diǎn)特征提取子模塊(2.1)，通過光流跟蹤子模塊(2.2)；角點(diǎn)特征提取子模塊(2.1)接收機(jī)器人移動(dòng)過程中拍攝的照片，用Harris角點(diǎn)檢測方法提取其中的角點(diǎn)；角點(diǎn)特征提取模塊(2.1)中輸出的角點(diǎn)像素信息作為輸入傳給光流跟蹤子模塊(2.2)，經(jīng)處理后輸 出連續(xù)圖片中匹配的角點(diǎn)對(duì)； 所述視覺信息處理模塊(3)包括深度信息重構(gòu)子模塊(3.1)和三維重構(gòu)子模塊(3.2)；圖像處理模塊(2)得到的角點(diǎn)對(duì)做為輸入傳到深度信息子模塊(3.1)中去，用三角測量法進(jìn)行深度信息重構(gòu)；將得到的深度信息作為輸入傳給三維重構(gòu)子模塊(3.2)，根據(jù)光學(xué)相關(guān)知識(shí)進(jìn)行三維坐標(biāo)的重構(gòu)； 所述基于擴(kuò)展卡爾曼濾波的EKF-SLAM模塊(4)包括預(yù)測子模塊(4.1)、觀測子模塊(4.2)、更新子模塊(4.3)和地圖管理子模塊(4.4);預(yù)測子模塊(4.1)根據(jù)預(yù)測模型以及先前系統(tǒng)狀態(tài)推測出當(dāng)前系統(tǒng)的狀態(tài)；觀測子模塊(4.2)接收三維重構(gòu)的數(shù)據(jù)，其中，如果符合觀測數(shù)據(jù)的要求，則將其作為觀測數(shù)據(jù)處理；更新子模塊(4.3)，根據(jù)擴(kuò)展卡爾曼濾波的更新公式進(jìn)行矩陣運(yùn)算；地圖管理子模塊(4.4)，根據(jù)當(dāng)前的狀態(tài)以及觀測數(shù)據(jù)，對(duì)地圖實(shí)施添加、修改和刪除管理操作。
2.根據(jù)權(quán)利要求1所述的基于天花板視覺的同步定位與地圖繪制的方法，其特征在于，在圖像處理模塊(2)中: 所述的角點(diǎn)特征提取子模塊(2.1)，根據(jù)Harris角點(diǎn)檢測法提取角點(diǎn)，并存儲(chǔ)角點(diǎn)像素坐標(biāo)； 所述光流跟蹤子模塊(2.2)，在前一幀獲取的特征點(diǎn)周圍基于光流金字塔，進(jìn)行特征點(diǎn)匹配，如果匹配成功的特征點(diǎn)數(shù)目少于指定數(shù)目，則重新檢測特征點(diǎn)；否則，將獲取的特征點(diǎn)的中位數(shù)作為輸出。
3.根據(jù)權(quán)利要求1所述的基于天花板視覺的同步定位與地圖繪制的方法，其特征在于，在視覺處理模塊(3)中: 所述深度信息重構(gòu)子模塊(3.1)，采用三角測量法以及連續(xù)兩禎圖片中匹配的角點(diǎn)提取深度信息；用連續(xù)圖片中匹配角點(diǎn)在運(yùn)動(dòng)方向的像素移動(dòng)配合上標(biāo)定出的攝像頭焦距得到有誤差的深度信息，推導(dǎo)公式如下: Z=^-
T1-T

tfv I其中f為焦距，T為拍攝這連續(xù)圖片時(shí)機(jī)器人移動(dòng)的位移，X1和&分別為連續(xù)圖片中匹配點(diǎn)的X像素坐標(biāo)； 所述三維重構(gòu)子模塊(3.2)，利用標(biāo)定出的內(nèi)參數(shù)矩陣，結(jié)合當(dāng)前像素坐標(biāo)，實(shí)現(xiàn)三維坐標(biāo)重構(gòu)。
4.根據(jù)權(quán)利要求1所述的基于天花板視覺的同步定位與地圖繪制的方法，其特征在于，在基于擴(kuò)展卡爾曼濾波的EKF-SLAM模塊(4)中:所述預(yù)測模塊(4.1)包括根據(jù)預(yù)測模型以及先前系統(tǒng)狀態(tài)推測出當(dāng)前系統(tǒng)的狀態(tài)，預(yù)測部分分為機(jī)器人和角點(diǎn)位置的預(yù)測，機(jī)器人位置的預(yù)測由它的前進(jìn)路線而定； 所述觀測模塊(4.2)接收三維重構(gòu)的數(shù)據(jù)，如果符合觀測數(shù)據(jù)的要求，則將其作為觀測數(shù)據(jù)處理；觀測到的點(diǎn)如果已經(jīng)在地圖中且滿足觀測要求，則將其作為觀測值更新地圖中點(diǎn)的位置；否則，重構(gòu)出它的初始值，并將其加入到地圖中去； 所述更新模塊(4.3)根據(jù)擴(kuò)展卡爾曼濾波的更新公式進(jìn)行矩陣運(yùn)算，更新部分包括機(jī)器人位置更新、路標(biāo)位置更新； 所述地圖管理模塊(4.4)根據(jù)當(dāng)前的狀態(tài)以及觀測數(shù)據(jù)，對(duì)地圖實(shí)施添加、修改和刪除 管理操作。
【文檔編號(hào)】G05D1/02GK103680291SQ201210332451
【公開日】2014年3月26日 申請(qǐng)日期:2012年9月9日 優(yōu)先權(quán)日:2012年9月9日
【發(fā)明者】張文強(qiáng), 付前忠, 邱曉欣, 何慧鈞, 張睿, 薛向陽 申請(qǐng)人:復(fù)旦大學(xué)