基于麥克納姆輪的智能小車SLAM導航研究

吳 迪，杜 峰，蔡一杰，史星彥，胡樂媛

（天津職業技術師范大學，天津300222）

0 引言

同步定位與建圖（Simultaneous Localization And Mapping，SLAM）技術作為智能化研究的重要一環，目前已廣泛應用于物流車、倉儲機器人等領域，而麥克納姆輪作為一種全向移動的運動方式，其轉向范圍較小，在一些特定的復雜環境下比普通的運動模式更為簡單、便捷，如Airtrax開發的Sidewinder系列叉車、Vetex開發的RoboMate全向移動機器人平臺、Kuka公司的YouBot等產品，可在狹窄、緊湊的區域內運動。當前對麥克納姆輪的相關研究已逐漸成熟，Sarmento L等[1]設計的基于麥克納姆輪的全向移動機器人實現了基于無線的遠程遙控；Sang Won Yoon等[2]針對麥克納姆輪在運動過程中的滑移問題，提出多傳感器數據進行卡爾曼濾波融合，消除了傳感器和滑移的噪聲影響，實現了麥克納姆輪移動機器人的精確定位。本文以麥克納姆輪智能小車為研究對象，在ROS（Robot Operating System）系統下研究關于SLAM自主導航與避障，并驗證其可行性。

1 系統框架

本文在基于麥克納姆輪的智能小車上，搭載ROS操作系統，通過傳感器探索未知區域收集環境信息，實現實時同步定位與建圖（SLAM），并通過所建地圖規劃路徑，實現自主導航避障。

1.1 硬件架構

系統硬件包括麥克納姆輪底盤小車，激光雷達、里程計等傳感器，麥克納姆輪底盤小車集成了ROS系統，激光雷達、IMU（Inertial Measurement Unit）、里程計等傳感器搭載在小車上，其硬件架構如圖1所示。

圖1 硬件架構

智能小車裝載樹莓派（內置ROS系統），通過WIFI與PC相連，借用ROS的分布式通信可將主機轉移至PC端，實現遠程操控。激光雷達收集點云信息，通過USB信號傳輸給樹莓派，樹莓派對信息進行相關算法處理后生成控制指令，傳輸給驅動板控制車輪轉速，驅使小車運動，同樣IMU、里程計的速度信號可反饋至樹莓派。

1.2 軟件架構

智能小車裝載樹莓派，以ROS系統為軟件平臺，連接上層環境感知與下層運動控制[3]，ROS是一款機器人操作系統，集成了大量的庫、工具、協議等，提供了一系列可供傳感器數據傳遞、可視的通用類功能包，基于TCP/IP的通信方式，實現軟硬件的實時通信。軟件層架構如圖2所示。

圖2 軟件架構

ROS模塊化的通信機制將整體程序分為三部分，感知層、決策層與下層控制。感知層為各傳感器的數據輸入，導入決策層后通過相關算法分析，生成控制指令傳輸給下層控制，控制小車運動[4]。

2 運動學模型

2.1 麥克納姆輪簡介

麥克納姆輪是一種可全向移動的控制方式，由輪轂和輥子組成，輪轂軸與輥子轉軸空間上呈45°角，如圖3所示，麥輪在運動過程中，輥子與地面摩擦產生摩擦力，摩擦力的方向與輥子軸的方向相反。

圖3 麥克納姆輪

麥輪四個一組應用，兩個左旋輪，兩個右旋輪，左右輪呈手性對稱，但不同安裝方式其受力不同，運動效果也不同。本文安裝方式如圖4所示。

圖4 麥輪安裝圖

2.2 運動學模型

同理可得出各個輪子的vix，viy：

圖5 運動分析圖

可以得到麥克納姆輪智能小車正運動學方程[5]：逆運動學方程[6]：

因此，若給定小車一個速度狀態，可通過式（4）控制小車輪子轉速進而控制智能小車運動。

3 SLAM：同步定位與建圖

SLAM即同步定位與建圖，是指通過對陌生區域的探索，實時構建出此環境的完全地圖。在陌生區域內，要構建地圖就需要自身準確的定位信息，而要掌握自身位姿的信息也需要對比地圖分布，因此，在SLAM問題中，主要處理的是地圖信息及本身的位姿信息，且定位與建圖同步進行。SLAM模型如圖6所示。

圖6 SLAM模型

