本發(fā)明實(shí)施例涉及人工智能技術(shù)領(lǐng)域，尤其涉及一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法及裝置。

背景技術(shù)：

機(jī)器人自主導(dǎo)航技術(shù)是智能機(jī)器人領(lǐng)域的熱門技術(shù)，通過自主導(dǎo)航技術(shù)，機(jī)器人可以智能的在環(huán)境中移動(dòng)，從而完成導(dǎo)引、搬運(yùn)、交互等任務(wù)。所以自主導(dǎo)航技術(shù)是機(jī)器人走向智能化的基礎(chǔ)，無法自主移動(dòng)的機(jī)器人不能稱之為智能機(jī)器人。

現(xiàn)有的機(jī)器人自主導(dǎo)航技術(shù)可大致分為兩大類：一類是有源的自主導(dǎo)航技術(shù)，一類是無源的自主導(dǎo)航技術(shù)。有源的自主導(dǎo)航技術(shù)即機(jī)器人需要依賴外部設(shè)備實(shí)現(xiàn)的自主導(dǎo)航，例如需要在環(huán)境中部署基站、使用GPS設(shè)備等，因此靈活性差。此外由于GPS本身的限制，其無法應(yīng)用于室內(nèi)環(huán)境下的定位，并且GPS的精度往往也無法滿足機(jī)器人自主行走的要求。無源的自主導(dǎo)航技術(shù)指機(jī)器人無需依賴外部設(shè)備，只使用自身傳感器實(shí)現(xiàn)自主導(dǎo)航。這一類技術(shù)靈活性好，可應(yīng)用在室內(nèi)或室外環(huán)境下，無需專業(yè)人員、專業(yè)設(shè)備部署。

但是，現(xiàn)有的無源自主導(dǎo)航技術(shù)通常使用單一傳感器，無法提供較為全面的數(shù)據(jù)，即便使用多種傳感器，也通常是將各傳感器采集的數(shù)據(jù)不合理地融合，導(dǎo)致自主導(dǎo)航效果較差。

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

本發(fā)明實(shí)施例提供一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法及裝置，用于解決現(xiàn)有的機(jī)器人自主導(dǎo)航效果較差的技術(shù)問題。

本發(fā)明實(shí)施例提供一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法，包括：根據(jù)激光雷達(dá)傳感器采集到的數(shù)據(jù)和編碼器數(shù)據(jù)，利用即時(shí)定位與地圖構(gòu)建技術(shù)，構(gòu)建總體環(huán)境地圖；實(shí)時(shí)根據(jù)所述激光雷達(dá)傳感器、加速度計(jì)傳感器、陀螺儀傳感器和磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)、所述總體環(huán)境地圖、以及編碼器數(shù)據(jù)，通過視覺定位算法獲得所述機(jī)器人在所述總體環(huán)境地圖中的當(dāng)前位置；實(shí)時(shí)根據(jù)所述總體環(huán)境地圖和所述機(jī)器人的當(dāng)前位置，通過路徑規(guī)劃算法獲得所述機(jī)器人從所述當(dāng)前位置到目標(biāo)位置的規(guī)劃路線；根據(jù)所述當(dāng)前位置和所述規(guī)劃路線，利用所述激光雷達(dá)傳感器、深度攝像頭、超聲波傳感器和紅外傳感器采集的數(shù)據(jù)、以及編碼器數(shù)據(jù)，通過局部路徑規(guī)劃算法控制所述機(jī)器人避開障礙物移動(dòng)。

本發(fā)明實(shí)施例提供一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置，包括：地圖構(gòu)建模塊，用于根據(jù)激光雷達(dá)傳感器采集到的數(shù)據(jù)和編碼器數(shù)據(jù)，利用即時(shí)定位與地圖構(gòu)建技術(shù)，構(gòu)建總體環(huán)境地圖；實(shí)時(shí)定位模塊，用于實(shí)時(shí)根據(jù)所述激光雷達(dá)傳感器、加速度計(jì)傳感器、陀螺儀傳感器和磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)、所述總體環(huán)境地圖、以及編碼器數(shù)據(jù)，通過視覺定位算法獲得所述機(jī)器人在所述總體環(huán)境地圖中的當(dāng)前位置；路線規(guī)劃模塊，用于實(shí)時(shí)根據(jù)所述總體環(huán)境地圖和所述機(jī)器人的當(dāng)前位置，通過路徑規(guī)劃算法獲得所述機(jī)器人從所述當(dāng)前位置到目標(biāo)位置的規(guī)劃路線；控制模塊，用于根據(jù)所述當(dāng)前位置和所述規(guī)劃路線，利用所述激光雷達(dá)傳感器、深度攝像頭、超聲波傳感器和紅外傳感器采集的數(shù)據(jù)、以及編碼器數(shù)據(jù)，通過局部路徑規(guī)劃算法控制所述機(jī)器人避開障礙物移動(dòng)。

本發(fā)明提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法及裝置，利用激光雷達(dá)傳感器采集到的數(shù)據(jù)和編碼器數(shù)據(jù)，構(gòu)建總體環(huán)境地圖，并根據(jù)編碼器數(shù)據(jù)、激光雷達(dá)傳感器、加速度計(jì)傳感器、陀螺儀傳感器和磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)對機(jī)器人進(jìn)行實(shí)時(shí)定位，根據(jù)機(jī)器人的當(dāng)前位置規(guī)劃路線，并結(jié)合激光雷達(dá)傳感器、深度攝像頭、超聲波傳感器和紅外傳感器采集的數(shù)據(jù)，控制機(jī)器人避開障礙物移動(dòng)，從而實(shí)現(xiàn)機(jī)器人自主導(dǎo)航，本方案基于不同傳感器的特點(diǎn)，利用不同的傳感器數(shù)據(jù)進(jìn)行融合并用于相應(yīng)的處理，在合理利用傳感器實(shí)現(xiàn)機(jī)器人導(dǎo)航的基礎(chǔ)上，靈活應(yīng)用于多種場景，并且兼顧成本，能夠?qū)崿F(xiàn)較好的自主導(dǎo)航效果。

附圖說明

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

圖1為本發(fā)明實(shí)施例一提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法的流程示意圖；

圖2為本發(fā)明實(shí)施例二提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法的流程示意圖；

圖3A為本發(fā)明實(shí)施例三提供的一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法的流程示意圖；

圖3B為本發(fā)明實(shí)施例三提供的另一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法的流程示意圖；

圖4為本發(fā)明實(shí)施例四提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法的流程示意圖；

圖5為本發(fā)明實(shí)施例五提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置的結(jié)構(gòu)示意圖；

