崔林威， 崔建偉， 黃子正， 李 響， 姜匯策

(東南大學 儀器科學與工程學院，江蘇 南京 210096)

我國人口老齡化問題日益嚴峻，正在步入老齡社會，老年人和殘障人士的生活自理問題日益突出。目前老年人和殘障人士多采用傳統(tǒng)的無動力式輪椅和電動式輪椅，面對復(fù)雜的室外環(huán)境時，僅依靠使用者無法保障安全出行，因此無法滿足使用者的出行需求[1]。另外，隨著無人駕駛、激光SLAM(Simultaneous Localization and Mapping,即時定位與地圖構(gòu)建)技術(shù)的迅速發(fā)展，依靠三維激光雷達獲取的點云進行建圖，可完成室內(nèi)外環(huán)境下的路徑規(guī)劃，這對于無人輪椅的研究有一定的借鑒意義。因此，針對現(xiàn)有輪椅的出行方式，為提高老年人生活、出行的便捷性，開展輪椅的導(dǎo)航研究對實現(xiàn)輪椅的智能化、提高老年人的出行質(zhì)量有重要意義。

多傳感器融合的智能式輪椅將是未來的發(fā)展趨勢，目前國內(nèi)外正在開展有關(guān)智能輪椅的導(dǎo)航研究。丁佳偉[2]將腦機接口技術(shù)與SLAM技術(shù)進行融合，將環(huán)境地圖構(gòu)建、自主避障、路徑規(guī)劃與導(dǎo)航技術(shù)加入到腦控輪椅中，實現(xiàn)腦控輪椅的自主導(dǎo)航。于福超等[3]設(shè)計了基于北斗導(dǎo)航定位的智能導(dǎo)診輪椅，采用北斗導(dǎo)航定位作為全局路徑規(guī)劃路徑信息，結(jié)合使用者操作實現(xiàn)精準行走來實際解決醫(yī)院的導(dǎo)診問題。張毅等[4]研究了基于ROS(Robot Operating System,機器人操作系統(tǒng))的智能輪椅室內(nèi)導(dǎo)航，采用激光測距儀并運用基于擴展卡爾曼濾波器的SLAM方法進行建圖，使輪椅具備語音自主導(dǎo)航等功能。Ferracuti等[5]提出了Human-in-the-Loop框架，在室內(nèi)場景輪椅導(dǎo)航到目的地過程中，根據(jù)路徑規(guī)劃錯誤后人產(chǎn)生的EEG電位作為導(dǎo)航算法的附加輸入，實現(xiàn)了路徑的實時修改。Shan等[6]提出一種輕量級和地面優(yōu)化的激光雷達測距和建圖算法(LeGO-LOAM)，適用于具有可變地形的復(fù)雜環(huán)境，可對移動機器人進行位姿估計，并可實時估計地面車輛的六自由度位姿，可用于無人車的建圖與位姿估計。

以上研究在一定程度上實現(xiàn)了輪椅的自主導(dǎo)航，但也存在二維雷達檢測精度不足、北斗定位信息存在誤差，以及在復(fù)雜環(huán)境下導(dǎo)航功能下降的問題。因此，本文結(jié)合百度地圖和MEMS三維激光雷達研究了智能輪椅的室外SLAM算法，在移動區(qū)域未建圖時，借助百度地圖的路徑規(guī)劃信息使用輪椅，同時利用激光雷達進行三維建圖；完成建圖后，僅依靠導(dǎo)航、避障算法完成輪椅的移動和環(huán)境感知，從而提高了輪椅的智能化水平和安全性能。

1 系統(tǒng)方案

所設(shè)計的系統(tǒng)旨在通過ROS和MEMS固態(tài)激光雷達完成輪椅室外行駛過程中的環(huán)境建模、路徑規(guī)劃和避障功能，結(jié)合百度地圖導(dǎo)航與激光雷達完成三維環(huán)境建模，進而在不依靠百度地圖的情況下完成路徑規(guī)劃，從而實現(xiàn)輪椅的自主移動。

