狹窄緊湊環境下智能小車SLAM導航實驗

王 鐸，杜 峰，關志偉，2，趙 彪，劉云鵬

（1.天津職業技術師范大學汽車與交通學院，天津 300222；2.天津中德應用技術大學汽車與軌道交通學院，天津 300350）

同步定位與建圖（simultaneous localization and mapping，SLAM）是指搭載傳感器的主體，從陌生環境的任意地點出發，通過傳感器完成與環境的信息交互來確定自身位置，構建增量地圖，從而實現同步定位與地圖構建的功能[1]。該技術作為人工智能技術不可或缺的一環，已大量應用于全方位移動平臺、倉儲機器人、物流機器人等領域，而這些領域有一個共同特征——工作環境狹窄而緊湊，智能移動平臺的活動空間較小[2]。傳統的阿克曼轉向方式因轉向時占用空間較大而不適用于狹窄緊湊的工況[3]，故迫切需要一種更高效的轉向方式來應對該工作環境。麥克納姆輪的出現完美地解決了這個問題。麥克納姆輪具有靈活的運動控制方式、良好的機動性，能夠在任意角度實現平移或原地旋轉等優勢，這種移動方式在空間狹窄，活動空間有限的環境里尤為合適[4]。如Airtrax開發的Sidewinder叉車、Vetex公司研發生產的RoboMate全向移動平臺等產品，均針對狹小、多變的工作環境而設計。Sarmento等[5]將無線遠程遙控技術應用到裝載了麥克納姆輪技術的智能移動平臺中，為麥克納姆輪技術在智能移動平臺的進一步普及做出了貢獻。隨著麥克納姆輪技術日趨成熟，國內外學者對其在智能移動平臺的自主導航問題進行了更深入的研究，徐守江[6]針對未知全局靜態環境智能移動平臺的路徑規劃問題，提出了一種新穎的基于粒子群算法的滾動規劃算法。李維維等[7]為了提高智能移動平臺在柵格環境下的路徑規劃質量和規劃穩定性，提出了啟發式信息素交流異構雙種群蟻群算法的規劃方法。針對農業機器人的自主導航問題，Nehme等[8]提出了一種穩健的基于激光雷達的藤行結構跟蹤解決方案。在機器人自主導航的領域，國內外的研究已經有了很大的進展，但針對狹窄緊湊環境下的機器人自主導航研究還稍顯薄弱。本文選取裝載ROS系統的麥克納姆輪智能小車平臺作為研究對象，對狹窄環境下的智能小車膨脹半徑參數進行實驗優化研究。

1 智能小車系統框架

1.1 硬件架構

智能小車的硬件結構包括：里程計與單線激光雷達等傳感器、麥克納姆輪底盤、樹莓派（配有ROS系統）主機、集成了IMU的驅動面板（STM32）、編碼電動機。硬件架構如圖1所示，小車硬件架構如圖1（a）所示，實物圖如圖1（b）所示，車身長30 cm，輪間距31 cm。

圖1 智能小車硬件架構

借助智能小車平臺發出的Wi-Fi信號，PC端通過ROS的分布式通訊遠端登入樹莓派，實現對樹莓派的遠程管理。單線激光雷達經USB接口把采集的點云數據發送給樹莓派，樹莓派向驅動板傳達控制指令。當小車移動后，里程計與IMU也會反饋信息至樹莓派，用以確定小車的位姿信息。

1.2 軟件架構

ROS[9]是一種集成使用了多種通信協議、工具庫的智能機器人控制系統，提供了各種通信功能包，使用TCP/UDP網絡，可以進行各種通信和機制之間的實時數據交互傳輸。因此，搭載ROS系統的樹莓派平臺可實現對上層工作環境變化的實時感知和對下層決策過程的管理。智能小車系統的軟件框架結構如圖2所示。

圖2 智能小車軟件架構

智能小車的通信交互機制主要可簡單概括歸納為以下3個核心模塊：感知層、決策層、控制層。感知層為傳感器數據，導入決策層即為ROS主機通過算法進行分析并下達命令傳送至小車驅動板，即控制層，支配小車運動[10]。

