白 創(chuàng),閆 昱,陳 立

(長沙理工大學(xué)物理與電子科學(xué)學(xué)院,湖南 長沙 410114)

0 引言

為了確保室內(nèi)機(jī)器人的人機(jī)交互的安全性和穩(wěn)定性，更精確的移動機(jī)器人導(dǎo)航系統(tǒng)必不可少[1]，室內(nèi)機(jī)器人導(dǎo)航系統(tǒng)可以分為同步定位與建圖(simultaneous localization and mapping,SLAM)[2]、重定位和路徑規(guī)劃3大模塊。黃通交等[3]設(shè)計(jì)了一種移動機(jī)器人系統(tǒng)，使用Cartographer算法[4]進(jìn)行室內(nèi)建圖，但是在Long-Corridor場景下建圖精度較差；Xu等[5]為了提高在Long-Corridor場景下的建圖精度，在Cartographer算法中增加延時(shí)判斷模塊，過濾錯(cuò)誤的回環(huán)點(diǎn)的方法，但是提升的精度不高；Wang等[6]提出了一種基于機(jī)器人操作系統(tǒng)(robot operating system,ROS)[7]的自主跨層導(dǎo)航系統(tǒng)，使用自適應(yīng)蒙特卡羅定位(adaptive Monte Carlo localization,AMCL)算法[8]進(jìn)行定位，但是定位的耗時(shí)較長；Lim等[9]提出了一種利用聲納數(shù)據(jù)對移動機(jī)器人進(jìn)行重定位的方法，定位耗時(shí)短，但是需要對環(huán)境中的每一種物體進(jìn)行物理建模，否則會出現(xiàn)定位錯(cuò)誤；張明岳[10]對室內(nèi)自主移動與導(dǎo)航機(jī)器人展開研究，在路徑規(guī)劃采用A*算法[11]，但是具有冗余拐點(diǎn)較多、搜索時(shí)間較長的問題;Szazaerba等[12]提出了一種稀疏 A*算法，在基本 A*算法中添加了搜索約束條件，提高了算法的搜索效率，但是沒有對冗余拐點(diǎn)進(jìn)行優(yōu)化。

因此，本文基于機(jī)器人操作系統(tǒng)，對3大模塊同時(shí)進(jìn)行優(yōu)化，實(shí)現(xiàn)一種高性能室內(nèi)移動機(jī)器人導(dǎo)航系統(tǒng)。在SLAM模塊中，選擇雷達(dá)、里程計(jì)和IMU這3種數(shù)據(jù)融合的方式，極大提高了Long-Corridor場景下建圖精度；在重定位模塊中，采用一種基于掃描匹配的方法，減少了重定位耗時(shí)；在路徑規(guī)劃模塊中，提出一種改進(jìn)A*全局規(guī)劃算法，縮短了算法搜索耗時(shí)并去除了路徑冗余拐點(diǎn)。

1 多傳感器融合的Cartographer算法

在室內(nèi)Long-Corridor場景下，先用擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)算法[13]融合里程計(jì)數(shù)據(jù)和慣性測量單元(inertial measurement unit,IMU)數(shù)據(jù)，再將融合后的數(shù)據(jù)與激光數(shù)據(jù)經(jīng)過EKF算法再次融合進(jìn)行位姿估計(jì)，即可得到較為精準(zhǔn)的定位，從而提高Cartographer算法在Long-Corridor場景下建圖精度。下面以EKF算法融合里程計(jì)數(shù)據(jù)與IMU數(shù)據(jù)為例說明。

室內(nèi)機(jī)器人采用二輪差分小車，其運(yùn)動方程可表示為

(1)

將運(yùn)動方程擴(kuò)展到EKF下對應(yīng)的系統(tǒng)狀態(tài)轉(zhuǎn)移方程可表示為

(2)

里程計(jì)與IMU的觀測方程可以分別表示為：

(3)

(4)

Hot為里程計(jì)的觀測矩陣；Hit為IMU的觀測矩陣；Qot為里程計(jì)數(shù)據(jù)的觀測誤差；Qit為IMU數(shù)據(jù)的觀測誤差；I6為單位6階觀測矩陣。

