趙世鑫，王春彥，王佳楠，李春雨

（北京理工大學(xué)宇航學(xué)院，北京 100081）

1 引 言

地面移動(dòng)機(jī)器人具有運(yùn)動(dòng)穩(wěn)定性高、承載能力強(qiáng)、機(jī)械結(jié)構(gòu)簡(jiǎn)單等特點(diǎn)，能夠在復(fù)雜惡劣環(huán)境中代替人類執(zhí)行特定任務(wù)，已經(jīng)成為當(dāng)前的研究熱點(diǎn)[1-2]。移動(dòng)機(jī)器人在樓宇巡邏、地面清潔、倉(cāng)儲(chǔ)貨運(yùn)等場(chǎng)景中都具有巨大的應(yīng)用價(jià)值，可以極大豐富人們的生活體驗(yàn)。自主導(dǎo)航、自主規(guī)劃以及地圖構(gòu)建共同構(gòu)成了移動(dòng)機(jī)器人系統(tǒng)的核心，是移動(dòng)機(jī)器人實(shí)現(xiàn)智能化、自主化的前提。

常見(jiàn)的移動(dòng)機(jī)器人通常采用多傳感器組合導(dǎo)航的方式，Zheng等[3]采用激光雷達(dá)、慣性測(cè)量單元以及GPS 多傳感器融合的方法解決輪式機(jī)器人的導(dǎo)航問(wèn)題。陳智偉等[4]利用激光雷達(dá)、相機(jī)和慣性測(cè)量單元（Inertial Measurement Unit，IMU）組成車載多傳感器系統(tǒng)獲取環(huán)境信息，通過(guò)訓(xùn)練RBF 神經(jīng)網(wǎng)絡(luò)對(duì)環(huán)境進(jìn)行建模。李中道等[5]提出一種基于超寬帶與激光雷達(dá)的融合定位算法，算法基于粒子濾波對(duì)兩個(gè)傳感器的數(shù)據(jù)進(jìn)行互補(bǔ)融合解算，以提高定位精度。上述方法使用多傳感器進(jìn)行組合導(dǎo)航雖然有助于提高系統(tǒng)定位精度，但同時(shí)也增加了系統(tǒng)的復(fù)雜性，對(duì)機(jī)載計(jì)算機(jī)的運(yùn)算能力提出更高的要求。

根據(jù)使用傳感器的類型，常用的定位方法大致可以分為兩類，基于激光雷達(dá)的定位方法和基于視覺(jué)的定位方法[6]。Gmapping[7]、hector slam[8]等采用單線激光雷達(dá)作為輸入，是一種二維SLAM 算法。LOAM[9]、LeGO-LOAM[10]、LIO-SAM[11]、LOAM_livox[12]等主要針對(duì)多線激光雷達(dá)的三維SLAM 算法，可以輸出傳感器在三維空間中的位姿。VINS-mono[13]、Open-VINS[14]等融合視覺(jué)信息與 IMU 信息實(shí)現(xiàn)定位。ORB-SLAM2[15]是一種基于ORB 特征點(diǎn)的幾何SLAM 框架，支持單目、雙目以及RGB-D 相機(jī)。

在移動(dòng)機(jī)器人的路徑規(guī)劃中，A*算法具有廣泛應(yīng)用。許多學(xué)者基于A*算法提出了不同的改進(jìn)方法，Ju 等[16]提出了一種基于人工勢(shì)場(chǎng)法和A*算法的融合算法，提高了A*算法的動(dòng)態(tài)避障能力。張志文等[17]使用JPS 算法提升A*算法節(jié)點(diǎn)擴(kuò)展效率，利用貝塞爾曲線對(duì)規(guī)劃路徑進(jìn)行平滑處理，提高了A*算法的規(guī)劃能力。楊瑤等[18]將車輛的運(yùn)動(dòng)學(xué)模型融入A*算法的搜索過(guò)程，采用自由邊界三次插值平滑路徑，生成更加符合車輛運(yùn)動(dòng)特性的軌跡。上述方法都是在二維平面進(jìn)行規(guī)劃，沒(méi)有充分利用三維環(huán)境信息。

本文提出了一種基于單一傳感器的低成本方案解決移動(dòng)機(jī)器人在室內(nèi)未知環(huán)境中自主規(guī)劃與地圖構(gòu)建問(wèn)題。該方案選取廉價(jià)的RGB-D 相機(jī)作為環(huán)境感知傳感器，利用RGB-D 相機(jī)提供的環(huán)境深度信息和RGB 圖像信息實(shí)現(xiàn)自主定位，實(shí)時(shí)解算移動(dòng)機(jī)器人的位置和姿態(tài)，無(wú)需額外的里程計(jì)(車輪編碼器)或者IMU 等傳感器。此外，利用RGB-D 相機(jī)采集的點(diǎn)云數(shù)據(jù)，可以構(gòu)建未知環(huán)境的三維地圖，完成對(duì)未知環(huán)境的探測(cè)。為提高移動(dòng)機(jī)器人在未知環(huán)境中運(yùn)動(dòng)軌跡的安全性和平穩(wěn)性，本文基于A*算法提出了一種適用于二維平面運(yùn)動(dòng)物體的三維規(guī)劃方法。首先，根據(jù)RGB-D 相機(jī)提供的點(diǎn)云數(shù)據(jù)構(gòu)建局部3D 占據(jù)柵格地圖，在三維空間中進(jìn)行碰撞檢測(cè)，提高規(guī)劃路徑的安全性；其次，改進(jìn)A*算法節(jié)點(diǎn)擴(kuò)展方式，將節(jié)點(diǎn)的搜索過(guò)程轉(zhuǎn)移到狀態(tài)空間，提高規(guī)劃路徑的平滑性；最后，基于B 樣條曲線構(gòu)建優(yōu)化方程，進(jìn)一步提升初始路徑的安全性和平滑性，保障移動(dòng)機(jī)器人在未知的環(huán)境中平穩(wěn)安全移動(dòng)。