圖6為本發(fā)明實(shí)施例六提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置的結(jié)構(gòu)示意圖；

圖7A為本發(fā)明實(shí)施例七提供的一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置的結(jié)構(gòu)示意圖；

圖7B為本發(fā)明實(shí)施例七提供的另一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置的結(jié)構(gòu)示意圖；

圖8為本發(fā)明實(shí)施例八提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置的結(jié)構(gòu)示意圖。

具體實(shí)施方式

為使本發(fā)明實(shí)施例的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

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

激光雷達(dá)：提供機(jī)器人距離周圍環(huán)境障礙物的距離信息，通常為周圍環(huán)境的一個(gè)二維切面?？捎糜诘貓D構(gòu)建、定位、實(shí)時(shí)避障等。精度較高，穩(wěn)定性高，噪聲低，成本高。可視距離遠(yuǎn)，近距離處盲區(qū)小。

深度攝像頭：提供機(jī)器人距離周圍環(huán)境障礙物的距離信息，通常為周圍環(huán)境的三維點(diǎn)云。可用于地圖構(gòu)建、定位、實(shí)時(shí)避障等。精度較低，穩(wěn)定性低，噪聲高，成本相對較低?？梢暰嚯x近，近距離盲區(qū)大。

超聲波傳感器：提供機(jī)器人距離周圍環(huán)境障礙物的距離信息，通常為一維單點(diǎn)?？捎糜趯?shí)時(shí)避障。數(shù)據(jù)精度低，穩(wěn)定性低，噪聲高，成本很低?？梢暰嚯x較遠(yuǎn)，近距離處盲區(qū)大。

紅外傳感器：提供機(jī)器人距離周圍環(huán)境障礙物的距離信息，通常為一維單點(diǎn)?？捎糜趯?shí)時(shí)避障。數(shù)據(jù)精度相對高，穩(wěn)定性相對高，噪聲相對高，成本很低?？梢暰嚯x近，近距離處無盲區(qū)。

加速度計(jì)：可提供機(jī)器人的瞬時(shí)線加速度值。可用于定位、實(shí)時(shí)避障。

陀螺儀：可提供機(jī)器人的瞬時(shí)角速度。可用于定位、實(shí)時(shí)避障。

磁強(qiáng)計(jì)：可提供機(jī)器人的絕對朝向估計(jì)?？捎糜诙ㄎ?、實(shí)時(shí)避障。

編碼器：可提供機(jī)器人行走的里程、速度等估計(jì)。用于定位、實(shí)時(shí)避障。

圖1為本發(fā)明實(shí)施例一提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法的流程示意圖，如圖1所示，本實(shí)施例以該方法應(yīng)用于機(jī)器人導(dǎo)航裝置中來舉例說明，該機(jī)器人導(dǎo)航裝置可集成在機(jī)器人自主導(dǎo)航系統(tǒng)中，該方法包括：

101、根據(jù)激光雷達(dá)傳感器采集到的數(shù)據(jù)和編碼器數(shù)據(jù)，利用即時(shí)定位與地圖構(gòu)建技術(shù)，構(gòu)建總體環(huán)境地圖。

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

以實(shí)際場景舉例來說：當(dāng)將機(jī)器人置于一個(gè)新環(huán)境時(shí)，需要使用即時(shí)定位與地圖構(gòu)建方法，繪制當(dāng)前環(huán)境的地圖。具體的，控制機(jī)器人在該環(huán)境中移動(dòng)，激光雷達(dá)傳感器不斷收集數(shù)據(jù)，并利用SLAM算法，實(shí)時(shí)計(jì)算并繪制出相應(yīng)的總體環(huán)境地圖，該總體環(huán)境地圖為二維柵格地圖，該二維柵格地圖后續(xù)主要用于全局的路徑規(guī)劃及機(jī)器人自主定位。其中，激光雷達(dá)傳感器采集的數(shù)據(jù)用于SLAM算法的數(shù)據(jù)匹配，編碼器數(shù)據(jù)用于為SLAM算法的每次迭代提供初始估計(jì)，從而加速地圖構(gòu)建過程。

上述建立的總體環(huán)境地圖可以被重復(fù)使用，因此地圖構(gòu)建過程往往只需要進(jìn)行一次。只有當(dāng)環(huán)境有很大程度的改變，或機(jī)器人被置于新環(huán)境時(shí)，才需要重新構(gòu)建總體環(huán)境地圖。

由于全局的路徑規(guī)劃只是作為機(jī)器人實(shí)際行走過程的大方向參考，因此只需確定大致的路徑即可。相應(yīng)的，進(jìn)行全局路徑規(guī)劃只要求總體環(huán)境地圖包括環(huán)境的主要信息即可，以室內(nèi)環(huán)境為例，可以包括：墻、室內(nèi)家具、室外電線桿等，而激光雷達(dá)傳感器提供的環(huán)境二維切面數(shù)據(jù)足夠提供以上主要信息。另外，機(jī)器人進(jìn)行自主定位時(shí)，需要實(shí)時(shí)的傳感器數(shù)據(jù)與地圖做匹配，而該匹配過程主要基于大的線特征，例如：墻、大型障礙物等，而激光雷達(dá)的二維切面信息也足以提供這些信息。盡管自主定位對于精度的要求較高，激光雷達(dá)傳感器采集的數(shù)據(jù)往往精度高，噪聲小，因此基于激光雷達(dá)數(shù)據(jù)構(gòu)建的地圖可以優(yōu)化自主定位的效果?？梢?，采用激光雷達(dá)傳感器采集的數(shù)據(jù)構(gòu)建總體環(huán)境地圖，能夠在構(gòu)建環(huán)境地圖和實(shí)現(xiàn)自主定位的基礎(chǔ)上，節(jié)省地圖構(gòu)建所需的時(shí)間，減小數(shù)據(jù)處理量，提高導(dǎo)航效率。實(shí)際應(yīng)用中，可以將激光雷達(dá)傳感器安裝在機(jī)器人的底盤上。

102、實(shí)時(shí)根據(jù)所述激光雷達(dá)傳感器、加速度計(jì)傳感器、陀螺儀傳感器和磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)、所述總體環(huán)境地圖、以及編碼器數(shù)據(jù)，通過視覺定位算法獲得所述機(jī)器人在所述總體環(huán)境地圖中的當(dāng)前位置。

以實(shí)際場景舉例來說：基于前述步驟構(gòu)建的總體環(huán)境地圖，利用激光雷達(dá)傳感器、加速度計(jì)傳感器、陀螺儀傳感器和磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)進(jìn)行實(shí)時(shí)定位，獲得機(jī)器人的當(dāng)前位置，該當(dāng)前位置為機(jī)器人當(dāng)前在總體環(huán)境地圖下的位置信息。

