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

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

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

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