本發(fā)明涉及移動(dòng)機(jī)器人領(lǐng)域，具體為一種多傳感器融合的移動(dòng)機(jī)器人路徑規(guī)劃方法及裝置。

背景技術(shù)：

1、移動(dòng)機(jī)器人定位問題是指確定一個(gè)機(jī)器人相對(duì)于其周圍環(huán)境的姿態(tài)。它被認(rèn)為是移動(dòng)機(jī)器人完成自主任務(wù)的一個(gè)重要功能。

2、導(dǎo)航技術(shù)作為移動(dòng)機(jī)器人技術(shù)研發(fā)的關(guān)鍵技術(shù)，主要包括同步定位、建圖和路徑規(guī)劃。

3、slam需要在位置環(huán)境中完成機(jī)器人的定位和映射任務(wù)，而路徑規(guī)劃的目標(biāo)是在復(fù)雜的環(huán)境中生成無(wú)碰撞的路徑，以確保移動(dòng)機(jī)器人能夠自由移動(dòng)。slam的關(guān)鍵技術(shù)是在復(fù)雜環(huán)境下構(gòu)建機(jī)器人的姿態(tài)和環(huán)境地圖。根據(jù)位置和地圖信息，估計(jì)機(jī)器人的未知運(yùn)動(dòng)，并隨著運(yùn)動(dòng)過程增量地構(gòu)造地圖，實(shí)現(xiàn)機(jī)器人的自主導(dǎo)航和避障功能。根據(jù)單個(gè)傳感器進(jìn)行實(shí)時(shí)定位和映射，系統(tǒng)的精度不夠、魯棒性較差。因此，本技術(shù)提出了一種多傳感器融合的移動(dòng)機(jī)器人路徑規(guī)劃方法及裝置，以至少部分解決現(xiàn)有技術(shù)中可能存在的問題。

技術(shù)實(shí)現(xiàn)思路

1、為了克服上述問題或者至少部分地解決上述問題，本發(fā)明實(shí)施例提供一種多傳感器融合的移動(dòng)機(jī)器人路徑規(guī)劃方法及裝置，可以提高系統(tǒng)的精度和魯棒性。

2、本發(fā)明的實(shí)施例是這樣實(shí)現(xiàn)的：

3、本技術(shù)實(shí)施例提供一種實(shí)現(xiàn)多傳感器融合的移動(dòng)機(jī)器人路徑規(guī)劃方法，所述方法包括：

4、基于ekf數(shù)據(jù)融合：利用ekf，將深度相機(jī)的測(cè)量值作為當(dāng)前時(shí)刻的系統(tǒng)預(yù)測(cè)值，并將激光雷達(dá)的測(cè)量值作為系統(tǒng)測(cè)量值，對(duì)深度相機(jī)的測(cè)量數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)進(jìn)行映射，確定數(shù)據(jù)融合的最優(yōu)估計(jì)值，獲得優(yōu)化的協(xié)方差矩陣；

5、建圖與回環(huán)檢測(cè)：利用數(shù)據(jù)融合的協(xié)方差矩陣數(shù)據(jù)，采用基于圖優(yōu)化的算法構(gòu)建傳感器數(shù)據(jù)處理完成后的地圖，以及使用icp算法將當(dāng)前激光點(diǎn)云信息與子圖進(jìn)行匹配，獲得姿態(tài)變換關(guān)系，并形成插入后端優(yōu)化的閉環(huán)約束，通過優(yōu)化手段計(jì)算出最佳候選姿態(tài)，消除偏執(zhí)，完成閉環(huán)檢測(cè)；

6、路徑規(guī)劃：基于所述地圖和候選姿態(tài)，利用蟻群算法和混合策略更新信息素，進(jìn)行路徑規(guī)劃，獲得可行的路徑最優(yōu)解。

7、在本發(fā)明的一些實(shí)施例中，所述基于ekf數(shù)據(jù)融合之前步驟還包括，數(shù)據(jù)預(yù)處理步驟：利用深度相機(jī)的數(shù)據(jù)處理，將深度相機(jī)獲得的圖像數(shù)據(jù)轉(zhuǎn)換為激光數(shù)據(jù)，以完成傳感器之間的數(shù)據(jù)融合，具體包括：