103、實(shí)時(shí)根據(jù)所述總體環(huán)境地圖和所述機(jī)器人的當(dāng)前位置，通過路徑規(guī)劃算法獲得所述機(jī)器人從所述當(dāng)前位置到目標(biāo)位置的規(guī)劃路線。

其中，路徑規(guī)劃算法包括但不限于：A star算法、迪杰斯特拉算法等。

以實(shí)際場景舉例來說：對機(jī)器人進(jìn)行定位，即獲取機(jī)器人的當(dāng)前位置后，即可根據(jù)機(jī)器人的當(dāng)前位置和前述步驟構(gòu)建的總體環(huán)境地圖，進(jìn)行從當(dāng)前位置至目標(biāo)位置的路線規(guī)劃，獲得規(guī)劃路線。

104、根據(jù)所述當(dāng)前位置和所述規(guī)劃路線，利用所述激光雷達(dá)傳感器、深度攝像頭、超聲波傳感器和紅外傳感器采集的數(shù)據(jù)、以及編碼器數(shù)據(jù)，通過局部路徑規(guī)劃算法控制所述機(jī)器人避開障礙物移動(dòng)。

其中，局部規(guī)劃算法包括但不限于：動(dòng)態(tài)窗口模擬、D Star算法等。

以實(shí)際場景舉例來說：基于實(shí)時(shí)得到的規(guī)劃路線和機(jī)器人的當(dāng)前位置，通過激光雷達(dá)傳感器、深度攝像頭、超聲波傳感器和紅外傳感器采集的數(shù)據(jù)，通過局部路徑規(guī)劃算法進(jìn)行決策，可控制機(jī)器人移動(dòng)行走，并在移動(dòng)行走過程中實(shí)時(shí)避開周圍的靜態(tài)或動(dòng)態(tài)障礙物。

具體的，由于局部路徑規(guī)劃要保證機(jī)器人行走的安全及流暢，因此需要使用激光雷達(dá)、深度攝像頭、超聲波傳感器、紅外傳感器采集的數(shù)據(jù)和編碼器數(shù)據(jù)進(jìn)行融合。

實(shí)際應(yīng)用中，由于激光雷達(dá)傳感器無法檢測到高于或低于自身激光雷達(dá)切面的障礙物，因此可以在激光雷達(dá)傳感器更低處安裝紅外傳感器和超聲波傳感器，以檢測近地面障礙物。由于超聲波有近距離盲區(qū)但可視距離遠(yuǎn)，而紅外可視距離近但無近距離盲區(qū)，因此將兩者配合使用，達(dá)到可視距離遠(yuǎn)無盲區(qū)的效果。此外，使用深度攝像頭獲取近處環(huán)境的三維點(diǎn)云數(shù)據(jù)，可以直接得到機(jī)器人前方遮擋的所有物體信息。

本實(shí)施例提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法，利用激光雷達(dá)傳感器采集到的數(shù)據(jù)和編碼器數(shù)據(jù)，構(gòu)建總體環(huán)境地圖，并根據(jù)編碼器數(shù)據(jù)、激光雷達(dá)傳感器、加速度計(jì)傳感器、陀螺儀傳感器和磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)對機(jī)器人進(jìn)行實(shí)時(shí)定位，根據(jù)機(jī)器人的當(dāng)前位置規(guī)劃路線，并結(jié)合激光雷達(dá)傳感器、深度攝像頭、超聲波傳感器和紅外傳感器采集的數(shù)據(jù)，控制機(jī)器人避開障礙物移動(dòng)，從而實(shí)現(xiàn)機(jī)器人自主導(dǎo)航，本方案基于不同傳感器的特點(diǎn)，利用不同的傳感器數(shù)據(jù)進(jìn)行融合并用于相應(yīng)的處理，在合理利用傳感器實(shí)現(xiàn)機(jī)器人導(dǎo)航的基礎(chǔ)上，靈活應(yīng)用于多種場景，并且兼顧成本，能夠?qū)崿F(xiàn)較好的自主導(dǎo)航效果。

圖2為本發(fā)明實(shí)施例二提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法的流程示意圖，如圖2所示，本實(shí)施例仍以該方法應(yīng)用于機(jī)器人導(dǎo)航裝置中來舉例說明，在實(shí)施例一的基礎(chǔ)上，102包括：

201、根據(jù)磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)，對所述機(jī)器人的姿態(tài)進(jìn)行初始化；

202、實(shí)時(shí)根據(jù)當(dāng)前時(shí)刻與上一時(shí)刻所述加速度計(jì)傳感器和所述陀螺儀傳感器采集的數(shù)據(jù)、以及所述編碼器數(shù)據(jù)，計(jì)算當(dāng)前時(shí)刻相對于上一時(shí)刻所述機(jī)器人的位置和姿態(tài)變化估計(jì)；

203、確定所述總體環(huán)境地圖中與所述位置和姿態(tài)變化估計(jì)對應(yīng)的地圖點(diǎn)，獲得所述地圖點(diǎn)的地圖數(shù)據(jù)；

204、通過將所述激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)與所述地圖點(diǎn)的地圖數(shù)據(jù)分別進(jìn)行匹配，獲得所述機(jī)器人的當(dāng)前位置，所述機(jī)器人的當(dāng)前位置為匹配度最高的地圖點(diǎn)的位置。

以實(shí)際場景舉例來說：在初始化時(shí)，首先讀取磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)，獲得機(jī)器人絕對朝向的初始估計(jì)，使用這個(gè)初始估計(jì)進(jìn)行方向初始化。在機(jī)器人移動(dòng)過程中，不斷讀取編碼器數(shù)據(jù)、以及所述加速度計(jì)傳感器和所述陀螺儀傳感器采集的數(shù)據(jù)，計(jì)算當(dāng)前時(shí)刻相對于上一時(shí)刻的位置和姿態(tài)變化估計(jì)。其中，對編碼器數(shù)據(jù)進(jìn)行積分處理即可獲得位置和姿態(tài)變化估計(jì)，對加速度計(jì)傳感器采集的數(shù)據(jù)進(jìn)行積分可以獲得位置變化估計(jì)，對陀螺儀傳感器采集的數(shù)據(jù)進(jìn)行積分可以獲得姿態(tài)變化估計(jì)，之后通過對上述三者加權(quán)平均，即可得到當(dāng)前時(shí)刻相對于上一時(shí)刻的位置和姿態(tài)變化估計(jì)。之后，根據(jù)位置和姿態(tài)變化估計(jì)，用相應(yīng)地圖點(diǎn)的地圖數(shù)據(jù)和激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)做匹配，匹配度最高的地圖點(diǎn)即為機(jī)器人的當(dāng)前位置。

