本發(fā)明涉及腿足機(jī)器人環(huán)境感知和地圖構(gòu)建，特別是一種面向腿足機(jī)器人的可通行地圖構(gòu)建方法。

背景技術(shù)：

1、隨著智能移動(dòng)機(jī)器人技術(shù)的迅速發(fā)展，可通行地圖在機(jī)器人自主導(dǎo)航中的重要性日益提升。特別是在復(fù)雜的室內(nèi)外環(huán)境中執(zhí)行任務(wù)的腿足機(jī)器人，如人形機(jī)器人、四足機(jī)器人等；相較于輪式機(jī)器人和無(wú)人駕駛車輛等，腿足機(jī)器人面臨的地形是更為復(fù)雜和不可預(yù)測(cè)的，尤其依賴于實(shí)時(shí)、精確的可通行地形信息來(lái)確保其運(yùn)行的高效性和安全性。腿足機(jī)器人可通行地圖通過(guò)動(dòng)態(tài)反映環(huán)境中各精細(xì)區(qū)域的可通行狀況，使機(jī)器人能夠迅速識(shí)別障礙與通行代價(jià)、選擇最佳路徑，從而避免陷入無(wú)法通行的區(qū)域，減少能源消耗和時(shí)間成本。因此，開(kāi)發(fā)一種能夠?qū)崟r(shí)生成和更新的腿足機(jī)器人可通行地圖具有重要的實(shí)際意義。

2、可通行地圖的主流構(gòu)建方法包括從3d地圖中提取可通行區(qū)域和從2.5d柵格地圖(多為高程圖)中計(jì)算柵格可通行度，這兩種方法各有優(yōu)劣。3d地圖能夠提供豐富的環(huán)境細(xì)節(jié)，適用于復(fù)雜地形的精確可通行性判斷，但其計(jì)算資源需求大，生成和維護(hù)復(fù)雜，且在實(shí)時(shí)應(yīng)用中效率較低。2.5d柵格地圖計(jì)算效率高，易于構(gòu)建和更新，適合機(jī)器人在動(dòng)態(tài)環(huán)境中進(jìn)行實(shí)時(shí)的可通行度判斷，但其僅能表示地形的高度信息，無(wú)法詳細(xì)區(qū)分地形的其他特性，如坡度、地面材料等，構(gòu)建的可通行地圖精確度較低。為解決以上問(wèn)題，已有相關(guān)研究圍繞精確、快速構(gòu)建可通行地圖展開(kāi)?？悼碌热?cn118392156a)通過(guò)計(jì)算高程圖中各柵格處的高度、柵格的斜率、曲率和粗糙度，得到初始可通過(guò)性代價(jià)地圖，再基于機(jī)器人近似成的矩形的邊緣，對(duì)初始可通過(guò)性代價(jià)地圖中的障礙物進(jìn)行膨脹，構(gòu)建出最終的可通過(guò)性代價(jià)地圖。李超等人(cn118329008a)通過(guò)將深度相機(jī)數(shù)據(jù)融合進(jìn)現(xiàn)有高程圖中提升精細(xì)度，深度相機(jī)不斷采集地形的點(diǎn)云數(shù)據(jù)，由感知計(jì)算單元進(jìn)行預(yù)處理后與前一幀點(diǎn)云進(jìn)行配準(zhǔn)，獲取精準(zhǔn)的幀間相對(duì)位姿；再根據(jù)相對(duì)位姿來(lái)更新高程圖，得到局部地形地圖；配準(zhǔn)時(shí)，使用機(jī)器人的定位信息獲取兩配準(zhǔn)點(diǎn)云相對(duì)位姿的初值。李智軍等人(cn116147642a)提供了一種融合地形與力的四足機(jī)器人可達(dá)性地圖構(gòu)建方法及系統(tǒng)，通過(guò)融合視覺(jué)信息和慣性信息建立包含高度信息的二維柵格地圖并設(shè)定高度閾值判斷地形可達(dá)性。李傳鵬(cn117830991a)等人提出了一種基于多模融合的四足機(jī)器人復(fù)雜場(chǎng)景感知方法及系統(tǒng)，通過(guò)深度神經(jīng)網(wǎng)絡(luò)對(duì)前方地形進(jìn)行重構(gòu)，通過(guò)對(duì)rgb圖像聚類獲取超像素圖像，然后采用多層感知器網(wǎng)絡(luò)處理生成圖像坐標(biāo)系下的可通行區(qū)域，進(jìn)而生成代價(jià)地圖。

