基於多傳感器數據融合的機器人導航方法及裝置與流程

2023-04-30 14:15:28

本發明實施例涉及人工智慧技術領域，尤其涉及一種基於多傳感器數據融合的機器人導航方法及裝置。

背景技術：

機器人自主導航技術是智慧機器人領域的熱門技術，通過自主導航技術，機器人可以智能的在環境中移動，從而完成導引、搬運、交互等任務。所以自主導航技術是機器人走向智能化的基礎，無法自主移動的機器人不能稱之為智慧機器人。

現有的機器人自主導航技術可大致分為兩大類：一類是有源的自主導航技術，一類是無源的自主導航技術。有源的自主導航技術即機器人需要依賴外部設備實現的自主導航，例如需要在環境中部署基站、使用GPS設備等，因此靈活性差。此外由於GPS本身的限制，其無法應用於室內環境下的定位，並且GPS的精度往往也無法滿足機器人自主行走的要求。無源的自主導航技術指機器人無需依賴外部設備，只使用自身傳感器實現自主導航。這一類技術靈活性好，可應用在室內或室外環境下，無需專業人員、專業設備部署。

但是，現有的無源自主導航技術通常使用單一傳感器，無法提供較為全面的數據，即便使用多種傳感器，也通常是將各傳感器採集的數據不合理地融合，導致自主導航效果較差。

技術實現要素：

本發明實施例提供一種基於多傳感器數據融合的機器人導航方法及裝置，用於解決現有的機器人自主導航效果較差的技術問題。

本發明實施例提供一種基於多傳感器數據融合的機器人導航方法，包括：根據雷射雷達傳感器採集到的數據和編碼器數據，利用即時定位與地圖構建技術，構建總體環境地圖；實時根據所述雷射雷達傳感器、加速度計傳感器、陀螺儀傳感器和磁強計傳感器採集的數據、所述總體環境地圖、以及編碼器數據，通過視覺定位算法獲得所述機器人在所述總體環境地圖中的當前位置；實時根據所述總體環境地圖和所述機器人的當前位置，通過路徑規划算法獲得所述機器人從所述當前位置到目標位置的規劃路線；根據所述當前位置和所述規劃路線，利用所述雷射雷達傳感器、深度攝像頭、超聲波傳感器和紅外傳感器採集的數據、以及編碼器數據，通過局部路徑規划算法控制所述機器人避開障礙物移動。

本發明實施例提供一種基於多傳感器數據融合的機器人導航裝置，包括：地圖構建模塊，用於根據雷射雷達傳感器採集到的數據和編碼器數據，利用即時定位與地圖構建技術，構建總體環境地圖；實時定位模塊，用於實時根據所述雷射雷達傳感器、加速度計傳感器、陀螺儀傳感器和磁強計傳感器採集的數據、所述總體環境地圖、以及編碼器數據，通過視覺定位算法獲得所述機器人在所述總體環境地圖中的當前位置；路線規劃模塊，用於實時根據所述總體環境地圖和所述機器人的當前位置，通過路徑規划算法獲得所述機器人從所述當前位置到目標位置的規劃路線；控制模塊，用於根據所述當前位置和所述規劃路線，利用所述雷射雷達傳感器、深度攝像頭、超聲波傳感器和紅外傳感器採集的數據、以及編碼器數據，通過局部路徑規划算法控制所述機器人避開障礙物移動。

本發明提供的基於多傳感器數據融合的機器人導航方法及裝置，利用雷射雷達傳感器採集到的數據和編碼器數據，構建總體環境地圖，並根據編碼器數據、雷射雷達傳感器、加速度計傳感器、陀螺儀傳感器和磁強計傳感器採集的數據對機器人進行實時定位，根據機器人的當前位置規劃路線，並結合雷射雷達傳感器、深度攝像頭、超聲波傳感器和紅外傳感器採集的數據，控制機器人避開障礙物移動，從而實現機器人自主導航，本方案基於不同傳感器的特點，利用不同的傳感器數據進行融合併用於相應的處理，在合理利用傳感器實現機器人導航的基礎上，靈活應用於多種場景，並且兼顧成本，能夠實現較好的自主導航效果。

附圖說明

為了更清楚地說明本發明實施例或現有技術中的技術方案，下面將對實施例或現有技術描述中所需要使用的附圖作一簡單地介紹。

圖1為本發明實施例一提供的基於多傳感器數據融合的機器人導航方法的流程示意圖；

圖2為本發明實施例二提供的基於多傳感器數據融合的機器人導航方法的流程示意圖；

圖3A為本發明實施例三提供的一種基於多傳感器數據融合的機器人導航方法的流程示意圖；

圖3B為本發明實施例三提供的另一種基於多傳感器數據融合的機器人導航方法的流程示意圖；

