一種多線激光雷達室外小范圍導航算法設計

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

（武漢科技大學機械自動化學院，湖北 武漢 430081 ）

1 引言

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

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

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

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

2 算法設計

2.1 三維空間的壓縮分割

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

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

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

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

2.2 Move_Base導航包框架結構

Move_base是ROS提供的導航功能包，也是為機器人提供路徑規劃的中樞，它通過接收各個傳感器的數據來進行機器人的全局路徑規劃和局部路徑規劃，有了局部路徑便可不斷轉換成機器人電機的速度信息，驅動著機器人按照全局路徑不斷向目標點移動，完成導航。

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

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

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

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

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

對上式做一個logit變換，把一個概率p轉換到全實數空間R上：

其逆變換為：

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

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

2.4 生成真實環境下scan模塊的生成

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

圖4 激光雷達當前Scan信息Fig.4 Lidar Current Scan Information

圖5 當前環境實際狀態Fig.5 Actual State of the Current Environment

利用ROS提供的Octomap_Server完成八叉樹地圖的實時顯示，將其投影成二維柵格地圖，此時的柵格地圖在經八叉樹地圖投影后能夠描述出復雜的環境。已知二維柵格地圖是單線激光Scan 掃描可生成的，故為了給Move_Base 提供可使用的Scan 消息，需對上一步得到的當前狀態下動態柵格地圖提取其信息，轉換成Scan的消息結構并發出。

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

表1 消息結構Tab.1 Message Structure

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

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

3 實驗

3.1 實驗平臺及環境

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

圖6 Velodyne雷達與移動平臺Fig.6 Velodyne and Mobile Platform

3.2 實驗步驟

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

圖7 點云地圖轉化為八叉樹地圖Fig.7 Point Cloud Map into Octree Map

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

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

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

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

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

3.3 實驗分析

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

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

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

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

圖10 全局路徑導航地圖Fig.10 Global Path Navigation Map

4 結論

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