3、然而，以上方法仍未解決高程圖在實(shí)際使用過(guò)程中易出現(xiàn)空洞與毛刺的問(wèn)題，不同地形的可通行度的計(jì)算方法也沒(méi)有區(qū)分，導(dǎo)致最終構(gòu)建的可通行圖的實(shí)用性不強(qiáng)。

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

1、針對(duì)現(xiàn)有技術(shù)中存在的不足，本發(fā)明提出了一種面向腿足機(jī)器人的可通行地圖構(gòu)建方法。

2、本發(fā)明的技術(shù)方案包括以下內(nèi)容：

3、一種面向腿足機(jī)器人的可通行地圖構(gòu)建方法，包括以下步驟：

4、s1、通過(guò)激光雷達(dá)獲取機(jī)器人周邊的原始點(diǎn)云信息，通過(guò)慣性測(cè)量單元獲取機(jī)器人本體慣性測(cè)量信息，通過(guò)衛(wèi)星信號(hào)接收機(jī)獲取機(jī)器人本體經(jīng)緯度信息，通過(guò)相機(jī)獲得機(jī)器人前方的圖像信息；

5、s2、對(duì)所述步驟s1獲取的原始點(diǎn)云信息、慣性測(cè)量信息、經(jīng)緯度信息和相機(jī)圖像信息進(jìn)行預(yù)處理；其中，所述預(yù)處理包括原始點(diǎn)云信息預(yù)處理、imu預(yù)積分處理、經(jīng)緯度信息的utm坐標(biāo)系投影和圖像語(yǔ)義信息提?。?/p>

6、s3、對(duì)所述步驟s2中預(yù)處理后的點(diǎn)云信息和慣性測(cè)量信息進(jìn)行誤差狀態(tài)卡爾曼濾波，得到機(jī)器人粗略位姿；以投影后的經(jīng)緯度信息為先驗(yàn)約束，以機(jī)器人粗略位姿為待優(yōu)化的因子，通過(guò)圖優(yōu)化的方法獲得機(jī)器人精確位姿；

7、s4、對(duì)所述步驟s2中預(yù)處理后的點(diǎn)云信息和步驟s3中獲得的機(jī)器人精確位姿，通過(guò)擴(kuò)展卡爾曼濾波的方法構(gòu)建2.5d柵格地形圖，并通過(guò)卷積神經(jīng)網(wǎng)絡(luò)對(duì)該地形圖進(jìn)行處理，得到平滑且無(wú)空洞的柵格地形圖；

8、s5、對(duì)所述步驟s4中獲得的柵格地形圖，提取其各單元柵格內(nèi)的法向量，根據(jù)法向量與步驟s3中獲得的機(jī)器人精確位姿，計(jì)算各單元柵格內(nèi)的坡度、坡度變化率與粗糙度；結(jié)合步驟s2中獲得的圖像語(yǔ)義信息，計(jì)算各單元柵格內(nèi)的可通行度，并通過(guò)擴(kuò)展卡爾曼濾波實(shí)時(shí)維護(hù)一張腿足機(jī)器人可通行地圖。

9、進(jìn)一步地，所述步驟s2包括以下子步驟：

10、s21、原始點(diǎn)云信息的預(yù)處理：對(duì)原始點(diǎn)云信息進(jìn)行離群點(diǎn)濾波和運(yùn)動(dòng)畸變矯正，并去除動(dòng)態(tài)點(diǎn)云，得到可信點(diǎn)云；

11、s22、imu預(yù)積分的處理：根據(jù)imu角速度和加速度數(shù)據(jù)，計(jì)算測(cè)量時(shí)兩幀之間的相對(duì)運(yùn)動(dòng)，獲得機(jī)器人位姿變化；

12、s23、經(jīng)緯度信息的utm坐標(biāo)系投影：衛(wèi)星接收機(jī)將全球衛(wèi)星導(dǎo)航定位系統(tǒng)gnss發(fā)送的報(bào)文解讀為以wgs84地心坐標(biāo)系為基準(zhǔn)的經(jīng)緯度信息，再將該經(jīng)緯度信息轉(zhuǎn)化為utm坐標(biāo)系，即機(jī)器人本地坐標(biāo)系下的xyz三軸坐標(biāo)信息；