系統(tǒng)的硬件部分包括MEMS固態(tài)激光雷達、工控機、輪椅電機和編碼器。其中，MEMS固態(tài)雷達用于掃描輪椅前方道路的激光點云；工控機安裝Ubuntu 16.04操作系統(tǒng)和ROS平臺，用于獲取、處理點云數(shù)據(jù)，完成室外環(huán)境建模和路徑規(guī)劃，并將移動指令發(fā)送給輪椅內(nèi)部的嵌入式控制板，驅(qū)動輪椅電機控制輪椅的前、后、左、右移動；編碼器用于獲取輪椅的精確移動信息，并提供里程計信息。機器人硬件系統(tǒng)如圖1所示。

圖1 機器人硬件系統(tǒng)

2 智能輪椅三維環(huán)境建圖

2.1 三維激光雷達

為了使智能輪椅具備導(dǎo)航功能，需要在輪椅移動過程中預(yù)先使用雷達、深度攝像頭等傳感器對環(huán)境進行建圖，建圖的精確度會影響到路徑規(guī)劃的準確性?？紤]到攝像頭易受室外強光、反射等環(huán)境因素的影響，且二維激光雷達不能全面反映路面信息，因此選用速騰聚創(chuàng)公司的激光雷達RS-LiDAR-M1。該款雷達相比其他激光雷達，不但同樣具有體積輕巧、成本低等優(yōu)點，而且便于安裝在移動機器人上。該雷達等效于125線激光雷達，在2D MEMS振鏡快速震動的同時發(fā)射高頻率激光束對外界環(huán)境進行持續(xù)掃描，經(jīng)過測距算法得到三維空間點云數(shù)據(jù)后便可獲得環(huán)境的深度信息，從而獲取準確的三維路面信息，使機器人更好地感知周圍的環(huán)境，MEMS固態(tài)激光雷達如圖2所示，其參數(shù)如表1所示[7]。

圖2 MEMS固態(tài)激光雷達

表1 RS-LiDAR-M1激光雷達參數(shù)對比

2.2 激光點云獲取

在ROS平臺中，激光雷達可以作為子節(jié)點以話題(Topic)的形式輸出點云的三維坐標。通過訂閱該話題便可獲取實時點云數(shù)據(jù)，同時可以通過可視化工具Rviz進行點云的可視化呈現(xiàn)[8]。

圖3為RS-LiDAR-M1在室外采集的激光點云數(shù)據(jù)利用Rviz工具呈現(xiàn)的點云可視化結(jié)果。由圖3可以看出，MEMS激光雷達獲得的點云比較密集，能夠真實反映出環(huán)境中的人、物體等信息，有助于提高室外環(huán)境建模的準確度。

圖3 MEMS雷達室外激光點云

2.3 室外三維場景建模

SLAM技術(shù)是移動機器人研究的熱點，用于實現(xiàn)機器人的實時建圖和定位，包括地圖表示、前端雷達數(shù)據(jù)處理、后端噪聲處理、回環(huán)檢測和地圖融合4個步驟[9-10]。激光SLAM能夠穩(wěn)定、準確地建圖，主流算法有：基于關(guān)鍵點的LOAM及改進算法LeGO-LOAM 和A-LOAM、LIO等[11]。LeGO-LOAM算法具有輕量化的特點，能夠在低功耗嵌入式系統(tǒng)上實現(xiàn)實時姿態(tài)估計，其運行時間短，可用更少的計算時間實現(xiàn)較高的位置估計精度，可進行地面點云優(yōu)化等。這些優(yōu)點適合應(yīng)用于智能輪椅的室外建圖，因此選用LeGO-LOAM算法進行三維建圖。

