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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(3.3)將?；蜿犃兄衒值最小的點作為新起點，并將所有處理過的單元和棧或隊列中的單元標(biāo)記為已處理；

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

(3.5)將從機(jī)器人當(dāng)前所處位置的投影點到目的地位置的投影點之間的所有新起點連接起來作為從機(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)三維點云地圖進(jìn)行特征點匹配，確定機(jī)器人在室內(nèi)三維點云地圖中的當(dāng)前位置；將預(yù)存儲的目的地rgb圖像與室內(nèi)三維點云地圖進(jìn)行特征點匹配，確定目的地在室內(nèi)三維點云地圖中的目的地位置；

導(dǎo)航模塊，用于在室內(nèi)三維點云地圖中搜索從當(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)三維點云地圖。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

路徑規(guī)劃模塊，用于在新起點為目的地點時，將從機(jī)器人當(dāng)前所處位置的投影點到目的地位置的投影點之間的所有新起點連接起來作為從機(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)點：

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

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

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

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

附圖說明

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

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

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

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

具體實施方式

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(4.3)將?；蜿犃兄衒值最小的點作為新起點，并將所有處理過的單元和?；蜿犃兄械膯卧獦?biāo)記為已處理；

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

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

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

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

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

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

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

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

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

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

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