13、s24、圖像語(yǔ)義信息提?。和ㄟ^(guò)卷積神經(jīng)網(wǎng)絡(luò)提取原始圖像信息的特征，并利用全連接層輸出物體的類別和邊界框，實(shí)現(xiàn)語(yǔ)義信息提取。

14、進(jìn)一步地，所述步驟s3包括以下子步驟：

15、s31、根據(jù)所述步驟s2得到的可信點(diǎn)云和由imu預(yù)積分得到的機(jī)器人兩幀之間的位姿變化推斷出當(dāng)前幀的初始位姿；將該初始位姿轉(zhuǎn)化到上一幀時(shí)刻坐標(biāo)系下，并通過(guò)兩幀點(diǎn)云間的幾何配準(zhǔn)關(guān)系構(gòu)建誤差模型，得到最優(yōu)的機(jī)器人位姿狀態(tài)，即機(jī)器人粗略位姿；

16、s32、利用圖優(yōu)化的方法，以機(jī)器人粗略位姿為頂點(diǎn)，以所述步驟s2中得到的經(jīng)緯度信息為約束邊，構(gòu)建圖模型，求解圖優(yōu)化問(wèn)題，得到機(jī)器人精確位姿狀態(tài)。

17、進(jìn)一步地，所述步驟s4包括以下子步驟：

18、s41、根據(jù)所述步驟s2中得到的可信點(diǎn)云和所述步驟s3中得到的機(jī)器人精確位姿狀態(tài)，通過(guò)擴(kuò)展卡爾曼濾波的方式得到一張2.5d柵格地形圖；其中，每個(gè)網(wǎng)格包含若干點(diǎn)、一個(gè)平均高度值和一個(gè)由卡爾曼濾波得到的方差估計(jì)；

19、s42、將所述步驟s41得到的柵格地形圖向量化后輸入輸入一個(gè)gan網(wǎng)絡(luò)中，通過(guò)下采樣提取圖像全局特征，通過(guò)上采樣逐步恢復(fù)圖像的空間分辨率，輸出一個(gè)向量化的平滑及補(bǔ)全后的柵格地形圖。

20、進(jìn)一步地，所述步驟s5包括以下子步驟：

21、s51、根據(jù)所述步驟s4中得到的柵格地形圖，對(duì)各單元柵格中的若干點(diǎn)云進(jìn)行平面擬合，取擬合后平面的法向量為該單元柵格的法向量；

22、s52、根據(jù)所述步驟s51提取到的各柵格法向量，及其鄰域柵格內(nèi)的法向量，計(jì)算各柵格的坡度及粗糙度；

23、s53、根據(jù)所述步驟s52得到的各單元柵格及其周邊柵格參數(shù)，加權(quán)計(jì)算各單元格可通行度。

24、進(jìn)一步地，所述步驟s53中，可通行度加權(quán)計(jì)算中的權(quán)重參數(shù)由步驟s2中獲得的圖像語(yǔ)義信息決定，不同語(yǔ)義區(qū)域?qū)?yīng)的各參數(shù)權(quán)重不同。

25、綜上所述，本發(fā)明的有益效果是：針對(duì)在復(fù)雜室內(nèi)外環(huán)境中執(zhí)行任務(wù)的機(jī)器人，利用多傳感器信息，發(fā)明了一種能夠?qū)崟r(shí)生成和更新的2.5d腿足機(jī)器人可通行地圖的方法。本發(fā)明在保留與可通行度相關(guān)信息的基礎(chǔ)上，選擇構(gòu)建信息量更小的2.5d柵格地圖，與三維可通行地圖相比更加輕量，耗費(fèi)計(jì)算和存儲(chǔ)成本更小，可部署在機(jī)器人上實(shí)時(shí)運(yùn)行。本發(fā)明創(chuàng)新性地采用對(duì)抗式神經(jīng)網(wǎng)絡(luò)解決了高程圖在實(shí)際應(yīng)用中出現(xiàn)的空洞與毛刺的現(xiàn)象，為后續(xù)計(jì)算可通行度提供了更精確真實(shí)的機(jī)器人周邊信息。本發(fā)明在計(jì)算可通行度時(shí)，創(chuàng)新性地提出了根據(jù)語(yǔ)義信息決定不同語(yǔ)義區(qū)域內(nèi)可通行度計(jì)算公式中各信息參數(shù)的權(quán)重，實(shí)現(xiàn)了對(duì)現(xiàn)實(shí)世界中不同地形、材質(zhì)的可通行度自適應(yīng)計(jì)算，使得到的可通行圖更具可信度。