另外，本文選用的激光雷達采集點云的速度為每秒75萬點，高密度的點云能夠保證建圖算法的精確度，但同時也增加了工控機的處理負擔和建圖的時間，因此設(shè)計了融合體素柵格濾波器和LeGO-LOAM算法的建圖算法，包括點云濾波、點云分割、點云特征提取、位姿估計、激光點云建圖共5個部分，具體流程如下。

(1) 點云濾波。

在LeGO-LOAM算法中加入體素柵格濾波器，實現(xiàn)對原始點云進行降采樣的效果，以降低點云的密度。在處理時將原始點云分割成一個個3D網(wǎng)格，并計算每個網(wǎng)格內(nèi)的點的質(zhì)心，用于近似表示該3D網(wǎng)格內(nèi)的其他點，同時在處理過程中采用下采樣，可以加速點云處理過程且不破壞點云本身的幾何結(jié)構(gòu)[12]。式(1)為計算在一個體素網(wǎng)格內(nèi)的點云質(zhì)心的公式。

(1)

式中：(x,y,z)為質(zhì)心坐標；(xi,yi,zi)為體素網(wǎng)格內(nèi)的第i個點；k為網(wǎng)格內(nèi)點的數(shù)量。

(2) 點云分割。

點云分割部分用于將地面點云和非地面點云分割開，同時對激光點云進行離群點去除和分簇，只留下點數(shù)量較多的點云簇。

MEMS激光雷達每掃描一次路面會發(fā)送一幀數(shù)據(jù)，假設(shè)t時刻獲取的一幀點云為Pt={p1,p2,…,pn}，其中pi為t時刻的點云Pt中的一個點，可以將該點云投影到分辨率為600像素×125像素的二維圖像中，每個點在圖像中的像素即為實際物體距離激光雷達的距離。如圖4所示，激光雷達豎直方向的掃描角度為-12.5°～12.5°，由于激光雷達水平安裝，地面點必然出現(xiàn)在[-12.5°，1°] 掃描線上，因此將出現(xiàn)在該角度范圍內(nèi)的點視為地面點。

圖4 激光雷達豎直方向掃描角度

對于剩下的點，使用基于圖像的分割方法對距離圖像進行分簇，同一類的點標記上唯一的標識，并將低于30個點的類作為噪聲點處理，從而可以將一些微小物體點(如樹葉、云等)作為噪聲去除，減少相鄰幀之間微小物體不重復(fù)出現(xiàn)對幀間匹配造成的干擾。這樣保存下來的一幀數(shù)據(jù)就是靜態(tài)物體的點云數(shù)據(jù)，并且具有每一類的標簽、在距離圖像中的行列索引和像素(距離)值。

(3) 點云特征提取。

為了獲得t時刻和t-1時刻的姿態(tài)變換關(guān)系，需要從已分簇的點云里提取出點云的邊緣特征和平面特征，首先將距離圖像在水平方向上分成多個子圖像，然后計算子圖像中每一行點的曲率，根據(jù)曲率選擇出邊緣點和平面點，獲得點集合。

將一幀圖像劃分為6個子圖像，每個子圖像的分辨率為100像素×125像素，在每個子圖像中選取t時刻的點云Pt的一個點pi，在pi豎直方向上，左右各找5個點，構(gòu)建集合S，并對每個子圖像按照式(2)計算點在集合S中的曲率：

(2)

(4) 位姿估計。

位姿估計模塊包括特征點關(guān)聯(lián)和姿態(tài)解算，其中特征點關(guān)聯(lián)是為了將k時刻的點云和k+1時刻的點云關(guān)聯(lián)起來，姿態(tài)解算是為了估計雷達的位姿。本文選取分辨率為0.9°的高精度編碼器BCE25H5H23D獲取輪椅的移動信息。