本文設(shè)計(jì)并實(shí)現(xiàn)了一種基于單一傳感器的自主系統(tǒng)，該系統(tǒng)能夠感知非結(jié)構(gòu)化的環(huán)境，并且提供足夠精度的自定位信息。系統(tǒng)采用體積小、重量輕、低功耗的Intel NUC 作為主計(jì)算單元，易于部署到小型自主移動(dòng)平臺(tái)。此外，NUC 的運(yùn)算能力支持?jǐn)U展更多的功能，如物體的檢測(cè)與識(shí)別等計(jì)算機(jī)視覺(jué)任務(wù)。系統(tǒng)的軟件框架基于機(jī)器人操作系統(tǒng)（Robot Operating System,ROS）搭建，具有較強(qiáng)的靈活性，可以兼容不同的硬件設(shè)備，使得系統(tǒng)支持不同的硬件解決方案成為可能。此外，本文提出的軟件算法求解速度快，不需要消耗太多的計(jì)算資源，易于部署到不同的運(yùn)算平臺(tái)。本文提供了包括軟件部分和硬件部分在內(nèi)的完整系統(tǒng)框架，以及該系統(tǒng)在現(xiàn)實(shí)環(huán)境中的應(yīng)用效果。

與現(xiàn)有的工作相比，本文提出的系統(tǒng)采用單一傳感器作為輸入，使用低成本的相機(jī)實(shí)現(xiàn)移動(dòng)機(jī)器人自主系統(tǒng)的構(gòu)建。為解決無(wú)人系統(tǒng)在未知環(huán)境的自主避障問(wèn)題，改進(jìn)了傳統(tǒng)的A*算法，提高了系統(tǒng)的環(huán)境適應(yīng)能力。此外，本文還提出了一些規(guī)劃策略，以提高系統(tǒng)的實(shí)時(shí)性。本文的貢獻(xiàn)總結(jié)如下。

（1）本文提出了一種可以在未知環(huán)境中實(shí)現(xiàn)自主規(guī)劃與地圖構(gòu)建的低成本解決方案。其核心部件只有一個(gè)感知環(huán)境信息的RGB-D 相機(jī)以及一臺(tái)提供計(jì)算服務(wù)的機(jī)載計(jì)算機(jī)。

（2）本文提出了一種適用于二維平面運(yùn)動(dòng)物體的三維規(guī)劃方法，充分利用三維環(huán)境地圖，實(shí)現(xiàn)三維避障。

（3）本文提供了一套基于開(kāi)源軟件的系統(tǒng)框架，包含軟件架構(gòu)和硬件架構(gòu)，便于快速部署。

2 系統(tǒng)概況

系統(tǒng)的軟件框架基于ROS 搭建，整套系統(tǒng)采用模塊化的設(shè)計(jì)理念，按照功能劃分為不同的模塊單獨(dú)進(jìn)行開(kāi)發(fā)，再按照相應(yīng)的需求將不同的功能模塊進(jìn)行組合，完成整套系統(tǒng)開(kāi)發(fā)，如圖1所示?；赗OS 的特性，將每個(gè)功能模塊設(shè)計(jì)為一個(gè)節(jié)點(diǎn)，節(jié)點(diǎn)的運(yùn)行是由ROS Master 統(tǒng)一進(jìn)行調(diào)度和分配，節(jié)點(diǎn)之間的通信采用ROS 的話題機(jī)制，發(fā)布者將消息發(fā)布到指定的話題中，接收方訂閱相同的話題，雙方即可建立通信鏈路，完成信息傳輸。

圖1 系統(tǒng)軟件框架Fig.1 System software framework

系統(tǒng)使用了部分基于 C++編寫的開(kāi)源代碼包，驅(qū)動(dòng)包 realsense-ros[19]用于處理 Realsense D455 RGB-D 相機(jī)的數(shù)據(jù)，將其轉(zhuǎn)化為ROS 標(biāo)準(zhǔn)信息類型，通過(guò)ROS Topic 的形式發(fā)布。定位模塊基于ORB-SLAM2 算法設(shè)計(jì)，定位模塊訂閱ROS 相關(guān)的話題，獲取RGB 圖像和深度圖像，計(jì)算相機(jī)當(dāng)前的位置和姿態(tài)，輸出位姿矩陣Tcw∈SE(3)。Tcw包含旋轉(zhuǎn)矩陣和平移向量，位姿轉(zhuǎn)換模塊將矩陣Tcw轉(zhuǎn)換為ROS 標(biāo)準(zhǔn)格式的里程計(jì)信息（ nav_msgs/Odometry） 和位姿信息(geometry_msgs/PoseStamped)供系統(tǒng)其他模塊使用。局部建圖模塊和軌跡規(guī)劃模塊基于Fast-Planner[20]框架搭建，局部建圖模塊接收RGB-D 相機(jī)的深度圖像和里程計(jì)信息，生成局部3D 占據(jù)柵格地圖用于路徑搜索，同時(shí)生成ESDF地圖用于軌跡優(yōu)化。由于傳感器的感知范圍有限，因此采用局部規(guī)劃的策略，系統(tǒng)只在局部地圖中進(jìn)行軌跡規(guī)劃，到達(dá)局部地圖的邊緣則停止規(guī)劃，系統(tǒng)間隔固定的時(shí)間周期調(diào)用規(guī)劃器以更新軌跡，隨著機(jī)器人的移動(dòng)逐步規(guī)劃到目標(biāo)點(diǎn)。運(yùn)動(dòng)控制模塊結(jié)合期望軌跡信息和當(dāng)前里程計(jì)信息計(jì)算誤差量，采用PD 控制器計(jì)算期望速度和角速度，實(shí)現(xiàn)軌跡跟蹤，并將速度指令轉(zhuǎn)換為Twist消息類型（geometry_msgs/Twist），以ROS Topic的方式發(fā)布，使得系統(tǒng)可以快速地移植到任意一種支持Twist 控制指令的移動(dòng)平臺(tái)上。

