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

背景技術(shù)：

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

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

目前，對(duì)于實(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ǎng)景，無(wú)法對(duì)地圖產(chǎn)生直觀的感受。

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

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

然而，由于真實(shí)場(chǎng)景下點(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ī)器人對(duì)環(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)注，成為移動(dòng)機(jī)器人路徑規(guī)劃的研究熱點(diǎn)之一。但機(jī)器人傳感器信息資源有限，使得網(wǎng)格地圖障礙物信息很難計(jì)算與處理，同時(shí)由于機(jī)器人要?jiǎng)討B(tài)快速地更新地圖數(shù)據(jù)，在網(wǎng)格數(shù)較多、分辨率較高時(shí)難以保證路徑規(guī)劃的實(shí)時(shí)性。因此，地圖構(gòu)建方法必須在地圖網(wǎng)格分辨率與路徑規(guī)劃實(shí)時(shí)性上尋求平衡。

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

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

針對(duì)現(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ù)問(wèn)題。

為實(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ù)存儲(chǔ)的目的地rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定目的地在室內(nèi)三維點(diǎn)云地圖中的目的地位置；

(3)在室內(nèi)三維點(diǎn)云地圖中搜索從當(dāng)前位置到目的地位置的最優(yōu)路徑，并驅(qū)動(dòng)機(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)，針對(duì)關(guān)鍵點(diǎn)周圍的像素點(diǎn)，計(jì)算所述第一幀圖像的特征描述子以及所述第二幀圖像的特征描述子；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(2.8)找到與該最近鄰三維點(diǎn)對(duì)應(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)通過(guò)得到的3d與2d的匹配對(duì)計(jì)算得到機(jī)器人當(dāng)前在室內(nèi)三維點(diǎn)云地圖中的位置和目的地在室內(nèi)三維點(diǎn)云地圖中的位置，進(jìn)而得到八叉樹(shù)地圖內(nèi)的機(jī)器人當(dāng)前所處的位置和目的地的位置。

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

(3.1)將所述八叉樹(shù)地圖以及在所述八叉樹(shù)地圖內(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)，并將所有處理過(guò)的單元和棧或隊(duì)列中的單元標(biāo)記為已處理；

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

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

(3.6)驅(qū)動(dòng)機(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ù)存儲(chǔ)的目的地rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定目的地在室內(nèi)三維點(diǎn)云地圖中的目的地位置；

導(dǎo)航模塊，用于在室內(nèi)三維點(diǎn)云地圖中搜索從當(dāng)前位置到目的地位置的最優(yōu)路徑，并驅(qū)動(dòng)機(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)，針對(duì)關(guān)鍵點(diǎn)周圍的像素點(diǎn)，計(jì)算所述第一幀圖像的特征描述子以及所述第二幀圖像的特征描述子；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附圖說(shuō)明

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

圖2為本發(fā)明實(shí)施例公開(kāi)的另一中機(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í)施例，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。此外，下面所描述的本發(fā)明各個(gè)實(shí)施方式中所涉及到的技術(shù)特征只要彼此之間未構(gòu)成沖突就可以相互組合。

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

如圖1所示為本發(fā)明實(shí)施例公開(kāi)的一種機(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ù)存儲(chǔ)的目的地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ū)動(dòng)機(jī)器人按所述最優(yōu)路徑行走。

如圖2所示為本發(fā)明實(shí)施例公開(kāi)的另一種機(jī)器人自主定位與導(dǎo)航方法的流程示意圖，從圖2可以看出，作為輸入的rgb圖像、深度圖像和目的地位置rgb圖像數(shù)據(jù)需要經(jīng)過(guò)室內(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ū)動(dòng)機(jī)器人使其在該環(huán)境中緩慢移動(dòng)，同時(shí)驅(qū)動(dòng)機(jī)器人上的rgb-d相機(jī)對(duì)機(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)過(guò)程中形成封閉的回路。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

由于后續(xù)的機(jī)器人導(dǎo)航無(wú)法通過(guò)三維點(diǎn)云圖像來(lái)完成，所以需要將得到的稠密室內(nèi)三維點(diǎn)云地圖進(jìn)行轉(zhuǎn)化得到八叉樹(shù)地圖，將空間分解為相互連接且不重疊的空間單元，并且能夠得到每一個(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ù)存儲(chǔ)的第二rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定目的地在室內(nèi)三維點(diǎn)云地圖中的目的地位置；

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

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

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中，圖片特征點(diǎn)提取采用sift角點(diǎn)檢測(cè)算法，除此之外，本發(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)行樹(shù)搜索查找近鄰點(diǎn)、或只使用2d-3d匹配或3d-2d匹配進(jìn)行檢索。

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

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

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

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

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

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

(3.5)找到與該2d特征點(diǎn)對(duì)應(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匹配，對(duì)圖片中每一個(gè)2d特征點(diǎn)，先使用線性搜索檢索候選三維匹配點(diǎn)，然后使用距離檢測(cè)決定候選點(diǎn)是否與該2d特征點(diǎn)匹配。具體包括以下操作：

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

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

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中選擇在詞匯樹(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)無(wú)三維點(diǎn)與其匹配。

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

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

(3.8)找到與該最近鄰三維點(diǎn)對(duì)應(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í)施例中，對(duì)前述每一個(gè)最近鄰三維點(diǎn)，進(jìn)行3d-2d匹配，具體方法為在圖片中查找與前述最近鄰三維點(diǎn)相匹配的所有候選2d特征點(diǎn)，然后使用距離檢測(cè)決定候選2d特征點(diǎn)是否與該最近鄰三維點(diǎn)相匹配。具體包括以下操作：

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

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

優(yōu)選地，在本發(fā)明的一個(gè)實(shí)施例中使用詞匯樹(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)通過(guò)得到的3d與2d的匹配對(duì)計(jì)算得到機(jī)器人當(dāng)前在室內(nèi)三維點(diǎn)云地圖中的位置和目的地在室內(nèi)三維點(diǎn)云地圖中的位置，進(jìn)而得到八叉樹(shù)地圖內(nèi)的機(jī)器人當(dāng)前所處的位置和目的地的位置。

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

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

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

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

(4.1)將八叉樹(shù)地圖以及在八叉樹(shù)地圖內(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)，并將所有處理過(guò)的單元和?；蜿?duì)列中的單元標(biāo)記為已處理；

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

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

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

在本發(fā)明的一個(gè)實(shí)施例中，公開(kāi)了一種機(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ù)存儲(chǔ)的目的地rgb圖像與室內(nèi)三維點(diǎn)云地圖進(jìn)行特征點(diǎn)匹配，確定目的地在室內(nèi)三維點(diǎn)云地圖中的目的地位置；

導(dǎo)航模塊，用于在室內(nèi)三維點(diǎn)云地圖中搜索從當(dāng)前位置到目的地位置的最優(yōu)路徑，并驅(qū)動(dòng)機(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)。