基于SLAM的校園配送AGV地圖構建及路徑規劃研究*

朱偉枝，謝娟烘，盧子榮

（廣東理工學院，廣東肇慶 526100）

0 引言

隨著科技的發展，無接觸配送、無人配送等多個熱門詞匯開始進入我們的視線[1]。無人配送是指物品流通環節中沒有或是少量人工參與，用機器替代人工或者人機協作的配送方式，達到提高效率、減少成本的目的。無人配送方式需求場景眾多，包括快遞、外賣、B2C零售、商超便利、生鮮宅配、餐館/KTV配送、C2C配送需求等，為生產生活需求提供了相當大的便利性[2]。在國內外疫情防控形勢依然嚴峻的情況下，無人配送無疑是最安全、最有效率的一種配送方式，在未來將成為主流趨勢。

在無人配送方面，國內外都做了許多研究與實踐。Nasri等[3]討論了在不確定交通條件下，如何優化無人駕駛車輛的路線和速度。張明雨[4]利用代價地圖的cost信息來構建狀態空間，結合Q-earning算法以達到修改全局路徑實現機器人的動態導航。2018年，阿里菜鳥物流無人車實現了支持用戶掃碼和刷臉功能。2021年，美團發布了新一代自研L4級別無人駕駛車，并投入到了買菜服務配送中。

在當今大學校園里，學生人數眾多，學校可以說是傳染病突發公共事件的高發場所[5]。基于此，本文提供一種校園AGV（Automated Guided Vehicle）地圖構建及路徑規劃設計方案。AGV是一輛自動導引運輸車，其搭載的激光雷達會實時掃描周邊環境，采集周圍的環境信息，對環境監測信息發出反應，識別移動與非移動信號，從而映射到AGV系統的傳動機構，實現避障，并根據設定地點進行路線規劃及導航。

1 AGV小車總體設計

1.1 控制部分

小車的控制部分由一塊樹莓派4B作為主控，Ardui?no作為編碼電機控制器。在樹莓派4B上燒錄好Ubuntu系統，并安裝ROS機器人操作平臺。電腦可通過樹莓派上的藍牙和WiFi串口上傳或下載相關驅動和數據，通過Rviz觀察SLAM建圖情況并監控小車實時動態。Arduino通過電腦端的Arduino IDE將編碼電機的驅動程序下載下來，并通過USB線及相應串口通信驅動連接至樹莓派。

1.2 動力部分

小車的動力部分由一對12 V帶編碼器的有刷直流電機、一套34∶1的齒輪減速器、一對輪胎、一個導向輪以及一塊12 V蓄電池組成。小車通過Arduino可以接收到電機上的編碼器傳達的傳動數據，通過齒輪減速箱可以有效地放大扭矩。

1.3 雷達部分

小車搭載的激光雷達型號是思嵐RPLIDAR A1，采用激光三角測距技術，掃描頻率5.5 Hz，采樣頻率8 k/s，掃描范圍最遠12 m，測距核心順時針旋轉，可實現對周圍環境的360°掃描測距檢測，從而獲得周圍環境的輪廓圖[6]。該激光雷達對ROS系統具有良好的整合適配性，讓機器人能更快速、精確地建圖。

2 基于SLAM的全局地圖掃描

2.1 定位導航

定位導航包括定位、建圖和路徑規劃。定位和建圖通常由SLAM（Simultaneous Localization and Mapping）來實現，而路徑規劃則是根據現有的地圖，在點與點之間計算好最優路線[7]。SLAM和運動規劃是AGV的兩個核心技術，SLAM技術可以得到稀疏的定位地圖，結合后處理可以得到稠密的三維點云地圖[8]。此時再將其轉化為柵格化地圖，AGV就可以在這個地圖的基礎上，結合傳感器獲取外部環境信息進行運動規劃，找到一條適合自身行走的最佳路徑。

2.2 基于LOAM算法的地圖構建

2.2.1 特征提取

LOAM（Lidar Odometry and Mapping）算法[9]的兩個核心模塊為特征提取（Lidar Registration）和里程計解算（Odometry and Mapping）。本文提取的兩種特征分別為平面點和邊緣點。提取特征點的常用方法有3種：特征向量法、直方圖法和旋轉圖像法。然而，雖然這些方法能夠準確地覆蓋一幀的大部分信息，但是由于計算量大，很難用于激光SLAM的相鄰幀匹配，因此本文引用曲率計算方法，公式如下：