3 系統(tǒng)關(guān)鍵算法分析

3.1 路徑規(guī)劃算法

路徑規(guī)劃是指移動(dòng)機(jī)器人按照某一性能指標(biāo)（如距離、時(shí)間等）搜索一條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)或次優(yōu)路徑。常用的搜索算法有Dijkstra、A*、RRT、RRT*等，但是利用這些算法得到的路徑往往是非光滑的。此外，傳統(tǒng)的規(guī)劃算法僅在傳感器掃描得到二維平面上搜索，因此只能避開(kāi)搜索平面內(nèi)的障礙物，對(duì)于平面上方或者下方的障礙物難以有效規(guī)避。本系統(tǒng)采用的環(huán)境感知傳感器為RGB-D 相機(jī)，可以建立機(jī)器人周圍環(huán)境的三維地圖。因此，本文對(duì)傳統(tǒng)的A*算法進(jìn)行改進(jìn)，優(yōu)化節(jié)點(diǎn)擴(kuò)展策略，使得搜索的路徑更加平滑，同時(shí)在三維空間中判斷路徑是否發(fā)生碰撞，實(shí)現(xiàn)三維避障。

傳統(tǒng)A*算法在擴(kuò)展節(jié)點(diǎn)時(shí)，以當(dāng)前節(jié)點(diǎn)為中心，向著8 個(gè)方向探索，尋找與當(dāng)前節(jié)點(diǎn)在幾何上相鄰的節(jié)點(diǎn)，節(jié)點(diǎn)與節(jié)點(diǎn)之間以折線段相連，規(guī)劃的路徑缺乏平滑性，如圖2(a)所示。針對(duì)上述問(wèn)題，基于Dolgov 等[21]提出的方法改進(jìn)A*算法的節(jié)點(diǎn)擴(kuò)展方式。假設(shè)移動(dòng)機(jī)器人的狀態(tài)向量為x(t)=[P,V]，則狀態(tài)空間模型為

圖2 節(jié)點(diǎn)擴(kuò)展方式對(duì)比Fig.2 Comparison of node expansion modes

其中，

式中，I2為二階單位矩陣；P=[px,py]T表示移動(dòng)機(jī)器人在慣性系下的位置；V=[vx,vy]T表示移動(dòng)機(jī)器人在慣性系下的速度；輸入u=[ax,ay]T為移動(dòng)機(jī)器人的加速度，其中ax,ay∈[-amax,amax]。

節(jié)點(diǎn)擴(kuò)展時(shí)，在每個(gè)坐標(biāo)軸方向上分別離散化輸入ud為將當(dāng)前的狀態(tài)x(0)以及輸入ud∈U帶入系統(tǒng)狀態(tài)方程的解

可以得到在狀態(tài)空間相鄰的節(jié)點(diǎn)。改進(jìn)后的A*算法節(jié)點(diǎn)擴(kuò)展過(guò)程如圖2(b)所示。

傳統(tǒng)A*算法中常用的代價(jià)函數(shù)有曼哈頓距離、對(duì)角線距離以及歐幾里得距離等，這些代價(jià)函數(shù)都是計(jì)算幾何空間的距離，考慮到改進(jìn)后的A*算法節(jié)點(diǎn)擴(kuò)展是在狀態(tài)空間中進(jìn)行，因此采用文獻(xiàn)[22]中的方法計(jì)算路徑的代價(jià)，找到一條在時(shí)間和控制成本上最優(yōu)的路徑。定義代價(jià)函數(shù)為

式中，ρ為常值。

與傳統(tǒng)A*算法類似，我們使用gc表示從起始狀態(tài)xs到當(dāng)前狀態(tài)xc的實(shí)際代價(jià)，假設(shè)最優(yōu)路徑共包含J段，則gc的計(jì)算公式如下

式中，udj表示第j段路徑的輸入值，τ表示第j段路徑的持續(xù)時(shí)間。

預(yù)估代價(jià)對(duì)于提升A*算法的搜索效率是非常重要的。根據(jù)當(dāng)前節(jié)點(diǎn)的狀態(tài)信息xc以及目標(biāo)點(diǎn)的狀態(tài)信息xg，采用龐特里亞金極小值原理求解式子(3)，可以得到最優(yōu)控制輸入u*(t)以及最小代價(jià)J*(T)。

式中，pμc,vμc表示當(dāng)前節(jié)點(diǎn)的位置和速度，pμg,vμg表示目標(biāo)節(jié)點(diǎn)的位置和速度。根據(jù)式(5)可以看出，J*(T)只與T有關(guān)，因此可以對(duì)T求導(dǎo)得到最優(yōu)的代價(jià)。