2 運動學模型

2.1 麥克納姆輪簡介

麥克納姆輪[11]的概念最早由瑞典Mecanum公司的工程師提出，麥克納姆輪是一種全新的移動方式，裝載麥克納姆輪的智能小車在不改變自身位姿的狀態下可以實現全方位移動。麥克納姆輪如圖3所示。

圖3 麥克納姆輪

麥克納姆輪由輪轂和輥子兩部分組成，輪轂軸和輥子旋轉傳導軸之間處于45°夾角，如圖3（b）所示，麥克納姆輪在運動過程中，輥子和地面之間的摩擦會產生一定的摩擦力，其產生的方向與輥子軸相反[12]。麥克納姆輪通常4個1組進行使用，其中2個輪為左旋，2個輪為右旋，以手征對稱的方式2個1組裝配在小車上[13]。

2.2 運動學模型

智能小車運動學分析示意圖如圖4所示。

圖4 智能小車運動學分析示意圖

以模型質心O點為原點，建立全局坐標系，該模型具有3個自由度，中心點速度vx、vy、ωz，設右前輪的中心為O2，轉動的角速度為ω2，輥子的速度為vg2，設2輪在全局坐標系下沿x軸方向的分速度為v2x，沿y軸方向的分速度為v2y，分速度與輥子的速度及輪的轉速有如下關系[14]

對小車，有

將式（1）、（2）聯立消去vg2可得

同理，其他3輪通過推導，也可得出式（3）。

將所有車輪轉動角速度公式聯立轉化為矩陣形式為

式中：R為車輪半徑；α為電機軸與輥子夾角；l1為前輪中心線與車身中心線的距離；l2為前輪中心點水平線與車身質心水平線的距離。

依據該矩陣可給定小車一個速度狀態，進而控制小車運動。

3 同步定位與地圖構建

3.1 Gmapping算法

Gmapping算法在構建場地較小面積的地圖時，相比于其他SLAM算法具有計算量更小、魯棒性更強、且更適用于單線激光雷達等優勢，因此也更適合于本文研究的智能小車平臺及狹窄緊湊的實驗環境，故本文采用Gmapping算法構建高精度地圖。Gmapping實時計算技術[15]以RBPF SLAM為基礎，通過激光雷達和里程計所采集到的實時周圍環境和位姿信息，完善了粒子分布計算和數據導入的自適應重采樣計算機制，極大改善了實時計算效率，適用于對數據實時化處理精度要求相對較高的實際應用場合。該算法的流程如下：

（1）輸入t－1時刻的里程計消息Ut-1，粒子集合St-1，t時刻的觀測值Zt。

（3）觀察更新，以運動更新后的粒子位置和上一時刻粒子權值為依據，計算當下粒子權值。

（5）更新粒子集。

（6）進行St重采樣。

RBPF SLAM中的后驗概率p（x1：t|z1：t，u1：t）的近似值被修改為p（x1：t|m（i）t-1，x（i）t-1，z1：t，u1：t-1），將上一次的里程計數據也考慮在內，極大減少了粒子擴散的問題。通過傳感器與里程計數據可以推測出機器人的位姿與軌跡信息，從而計算地圖的概率分布，最終得到精確地圖。

通過與智能小車平臺發出的無線Wi-Fi信號相連接，在PC端使用鍵盤控制小車移動探索區域建圖，遍歷實驗室，通過Gmapping算法構建出的地圖效果如圖5所示。

圖5 基于Gmapping算法的建圖

3.2 智能小車自主導航

3.2.1 智能小車定位