LeGO-LOAM算法基于提取的特征點構(gòu)建相鄰兩次掃描的約束關(guān)系，并使用兩次LM優(yōu)化算法得到姿態(tài)變換矩陣。其中特征點關(guān)聯(lián)使用Scan-to-Scan方式，分為邊緣點匹配和平面點匹配兩個部分，計算點到直線的距離和點到平面的距離,姿態(tài)解算根據(jù)匹配的特征點云估計激光雷達的位姿。

(5) 激光點云建圖。

LeGO-LOAM算法使用迭代最近點(Iterative Closest Point,ICP)算法進行點云配準，在得到一幀點云和相鄰幀的姿態(tài)變換信息后，會調(diào)用一次建圖函數(shù)，將t時刻的激光點云和t-1時刻的全局地圖特征點云進行匹配，并將其加入到全局地圖中，實現(xiàn)對已有地圖的實時更新，利用GTASM優(yōu)化得到最終的全局地圖[13]。

3 百度地圖與激光雷達融合導(dǎo)航

3.1 百度地圖路徑規(guī)劃信息

為了實現(xiàn)輪椅的自主導(dǎo)航，需要在移動區(qū)域完成雷達建圖。對于激光雷達沒有完成建圖的區(qū)域，通過開發(fā)基于百度地圖路徑規(guī)劃的輪椅移動方式，輪椅使用者依據(jù)百度地圖API函數(shù)提供的路徑規(guī)劃信息操控輪椅進行移動，同時雷達利用LeGO-LOAM算法進行建圖，建圖完畢后就可以不依靠百度地圖完成實時路徑規(guī)劃與導(dǎo)航[14]。

為了調(diào)用百度地圖API的路徑規(guī)劃函數(shù)，需要遵循以下步驟：① 在百度地圖開放平臺申請百度賬號；② 申請成為百度開發(fā)者；③ 使用百度賬號獲得服務(wù)密匙(ak)；④ 發(fā)送URL，調(diào)用百度地圖的相關(guān)服務(wù)。進而可編寫JavaScript腳本調(diào)用DrivingRoute函數(shù)和search函數(shù)實現(xiàn)路徑的查找與顯示，百度地圖API-路徑規(guī)劃如圖5所示。

圖5 百度地圖API-路徑規(guī)劃

3.2 輪椅路徑規(guī)劃及導(dǎo)航

設(shè)計的輪椅的導(dǎo)航方式包括未建模和已建模兩種情況，當后臺不存在建模信息時，依靠百度地圖，人為操控輪椅完成移動。在完成室外建圖后，僅依靠道路感知算法和MEMS激光雷達完成輪椅的自主移動功能，即用戶可獲取指定目標點的移動路徑，同時輪椅自主避障，此時依賴ROS功能包完成導(dǎo)航。

ROS提供了兩個功能包：① move_base，實現(xiàn)機器人導(dǎo)航中的最優(yōu)路徑規(guī)劃；② amcl，實現(xiàn)點云地圖中的機器人定位。機器人只需要發(fā)布必要的傳感器信息和導(dǎo)航的目標位置，ROS即可完成輪椅的實時路徑規(guī)劃、避障和移動指令的發(fā)布。導(dǎo)航算法實施如下：首先，導(dǎo)航功能包需要采集MEMS激光雷達發(fā)布的點云信息，以達到實時避障的效果，激光雷達以10 Hz的頻率向該功能包發(fā)布話題為/rslidar_points的點云信息；然后，功能包訂閱機器人發(fā)布的里程計信息和坐標變換完成自身定位，同時結(jié)合道路感知算法完成實時避障；最后，功能包輸出cmd_vel話題的控制指令，用以控制機器人完成相應(yīng)的運動。

3.3 路面感知算法