將αμ,βμ的值帶入式(6)，可以得到一系列極值點(diǎn)，挑選出符合運(yùn)動(dòng)學(xué)模型且代價(jià)最小的Topt，并將J*(Topt)作為預(yù)估代價(jià)hc。

傳統(tǒng)規(guī)劃算法只在二維平面進(jìn)行搜索。如圖3 所示，藍(lán)色平面為傳感器掃描平面，由于移動(dòng)機(jī)器人自身有一定的高度且傳感器的安裝位置也有一定的高度，圖3(a)中紅色區(qū)域可能與機(jī)器人發(fā)生碰撞，因此二維平面的搜索難以保證機(jī)器人的安全。針對(duì)上述問(wèn)題，本文改進(jìn)了傳統(tǒng)A*算法的碰撞檢測(cè)部分。利用系統(tǒng)創(chuàng)建的三維環(huán)境地圖，不僅在搜索平面內(nèi)進(jìn)行碰撞檢測(cè)，還根據(jù)機(jī)器人自身高度在垂直方向上進(jìn)行碰撞檢測(cè)，如圖3(b)所示。系統(tǒng)在垂直方向任一點(diǎn)檢測(cè)到碰撞，則重新進(jìn)行規(guī)劃，以避開(kāi)障礙物。

圖3 不同規(guī)劃方法對(duì)比示意圖Fig.3 Comparison of different planning methods

3.2 軌跡優(yōu)化算法

由于控制輸入的離散化，搜索得到的路徑不一定是最優(yōu)的。此外，由于搜索過(guò)程沒(méi)有考慮與障礙物之間的距離信息，得到的路徑可能距離障礙物較近，因此需要對(duì)路徑進(jìn)行優(yōu)化?；谖墨I(xiàn)[22]中提出的方法，利用B 樣條曲線局部可調(diào)整的特性，結(jié)合約束條件，生成平滑、安全、動(dòng)態(tài)可行的軌跡。

B 樣條曲線是由它的階次數(shù)pb、控制點(diǎn){Q0,Q1,…,QN}以及節(jié)點(diǎn)向量 [t0,t1,…,tM]構(gòu)成，其中Qi∈?3,tm∈?。首先將路徑規(guī)劃算法搜索到的幾何路徑轉(zhuǎn)化為B 樣條曲線，然后對(duì)B 樣條曲線控制點(diǎn)的子集進(jìn)行優(yōu)化?？?/p>

代價(jià)函數(shù)定義為

式中，fs是平滑代價(jià)函數(shù)，fc是碰撞代價(jià)函數(shù)，fv是速度軟約束函數(shù)，λs,λc,λv為權(quán)重系數(shù)。

平滑代價(jià)函數(shù)主要是利用軌跡的幾何信息進(jìn)行平滑，此處選擇彈性帶代價(jià)[23]，平滑代價(jià)函數(shù)為

碰撞代價(jià)函數(shù)為障礙物在每個(gè)控制點(diǎn)上的斥力的總和，軌跡碰撞代價(jià)函數(shù)定義如下

式中，M代表控制點(diǎn)Qi在Z軸上平移得到的點(diǎn)的集合，d(Qi,j)代表控制點(diǎn)Qi,j與其鄰近障礙物之間的距離，可通過(guò)查詢ESDF 地圖[24]獲得，F(xiàn)c是一個(gè)可微分函數(shù)，其值與閾值dthr以及距離函數(shù)d(Qi,j)有關(guān)，表達(dá)式如下

速度軟約束函數(shù)主要對(duì)軌跡中超過(guò)最大允許值vmax的速度項(xiàng)進(jìn)行懲罰，速度軟約束函數(shù)定義如下

其中

3.3 地圖構(gòu)建算法

3.3.1 局部地圖

局部地圖采用3D 占據(jù)柵格地圖模型，如圖4所示。占據(jù)柵格地圖將環(huán)境分為一系列大小相同的立方體，每個(gè)立方體存儲(chǔ)著環(huán)境的三種狀態(tài)信息，分別為占用、空閑以及未知。整個(gè)地圖在初始化時(shí)將所有的立方體的狀態(tài)設(shè)置為未知，隨著傳感器的掃描進(jìn)行更新，當(dāng)環(huán)境中某處存在障礙物時(shí)，其對(duì)應(yīng)的立方體的狀態(tài)變?yōu)檎加?，此時(shí)代表移動(dòng)機(jī)器人無(wú)法穿過(guò)此區(qū)域；環(huán)境中空曠的部分對(duì)應(yīng)立方體的狀態(tài)為空閑，表示移動(dòng)機(jī)器人可以安全通過(guò)。地圖的分辨率可以根據(jù)實(shí)際需求進(jìn)行設(shè)置。立方體的狀態(tài)信息采用概率值表示，概率值為0～1，概率值越接近0，空閑的可能性越大，概率值越接近1，占用的可能性越大；概率值為0.5 表示該單元的狀態(tài)未知。

圖4 3D 占據(jù)柵格地圖Fig.4 3D occupancy grid map

3.3.2 全局地圖