在SLAM中智能小車根據已構建好的地圖進行自主導航的前提是要確定自身的位姿與當下在地圖中所處的位置。智能小車定位問題可分為局部定位與全局定位，其判斷依據為是否確定小車初始位姿。在初始位姿已知的情況下，智能小車準確追蹤自身位姿即為局部定位，也稱位姿跟蹤。智能小車初始位姿狀態不確定，此種情況下追蹤自身位姿被稱為全局定位。然而，也存在另一種情況，采用SLAM創建環境地圖后，小車遭遇“綁架問題”，即智能小車突然被移置其他位置，無法識別自身在地圖中所處的位姿狀態。針對此種問題，本文采用自適應蒙特卡洛定位算法（adaptive Monte Carlo localization，AMCL）。該算法將智能小車運動中獲取的觀測數據更新粒子進行重采樣處理，并在其過程中加入隨機粒子以完成更精確的定位，添加的粒子數目可通過平均測量概率決定。粒子的權值為觀測模型的隨即估計，將粒子的權值與總體觀測概率進行比對，可得出隨機粒子數量的近似值

式中：z為系統觀測值；u為控制輸入；m為地圖。

粒子的分布可以根據姿態空間上的均勻分布繪制粒子，并利用當前觀測模型對粒子進行加權，該算法適用于大范圍的局部定位[16]。

3.2.2 全局路徑規劃

全局路徑規劃是指智能小車在地圖中從當前位置移動到目標位置時，所選擇的一條最佳移動路線，該路線在絕大多數情況下是最短的路線，在特殊情況下也可能由其他需求決定，如功率或速度。全局路徑規劃主要采用Dijkstra或A*算法[17]，本文采用A*算法。A*算法與Dijkstra算法的不同之處在于，其在路徑搜索中添加了影響因子，使用啟發式函數h（n），將啟發信息融入估計函數中，使得當前路徑的選擇能快速收斂至目標點附近，這樣使得在尋找最短路徑時無需遍歷所有節點，節省搜索時間的同時，也減少了計算過程。其估計函數可表示為

式中：g（n）為從初始節點到當前節點n的真實代價值；h（n）為當前節點n到目標點的估計代價；函數f（n）為初始點到當前節點n的總估價函數。

3.2.3 局部路徑規劃

局部路徑規劃主要通過控制智能小車的角速度和線速度，引導智能小車盡可能沿著己經規劃好的最優線路移動，使得小車盡可能符合整體最優線路的規劃。當周圍環境中出現地圖中不存在的障礙物時，智能小車將依據傳感器實時監測到的信息，判定與障礙物之間的距離并重新規劃路徑進行避障。局部路徑規劃通常使用動態窗口法（dynamical window approach，DWA）[18]。DWA算法是在一定時間內，通過外部傳感器采樣智能小車線速度和角速度，利用智能小車運動學模型與相關障礙物信息對速度空間進行一定范圍限制（稱之窗口），然后對采樣的速度空間進行數值模擬，規劃小車可行走的多條路徑，再通過評價函數對多條路徑進行評估，選取最優的路徑作為小車行駛路線。本文采用DWA算法用以進行動態避障。

3.2.4 代價地圖

代價地圖用來儲存周圍環境障礙信息，全局路徑代價地圖（global_costmap）儲存全局路徑規劃信息，局部路徑代價地圖（local_costmap）儲存本地路徑規劃信息。因此，還需要創建通用配置文件來為這2種代價地圖儲存一些通用或單獨的配置文件，該文件用來存儲一些參數，其中inflation_radius參數（以下簡稱膨脹半徑參數）用來設置障礙物的膨脹參數，即小車與障礙物之間應保持的最小安全距離。本車默認值為1.2，初步判斷，如果該參數初始值設置過大，將導致小車的通過性與安全性較差，進而影響小車在狹窄環境下的導航效果。因此，該參數的優化是本研究的重點。

3.3 move_base功能包

當控制智能小車利用Gmapping算法建好地圖后，通過ROS系統中的Rviz可視化界面為智能小車在已構建好的地圖中設定目標位置，智能小車便會調用move_base功能包躲避障礙物并移動到給定的目標位置。智能小車運動時，move_base功能包會進行全面的路徑規劃。在進行路徑規劃時，根據傳感器實時數據、已構建好的地圖、配置文件中給定的速度，實時調整小車的角速度、線速度和加速度。該功能包必須具備的話題包括LaserScan（激光雷達信息）、Odom（里程計信息）、TF（各個坐標系的轉換關系）、Path（規劃路徑）及Map（構建的地圖信息）。該功能包節點流程如圖6所示。move_base功能包主要實現全局路徑規劃（global_planner）與局部路徑規劃（local_planner）兩大功能?；趍ove_base功能包的小車導航如圖7所示。

