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

（天津職業(yè)技術(shù)師范大學(xué)，天津300222）

0 引言

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

1 系統(tǒng)框架

本文在基于麥克納姆輪的智能小車上，搭載ROS操作系統(tǒng)，通過傳感器探索未知區(qū)域收集環(huán)境信息，實(shí)現(xiàn)實(shí)時(shí)同步定位與建圖（SLAM），并通過所建地圖規(guī)劃路徑，實(shí)現(xiàn)自主導(dǎo)航避障。

1.1 硬件架構(gòu)

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

圖1 硬件架構(gòu)

智能小車裝載樹莓派（內(nèi)置ROS系統(tǒng)），通過WIFI與PC相連，借用ROS的分布式通信可將主機(jī)轉(zhuǎn)移至PC端，實(shí)現(xiàn)遠(yuǎn)程操控。激光雷達(dá)收集點(diǎn)云信息，通過USB信號(hào)傳輸給樹莓派，樹莓派對(duì)信息進(jìn)行相關(guān)算法處理后生成控制指令，傳輸給驅(qū)動(dòng)板控制車輪轉(zhuǎn)速，驅(qū)使小車運(yùn)動(dòng)，同樣IMU、里程計(jì)的速度信號(hào)可反饋至樹莓派。

1.2 軟件架構(gòu)

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

圖2 軟件架構(gòu)

ROS模塊化的通信機(jī)制將整體程序分為三部分，感知層、決策層與下層控制。感知層為各傳感器的數(shù)據(jù)輸入，導(dǎo)入決策層后通過相關(guān)算法分析，生成控制指令傳輸給下層控制，控制小車運(yùn)動(dòng)[4]。

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

2.1 麥克納姆輪簡介

麥克納姆輪是一種可全向移動(dòng)的控制方式，由輪轂和輥?zhàn)咏M成，輪轂軸與輥?zhàn)愚D(zhuǎn)軸空間上呈45°角，如圖3所示，麥輪在運(yùn)動(dòng)過程中，輥?zhàn)优c地面摩擦產(chǎn)生摩擦力，摩擦力的方向與輥?zhàn)虞S的方向相反。

圖3 麥克納姆輪

麥輪四個(gè)一組應(yīng)用，兩個(gè)左旋輪，兩個(gè)右旋輪，左右輪呈手性對(duì)稱，但不同安裝方式其受力不同，運(yùn)動(dòng)效果也不同。本文安裝方式如圖4所示。

圖4 麥輪安裝圖

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

同理可得出各個(gè)輪子的vix，viy：

圖5 運(yùn)動(dòng)分析圖

可以得到麥克納姆輪智能小車正運(yùn)動(dòng)學(xué)方程[5]：逆運(yùn)動(dòng)學(xué)方程[6]：

因此，若給定小車一個(gè)速度狀態(tài)，可通過式（4）控制小車輪子轉(zhuǎn)速進(jìn)而控制智能小車運(yùn)動(dòng)。

3 SLAM：同步定位與建圖

SLAM即同步定位與建圖，是指通過對(duì)陌生區(qū)域的探索，實(shí)時(shí)構(gòu)建出此環(huán)境的完全地圖。在陌生區(qū)域內(nèi)，要構(gòu)建地圖就需要自身準(zhǔn)確的定位信息，而要掌握自身位姿的信息也需要對(duì)比地圖分布，因此，在SLAM問題中，主要處理的是地圖信息及本身的位姿信息，且定位與建圖同步進(jìn)行。SLAM模型如圖6所示。

圖6 SLAM模型

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

3.1 基于圖優(yōu)化的SLAM理論

SLAM算法大體分為兩類：基于濾波的SLAM算法與基于圖優(yōu)化的SLAM算法?；跒V波的SLAM算法原理是根據(jù)傳感數(shù)據(jù)信息去估計(jì)機(jī)器人當(dāng)前時(shí)刻的位姿信息，其問題在于，若位姿估計(jì)錯(cuò)誤會(huì)使得之后的計(jì)算無法修正，特別是大型復(fù)雜環(huán)境下此缺點(diǎn)尤為突出。

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

3.2 Cartographer算法架構(gòu)

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

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

圖7 建圖步驟

圖8 Cartographer算法框架

（1）局部SLAM

由IMU和里程計(jì)通過互補(bǔ)濾波的方式預(yù)測初始的位姿，建立匹配機(jī)制，即通過非線性最小二乘進(jìn)行優(yōu)化求解得到最優(yōu)匹配，算法中通過調(diào)用Ceres庫[10]執(zhí)行。激光掃描后，得出的位姿可看作一個(gè)節(jié)點(diǎn)，且點(diǎn)云信息會(huì)經(jīng)由體素濾波器去除冗余的點(diǎn)云信息，然后與IMU、里程計(jì)預(yù)測的位姿子圖進(jìn)行匹配Scan to map[10]，優(yōu)化位姿的同時(shí)不斷插入數(shù)據(jù)幀使得子圖更新，當(dāng)子圖不再更新即認(rèn)為子圖創(chuàng)建完成。接著去創(chuàng)建下一個(gè)子圖，得出的最優(yōu)位姿用作下一次的初始位姿，循環(huán)過程。

（2）回環(huán)檢測

Cartographer通過創(chuàng)建大量的子圖來實(shí)現(xiàn)場景建圖，子圖在短時(shí)間內(nèi)的準(zhǔn)確度是可靠的，但長時(shí)間會(huì)存在累積誤差，為了消除累積誤差，需要通過回環(huán)檢測來構(gòu)建回環(huán)，最終是為了優(yōu)化所有子圖的位姿，檢測回環(huán)的方法為分支定界法[10]，將檢測的節(jié)點(diǎn)所攜帶的點(diǎn)云與當(dāng)時(shí)的子圖進(jìn)行匹配，可得到回環(huán)約束位姿。

（3）全局SLAM

根據(jù)局部SLAM過程，節(jié)點(diǎn)與節(jié)點(diǎn)之間存在邊的約束，節(jié)點(diǎn)與子圖之間可看作局部約束，但對(duì)于回環(huán)約束，有可能節(jié)點(diǎn)與子圖不一定在同一軌跡中，即同一節(jié)點(diǎn)可能與多個(gè)子圖存在匹配關(guān)系，這也就是全局約束，針對(duì)全局的約束問題進(jìn)行優(yōu)化即為全局SLAM的過程。

4 實(shí)驗(yàn)

實(shí)驗(yàn)設(shè)備為PIBOT智能小車，裝載激光雷達(dá)思嵐A2，內(nèi)置編碼里程計(jì)、IMU，環(huán)境為室內(nèi)實(shí)驗(yàn)室，如圖 9（a）所示。

圖9 實(shí)驗(yàn)設(shè)備與環(huán)境

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

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

圖10 SLAM建圖與導(dǎo)航圖

5 結(jié)束語

麥克納姆輪全向移動(dòng)智能小車搭載ROS系統(tǒng)，基于Cartographer算法下能完成室內(nèi)環(huán)境下的同步定位與地圖構(gòu)建，且能實(shí)現(xiàn)自主導(dǎo)航與實(shí)時(shí)避障功能，對(duì)全向移動(dòng)的定位導(dǎo)航具有良好的應(yīng)用價(jià)值。