具體地，可以采用蒙特卡洛粒子濾波方法進(jìn)行機(jī)器人自主定位。相應(yīng)的，根據(jù)磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)，對粒子的姿態(tài)進(jìn)行初始化，在機(jī)器人移動(dòng)過程中，實(shí)時(shí)根據(jù)當(dāng)前時(shí)刻與上一時(shí)刻所述加速度計(jì)傳感器和所述陀螺儀傳感器采集的數(shù)據(jù)、以及所述編碼器數(shù)據(jù)，計(jì)算當(dāng)前時(shí)刻相對于上一時(shí)刻的粒子位置和姿態(tài)變化估計(jì)，根據(jù)位置和姿態(tài)變化估計(jì)確定一些地圖點(diǎn)，將這些地圖點(diǎn)的地圖數(shù)據(jù)與激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)做匹配，匹配度最高的地圖點(diǎn)即為機(jī)器人的當(dāng)前位置。

在上述匹配過程中，激光雷達(dá)傳感器的精度高、噪聲低，且觀測距離遠(yuǎn)，可以獲取遠(yuǎn)處的線特征，而匹配的主要依據(jù)便是線特征。因此，只基于激光雷達(dá)傳感器采集的數(shù)據(jù)進(jìn)行匹配即可，可以有效節(jié)省匹配所需時(shí)間，提高機(jī)器人定位的效率，從而更快實(shí)現(xiàn)導(dǎo)航。

并且，結(jié)合使用編碼器數(shù)據(jù)、加速度計(jì)傳感器、陀螺儀傳感器計(jì)算當(dāng)前粒子的位置和姿態(tài)變化估計(jì)，實(shí)現(xiàn)多源融合，可以提高機(jī)器人定位的準(zhǔn)確度，從而更準(zhǔn)確可靠地實(shí)現(xiàn)機(jī)器人導(dǎo)航。

本實(shí)施例提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法，在實(shí)現(xiàn)機(jī)器人自主定位的過程中，結(jié)合使用編碼器數(shù)據(jù)、加速度計(jì)傳感器、陀螺儀傳感器計(jì)算當(dāng)前的位置和姿態(tài)變化估計(jì)，僅基于激光雷達(dá)傳感器采集的數(shù)據(jù)進(jìn)行匹配，不但可以有效節(jié)省匹配所需時(shí)間，還能提高機(jī)器人定位的準(zhǔn)確度，從而更快速準(zhǔn)確可靠地實(shí)現(xiàn)機(jī)器人導(dǎo)航。

圖3A為本發(fā)明實(shí)施例三提供的一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法的流程示意圖，如圖3A所示，本實(shí)施例仍以該方法應(yīng)用于機(jī)器人導(dǎo)航裝置中來舉例說明，在實(shí)施例一或?qū)嵤├幕A(chǔ)上，104包括：

301、實(shí)時(shí)根據(jù)所述超聲波傳感器數(shù)據(jù)或所述紅外傳感器采集的數(shù)據(jù)，判斷所述機(jī)器人是否處于移動(dòng)受阻狀態(tài)；

302、若所述機(jī)器人未處于移動(dòng)受阻狀態(tài)，則根據(jù)所述深度攝像頭和所述激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)，構(gòu)建局部環(huán)境地圖，并根據(jù)所述局部環(huán)境地圖、所述機(jī)器人的當(dāng)前位置和規(guī)劃路線，通過局部路徑規(guī)劃算法控制所述機(jī)器人避開障礙物移動(dòng)。

以實(shí)際場景舉例來說：局部規(guī)劃策略總體分為兩個(gè)情形，一個(gè)情形是正常狀態(tài)下的規(guī)劃策略，另一個(gè)情形是特殊狀態(tài)處理。具體的，針對第一種情形，實(shí)時(shí)根據(jù)超聲波傳感器數(shù)據(jù)或紅外傳感器采集的數(shù)據(jù)，判斷所述機(jī)器人是否處于移動(dòng)受阻狀態(tài)，若未處于移動(dòng)受阻狀態(tài)，即機(jī)器人正常行走過程中，使用常規(guī)的規(guī)劃策略進(jìn)行規(guī)劃，即根據(jù)深度攝像頭和所激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)構(gòu)建的局部環(huán)境地圖，結(jié)合前面獲取的機(jī)器人的當(dāng)前位置和規(guī)劃路線，通過局部路徑規(guī)劃算法控制機(jī)器人避開障礙物移動(dòng)。具體地，可以采用動(dòng)態(tài)窗口模擬算法進(jìn)行機(jī)器人移動(dòng)控制。

本實(shí)施例中，動(dòng)態(tài)窗口模擬算法會(huì)依據(jù)各傳感器采集數(shù)據(jù)，實(shí)時(shí)構(gòu)建機(jī)器人周圍小范圍內(nèi)的局部環(huán)境地圖，再基于該局部環(huán)境地圖模擬計(jì)算，進(jìn)行決策。具體的，局部環(huán)境地圖的構(gòu)建采用二維面數(shù)據(jù)和三維點(diǎn)云數(shù)據(jù)，即激光雷達(dá)傳感器和深度攝像頭采集的數(shù)據(jù)。采用這兩種傳感器的原因是局部環(huán)境地圖需要實(shí)時(shí)更新，如果將類似超聲波傳感器和紅外傳感器采集的一維點(diǎn)數(shù)據(jù)構(gòu)建到局部環(huán)境地圖，可能會(huì)導(dǎo)致無法清理掉障礙物的問題，即當(dāng)某一時(shí)刻下，超聲波傳感器或紅外傳感器觀測到有一個(gè)障礙物，而下一時(shí)刻觀測的并不是這個(gè)障礙物點(diǎn)所在的位置，也就無法確定上一時(shí)刻的障礙物是否還依然存在，逐漸地局部環(huán)境地圖會(huì)逐漸累積障礙物，導(dǎo)致決策下的機(jī)器人無路可走，最終導(dǎo)致導(dǎo)航失敗。而二維面數(shù)據(jù)和三維點(diǎn)云數(shù)據(jù)就不存在這個(gè)問題，因?yàn)榧す饫走_(dá)傳感器和深度攝像頭觀測的不是一個(gè)點(diǎn)，而是一個(gè)范圍，因此可以在大部分范圍內(nèi)保證地圖的實(shí)時(shí)性，提高導(dǎo)航的可靠性。

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