圖4為本發明實施例四提供的基於多傳感器數據融合的機器人導航方法的流程示意圖；

圖5為本發明實施例五提供的基於多傳感器數據融合的機器人導航裝置的結構示意圖；

圖6為本發明實施例六提供的基於多傳感器數據融合的機器人導航裝置的結構示意圖；

圖7A為本發明實施例七提供的一種基於多傳感器數據融合的機器人導航裝置的結構示意圖；

圖7B為本發明實施例七提供的另一種基於多傳感器數據融合的機器人導航裝置的結構示意圖；

圖8為本發明實施例八提供的基於多傳感器數據融合的機器人導航裝置的結構示意圖。

具體實施方式

為使本發明實施例的目的、技術方案和優點更加清楚，下面將結合本發明實施例中的附圖，對本發明實施例中的技術方案進行清楚、完整地描述。基於本發明中的實施例，本領域普通技術人員在沒有做出創造性勞動前提下所獲得的所有其他實施例，都屬於本發明保護的範圍。

為了清楚起見，首先說明本發明使用的特定詞或短語的定義。

雷射雷達：提供機器人距離周圍環境障礙物的距離信息，通常為周圍環境的一個二維切面。可用於地圖構建、定位、實時避障等。精度較高，穩定性高，噪聲低，成本高。可視距離遠，近距離處盲區小。

深度攝像頭：提供機器人距離周圍環境障礙物的距離信息，通常為周圍環境的三維點雲。可用於地圖構建、定位、實時避障等。精度較低，穩定性低，噪聲高，成本相對較低。可視距離近，近距離盲區大。

超聲波傳感器：提供機器人距離周圍環境障礙物的距離信息，通常為一維單點。可用於實時避障。數據精度低，穩定性低，噪聲高，成本很低。可視距離較遠，近距離處盲區大。

紅外傳感器：提供機器人距離周圍環境障礙物的距離信息，通常為一維單點。可用於實時避障。數據精度相對高，穩定性相對高，噪聲相對高，成本很低。可視距離近，近距離處無盲區。

加速度計：可提供機器人的瞬時線加速度值。可用於定位、實時避障。

陀螺儀：可提供機器人的瞬時角速度。可用於定位、實時避障。

磁強計：可提供機器人的絕對朝向估計。可用於定位、實時避障。

編碼器：可提供機器人行走的裡程、速度等估計。用於定位、實時避障。

圖1為本發明實施例一提供的基於多傳感器數據融合的機器人導航方法的流程示意圖，如圖1所示，本實施例以該方法應用於機器人導航裝置中來舉例說明，該機器人導航裝置可集成在機器人自主導航系統中，該方法包括：

101、根據雷射雷達傳感器採集到的數據和編碼器數據，利用即時定位與地圖構建技術，構建總體環境地圖。

具體地，即時定位與地圖構建方法(Simultaneous Localization And Mapping，簡稱SLAM)包括但不限於：掃描匹配、圖優化等方法。

以實際場景舉例來說：當將機器人置於一個新環境時，需要使用即時定位與地圖構建方法，繪製當前環境的地圖。具體的，控制機器人在該環境中移動，雷射雷達傳感器不斷收集數據，並利用SLAM算法，實時計算並繪製出相應的總體環境地圖，該總體環境地圖為二維柵格地圖，該二維柵格地圖後續主要用於全局的路徑規劃及機器人自主定位。其中，雷射雷達傳感器採集的數據用於SLAM算法的數據匹配，編碼器數據用於為SLAM算法的每次迭代提供初始估計，從而加速地圖構建過程。

上述建立的總體環境地圖可以被重複使用，因此地圖構建過程往往只需要進行一次。只有當環境有很大程度的改變，或機器人被置於新環境時，才需要重新構建總體環境地圖。

由於全局的路徑規劃只是作為機器人實際行走過程的大方向參考，因此只需確定大致的路徑即可。相應的，進行全局路徑規劃只要求總體環境地圖包括環境的主要信息即可，以室內環境為例，可以包括：牆、室內家具、室外電線桿等，而雷射雷達傳感器提供的環境二維切面數據足夠提供以上主要信息。另外，機器人進行自主定位時，需要實時的傳感器數據與地圖做匹配，而該匹配過程主要基於大的線特徵，例如：牆、大型障礙物等，而雷射雷達的二維切面信息也足以提供這些信息。儘管自主定位對於精度的要求較高，雷射雷達傳感器採集的數據往往精度高，噪聲小，因此基於雷射雷達數據構建的地圖可以優化自主定位的效果。可見，採用雷射雷達傳感器採集的數據構建總體環境地圖，能夠在構建環境地圖和實現自主定位的基礎上，節省地圖構建所需的時間，減小數據處理量，提高導航效率。實際應用中，可以將雷射雷達傳感器安裝在機器人的底盤上。