將IMU和里程計(jì)的觀測方程整合到一起，即可得到總體的觀測方程，即

(5)

Ht為總體觀測矩陣；Qt為總體觀測誤差。

式(5)展開后可表示為

(6)

通過將狀態(tài)轉(zhuǎn)移方程、觀測方程和誤差方程代入到EKF算法中，可得到里程計(jì)與IMU這2種傳感器的融合數(shù)據(jù)，然后用同樣的方法將此數(shù)據(jù)與雷達(dá)數(shù)據(jù)進(jìn)行融合，即可得到最終的最優(yōu)位姿估計(jì)值。

2 基于掃描匹配的重定位

重定位屬于SLAM與路徑規(guī)劃的中間一環(huán)， 它主要用于解決機(jī)器人遭遇“綁架”和路徑規(guī)劃起點(diǎn)與建圖起點(diǎn)不一致時(shí)，導(dǎo)致的路徑規(guī)劃失敗問題。傳統(tǒng)的重定位方法是采用AMCL，它是一種概率模型定位系統(tǒng)，通過將粒子分布在全圖，然后在每個(gè)粒子的位置上模擬傳感器，將傳感器信息與觀測到的信息進(jìn)行比較，由于粒子數(shù)過多，收斂需要非常長的時(shí)間。

Cartographer在建圖時(shí)會構(gòu)建許多由多個(gè)Scans組成的Submap，掃描匹配是通過將當(dāng)前幀的雷達(dá)數(shù)據(jù)與Submap進(jìn)行匹配，來定位當(dāng)前的位姿，從而達(dá)到重定位的目的。此問題可以轉(zhuǎn)化成優(yōu)化問題來求解，優(yōu)化問題可以看成一個(gè)非線性最小二乘問題，表達(dá)式為

(7)

ξ*為機(jī)器人的真實(shí)位姿；Tζ為變換矩陣，能把所有的Scan中的點(diǎn)hk從Scan坐標(biāo)系下變換到Submap坐標(biāo)系下；M函數(shù)為雙三次插值函數(shù)，它把每個(gè)Scan中的雷達(dá)點(diǎn)變成了是否在Submap中的概率值[14]，每個(gè)雷達(dá)點(diǎn)的概率值通過函數(shù)M變換后如果都能趨近于1，說明這幀Scan很好地匹配了某個(gè)Submap，此時(shí)便得到機(jī)器人的當(dāng)前位姿。

3 基于改進(jìn)A*算法的全局規(guī)劃

A*算法是常見的全局路徑規(guī)劃算法，是一種基于Dijkstra算法[15]的改進(jìn)算法，但是存在著搜索時(shí)間過長以及拐點(diǎn)較多等缺點(diǎn)。A*算法分別考慮了移動機(jī)器人出發(fā)點(diǎn)到當(dāng)前節(jié)點(diǎn)的真實(shí)代價(jià)和當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的估計(jì)代價(jià)，算法的代價(jià)計(jì)算公式為

f(n)=g(n)+h(n)

(8)

n為機(jī)器人的當(dāng)前節(jié)點(diǎn)；f(n)為全局估計(jì)代價(jià)；g(n)為出發(fā)點(diǎn)到節(jié)點(diǎn)n的真實(shí)代價(jià)；h(n)為從節(jié)點(diǎn)n到目標(biāo)點(diǎn)的估計(jì)代價(jià)。

A*算法的優(yōu)化主要分為2方面：對搜索時(shí)間的優(yōu)化；對路徑的多余拐角進(jìn)行優(yōu)化，減少不必要的轉(zhuǎn)彎。為了優(yōu)化算法搜索時(shí)間，為h(n)增加權(quán)重參數(shù)q(n)，因此全局評估代價(jià)計(jì)算公式為

f(n)=g(n)+h(n)·q(n)

(9)

