本發(fā)明屬于機(jī)器人建圖，尤其涉及一種面向自主探索的多分辨率多層次地圖信息結(jié)構(gòu)、動(dòng)態(tài)更新系統(tǒng)及構(gòu)建方法。

背景技術(shù)：

1、在自主探索任務(wù)中，準(zhǔn)確的定位與建圖(simultaneous?localization?andmapping,slam)技術(shù)是實(shí)現(xiàn)環(huán)境探索和任務(wù)執(zhí)行的核心環(huán)節(jié)之一，是任務(wù)執(zhí)行的基礎(chǔ)。地圖的構(gòu)建不僅需要完成對(duì)未知環(huán)境空間信息的重建，更是對(duì)自主探索效率、任務(wù)覆蓋率以及搜索路徑優(yōu)化的重要評(píng)估依據(jù)。同時(shí)，精準(zhǔn)的地圖信息能夠?yàn)橄掠我?guī)劃算法提供必要的輸入，例如障礙物的距離、梯度分布以及自由空間的拓?fù)浣Y(jié)構(gòu)等，這些數(shù)據(jù)直接影響到路徑規(guī)劃的安全性、平滑性和效率。

2、地圖構(gòu)建技術(shù)已經(jīng)有數(shù)十年的研究發(fā)展，一般基于雙目視覺、激光雷達(dá)或超聲波測(cè)距等傳感器提供基礎(chǔ)的環(huán)境感知能力，在機(jī)器人平臺(tái)上，雙目視覺由于其輕量化、成本低以及適配性強(qiáng)的優(yōu)勢(shì)，被廣泛用于環(huán)境建圖，通過雙目相機(jī)計(jì)算處理得到的深度圖，可以構(gòu)建二維或三維地圖。

3、視覺地圖構(gòu)建方法主要分為深度學(xué)習(xí)建圖和幾何方法建圖。深度學(xué)習(xí)建圖方法以語(yǔ)義地圖為研究熱點(diǎn)，通過語(yǔ)義分割和目標(biāo)檢測(cè)等技術(shù)，將場(chǎng)景中的關(guān)鍵對(duì)象識(shí)別并構(gòu)建語(yǔ)義地圖，適用于動(dòng)態(tài)環(huán)境下的三維地圖生成。然而，這類方法依賴于高性能硬件支持，實(shí)時(shí)性較差，不適合資源受限的機(jī)器人機(jī)載平臺(tái)。

4、相比之下，幾何方法在實(shí)時(shí)性和資源利用效率上具有顯著優(yōu)勢(shì)，適用于機(jī)器人環(huán)境建圖。傳統(tǒng)幾何方法包括光線映射和投影方法，前者通過傳感器光點(diǎn)更新光線的觀測(cè)點(diǎn)，后者將體素投影到深度圖中，記錄深度等信息。為提高存儲(chǔ)和查詢效率，體素哈希方法通過動(dòng)態(tài)增長(zhǎng)和高效的增刪改查操作優(yōu)化了地圖存儲(chǔ)結(jié)構(gòu)，適合動(dòng)態(tài)探索任務(wù)。此外，八叉樹地圖通過改進(jìn)存儲(chǔ)結(jié)構(gòu)壓縮內(nèi)存，截?cái)喾?hào)距離場(chǎng)(truncated?signed?distance?field，tsdf)利用距離場(chǎng)表示構(gòu)建三維物體表面，esdf則提供障礙物距離和梯度信息，用于運(yùn)動(dòng)規(guī)劃。

5、盡管現(xiàn)有方法各有優(yōu)勢(shì)，但在自主探索任務(wù)中，地圖構(gòu)建需要同時(shí)兼顧自由空間、障礙物和物體表面信息，以確保飛行安全、路徑優(yōu)化和目標(biāo)識(shí)別的準(zhǔn)確性。然而，大多數(shù)現(xiàn)有方法仍局限于單一信息的處理，在探索復(fù)雜場(chǎng)景下顯得不足，沒有充分利用已知和未知區(qū)域的地圖信息引導(dǎo)和提升探索任務(wù)效率，這對(duì)建圖技術(shù)提出了更高的綜合性要求。