303、若所述機(jī)器人處于移動(dòng)受阻狀態(tài)，則根據(jù)所述超聲波傳感器數(shù)據(jù)或所述紅外傳感器當(dāng)前采集的數(shù)據(jù)及預(yù)設(shè)的方向選擇策略，控制所述機(jī)器人避開障礙物移動(dòng)。

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

本實(shí)施例提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法，在對機(jī)器人的移動(dòng)控制策略中，基于超聲波傳感器數(shù)據(jù)或紅外傳感器采集的數(shù)據(jù)判定當(dāng)前狀態(tài)，若正常，則根據(jù)深度攝像頭和激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)構(gòu)建局部環(huán)境地圖，并基于該局部環(huán)境地圖，結(jié)合機(jī)器人當(dāng)前位置和規(guī)劃路線，控制機(jī)器人避開障礙物移動(dòng)，否則，采用預(yù)設(shè)的方向選擇策略進(jìn)行逃離，從而確保機(jī)器人導(dǎo)航的可靠性。

進(jìn)一步的，圖4為本發(fā)明實(shí)施例四提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法的流程示意圖，如圖4所示，本實(shí)施例仍以該方法應(yīng)用于機(jī)器人導(dǎo)航裝置中來舉例說明，在實(shí)施例三的基礎(chǔ)上，302中所述根據(jù)所述深度攝像頭和所述激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)，構(gòu)建局部環(huán)境地圖，包括：

401、對所述深度攝像頭當(dāng)前采集的數(shù)據(jù)進(jìn)行去噪，并將去噪后的數(shù)據(jù)向二維平面進(jìn)行投影，獲得二維投影數(shù)據(jù)；

402、若二維投影數(shù)據(jù)相應(yīng)位置不存在所述激光雷達(dá)傳感器采集的數(shù)據(jù)，則將相應(yīng)位置的局部環(huán)境地圖數(shù)據(jù)以對應(yīng)的二維投影數(shù)據(jù)進(jìn)行填充；

403、若所述二維投影數(shù)據(jù)相應(yīng)位置存在所述激光雷達(dá)傳感器采集的數(shù)據(jù)，則將所述二維投影數(shù)據(jù)和所述激光雷達(dá)傳感器采集的數(shù)據(jù)進(jìn)行加權(quán)平均，以獲得相應(yīng)位置的局部環(huán)境地圖數(shù)據(jù)。

以實(shí)際場景舉例來說：局部規(guī)劃策略總體分為兩個(gè)情形，一個(gè)情形是正常狀態(tài)下的規(guī)劃策略，另一個(gè)情形是特殊狀態(tài)處理。具體的，針對第一種情形，實(shí)時(shí)根據(jù)超聲波傳感器數(shù)據(jù)或紅外傳感器采集的數(shù)據(jù)，判斷所述機(jī)器人是否處于移動(dòng)受阻狀態(tài)，若未處于移動(dòng)受阻狀態(tài)，即機(jī)器人正常行走過程中，使用常規(guī)的規(guī)劃策略進(jìn)行規(guī)劃，即根據(jù)深度攝像頭和所激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)構(gòu)建的局部環(huán)境地圖，結(jié)合前面獲取的機(jī)器人的當(dāng)前位置和規(guī)劃路線，通過局部路徑規(guī)劃算法控制機(jī)器人避開障礙物移動(dòng)。

本實(shí)施例中，以采用動(dòng)態(tài)窗口模擬算法控制機(jī)器人移動(dòng)為例，動(dòng)態(tài)窗口模擬算法是基于二維柵格地圖，因此需要先對三維點(diǎn)云數(shù)據(jù)進(jìn)行去噪，然后將三維點(diǎn)云數(shù)據(jù)向二維平面進(jìn)行投影獲得二維投影數(shù)據(jù)，此時(shí)將三維點(diǎn)云數(shù)據(jù)分列投影，如果某一列有障礙物點(diǎn)，則此列的投影結(jié)果為障礙物，否則投影為空閑。將二維投影數(shù)據(jù)與激光雷達(dá)傳感器采集的二維平面數(shù)據(jù)進(jìn)行融合，具體策略為，使用二維投影數(shù)據(jù)對激光雷達(dá)傳感器采集的二維數(shù)據(jù)進(jìn)行更新，即對于激光雷達(dá)傳感器未觀測到的位置直接使用獲得的二維投影數(shù)據(jù)進(jìn)行填充，對于激光雷達(dá)傳感器觀測到的位置，采用二維投影數(shù)據(jù)和激光雷達(dá)傳感器采集到的數(shù)據(jù)進(jìn)行加權(quán)平均。此方法適用于多個(gè)深度攝像頭和多個(gè)激光雷達(dá)傳感器。

本實(shí)施例提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航方法，將深度攝像頭采集的三維點(diǎn)云數(shù)據(jù)進(jìn)行去噪投影，基于獲得的二維投影數(shù)據(jù)和激光雷達(dá)傳感器采集到的二維平面數(shù)據(jù)進(jìn)行融合，構(gòu)建局部環(huán)境地圖，實(shí)現(xiàn)對機(jī)器人的移動(dòng)控制，本實(shí)施例通過將二維平面數(shù)據(jù)，三維點(diǎn)云數(shù)據(jù)進(jìn)行融合處理，可以達(dá)到較好的局部規(guī)劃效果，提高導(dǎo)航準(zhǔn)確性。

本領(lǐng)域普通技術(shù)人員可以理解：實(shí)現(xiàn)上述各方法實(shí)施例的全部或部分步驟可以通過程序指令相關(guān)的硬件來完成。前述的程序可以存儲(chǔ)于一可讀取存儲(chǔ)介質(zhì)中。該程序在執(zhí)行時(shí)，執(zhí)行包括上述各方法實(shí)施例的步驟；而前述的存儲(chǔ)介質(zhì)包括：ROM、RAM、磁碟或者光盤等各種可以存儲(chǔ)程序代碼的介質(zhì)。

圖5為本發(fā)明實(shí)施例五提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置的結(jié)構(gòu)示意圖，如圖5所示，該機(jī)器人導(dǎo)航裝置可集成在機(jī)器人自主導(dǎo)航系統(tǒng)中，該裝置包括：

地圖構(gòu)建模塊51，用于根據(jù)激光雷達(dá)傳感器采集到的數(shù)據(jù)和編碼器數(shù)據(jù)，利用即時(shí)定位與地圖構(gòu)建技術(shù)，構(gòu)建總體環(huán)境地圖；

