蔡志宏，趙 慧，周 亮，杜幸運

（武漢科技大學(xué)機(jī)械自動化學(xué)院，湖北 武漢 430081 ）

1 引言

移動機(jī)器人研究的問題包含眾多領(lǐng)域，其中同步定位與建圖（SLAM）[1]被提出作為移動機(jī)器人的靈魂所在。SLAM 在過去近30年中得到廣泛的關(guān)注與研究，由于傳感器種類和安裝方式的不同，故SLAM的實現(xiàn)方式存在一定的差異。SLAM主要分為激光SLAM 和視覺SLAM（VSLAM）。前者起步更早，在理論、技術(shù)和產(chǎn)品落地上都相對成熟，可靠性高，建圖相對直觀，精度高，不存在累計誤差。但受到傳感器自身限制，測距范圍、維度不同的激光雷達(dá)價格差異甚高，特別是多線激光雷達(dá)，不管是Sick、北洋，還是Velodyne，價格從幾萬到幾十萬不等，成本控制多年來一直是激光SLAM難以攻破的難關(guān)；而視覺SLAM相比結(jié)構(gòu)簡單，安裝方式多元化，成本低，無傳感器探測距離限制[2]。但因其易受光照影響，在室外多變的環(huán)境下，幾乎無法達(dá)到正常的效果，運行負(fù)荷大，依賴具有較強(qiáng)運算能力的中央處理器才能達(dá)到實時效果，構(gòu)建出的點云地圖也無法直接用于導(dǎo)航[3]，并且點云地圖通常規(guī)模很大，對應(yīng)的pcd文件（點云地圖格式）也會很大，其中存在許多不必要的細(xì)節(jié)，由于空間被占用，為了在有限的內(nèi)存中使用而不得不降低分辨率，從而導(dǎo)致地圖質(zhì)量下降。

在20 世紀(jì)的80 年代，文獻(xiàn)[4]提出了柵格地圖的概念，并針對該模型進(jìn)行了深入的研究，確定了柵格地圖的理論體系結(jié)構(gòu)。其將地圖分為獨立存在的柵格單元，每個單元表示該區(qū)域被障礙物占據(jù)的概率值，依次用1、0、0.5來表示被占用、空閑和未知這三種狀態(tài)，即使對于復(fù)雜地形也可通過改變單元狀態(tài)來表示，使已探索、未探索和障礙物這三種類型的區(qū)域清楚顯示出，可用于二維導(dǎo)航技術(shù)。并且柵格地圖便于維護(hù)，能夠以數(shù)組的形式存儲在計算機(jī)中[5]，即使搭載運算能力一般的中央處理器依然可以達(dá)到實時的效果。

現(xiàn)階段室外導(dǎo)航多借助高精度GPS配合慣性測量單元（In‐tertial Measurement Unit）大范圍定位或是采用光線跟蹤（Ray?tracing）[6]的方法對整條激光測量形成進(jìn)行處理，從而達(dá)到定位效果，多用于高成本研發(fā)性室外移動平臺。

針對室外小范圍運行的移動平臺，成本控制相對較低，無法采用成本過高的設(shè)備以及運算能力較強(qiáng)的工控機(jī)，雖有多線激光雷達(dá)可將室外信息描述完整，但多線雷達(dá)所得到的是三維點云數(shù)據(jù)，無法直接用于導(dǎo)航。故這里提出一種算法，將得到的點云地圖進(jìn)行轉(zhuǎn)換成八叉樹地圖[7]后投影成信息更全面的二維柵格地圖，并提取該地圖信息轉(zhuǎn)換成導(dǎo)航包所需要的消息發(fā)出，將成熟的室內(nèi)定位導(dǎo)航技術(shù)運用于室外。為保證室外運行的實時性以及成本控制，該算法整體設(shè)計并不復(fù)雜，搭載運算能力一般的工控機(jī)依然可以達(dá)到室外的實時效果。

2 算法設(shè)計

2.1 三維空間的壓縮分割