102、實時根據所述雷射雷達傳感器、加速度計傳感器、陀螺儀傳感器和磁強計傳感器採集的數據、所述總體環境地圖、以及編碼器數據，通過視覺定位算法獲得所述機器人在所述總體環境地圖中的當前位置。

以實際場景舉例來說：基於前述步驟構建的總體環境地圖，利用雷射雷達傳感器、加速度計傳感器、陀螺儀傳感器和磁強計傳感器採集的數據進行實時定位，獲得機器人的當前位置，該當前位置為機器人當前在總體環境地圖下的位置信息。

103、實時根據所述總體環境地圖和所述機器人的當前位置，通過路徑規划算法獲得所述機器人從所述當前位置到目標位置的規劃路線。

其中，路徑規划算法包括但不限於：A star算法、迪傑斯特拉算法等。

以實際場景舉例來說：對機器人進行定位，即獲取機器人的當前位置後，即可根據機器人的當前位置和前述步驟構建的總體環境地圖，進行從當前位置至目標位置的路線規劃，獲得規劃路線。

104、根據所述當前位置和所述規劃路線，利用所述雷射雷達傳感器、深度攝像頭、超聲波傳感器和紅外傳感器採集的數據、以及編碼器數據，通過局部路徑規划算法控制所述機器人避開障礙物移動。

其中，局部規划算法包括但不限於：動態窗口模擬、D Star算法等。

以實際場景舉例來說：基於實時得到的規劃路線和機器人的當前位置，通過雷射雷達傳感器、深度攝像頭、超聲波傳感器和紅外傳感器採集的數據，通過局部路徑規划算法進行決策，可控制機器人移動行走，並在移動行走過程中實時避開周圍的靜態或動態障礙物。

具體的，由於局部路徑規劃要保證機器人行走的安全及流暢，因此需要使用雷射雷達、深度攝像頭、超聲波傳感器、紅外傳感器採集的數據和編碼器數據進行融合。

實際應用中，由於雷射雷達傳感器無法檢測到高於或低於自身雷射雷達切面的障礙物，因此可以在雷射雷達傳感器更低處安裝紅外傳感器和超聲波傳感器，以檢測近地面障礙物。由於超聲波有近距離盲區但可視距離遠，而紅外可視距離近但無近距離盲區，因此將兩者配合使用，達到可視距離遠無盲區的效果。此外，使用深度攝像頭獲取近處環境的三維點雲數據，可以直接得到機器人前方遮擋的所有物體信息。

本實施例提供的基於多傳感器數據融合的機器人導航方法，利用雷射雷達傳感器採集到的數據和編碼器數據，構建總體環境地圖，並根據編碼器數據、雷射雷達傳感器、加速度計傳感器、陀螺儀傳感器和磁強計傳感器採集的數據對機器人進行實時定位，根據機器人的當前位置規劃路線，並結合雷射雷達傳感器、深度攝像頭、超聲波傳感器和紅外傳感器採集的數據，控制機器人避開障礙物移動，從而實現機器人自主導航，本方案基於不同傳感器的特點，利用不同的傳感器數據進行融合併用於相應的處理，在合理利用傳感器實現機器人導航的基礎上，靈活應用於多種場景，並且兼顧成本，能夠實現較好的自主導航效果。

圖2為本發明實施例二提供的基於多傳感器數據融合的機器人導航方法的流程示意圖，如圖2所示，本實施例仍以該方法應用於機器人導航裝置中來舉例說明，在實施例一的基礎上，102包括：

201、根據磁強計傳感器採集的數據，對所述機器人的姿態進行初始化；

202、實時根據當前時刻與上一時刻所述加速度計傳感器和所述陀螺儀傳感器採集的數據、以及所述編碼器數據，計算當前時刻相對於上一時刻所述機器人的位置和姿態變化估計；

203、確定所述總體環境地圖中與所述位置和姿態變化估計對應的地圖點，獲得所述地圖點的地圖數據；

204、通過將所述雷射雷達傳感器當前採集的數據與所述地圖點的地圖數據分別進行匹配，獲得所述機器人的當前位置，所述機器人的當前位置為匹配度最高的地圖點的位置。

以實際場景舉例來說：在初始化時，首先讀取磁強計傳感器採集的數據，獲得機器人絕對朝向的初始估計，使用這個初始估計進行方向初始化。在機器人移動過程中，不斷讀取編碼器數據、以及所述加速度計傳感器和所述陀螺儀傳感器採集的數據，計算當前時刻相對於上一時刻的位置和姿態變化估計。其中，對編碼器數據進行積分處理即可獲得位置和姿態變化估計，對加速度計傳感器採集的數據進行積分可以獲得位置變化估計，對陀螺儀傳感器採集的數據進行積分可以獲得姿態變化估計，之後通過對上述三者加權平均，即可得到當前時刻相對於上一時刻的位置和姿態變化估計。之後，根據位置和姿態變化估計，用相應地圖點的地圖數據和雷射雷達傳感器當前採集的數據做匹配，匹配度最高的地圖點即為機器人的當前位置。

