本發(fā)明屬于計(jì)算機(jī)視覺、機(jī)器人技術(shù)領(lǐng)域，更具體地，涉及一種機(jī)器人自主定位與導(dǎo)航方法及系統(tǒng)。

背景技術(shù)：

室內(nèi)機(jī)器人自主定位與導(dǎo)航是最近幾年出現(xiàn)的一個(gè)比較熱門的研究領(lǐng)域。主要包含以下三個(gè)部分：1)室內(nèi)場景地圖的重建與機(jī)器人狀態(tài)的估計(jì)；2)機(jī)器人與目標(biāo)對象重定位；3)機(jī)器人路徑規(guī)劃。對于室內(nèi)機(jī)器人自主定位與導(dǎo)航來說，主要問題在于傳感器的選擇(即外界信息的獲取)以及機(jī)器人的定位導(dǎo)航(即外界信息的處理)。目前是利用激光雷達(dá)或者慣性測量原件與視覺傳感器結(jié)合來完成這一任務(wù)，但是所需成本較高。隨著相機(jī)的普及與計(jì)算機(jī)視覺理論的發(fā)展，使用基于純視覺的方法來完成這一任務(wù)成為主流的研究方向。

機(jī)器人定位與建圖(simultaneouslocalizationandmapping，slam)，是指搭載傳感器的機(jī)器人，在移動時(shí)建立對環(huán)境的描述模型，同時(shí)估計(jì)自己的運(yùn)動。slam同時(shí)包含定位與建圖兩個(gè)問題，被認(rèn)為是實(shí)現(xiàn)機(jī)器人自主性的關(guān)鍵問題之一，對機(jī)器人的導(dǎo)航、控制、任務(wù)規(guī)劃等領(lǐng)域有重要的研究意義。

目前，對于實(shí)時(shí)定位與地圖重建方法比較成熟的方案是激光雷達(dá)slam，即利用激光雷達(dá)傳感器建立二維地圖，進(jìn)而完成機(jī)器人導(dǎo)航任務(wù)。然而該方法使用的掃描設(shè)備結(jié)構(gòu)復(fù)雜、價(jià)格昂貴且不易操作，并且不能夠?qū)崟r(shí)重現(xiàn)室內(nèi)三維場景，無法對地圖產(chǎn)生直觀的感受。

圖定位(image-basedlocalization)能夠從一張新拍的圖片中精確定位相機(jī)相對于已有三維場景的位置(position)和旋轉(zhuǎn)角(orientation)。隨著行人導(dǎo)航、機(jī)器人導(dǎo)航、增強(qiáng)現(xiàn)實(shí)、運(yùn)動恢復(fù)結(jié)構(gòu)(structurefrommotion，sfm)等技術(shù)的發(fā)展，圖定位受到了越來越多的關(guān)注。

使用實(shí)時(shí)定位與地圖重建slam或運(yùn)動恢復(fù)結(jié)構(gòu)的技術(shù)中，由于生成的點(diǎn)云部分或全部點(diǎn)帶有相應(yīng)的特征點(diǎn)描述子，在此情景下圖定位的實(shí)現(xiàn)方法一般分為三步，2d圖像特征點(diǎn)和特征點(diǎn)描述子提取，對特征點(diǎn)點(diǎn)云和2d圖像進(jìn)行描述子鄰近搜索和配對，利用基于隨機(jī)抽樣一致性方法(randomsampleconsensus，ransac)的n點(diǎn)透視問題解法(n-pointperspectiveposeproblem，pnp)來估計(jì)相機(jī)相對于離線地圖的位姿。

然而，由于真實(shí)場景下點(diǎn)云數(shù)量巨大，以及室內(nèi)環(huán)境中重復(fù)人造物體的干擾，導(dǎo)致圖定位技術(shù)在精度和運(yùn)行速度上難以取得令人滿意的效果，因此研究改進(jìn)的特征點(diǎn)點(diǎn)云到2d圖像的特征點(diǎn)匹配算法，提高圖定位技術(shù)的精度和運(yùn)行速度在理論和實(shí)際應(yīng)用上都具有很高的價(jià)值。

機(jī)器人路徑規(guī)劃算法是根據(jù)機(jī)器人對環(huán)境感知得到的數(shù)據(jù)以及構(gòu)建得到的地圖模型，自行規(guī)劃出一條安全的運(yùn)行路線，并且高效地完成任務(wù)。地圖構(gòu)建路徑規(guī)劃技術(shù)是按照機(jī)器人自身傳感器搜索的障礙物信息，將機(jī)器人周圍區(qū)域劃分為不同的網(wǎng)格空間(如自由空間和限制空間等)，計(jì)算網(wǎng)格空間的障礙物占有情況，再依據(jù)一定規(guī)則確定最優(yōu)路徑。

目前，地圖構(gòu)建技術(shù)已引起機(jī)器人研究領(lǐng)域的廣泛關(guān)注，成為移動機(jī)器人路徑規(guī)劃的研究熱點(diǎn)之一。但機(jī)器人傳感器信息資源有限，使得網(wǎng)格地圖障礙物信息很難計(jì)算與處理，同時(shí)由于機(jī)器人要動態(tài)快速地更新地圖數(shù)據(jù)，在網(wǎng)格數(shù)較多、分辨率較高時(shí)難以保證路徑規(guī)劃的實(shí)時(shí)性。因此，地圖構(gòu)建方法必須在地圖網(wǎng)格分辨率與路徑規(guī)劃實(shí)時(shí)性上尋求平衡。

隨著機(jī)器人理論、計(jì)算機(jī)視覺理論的完善和視覺傳感器的發(fā)展與普及，基于純視覺的室內(nèi)機(jī)器人定位和導(dǎo)航技術(shù)取得了很大的進(jìn)展。因此，研究基于rgb-d相機(jī)的室內(nèi)機(jī)器人定位導(dǎo)航策略，不僅有很強(qiáng)的理論意義，而且具有非常廣闊的應(yīng)用前景。

技術(shù)實(shí)現(xiàn)要素：

