狹窄緊湊環(huán)境下智能小車SLAM導(dǎo)航實驗

王 鐸，杜 峰，關(guān)志偉，2，趙 彪，劉云鵬

（1.天津職業(yè)技術(shù)師范大學(xué)汽車與交通學(xué)院，天津 300222；2.天津中德應(yīng)用技術(shù)大學(xué)汽車與軌道交通學(xué)院，天津 300350）

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

1 智能小車系統(tǒng)框架

1.1 硬件架構(gòu)

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

圖1 智能小車硬件架構(gòu)

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

1.2 軟件架構(gòu)

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

圖2 智能小車軟件架構(gòu)

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

2 運動學(xué)模型

2.1 麥克納姆輪簡介

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

圖3 麥克納姆輪

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

2.2 運動學(xué)模型

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

圖4 智能小車運動學(xué)分析示意圖

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

對小車，有

將式（1）、（2）聯(lián)立消去vg2可得

同理，其他3輪通過推導(dǎo)，也可得出式（3）。

將所有車輪轉(zhuǎn)動角速度公式聯(lián)立轉(zhuǎn)化為矩陣形式為

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

依據(jù)該矩陣可給定小車一個速度狀態(tài)，進而控制小車運動。

3 同步定位與地圖構(gòu)建

3.1 Gmapping算法

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

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

（3）觀察更新，以運動更新后的粒子位置和上一時刻粒子權(quán)值為依據(jù)，計算當(dāng)下粒子權(quán)值。

（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），將上一次的里程計數(shù)據(jù)也考慮在內(nèi)，極大減少了粒子擴散的問題。通過傳感器與里程計數(shù)據(jù)可以推測出機器人的位姿與軌跡信息，從而計算地圖的概率分布，最終得到精確地圖。

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

圖5 基于Gmapping算法的建圖

3.2 智能小車自主導(dǎo)航

3.2.1 智能小車定位

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

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

粒子的分布可以根據(jù)姿態(tài)空間上的均勻分布繪制粒子，并利用當(dāng)前觀測模型對粒子進行加權(quán)，該算法適用于大范圍的局部定位[16]。

3.2.2 全局路徑規(guī)劃

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

式中：g（n）為從初始節(jié)點到當(dāng)前節(jié)點n的真實代價值；h（n）為當(dāng)前節(jié)點n到目標(biāo)點的估計代價；函數(shù)f（n）為初始點到當(dāng)前節(jié)點n的總估價函數(shù)。

3.2.3 局部路徑規(guī)劃

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

3.2.4 代價地圖

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

3.3 move_base功能包

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

圖6 move_base節(jié)點流程圖

從圖7（a）可以看出，在構(gòu)建好的地圖中給定目標(biāo)點，智能小車根據(jù)move_base功能包所規(guī)劃的最優(yōu)路徑（全局路徑規(guī)劃）進行自主導(dǎo)航。當(dāng)在路徑上放置動態(tài)障礙物時，如圖7（b）所示，此時小車開始利用局部路徑規(guī)劃的DWA算法重新規(guī)劃局部路徑完成實時避障，如圖7（c）所示。

圖7 基于Move_base功能包的小車導(dǎo)航

4 實驗與結(jié)果分析

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

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

圖8 不同參數(shù)的膨脹半徑全局路徑規(guī)劃路線

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

表1 智能小車導(dǎo)航結(jié)果

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

5 結(jié)語

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