具體地，可以採用蒙特卡洛粒子濾波方法進行機器人自主定位。相應的，根據磁強計傳感器採集的數據，對粒子的姿態進行初始化，在機器人移動過程中，實時根據當前時刻與上一時刻所述加速度計傳感器和所述陀螺儀傳感器採集的數據、以及所述編碼器數據，計算當前時刻相對於上一時刻的粒子位置和姿態變化估計，根據位置和姿態變化估計確定一些地圖點，將這些地圖點的地圖數據與雷射雷達傳感器當前採集的數據做匹配，匹配度最高的地圖點即為機器人的當前位置。

在上述匹配過程中，雷射雷達傳感器的精度高、噪聲低，且觀測距離遠，可以獲取遠處的線特徵，而匹配的主要依據便是線特徵。因此，只基於雷射雷達傳感器採集的數據進行匹配即可，可以有效節省匹配所需時間，提高機器人定位的效率，從而更快實現導航。

並且，結合使用編碼器數據、加速度計傳感器、陀螺儀傳感器計算當前粒子的位置和姿態變化估計，實現多源融合，可以提高機器人定位的準確度，從而更準確可靠地實現機器人導航。

本實施例提供的基於多傳感器數據融合的機器人導航方法，在實現機器人自主定位的過程中，結合使用編碼器數據、加速度計傳感器、陀螺儀傳感器計算當前的位置和姿態變化估計，僅基於雷射雷達傳感器採集的數據進行匹配，不但可以有效節省匹配所需時間，還能提高機器人定位的準確度，從而更快速準確可靠地實現機器人導航。

圖3A為本發明實施例三提供的一種基於多傳感器數據融合的機器人導航方法的流程示意圖，如圖3A所示，本實施例仍以該方法應用於機器人導航裝置中來舉例說明，在實施例一或實施例二的基礎上，104包括：

301、實時根據所述超聲波傳感器數據或所述紅外傳感器採集的數據，判斷所述機器人是否處於移動受阻狀態；

302、若所述機器人未處於移動受阻狀態，則根據所述深度攝像頭和所述雷射雷達傳感器當前採集的數據，構建局部環境地圖，並根據所述局部環境地圖、所述機器人的當前位置和規劃路線，通過局部路徑規划算法控制所述機器人避開障礙物移動。

以實際場景舉例來說：局部規劃策略總體分為兩個情形，一個情形是正常狀態下的規劃策略，另一個情形是特殊狀態處理。具體的，針對第一種情形，實時根據超聲波傳感器數據或紅外傳感器採集的數據，判斷所述機器人是否處於移動受阻狀態，若未處於移動受阻狀態，即機器人正常行走過程中，使用常規的規劃策略進行規劃，即根據深度攝像頭和所雷射雷達傳感器當前採集的數據構建的局部環境地圖，結合前面獲取的機器人的當前位置和規劃路線，通過局部路徑規划算法控制機器人避開障礙物移動。具體地，可以採用動態窗口模擬算法進行機器人移動控制。

本實施例中，動態窗口模擬算法會依據各傳感器採集數據，實時構建機器人周圍小範圍內的局部環境地圖，再基於該局部環境地圖模擬計算，進行決策。具體的，局部環境地圖的構建採用二維面數據和三維點雲數據，即雷射雷達傳感器和深度攝像頭採集的數據。採用這兩種傳感器的原因是局部環境地圖需要實時更新，如果將類似超聲波傳感器和紅外傳感器採集的一維點數據構建到局部環境地圖，可能會導致無法清理掉障礙物的問題，即當某一時刻下，超聲波傳感器或紅外傳感器觀測到有一個障礙物，而下一時刻觀測的並不是這個障礙物點所在的位置，也就無法確定上一時刻的障礙物是否還依然存在，逐漸地局部環境地圖會逐漸累積障礙物，導致決策下的機器人無路可走，最終導致導航失敗。而二維面數據和三維點雲數據就不存在這個問題，因為雷射雷達傳感器和深度攝像頭觀測的不是一個點，而是一個範圍，因此可以在大部分範圍內保證地圖的實時性，提高導航的可靠性。

針對另一種情形，即實時根據超聲波傳感器數據或紅外傳感器採集的數據，判斷所述機器人處於移動受阻狀態時，即機器人遇到特殊情況，例如：距離障礙物過近、由於定位誤差導致的與障礙物重合、被障礙物包圍等情況，則進入特殊狀態處理，具體的，可以根據超聲波傳感器數據或紅外傳感器當前採集的數據及預設的方向選擇策略，控制所述機器人避開障礙物移動。相應的，如圖3B所示，圖3B為本發明實施例三提供的另一種基於多傳感器數據融合的機器人導航方法的流程示意圖，如圖3B所示，本實施例仍以該方法應用於基於多傳感器數據融合的機器人導航裝置中來舉例說明，在圖3A所示實施方式的基礎上，在301之後，還可以包括：