針對現(xiàn)有技術(shù)的以上缺陷或改進(jìn)需求，本發(fā)明提供了一種機(jī)器人自主定位與導(dǎo)航方法及系統(tǒng)，使得機(jī)器人能夠獨(dú)立完成從室內(nèi)地圖構(gòu)建到機(jī)器人自身及目標(biāo)重定位，再到機(jī)器人路徑規(guī)劃這一任務(wù)。由此解決現(xiàn)有技術(shù)中，設(shè)備結(jié)構(gòu)復(fù)雜、價(jià)格昂貴、不易操作、數(shù)據(jù)量巨大、路徑規(guī)劃實(shí)時(shí)性較低等技術(shù)問題。

為實(shí)現(xiàn)上述目的，按照本發(fā)明的一個(gè)方面，提供了一種機(jī)器人自主定位與導(dǎo)航方法，包括：

(1)采集機(jī)器人在當(dāng)前位置的rgb圖像；

(2)將當(dāng)前位置的rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定機(jī)器人在室內(nèi)三維點(diǎn)云地圖中的當(dāng)前位置；將預(yù)存儲的目的地rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定目的地在室內(nèi)三維點(diǎn)云地圖中的目的地位置；

(3)在室內(nèi)三維點(diǎn)云地圖中搜索從當(dāng)前位置到目的地位置的最優(yōu)路徑，并驅(qū)動機(jī)器人按所述最優(yōu)路徑行走。

優(yōu)選地，所述室內(nèi)三維點(diǎn)云地圖按照如下方式構(gòu)建：

采集機(jī)器人在行進(jìn)路線上拍攝得到的rgb圖像和深度圖像，其中，機(jī)器人的行進(jìn)路線為閉合路線；

利用所述rgb圖像和深度圖像構(gòu)建室內(nèi)三維點(diǎn)云地圖。

優(yōu)選地，所述利用所述rgb圖像和深度圖像構(gòu)建室內(nèi)三維點(diǎn)云地圖，具體包括：

(s1)將獲取的起始幀圖像作為第一幀圖像，將與所述起始幀圖像相鄰的下一幀圖像作為第二幀圖像；

(s2)提取所述第一幀圖像與所述第二幀圖像的特征點(diǎn)，分別計(jì)算所述第一幀圖像與所述第二幀圖像上的關(guān)鍵點(diǎn)，針對關(guān)鍵點(diǎn)周圍的像素點(diǎn)，計(jì)算所述第一幀圖像的特征描述子以及所述第二幀圖像的特征描述子；

(s3)采用所述第一幀圖像中的特征點(diǎn)到所述第二幀圖像的最近鄰特征點(diǎn)距離與次近鄰特征點(diǎn)距離的比值來對所述第一幀圖像的特征描述子與所述第二幀圖像的特征描述子進(jìn)行匹配，得到所述第一幀圖像與所述第二幀圖像之間的特征點(diǎn)匹配對；

(s4)根據(jù)所述第一幀圖像對應(yīng)的rgb圖像和深度圖像以及所述第二幀圖像對應(yīng)的rgb圖像和深度圖像，利用針孔相機(jī)模型計(jì)算出所述第一幀圖像對應(yīng)的rgb圖像和所述第二幀圖像對應(yīng)的rgb圖像中的二維點(diǎn)所對應(yīng)的三維點(diǎn)；

(s5)根據(jù)所述第一幀圖像與所述第二幀圖像之間的特征點(diǎn)匹配對對應(yīng)的三維點(diǎn)，利用所述第一幀圖像與所述第二幀圖像之間的幾何關(guān)系以及非線性優(yōu)化方法估計(jì)出所述第一幀圖像與所述第二幀圖像之間的旋轉(zhuǎn)平移矩陣，得到所述機(jī)器人從所述第一幀圖像到所述第二幀圖像之間的位姿變換；

(s6)將所述第二幀圖像作為新的第一幀圖像，將與所述第二幀圖像相鄰的下一幀圖像作為新的第二幀圖像，檢測所述新的第一幀圖像是否與所述起始幀圖像重合，若不重合，則執(zhí)行步驟(s2)，否則，執(zhí)行步驟(s7)；

(s7)將封閉路徑上的所有幀圖像對應(yīng)的機(jī)器人位姿構(gòu)建一個(gè)姿態(tài)圖，所述姿態(tài)圖的節(jié)點(diǎn)代表機(jī)器人在每一個(gè)幀圖像上的姿態(tài)，邊表示節(jié)點(diǎn)之間的位姿變換，然后利用非線性優(yōu)化方法對所述姿態(tài)圖進(jìn)行優(yōu)化，得到機(jī)器人在每一幀圖像的位姿，進(jìn)而得到每一幀圖像對應(yīng)的三維點(diǎn)云圖像；

(s8)拼接每一幀圖像對應(yīng)的三維點(diǎn)云圖像，形成室內(nèi)三維點(diǎn)云地圖，并將所述室內(nèi)三維點(diǎn)云地圖轉(zhuǎn)化為八叉樹地圖。

優(yōu)選地，所述步驟(2)具體包括：

(2.1)使用聚類算法對所述室內(nèi)三維點(diǎn)云地圖中sift特征點(diǎn)的描述子進(jìn)行聚類，得到詞匯樹；

(2.2)獲取機(jī)器人當(dāng)前位置的第一rgb圖像和機(jī)器人接收到的第二rgb圖像，并對所述第一rgb圖像以及所述第二rgb圖像進(jìn)行sift特征點(diǎn)提取，然后使用聚類算法得到視覺詞匯；

(2.3)進(jìn)行2d-3d檢索，對于所述第一rgb圖像以及所述第二rgb圖像中的每個(gè)2d特征點(diǎn)，根據(jù)該2d特征點(diǎn)對應(yīng)的視覺詞匯，在所述詞匯樹中查找與該2d特征點(diǎn)具有相同單詞的候選三維匹配點(diǎn)；

(2.4)進(jìn)行2d-3d匹配，對于所述第一rgb圖像以及所述第二rgb圖像中的每個(gè)2d特征點(diǎn)，計(jì)算該2d特征點(diǎn)與對應(yīng)的候選三維匹配點(diǎn)在特征描述子空間上的距離；