ORB-SLAM2 雖然具有構(gòu)建全局地圖的功能，但是構(gòu)建的地圖較為稀疏，且地圖構(gòu)建效率低。本文只使用ORB-SLAM2 的定位功能；局部地圖采用的3D 占據(jù)柵格地圖是安裝設(shè)定的分辨率對(duì)環(huán)境均勻切分，優(yōu)點(diǎn)為可以使用坐標(biāo)索引的方法查詢?cè)搯卧臓顟B(tài)信息，提高軌跡規(guī)劃的效率。缺點(diǎn)是對(duì)存儲(chǔ)空間的消耗非常大，不適于構(gòu)建大范圍的地圖。因此，全局地圖構(gòu)建采用OctoMap[25]。OctoMap 是一種高效表示3D 環(huán)境的方式，它可以根據(jù)激光雷達(dá)或者RGB-D 相機(jī)的測(cè)量值，實(shí)時(shí)構(gòu)建并更新三維環(huán)境地圖。OctoMap 本質(zhì)上是一個(gè)基于Octree 數(shù)據(jù)結(jié)構(gòu)的三維空間網(wǎng)格映射框架。Octree 是一種包含多個(gè)節(jié)點(diǎn)的分層結(jié)構(gòu)，這些節(jié)點(diǎn)（也稱為體素）是空間中的立方體，每個(gè)節(jié)點(diǎn)可以包含8 個(gè)或者零個(gè)子節(jié)點(diǎn)，當(dāng)節(jié)點(diǎn)的8 個(gè)子節(jié)點(diǎn)都被占用或都空閑時(shí)，當(dāng)前節(jié)點(diǎn)將不再劃分子節(jié)點(diǎn)，僅對(duì)自身狀態(tài)進(jìn)行標(biāo)記。其他情況下，每個(gè)節(jié)點(diǎn)被遞歸地劃分為8個(gè)子節(jié)點(diǎn)，同時(shí)分別計(jì)算子節(jié)點(diǎn)的占用概率值，重復(fù)上述操作，直到達(dá)到預(yù)設(shè)的最小節(jié)點(diǎn)尺寸。

定義子節(jié)點(diǎn)n被占據(jù)的概率為P(n|z1:t)，傳感器的測(cè)量值為z1:t，P(n|z1:t)的計(jì)算方法如下

式中，zt為傳感器在t時(shí)刻的測(cè)量值；P(n) 為先驗(yàn)概率，統(tǒng)一設(shè)定為0.5；P(n|z1:t-1)為t-1時(shí)刻的估計(jì)值；P(n|zt)表示在給定測(cè)量值z(mì)t的條件下節(jié)點(diǎn)n被占據(jù)的概率。

為簡(jiǎn)化計(jì)算，對(duì)式(13)進(jìn)行Logit 變換

其中

從式(14)可以看出，每次更新地圖時(shí)只需計(jì)算當(dāng)前測(cè)量值z(mì)t的對(duì)數(shù)幾率值L(n|zt)，顯著降低了計(jì)算量。為了應(yīng)對(duì)傳感器噪聲和環(huán)境的變化帶來(lái)的影響，設(shè)定占用率估計(jì)值的上下限

式中，lmin和lmax表示對(duì)數(shù)幾率值的上限和下限。

修改后的更新公式限制了改變節(jié)點(diǎn)狀態(tài)所需的更新次數(shù)，保證算法可以快速濾除傳感器噪聲以及環(huán)境變化帶來(lái)的影響。

4 系統(tǒng)實(shí)現(xiàn)

圖5為本文所設(shè)計(jì)系統(tǒng)的硬件框架，系統(tǒng)大致可以分為4 部分：具有環(huán)境感知能力的傳感器；負(fù)責(zé)地圖建立、導(dǎo)航規(guī)劃的機(jī)載計(jì)算機(jī)；具備移動(dòng)能力的差分輪式機(jī)器人以及用于遠(yuǎn)程監(jiān)視、數(shù)據(jù)收集及顯示的遠(yuǎn)程計(jì)算機(jī)。各個(gè)模塊之間通過(guò)USB 以及局域網(wǎng)傳輸數(shù)據(jù)，本節(jié)將詳細(xì)介紹系統(tǒng)的組成部分。

圖5 系統(tǒng)硬件框架Fig.5 System hardware framework

機(jī)載計(jì)算機(jī)采用Intel NUC10i7FNH 微型計(jì)算機(jī)，如圖6所示。NUC10i7FNH 搭載第10 代智能Intel?酷睿i7 處理器，擁有六核十二線程，基準(zhǔn)頻率1.1GHz，具有睿頻加速技術(shù)，最大加速頻率4.7GHz。系統(tǒng)中涉及的定位、規(guī)劃以及建圖算法都運(yùn)行在機(jī)載計(jì)算機(jī)上，使得移動(dòng)機(jī)器人成為一個(gè)不依賴外部資源的獨(dú)立系統(tǒng)。機(jī)載計(jì)算機(jī)運(yùn)行Ubuntu18 系統(tǒng)且安裝了ROS，NUC 提供的強(qiáng)大計(jì)算能力以及ROS 的靈活性，使得現(xiàn)有系統(tǒng)易于實(shí)現(xiàn)功能擴(kuò)展。

圖6 NUC10i7FNH 迷你電腦Fig.6 NUC10i7FNH mini computer

環(huán)境感知傳感器采用Realsense D455 RGB-D相機(jī)，如圖7所示。Realsense D455 擁有一個(gè)紅外投射器、一對(duì)雙目相機(jī)以及一個(gè)RGB 相機(jī)，深度測(cè)量基于雙目測(cè)距原理，紅外投射器用于輔助測(cè)距，通過(guò)投射不可見(jiàn)的固定的紅外紋理樣式，提高相機(jī)在缺少紋理的環(huán)境中的測(cè)距精度。Realsense D455 輸出的數(shù)據(jù)流通過(guò)USB 線傳輸?shù)綑C(jī)載計(jì)算機(jī)。

