一種多線激光雷達(dá)室外小范圍導(dǎo)航算法設(shè)計(jì)

蔡志宏，趙 慧，周 亮，杜幸運(yùn)

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

1 引言

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

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

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

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

2 算法設(shè)計(jì)

2.1 三維空間的壓縮分割

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

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

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

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

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

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

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

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

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

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

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

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

其逆變換為：

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

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

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

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)

3.1 實(shí)驗(yàn)平臺(tái)及環(huán)境

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

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

3.2 實(shí)驗(yàn)步驟

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

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

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

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

3.3 實(shí)驗(yàn)分析

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

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

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

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

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

4 結(jié)論

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