式中：c為計算得到的曲率；k為一次掃描周期；L為雷達當前坐標系；i為第k次掃描點云的第i個點；為第k次掃描點云中的第i個點在雷達當前坐標系L中的位置向量；S為點前后指定數量的點，具體在實際代碼中實現就是計算前后5個點。

經計算后，選取曲率特別大、平滑度高的特征點作為邊緣點，曲率特別小、平滑度低的特征點作為平面點。

2.2.2 特征匹配

提取特征點后，采用scan-to-scan方法對連續特征幀進行匹配，并建立誤差模型。由于每個特征點都會隨著雷達的運動而改變，因此本文為了方便處理，將所有的特征點都重投影到每一幀的初始時刻，從而獲得對應的姿態變換信息。本文采用點到線的距離以及點到面的距離計算方法分別構造邊緣點以及平面點的約束公式。

邊緣點約束公式：

平面點約束公式：

式中：k為一次掃描周期；為第k次掃描點云中的第i個點在雷達當前坐標系L中的位置向量；為經過旋轉和平移后的位置向量。

優化式（2）和（3），列出第k+1次掃描點云中的第i個點在雷達當前坐標系L中的位置向量和變化后之間的關系為：

經過運動補償后為：

式中：R是滿足羅德里格斯變換的旋轉矩陣；是k+1幀中第i點相對于t k+1時刻的平移。

R變換得：

式中：T L k+1是k+1幀起始時刻點在雷達坐標系中的平移，即：

設函數：

采用列文伯格-馬夸特法（LM）算法求解，迭代過程為：

式中：J為雅可比矩陣。

將上述式子不斷匹配求解，直到收斂，可以獲得雷達坐標系L下的結果，實現相鄰幀之間的變換。而為了定位和建圖，還需要轉化到全局地圖坐標系W下。本文在環境建圖中采用map-to-map方式匹配，并運用PCA主成分思想提取點云中的邊緣點和平面點，再結合式（2）和（3），利用LM法來求解。

3 路徑規劃

3.1 全局路徑規劃

全局路徑規劃[10]主要有三種基本算法：貪婪算法、Dijkstra算法和A*算法[11]。貪婪算法在規劃路徑時尋找的是離目標節點最近的節點作為下一次搜索的起點，但不能保證能找到最優路徑。Dijkstra算法考慮的是當前節點與其周圍節點之間的距離，遍歷完所有節點才可以找到最優路徑。而A*算法是結合了貪婪算法和Dijkstra算法的思想，綜合了兩種算法的優點，是目前應用最廣泛的路徑規劃算法之一[12]。

A*算法是一種典型的啟發式搜索算法，用路徑優劣評價公式表示：

式中：f(n)為節點n的估價函數；g(n)為初始節點到當前節點n的實際代價；h(n)為從節點n到目標節點最佳路徑的估計代價。

算法基本實現過程：從起始點開始計算其每一個子節點的f(n)值，從中選擇f(n)值最小的子節點作為搜索的下一點，往復迭代，直到下一子節點為目標節點[13]。

3.2 局部路徑規劃

AGV在獲得目的地信息后，已經通過全局路徑規劃獲得一條最優路徑，但是環境一旦發生變化，未及時更新地圖時，AGV就未必能做出相應的行動策略。因此，為了應對突發事件，需要AGV能夠實時獲取環境障礙物的信息，即局部路徑規劃[14]，以提高對環境的適應能力。本文采用動態窗口法（Dynamic Window Approach，DWA）[15]來實現AGV的局部路徑規劃。

DWA算法的原理是在線速度v和角速度ω構成的速度空間(v,ω)中不斷采樣，模擬AGV在采樣得到的速度下的運動軌跡，并針對這些軌跡進行評價，從而選取最優的軌跡來驅動AGV運動。

4 實驗與測試

4.1 上位機算法裝載與調試

本文所采用的算法分別為LOAM-Slam建圖算法、A*全局路徑規劃算法和DWA局部避障算法。其中，A*算法和DWA算法已經集成在ROS功能包中，因此只需要搭建LOAM算法并完成算法切換。

本文選用的系統與依賴庫的搭配是Ubuntu 20.04+ROS+PCL1.12.1編譯器+Ceres依賴庫，其中，PCL1.12.1編譯器和Ceres依賴庫都是開源社區的C++庫。PCL用于在AGV掃描建圖中編譯平滑的映射數據，Ceres用于解決地圖建模時的大型復雜的優化問題，兩者相輔相成，適合在大型開放場景中進行地圖掃描與建模，并能夠在較低的功耗中實現算法。