6、綜合來(lái)看，這些方法在三維建圖中各有優(yōu)勢(shì)，但仍面臨效率、精度和適用性之間的平衡問題，特別是在機(jī)器人自主探索場(chǎng)景中的實(shí)時(shí)需求下，環(huán)境細(xì)節(jié)和探索效率難以平衡。

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

1、本發(fā)明目的在于提供一種面向自主探索的多分辨率多層次地圖信息結(jié)構(gòu)、動(dòng)態(tài)更新系統(tǒng)及構(gòu)建方法，以解決上述的技術(shù)問題。

2、為解決上述技術(shù)問題，本發(fā)明的一種面向自主探索的多分辨率多層次地圖構(gòu)建與動(dòng)態(tài)更新方法的具體技術(shù)方案如下：

3、一種面向自主探索的多分辨率多層次地圖信息結(jié)構(gòu)，包括第一層地圖、第二層地圖、第三層地圖和第四層地圖；

4、所述第一層地圖包括未知空間vunknown和已知空間vknown，所述未知空間vunknown表示環(huán)境中尚未探索、傳感器感知范圍外的區(qū)域，所述已知空間vknown為已被感知探索的區(qū)域，通過傳感器數(shù)據(jù)的累積更新，vknown逐漸擴(kuò)展，形成環(huán)境地圖，用體素表示；

5、所述第二層地圖基于已知空間vknown，包括自由空間vfree、障礙物空間vobstacle和剩余空間vresidue，三者通過三維坐標(biāo)所在柵格的占據(jù)概率值與閾值進(jìn)行比較標(biāo)定，其中所述自由空間vfree為機(jī)器人通過傳感器數(shù)據(jù)確定無(wú)任何障礙物存在的區(qū)域，為環(huán)境中路徑可行區(qū)域，占據(jù)概率值低于閾值，所述障礙物空間vobstacle為不可通行的區(qū)域，通過深度信息和視覺信息判斷存在障礙物，占據(jù)概率值高于閾值，所述剩余空間vresidue為視野盲區(qū)，占據(jù)概率值等于閾值；

6、所述第三層地圖基于障礙物空間vobstacle，包括未觀察空間vunobserved、已觀察空間vobserved和不需要探索空間vnoneed,三者通過機(jī)器人到障礙物表面距離、視野區(qū)域與相應(yīng)閾值判斷，其中所述未觀察空間vunobserved為機(jī)器人到障礙物表面距離大于設(shè)定閾值的區(qū)域，且在fov閾值范圍內(nèi)，所述已觀察空間vobserved為機(jī)器人到障礙物表面距離小于等于設(shè)定閾值的區(qū)域，且在fov閾值范圍內(nèi)，不需要探索空間vnoneed為機(jī)器人感知fov閾值范圍外，即需要額外調(diào)整機(jī)器人roll和pitch角的情況；

7、所述第四層地圖基于未觀察空間vunobserved，包括目標(biāo)空間vtarget和非目標(biāo)空間vuntarget，兩者通過深度圖變化方差值與閾值比較進(jìn)行判斷，其中所述目標(biāo)空間vtarget為深度變化不光滑的區(qū)域，變化值大于等于閾值，所述非目標(biāo)空間vuntarget兩為深度變化光滑的區(qū)域，變化值小于閾值。

8、本發(fā)明還公開了一種地圖信息結(jié)構(gòu)的地圖動(dòng)態(tài)更新系統(tǒng)，包括如下模塊：