(2.5)找到與該2d特征點(diǎn)對應(yīng)的候選三維匹配點(diǎn)中在特征描述子空間上距離最近和距離次近的兩個(gè)三維點(diǎn)，若最近距離與次近距離的比值小于第一預(yù)設(shè)閾值，則該2d特征點(diǎn)與在特征描述子空間上距離該2d特征點(diǎn)距離最近的目標(biāo)三維點(diǎn)相匹配；

(2.6)對于與該2d特征點(diǎn)匹配的目標(biāo)三維點(diǎn)，查找所述目標(biāo)三維點(diǎn)在歐式空間上的若干個(gè)最近鄰三維點(diǎn)；

(2.7)對每一個(gè)最近鄰三維點(diǎn)，進(jìn)行3d-2d匹配，在2d圖像中查找與該最近鄰三維點(diǎn)具有相同詞匯的候選2d特征點(diǎn)，計(jì)算該最近鄰三維點(diǎn)與候選2d特征點(diǎn)在特征描述子空間上的距離；

(2.8)找到與該最近鄰三維點(diǎn)對應(yīng)的候選2d特征點(diǎn)中在特征描述子空間上距離最近和距離次近的兩個(gè)2d特征點(diǎn)，若最近距離與次近距離的比值小于第二預(yù)設(shè)閾值，則該最近鄰三維點(diǎn)與在特征描述子空間上距離該最近鄰三維點(diǎn)距離最近的目標(biāo)2d特征點(diǎn)相匹配；

(2.9)檢查當(dāng)前找到的三維點(diǎn)和二維點(diǎn)的匹配個(gè)數(shù)，若達(dá)到第三預(yù)設(shè)閾值則執(zhí)行步驟(2.10)，否則，執(zhí)行步驟(2.3)；

(2.10)通過得到的3d與2d的匹配對計(jì)算得到機(jī)器人當(dāng)前在室內(nèi)三維點(diǎn)云地圖中的位置和目的地在室內(nèi)三維點(diǎn)云地圖中的位置，進(jìn)而得到八叉樹地圖內(nèi)的機(jī)器人當(dāng)前所處的位置和目的地的位置。

優(yōu)選地，所述步驟(3)具體包括：

(3.1)將所述八叉樹地圖以及在所述八叉樹地圖內(nèi)的機(jī)器人當(dāng)前所處的位置和目的地的位置進(jìn)行投影，得到二維平面的柵格地圖；

(3.2)獲取所述柵格地圖中機(jī)器人當(dāng)前所處的位置的投影點(diǎn)作為起點(diǎn)的所有鄰近像元，并計(jì)算各鄰近像元的f值，存入?；蜿?duì)列中，其中f值表示鄰近像元到目的地的距離值；

(3.3)將?；蜿?duì)列中f值最小的點(diǎn)作為新起點(diǎn)，并將所有處理過的單元和?；蜿?duì)列中的單元標(biāo)記為已處理；

(3.4)檢測新起點(diǎn)是否為目的地點(diǎn)，若不是，則獲取新起點(diǎn)周圍未經(jīng)過處理的鄰近像元的f值并存入棧中，然后執(zhí)行步驟(3.3)，否則執(zhí)行步驟(3.5)；

(3.5)將從機(jī)器人當(dāng)前所處位置的投影點(diǎn)到目的地位置的投影點(diǎn)之間的所有新起點(diǎn)連接起來作為從機(jī)器人當(dāng)前位置到目的地位置的最優(yōu)路徑；

(3.6)驅(qū)動機(jī)器人按所述最優(yōu)路徑進(jìn)行行走，最終到達(dá)目的地位置完成導(dǎo)航任務(wù)。

按照本發(fā)明的另一方面，提供了一種機(jī)器人自主定位與導(dǎo)航系統(tǒng)，包括：

圖像采集模塊，用于采集機(jī)器人在當(dāng)前位置的rgb圖像；

重定位模塊，用于將當(dāng)前位置的rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定機(jī)器人在室內(nèi)三維點(diǎn)云地圖中的當(dāng)前位置；將預(yù)存儲的目的地rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定目的地在室內(nèi)三維點(diǎn)云地圖中的目的地位置；

導(dǎo)航模塊，用于在室內(nèi)三維點(diǎn)云地圖中搜索從當(dāng)前位置到目的地位置的最優(yōu)路徑，并驅(qū)動機(jī)器人按所述最優(yōu)路徑行走。

優(yōu)選地，所述圖像采集模塊，還用于采集機(jī)器人在行進(jìn)路線上拍攝得到的rgb圖像和深度圖像，其中，機(jī)器人的行進(jìn)路線為閉合路線；

所述系統(tǒng)還包括：

室內(nèi)地圖構(gòu)建模塊，用于利用所述rgb圖像和深度圖像構(gòu)建室內(nèi)三維點(diǎn)云地圖。

優(yōu)選地，所述室內(nèi)地圖構(gòu)建模塊包括：

選取模塊，用于將獲取的起始幀圖像作為第一幀圖像，將與所述起始幀圖像相鄰的下一幀圖像作為第二幀圖像；

特征點(diǎn)提取模塊，用于提取所述第一幀圖像與所述第二幀圖像的特征點(diǎn)，分別計(jì)算所述第一幀圖像與所述第二幀圖像上的關(guān)鍵點(diǎn)，針對關(guān)鍵點(diǎn)周圍的像素點(diǎn)，計(jì)算所述第一幀圖像的特征描述子以及所述第二幀圖像的特征描述子；

特征點(diǎn)匹配模塊，用于采用所述第一幀圖像中的特征點(diǎn)到所述第二幀圖像的最近鄰特征點(diǎn)距離與次近鄰特征點(diǎn)距離的比值來對所述第一幀圖像的特征描述子與所述第二幀圖像的特征描述子進(jìn)行匹配，得到所述第一幀圖像與所述第二幀圖像之間的特征點(diǎn)匹配對；

三維點(diǎn)匹配模塊，用于根據(jù)所述第一幀圖像對應(yīng)的rgb圖像和深度圖像以及所述第二幀圖像對應(yīng)的rgb圖像和深度圖像，利用針孔相機(jī)模型計(jì)算出所述第一幀圖像對應(yīng)的rgb圖像以及所述第二幀圖像對應(yīng)的rgb圖像中的二維點(diǎn)所對應(yīng)的三維點(diǎn)；