圖7 Realsense D455 RGB-D 相機(jī)Fig.7 Realsense D455 RGB-D camera

移動(dòng)機(jī)器人擁有6 個(gè)獨(dú)立驅(qū)動(dòng)的輪子，如圖8所示，6 個(gè)輪子分為左右兩組，采用差速驅(qū)動(dòng)模式運(yùn)行。底盤控制器選用STM32F103ZET6 作為主控芯片，該芯片基于超低功耗的 ARM Cortex-M3 處理器內(nèi)核，工作頻率最高可達(dá)72MHz，而且具有豐富的硬件資源和外設(shè)接口，完全能夠滿足移動(dòng)機(jī)器人底層控制需求。底層控制器利用自身的MAC 口接入局域網(wǎng)，基于UDP（User Datagram Protocol）協(xié)議與機(jī)載計(jì)算機(jī)進(jìn)行數(shù)據(jù)交互。

圖8 移動(dòng)機(jī)器人底盤Fig.8 Mobile robot chassis

遠(yuǎn)程計(jì)算機(jī)用于實(shí)時(shí)監(jiān)控系統(tǒng)的狀態(tài)，其不參與移動(dòng)機(jī)器人自主規(guī)劃系統(tǒng)的任何運(yùn)算，也不是系統(tǒng)的重要組成部分。遠(yuǎn)程計(jì)算機(jī)的主要任務(wù)是通過(guò)安全外殼協(xié)議（Secure Shell，SSH）與機(jī)載計(jì)算機(jī)建立通信鏈路，收集并顯示系統(tǒng)數(shù)據(jù)。在本文設(shè)計(jì)的系統(tǒng)中，遠(yuǎn)程計(jì)算機(jī)為運(yùn)行Linux系統(tǒng)的個(gè)人筆記本電腦。此外，系統(tǒng)還配備了樂(lè)迪T8FB 航模遙控器，遙控器可以取代機(jī)器人的自動(dòng)規(guī)劃系統(tǒng)，手動(dòng)控制機(jī)器人的運(yùn)動(dòng)。遙控器作為一種安全措施，只在緊急情況下使用。

實(shí)驗(yàn)測(cè)試所用的移動(dòng)機(jī)器人平臺(tái)如圖9所示，RGB-D 相機(jī)安裝于機(jī)器人頂端，可以保持寬廣的視野；機(jī)載計(jì)算機(jī)位于機(jī)器人中部，通過(guò)USB 線連接RGB-D 相機(jī)獲取環(huán)境信息，通過(guò)網(wǎng)線連接通信模塊接入局域網(wǎng)；通信模塊采用TP-LINK AC1200 路由器，通信模塊用于構(gòu)建通信局域網(wǎng)，機(jī)器人底盤控制器、機(jī)載計(jì)算機(jī)以及遠(yuǎn)程計(jì)算機(jī)共同接入局域網(wǎng)以實(shí)現(xiàn)信息交互；機(jī)器人底盤控制器基于UDP 協(xié)議和機(jī)載計(jì)算機(jī)實(shí)現(xiàn)通信，接收機(jī)載計(jì)算機(jī)的控制指令，同時(shí)反饋移動(dòng)機(jī)器人的狀態(tài)信息；機(jī)載計(jì)算機(jī)與遠(yuǎn)程計(jì)算機(jī)基于ROS 分布式通信協(xié)議實(shí)現(xiàn)信息交互，遠(yuǎn)程計(jì)算機(jī)可向機(jī)載計(jì)算機(jī)發(fā)布指令，讀取并顯示系統(tǒng)創(chuàng)建的地圖、移動(dòng)機(jī)器人的運(yùn)行軌跡等；電源模塊穩(wěn)定輸出12V 直流電壓，為機(jī)載計(jì)算機(jī)和通信模塊提供穩(wěn)定電源。表1列出了本文所提系統(tǒng)硬件設(shè)備的詳細(xì)價(jià)格。

圖9 系統(tǒng)驗(yàn)證平臺(tái)Fig.9 System verification platform

表1 成本分析Table 1 Cost analysis

5 實(shí)驗(yàn)結(jié)果

5.1 規(guī)劃算法實(shí)時(shí)性分析

機(jī)器人在未知環(huán)境中移動(dòng)要求自主規(guī)劃系統(tǒng)具有良好的實(shí)時(shí)性，而規(guī)劃模塊的實(shí)時(shí)性對(duì)于保障機(jī)器人的安全起著關(guān)鍵性的作用。為提高軌跡規(guī)劃模塊的實(shí)時(shí)性，系統(tǒng)并未使用全局地圖進(jìn)行軌跡規(guī)劃，而是以移動(dòng)機(jī)器人為中心構(gòu)建局部地圖，規(guī)劃算法在局部地圖中進(jìn)行搜索，進(jìn)而提升系統(tǒng)應(yīng)對(duì)復(fù)雜環(huán)境的能力。為驗(yàn)證規(guī)劃算法的實(shí)時(shí)性，在有大量障礙物的環(huán)境中進(jìn)行測(cè)試，每次規(guī)劃起點(diǎn)與終點(diǎn)的距離為3～8 m，軌跡規(guī)劃的周期為20 ms，測(cè)試結(jié)果如圖10所示。根據(jù)圖10中的結(jié)果可以看出，路徑搜索的時(shí)間基本在4 ms以內(nèi)，軌跡優(yōu)化所需的時(shí)間穩(wěn)定在6 ms 以內(nèi)，軌跡規(guī)劃所需的總時(shí)間不超過(guò)10 ms，規(guī)劃模塊的性能完全滿足機(jī)器人安全移動(dòng)的需求。