在室外，由多線激光雷達(dá)構(gòu)建的點云地圖內(nèi)存占用通常很大，需要過多的內(nèi)存空間，其中存在大量多余的信息，更是難以用于導(dǎo)航，無法直觀提取出地圖信息。而八叉樹可將點云信息進(jìn)行壓縮，形成單獨存在的離散“塊”，并可調(diào)整“塊”的大小，即分辨率，節(jié)省了大量空間，如圖1所示。

圖1 八叉樹示意圖Fig.1 Schematic Diagram of the Octree

八叉樹地圖是由其最大遞歸深度不斷進(jìn)行分割實現(xiàn)的。根據(jù)環(huán)境的尺寸構(gòu)建出同等大小的立方體，將單位元素放入其中不斷進(jìn)行八等份的分割，如圖2所示。分割后這些元素被八個子立方體所包含。根據(jù)空間分割理論，當(dāng)子節(jié)點中的元素數(shù)量與父節(jié)點中一致時，繼續(xù)分割下去數(shù)量還是一樣，會造成無窮分割的結(jié)果，故停止分割，此時到達(dá)最大遞歸深度。

圖2 八叉樹原理圖Fig.2 Octree Schematic

2.2 Move_Base導(dǎo)航包框架結(jié)構(gòu)

Move_base是ROS提供的導(dǎo)航功能包，也是為機(jī)器人提供路徑規(guī)劃的中樞，它通過接收各個傳感器的數(shù)據(jù)來進(jìn)行機(jī)器人的全局路徑規(guī)劃和局部路徑規(guī)劃，有了局部路徑便可不斷轉(zhuǎn)換成機(jī)器人電機(jī)的速度信息，驅(qū)動著機(jī)器人按照全局路徑不斷向目標(biāo)點移動，完成導(dǎo)航。

Move_Base框架，如圖3所示，可以看出中間的Move_Base模塊需要接收4個消息才能正常運行，分別是左上角的AMCL模塊（ROS 的導(dǎo)航定位模塊，也叫自適應(yīng)蒙特卡洛定位），左下角的Odom模塊（機(jī)器人里程計信息），右上角的Map模塊（全局地圖信息）以及最重要的Scan模塊（激光信息）。

圖3 Move_Base框架圖Fig.3 Move_Base Frame Diagram

2.3 八叉樹地圖的生成與更新

ROS中有一個基于Octomap的功能包Octomap_Server，可將接受來的點云信息（PointCloud）轉(zhuǎn)化八叉樹形式，生成相應(yīng)的八叉樹地圖。

在八叉樹中，一個子節(jié)點是否被占據(jù)是通過概率表達(dá)，而不能用0和1來表示。由于觀測周圍環(huán)境時噪聲始終存在，且觀測點存在不斷移動的情況，故每個子節(jié)點的狀態(tài)也會隨時間的變化而變化。根據(jù)其推導(dǎo)原理，假設(shè)每個時刻分別為t=1，2，3，4…，T，則記錄下每個時刻所對應(yīng)的信息為i1，i2，i3，i4，…，iT。故第n個子節(jié)點所更新的信息為：

對上式做一個logit變換，把一個概率p轉(zhuǎn)換到全實數(shù)空間R上：

其逆變換為：

α稱之為log?odds，用L（）表示每個葉子節(jié)點的log?odds，則可以得到下面的式子：

由（4）可知，每當(dāng)八叉樹信息更新時，只需將新的信息放入，并且每個值均存在上下限，避免了計算溢出，計算八叉樹地圖時將實數(shù)轉(zhuǎn)成概率即可。由八叉樹的構(gòu)造原理得知當(dāng)需要父節(jié)點的概率時，可通過子節(jié)點計算得到，方法是取均值或所有子節(jié)點的上下限。

2.4 生成真實環(huán)境下scan模塊的生成