圖6 move_base節點流程圖

從圖7（a）可以看出，在構建好的地圖中給定目標點，智能小車根據move_base功能包所規劃的最優路徑（全局路徑規劃）進行自主導航。當在路徑上放置動態障礙物時，如圖7（b）所示，此時小車開始利用局部路徑規劃的DWA算法重新規劃局部路徑完成實時避障，如圖7（c）所示。

圖7 基于Move_base功能包的小車導航

4 實驗與結果分析

實驗研究發現，代價地圖中的膨脹半徑參數可能是在狹窄壞境下影響小車通過性能的最關鍵因素。因此，需要利用圖5所建立的實際狹窄環境地圖進行驗證和確認，以達到進一步提高智能小車避障與通過性的要求。

使用Gmapping算法構建地圖，在地圖上設置一個目標點，同時在目標點前設置建圖時不存在的障礙物，使智能小車可通過的空間長度僅有35 cm（小車輪距為31 cm），且只能從此通道通過（如圖7（b）所示）。在不同的膨脹半徑參數下，使用PC端控制小車到達目標點，觀察智能小車的全局路徑規劃，以及是否能夠通過局部路徑規劃實時規避障礙物到達目標點。全局路徑規劃如圖8所示。

圖8 不同參數的膨脹半徑全局路徑規劃路線

從圖8可以看出，隨著膨脹半徑參數的遞減，周圍環境的自由空間逐漸增大，智能小車的全局路徑規劃策略也隨之變化；膨脹半徑參數越小，全局路徑規劃越簡潔，智能小車的空間利用效率越高。當膨脹半徑為0.1時，智能小車的路徑規劃最簡單，但卻與周圍環境發生碰撞，如圖8（e）、（f）所示。究其原因是由于膨脹半徑參數選取過小，智能小車與周圍環境最小安全距離過小。代價地圖通用配置文件中檢測障礙物最小高度的參數（min_obstacle_height）為0.05 m，如果障礙物低于0.05 m，智能小車無法識別。膨脹半徑過小，智能小車就會因規劃的路徑與障礙物過近而相碰。因此，智能小車或機器人的結構不同，選取的膨脹半徑參數亦不相同。智能小車具體導航結果如表1所示。

表1 智能小車導航結果

從表1可以看出，智能小車在膨脹半徑參數分別為1.2、1.0、0.8時，雖然不能繞過障礙物通過狹窄空間，但也不會與障礙物發生碰撞。這是因為膨脹半徑參數選取較大，智能小車判定通過不了狹窄空間，所以終止了命令。而膨脹半徑參數為0.6和0.4時，小車沒有通過狹窄空間，同時與障礙物發生了碰撞，是因為此時智能小車判斷可以通過障礙物，但由于選取的膨脹半徑參數不合適，與實際場景產生偏差，導致小車與障礙物相碰。綜上所述，膨脹半徑參數為0.2是最適合本智能小車在當前環境下工作的參數，既可以在全局路徑規劃時與周圍的環境保持足夠的安全距離，同時對空間的利用也相對高效，而且進行局部路徑規劃時，也能保證智能小車繞過障礙物通過足夠狹小的空間。

5 結語

本文針對智能小車自主導航時在狹窄、緊湊的工況下通過性較差的問題，提出了智能小車代價地圖通用配置文件的膨脹半徑參數是決定智能小車在狹窄環境下通過性能的決定性參數的假定，并通過實驗方法予以驗證，找到了最適合所研究環境的膨脹半徑參數，同時也證實了不同的智能小車或機器人在不同環境下的參數設置不能簡單的統一處理。在未來的物流領域發展中，單線激光雷達SLAM將會占據越來越重要的地位。因此，本實驗研究對于智能小車在狹窄緊湊環境下的導航問題具有一定的指導意義。