位姿確定模塊，用于根據(jù)所述第一幀圖像與所述第二幀圖像之間的特征點(diǎn)匹配對對應(yīng)的三維點(diǎn)，利用所述第一幀圖像與所述第二幀圖像之間的幾何關(guān)系以及非線性優(yōu)化方法估計(jì)出所述第一幀圖像與所述第二幀圖像之間的旋轉(zhuǎn)平移矩陣，得到所述機(jī)器人從所述第一幀圖像到所述第二幀圖像之間的位姿變換；

第一檢測模塊，用于將所述第二幀圖像作為新的第一幀圖像，將與所述第二幀圖像相鄰的下一幀圖像作為新的第二幀圖像，檢測所述新的第一幀圖像是否與所述起始幀圖像重合，若不重合，則返回執(zhí)行所述特征點(diǎn)提取模塊的操作，直到所述新的第一幀圖像與所述起始幀圖像重合；

優(yōu)化模塊，用于將封閉路徑上的所有幀圖像對應(yīng)的機(jī)器人位姿構(gòu)建一個(gè)姿態(tài)圖，所述姿態(tài)圖的節(jié)點(diǎn)代表機(jī)器人在每一個(gè)幀圖像上的姿態(tài)，邊表示節(jié)點(diǎn)之間的位姿變換，然后利用非線性優(yōu)化方法對所述姿態(tài)圖進(jìn)行優(yōu)化，得到機(jī)器人在每一幀圖像的位姿，進(jìn)而得到每一幀圖像對應(yīng)的三維點(diǎn)云圖像；

地圖構(gòu)建子模塊，用于拼接每一幀圖像對應(yīng)的三維點(diǎn)云圖像，形成室內(nèi)三維點(diǎn)云地圖，并將所述室內(nèi)三維點(diǎn)云地圖轉(zhuǎn)化為八叉樹地圖。

優(yōu)選地，所述重定位模塊包括：

聚類模塊，用于使用聚類算法對所述室內(nèi)三維點(diǎn)云地圖中sift特征點(diǎn)的描述子進(jìn)行聚類，得到詞匯樹；

所述聚類模塊，還用于獲取機(jī)器人當(dāng)前位置的第一rgb圖像和機(jī)器人接收到的第二rgb圖像，并對所述第一rgb圖像以及所述第二rgb圖像進(jìn)行sift特征點(diǎn)提取，然后使用聚類算法得到視覺詞匯；

2d-3d檢索模塊，用于進(jìn)行2d-3d檢索，對于所述第一rgb圖像以及所述第二rgb圖像中的每個(gè)2d特征點(diǎn)，根據(jù)該2d特征點(diǎn)對應(yīng)的視覺詞匯，在所述詞匯樹中查找與該2d特征點(diǎn)具有相同單詞的候選三維匹配點(diǎn)；

2d-3d匹配模塊，用于進(jìn)行2d-3d匹配，對于所述第一rgb圖像以及所述第二rgb圖像中的每個(gè)2d特征點(diǎn)，計(jì)算該2d特征點(diǎn)與對應(yīng)的候選三維匹配點(diǎn)在特征描述子空間上的距離；

所述2d-3d匹配模塊，還用于找到與該2d特征點(diǎn)對應(yīng)的候選三維匹配點(diǎn)中在特征描述子空間上距離最近和距離次近的兩個(gè)三維點(diǎn)，若最近距離與次近距離的比值小于第一預(yù)設(shè)閾值，則該2d特征點(diǎn)與在特征描述子空間上距離該2d特征點(diǎn)距離最近的目標(biāo)三維點(diǎn)相匹配；

3d-2d檢索模塊，用于對于與該2d特征點(diǎn)匹配的目標(biāo)三維點(diǎn)，查找所述目標(biāo)三維點(diǎn)在歐式空間上的若干個(gè)最近鄰三維點(diǎn)；

3d-2d匹配模塊，用于對每一個(gè)最近鄰三維點(diǎn)，進(jìn)行3d-2d匹配，在2d圖像中查找與該最近鄰三維點(diǎn)具有相同詞匯的候選2d特征點(diǎn)，計(jì)算該最近鄰三維點(diǎn)與候選2d特征點(diǎn)在特征描述子空間上的距離；

所述3d-2d匹配模塊，還用于找到與該最近鄰三維點(diǎn)對應(yīng)的候選2d特征點(diǎn)中在特征描述子空間上距離最近和距離次近的兩個(gè)2d特征點(diǎn)，若最近距離與次近距離的比值小于第二預(yù)設(shè)閾值，則該最近鄰三維點(diǎn)與在特征描述子空間上距離該最近鄰三維點(diǎn)距離最近的目標(biāo)2d特征點(diǎn)相匹配；

第二檢測模塊，用于檢查當(dāng)前找到的三維點(diǎn)和二維點(diǎn)的匹配個(gè)數(shù)，若沒有達(dá)到第三預(yù)設(shè)閾值則返回執(zhí)行所述2d-3d檢索模塊及后續(xù)操作，直到匹配個(gè)數(shù)達(dá)到第三預(yù)設(shè)閾值；

重定位子模塊，用于在匹配個(gè)數(shù)達(dá)到第三預(yù)設(shè)閾值時(shí)，通過得到的3d與2d的匹配對計(jì)算得到機(jī)器人當(dāng)前在室內(nèi)三維點(diǎn)云地圖中的位置和目的地在室內(nèi)三維點(diǎn)云地圖中的位置，進(jìn)而得到八叉樹地圖內(nèi)的機(jī)器人當(dāng)前所處的位置和目的地的位置。

優(yōu)選地，所述導(dǎo)航模塊包括：

投影模塊，用于將所述八叉樹地圖以及在所述八叉樹地圖內(nèi)的機(jī)器人當(dāng)前所處的位置和目的地的位置進(jìn)行投影，得到二維平面的柵格地圖；