由于Move_Base中需要接收Scan的消息，而多線激光雷達(dá)受到安裝位置的影響，通常安裝在較高的位置，其發(fā)出的Scan節(jié)點往往是與激光雷達(dá)同水平面方向上的激光射線，無法反映出室外復(fù)雜的多維地圖環(huán)境，若Move_Base直接使用激光雷達(dá)發(fā)出的Scan消息，會造成理論導(dǎo)航結(jié)果與實際導(dǎo)航結(jié)果相差甚遠(yuǎn)，如移動機(jī)器人運行至無法通行的道路上，如低矮的臺階、高度低于雷達(dá)安裝位置的若干障礙物。如圖4所示，激光雷達(dá)當(dāng)前安裝位置所發(fā)出的Scan信息無法反映出當(dāng)前環(huán)境的所有信息，如圖5所示。

圖4 激光雷達(dá)當(dāng)前Scan信息Fig.4 Lidar Current Scan Information

圖5 當(dāng)前環(huán)境實際狀態(tài)Fig.5 Actual State of the Current Environment

利用ROS提供的Octomap_Server完成八叉樹地圖的實時顯示，將其投影成二維柵格地圖，此時的柵格地圖在經(jīng)八叉樹地圖投影后能夠描述出復(fù)雜的環(huán)境。已知二維柵格地圖是單線激光Scan 掃描可生成的，故為了給Move_Base 提供可使用的Scan 消息，需對上一步得到的當(dāng)前狀態(tài)下動態(tài)柵格地圖提取其信息，轉(zhuǎn)換成Scan的消息結(jié)構(gòu)并發(fā)出。

在ROS下動態(tài)觀察當(dāng)前柵格地圖的信息，地圖Map的消息格式，如表1所示。其中動態(tài)信息Data是由一組數(shù)組構(gòu)成的，即上文所說的?1、1、0.5這三種數(shù)據(jù)組成，分別表示地圖上各個像素點的狀態(tài)（占用、非占用、未知）。

表1 消息結(jié)構(gòu)Tab.1 Message Structure

Scan的消息類型，如表1所示。其中動態(tài)消息類型是由像素點到激光原點的距離（Ranges）和該像素點的強(qiáng)度值（Intensities）構(gòu)成，只需將地圖數(shù)據(jù)中被占用的像素點（即墻面信息或不可移動區(qū)域）的坐標(biāo)提取出，地圖中像素點是以數(shù)組形式呈現(xiàn)，在假象坐標(biāo)系中，若實際地圖中某點坐標(biāo)為（x，y），則對應(yīng)柵格地圖中數(shù)組順序為：

其中map.info.width代表當(dāng)前柵格地圖的寬度，接著計算被占用的像素點到原點之間的距離，并賦予該像素點的強(qiáng)度為100，即表示激光掃到的點為不可移動區(qū)域，最后以Scan消息格式發(fā)送出便可得到Move_Base所需要的Scan消息。

3 實驗

3.1 實驗平臺及環(huán)境

這里的激光雷達(dá)采用的是Velodyne VLP?16激光雷達(dá)，系統(tǒng)環(huán)境為Ubuntu18.04，ROS 版本為Melodic，移動平臺是基于一臺后輪驅(qū)動清掃車所改裝，如圖6所示。在室外強(qiáng)光的環(huán)境下進(jìn)行實驗。

圖6 Velodyne雷達(dá)與移動平臺Fig.6 Velodyne and Mobile Platform

3.2 實驗步驟

（1）Velodyne激光雷達(dá)生成的信息為三維點云信息，如圖7（a）所示。在Octomap_Server 的處理下，將當(dāng)前環(huán)境下的三維點云數(shù)據(jù)轉(zhuǎn)化為八叉樹形式，如圖7（b）所示。原來的點云以方塊的形式壓縮表示，以不同的方式保留了地圖的有效信息。

圖7 點云地圖轉(zhuǎn)化為八叉樹地圖Fig.7 Point Cloud Map into Octree Map

（2）將得到的八叉樹地圖進(jìn)行投影處理便可成為具有更多信息量的二維柵格地圖，不同水平面上收到的障礙物信息集中在同一張柵格地圖中，如圖8（a）所示。

