基于ROS的移動平臺自主導(dǎo)航研究

張 軍，張宇山，王古超

(1．安徽理工大學(xué)機(jī)械工程學(xué)院，安徽 淮南 232001；2.安徽理工大學(xué)人工智能學(xué)院，安徽 淮南 232001)

隨著中國制造2025的實施，人工智能也再次強(qiáng)勢崛起；其基于數(shù)學(xué)理論的各種智能算法已經(jīng)逐漸應(yīng)用于工業(yè)、服務(wù)業(yè)等多個行業(yè)，以此來實現(xiàn)人類的各種感官功能，實現(xiàn)智能控制。為了簡化機(jī)器人開發(fā)的流程，需要開發(fā)一款面向機(jī)器人系統(tǒng)的設(shè)計平臺。ROS全稱為Robot Operating System,是一款優(yōu)秀的機(jī)器人操作系統(tǒng)，具有獨(dú)特的分布式框架[1]；運(yùn)用基于點(diǎn)對點(diǎn)的進(jìn)程設(shè)計，其每一個進(jìn)程作為一個結(jié)點(diǎn)的形式存在，充當(dāng)獨(dú)立的功能，以此進(jìn)行機(jī)器人軟件的開發(fā)與設(shè)計。與此同時，ROS還具有極大的兼容性、豐富的可視化與仿真工具[2]等特點(diǎn)，為廣大開發(fā)者提供了便利，使代碼的編程變得更加容易，節(jié)省大量時間。如今其應(yīng)用越來越廣，例如室內(nèi)導(dǎo)航、服務(wù)機(jī)器人，水上機(jī)器人等。

然而，在現(xiàn)有的自主導(dǎo)航系統(tǒng)[3-5]中 ，建圖與導(dǎo)航基本上還無法實現(xiàn)深度融合，無法實現(xiàn)時間上的同步。必須在時間上順次執(zhí)行，才能夠?qū)崿F(xiàn)導(dǎo)航的功能。在實現(xiàn)導(dǎo)航功能之前，必須人為給系統(tǒng)以指令，以此來預(yù)先建立完整的柵格地圖[6-7]；接著，需要加載完整的地圖；最后才能采用基于蒙特卡洛采樣原理[8-9]的Acml算法來實現(xiàn)機(jī)器人的定位，才能實現(xiàn)機(jī)器人的自主導(dǎo)航。其智能化的自感知技術(shù)，解決了卡爾曼濾波算法[10-12]的高斯分布局限，但是并沒有完全實現(xiàn)真正意義上的智能化，從而限制了機(jī)器人的適用性?，F(xiàn)如今，對于解決“如何實現(xiàn)機(jī)器人在未知環(huán)境下真正的智能化與無人化的自主探索導(dǎo)航”問題已成為人工智能的又一個研究熱點(diǎn)。本文將運(yùn)用ROS開發(fā)平臺搭建一整套智能化的自主探索導(dǎo)航系統(tǒng)。

1 Mecanum移動平臺系統(tǒng)設(shè)計

此移動平臺的整個控制系統(tǒng)采用“上位機(jī)+下位機(jī)”的模式進(jìn)行布局；在硬件上，由麥克納姆輪實現(xiàn)移動平臺的全向移動，由微機(jī)處理器實現(xiàn)對移動平臺運(yùn)動的閉環(huán)控制,如下圖的移動平臺實物。

圖1 移動平臺實物

下位機(jī)由Stm32f103(ARM嵌入式框架)控制中心組成，上位機(jī)由樹莓派3B處理中心構(gòu)成系統(tǒng)的大腦。利用Stm32f103具有豐富的標(biāo)準(zhǔn)通信接口[13-14]，實現(xiàn)對各種傳感器數(shù)據(jù)的收集，如編碼器與慣導(dǎo)模塊。但是，其處理器的處理速度相對較慢，無法實現(xiàn)通信的實時性，并且高級語言編程困難，無法實現(xiàn)ROS的開發(fā)環(huán)境。為此，上位機(jī)采用樹莓派，其強(qiáng)大的處理能力，可以運(yùn)行Ubuntu系統(tǒng)實現(xiàn)ROS的實時通信，可以作為算法運(yùn)行的硬件基礎(chǔ)。本移動平臺自主導(dǎo)航系統(tǒng)的硬件布局如圖2所示。