獲取計(jì)算模塊，用于獲取所述柵格地圖中機(jī)器人當(dāng)前所處的位置的投影點(diǎn)作為起點(diǎn)的所有鄰近像元，并計(jì)算各鄰近像元的f值，存入?；蜿?duì)列中，其中f值表示鄰近像元到目的地的距離值；

第三檢測模塊，用于將棧或隊(duì)列中f值最小的點(diǎn)作為新起點(diǎn)，并將所有處理過的單元和棧或隊(duì)列中的單元標(biāo)記為已處理，并檢測新起點(diǎn)是否為目的地點(diǎn)，若不是，則獲取新起點(diǎn)周圍未經(jīng)過處理的鄰近像元的f值并存入棧中，然后執(zhí)行所述獲取計(jì)算模塊及后續(xù)操作；

路徑規(guī)劃模塊，用于在新起點(diǎn)為目的地點(diǎn)時(shí)，將從機(jī)器人當(dāng)前所處位置的投影點(diǎn)到目的地位置的投影點(diǎn)之間的所有新起點(diǎn)連接起來作為從機(jī)器人當(dāng)前位置到目的地位置的最優(yōu)路徑；

導(dǎo)航子模塊，用于驅(qū)動機(jī)器人按所述最優(yōu)路徑進(jìn)行行走，最終到達(dá)目的地位置完成導(dǎo)航任務(wù)。

總體而言，通過本發(fā)明所構(gòu)思的以上技術(shù)方案與現(xiàn)有技術(shù)相比，主要有以下的技術(shù)優(yōu)點(diǎn)：

(1)按照本發(fā)明實(shí)施的室內(nèi)機(jī)器人定位與導(dǎo)航策略完全依賴于計(jì)算機(jī)視覺和機(jī)器人技術(shù)，其過程相較已有方法得到簡化，從數(shù)據(jù)輸入到模型輸出僅在地圖構(gòu)建時(shí)需要驅(qū)動機(jī)器人，其它過程全部實(shí)現(xiàn)了自動化處理。

(2)實(shí)驗(yàn)設(shè)備價(jià)格適中、易于操作，所需要的數(shù)據(jù)形式單一、采集方便。

(3)使用一種2d-3d搜索和3d-2d搜索復(fù)合的主動搜索方法來做點(diǎn)云和2d圖像特征點(diǎn)匹配，能夠?qū)崿F(xiàn)高精度的匹配和較少的計(jì)算時(shí)間，具有較好的實(shí)用性。

(3)整體策略能夠僅以視覺信息為基礎(chǔ)，完成機(jī)器人從構(gòu)圖到定位再到導(dǎo)航的任務(wù)，方案的完整度、可行性等指標(biāo)上相較于以往的方案都有很大的改善。

附圖說明

圖1為本發(fā)明實(shí)施例公開的一種機(jī)器人自主定位與導(dǎo)航方法的流程示意圖；

圖2為本發(fā)明實(shí)施例公開的另一中機(jī)器人自主定位與導(dǎo)航方法的流程示意圖；

圖3為本發(fā)明實(shí)施例中室內(nèi)三維地圖構(gòu)建和估計(jì)機(jī)器人位姿的流程圖；

圖4為本發(fā)明實(shí)施例中機(jī)器人自身以及目標(biāo)重定位的流程圖。

具體實(shí)施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。此外，下面所描述的本發(fā)明各個(gè)實(shí)施方式中所涉及到的技術(shù)特征只要彼此之間未構(gòu)成沖突就可以相互組合。

本發(fā)明公開的方法涉及到特征點(diǎn)匹配、多視圖幾何、圖優(yōu)化、slam、圖片重定位、路徑規(guī)劃，可以直接用于基于視覺傳感器的機(jī)器在室內(nèi)環(huán)境中建立三維地圖，進(jìn)而完成對自身以及目標(biāo)位置的重定位和路徑規(guī)劃任務(wù)。因此該方法能夠用于無人駕駛汽車、室內(nèi)導(dǎo)航等多個(gè)領(lǐng)域。

如圖1所示為本發(fā)明實(shí)施例公開的一種機(jī)器人自主定位與導(dǎo)航方法的流程示意圖。在圖1所示的方法中，包括以下操作：

(1)采集機(jī)器人在當(dāng)前位置的rgb圖像；

(2)將當(dāng)前位置的rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定機(jī)器人在室內(nèi)三維點(diǎn)云地圖中的當(dāng)前位置；將預(yù)存儲的目的地rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定目的地在室內(nèi)三維點(diǎn)云地圖中的目的地位置；

作為一種可選的實(shí)施方式，室內(nèi)三維點(diǎn)云地圖按照如下方式構(gòu)建：

采集機(jī)器人在行進(jìn)路線上拍攝得到的rgb圖像和深度圖像，其中，機(jī)器人的行進(jìn)路線為閉合路線；

利用上述rgb圖像和深度圖像構(gòu)建室內(nèi)三維點(diǎn)云地圖。

(3)在室內(nèi)三維點(diǎn)云地圖中搜索從當(dāng)前位置到目的地位置的最優(yōu)路徑，并驅(qū)動機(jī)器人按所述最優(yōu)路徑行走。

如圖2所示為本發(fā)明實(shí)施例公開的另一種機(jī)器人自主定位與導(dǎo)航方法的流程示意圖，從圖2可以看出，作為輸入的rgb圖像、深度圖像和目的地位置rgb圖像數(shù)據(jù)需要經(jīng)過室內(nèi)地圖構(gòu)建、機(jī)器人和目標(biāo)重定位、路徑規(guī)劃等幾個(gè)步驟，最終完成室內(nèi)機(jī)器人自主導(dǎo)航任務(wù)。其具體實(shí)施方式如下：

(1)采集機(jī)器人在行進(jìn)路線上拍攝得到的rgb圖像和深度圖像，其中，行進(jìn)路線為封閉路線；

