朱偉枝，謝娟烘，盧子榮

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

0 引言

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

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

在當今大學校園里，學生人數(shù)眾多，學?？梢哉f是傳染病突發(fā)公共事件的高發(fā)場所[5]?；诖?，本文提供一種校園AGV（Automated Guided Vehicle）地圖構(gòu)建及路徑規(guī)劃設(shè)計方案。AGV是一輛自動導(dǎo)引運輸車，其搭載的激光雷達會實時掃描周邊環(huán)境，采集周圍的環(huán)境信息，對環(huán)境監(jiān)測信息發(fā)出反應(yīng)，識別移動與非移動信號，從而映射到AGV系統(tǒng)的傳動機構(gòu)，實現(xiàn)避障，并根據(jù)設(shè)定地點進行路線規(guī)劃及導(dǎo)航。

1 AGV小車總體設(shè)計

1.1 控制部分

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

1.2 動力部分

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

1.3 雷達部分

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

2 基于SLAM的全局地圖掃描

2.1 定位導(dǎo)航

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

2.2 基于LOAM算法的地圖構(gòu)建

2.2.1 特征提取

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

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

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

2.2.2 特征匹配

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

邊緣點約束公式：

平面點約束公式：

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

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

經(jīng)過運動補償后為：

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

R變換得：

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

設(shè)函數(shù)：

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

式中：J為雅可比矩陣。

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

3 路徑規(guī)劃

3.1 全局路徑規(guī)劃

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

A*算法是一種典型的啟發(fā)式搜索算法，用路徑優(yōu)劣評價公式表示：

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

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

3.2 局部路徑規(guī)劃

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

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

4 實驗與測試

4.1 上位機算法裝載與調(diào)試

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

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

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

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

4.2 Gazebo環(huán)境搭建

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

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

圖1 某高校主要配送道路的衛(wèi)星俯瞰圖

圖2 繪制墻面效果

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

圖3 小車模型導(dǎo)入

圖4 地圖構(gòu)建

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

4.3 室內(nèi)場景模擬

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

5 結(jié)束語

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