圖2 移動平臺硬件布局

在硬件上，下位機(jī)的單片機(jī)主要負(fù)責(zé)對編碼器與慣導(dǎo)模塊的傳感器數(shù)據(jù)收集，并且解析上位機(jī)的位姿數(shù)據(jù)，來驅(qū)動電機(jī)的精確運(yùn)行。上位機(jī)負(fù)責(zé)對高維的大數(shù)據(jù)(激光雷達(dá)的數(shù)據(jù))進(jìn)行收集；高速運(yùn)行復(fù)雜的自主導(dǎo)航算法；精確計算出移動平臺的位姿數(shù)據(jù)；實現(xiàn)上下位機(jī)的位姿數(shù)據(jù)共享。

在軟件算法方面，本系統(tǒng)采用多線程并行設(shè)計的理念，運(yùn)行在時空上并行的節(jié)點(diǎn)slam_gmapping節(jié)點(diǎn)、move_base節(jié)點(diǎn)和my_robot節(jié)點(diǎn)，分別獨(dú)立地實現(xiàn)建圖、導(dǎo)航和驅(qū)動電機(jī)模塊的功能。并且，通過話題(topic)的方式實現(xiàn)各個節(jié)點(diǎn)間數(shù)據(jù)共享的功能。

2 下位機(jī)設(shè)計

下位機(jī)要實現(xiàn)電機(jī)的動作，以及為上位機(jī)的開發(fā)提供底層數(shù)據(jù)信息。由圖2來開發(fā)下位機(jī)，主要開發(fā)任務(wù)設(shè)計如下：

a)實現(xiàn)對編碼器、IMU傳感器的數(shù)據(jù)收集與反饋；

b)與上位機(jī)通信，實現(xiàn)數(shù)據(jù)交換；

c)解析上位機(jī)的速度位姿指令，并且驅(qū)動各自電機(jī)。

(1)Ros移植

本文利用rosserial API來包裝標(biāo)準(zhǔn)ROS序列化消息，并且通過串口通信協(xié)議可以實現(xiàn)上下位機(jī)的通信。其特點(diǎn)是讓Stm32f103單片機(jī)內(nèi)部實現(xiàn)ROS系統(tǒng)內(nèi)部結(jié)點(diǎn)的編程方式；這樣可以采用更為便利的程序?qū)崿F(xiàn)上下位機(jī)的無縫連接，并且采用與上位機(jī)相同編程方式，無需在上位機(jī)再次編程就可以實現(xiàn)結(jié)點(diǎn)的訂閱與發(fā)布；這樣便可以調(diào)用通用的API即可避免直接面對串口通信的編程，實現(xiàn)在單片機(jī)內(nèi)部就可以實現(xiàn)ROS系統(tǒng)中結(jié)點(diǎn)的發(fā)布與訂閱話題的任務(wù)。

(2)速度指令解析

實現(xiàn)對上位機(jī)的速度指令的解析需要分析底盤的運(yùn)動學(xué)，運(yùn)用剛體運(yùn)動學(xué)知識建立移動平臺重心的運(yùn)動與各車輪轉(zhuǎn)速的數(shù)學(xué)關(guān)系，以此實現(xiàn)移動平臺的全方位運(yùn)動。首先，以移動平臺的質(zhì)心為坐標(biāo)原點(diǎn)，對移動平臺建立如下圖3的運(yùn)動學(xué)模型。

圖3 移動平臺運(yùn)動模型

逆運(yùn)動學(xué)模型是在已知重心的速度v與角速度ωz的情況下，計算各輪的轉(zhuǎn)速。依據(jù)剛體運(yùn)動學(xué)理論，可以推導(dǎo)出逆運(yùn)動學(xué)模型下的運(yùn)動方程，如公式(1)所示。這樣便可以由質(zhì)心的運(yùn)動參數(shù)計算出各個全向輪的實時轉(zhuǎn)速，從而實現(xiàn)對上位機(jī)位姿數(shù)據(jù)的準(zhǔn)確解析。

(1)

正運(yùn)動學(xué)模型是在已知各輪的轉(zhuǎn)速的情況下，計算重心的速度v與角速度ωz。可以由(1)式推導(dǎo)出正運(yùn)動學(xué)模型的運(yùn)動方程如下式(2)。