9、傳感器數(shù)據(jù)處理模塊：用于讀取雙目相機(jī)的深度圖和無(wú)人機(jī)位姿信息，其中位姿信息通過計(jì)算雙目相機(jī)在世界坐標(biāo)系中的三維位置和姿態(tài)獲取，結(jié)合相機(jī)與機(jī)器人之間的旋轉(zhuǎn)矩陣和平移矩陣，以及機(jī)器人與世界坐標(biāo)系之間的轉(zhuǎn)換關(guān)系進(jìn)行計(jì)算，得到精確的定位信息；

10、多分辨率處理模塊：用于生成深度變化的方差圖，通過計(jì)算深度圖中像素點(diǎn)在uv平面視差的鄰域內(nèi)所有像素點(diǎn)的方差值，提取深度變化信息；

11、障礙物表面最近距離處理模塊：用于計(jì)算機(jī)器人到障礙物表面的最近距離，通過計(jì)算fov視野內(nèi)所有點(diǎn)在當(dāng)前狀態(tài)下的深度信息，與原始存儲(chǔ)信息比較取小值，代表機(jī)器人到障礙物表面的最短距離，實(shí)時(shí)更新障礙物的距離信息；

12、光線投影更新模塊：用于基于傳感器數(shù)據(jù)處理模塊信息更新體素占據(jù)狀態(tài)，通過提取的相機(jī)坐標(biāo)系中每個(gè)像素點(diǎn)的深度值，計(jì)算從相機(jī)位置到對(duì)應(yīng)深度點(diǎn)的射線路徑，沿射線逐步遍歷其經(jīng)過的體素，對(duì)每個(gè)體素的占據(jù)狀態(tài)進(jìn)行更新，通過累加其占據(jù)與否的數(shù)值，動(dòng)態(tài)構(gòu)建體素地圖；

13、占據(jù)信息處理模塊：用于對(duì)體素屬性進(jìn)行判斷，通過比較每個(gè)體素的占據(jù)數(shù)值和不占據(jù)數(shù)值，如果占據(jù)數(shù)值大于不占據(jù)數(shù)值，則計(jì)算并賦值為占據(jù)概率；反之，則賦值為非占據(jù)概率；接著，將計(jì)算得到的占據(jù)概率與預(yù)設(shè)的閾值進(jìn)行比較，當(dāng)占據(jù)概率超過設(shè)定閾值時(shí)，標(biāo)記該體素為障礙物，否則標(biāo)記為自由空間，數(shù)值與未知區(qū)域閾值比較，更小值為未知區(qū)域邊界；

14、歐式距離符號(hào)場(chǎng)地圖更新模塊：采用分軸波前傳播的方法，逐軸傳播計(jì)算，動(dòng)態(tài)更新最短距離，通過限定更新范圍為局部區(qū)域，僅對(duì)受影響的體素區(qū)域進(jìn)行更新計(jì)算，降低三維全局遍歷的計(jì)算開銷。

15、進(jìn)一步的，所述傳感器數(shù)據(jù)處理模塊的相機(jī)位置信息轉(zhuǎn)換關(guān)系為：

16、pw＝ru2w·(rc2u·pc+tc2u)+tu2w

17、其中rw為相機(jī)在世界坐標(biāo)系下的三維坐標(biāo)，ru2w為機(jī)器人imu坐標(biāo)系映射世界坐標(biāo)系的旋轉(zhuǎn)矩陣，tu2w為機(jī)器人imu坐標(biāo)系映射世界坐標(biāo)系的平移矩陣，rc2u為相機(jī)映射機(jī)器人imu坐標(biāo)系的旋轉(zhuǎn)矩陣，tc2u為相機(jī)映射機(jī)器人imu坐標(biāo)系的平移矩陣，pc為相機(jī)坐標(biāo)系下的點(diǎn)。

