本發(fā)明涉及一種在未知封閉空間內(nèi)自主建圖的移動機器人及工作方法,屬于點云數(shù)據(jù)處理和環(huán)境建圖。
背景技術:
1、隨著人工智能和機器人技術的飛速發(fā)展,自主導航和決策系統(tǒng)在多個領域,特別是工業(yè)、礦業(yè)和交通運輸中,正逐漸展現(xiàn)出其巨大的應用潛力。在未知環(huán)境下的建圖和導航,充分利用3d雷達的點云信息至關重要,點云信息密度是指單位空間內(nèi)所包含的點云信息量,它反映了環(huán)境的復雜性和豐富程度。在未知全盲受限空間場景中,使用激光雷達建圖時,點云信息密度的分布往往是不均勻的,一些關鍵區(qū)域的點云信息密度可能較高,而一些普通區(qū)域的點云信息密度則相對較低。因此,如何準確評估和利用這些信息密度,對于機器人在該場景下的自主導航和決策至關重要。
2、現(xiàn)有的環(huán)境建圖技術大多依賴全人工控制或半自主操作來實現(xiàn)。其中,全人工控制建圖是指由操作人員手動控制移動設備,采集未知環(huán)境信息并完成建圖;半自主建圖則是通過設備自主規(guī)劃移動路徑,并結合人工干預用于提升地圖的完整性和保障設備的移動安全。然而,這些方法存在一定的應用局限性,例如在地下封閉環(huán)境中,移動設備與控制端之間難以建立穩(wěn)定可靠的通信,導致人工難以介入,從而無法順利執(zhí)行全人工或半自主條件下的環(huán)境建圖。
3、綜上所述,如何提供一種用于未知封閉環(huán)境中的移動智能體全自主建圖方案,使其不依靠人工干預即可全自動對未知環(huán)境進行精確建圖,是本發(fā)明所需研究的方向。
技術實現(xiàn)思路
1、針對上述現(xiàn)有技術存在的問題,本發(fā)明提供一種在未知封閉空間內(nèi)自主建圖的移動機器人及工作方法,能在完全不依靠人工干預的情況下實現(xiàn)在未知封閉環(huán)境中全自動路徑規(guī)劃及避障,從而對未知環(huán)境進行精確建圖。
2、為了實現(xiàn)上述目的,本發(fā)明采用的技術方案是:一種在未知封閉空間內(nèi)自主建圖的移動機器人,包括機器人主體、三維激光雷達、二維激光雷達、慣性測量單元(imu)、輪式里程計和中心處理器;
3、所述三維激光雷達通過支架裝在機器人主體頂部,且其能以支架為中心進行旋轉(zhuǎn),用于獲取機器人主體所處環(huán)境的360°實時激光掃描數(shù)據(jù);
4、所述二維激光雷達通過轉(zhuǎn)盤裝在機器人主體頂部、且處于三維激光雷達的支架前方,通過轉(zhuǎn)盤能帶動二維激光雷達360°旋轉(zhuǎn),用于在機器人主體移動過程中獲取實時掃描數(shù)據(jù),并反饋給中心處理器構建二維柵格地圖進行自主路徑規(guī)劃及避障;
5、所述慣性測量單元裝在機器人主體內(nèi)部的重心位置,用于測量機器人主體的加速度信息和角速度信息;
6、所述輪式里程計裝在機器人主體的驅(qū)動輪上,用于獲取機器人主體的車輪轉(zhuǎn)動信息和行進距離信息;
7、所述中心處理器裝在機器人主體內(nèi)部,用于獲取三維激光雷達、二維激光雷達、慣性測量單元和輪速里程計的監(jiān)測數(shù)據(jù),進行分析處理后控制機器人主體的移動路徑,直至完成自主建圖工作。
8、進一步,所述機器人主體為輪式機器人,中心處理器通過控制機器人主體前后輪的轉(zhuǎn)向角度和速度,實現(xiàn)機器人的前進、后退、轉(zhuǎn)彎動作;所述三維激光雷達為256線激光雷達。
9、上述移動機器人在未知封閉空間內(nèi)的自主建圖方法,具體步驟為:
10、步驟一、確定機器人主體實時位姿:中心處理器利用三維激光雷達和二維激光雷達對環(huán)境信息進行采集,并通過慣性測量單元和輪速里程計反饋的數(shù)據(jù)進行融合定位,使用擴展卡爾曼濾波器(ekf)將慣性測量單元和里程計數(shù)據(jù)結合,基于運動模型預測和更新機器人主體的位姿;
11、步驟二、自主建圖:從三維激光雷達采集的三維點云數(shù)據(jù)中提取特征點,特征點包括角點和平面點兩類;通過三維激光雷達掃描每幀之間的特征點匹配,利用角點匹配邊緣特征,平面點匹配平面特征,估計當前幀和上一幀之間的相對位姿變化;根據(jù)多個幀之間的特征點匹配結果,通過levenberg-marquardt優(yōu)化算法優(yōu)化三維激光雷達的全局位姿,將每幀三維激光雷達數(shù)據(jù)都插入到全局點云地圖中,構建形成完整的三維地圖,以較低頻率更新地圖;將每一幀局部點云數(shù)據(jù)轉(zhuǎn)化為全局坐標系中的點云信息,并加入全局點云地圖;
12、步驟三、確定機器人主體移動的目標點:對二維激光雷達采集的點云數(shù)據(jù)進行半徑濾波操作;構建當前點云數(shù)據(jù)的最小包圍盒并將其分割為若干體素格;計算每個體素格的點云信息密度,選擇未達到密度閾值的體素格,其中心投影點作為二維柵格地圖中局部路徑規(guī)劃目標點;
13、步驟四、機器人移動及路徑優(yōu)化:通過運動學約束、時間約束、障礙物約束和平滑約束,確保步驟三規(guī)劃的移動路徑滿足機器人運動學和避障要求;接著通過全局代價函數(shù)同時優(yōu)化路徑的幾何形狀和時間參數(shù),最小化代價函數(shù)以獲得最優(yōu)路徑;利用二維激光雷達進行環(huán)境感知和實時避障,直至機器人主體移動到步驟三計算出的目標點;
14、步驟五、重復步驟一至四持續(xù)更新全局地圖,直至完成對當前環(huán)境的自主建圖工作。
15、進一步,所述步驟一具體為:
16、步驟1.1:對數(shù)據(jù)預處理,輪式里程計從驅(qū)動輪的編碼器獲取數(shù)據(jù),轉(zhuǎn)換成線速度和角速度;慣性測量單元獲取加速度和角速度數(shù)據(jù),積分以估計速度和姿態(tài)變化;
17、步驟1.2:使用慣性測量單元的監(jiān)測數(shù)據(jù)來預測機器人下一時刻的位置、速度和姿態(tài),利用輪式里程計的測量值來校正預測狀態(tài);在狀態(tài)空間模型中對輪式里程計和慣性測量單元各自數(shù)據(jù)進行融合,其中輪式里程計被視為觀測值,更新卡爾曼濾波器的狀態(tài)估計;
18、步驟1.3:基于貝葉斯推理,結合先驗概率(預測)和觀測概率(測量)來估計狀態(tài)向量及其協(xié)方差矩陣;持續(xù)接收慣性測量單元和輪式里程計數(shù)據(jù),不斷進行預測和更新步驟,實時校正機器人的狀態(tài)估計,實現(xiàn)基于運動模型預測和更新機器人主體的位姿。
19、進一步,所述步驟二具體為:
20、步驟2.1:在三維點云數(shù)據(jù)中提取兩種特征點,角點和面點,曲率計算公式為其中k為一次掃描周期,l為三維激光雷達當前坐標系,i表示第k次掃描點云的第i個點,代表位置,第k次掃描的坐標系為l,點云中的i個點在l中的位置,s為點附近指定規(guī)則的點,包括前后左右點,0.25度間隔方向的點,為向量范數(shù),刻畫向量長度大小的一種度量,默認為2范數(shù),為度量空間,表示兩個向量之間的歐氏距離;
21、步驟2.2:基于步驟2.1中的曲率計算公式,篩選特征點,按照c在不同范圍的閾值將數(shù)據(jù)中的點分類,分成曲率特別大的點、曲率較大的點和曲率較小的點;
22、步驟2.3:在分類后的點中選擇曲率特別大的點為邊緣點,曲率較小的點為平面點;
23、步驟2.4:對連續(xù)特征幀進行匹配,建立誤差模型,利用點到線的距離和點到面的距離構造誤差函數(shù),問題變?yōu)榍蠼饧吹趉+1幀點云中的第i個點的變換點到參考點云的所有的點到線和點到面的距離的累加和為求解的問題模型;
24、步驟2.5:加入旋轉(zhuǎn)r和平移t,第k+1幀原始數(shù)據(jù)點云的第i個點和變化后的點云之間的關系為:
25、步驟2.6:由步驟2.4和步驟2.5推導loss函數(shù)為其中為第k+1次掃描點云中第i個點的位置向量,為第k+1次掃描點云中第i個點的位置向量經(jīng)過旋轉(zhuǎn)和平移后的位置向量;
26、步驟2.7:掃描過程中,用恒定的角速度和線速度對三維激光雷達運動進行建模,在掃描范圍內(nèi)對不同時間接收到的點的姿態(tài)變換進行線性插值;
27、步驟2.8:設t為時間戳,記tk+1是掃描k+1的開始時間,設為激光雷達在[tk+1,t]之間進行的姿態(tài)變換,包含三維激光雷達在6自由度下的剛體運動,其中tx,ty,tz分別為沿l的x,y,z軸平移,θx,θy,θz為三個對應軸的旋轉(zhuǎn)角,遵循右手法則,給定一個點i,i∈pk+1,設ti為其時間戳,并設為[tk+1,ti]之間的位姿變換,假設雷達為恒定的角速度和線速度運動,則能通過進行線性插值公式計算;
28、步驟2.9:使用非線性優(yōu)化方法進行求解;
29、步驟2.10:累計一定數(shù)目點云數(shù)據(jù)開始建圖,以較低頻率將點云數(shù)據(jù)融入到世界地圖中,精確估計激光在世界坐標系中的位姿。
30、進一步,所述步驟三具體為:
31、步驟3.1:對當前二維激光雷達采集的點云數(shù)據(jù)構造k-d樹,建立點云拓撲關系;求點云中任意一點鄰域范圍內(nèi)鄰近點個數(shù),判斷鄰近點個數(shù)是否小于判斷閾值,若小于則認為噪聲點并去除,重復上述步驟直至點云中所有的點都處理完畢;
32、步驟3.2:以map坐標系為參考,利用pcl遍歷點云數(shù)據(jù)中的每一個點,并依次進行比較x,y,z三個值大??;
33、步驟3.3:由步驟3.2得出當前局部點云數(shù)據(jù)x,y,z三個方向上的最大值記為xmax,ymax和zmax,最小值記為xmin,ymin和zmin;
34、步驟3.4:根據(jù)步驟3.3得到各坐標軸上的最大及最小值,計算最小包圍盒的邊長lx,ly,lz,計算公式為
35、步驟3.5:將三個坐標軸方向上的邊長劃分為m1,m2,m3份,實現(xiàn)將最小包圍盒分割成n個體素格,令每個體素格的體積為v=a·b·c,式中a,b,c為x方向,y方向,z方向體素格的邊長;
36、步驟3.6:設點云數(shù)據(jù)中第p個點的坐標為(xp,yp,zp),對每個體素格進行標號,令q=i·4+j+k,其中表示向下取整符號,點(xp,yp,zp)位于第q個體素格中;
37、步驟3.7:遍歷點云數(shù)據(jù)中的每一個點,按步驟3.6將其分配到對應的體素格中,設第q個體素格中的點的總個數(shù)為wq,第q個體素格的點云密度為其中v=a·b·c;
38、步驟3.8:通過與設定的點云密度閾值比較,假設點云密度最小的第q個區(qū)域的點云密度未達到閾值要求,則用第q個體素格的中心作為本次局部路徑規(guī)劃的目標點。
39、進一步,所述步驟四具體為:
40、步驟4.1:通過二維激光雷達檢測到障礙物的位置,并將其映射到地圖中,將障礙物所在的柵格賦予高代價值;
41、步驟4.2:障礙物周圍的一定范圍會被標記為膨脹區(qū)域;利用局部代價地圖實時更新障礙物的位置信息;
42、步驟4.3:將全局路徑規(guī)劃器生成的初始軌跡作為輸入,將初始運動路線離散為一系列的路徑點,為每個路徑點分配一個時間分段;
43、步驟4.4:每個路徑點由xi=(xi,yi,θi)t表示,其中xi和yi是路徑點的空間位置,ti是時間信息;運動空間內(nèi)機器人主體所在點的位置姿態(tài)坐標序列由q={xi}i=0,…,n,n∈n表示;
44、步驟4.5:相鄰兩個路徑點之間的時間間隔δti=ti+1-ti,代表機器人在這兩個路徑點之間的移動時間,時間序列由τ={δti}i=0,…,n,n∈n表示,n表示控制點的個數(shù);
45、步驟4.6:將上述控制點的時間序列和位置序列合并得到b=(q,τ),n表示控制點的個數(shù),在時間序列和控制點序列兩個方面利用加權的方式進行多目標優(yōu)化;
46、步驟4.7:通過代價函數(shù)最小化路徑點之間的角度變化,約束路徑點之間的平滑性,其中i=0,…,n,n∈n,n表示控制點的個數(shù);
47、步驟4.8:通過代價函數(shù)最小化路徑的總時間,保持時間的均勻分布,其中是路徑點之間的平均時間間隔,i=0,…,n,n∈n,n表示控制點的個數(shù);
48、步驟4.9:通過最小路徑點到障礙物的距離代價函數(shù)確保路徑點遠離障礙物,其中d(xi,o)是路徑點xi到障礙物的距離,i=0,…,n,n∈n,n表示控制點的個數(shù);
49、步驟4.10:通過代價函數(shù)保持機器人在路徑上的速度和加速度滿足其運動學限制,其中i=0,…,n,n∈n,n表示控制點的個數(shù);
50、步驟4.11:利用獲得b的最優(yōu)解,找到最優(yōu)的路徑點位置和時間分布,式中b*表示最優(yōu)結果,f(b)為綜合考慮多種目標的總代價,γk表示每一個優(yōu)化目標的代價函數(shù)的權值,k=1,2,3,4;
51、步驟4.12:機器人主體按照上述確定的路徑、速度及加速度數(shù)據(jù)進行移動,直至機器人主體移動到步驟三計算出的目標點。
52、與現(xiàn)有技術相比,本發(fā)明具有如下優(yōu)點:
53、1、本發(fā)明通過構建場景信息密度的數(shù)學模型,得到了信息密度與機器人主體的導航和地圖構建性能之間的關系,實現(xiàn)了對未知環(huán)境空間點云的稀疏程度的量化,實現(xiàn)移動機器人在未知環(huán)境下的自主導航和決策。
54、2、本發(fā)明基于點云信息密度確定目標點,并利用三維激光雷達進行未知封閉環(huán)境的三維點云地圖的構建,基于二維激光雷達在二維柵格地圖上進行路徑規(guī)劃和導航,實現(xiàn)移動機器人在未知環(huán)境自主建圖,能在完全不依靠人工干預的情況下實現(xiàn)在未知封閉環(huán)境中全自動路徑規(guī)劃及避障,從而對未知環(huán)境進行精確建圖,且效率較高。