圖8 柵格地圖信息生成激光信息（scan）Fig.8 Grid Map Information Generates Laser Information（Scan）

（3）提?。?）中生成的柵格地圖信息，在ROS 系統(tǒng)下實現(xiàn)該算法，將其轉(zhuǎn)換成Move_Base導(dǎo)航包所需要的Scan模塊發(fā)出，在ROS提供的RVIZ可視化界面如圖8中（b）可以看出，紫色的激光點即生成的Scan完全匹配上先前的柵格地圖，該激光點反映出的信息為多線雷達(dá)掃描多個平面后的完整地圖信息，意味著發(fā)送至Move_Base的Scan模塊可以作為室外環(huán)境的真實映射。

（4）至此Move_Base 所需的五個消息分別為/t（f位姿變換）、/odom（里程計）、/cmd_ve（l電機(jī)編碼器）、/map（全局地圖）、/scan（實時激光點）全部收到，為進(jìn)行進(jìn)一步室外路徑規(guī)劃和導(dǎo)航做好準(zhǔn)備。

（5）在室外，使用Cartographer建圖算法，通過接受上一步發(fā)出的Scan消息來對室外環(huán)境進(jìn)行建圖，并運行Move_Base導(dǎo)航包，便可在對移動平臺進(jìn)行導(dǎo)航及路徑規(guī)劃，達(dá)到室外避障的效果。

3.3 實驗分析

在上一步中得到的激光點通過Cartographer 建圖算法建立出室外的二維柵格地圖，如圖9（a）所示。并在移動過程中Cartog‐rapher自帶的定位算法會將該激光點與建立好的柵格地圖進(jìn)行匹配以及與里程計信息融合的聯(lián)合定位，如圖9（b）所示。其中兩色相間的坐標(biāo)軸即為移動平臺當(dāng)前的精確位置以及位姿，其中一色坐標(biāo)軸代表移動平臺的正方向。

在完成移動平臺定位后即可開始進(jìn)行路徑規(guī)劃與導(dǎo)航，以達(dá)到使移動平臺按照預(yù)先設(shè)置好的軌跡行駛。如圖9（b）所示，收集出地圖中需要經(jīng)過的目標(biāo)點以及移動平臺行駛到該點時的位姿，生成出移動平臺的行駛路徑，其中一點代表下一個局部目標(biāo)點，每當(dāng)移動平臺移動到目標(biāo)點后便尋找下一個目標(biāo)點，另一點代表上一步生成反映真實環(huán)境的激光點，可供Move_Base功能包使用來規(guī)避軌跡上臨時出現(xiàn)的障礙。

圖9 激光與地圖匹配Fig.9 Laser and Map Match

移動平臺能夠不斷按照尋找下一個目標(biāo)點的方式走過圖中軌跡，完成了在室外導(dǎo)航的目的，實現(xiàn)了在僅使用激光雷達(dá)作為傳感器的情況下小范圍定位導(dǎo)航功能。如圖10所示，從啟動開始，在移動的過程中，綠色激光點始終能夠與地圖精確配準(zhǔn)，從未發(fā)生畸變，對于移動平臺的定位十分準(zhǔn)確，具有足夠的實時效果，保證了在室外行駛的安全性。

圖10 全局路徑導(dǎo)航地圖Fig.10 Global Path Navigation Map

4 結(jié)論

針對室外小范圍定位導(dǎo)航的問題提出一種支持算法，通過多線激光雷達(dá)將點云地圖轉(zhuǎn)換為的八叉樹地圖投影成柵格地圖，由地圖生成相應(yīng)的激光點，從而能夠完整地描述出當(dāng)前的室外環(huán)境，將成熟的室內(nèi)導(dǎo)航技術(shù)運用在了室外，解決了多線激光雷達(dá)無法建立柵格地圖進(jìn)行路徑規(guī)劃與導(dǎo)航的問題，并整體降低了系統(tǒng)的成本，在對實時性要求極高的室外環(huán)境下依然可保證實時的準(zhǔn)確定位，成功實現(xiàn)室外小范圍定位導(dǎo)航。