18、進(jìn)一步的，所述多分辨率處理模塊利用無(wú)人機(jī)平臺(tái)搭載的雙目相機(jī)獲取深度圖數(shù)據(jù)，深度圖中的每個(gè)像素點(diǎn)d(u,v)表示相機(jī)視角下的深度值，(u,v)為像素點(diǎn)在深度圖中的二維坐標(biāo)；對(duì)深度圖中每個(gè)像素點(diǎn)d(u,v)，提取其在uv平面上的八連通鄰域像素點(diǎn)的深度值，記為集合n(u,v):

19、n(u,v)＝{d(u+i,v+j)|i,j∈{-1,0,1}}

20、

21、

22、

23、

24、其中，σ2(u,v)為(u,v)像素點(diǎn)深度方差值，μ(u,v)為鄰域深度值的均值，根據(jù)深度方差σ2(u,v)獲取像素點(diǎn)對(duì)應(yīng)位置的深度變化強(qiáng)度，與深度變化閾值δ比較，對(duì)于高于δ的部分，進(jìn)行連通域r標(biāo)記，對(duì)于r利用深度信息d進(jìn)行進(jìn)一步分割，相鄰一定深度差內(nèi)的點(diǎn)劃分為同一新連通域rrk，區(qū)域中的體素?cái)?shù)量為nk，區(qū)域內(nèi)體素深度的平均值為μk，計(jì)算新的區(qū)域內(nèi)深度變化方差與閾值θ進(jìn)行比較，獲取vtarget與vuntarget；