實(shí)時(shí)定位模塊52，用于實(shí)時(shí)根據(jù)所述激光雷達(dá)傳感器、加速度計(jì)傳感器、陀螺儀傳感器和磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)、所述總體環(huán)境地圖、以及編碼器數(shù)據(jù)，通過視覺定位算法獲得所述機(jī)器人在所述總體環(huán)境地圖中的當(dāng)前位置；

路線規(guī)劃模塊53，用于實(shí)時(shí)根據(jù)所述總體環(huán)境地圖和所述機(jī)器人的當(dāng)前位置，通過路徑規(guī)劃算法獲得所述機(jī)器人從所述當(dāng)前位置到目標(biāo)位置的規(guī)劃路線；

控制模塊54，用于根據(jù)所述當(dāng)前位置和所述規(guī)劃路線，利用所述激光雷達(dá)傳感器、深度攝像頭、超聲波傳感器和紅外傳感器采集的數(shù)據(jù)、以及編碼器數(shù)據(jù)，通過局部路徑規(guī)劃算法控制所述機(jī)器人避開障礙物移動(dòng)。

具體地，地圖構(gòu)建模塊51采用的即時(shí)定位與地圖構(gòu)建方法包括但不限于：掃描匹配、圖優(yōu)化等方法。以實(shí)際場景舉例來說：當(dāng)將機(jī)器人置于一個(gè)新環(huán)境時(shí)，需要使用即時(shí)定位與地圖構(gòu)建方法，繪制當(dāng)前環(huán)境的地圖。具體的，控制機(jī)器人在該環(huán)境中移動(dòng)，激光雷達(dá)傳感器不斷收集數(shù)據(jù)，地圖構(gòu)建模塊51利用SLAM算法，實(shí)時(shí)計(jì)算并繪制出相應(yīng)的總體環(huán)境地圖?；诘貓D構(gòu)建模塊51構(gòu)建的總體環(huán)境地圖，實(shí)時(shí)定位模塊52利用激光雷達(dá)傳感器、加速度計(jì)傳感器、陀螺儀傳感器和磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)進(jìn)行實(shí)時(shí)定位，獲得機(jī)器人的當(dāng)前位置。實(shí)時(shí)定位模塊52獲取機(jī)器人的當(dāng)前位置后，路線規(guī)劃模塊53根據(jù)實(shí)時(shí)定位模塊52獲得的機(jī)器人的當(dāng)前位置和地圖構(gòu)建模塊51構(gòu)建的總體環(huán)境地圖，進(jìn)行從當(dāng)前位置至目標(biāo)位置的路線規(guī)劃，獲得規(guī)劃路線。基于實(shí)時(shí)得到的規(guī)劃路線和機(jī)器人的當(dāng)前位置，控制模塊54通過激光雷達(dá)傳感器、深度攝像頭、超聲波傳感器和紅外傳感器采集的數(shù)據(jù)，通過局部路徑規(guī)劃算法進(jìn)行決策，可控制機(jī)器人移動(dòng)行走，并在移動(dòng)行走過程中實(shí)時(shí)避開周圍的靜態(tài)或動(dòng)態(tài)障礙物。

其中，所述總體環(huán)境地圖為二維柵格地圖。激光雷達(dá)傳感器采集的數(shù)據(jù)用于SLAM算法的數(shù)據(jù)匹配，編碼器數(shù)據(jù)用于為SLAM算法的每次迭代提供初始估計(jì)，從而加速地圖構(gòu)建過程。并且激光雷達(dá)傳感器采集的數(shù)據(jù)往往精度高，噪聲小，因此基于激光雷達(dá)數(shù)據(jù)構(gòu)建的地圖可以優(yōu)化自主定位的效果。采用激光雷達(dá)傳感器采集的數(shù)據(jù)構(gòu)建總體環(huán)境地圖，能夠在構(gòu)建環(huán)境地圖和實(shí)現(xiàn)自主定位的基礎(chǔ)上，節(jié)省地圖構(gòu)建所需的時(shí)間，減小數(shù)據(jù)處理量，提高導(dǎo)航效率。實(shí)際應(yīng)用中，可以將激光雷達(dá)傳感器安裝在機(jī)器人的底盤上。

上述建立的總體環(huán)境地圖可以被重復(fù)使用，因此地圖構(gòu)建模塊51往往只需要進(jìn)行一次地圖構(gòu)建過程。只有當(dāng)環(huán)境有很大程度的改變，或機(jī)器人被置于新環(huán)境時(shí)，地圖構(gòu)建模塊51才需要重新構(gòu)建總體環(huán)境地圖。

其中，路徑規(guī)劃算法包括但不限于：A star算法、迪杰斯特拉算法等，局部規(guī)劃算法包括但不限于：動(dòng)態(tài)窗口模擬、D Star算法等。

具體的，由于局部路徑規(guī)劃要保證機(jī)器人行走的安全及流暢，因此需要使用激光雷達(dá)、深度攝像頭、超聲波傳感器、紅外傳感器采集的數(shù)據(jù)和編碼器數(shù)據(jù)進(jìn)行融合。

其中，本實(shí)施例提供的機(jī)器人導(dǎo)航裝置可以執(zhí)行實(shí)施例一的方法實(shí)施例的技術(shù)方案，其實(shí)現(xiàn)原理和技術(shù)效果類似，此處不再贅述。

本實(shí)施例提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置，利用激光雷達(dá)傳感器采集到的數(shù)據(jù)和編碼器數(shù)據(jù)，構(gòu)建總體環(huán)境地圖，并根據(jù)編碼器數(shù)據(jù)、激光雷達(dá)傳感器、加速度計(jì)傳感器、陀螺儀傳感器和磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)對機(jī)器人進(jìn)行實(shí)時(shí)定位，根據(jù)機(jī)器人的當(dāng)前位置規(guī)劃路線，并結(jié)合激光雷達(dá)傳感器、深度攝像頭、超聲波傳感器和紅外傳感器采集的數(shù)據(jù)，控制機(jī)器人避開障礙物移動(dòng)，從而實(shí)現(xiàn)機(jī)器人自主導(dǎo)航，本方案基于不同傳感器的特點(diǎn)，利用不同的傳感器數(shù)據(jù)進(jìn)行融合并用于相應(yīng)的處理，在合理利用傳感器實(shí)現(xiàn)機(jī)器人導(dǎo)航的基礎(chǔ)上，靈活應(yīng)用于多種場景，并且兼顧成本，能夠?qū)崿F(xiàn)較好的自主導(dǎo)航效果。

圖6為本發(fā)明實(shí)施例六提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置的結(jié)構(gòu)示意圖，如圖6所示，在實(shí)施例五的基礎(chǔ)上，實(shí)時(shí)定位模塊52包括：