8、對(duì)得到的深度圖像進(jìn)行有效區(qū)域篩選，得到待處理的深度圖像；

9、通過對(duì)深度圖像和攝像機(jī)參數(shù)模型，得到了世界坐標(biāo)系中深度攝像機(jī)各像素的位置坐標(biāo)；

10、將空間點(diǎn)云投影到相應(yīng)的激光掃描范圍內(nèi)，計(jì)算直線ao與do之間的夾角；其中，激光雷達(dá)的掃描結(jié)果為，激光束分為n個(gè)部分，激光數(shù)據(jù)用[n]表示，投影到陣列中的點(diǎn)m的索引值n，其中，，激光[n]是從x軸上的d點(diǎn)到相機(jī)光點(diǎn)中心的距離。

11、在本發(fā)明的一些實(shí)施例中，所述基于ekf數(shù)據(jù)融合步驟，包括：

12、利用機(jī)器人的初始位置初始化當(dāng)前位置和協(xié)方差誤差，包括，使用機(jī)器人上的深度攝像機(jī)來(lái)收集有關(guān)運(yùn)動(dòng)的數(shù)據(jù)，將該值作為當(dāng)前moment系統(tǒng)輸入來(lái)預(yù)測(cè)下一moment的位姿，并計(jì)算預(yù)測(cè)的誤差協(xié)方差；

13、根據(jù)當(dāng)前姿態(tài)預(yù)測(cè)，其中，是狀態(tài)方程的雅可比矩陣，是預(yù)測(cè)狀態(tài)的高斯噪聲的協(xié)方差矩陣；

14、計(jì)算卡爾曼增益，利用多傳感器的測(cè)量值來(lái)校正之前的系統(tǒng)預(yù)測(cè)，從而進(jìn)行數(shù)據(jù)融合；

15、利用卡爾曼增益法計(jì)算出傳感器數(shù)據(jù)融合的最優(yōu)估計(jì)值，并完成協(xié)方差矩陣的更新，其中，為轉(zhuǎn)移矩陣，表示測(cè)量高斯噪聲的協(xié)方差矩陣。

16、在本發(fā)明的一些實(shí)施例中，所述利用深度相機(jī)的數(shù)據(jù)處理，將深度相機(jī)獲得的圖像數(shù)據(jù)轉(zhuǎn)換為激光數(shù)據(jù)，以完成傳感器之間的數(shù)據(jù)融合，之前還包括：

17、在深度攝像機(jī)的數(shù)據(jù)和激光雷達(dá)的數(shù)據(jù)進(jìn)行關(guān)聯(lián)；

18、根據(jù)傳感器獲得的觀測(cè)信息之間的馬氏距離，判斷深度攝像機(jī)的數(shù)據(jù)和激光雷達(dá)的數(shù)據(jù)之間的相關(guān)性，其中，當(dāng)距離小于設(shè)定的閾值x2時(shí)，數(shù)據(jù)相關(guān)性較高。

19、在本發(fā)明的一些實(shí)施例中，所述建圖與回環(huán)檢測(cè)步驟，包括：

20、利用融合的傳感器數(shù)據(jù)，采用基于圖優(yōu)化的算法構(gòu)建傳感器數(shù)據(jù)處理完成后的地圖；

21、使用icp算法將當(dāng)前激光點(diǎn)云信息與子圖進(jìn)行匹配，獲得姿態(tài)變換關(guān)系，并形成插入后端優(yōu)化的閉環(huán)約束；

22、檢測(cè)機(jī)制判斷移動(dòng)機(jī)器人在激光點(diǎn)云中的相對(duì)位置與載體在子地圖中的相對(duì)位置之間的相似性；

23、計(jì)算出最佳候選姿態(tài)，并消除偏執(zhí)，完成閉環(huán)檢測(cè)，以及對(duì)和幀之間的機(jī)器人姿態(tài)進(jìn)行優(yōu)化和校正，并輸出校正后的姿態(tài)軌跡和地圖。

24、在本發(fā)明的一些實(shí)施例中，所述路徑規(guī)劃步驟，具體包括：

25、在初始狀態(tài)下，螞蟻選擇可行路徑的概率相等，螞蟻將根據(jù)選擇路徑上狀態(tài)過渡點(diǎn)的信息素含量來(lái)確定下一個(gè)節(jié)點(diǎn)的方向；