為了精確識別路面信息，提高輪椅安全性能，調(diào)用了RoboSense的路面感知算法RoboSense Smart Sensor感知SDK，協(xié)助導(dǎo)航算法完成道路物體感知。該SDK融合了基于幾何規(guī)則的傳統(tǒng)點云算法和基于數(shù)據(jù)驅(qū)動的深度學習算法，可以為自動駕駛決策規(guī)劃模塊提供像素級的全方位、結(jié)構(gòu)化、高精度的感知信息。SDK包括驅(qū)動模塊、通信模塊和感知模塊，其中驅(qū)動模塊提供數(shù)據(jù)解析功能；通信模塊用于對外信息的發(fā)送和接收；感知模塊作為核心子模塊，提供整體感知功能。

RoboSense Smart Sensor感知SDK能夠輸出以下信息：① 障礙物檢測信息，包括行人、騎行者、小車、大車、拖車等；② 可行駛區(qū)域信息，將路面物體圍成的封閉區(qū)域作為可行駛區(qū)域；③ 車道線信息，輸出檢測和預(yù)測到的車道線信息；④ 道路邊界信息，以檢測和預(yù)測的方式輸出邊界信息。通過調(diào)用SDK輸出的路面感知結(jié)果即可獲得準確的路面信息，從而保證輪椅實時導(dǎo)航過程中的安全性。

4 實驗

筆者在激光雷達上研究了三維激光點云獲取與基于體素網(wǎng)格濾波器和LeGO-LOAM的三維建圖算法，然后進行了智能輪椅的導(dǎo)航技術(shù)研究，包括基于百度云和三維建圖的路徑規(guī)劃和路徑規(guī)劃過程中的實時路面物體感知算法，下面按照本文設(shè)計的導(dǎo)航方式對上述研究進行實驗驗證。

4.1 建圖及導(dǎo)航算法仿真

在實際測試前，對文中提出的導(dǎo)航模式、建圖和導(dǎo)航算法進行了仿真，以保證實際應(yīng)用時的安全性。首先在Gazebo界面中導(dǎo)入一款建筑內(nèi)部的3D模型，如圖6(a)所示，然后驗證未建圖的導(dǎo)航模式，利用鍵盤模擬人為使用百度地圖的路徑規(guī)劃信息操控輪椅移動，在移動過程中利用改進的LeGO-LOAM算法進行建模，建好的三維地圖如圖6(b)所示。

圖6 三維建圖仿真

為了利用3.2節(jié)中的ROS功能包進行導(dǎo)航，需要將圖6(b)的三維地圖轉(zhuǎn)換為二維柵格地圖，采用Octomap庫完成二維柵格地圖的轉(zhuǎn)換，轉(zhuǎn)換后的二維柵格地圖如圖7所示。

圖7 二維柵格地圖

最后對導(dǎo)航算法進行仿真，將圖7導(dǎo)入到導(dǎo)航算法中，當操作者需要到達地圖的某個位置，可直接點擊地圖，結(jié)合避障算法，機器人可到達指定位置，路徑規(guī)劃信息和機器人的實時位置如圖8所示。仿真實驗表明，在未建圖時利用激光雷達進行三維建圖，然后將其轉(zhuǎn)化為二維柵格地圖，結(jié)合導(dǎo)航算法能夠?qū)崿F(xiàn)機器人導(dǎo)航，在實際控制輪椅時，訂閱導(dǎo)航功能包的cmd_vel信息可獲得輪椅的移動指令。

4.2 智能輪椅室外三維建模

根據(jù)圖1構(gòu)建的裝有激光雷達的智能輪椅如圖9所示，并在學校內(nèi)部進行了室外三維建模實驗。

圖8 自主導(dǎo)航仿真

圖9 裝有激光雷達的智能輪椅

在室外環(huán)境未建圖時，輪椅無法對陌生環(huán)境進行路徑規(guī)劃和導(dǎo)航移動，此時利用百度云進行路徑規(guī)劃，人工操控輪椅在室外進行移動建圖。圖10為基于百度云開發(fā)的地圖可視化操作界面，用戶可利用該界面操控輪椅到達目標位置。

圖10 基于百度云的地圖可視化界面