初始化單元521，用于根據(jù)所述磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)，對所述機(jī)器人的姿態(tài)進(jìn)行初始化；

估計(jì)單元522，用于實(shí)時(shí)根據(jù)當(dāng)前時(shí)刻與上一時(shí)刻所述加速度計(jì)傳感器和所述陀螺儀傳感器采集的數(shù)據(jù)、以及所述編碼器數(shù)據(jù)，計(jì)算當(dāng)前時(shí)刻相對于上一時(shí)刻所述機(jī)器人的位置和姿態(tài)變化估計(jì)；

獲取單元523，用于確定所述總體環(huán)境地圖中與所述位置和姿態(tài)變化估計(jì)對應(yīng)的地圖點(diǎn)，獲得所述地圖點(diǎn)的地圖數(shù)據(jù)；

匹配單元524，用于通過將所述激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)與所述地圖點(diǎn)的地圖數(shù)據(jù)分別進(jìn)行匹配，獲得所述機(jī)器人的當(dāng)前位置，所述機(jī)器人的當(dāng)前位置為匹配度最高的地圖點(diǎn)的位置。

以實(shí)際場景舉例來說：在初始化時(shí)，初始化單元521讀取磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)，獲得機(jī)器人絕對朝向的初始估計(jì)，使用這個(gè)初始估計(jì)進(jìn)行方向初始化。在機(jī)器人移動(dòng)過程中，估計(jì)單元522不斷讀取編碼器數(shù)據(jù)、以及所述加速度計(jì)傳感器和所述陀螺儀傳感器采集的數(shù)據(jù)，計(jì)算當(dāng)前時(shí)刻相對于上一時(shí)刻機(jī)器人的位置和姿態(tài)變化估計(jì)。其中，對編碼器數(shù)據(jù)進(jìn)行積分處理即可獲得位置和姿態(tài)變化估計(jì)，對加速度計(jì)傳感器采集的數(shù)據(jù)進(jìn)行積分可以獲得位置變化估計(jì)，對陀螺儀傳感器采集的數(shù)據(jù)進(jìn)行積分可以獲得姿態(tài)變化估計(jì)，之后通過對上述三者加權(quán)平均，即可得到當(dāng)前時(shí)刻相對于上一時(shí)刻的位置和姿態(tài)變化估計(jì)。之后，獲取單元523根據(jù)位置和姿態(tài)變化估計(jì)確定相應(yīng)的地圖點(diǎn)，匹配單元524用相應(yīng)地圖點(diǎn)的地圖數(shù)據(jù)和激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)做匹配，匹配度最高的地圖點(diǎn)即為機(jī)器人的當(dāng)前位置。

具體地，可以采用蒙特卡洛粒子濾波方法進(jìn)行機(jī)器人自主定位。相應(yīng)的，初始化單元521根據(jù)磁強(qiáng)計(jì)傳感器采集的數(shù)據(jù)，對粒子的姿態(tài)進(jìn)行初始化，在機(jī)器人移動(dòng)過程中，估計(jì)單元522實(shí)時(shí)根據(jù)當(dāng)前時(shí)刻與上一時(shí)刻所述加速度計(jì)傳感器和所述陀螺儀傳感器采集的數(shù)據(jù)、以及所述編碼器數(shù)據(jù)，計(jì)算當(dāng)前時(shí)刻相對于上一時(shí)刻的粒子位置和姿態(tài)變化估計(jì)，獲取單元523根據(jù)位置和姿態(tài)變化估計(jì)確定一些地圖點(diǎn)，匹配單元524將這些地圖點(diǎn)的地圖數(shù)據(jù)與激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)做匹配，匹配度最高的地圖點(diǎn)即為機(jī)器人的當(dāng)前位置。

在上述匹配過程中，激光雷達(dá)傳感器的精度高、噪聲低，且觀測距離遠(yuǎn)，可以獲取遠(yuǎn)處的線特征，而匹配的主要依據(jù)便是線特征。因此，只基于激光雷達(dá)傳感器采集的數(shù)據(jù)進(jìn)行匹配即可，可以有效節(jié)省匹配所需時(shí)間，提高機(jī)器人定位的效率，從而更快實(shí)現(xiàn)導(dǎo)航。

并且，結(jié)合使用編碼器數(shù)據(jù)、加速度計(jì)傳感器、陀螺儀傳感器計(jì)算當(dāng)前位置和姿態(tài)變化估計(jì)，實(shí)現(xiàn)多源融合，可以提高機(jī)器人定位的準(zhǔn)確度，從而更準(zhǔn)確可靠地實(shí)現(xiàn)機(jī)器人導(dǎo)航。

其中，本實(shí)施例提供的機(jī)器人導(dǎo)航裝置可以執(zhí)行實(shí)施例二的方法實(shí)施例的技術(shù)方案，其實(shí)現(xiàn)原理和技術(shù)效果類似，此處不再贅述。

本實(shí)施例提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置，在實(shí)現(xiàn)機(jī)器人自主定位的過程中，結(jié)合使用編碼器數(shù)據(jù)、加速度計(jì)傳感器、陀螺儀傳感器計(jì)算當(dāng)前位置和姿態(tài)變化估計(jì)，僅基于激光雷達(dá)傳感器采集的數(shù)據(jù)進(jìn)行匹配，不但可以有效節(jié)省匹配所需時(shí)間，還能提高機(jī)器人定位的準(zhǔn)確度，從而更快速準(zhǔn)確可靠地實(shí)現(xiàn)機(jī)器人導(dǎo)航。

圖7A為本發(fā)明實(shí)施例七提供的一種基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置的結(jié)構(gòu)示意圖，如圖7A所示，在實(shí)施例五或?qū)嵤├幕A(chǔ)上，控制模塊54包括：

檢測單元541，用于實(shí)時(shí)根據(jù)所述超聲波傳感器數(shù)據(jù)或所述紅外傳感器采集的數(shù)據(jù)，判斷所述機(jī)器人是否處于移動(dòng)受阻狀態(tài)；

第一控制單元542，用于若檢測單元541檢測到所述機(jī)器人未處于移動(dòng)受阻狀態(tài)，則根據(jù)所述深度攝像頭和所述激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)，構(gòu)建局部環(huán)境地圖，并根據(jù)所述局部環(huán)境地圖、所述機(jī)器人的當(dāng)前位置和規(guī)劃路線，通過局部路徑規(guī)劃算法控制所述機(jī)器人避開障礙物移動(dòng)。

