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

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

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

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

1.1 硬件架構(gòu)

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

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

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

1.2 軟件架構(gòu)

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

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

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

2 運(yùn)動(dòng)學(xué)模型

2.1 麥克納姆輪簡介

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

圖3 麥克納姆輪

麥克納姆輪由輪轂和輥?zhàn)觾刹糠纸M成，輪轂軸和輥?zhàn)有D(zhuǎn)傳導(dǎo)軸之間處于45°夾角，如圖3（b）所示，麥克納姆輪在運(yùn)動(dòng)過程中，輥?zhàn)雍偷孛嬷g的摩擦?xí)a(chǎn)生一定的摩擦力，其產(chǎn)生的方向與輥?zhàn)虞S相反[12]。麥克納姆輪通常4個(gè)1組進(jìn)行使用，其中2個(gè)輪為左旋，2個(gè)輪為右旋，以手征對稱的方式2個(gè)1組裝配在小車上[13]。

2.2 運(yùn)動(dòng)學(xué)模型

智能小車運(yùn)動(dòng)學(xué)分析示意圖如圖4所示。

圖4 智能小車運(yùn)動(dòng)學(xué)分析示意圖

以模型質(zhì)心O點(diǎn)為原點(diǎn)，建立全局坐標(biāo)系，該模型具有3個(gè)自由度，中心點(diǎn)速度vx、vy、ωz，設(shè)右前輪的中心為O2，轉(zhuǎn)動(dòng)的角速度為ω2，輥?zhàn)拥乃俣葹関g2，設(shè)2輪在全局坐標(biāo)系下沿x軸方向的分速度為v2x，沿y軸方向的分速度為v2y，分速度與輥?zhàn)拥乃俣燃拜喌霓D(zhuǎn)速有如下關(guān)系[14]

對小車，有

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

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

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

式中：R為車輪半徑；α為電機(jī)軸與輥?zhàn)訆A角；l1為前輪中心線與車身中心線的距離；l2為前輪中心點(diǎn)水平線與車身質(zhì)心水平線的距離。

依據(jù)該矩陣可給定小車一個(gè)速度狀態(tài)，進(jìn)而控制小車運(yùn)動(dòng)。

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

3.1 Gmapping算法

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

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

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

（5）更新粒子集。

（6）進(jìn)行St重采樣。

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

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

圖5 基于Gmapping算法的建圖

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

3.2.1 智能小車定位

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

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

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

3.2.2 全局路徑規(guī)劃

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

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

3.2.3 局部路徑規(guī)劃

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

3.2.4 代價(jià)地圖

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

3.3 move_base功能包

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

圖6 move_base節(jié)點(diǎn)流程圖

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

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

4 實(shí)驗(yàn)與結(jié)果分析

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

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

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

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

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

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

5 結(jié)語

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