當配置好系統環境，并下載安裝好Ceres依賴庫和LOAM算法之后，還需要配置重定向測試文件。測試文件Bag由LOAM算法社區提供，可以實現無實體小車的模擬場景測試。

運行LOAM時，通過roslaunch命令啟動雷達進行掃描。在地圖創建完成后，退出終端命令窗，系統會自動將4個代表地圖不同數據的pcd文件保存在/tmp目錄下，而每次重啟系統，/tmp目錄下的文件都會被清空。因此在退出系統前，要注意在Rviz中將Map Cloud勾選，才能在測試包運行完畢暫停后，將最終的地圖掃描數據保存到PCL編譯器目錄下層。

4.2 Gazebo環境搭建

Gazebo是ROS系統中的一個動態環境模擬軟件，通常用來測試在復雜環境下的多機器人協同運作實驗。Ga?zebo提供了高保真的物理引擎測試環境，以及一整套的傳感器模型，能夠無限貼近現實地模擬出機器人在實際場景中的運作方式。

如圖1所示，采集一張某高校的衛星俯瞰圖，并勾勒標注好需要進行測試的主要配送道路。在Gazebo中打開場景繪制器，根據衛星俯瞰圖勾勒出的輪廓，在場景繪制器中使用Wall工具繪制出需要測試的封閉道路并保存場景。如圖2所示，在相應的輪廓線條上雙擊編輯每堵Wall的參數，并對每堵不同高度位置的Wall進行填色以作區分。

圖1 某高校主要配送道路的衛星俯瞰圖

圖2 繪制墻面效果

在搭建完仿真環境后，將模擬的小車放入地圖當中，小車可以是Gazebo仿真庫提供的模型，也可以是通過建模外部導入的Urdf模型。本次選用Gazebo仿真庫的模型小車，并在其上添加激光雷達、相機等傳感器，如圖3所示。在Gazebo終端不關閉的情況下，再開啟一個終端來打開Rviz可視化界面。配置LOAM算法后，將雷達和深度相機的傳感關系建立，在Rviz界面中將地圖添加進去。當小車加載完后，通過巡邏整張地圖進行建圖。通過LOAM算法優化并進行灰化處理后的地圖模型如圖4所示。

圖3 小車模型導入

圖4 地圖構建

在建完地圖模型后，通過roscore命令切換出2DSlam來運行A*和DWA算法。通過plan_node.cpp函數構建好Global全局環境變量，運行后的效果如圖5所示，為配送道路某轉角處的路徑處理。同理，在A*算法的基礎上，切換至DWA動態窗口算法后，小車對路徑的選擇會明顯更加謹慎。由于沒有回環檢測，對噪點的過濾效果也不如前者，但是在局部避障方面，DWA算法因為評分機制的存在，能夠讓其在近距離內有效的測算出移動障礙物的相對位姿并及時優化路徑來避開。

4.3 室內場景模擬

AGV小車的開啟步驟與在Gazebo環境下配置流程大致相同。在正式開啟前，需要啟動安裝在工作環境中的激光雷達ROS驅動，并發出tf，以轉換Arduino端發送過來的車輪里程計中的坐標系信息。而APM駕駛儀是與Arduino直連的，可以在樹莓派上通過COMP串口通信獲取數據。完成以上流程后，就可以編輯啟動文件來開啟AGV小車了。圖6所示為室內環境下AGV小車模擬實際場景的定位導航及運動規劃效果。

5 結束語

本文提供了一種可適用于校園AGV配送的定位導航設計方案，就該方案展開了地圖構建和路徑規劃兩大步驟的研究。地圖構建部分采用基于LOAM的算法，將復雜的SLAM問題分為高頻的運動估計和低頻的環境建圖。在運動估計中通過曲率計算提取平面點和邊緣點兩種特征，再采用scan-to-scan方式對連續特征幀進行匹配，并建立誤差模型。在環境建圖中采用map-to-map方式匹配，運用PCA主成分思想提取點云中的特征點，再利用LM法來求解。路徑規劃部分先采用A*算法獲得一條最優的全局路徑規劃路徑，再采用DWA算法對局部進行規劃，以提高對環境的適應能力。實驗證明，本文搭建的AGV配送小車能夠滿足在校園環境多種不同模擬場景中的定位導航需求，為校園AGV配送系統的設計及相關產業的發展提供了一定的技術支持。