(2)

正運(yùn)動學(xué)模型主要是在收集電機(jī)編碼器數(shù)據(jù)的基礎(chǔ)上，計算出移動底盤的里程計數(shù)據(jù)，并且以反饋控制的方式，實現(xiàn)對整個系統(tǒng)的閉環(huán)控制。

3 上位機(jī)設(shè)計

上位機(jī)的開發(fā)主要是實現(xiàn)：

a)對IMU模塊的九軸傳感器數(shù)據(jù)處理，實現(xiàn)多傳感器數(shù)據(jù)融合，提高數(shù)據(jù)精度；

b)Gmapping功能包建圖；

c)Navigation的Movebase導(dǎo)航；

d)PID參數(shù)調(diào)節(jié)

下位機(jī)目標(biāo)是圍繞導(dǎo)航與slam建圖功能包，匹配各個數(shù)據(jù)類型輸入與輸出，完成自主導(dǎo)航系統(tǒng)的功能。

(1)IMU數(shù)據(jù)濾波與融合

由于編碼器數(shù)據(jù)具有短距離定位精度高，但是累計誤差無法消除。以此，隨著時間的推移，數(shù)據(jù)會逐漸失準(zhǔn)。九軸傳感器的數(shù)據(jù)誤差主要由傳感器自身的零點(diǎn)漂移以及系統(tǒng)誤差造成，具有誤差較大且具有上限。采用數(shù)據(jù)融合算法[15]對兩者數(shù)據(jù)再度處理。對IMU與編碼器位姿數(shù)據(jù)的差值設(shè)定閾值，閾值之內(nèi)的融合數(shù)據(jù)由兩者的加權(quán)均值經(jīng)卡爾曼濾波產(chǎn)生。閾值之外，編碼器數(shù)據(jù)將嚴(yán)重失真，數(shù)據(jù)融合的結(jié)果由IMU數(shù)據(jù)經(jīng)卡爾曼濾波產(chǎn)生。

(2)Slam建圖

本文采用激光雷達(dá)采樣深度信息作為觀測數(shù)據(jù)，以Gmapping功能包為核心。將粒子濾波器用于解決機(jī)器人定位與建圖問題的過程中，形成了Rao-Blackwellize粒子濾波(Rao-Blackwellized Particle Filter,RBPF)算法。與傳統(tǒng)的基于EKF算法在處理非高斯分布的噪音以及算法的復(fù)雜度方面都做了改善，因此又稱為FAST SLAM算法，通過運(yùn)用FAST SLAM算法可以建立2D柵格地圖。算法主要基于貝葉斯概率模型，利用特征估計的獨(dú)立性假設(shè)，采用Rao-Blackwellise分解，得到機(jī)器人的路徑與地圖的聯(lián)合后驗概率分布。系統(tǒng)采用激光雷達(dá)搭配Gmapping算法，而舍棄相機(jī)傳感器，其主要原因在于其數(shù)據(jù)量小的同時精度較高，且受環(huán)境因素影響較小。目前，此定位算法已經(jīng)廣泛使用于室內(nèi)地圖的構(gòu)建。

(3)Navigation導(dǎo)航

導(dǎo)航功能包采用Movebase，運(yùn)用傳感器與里程計信息數(shù)據(jù)以及Gmapping地圖的數(shù)據(jù)反饋進(jìn)行導(dǎo)航。運(yùn)用全局規(guī)劃器進(jìn)行路線最短設(shè)定，運(yùn)用局部規(guī)劃器進(jìn)行避障，最終到達(dá)預(yù)設(shè)的目標(biāo)位置，實現(xiàn)自主導(dǎo)航的功能?？蚣芟到y(tǒng)主要由全局規(guī)劃與局部規(guī)劃兩部分來實現(xiàn)導(dǎo)航功能的。對于全局規(guī)劃，一方面利用全局地圖實現(xiàn)移動平臺全局路徑規(guī)劃， 另一方面及時將全局路徑分段發(fā)送給局部規(guī)劃。局部規(guī)劃則直接與底盤控制器相關(guān)，接收來自全局規(guī)劃的路徑，并進(jìn)行局部避障，實現(xiàn)全局規(guī)劃的分段路徑；同時，將局部路徑的實現(xiàn)轉(zhuǎn)換為運(yùn)動控制命令給底盤控制器，實現(xiàn)底盤的運(yùn)動。