圖10 規(guī)劃算法運(yùn)行時(shí)間統(tǒng)計(jì)圖Fig.10 Statistical chart of planning algorithm running time

5.2 三維規(guī)劃實(shí)驗(yàn)

本文提出的規(guī)劃算法基于三維環(huán)境地圖，在立體空間中進(jìn)行搜索，是一種三維空間搜索方法。相比于二維平面搜索，可以有效提升軌跡的安全性。為測(cè)試規(guī)劃算法的三維避障能力，設(shè)置測(cè)試場(chǎng)景如圖11所示，紅色平面為傳感器所在的平面，場(chǎng)景中有三種不同高度的障礙物，分別位于傳感器平面的上方和下方，規(guī)劃的起點(diǎn)和終點(diǎn)設(shè)置在障礙物的兩側(cè)，以保證規(guī)劃的軌跡需要通過(guò)障礙物。由于RGD-D 相機(jī)觀測(cè)能力有限，圖11與圖12中淺黃色區(qū)域?yàn)閭鞲衅饔^測(cè)盲區(qū)。設(shè)定搜索步長(zhǎng)τ=1.2，權(quán)重系數(shù)λs=10.0，λc=5.0，λv=0.01，移動(dòng)機(jī)器人的最大速度vmax=0.5，碰撞代價(jià)閾值dthr=0.4。

實(shí)驗(yàn)結(jié)果如圖12所示?；诙S平面搜索得到的軌跡無(wú)法避開(kāi)靠近搜索平面但分別位于平面下方的障礙物1 和平面上方的障礙物2，規(guī)劃得到的軌跡近似為一條直線，如圖12(a)所示?；谌S空間搜索的軌跡規(guī)劃方法判斷出障礙物1 和障礙物2 會(huì)與移動(dòng)機(jī)器人的底部和頂部發(fā)生碰撞，因此選擇繞開(kāi)障礙物1 和障礙物2，規(guī)劃出的軌跡如圖12(b)所示。圖12(b)中綠色曲線是基于改進(jìn)的A*算法搜索得到的路徑，紅色曲線是優(yōu)化后生成的軌跡。可以看出，相比于原始路徑，優(yōu)化后的軌跡在實(shí)現(xiàn)三維避障的同時(shí)，具有更優(yōu)的平滑性，更適于機(jī)器人的運(yùn)動(dòng)。

5.3 路徑規(guī)劃算法對(duì)比實(shí)驗(yàn)

為驗(yàn)證改進(jìn)后的A*算法的效果，本文對(duì)改進(jìn)A*算法與傳統(tǒng)A*算法進(jìn)行對(duì)比測(cè)試，結(jié)果如圖13所示。圖中紅色路徑為改進(jìn)A*算法的搜索結(jié)果，藍(lán)色路徑為傳統(tǒng)A*算法的搜索結(jié)果，從圖中可以看出改進(jìn)的A*算法搜索的路徑更加平滑。此外，由于改進(jìn)A*算法考慮了移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)特征，因此在起點(diǎn)附近會(huì)沿著移動(dòng)機(jī)器人的前進(jìn)方向進(jìn)行規(guī)劃，保證機(jī)器人運(yùn)動(dòng)的平順性。

為測(cè)試系統(tǒng)的實(shí)際效果，我們?cè)趯?shí)驗(yàn)室搭建了實(shí)驗(yàn)場(chǎng)地，如圖14所示，地面上堆放大量的箱子以及擋板作為障礙物，系統(tǒng)的目標(biāo)是在沒(méi)有環(huán)境地圖的情況下，實(shí)時(shí)規(guī)劃出一條安全的軌跡，保證移動(dòng)機(jī)器人不與障礙物發(fā)生碰撞，同時(shí)在移動(dòng)的過(guò)程中創(chuàng)建實(shí)驗(yàn)場(chǎng)地的三維地圖。為引導(dǎo)移動(dòng)機(jī)器人走完整個(gè)實(shí)驗(yàn)場(chǎng)地，我們?cè)谄瘘c(diǎn)和終點(diǎn)之間設(shè)定了兩個(gè)中間點(diǎn)，以約束機(jī)器人的運(yùn)動(dòng)軌跡。系統(tǒng)的整個(gè)運(yùn)行過(guò)程都是在沒(méi)有人為參與的情況下自主完成的。

5.4 實(shí)時(shí)避障實(shí)驗(yàn)

軌跡規(guī)劃子系統(tǒng)只依賴于傳感器掃描得到的局部地圖，無(wú)需預(yù)先加載環(huán)境地圖，系統(tǒng)以20 ms間隔調(diào)用規(guī)劃器，以50 ms 間隔進(jìn)行碰撞檢測(cè)，若軌跡與障礙物發(fā)生碰撞，則立即進(jìn)行重規(guī)劃。從圖15可以看出，在移動(dòng)機(jī)器人轉(zhuǎn)彎前，由于Realsense D455 相機(jī)的視場(chǎng)角有限，無(wú)法感知前方可能存在的障礙物，規(guī)劃出的軌跡比較平直；當(dāng)移動(dòng)機(jī)器人轉(zhuǎn)過(guò)彎后，傳感器檢測(cè)到軌跡前方的障礙物（綠色部分），如圖16所示，系統(tǒng)迅速調(diào)整軌跡偏離障礙物，保證移動(dòng)機(jī)器人安全行駛。實(shí)驗(yàn)結(jié)果表明系統(tǒng)可以做到實(shí)時(shí)避障。