303、若所述機器人處於移動受阻狀態，則根據所述超聲波傳感器數據或所述紅外傳感器當前採集的數據及預設的方向選擇策略，控制所述機器人避開障礙物移動。

其中，超聲波傳感器和紅外傳感器用於特殊情況處理，例如，當超聲波傳感器或紅外傳感器觀測到距離障礙物過近、機器人被包圍或由於定位誤差機器人認為自身與障礙物重合等情況時，則進行特殊狀態處理。此時不再依據局部環境地圖進行決策，而是直接選擇向遠離障礙物的方向進行逃離，具體的方向選擇策略可使用包括但不限於最近點反方向、人工勢場合成等方式進行選擇。

本實施例提供的基於多傳感器數據融合的機器人導航方法，在對機器人的移動控制策略中，基於超聲波傳感器數據或紅外傳感器採集的數據判定當前狀態，若正常，則根據深度攝像頭和雷射雷達傳感器當前採集的數據構建局部環境地圖，並基於該局部環境地圖，結合機器人當前位置和規劃路線，控制機器人避開障礙物移動，否則，採用預設的方向選擇策略進行逃離，從而確保機器人導航的可靠性。

進一步的，圖4為本發明實施例四提供的基於多傳感器數據融合的機器人導航方法的流程示意圖，如圖4所示，本實施例仍以該方法應用於機器人導航裝置中來舉例說明，在實施例三的基礎上，302中所述根據所述深度攝像頭和所述雷射雷達傳感器當前採集的數據，構建局部環境地圖，包括：

401、對所述深度攝像頭當前採集的數據進行去噪，並將去噪後的數據向二維平面進行投影，獲得二維投影數據；

402、若二維投影數據相應位置不存在所述雷射雷達傳感器採集的數據，則將相應位置的局部環境地圖數據以對應的二維投影數據進行填充；

403、若所述二維投影數據相應位置存在所述雷射雷達傳感器採集的數據，則將所述二維投影數據和所述雷射雷達傳感器採集的數據進行加權平均，以獲得相應位置的局部環境地圖數據。

以實際場景舉例來說：局部規劃策略總體分為兩個情形，一個情形是正常狀態下的規劃策略，另一個情形是特殊狀態處理。具體的，針對第一種情形，實時根據超聲波傳感器數據或紅外傳感器採集的數據，判斷所述機器人是否處於移動受阻狀態，若未處於移動受阻狀態，即機器人正常行走過程中，使用常規的規劃策略進行規劃，即根據深度攝像頭和所雷射雷達傳感器當前採集的數據構建的局部環境地圖，結合前面獲取的機器人的當前位置和規劃路線，通過局部路徑規划算法控制機器人避開障礙物移動。

本實施例中，以採用動態窗口模擬算法控制機器人移動為例，動態窗口模擬算法是基於二維柵格地圖，因此需要先對三維點雲數據進行去噪，然後將三維點雲數據向二維平面進行投影獲得二維投影數據，此時將三維點雲數據分列投影，如果某一列有障礙物點，則此列的投影結果為障礙物，否則投影為空閒。將二維投影數據與雷射雷達傳感器採集的二維平面數據進行融合，具體策略為，使用二維投影數據對雷射雷達傳感器採集的二維數據進行更新，即對於雷射雷達傳感器未觀測到的位置直接使用獲得的二維投影數據進行填充，對於雷射雷達傳感器觀測到的位置，採用二維投影數據和雷射雷達傳感器採集到的數據進行加權平均。此方法適用於多個深度攝像頭和多個雷射雷達傳感器。

本實施例提供的基於多傳感器數據融合的機器人導航方法，將深度攝像頭採集的三維點雲數據進行去噪投影，基於獲得的二維投影數據和雷射雷達傳感器採集到的二維平面數據進行融合，構建局部環境地圖，實現對機器人的移動控制，本實施例通過將二維平面數據，三維點雲數據進行融合處理，可以達到較好的局部規劃效果，提高導航準確性。

本領域普通技術人員可以理解：實現上述各方法實施例的全部或部分步驟可以通過程序指令相關的硬體來完成。前述的程序可以存儲於一可讀取存儲介質中。該程序在執行時，執行包括上述各方法實施例的步驟；而前述的存儲介質包括：ROM、RAM、磁碟或者光碟等各種可以存儲程序代碼的介質。