4 實驗結(jié)果

本文采用rviz可視化界面，對如圖4所示的室內(nèi)環(huán)境進(jìn)行自主導(dǎo)航導(dǎo)航實驗。

圖4 室內(nèi)環(huán)境

地圖最終上顯示了一條從當(dāng)前狀態(tài)到目標(biāo)狀態(tài)的全局規(guī)劃路徑，以綠色曲線表示；一條局部規(guī)劃路徑，以紅色曲線顯示；機(jī)器人目標(biāo)位姿以紅色箭頭顯示，如圖5所示。

圖5 導(dǎo)航路徑顯示

隨著時間的積累，需要不斷采集實驗的關(guān)鍵數(shù)據(jù)，來分析此系統(tǒng)的性能。實驗的建圖結(jié)果如圖6所示，最終將建立室內(nèi)完整的環(huán)境柵格地圖；在實現(xiàn)自主導(dǎo)航實驗之前，還需要對慣導(dǎo)單元進(jìn)行糾偏，來消除其系統(tǒng)誤差，慣導(dǎo)模塊的校準(zhǔn)結(jié)果如圖7所示，最終可以將誤差控制在合適的范圍；采集電機(jī)的關(guān)鍵數(shù)據(jù)，繪制電機(jī)轉(zhuǎn)速輸出特性曲線如圖8所示，其反映了電機(jī)轉(zhuǎn)速隨脈沖的變化曲線?？梢钥闯觯D(zhuǎn)速可以實時地跟隨脈沖的變化；根據(jù)運(yùn)動方程和觀測方程在MATLAB中對算法進(jìn)行仿真，并且收集數(shù)據(jù)如表1所示，數(shù)據(jù)反映了采樣算法的精度。

圖6 建圖結(jié)果地圖

圖7 IMU校準(zhǔn)結(jié)果

圖8 電機(jī)轉(zhuǎn)速響應(yīng)波形

表1 FAST-SLAM仿真數(shù)據(jù)

以及各個方向的位姿估計誤差統(tǒng)計如圖9所示。

圖9 位姿誤差

隨著時間的積累，發(fā)布不同的目標(biāo)點(diǎn)數(shù)據(jù)，根據(jù)導(dǎo)航結(jié)果，收集靜態(tài)地圖與動態(tài)地圖導(dǎo)航的統(tǒng)計數(shù)據(jù)，并且繪制于一張表格，以利于兩種導(dǎo)航方式的性能對比，如表2所示。

表2 導(dǎo)航結(jié)果

實驗結(jié)果分析如下：

根據(jù)圖5，隨著時間的推移，導(dǎo)航所形成的地圖在不斷的完善。同時，從列表2可以看出，在不考慮靜態(tài)地圖方法的前期建圖過程的時間，在短距離導(dǎo)航方面，動態(tài)地圖與靜態(tài)地圖方法實現(xiàn)導(dǎo)航的時間代價差在0.01s內(nèi)。因此，動態(tài)導(dǎo)航的方法更加適用于室內(nèi)機(jī)器人的智能導(dǎo)航，導(dǎo)航的速度也會隨時間的積累而越來越快，誤差也可以滿足要求。

5 結(jié)論

圍繞上位機(jī)的ROS系統(tǒng)進(jìn)行完整的導(dǎo)航系統(tǒng)設(shè)計，并對設(shè)計系統(tǒng)的導(dǎo)航效果進(jìn)行總體實驗驗證，結(jié)合導(dǎo)航數(shù)據(jù)與地圖的分析，得出如下結(jié)論：

(1)導(dǎo)航的同時可以同步創(chuàng)建完整的2D柵格地圖，通過時間的積累，地圖逐步更新與增加，最終形成未知環(huán)境的完整地圖。采用本框架結(jié)構(gòu)實現(xiàn)了對機(jī)器人的室內(nèi)自主導(dǎo)航，以及同步建圖。

(2)隨著時間的推移，地圖的不斷完善，移動平臺的平均速度越來越大，自主導(dǎo)航速度不斷加快，效果越來越明顯。

(3)自主導(dǎo)航相對已知地圖環(huán)境的導(dǎo)航來說，時間較長，但以時間的代價可以實現(xiàn)機(jī)器人智能化的提升。