當(dāng)q(n)取較小的值時(shí)，A*算法接近于迪杰斯特拉算法(Dijkstra)算法，會導(dǎo)致搜索速率過慢；當(dāng)q(n)取較大的值時(shí)，A*算法接近于深度優(yōu)先搜索(depth first search,DFS)算法，雖然搜索速度快，但無法找到最優(yōu)路徑。q(n)取合適的值，可以在不增加路徑長度的同時(shí)提升搜索速度。為了得到最佳的q(n)取值，在MATLAB下進(jìn)行仿真，分別選取5組不同的起點(diǎn)和終點(diǎn)，選擇不同的參數(shù)進(jìn)行測試，測試結(jié)果如圖1所示。

從圖1可知，當(dāng)q(n)取1.2時(shí)，可以在不增加路徑長度的前提下，使搜索時(shí)間減少22.79%。當(dāng)q(n)取值大于1.2時(shí)，雖然搜索時(shí)間會進(jìn)一步減少，但是路徑長度增加，從而無法得到最優(yōu)路徑。

圖1 q(n)取值對A*算法的影響

傳統(tǒng)A*算法計(jì)算的路徑會產(chǎn)生多余的拐角，消除多余拐角可以提高機(jī)器人行進(jìn)效率，圖2為了優(yōu)化路徑多余拐角，增加拐角優(yōu)化函數(shù)，方法流程如下：

a.從開集中按序列找到總代價(jià)最小的點(diǎn)。

b.得到這個(gè)點(diǎn)對應(yīng)的父節(jié)點(diǎn)的索引值，進(jìn)而得到父節(jié)點(diǎn)儲存的方向信息。

c.根據(jù)方向信息得到期望的點(diǎn)的方向是其反方向。

d.判定這個(gè)期望點(diǎn)的總代價(jià)是否跟原來那個(gè)點(diǎn)相同。

e.如果是則替換原來的點(diǎn)，否則跳過這個(gè)點(diǎn)；回到步驟a。

在MATLAB下進(jìn)行仿真，A*算法拐角優(yōu)化前后對比結(jié)果如圖2所示。可以看出,1號和2號位置處的拐角優(yōu)化后由3個(gè)減少為1個(gè)，這樣規(guī)劃出來的路線更加符合實(shí)際環(huán)境下機(jī)器人運(yùn)動軌跡。

圖2 A*算法拐角優(yōu)化前后對比

4 實(shí)驗(yàn)測試和分析

將文中算法部署到室內(nèi)機(jī)器人上進(jìn)行實(shí)驗(yàn)分析。圖3是基于ROS的分布式系統(tǒng)結(jié)構(gòu)，采取分布式配置是因?yàn)闄C(jī)器人硬件性能有限。上位機(jī)選擇聯(lián)想筆記本R7000，其CPU主頻為3 GHz，硬件上選擇樹莓派4b作為工控板，STM32作為底層驅(qū)動板，傳感器上選擇里程計(jì)、IMU和激光雷達(dá)。STM32采集傳感器數(shù)據(jù)后傳遞給樹莓派，樹莓派再通過ROS通信傳遞給上位機(jī)，上位機(jī)再做出決策來控制機(jī)器人運(yùn)動。實(shí)驗(yàn)分為建圖、重定位和路徑規(guī)劃，在長走廊場景下進(jìn)行建圖測試，在實(shí)驗(yàn)室場景下進(jìn)行重定位和路徑規(guī)劃測試。

圖3 基于ROS的分布式系統(tǒng)結(jié)構(gòu)

4.1 建圖測試

在Long-Corridor場景下進(jìn)行建圖測試，開啟建圖節(jié)點(diǎn)后，通過Rviz 軟件得到可視化的2D 柵格地圖，如圖4所示。圖4a是只含激光雷達(dá)數(shù)據(jù)的地圖，其中矩形框可以看出明顯的錯(cuò)誤，矩形框中的地圖部分與實(shí)際環(huán)境的差異較大，這是因?yàn)長ong-Corridor場景下的局部環(huán)境基本重復(fù)，特征點(diǎn)較少，激光雷達(dá)無法準(zhǔn)確感知到環(huán)境的改變。圖4b是含有激光雷達(dá)、里程計(jì)和IMU這3種數(shù)據(jù)的地圖，可以看出方框部分的地圖得到明顯的改善。由實(shí)驗(yàn)可知，機(jī)器人在Long-Corridor場景下，比較依賴于IMU和里程計(jì)傳感器數(shù)據(jù)，這是因?yàn)镮MU 數(shù)據(jù)能提供比較精確的角度增量，而里程計(jì)數(shù)據(jù)則能得到比較精確的線速度，和激光雷達(dá)數(shù)據(jù)融合后得到的地圖更符合真實(shí)場景。