以實(shí)際場景舉例：局部規(guī)劃策略總體分為兩個(gè)情形，一個(gè)情形是正常狀態(tài)下的規(guī)劃策略，另一個(gè)情形是特殊狀態(tài)處理。具體的，針對第一種情形，檢測單元541實(shí)時(shí)根據(jù)超聲波傳感器數(shù)據(jù)或紅外傳感器采集的數(shù)據(jù)，判斷所述機(jī)器人是否處于移動(dòng)受阻狀態(tài)，若未處于移動(dòng)受阻狀態(tài)，即機(jī)器人正常行走過程中，則第一控制單元542使用常規(guī)的規(guī)劃策略進(jìn)行規(guī)劃，即根據(jù)深度攝像頭和所激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)構(gòu)建的局部環(huán)境地圖，結(jié)合前面獲取的機(jī)器人的當(dāng)前位置和規(guī)劃路線，通過局部路徑規(guī)劃算法控制機(jī)器人避開障礙物移動(dòng)。具體地，可以采用動(dòng)態(tài)窗口模擬算法進(jìn)行機(jī)器人移動(dòng)控制。

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

第二控制單元543，用于若檢測單元541檢測到所述機(jī)器人處于移動(dòng)受阻狀態(tài)，則根據(jù)所述超聲波傳感器數(shù)據(jù)或所述紅外傳感器當(dāng)前采集的數(shù)據(jù)及預(yù)設(shè)的方向選擇策略，控制所述機(jī)器人避開障礙物移動(dòng)。

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

其中，本實(shí)施例提供的機(jī)器人導(dǎo)航裝置可以執(zhí)行實(shí)施例三的方法實(shí)施例的技術(shù)方案，其實(shí)現(xiàn)原理和技術(shù)效果類似，此處不再贅述。

本實(shí)施例提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置，在對機(jī)器人的移動(dòng)控制策略中，基于超聲波傳感器數(shù)據(jù)或紅外傳感器采集的數(shù)據(jù)判定當(dāng)前狀態(tài)，若正常，則根據(jù)深度攝像頭和激光雷達(dá)傳感器當(dāng)前采集的數(shù)據(jù)構(gòu)建局部環(huán)境地圖，并基于該局部環(huán)境地圖，結(jié)合機(jī)器人當(dāng)前位置和規(guī)劃路線，控制機(jī)器人避開障礙物移動(dòng)，否則，采用預(yù)設(shè)的方向選擇策略進(jìn)行逃離，從而確保機(jī)器人導(dǎo)航的可靠性。

圖8為本發(fā)明實(shí)施例八提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置的結(jié)構(gòu)示意圖，如圖8所示，在實(shí)施例七的基礎(chǔ)上，第一控制單元542包括：

投影子單元81，用于對所述深度攝像頭當(dāng)前采集的數(shù)據(jù)進(jìn)行去噪，并將去噪后的數(shù)據(jù)向二維平面進(jìn)行投影，獲得二維投影數(shù)據(jù)；

處理子單元82，用于若二維投影數(shù)據(jù)相應(yīng)位置不存在所述激光雷達(dá)傳感器采集的數(shù)據(jù)，則將相應(yīng)位置的局部環(huán)境地圖數(shù)據(jù)以對應(yīng)的二維投影數(shù)據(jù)進(jìn)行填充；

處理子單元82，還用于若所述二維投影數(shù)據(jù)相應(yīng)位置存在所述激光雷達(dá)傳感器采集的數(shù)據(jù)，則將所述二維投影數(shù)據(jù)和所述激光雷達(dá)傳感器采集的數(shù)據(jù)進(jìn)行加權(quán)平均，以獲得相應(yīng)位置的局部環(huán)境地圖數(shù)據(jù)。

本實(shí)施例中，以采用動(dòng)態(tài)窗口模擬算法控制機(jī)器人移動(dòng)為例，動(dòng)態(tài)窗口模擬算法是基于二維柵格地圖，因此需要投影子單元81先對三維點(diǎn)云數(shù)據(jù)進(jìn)行去噪，然后將三維點(diǎn)云數(shù)據(jù)向二維平面進(jìn)行投影獲得二維投影數(shù)據(jù)，此時(shí)將三維點(diǎn)云數(shù)據(jù)分列投影，如果某一列有障礙物點(diǎn)，則此列的投影結(jié)果為障礙物，否則投影為空閑。處理子單元82將二維投影數(shù)據(jù)與激光雷達(dá)傳感器采集的二維平面數(shù)據(jù)進(jìn)行融合，具體策略為，使用二維投影數(shù)據(jù)對激光雷達(dá)傳感器采集的二維數(shù)據(jù)進(jìn)行更新，即對于激光雷達(dá)傳感器未觀測到的位置直接使用獲得的二維投影數(shù)據(jù)進(jìn)行填充，對于激光雷達(dá)傳感器觀測到的位置，采用二維投影數(shù)據(jù)和激光雷達(dá)傳感器采集到的數(shù)據(jù)進(jìn)行加權(quán)平均。此方法適用于多個(gè)深度攝像頭和多個(gè)激光雷達(dá)傳感器。

其中，本實(shí)施例提供的機(jī)器人導(dǎo)航裝置可以執(zhí)行實(shí)施例四的方法實(shí)施例的技術(shù)方案，其實(shí)現(xiàn)原理和技術(shù)效果類似，此處不再贅述。

本實(shí)施例提供的基于多傳感器數(shù)據(jù)融合的機(jī)器人導(dǎo)航裝置，將深度攝像頭采集的三維點(diǎn)云數(shù)據(jù)進(jìn)行去噪投影，基于獲得的二維投影數(shù)據(jù)和激光雷達(dá)傳感器采集到的二維平面數(shù)據(jù)進(jìn)行融合，構(gòu)建局部環(huán)境地圖，實(shí)現(xiàn)對機(jī)器人的移動(dòng)控制，本實(shí)施例通過將二維平面數(shù)據(jù)，三維點(diǎn)云數(shù)據(jù)進(jìn)行融合處理，可以達(dá)到較好的局部規(guī)劃效果，提高導(dǎo)航準(zhǔn)確性。

最后應(yīng)說明的是：以上各實(shí)施例僅用以說明本發(fā)明的技術(shù)方案，而非對其限制；盡管參照前述各實(shí)施例對本發(fā)明進(jìn)行了詳細(xì)的說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解：其依然可以對前述各實(shí)施例所記載的技術(shù)方案進(jìn)行修改，或者對其中部分或者全部技術(shù)特征進(jìn)行等同替換；而這些修改或者替換，并不使相應(yīng)技術(shù)方案的本質(zhì)脫離本發(fā)明各實(shí)施例技術(shù)方案的范圍。