圖6中，灰色三角形與實線表示小車t（t=1，2，）時刻的實際運動位姿，表示小車t時刻的位姿狀態，白色三角形與虛線表示對小車t時刻估計的位姿信息，方形代表路標信息。小車運動使得自身位姿發生變化，里程計對位姿信息進行更新，但在實際運行過程中，因為環境因素，運動模型誤差，信息延遲等因素，對小車的位姿估計往往與實際值產生較大誤差，并且誤差伴隨著系統不斷運算而累積疊加，這也就需要智能小車在運動過程中通過傳感器的觀測信息（激光、IMU等）不斷矯正誤差，利用激光、IMU信息自定位[7]。總的來說，SLAM問題實質是針對位姿信息“估計-觀測-校正-估計”不斷循環的過程。

3.1 基于圖優化的SLAM理論

SLAM算法大體分為兩類：基于濾波的SLAM算法與基于圖優化的SLAM算法。基于濾波的SLAM算法原理是根據傳感數據信息去估計機器人當前時刻的位姿信息，其問題在于，若位姿估計錯誤會使得之后的計算無法修正，特別是大型復雜環境下此缺點尤為突出。

與基于濾波的方法不同，基于圖優化的SLAM不僅僅是對小車當前位姿的估計，而是對整體運動軌跡和地圖的完整估計。將SLAM問題描述成圖結構，圖由節點與邊組成，其節點（Node）表示小車位姿信息，而位姿之間的空間約束則構成邊（Edge）[8]。圖優化SLAM可分解成兩部分：（1）構建圖，小車位姿為節點，位姿約束關系為邊，稱為前端（Front-end）；（2）優化圖，即調整小車位姿節點以滿足邊的約束，稱為后端（Back-end）。本文采用基于圖優化的Cartographer-SLAM算法。

3.2 Cartographer算法架構

Cartographer算法[9]是基于圖優化理論的SLAM框架，分為局部SLAM（也稱前端）和全局SLAM（也稱后端），其建圖步驟如圖7所示，Cartographer框架如圖8所示。

[28]Eli Pariser, The Filter Bubble, London: Penguin Books Ltd (UK), 2012.

圖7 建圖步驟

圖8 Cartographer算法框架

（1）局部SLAM

由IMU和里程計通過互補濾波的方式預測初始的位姿，建立匹配機制，即通過非線性最小二乘進行優化求解得到最優匹配，算法中通過調用Ceres庫[10]執行。激光掃描后，得出的位姿可看作一個節點，且點云信息會經由體素濾波器去除冗余的點云信息，然后與IMU、里程計預測的位姿子圖進行匹配Scan to map[10]，優化位姿的同時不斷插入數據幀使得子圖更新，當子圖不再更新即認為子圖創建完成。接著去創建下一個子圖，得出的最優位姿用作下一次的初始位姿，循環過程。

（2）回環檢測

Cartographer通過創建大量的子圖來實現場景建圖，子圖在短時間內的準確度是可靠的，但長時間會存在累積誤差，為了消除累積誤差，需要通過回環檢測來構建回環，最終是為了優化所有子圖的位姿，檢測回環的方法為分支定界法[10]，將檢測的節點所攜帶的點云與當時的子圖進行匹配，可得到回環約束位姿。

（3）全局SLAM

根據局部SLAM過程，節點與節點之間存在邊的約束，節點與子圖之間可看作局部約束，但對于回環約束，有可能節點與子圖不一定在同一軌跡中，即同一節點可能與多個子圖存在匹配關系，這也就是全局約束，針對全局的約束問題進行優化即為全局SLAM的過程。

4 實驗

實驗設備為PIBOT智能小車，裝載激光雷達思嵐A2，內置編碼里程計、IMU，環境為室內實驗室，如圖 9（a）所示。

圖9 實驗設備與環境

通過PC端與小車WIFI相連，控制小車移動探索區域建圖，遍歷實驗室環境（圖9（b）），通過Cartographer算法構建出的地圖效果如圖9（c）所示，從圖9（b）和圖9（c）中可以看出，所構建的地圖能夠反映出實驗室的環境特征，其中桌面下端是連通的，符合環境特征。

智能小車導航，全局路徑規劃采用A*算法[11]，局部路徑規劃采用DWA算法[12]。根據構建的地圖給定機器人目標點與起始點，如圖10（a）所示，機器人按照規劃的路徑自主導航。在全局的路徑上放置障礙物，如圖 10（b）所示，其路徑隨之改變圖 10（c），機器人成功避開障礙物到達目標終點。

圖10 SLAM建圖與導航圖

5 結束語

麥克納姆輪全向移動智能小車搭載ROS系統，基于Cartographer算法下能完成室內環境下的同步定位與地圖構建，且能實現自主導航與實時避障功能，對全向移動的定位導航具有良好的應用價值。