圖5為本發明實施例五提供的基於多傳感器數據融合的機器人導航裝置的結構示意圖，如圖5所示，該機器人導航裝置可集成在機器人自主導航系統中，該裝置包括：

地圖構建模塊51，用於根據雷射雷達傳感器採集到的數據和編碼器數據，利用即時定位與地圖構建技術，構建總體環境地圖；

實時定位模塊52，用於實時根據所述雷射雷達傳感器、加速度計傳感器、陀螺儀傳感器和磁強計傳感器採集的數據、所述總體環境地圖、以及編碼器數據，通過視覺定位算法獲得所述機器人在所述總體環境地圖中的當前位置；

路線規劃模塊53，用於實時根據所述總體環境地圖和所述機器人的當前位置，通過路徑規划算法獲得所述機器人從所述當前位置到目標位置的規劃路線；

控制模塊54，用於根據所述當前位置和所述規劃路線，利用所述雷射雷達傳感器、深度攝像頭、超聲波傳感器和紅外傳感器採集的數據、以及編碼器數據，通過局部路徑規划算法控制所述機器人避開障礙物移動。

具體地，地圖構建模塊51採用的即時定位與地圖構建方法包括但不限於：掃描匹配、圖優化等方法。以實際場景舉例來說：當將機器人置於一個新環境時，需要使用即時定位與地圖構建方法，繪製當前環境的地圖。具體的，控制機器人在該環境中移動，雷射雷達傳感器不斷收集數據，地圖構建模塊51利用SLAM算法，實時計算並繪製出相應的總體環境地圖。基於地圖構建模塊51構建的總體環境地圖，實時定位模塊52利用雷射雷達傳感器、加速度計傳感器、陀螺儀傳感器和磁強計傳感器採集的數據進行實時定位，獲得機器人的當前位置。實時定位模塊52獲取機器人的當前位置後，路線規劃模塊53根據實時定位模塊52獲得的機器人的當前位置和地圖構建模塊51構建的總體環境地圖，進行從當前位置至目標位置的路線規劃，獲得規劃路線。基於實時得到的規劃路線和機器人的當前位置，控制模塊54通過雷射雷達傳感器、深度攝像頭、超聲波傳感器和紅外傳感器採集的數據，通過局部路徑規划算法進行決策，可控制機器人移動行走，並在移動行走過程中實時避開周圍的靜態或動態障礙物。

其中，所述總體環境地圖為二維柵格地圖。雷射雷達傳感器採集的數據用於SLAM算法的數據匹配，編碼器數據用於為SLAM算法的每次迭代提供初始估計，從而加速地圖構建過程。並且雷射雷達傳感器採集的數據往往精度高，噪聲小，因此基於雷射雷達數據構建的地圖可以優化自主定位的效果。採用雷射雷達傳感器採集的數據構建總體環境地圖，能夠在構建環境地圖和實現自主定位的基礎上，節省地圖構建所需的時間，減小數據處理量，提高導航效率。實際應用中，可以將雷射雷達傳感器安裝在機器人的底盤上。

上述建立的總體環境地圖可以被重複使用，因此地圖構建模塊51往往只需要進行一次地圖構建過程。只有當環境有很大程度的改變，或機器人被置於新環境時，地圖構建模塊51才需要重新構建總體環境地圖。

其中，路徑規划算法包括但不限於：A star算法、迪傑斯特拉算法等，局部規划算法包括但不限於：動態窗口模擬、D Star算法等。

具體的，由於局部路徑規劃要保證機器人行走的安全及流暢，因此需要使用雷射雷達、深度攝像頭、超聲波傳感器、紅外傳感器採集的數據和編碼器數據進行融合。

其中，本實施例提供的機器人導航裝置可以執行實施例一的方法實施例的技術方案，其實現原理和技術效果類似，此處不再贅述。

本實施例提供的基於多傳感器數據融合的機器人導航裝置，利用雷射雷達傳感器採集到的數據和編碼器數據，構建總體環境地圖，並根據編碼器數據、雷射雷達傳感器、加速度計傳感器、陀螺儀傳感器和磁強計傳感器採集的數據對機器人進行實時定位，根據機器人的當前位置規劃路線，並結合雷射雷達傳感器、深度攝像頭、超聲波傳感器和紅外傳感器採集的數據，控制機器人避開障礙物移動，從而實現機器人自主導航，本方案基於不同傳感器的特點，利用不同的傳感器數據進行融合併用於相應的處理，在合理利用傳感器實現機器人導航的基礎上，靈活應用於多種場景，並且兼顧成本，能夠實現較好的自主導航效果。

圖6為本發明實施例六提供的基於多傳感器數據融合的機器人導航裝置的結構示意圖，如圖6所示，在實施例五的基礎上，實時定位模塊52包括：

初始化單元521，用於根據所述磁強計傳感器採集的數據，對所述機器人的姿態進行初始化；