圖15 轉(zhuǎn)彎前未探測(cè)到障礙物Fig.15 No obstacles detected before turning

圖16 轉(zhuǎn)彎后探測(cè)到障礙物Fig.16 Obstacles detected after turning

5.5 全局地圖效果

本文所提系統(tǒng)基于OctoMap 算法實(shí)現(xiàn)三維地圖構(gòu)建，R G B-D 相機(jī)的點(diǎn)云數(shù)據(jù)作為octomap_sever[26]的數(shù)據(jù)輸入，地圖分辨率設(shè)置為0.05 m，全局坐標(biāo)系設(shè)置為“world”，點(diǎn)云數(shù)據(jù)以“camera_link”為參考坐標(biāo)系，坐標(biāo)系“world”與“camera_link”之間的坐標(biāo)變換關(guān)系由預(yù)先配置的TF 樹(shù)提供。移動(dòng)機(jī)器人在實(shí)驗(yàn)場(chǎng)地中的移動(dòng)軌跡以及實(shí)驗(yàn)生成的三維地圖如圖17所示，在樓宇走廊內(nèi)實(shí)驗(yàn)得到的三維地圖如圖18所示。實(shí)驗(yàn)結(jié)果表明，系統(tǒng)可以較為高效地實(shí)現(xiàn)三維地圖的實(shí)時(shí)構(gòu)建，完成對(duì)未知環(huán)境的探測(cè)任務(wù)。

圖17 實(shí)驗(yàn)場(chǎng)地三維地圖Fig.17 3D map of the experimental site

圖18 走廊三維地圖Fig.18 3D map of the corridor

5.6 導(dǎo)航精度分析

為定量分析系統(tǒng)自主定位的精度，在移動(dòng)機(jī)器人頂部放置反光球，記錄OptiTrack 動(dòng)作捕捉系統(tǒng)輸出的位姿數(shù)據(jù)作為機(jī)器人運(yùn)動(dòng)的真實(shí)值，并與本文所述系統(tǒng)定位模塊輸出的位姿數(shù)據(jù)進(jìn)行對(duì)比，實(shí)驗(yàn)結(jié)果如圖19～20 所示。

圖19 真實(shí)位姿與估計(jì)位姿對(duì)比Fig.19 Comparison of real pose and estimated pose

圖20 真實(shí)位姿與估計(jì)位姿之間的誤差Fig.20 Error between true pose and estimated pose

為定量分析3D 占據(jù)柵格地圖的精度，將文中建圖算法得到的三維環(huán)境地圖與真實(shí)環(huán)境進(jìn)行對(duì)比。通過(guò)ROS 平臺(tái)下的Rviz 可視化軟件的測(cè)量工具，對(duì)地圖的多個(gè)位置進(jìn)行測(cè)量，如圖21所示。并與真實(shí)環(huán)境下相同位置測(cè)量數(shù)據(jù)進(jìn)行對(duì)比，實(shí)驗(yàn)結(jié)果如表2所示。

圖21 3D 占據(jù)柵格地圖距離測(cè)量Fig.21 3D occupancy grid map distance measurement

表2 真實(shí)值與測(cè)量值對(duì)比Table 2 Comparison of real value and measured value

6 結(jié) 論

本文設(shè)計(jì)并實(shí)現(xiàn)了一種基于單一傳感器的低成本自主系統(tǒng)，用于實(shí)現(xiàn)移動(dòng)機(jī)器人的自主規(guī)劃以及對(duì)未知環(huán)境的探測(cè)。系統(tǒng)的核心部件為一臺(tái)提供計(jì)算服務(wù)的機(jī)載計(jì)算機(jī)和一個(gè)感知設(shè)備—Realsense D455 RGB-D 相機(jī)，硬件連接簡(jiǎn)單，便于部署。為保證機(jī)器人在未知環(huán)境中安全移動(dòng)，提出了一種基于A*算法三維規(guī)劃方法，改進(jìn)了傳統(tǒng)A*算法的節(jié)點(diǎn)擴(kuò)展方式，擴(kuò)大了碰撞檢測(cè)的范圍，提高了規(guī)劃軌跡的平滑性和安全性，從而更好地保障機(jī)器人的運(yùn)行安全。地圖構(gòu)建子系統(tǒng)基于OctoMap 算法，可以在機(jī)器人移動(dòng)期間構(gòu)建三維環(huán)境地圖，完成環(huán)境探測(cè)任務(wù)。該系統(tǒng)在實(shí)際環(huán)境中進(jìn)行了多次測(cè)試，測(cè)試結(jié)果表明該系統(tǒng)具有良好的穩(wěn)定性，且軟件算法運(yùn)算速度快，可以實(shí)現(xiàn)實(shí)時(shí)避障，具有極強(qiáng)的環(huán)境適應(yīng)性。在未來(lái)的研究中，可以優(yōu)化系統(tǒng)的軟件架構(gòu)，使系統(tǒng)更加簡(jiǎn)潔；還可以考慮提升規(guī)劃算法的性能，實(shí)現(xiàn)對(duì)動(dòng)態(tài)障礙物的規(guī)避。此外，也可以將系統(tǒng)擴(kuò)展為多機(jī)協(xié)同控制系統(tǒng)，實(shí)現(xiàn)多個(gè)移動(dòng)機(jī)器人協(xié)同規(guī)劃與探測(cè)。