機(jī)器人a處于一個(gè)室內(nèi)環(huán)境中，驅(qū)動機(jī)器人使其在該環(huán)境中緩慢移動，同時(shí)驅(qū)動機(jī)器人上的rgb-d相機(jī)對機(jī)器人行徑路線上的所有位置進(jìn)行拍攝得到一系列rgb圖像數(shù)據(jù)和深度圖像數(shù)據(jù)。值得注意的是，因?yàn)闄C(jī)器人需要實(shí)時(shí)地建立室內(nèi)環(huán)境地圖所以需要很大的計(jì)算量，因此為了獲取精確的地圖應(yīng)當(dāng)使得機(jī)器人行進(jìn)盡量緩慢。同時(shí)為了使得地圖優(yōu)化部分能夠順利完成，應(yīng)當(dāng)盡量使得機(jī)器人在行進(jìn)過程中形成封閉的回路。

本發(fā)明實(shí)施例中對數(shù)據(jù)的形式不做限定，機(jī)器人所處場景的rgb-d數(shù)據(jù)可以是一段連續(xù)的視頻也可以是連續(xù)的多幀圖像，但是應(yīng)該保證每一個(gè)關(guān)鍵幀都有其對應(yīng)的深度圖像。

(2)利用rgb圖像和深度圖像構(gòu)建室內(nèi)三維點(diǎn)云地圖；

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，機(jī)器人自身位姿估計(jì)和室內(nèi)場景地圖構(gòu)建slam采用rgb-dslam算法。除此之外，本發(fā)明實(shí)施例中使用的slam方法還可以任意選用能夠建立稠密三維地圖的slam方法如實(shí)時(shí)稠密跟蹤和構(gòu)圖(densetrackingandmappinginreal-time，dtmp)、稠密視覺里程計(jì)(densevisualodometryandslam，dvo)等。

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，步驟(2)的實(shí)現(xiàn)具體包括：

(2.1)將獲取的起始幀圖像作為第一幀圖像，將與起始幀圖像相鄰的下一幀圖像作為第二幀圖像；

(2.2)提取第一幀圖像與第二幀圖像的特征點(diǎn)，分別計(jì)算第一幀圖像與第二幀圖像上的關(guān)鍵點(diǎn)，針對關(guān)鍵點(diǎn)周圍的像素點(diǎn)，計(jì)算第一幀圖像的特征描述子以及第二幀圖像的特征描述子；

其中，為了提取彩色圖像的sift特征，在機(jī)器人拍攝的第一幀與其相鄰的下一幀的兩幅rgb圖像上分別計(jì)算關(guān)鍵點(diǎn)，針對這些關(guān)鍵點(diǎn)周圍的像素點(diǎn)計(jì)算各自的sift描述子，這些描述子能夠表達(dá)每一幅圖像的特征。

(2.3)采用第一幀圖像中的特征點(diǎn)到第二幀圖像的最近鄰特征點(diǎn)距離與次近鄰特征點(diǎn)距離的比值來對第一幀圖像的特征描述子與第二幀圖像的特征描述子進(jìn)行匹配，得到第一幀圖像與第二幀圖像之間的特征點(diǎn)匹配對；

其中，為了得到第一幀與第二幀之間的匹配關(guān)系，對兩關(guān)鍵幀的特征描述子用最近鄰算法進(jìn)行匹配，計(jì)算得到兩關(guān)鍵幀之間對應(yīng)的匹配對。

(2.4)根據(jù)第一幀圖像對應(yīng)的rgb圖像和深度圖像以及第二幀圖像對應(yīng)的rgb圖像和深度圖像，利用針孔相機(jī)模型計(jì)算出第一幀圖像對應(yīng)的rgb圖像和第二幀圖像對應(yīng)的rgb圖像中的二維點(diǎn)所對應(yīng)的三維點(diǎn)；

(2.5)根據(jù)第一幀圖像與第二幀圖像之間的特征點(diǎn)匹配對對應(yīng)的三維點(diǎn)，利用第一幀圖像與第二幀圖像之間的幾何關(guān)系以及非線性優(yōu)化方法估計(jì)出第一幀圖像與第二幀圖像之間的旋轉(zhuǎn)平移矩陣，得到機(jī)器人從第一幀圖像到第二幀圖像之間的位姿變換；

其中，旋轉(zhuǎn)平移矩陣表示兩關(guān)鍵幀的相對位姿變換，也即表示機(jī)器人從第一幀到相鄰下一幀之間的位姿變換。

(2.6)將第二幀圖像作為新的第一幀圖像，將與第二幀圖像相鄰的下一幀圖像作為新的第二幀圖像，檢測新的第一幀圖像是否與起始幀圖像重合，若不重合，則執(zhí)行步驟(2.2)，否則，執(zhí)行步驟(2.7)；

(2.7)將封閉路徑上的所有幀圖像對應(yīng)的機(jī)器人位姿構(gòu)建一個(gè)姿態(tài)圖，該姿態(tài)圖的節(jié)點(diǎn)代表機(jī)器人在每一個(gè)幀圖像上的姿態(tài)，邊表示節(jié)點(diǎn)之間的位姿變換，然后利用非線性優(yōu)化方法對上述姿態(tài)圖進(jìn)行優(yōu)化，得到機(jī)器人在每一幀圖像的位姿，進(jìn)而得到每一幀圖像對應(yīng)的三維點(diǎn)云圖像；

(2.8)拼接每一幀圖像對應(yīng)的三維點(diǎn)云圖像，形成室內(nèi)三維點(diǎn)云地圖，并將室內(nèi)三維點(diǎn)云地圖轉(zhuǎn)化為八叉樹地圖。

由于后續(xù)的機(jī)器人導(dǎo)航無法通過三維點(diǎn)云圖像來完成，所以需要將得到的稠密室內(nèi)三維點(diǎn)云地圖進(jìn)行轉(zhuǎn)化得到八叉樹地圖，將空間分解為相互連接且不重疊的空間單元，并且能夠得到每一個(gè)單元中是否有物體占據(jù)，從而利于機(jī)器人導(dǎo)航任務(wù)的進(jìn)行。如圖2所示為本發(fā)明實(shí)施例中室內(nèi)三維地圖構(gòu)建和估計(jì)機(jī)器人位姿的流程圖。

(3)采集機(jī)器人在當(dāng)前位置的第一rgb圖像，將第一rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定機(jī)器人在室內(nèi)三維點(diǎn)云地圖中的當(dāng)前位置；將預(yù)存儲的第二rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定目的地在室內(nèi)三維點(diǎn)云地圖中的目的地位置；