26、將網(wǎng)格圖上b從i點(diǎn)到g點(diǎn)的狀態(tài)轉(zhuǎn)移概率設(shè)為，確定從i點(diǎn)到g點(diǎn)信息素含量，其中，{i}表示b可以在節(jié)點(diǎn)i處選擇的轉(zhuǎn)移節(jié)點(diǎn)集，啟發(fā)式函數(shù)，信息素啟發(fā)式因子a，期望啟發(fā)式因子w；

27、根據(jù)從當(dāng)前節(jié)點(diǎn)到下一個(gè)節(jié)點(diǎn)的距離和從下一個(gè)節(jié)點(diǎn)到目標(biāo)的距離的和的平方，引入啟發(fā)式函數(shù)：，其中，由實(shí)時(shí)環(huán)境決定，表示當(dāng)前節(jié)點(diǎn)與下一個(gè)節(jié)點(diǎn)之間的距離，表示當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)之間的距離；

28、確定最大值和最小值兩點(diǎn)之間的信息素濃度：，其中，為某次迭代后的最優(yōu)路徑長(zhǎng)度，c為每次循環(huán)的次數(shù)；

29、通過來(lái)更新信息素，并調(diào)整整個(gè)路徑上的信息素獲得最優(yōu)路徑，其中，m為節(jié)點(diǎn)i和g之間的第b只螞蟻的信息素增量；

30、將第b只螞蟻在兩點(diǎn)之間的信息素濃度增量通過更新策略，以確保每只螞蟻都能找到可行的路徑，其中，是信息素?fù)]發(fā)系數(shù)，為節(jié)點(diǎn)i和g之間的信息素增量，是迄今為止找到的最優(yōu)解。

31、在本發(fā)明的一些實(shí)施例中，所述路徑規(guī)劃步驟之后，還包括全局路徑平滑處理，具體包括：

32、通過至少三次b樣條曲線對(duì)全局路徑進(jìn)行平滑處理。

33、在本發(fā)明的一些實(shí)施例中，所述全局路徑平滑處理之后的步驟，還包括，全局與局部路徑規(guī)劃算法融合，具體包括：

34、當(dāng)智能機(jī)器人的駕駛環(huán)境發(fā)生變化時(shí)，機(jī)器人通過其自身的環(huán)境傳感器將障礙物信息實(shí)時(shí)集成到環(huán)境地圖中；

35、采用局部路徑規(guī)劃方法對(duì)智能機(jī)器人平臺(tái)的驅(qū)動(dòng)路徑進(jìn)行重新規(guī)劃；

36、機(jī)器人繼續(xù)按照全局最優(yōu)解進(jìn)行驅(qū)動(dòng)，并結(jié)合全局路徑規(guī)劃信息計(jì)算評(píng)價(jià)函數(shù)，以確保最終的局部路徑規(guī)劃是全局最優(yōu)路徑，其中，表示指向目標(biāo)的矢量與連接起點(diǎn)和當(dāng)前位置的矢量之間的夾角，是靜態(tài)障礙物與指向目標(biāo)的當(dāng)前軌跡之間的垂直距離，表示未知障礙物與當(dāng)前指向的目標(biāo)軌跡之間的垂直距離，是指向目標(biāo)軌跡的當(dāng)前速度的評(píng)估函數(shù)，u，v，w，為加權(quán)系數(shù)；

37、結(jié)合全局規(guī)劃路徑，實(shí)時(shí)規(guī)劃局部移動(dòng)目標(biāo)點(diǎn)，采用預(yù)覽跟蹤方法，根據(jù)路徑規(guī)劃過程中的控制參數(shù)對(duì)目標(biāo)點(diǎn)進(jìn)行跟蹤；得到局部最優(yōu)路徑；

38、確定局部移動(dòng)目標(biāo)點(diǎn)是否為最終目標(biāo)點(diǎn)；如果不是，則跳轉(zhuǎn)到數(shù)據(jù)預(yù)處理步驟執(zhí)行；如果是，則驅(qū)動(dòng)機(jī)器人到達(dá)目標(biāo)點(diǎn)。

39、一種多傳感器融合的移動(dòng)機(jī)器人路徑規(guī)劃裝置，包括：