按照圖10中的路徑控制輪椅移動，同時使用激光雷達采集實時激光點云。利用融合體素網(wǎng)絡(luò)濾波器的LeGO-LOAM算法進行三維建圖，并進行點云濾波、點云分割、地面點優(yōu)化等處理，原始點云如圖11(a)，濾波后的點云如圖11(b)所示。在Rviz界面上顯示的已建好的三維地圖如圖12所示， RS-LiDAR-M1采集的高密度點云配合建圖算法可使建好的圖能更真實地反映路面環(huán)境。

圖11 激光雷達點云濾波

圖12 激光點云三維建圖

為了與上述建圖算法進行對比，采用LOAM算法也進行了三維建圖，實驗發(fā)現(xiàn)，在室外建圖時，LOAM算法提取的特征較多，往往會將草叢、樹葉等特征提取出來。但在實際應(yīng)用中，建好的地圖只需要包含車道線、路面障礙等信息即可，特征過多也會造成地圖冗余。另外，LeGO-LOAM算法在運行速度上降低了60%，能夠更高效率地完成建圖。

4.3 智能輪椅導(dǎo)航感知實驗

在完成建圖后，根據(jù)導(dǎo)航仿真實驗過程，將圖12構(gòu)建的三維地圖轉(zhuǎn)換為二維地圖，并導(dǎo)入到ROS的導(dǎo)航算法包內(nèi)，在該段路徑進行了導(dǎo)航實驗。

輪椅在長20.5 m的路徑內(nèi)進行了實驗，輪椅能夠在完成建圖的情況下，到達指定目標位置，同時在移動過程中能夠調(diào)用感知算法識別路面的物體信息，實現(xiàn)實時避障功能，室外實驗及路面感知效果如圖13所示，其中圖13(a)為輪椅和實驗人員，圖13(b)為導(dǎo)航過程中調(diào)用物體感知算法獲取的路面識別信息，進而機器人能夠根據(jù)障礙物信息主動避障。在圖13(b)中可以看到識別到的車輛(CAR)、車道線(紅色直線)、其他障礙(Barrier)等，輪椅在進行目標分類時，智能輪椅導(dǎo)航指標如表2所示。

圖13 室外實驗及路面感知效果

表2 智能輪椅導(dǎo)航指標

另外，在實驗過程中，受灌木、草叢等因素的影響，可能導(dǎo)致輪椅出現(xiàn)誤識別現(xiàn)象。路面目標檢測結(jié)果與部分錯誤分類結(jié)果的對比圖如圖14所示。由圖14可以看到，出現(xiàn)了將Barrier誤識別為CAR的結(jié)果，在誤識別時需要機器人將識別結(jié)果、路徑規(guī)劃信息與人為控制靈活結(jié)合起來，輔助完成輪椅的移動，避免因誤識別導(dǎo)致出現(xiàn)輪椅安全問題。

圖14 路面感知效果對比

5 結(jié)束語

在ROS平臺下研究了智能輪椅的室外場景建模、路徑規(guī)劃與導(dǎo)航感知算法。針對激光雷達采集的三維激光點云密度較大的問題，在LeGO-LOAM算法中融合了體素網(wǎng)格濾波器，在保證建圖精度的同時減少了建圖時間；然后研究了百度地圖和激光雷達融合的輪椅導(dǎo)航移動方式，同時在導(dǎo)航移動時調(diào)用路面感知SDK獲取實時的路面環(huán)境信息，實現(xiàn)了實時避障，該導(dǎo)航模式能夠保障輪椅導(dǎo)航過程中的安全性能，為智能輪椅的研發(fā)提供一定的借鑒。另外，下一步還需要提高輪椅的識別精確度和魯棒性，將輪椅路徑規(guī)劃信息、百度地圖和外部人為輔助更好地融合起來，并部署主動安全模塊，通過多種激光雷達、測距模塊等多種傳感器融合實現(xiàn)輪椅的主動安全功能，進一步提高輪椅的安全性能。