其中，預(yù)存儲的第二rgb圖像以及機(jī)器人當(dāng)前位置的第一rgb圖像可以通過以下方式獲得：實(shí)驗(yàn)人員b在室內(nèi)環(huán)境中的任意一處位置拍攝rgb圖像，將該rgb圖像發(fā)送給機(jī)器人，同時(shí)機(jī)器人a在當(dāng)前位置也拍攝rgb圖像。值得注意的是rgb圖像的拍攝盡量選取帶有紋理的圖像。

機(jī)器人和目的地的定位采用同樣的方式，對當(dāng)前的場景拍攝一張照片，通過圖定位技術(shù)即可確定機(jī)器人相對于三維點(diǎn)云地圖的初始位置和目的地相對于三維點(diǎn)云地圖的位置。

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，圖片特征點(diǎn)提取采用sift角點(diǎn)檢測算法，除此之外，本發(fā)明中使用的特征點(diǎn)還可以任意選用具有局部顯著性和穩(wěn)定性的特征如二值魯棒尺度不變特征點(diǎn)(binaryrobustinvariantscalablekeypoints，brisk)、加速分割試驗(yàn)特征(featuresfromacceleratedsegmenttest，fast)和加速穩(wěn)健特征(speeduprobustfeature，surf)等。

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，2d、3d點(diǎn)匹配采用2d-3d匹配結(jié)合3d-2d匹配的復(fù)合方法進(jìn)行，除此之外，本發(fā)明實(shí)施例中使用的2d、3d點(diǎn)匹配可以任意選用能夠成功找到準(zhǔn)確、數(shù)量足夠的2d、3d匹配的方法，如使用特征描述子直接進(jìn)行樹搜索查找近鄰點(diǎn)、或只使用2d-3d匹配或3d-2d匹配進(jìn)行檢索。

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，步驟(3)的實(shí)現(xiàn)具體包括：

(3.1)使用聚類算法對室內(nèi)三維點(diǎn)云地圖中sift特征點(diǎn)的描述子進(jìn)行聚類，得到詞匯樹；

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，可以使用近似k均值聚類算法對室內(nèi)三維點(diǎn)云sift特征點(diǎn)的描述子進(jìn)行聚類，得到一個(gè)詞匯樹。

(3.2)獲取機(jī)器人當(dāng)前位置的第一rgb圖像和機(jī)器人接收到的第二rgb圖像，并對第一rgb圖像以及第二rgb圖像進(jìn)行sift特征點(diǎn)提取，然后使用聚類算法得到視覺詞匯；

(3.3)進(jìn)行2d-3d檢索，對于第一rgb圖像以及第二rgb圖像中的每個(gè)2d特征點(diǎn)，根據(jù)該2d特征點(diǎn)對應(yīng)的視覺詞匯，在上述詞匯樹中查找與該2d特征點(diǎn)具有相同單詞的候選三維匹配點(diǎn)；

(3.4)進(jìn)行2d-3d匹配，對于第一rgb圖像以及第二rgb圖像中的每個(gè)2d特征點(diǎn)，計(jì)算該2d特征點(diǎn)與對應(yīng)的候選三維匹配點(diǎn)在特征描述子空間上的距離；

(3.5)找到與該2d特征點(diǎn)對應(yīng)的候選三維匹配點(diǎn)中在特征描述子空間上距離最近和距離次近的兩個(gè)三維點(diǎn)，若最近距離與次近距離的比值小于第一預(yù)設(shè)閾值，則該2d特征點(diǎn)與在特征描述子空間上距離該2d特征點(diǎn)距離最近的目標(biāo)三維點(diǎn)相匹配；

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，進(jìn)行2d-3d匹配，對圖片中每一個(gè)2d特征點(diǎn)，先使用線性搜索檢索候選三維匹配點(diǎn)，然后使用距離檢測決定候選點(diǎn)是否與該2d特征點(diǎn)匹配。具體包括以下操作：

(3.4.1)從第一rgb圖像或第二rgb圖像中讀取一個(gè)2d特征點(diǎn)；

(3.4.2)在詞匯樹較低層查找與該2d特征點(diǎn)具有相同詞匯的所有候選三維匹配點(diǎn)；

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中選擇在詞匯樹第3層查找候選三維匹配點(diǎn)。

(3.4.3)使用線性搜索在候選三維匹配點(diǎn)中找到與2d特征點(diǎn)在特征描述子空間上的最近點(diǎn)和次最近點(diǎn)，并計(jì)算他們與2d特征點(diǎn)的距離；

(3.4.4)若(3.4.3)中計(jì)算得到的最近距離和次最近距離的比值小于第一預(yù)設(shè)閾值，則認(rèn)為該2d特征點(diǎn)和與其在特征描述子空間上距離最近的候選三維點(diǎn)相匹配，記入匹配。否則認(rèn)為該2d特征點(diǎn)無三維點(diǎn)與其匹配。

(3.6)對于與該2d特征點(diǎn)匹配的目標(biāo)三維點(diǎn)，查找目標(biāo)三維點(diǎn)在歐式空間上的若干個(gè)最近鄰三維點(diǎn)；

(3.7)對每一個(gè)最近鄰三維點(diǎn)，進(jìn)行3d-2d匹配，在2d圖像中查找與該最近鄰三維點(diǎn)具有相同詞匯的候選2d特征點(diǎn)，計(jì)算該最近鄰三維點(diǎn)與候選2d特征點(diǎn)在特征描述子空間上的距離；

(3.8)找到與該最近鄰三維點(diǎn)對應(yīng)的候選2d特征點(diǎn)中在特征描述子空間上距離最近和距離次近的兩個(gè)2d特征點(diǎn)，若最近距離與次近距離的比值小于第二預(yù)設(shè)閾值，則該最近鄰三維點(diǎn)與在特征描述子空間上距離該最近鄰三維點(diǎn)距離最近的目標(biāo)2d特征點(diǎn)相匹配；

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，對前述每一個(gè)最近鄰三維點(diǎn)，進(jìn)行3d-2d匹配，具體方法為在圖片中查找與前述最近鄰三維點(diǎn)相匹配的所有候選2d特征點(diǎn)，然后使用距離檢測決定候選2d特征點(diǎn)是否與該最近鄰三維點(diǎn)相匹配。具體包括以下操作：