估計單元522，用於實時根據當前時刻與上一時刻所述加速度計傳感器和所述陀螺儀傳感器採集的數據、以及所述編碼器數據，計算當前時刻相對於上一時刻所述機器人的位置和姿態變化估計；

獲取單元523，用於確定所述總體環境地圖中與所述位置和姿態變化估計對應的地圖點，獲得所述地圖點的地圖數據；

匹配單元524，用於通過將所述雷射雷達傳感器當前採集的數據與所述地圖點的地圖數據分別進行匹配，獲得所述機器人的當前位置，所述機器人的當前位置為匹配度最高的地圖點的位置。

以實際場景舉例來說：在初始化時，初始化單元521讀取磁強計傳感器採集的數據，獲得機器人絕對朝向的初始估計，使用這個初始估計進行方向初始化。在機器人移動過程中，估計單元522不斷讀取編碼器數據、以及所述加速度計傳感器和所述陀螺儀傳感器採集的數據，計算當前時刻相對於上一時刻機器人的位置和姿態變化估計。其中，對編碼器數據進行積分處理即可獲得位置和姿態變化估計，對加速度計傳感器採集的數據進行積分可以獲得位置變化估計，對陀螺儀傳感器採集的數據進行積分可以獲得姿態變化估計，之後通過對上述三者加權平均，即可得到當前時刻相對於上一時刻的位置和姿態變化估計。之後，獲取單元523根據位置和姿態變化估計確定相應的地圖點，匹配單元524用相應地圖點的地圖數據和雷射雷達傳感器當前採集的數據做匹配，匹配度最高的地圖點即為機器人的當前位置。

具體地，可以採用蒙特卡洛粒子濾波方法進行機器人自主定位。相應的，初始化單元521根據磁強計傳感器採集的數據，對粒子的姿態進行初始化，在機器人移動過程中，估計單元522實時根據當前時刻與上一時刻所述加速度計傳感器和所述陀螺儀傳感器採集的數據、以及所述編碼器數據，計算當前時刻相對於上一時刻的粒子位置和姿態變化估計，獲取單元523根據位置和姿態變化估計確定一些地圖點，匹配單元524將這些地圖點的地圖數據與雷射雷達傳感器當前採集的數據做匹配，匹配度最高的地圖點即為機器人的當前位置。

在上述匹配過程中，雷射雷達傳感器的精度高、噪聲低，且觀測距離遠，可以獲取遠處的線特徵，而匹配的主要依據便是線特徵。因此，只基於雷射雷達傳感器採集的數據進行匹配即可，可以有效節省匹配所需時間，提高機器人定位的效率，從而更快實現導航。

並且，結合使用編碼器數據、加速度計傳感器、陀螺儀傳感器計算當前位置和姿態變化估計，實現多源融合，可以提高機器人定位的準確度，從而更準確可靠地實現機器人導航。

其中，本實施例提供的機器人導航裝置可以執行實施例二的方法實施例的技術方案，其實現原理和技術效果類似，此處不再贅述。

本實施例提供的基於多傳感器數據融合的機器人導航裝置，在實現機器人自主定位的過程中，結合使用編碼器數據、加速度計傳感器、陀螺儀傳感器計算當前位置和姿態變化估計，僅基於雷射雷達傳感器採集的數據進行匹配，不但可以有效節省匹配所需時間，還能提高機器人定位的準確度，從而更快速準確可靠地實現機器人導航。

圖7A為本發明實施例七提供的一種基於多傳感器數據融合的機器人導航裝置的結構示意圖，如圖7A所示，在實施例五或實施例六的基礎上，控制模塊54包括：

檢測單元541，用於實時根據所述超聲波傳感器數據或所述紅外傳感器採集的數據，判斷所述機器人是否處於移動受阻狀態；

第一控制單元542，用於若檢測單元541檢測到所述機器人未處於移動受阻狀態，則根據所述深度攝像頭和所述雷射雷達傳感器當前採集的數據，構建局部環境地圖，並根據所述局部環境地圖、所述機器人的當前位置和規劃路線，通過局部路徑規划算法控制所述機器人避開障礙物移動。

以實際場景舉例：局部規劃策略總體分為兩個情形，一個情形是正常狀態下的規劃策略，另一個情形是特殊狀態處理。具體的，針對第一種情形，檢測單元541實時根據超聲波傳感器數據或紅外傳感器採集的數據，判斷所述機器人是否處於移動受阻狀態，若未處於移動受阻狀態，即機器人正常行走過程中，則第一控制單元542使用常規的規劃策略進行規劃，即根據深度攝像頭和所雷射雷達傳感器當前採集的數據構建的局部環境地圖，結合前面獲取的機器人的當前位置和規劃路線，通過局部路徑規划算法控制機器人避開障礙物移動。具體地，可以採用動態窗口模擬算法進行機器人移動控制。