40、數(shù)據(jù)融合模塊，利用ekf，將深度相機(jī)的測(cè)量值作為當(dāng)前時(shí)刻的系統(tǒng)預(yù)測(cè)值，并將激光雷達(dá)的測(cè)量值作為系統(tǒng)測(cè)量值，對(duì)深度相機(jī)的測(cè)量數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)進(jìn)行映射，確定數(shù)據(jù)融合的最優(yōu)估計(jì)值，獲得優(yōu)化的協(xié)方差矩陣；

41、建圖與回環(huán)檢測(cè)模塊，利用數(shù)據(jù)融合的協(xié)方差矩陣數(shù)據(jù)，采用基于圖優(yōu)化的算法構(gòu)建傳感器數(shù)據(jù)處理完成后的地圖，以及使用icp算法將當(dāng)前激光點(diǎn)云信息與子圖進(jìn)行匹配，獲得姿態(tài)變換關(guān)系，并形成插入后端優(yōu)化的閉環(huán)約束，通過優(yōu)化手段計(jì)算出最佳候選姿態(tài)，消除偏執(zhí)，完成閉環(huán)檢測(cè)；

42、路徑規(guī)劃模塊，用于基于所述地圖和候選姿態(tài)，利用蟻群算法和混合策略更新信息素，進(jìn)行路徑規(guī)劃，獲得可行的路徑最優(yōu)解。

43、一種電子設(shè)備，包括處理器、存儲(chǔ)器及存儲(chǔ)在所述存儲(chǔ)器上并能夠在所述處理器上運(yùn)行的計(jì)算機(jī)程序，所述計(jì)算機(jī)程序被所述處理器執(zhí)行時(shí)實(shí)現(xiàn)上述實(shí)現(xiàn)多傳感器融合的移動(dòng)機(jī)器人路徑規(guī)劃方法。

44、一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)上存儲(chǔ)計(jì)算機(jī)程序，所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)上述實(shí)現(xiàn)多傳感器融合的移動(dòng)機(jī)器人路徑規(guī)劃方法。

45、相對(duì)于現(xiàn)有技術(shù)，本發(fā)明的實(shí)施例至少具有如下優(yōu)點(diǎn)或有益效果：

46、通過利用ekf，將深度相機(jī)的測(cè)量值作為當(dāng)前時(shí)刻的系統(tǒng)預(yù)測(cè)值，并將激光雷達(dá)的測(cè)量值作為系統(tǒng)測(cè)量值，對(duì)深度相機(jī)的測(cè)量數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)進(jìn)行映射，確定數(shù)據(jù)融合的最優(yōu)估計(jì)值，獲得優(yōu)化的協(xié)方差矩陣；利用數(shù)據(jù)融合的協(xié)方差矩陣數(shù)據(jù)，采用基于圖優(yōu)化的算法構(gòu)建傳感器數(shù)據(jù)處理完成后的地圖，以及使用icp算法將當(dāng)前激光點(diǎn)云信息與子圖進(jìn)行匹配，獲得姿態(tài)變換關(guān)系，并形成插入后端優(yōu)化的閉環(huán)約束，通過優(yōu)化手段計(jì)算出最佳候選姿態(tài)，消除偏執(zhí)，完成閉環(huán)檢測(cè)；基于所述地圖和候選姿態(tài)，利用使蟻群算法和混合策略更新信息素，進(jìn)行路徑規(guī)劃，獲得可行的路徑最優(yōu)解。通過基于多傳感器信息融合的室內(nèi)移動(dòng)機(jī)器人自動(dòng)定位和映射的路徑規(guī)劃slam框架，以彌補(bǔ)環(huán)境中較低的障礙物無(wú)法感知的問題。將二維激光雷達(dá)與深度視覺傳感器的信息融合，提高機(jī)器人的環(huán)境感知能力及在復(fù)雜環(huán)境中對(duì)障礙物的檢測(cè)能力，避免單個(gè)傳感器性能導(dǎo)致的檢測(cè)精度低的缺點(diǎn)，提高路徑規(guī)劃效率及地圖構(gòu)建的可靠性和準(zhǔn)確性，實(shí)現(xiàn)局部實(shí)時(shí)避障，并保持全局最優(yōu)。