(3.7.1)從(3.6)中獲得的所有最鄰近三維點(diǎn)中讀入一個(gè)三維點(diǎn)；

(3.7.2)查找該三維點(diǎn)在(3.1)得到的詞匯樹的較高層中對應(yīng)的詞匯，并找到屬于該詞匯的所有2d特征點(diǎn)，成為該三維點(diǎn)的候選2d匹配特征點(diǎn)；

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中使用詞匯樹的第5層查找詞匯。

(3.7.3)使用線性搜索在候選2d特征點(diǎn)中找到與三維點(diǎn)在特征描述子空間上距離最近點(diǎn)和次最近點(diǎn)，并計(jì)算它們與三維點(diǎn)在前述空間上的距離；

(3.7.4)若(3.7.3)中計(jì)算得到的最近距離和次最近距離的比值小于第二預(yù)設(shè)閾值，則認(rèn)為前述三維點(diǎn)和與其在特征描述子空間上距離最近的候選2d特征點(diǎn)相匹配，記入匹配；否則回到(3.7.1)，重新讀入另外一個(gè)鄰近三維點(diǎn)并進(jìn)行3d-2d匹配檢索。

(3.9)檢查當(dāng)前找到的三維點(diǎn)和二維點(diǎn)的匹配個(gè)數(shù)，若達(dá)到第三預(yù)設(shè)閾值則執(zhí)行步驟(3.10)，否則，執(zhí)行步驟(3.3)；

(3.10)通過得到的3d與2d的匹配對計(jì)算得到機(jī)器人當(dāng)前在室內(nèi)三維點(diǎn)云地圖中的位置和目的地在室內(nèi)三維點(diǎn)云地圖中的位置，進(jìn)而得到八叉樹地圖內(nèi)的機(jī)器人當(dāng)前所處的位置和目的地的位置。

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，對離線構(gòu)建的三維點(diǎn)云和對場景拍攝的圖片進(jìn)行特征點(diǎn)匹配，并對n個(gè)匹配對使用基于6點(diǎn)直接線性變換算法(directlineartransform，dlt)的隨機(jī)抽樣一致性算法(randomsampleconsensus，ransac)方法來估計(jì)當(dāng)前相機(jī)位姿。如圖3所示為本發(fā)明實(shí)施例中機(jī)器人自身以及目標(biāo)重定位的流程圖。

(4)在室內(nèi)三維點(diǎn)云地圖中搜索從當(dāng)前位置到目的地位置的最優(yōu)路徑，并驅(qū)動機(jī)器人按上述最優(yōu)路徑行走。

優(yōu)選地，可以利用a*算法求解得到最佳路徑從而驅(qū)動機(jī)器人完成導(dǎo)航。

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，步驟(4)的實(shí)現(xiàn)具體包括：

(4.1)將八叉樹地圖以及在八叉樹地圖內(nèi)的機(jī)器人當(dāng)前所處的位置和目的地的位置進(jìn)行投影，得到二維平面的柵格地圖，該柵格地圖每一個(gè)單元為一個(gè)正方形，代表了現(xiàn)實(shí)環(huán)境中的一定空間；

(4.2)獲取柵格地圖中機(jī)器人當(dāng)前所處的位置的投影點(diǎn)作為起點(diǎn)的所有鄰近像元，并計(jì)算各鄰近像元的f值，存入?；蜿?duì)列中，其中f值表示鄰近像元到目的地的距離值；

其中，f值的計(jì)算通常的做法是起點(diǎn)到當(dāng)前點(diǎn)的距離加上當(dāng)前點(diǎn)到目的地點(diǎn)的距離。

(4.3)將?；蜿?duì)列中f值最小的點(diǎn)作為新起點(diǎn)，并將所有處理過的單元和?；蜿?duì)列中的單元標(biāo)記為已處理；

(4.4)檢測新起點(diǎn)是否為目的地點(diǎn)，若不是，則獲取新起點(diǎn)周圍未經(jīng)過處理的鄰近像元的f值并存入棧中，然后執(zhí)行步驟(4.3)，否則執(zhí)行步驟(4.5)；

(4.5)將從機(jī)器人當(dāng)前所處位置的投影點(diǎn)到目的地位置的投影點(diǎn)之間的所有新起點(diǎn)連接起來作為從機(jī)器人當(dāng)前位置到目的地位置的最優(yōu)路徑；

(4.6)驅(qū)動機(jī)器人按上述最優(yōu)路徑進(jìn)行行走，最終到達(dá)目的地位置完成導(dǎo)航任務(wù)。

在本發(fā)明的一個(gè)實(shí)施例中，公開了一種機(jī)器人自主定位與導(dǎo)航系統(tǒng)，其中，該系統(tǒng)包括：

圖像采集模塊，用于采集機(jī)器人在當(dāng)前位置的rgb圖像；

重定位模塊，用于將當(dāng)前位置的rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定機(jī)器人在室內(nèi)三維點(diǎn)云地圖中的當(dāng)前位置；將預(yù)存儲的目的地rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定目的地在室內(nèi)三維點(diǎn)云地圖中的目的地位置；

導(dǎo)航模塊，用于在室內(nèi)三維點(diǎn)云地圖中搜索從當(dāng)前位置到目的地位置的最優(yōu)路徑，并驅(qū)動機(jī)器人按所述最優(yōu)路徑行走。

作為一種可選的實(shí)施方式，上述圖像采集模塊，還用于采集機(jī)器人在行進(jìn)路線上拍攝得到的rgb圖像和深度圖像，其中，機(jī)器人的行進(jìn)路線為閉合路線；

作為一種可選的實(shí)施方式，該系統(tǒng)還包括：室內(nèi)地圖構(gòu)建模塊，用于利用所述rgb圖像和深度圖像構(gòu)建室內(nèi)三維點(diǎn)云地圖。

其中，各模塊的具體實(shí)施方式可以參照方法實(shí)施例的描述，本發(fā)明實(shí)施例將不做復(fù)述。

本領(lǐng)域的技術(shù)人員容易理解，以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。