圖4 不同傳感器的2D柵格地圖

4.2 重定位測試

圖5是實(shí)驗(yàn)室環(huán)境2D地圖，在ABCD這4處位置分別基于掃描定位方法和AMCL的方法進(jìn)行重定位實(shí)驗(yàn)，其中Z位置是建圖的起點(diǎn)，也是小車重定位默認(rèn)初始位置。

圖5 實(shí)驗(yàn)室環(huán)境地圖

以B點(diǎn)為例說明重定位過程，將小車放置在B點(diǎn)，開啟重定位功能，此時(shí)機(jī)器人默認(rèn)在Z點(diǎn)，控制機(jī)器人前后左右移動一段距離，機(jī)器人便會重定位至B點(diǎn)。如圖6a所示，開啟AMCL節(jié)點(diǎn)后，粒子分布于全圖，機(jī)器人位置初始化為地圖中間。如圖6b所示，當(dāng)粒子基本收斂到機(jī)器人周圍時(shí)，說明重定位成功。圖7a是機(jī)器人開啟掃描匹配節(jié)點(diǎn)后，會不斷將當(dāng)前幀Scan與Submap進(jìn)行匹配，此時(shí)可以看出局部地圖未與全局地圖重合。圖7b所示，當(dāng)局部地圖與全局地圖重合時(shí)便說明重定位成功。每處位置進(jìn)行5次實(shí)驗(yàn)，然后取其耗時(shí)平均值，結(jié)果如表1所示。

圖6 基于AMCL重定位過程

圖7 基于掃描匹配重定位過程

表1 重定位實(shí)驗(yàn)平均耗時(shí)對比

由表1可知，掃描匹配平均耗時(shí)比AMCL減少80.43%，大幅度減少了重定位所花費(fèi)的時(shí)間。因此，在使用重定位功能時(shí)，采用基于掃描匹配的方法，定位效果好，并且速度更快。

4.3 路徑規(guī)劃實(shí)驗(yàn)

路徑規(guī)劃實(shí)驗(yàn)分為靜態(tài)環(huán)境實(shí)驗(yàn)和動態(tài)環(huán)境實(shí)驗(yàn)，靜態(tài)環(huán)境是為了驗(yàn)證機(jī)器人在無外界干擾情況下的導(dǎo)航能力，動態(tài)環(huán)境測試是為了驗(yàn)證機(jī)器人在室內(nèi)環(huán)境下的緊急避障能力，不同環(huán)境下的實(shí)驗(yàn)過程分別如圖8和圖9所示。

圖8 靜態(tài)環(huán)境實(shí)驗(yàn)過程

圖9 動態(tài)環(huán)境實(shí)驗(yàn)過程

圖8a中的路線是機(jī)器人調(diào)用改進(jìn)后的A*算法規(guī)劃出來的全局路徑，圖8b是機(jī)器人檢測到靜態(tài)障礙物后，調(diào)用ROS中自帶的動態(tài)窗口法(dynamic window approach,DWA)[16]進(jìn)行避障。圖9a是機(jī)器人正在按全局路徑行進(jìn)，圖9b是當(dāng)機(jī)器人檢測到突然放置的障礙物時(shí)，調(diào)用DWA算法進(jìn)行的緊急避障，圖8c和圖9c是實(shí)際場景。2種實(shí)驗(yàn)環(huán)境下機(jī)器人都成功完成路徑規(guī)劃，證明了機(jī)器人的避障能力可滿足室內(nèi)基本要求。

5 結(jié)束語

基于樹莓派4b和單片機(jī)STM32設(shè)計(jì)了一款室內(nèi)移動機(jī)器人導(dǎo)航系統(tǒng)，該導(dǎo)航系統(tǒng)具有低成本、高性能、可移植性高等特點(diǎn)。