針對另一種情形，即實時根據超聲波傳感器數據或紅外傳感器採集的數據，檢測單元541判斷所述機器人處於移動受阻狀態時，即機器人遇到特殊情況，例如：距離障礙物過近、由於定位誤差導致的與障礙物重合、被障礙物包圍等情況，則進入特殊狀態處理，具體的，可以根據超聲波傳感器數據或紅外傳感器當前採集的數據及預設的方向選擇策略，控制所述機器人避開障礙物移動。相應的，如圖7B所示，圖7B為本發明實施例七提供的另一種基於多傳感器數據融合的機器人導航裝置的結構示意圖，如圖7B所示，在圖7A所示實施方式的基礎上，控制模塊54還包括：

第二控制單元543，用於若檢測單元541檢測到所述機器人處於移動受阻狀態，則根據所述超聲波傳感器數據或所述紅外傳感器當前採集的數據及預設的方向選擇策略，控制所述機器人避開障礙物移動。

其中，超聲波傳感器和紅外傳感器用於特殊情況處理，例如，當檢測單元541通過超聲波傳感器或紅外傳感器觀測到距離障礙物過近、機器人被包圍或由於定位誤差機器人認為自身與障礙物重合等情況時，則第二控制單元543進行特殊狀態處理。此時不再依據局部環境地圖進行決策，而是直接選擇向遠離障礙物的方向進行逃離，具體的方向選擇策略可使用包括但不限於最近點反方向、人工勢場合成等方式進行選擇。

其中，本實施例提供的機器人導航裝置可以執行實施例三的方法實施例的技術方案，其實現原理和技術效果類似，此處不再贅述。

本實施例提供的基於多傳感器數據融合的機器人導航裝置，在對機器人的移動控制策略中，基於超聲波傳感器數據或紅外傳感器採集的數據判定當前狀態，若正常，則根據深度攝像頭和雷射雷達傳感器當前採集的數據構建局部環境地圖，並基於該局部環境地圖，結合機器人當前位置和規劃路線，控制機器人避開障礙物移動，否則，採用預設的方向選擇策略進行逃離，從而確保機器人導航的可靠性。

圖8為本發明實施例八提供的基於多傳感器數據融合的機器人導航裝置的結構示意圖，如圖8所示，在實施例七的基礎上，第一控制單元542包括：

投影子單元81，用於對所述深度攝像頭當前採集的數據進行去噪，並將去噪後的數據向二維平面進行投影，獲得二維投影數據；

處理子單元82，用於若二維投影數據相應位置不存在所述雷射雷達傳感器採集的數據，則將相應位置的局部環境地圖數據以對應的二維投影數據進行填充；

處理子單元82，還用於若所述二維投影數據相應位置存在所述雷射雷達傳感器採集的數據，則將所述二維投影數據和所述雷射雷達傳感器採集的數據進行加權平均，以獲得相應位置的局部環境地圖數據。

本實施例中，以採用動態窗口模擬算法控制機器人移動為例，動態窗口模擬算法是基於二維柵格地圖，因此需要投影子單元81先對三維點雲數據進行去噪，然後將三維點雲數據向二維平面進行投影獲得二維投影數據，此時將三維點雲數據分列投影，如果某一列有障礙物點，則此列的投影結果為障礙物，否則投影為空閒。處理子單元82將二維投影數據與雷射雷達傳感器採集的二維平面數據進行融合，具體策略為，使用二維投影數據對雷射雷達傳感器採集的二維數據進行更新，即對於雷射雷達傳感器未觀測到的位置直接使用獲得的二維投影數據進行填充，對於雷射雷達傳感器觀測到的位置，採用二維投影數據和雷射雷達傳感器採集到的數據進行加權平均。此方法適用於多個深度攝像頭和多個雷射雷達傳感器。

其中，本實施例提供的機器人導航裝置可以執行實施例四的方法實施例的技術方案，其實現原理和技術效果類似，此處不再贅述。

本實施例提供的基於多傳感器數據融合的機器人導航裝置，將深度攝像頭採集的三維點雲數據進行去噪投影，基於獲得的二維投影數據和雷射雷達傳感器採集到的二維平面數據進行融合，構建局部環境地圖，實現對機器人的移動控制，本實施例通過將二維平面數據，三維點雲數據進行融合處理，可以達到較好的局部規劃效果，提高導航準確性。

最後應說明的是：以上各實施例僅用以說明本發明的技術方案，而非對其限制；儘管參照前述各實施例對本發明進行了詳細的說明，本領域的普通技術人員應當理解：其依然可以對前述各實施例所記載的技術方案進行修改，或者對其中部分或者全部技術特徵進行等同替換；而這些修改或者替換，並不使相應技術方案的本質脫離本發明各實施例技術方案的範圍。