25、vtarget區(qū)域?qū)τ诜直媛收{(diào)整的作用為：首先，根據(jù)體素的狀態(tài)，若其為占據(jù)狀態(tài)，則為每個(gè)體素分配一個(gè)唯一的分辨率塊索引，并根據(jù)分辨率變化計(jì)算新的索引數(shù)組大小，其次，計(jì)算體素的局部坐標(biāo)和細(xì)分網(wǎng)格位置，通過細(xì)分因子將其映射到更高分辨率的網(wǎng)格中，細(xì)分后的網(wǎng)格索引根據(jù)體素在細(xì)分網(wǎng)格中的位置進(jìn)行更新，以提高目標(biāo)區(qū)域的精度，同時(shí)保證不同分辨率下的高效存儲(chǔ)和訪問。

26、進(jìn)一步的，所述障礙物表面最近距離處理模塊接收機(jī)器人當(dāng)前fov內(nèi)的深度圖數(shù)據(jù)d(x,y)，每個(gè)點(diǎn)(x,y)表示深度圖中對(duì)應(yīng)像素的深度值，以及與障礙物相關(guān)的已存儲(chǔ)深度信息dstored(x,y)，對(duì)每個(gè)點(diǎn)取較小值后更新最短距離dupdated(x,y),根據(jù)機(jī)器人設(shè)定的感知范圍閾值τ，進(jìn)行區(qū)域劃分，

27、dupdated(x,y)＝min(d(x,y),dstored(x,y))

28、dupdated(x,y)>τ，vunobserved

29、dupdated(x,y)≤τ，vobserved

30、對(duì)fov外的區(qū)域，根據(jù)機(jī)器人的感知能力，標(biāo)記為vnoneed，表示需要額外調(diào)整機(jī)器人的姿態(tài)。

31、進(jìn)一步的，所述光線投影更新模塊具體包括：

32、首先，光線投射算法通過計(jì)算相機(jī)坐標(biāo)系中每個(gè)像素點(diǎn)的深度值，生成從相機(jī)位置到深度點(diǎn)的光線向量，所述光線向量的起點(diǎn)為相機(jī)位置c＝(cx,cy,cz)，終點(diǎn)為深度值所對(duì)應(yīng)的點(diǎn)e＝(ex,ey,ez)，在三維空間中沿射線r方向逐步更新經(jīng)過的體素的占據(jù)狀態(tài)，光線投影更新模塊通過為每條光線生成唯一的投影序號(hào)，確保視野內(nèi)每個(gè)體素僅被更新一次：

33、r＝c-e＝(cx-ex,cy-ey,cz-ez)

34、其次，在計(jì)算體素交點(diǎn)過程中，環(huán)境離散為體素網(wǎng)絡(luò)，每個(gè)體素大小為δx×δy×δz，體素索引為(i,j,k)，體素中心為(xi,yi,zk)，初始體素索引為(i0,j0,k0)，光線向量的方向被用于初始化各軸方向上的變量和用于控制光線沿各個(gè)方向穿越空間體素時(shí)的步進(jìn)方式，通過判斷光線在每一軸方向上的穿越順序，跟蹤光線與空間體素的交點(diǎn)，根據(jù)各軸穿越的時(shí)間模塊判斷光線向量首先穿越哪個(gè)坐標(biāo)軸方向的邊界，然后依次更新體素的坐標(biāo)，光線向量通過循環(huán)逐步遍歷經(jīng)過的體素，直到達(dá)到終點(diǎn)，在該過程中，每個(gè)經(jīng)過的體素根據(jù)光線的穿越情況更新其占據(jù)狀態(tài)，若穿越該體素則將其標(biāo)記為占據(jù)或空閑；

35、

36、

37、

38、同理；

39、所述光線投影更新模塊在三維空間中按軸方向并行處理，確保射線覆蓋范圍內(nèi)的體素信息得到更新。

40、進(jìn)一步的，所述占據(jù)信息處理模塊通過隊(duì)列數(shù)據(jù)結(jié)構(gòu)，循環(huán)執(zhí)行并推出下一個(gè)體素信息，直到隊(duì)列為空，比較每個(gè)體素的占據(jù)數(shù)值cocc與非占據(jù)數(shù)值cunocc，若cocc大于cunocc，則計(jì)算該體素的占據(jù)概率pocc并賦值；反之，則賦值為非占據(jù)概率pfree，計(jì)算得到的占據(jù)概率與預(yù)設(shè)的閾值γ進(jìn)行比較，當(dāng)占據(jù)概率超過設(shè)定的閾值時(shí)，標(biāo)記該體素為vobstacle；若占據(jù)概率低于設(shè)定閾值，則標(biāo)記為vfree；對(duì)于占據(jù)概率等于閾值的情況，標(biāo)記該體素為vresidue；進(jìn)一步地，所述占據(jù)信息處理模塊將該體素與未知空間閾值進(jìn)行比較，當(dāng)占據(jù)概率小于設(shè)定閾值β時(shí)，標(biāo)記該體素為vunknown，隨著傳感器數(shù)據(jù)的累積，vknown逐漸擴(kuò)展，形成更新的環(huán)境地圖，提供給自主探索系統(tǒng)實(shí)時(shí)可靠的障礙物和可行路徑信息；

41、

42、pfree＝1-pocc

43、p>γ，vobstacle

44、β<p<γ，vfree

45、p＝γ，vresidue

46、p≤β，vunknown。

47、進(jìn)一步的，所述歐式距離符號(hào)場(chǎng)地圖更新模塊將更新過程分解為分別沿x、y、z軸的傳播計(jì)算，首先在每個(gè)軸上計(jì)算距離場(chǎng)的更新，在每個(gè)軸方向上，計(jì)算每個(gè)體素到最近障礙物表面的距離，并通過波前傳播更新該距離，對(duì)于每個(gè)體素，按軸方向逐步傳播，從障礙物表面開始，直到更新其最短距離。

48、本發(fā)明還公開了一種地圖動(dòng)態(tài)更新系統(tǒng)的多分辨率多層次地圖構(gòu)建方法，包括如下步驟：

49、步驟1：構(gòu)建歐式距離符號(hào)場(chǎng)地圖模塊：通過第二層地圖信息，基于vfree和vobstacle空間構(gòu)建，首先應(yīng)用傳感器數(shù)據(jù)處理模塊獲取輸入，然后采用光線投影更新模塊更新占據(jù)概率和體素信息，最后基于該信息，采用歐式距離符號(hào)場(chǎng)地圖更新模塊獲取更新的esdf地圖；

50、步驟2：構(gòu)建待探地圖模塊：通過第一、四層地圖信息，基于vunknown、vtarget和vuntarget空間構(gòu)建，首先應(yīng)用傳感器數(shù)據(jù)處理模塊獲取輸入，然后采用光線投影更新模塊更新占據(jù)概率和體素信息，經(jīng)過占據(jù)信息處理模塊處理體素屬性，設(shè)定障礙物、自由空間和待探索的邊界；

51、步驟3：構(gòu)建表面距離圖模塊：通過第三層地圖信息，基于vunobserved和vobserved空間構(gòu)建，調(diào)用傳感器數(shù)據(jù)處理模塊及障礙物表面最近距離處理模塊獲取，僅更新更小值；

52、步驟4：構(gòu)建深度變化圖模塊：通過深度信息和視覺信息構(gòu)建，調(diào)用傳感器數(shù)據(jù)處理模塊及多分辨率處理模塊獲取。

53、進(jìn)一步的，四層地圖信息的存儲(chǔ)基于體素哈希方法，體素哈希方法包括體素和體素塊，體素塊是由多個(gè)體素組成的固定尺寸三維立方體，其索引通過哈希函數(shù)映射到內(nèi)存地址，使得地圖存儲(chǔ)可動(dòng)態(tài)擴(kuò)展，不預(yù)先設(shè)定地圖尺寸，且在處理過程中，先計(jì)算體素塊的索引，再計(jì)算體素索引，哈希函數(shù)表示為：

54、value＝hash(elem)+const+bit_caculator

55、其中，hash(elem)為當(dāng)前矩陣元素的哈希值，bit_caculator為位移與混合運(yùn)算，結(jié)合累積結(jié)果和用于減少哈希沖突的隨機(jī)常數(shù)const，唯一的哈希值value，兼容任意存儲(chǔ)順序。

56、本發(fā)明的一種面向自主探索的多分辨率多層次地圖信息結(jié)構(gòu)、動(dòng)態(tài)更新系統(tǒng)及構(gòu)建方法具有以下優(yōu)點(diǎn)：

57、1、具有多層次地圖信息表達(dá)，通過構(gòu)建將環(huán)境信息分層表達(dá)，從全局未知與已知空間到局部自由空間、障礙物空間，再到目標(biāo)區(qū)域，表面距離地圖，esdf地圖等，本發(fā)明提供了更適合自主探索的地圖信息，增加自主探索的環(huán)境細(xì)節(jié)，提高了地圖的有效性。

58、2、通過光線投影編號(hào)的重復(fù)性判斷以及對(duì)已觀察空間vobserved的判定，有效避免了信息的重復(fù)處理，降低了無(wú)效內(nèi)存的占用，提升了地圖構(gòu)建的效率和資源利用率。

59、3、通過對(duì)目標(biāo)空間vtarget的動(dòng)態(tài)分辨率調(diào)整，針對(duì)感興趣的物體區(qū)域提供更精細(xì)的細(xì)節(jié)呈現(xiàn)，而對(duì)于一般的光滑墻面等無(wú)特殊需求的區(qū)域保持原有分辨率，在已知地圖區(qū)域保留探索細(xì)節(jié)的同時(shí)優(yōu)化了資源分配。

60、4、通過采用體素哈希方法，消除了對(duì)預(yù)設(shè)地圖尺寸的依賴，提高了地圖構(gòu)建方法的靈活性和普適性，能夠適應(yīng)多種復(fù)雜環(huán)境的自主探索需求。

61、5、本發(fā)明充分利用和挖掘地圖信息，尤其是已知區(qū)域，探索區(qū)域細(xì)節(jié)和不同類型環(huán)境信息量，實(shí)現(xiàn)平衡探索地圖的精度和效率。