基于優(yōu)化Hector-SLAM算法的機(jī)器人自主導(dǎo)航系統(tǒng)設(shè)計(jì)

汪建華，黃 磊，石雨婷，張曉倩，祁良劍

（南京林業(yè)大學(xué) 機(jī)械電子工程學(xué)院，江蘇 南京 210037）

自主移動(dòng)機(jī)器人必須具備自主導(dǎo)航和路徑規(guī)劃能力，而構(gòu)建環(huán)境地圖是實(shí)現(xiàn)路徑規(guī)劃的前提[1-5]。SLAM（simultaneous localization and mapping，實(shí)時(shí)定位與建圖）是機(jī)器人自主導(dǎo)航中的一項(xiàng)關(guān)鍵技術(shù)[6-7]，其是指機(jī)器人在未知環(huán)境中移動(dòng)時(shí)，利用傳感器定位自身位姿，并基于位姿增量式構(gòu)建地圖。近年來(lái)，隨著激光雷達(dá)和中央處理器的性能不斷提升，學(xué)者們通過不斷改進(jìn)和優(yōu)化SLAM算法來(lái)滿足這些設(shè)備的性能需求。支奕琛等[8]提出了一種基于ZBar 算法和融合Cartographer-SLAM 與Hector-SLAM算法的方法，以實(shí)現(xiàn)對(duì)二維碼目標(biāo)點(diǎn)的識(shí)別和標(biāo)記。該方法先采用ZBar算法識(shí)別機(jī)器人的二維碼信息，再采用Cartographer算法生成Geotiff地圖，最后采用Hector-SLAM 算法在地圖上繪制目標(biāo)點(diǎn)。經(jīng)實(shí)驗(yàn)驗(yàn)證，該方法能識(shí)別遠(yuǎn)距離目標(biāo)點(diǎn)的信息且精度較高，但對(duì)目標(biāo)點(diǎn)的預(yù)估定位存在誤差。Guan等[9]提出了一種集成Gmapping建議分布與Kullback-Leibler散度的粒子數(shù)適應(yīng)算法。經(jīng)實(shí)驗(yàn)驗(yàn)證，該算法能夠采用較少的粒子實(shí)現(xiàn)較高的定位精度，但是其對(duì)單個(gè)粒子的計(jì)算負(fù)載較大，所耗費(fèi)的計(jì)算時(shí)間較長(zhǎng)。蘇易衡等[10]提出了一種適用于低端激光雷達(dá)的優(yōu)化Hector-SLAM算法：先采用插值法獲取高精度的柵格地圖，再通過圖像相減與矩陣變換來(lái)減少系統(tǒng)對(duì)激光雷達(dá)刷新率的依賴，最后對(duì)SLAM中的關(guān)鍵幀判定機(jī)制進(jìn)行優(yōu)化，以消除旋轉(zhuǎn)時(shí)所獲取數(shù)據(jù)的干擾。經(jīng)實(shí)驗(yàn)驗(yàn)證，該算法在搭載低精度激光雷達(dá)條件下也能構(gòu)建清晰的地圖。周燕[11]提出了一種利用卡爾曼濾波算法將Hector-SLAM與人工地標(biāo)視覺定位結(jié)果相融合的組合定位算法。該算法可抑制Hector-SLAM所導(dǎo)致的累積誤差，削弱人工地標(biāo)視覺定位引發(fā)的跳變異常，提高了定位精度。

綜上所述，二維SLAM算法已被廣泛應(yīng)用于實(shí)際場(chǎng)景。目前，常用的激光SLAM算法包括Gmap‐ping、Karto-SLAM、Cartographer 和Hector-SLAM等。其中，Hector-SLAM 是一種基于高斯-牛頓（Gauss-Newton）掃描匹配的算法[12]，該算法在構(gòu)建地圖時(shí)不需要里程計(jì)數(shù)據(jù)，更適用于無(wú)人機(jī)以及行駛在不平坦地面上的車輛的定位和建圖，但其對(duì)激光雷達(dá)的精度和刷新率的要求較高。例如：美國(guó)Innovusion公司研制的獵鷹（Falcon）激光雷達(dá)，其探測(cè)距離高達(dá)500 m，測(cè)距精度為±2 cm，角分辨率為0.05°×0.05°，每秒發(fā)出的激光點(diǎn)云數(shù)高達(dá)100萬(wàn)個(gè)，但成本高昂；上海思嵐科技有限公司面向低端消費(fèi)電子產(chǎn)品推出了非接觸式激光掃描測(cè)距雷達(dá)（型號(hào)為RPLIDAR-A1），其探測(cè)距離為0.15～12 m，最高測(cè)距精度為±3 cm，在5～12 m時(shí)的測(cè)距精度為實(shí)際距離的2.5%，角分辨率為1°，每秒發(fā)出的激光點(diǎn)云數(shù)僅為8 000 個(gè)。顯然，RPLIDAR-A1 激光掃描測(cè)距雷達(dá)的精度遠(yuǎn)遠(yuǎn)不及獵鷹激光雷達(dá)，但勝在價(jià)格低廉。一般情況下，激光雷達(dá)的精度越高，價(jià)格越昂貴，在實(shí)際應(yīng)用中會(huì)受到限制。但Hector-SLAM算法在使用低精度激光雷達(dá)數(shù)據(jù)建圖時(shí)會(huì)產(chǎn)生大量累積誤差，在導(dǎo)航定位時(shí)易引發(fā)故障。針對(duì)上述問題，筆者擬作如下改進(jìn)：1）優(yōu)化Hector-SLAM算法中的插值函數(shù)，即采用雙三次插值法構(gòu)建高精度的柵格地圖，以提高掃描匹配的精準(zhǔn)度；2）采用擴(kuò)展卡爾曼濾波算法對(duì)里程計(jì)和慣性測(cè)量單元（inertial measurement unit, IMU）的數(shù)據(jù)進(jìn)行融合，通過組合定位方法來(lái)降低漂移效應(yīng)引起的累積誤差，以提高定位的準(zhǔn)確性；3）提出里程計(jì)輔助法與PL-ICP（point to line iterative closest points，點(diǎn)到線迭代最近點(diǎn)）配準(zhǔn)法相融合的方法，以實(shí)現(xiàn)運(yùn)動(dòng)估計(jì)過程的快速收斂，從而達(dá)到校正運(yùn)動(dòng)畸變的目的；4）設(shè)置IMU 的傾斜角閾值，以消除機(jī)器人旋轉(zhuǎn)時(shí)出現(xiàn)的地圖重影現(xiàn)象。在此基礎(chǔ)上，利用優(yōu)化的Hector-SLAM算法設(shè)計(jì)一種低成本、高性能的自主導(dǎo)航系統(tǒng)，并以AGV（automated guided vehicle，自動(dòng)導(dǎo)引車）為例開展建圖實(shí)驗(yàn)和自主導(dǎo)航實(shí)驗(yàn)，以驗(yàn)證系統(tǒng)的可行性。

1 優(yōu)化的Hector-SLAM算法

傳統(tǒng)Hector-SLAM 算法的建圖過程分為3個(gè)階段[13]：占據(jù)柵格地圖、掃描匹配和多分辨率地圖映射。具體流程如下：首先，利用激光雷達(dá)獲取環(huán)境信息，并采用雙線性插值法對(duì)離散數(shù)據(jù)進(jìn)行處理，以得到連續(xù)的柵格地圖；然后，將激光雷達(dá)獲取的實(shí)時(shí)數(shù)據(jù)與已知地圖進(jìn)行對(duì)齊匹配，構(gòu)造誤差函數(shù)，并采用高斯-牛頓法求解最優(yōu)位姿與偏差；最后，采用多種分辨率不同的柵格地圖對(duì)激光點(diǎn)云進(jìn)行映射，以避免匹配結(jié)果陷入局部最小值。

1.1 柵格地圖占據(jù)

柵格地圖[14-17]可有效反映環(huán)境信息，其將外部環(huán)境劃分為多個(gè)尺寸相同的柵格，每一個(gè)柵格具有3種狀態(tài)：空閑、占用或未知。在占據(jù)柵格地圖時(shí)，通過數(shù)值0 和1 來(lái)表示柵格單元被障礙物占據(jù)的概率，數(shù)值0 表示沒有障礙物，數(shù)值1 表示出現(xiàn)障礙物。由于Hector-SLAM算法構(gòu)建的柵格地圖是連續(xù)的，且掃描匹配也需要連續(xù)的數(shù)據(jù)進(jìn)行偏導(dǎo)運(yùn)算，但激光雷達(dá)獲取的是離散數(shù)據(jù)，因此須采用插值法對(duì)離散數(shù)據(jù)進(jìn)行處理。傳統(tǒng)的Hector-SLAM算法采用雙線性插值法完成柵格地圖的構(gòu)建，但該插值法僅考慮采樣點(diǎn)周圍4個(gè)直接相鄰點(diǎn)的像素值變化對(duì)采樣點(diǎn)的影響，具有低通濾波器的性質(zhì)。此外，采用雙線性插值法計(jì)算后，圖像會(huì)丟失高頻分量，導(dǎo)致邊緣不清晰。為彌補(bǔ)上述缺陷，本文采用雙三次插值法，將采樣點(diǎn)所有鄰近點(diǎn)的像素值變化的影響均考慮在內(nèi)。同時(shí)，由于低精度激光雷達(dá)的掃描頻率較低，其獲取的可用數(shù)據(jù)較少，采用雙三次插值法進(jìn)行處理，可降低數(shù)據(jù)量不足對(duì)建圖精度的影響，獲得梯度和精度更高的柵格地圖，從而達(dá)到減少系統(tǒng)噪聲、降低邊緣誤差以及提高掃描匹配準(zhǔn)確度的目的。

如圖1所示，雙三次插值法通過對(duì)采樣點(diǎn)周圍16個(gè)鄰近點(diǎn)的像素值進(jìn)行加權(quán)平均處理來(lái)獲得采樣點(diǎn)的像素值[18]。其中：采樣點(diǎn)的坐標(biāo)為(x+i,y+j)，其中x、y為整數(shù)部分，i、j為小數(shù)部分。

圖1 雙三次插值模型示意Fig.1 Schematic of bicubic interpolation model

雙三次插值的基函數(shù)S(x)可表示為：

式中：a為待定系數(shù)。

經(jīng)多次實(shí)驗(yàn)后發(fā)現(xiàn)，參數(shù)a取-0.5 時(shí)的插值效果最佳。利用雙三次插值法進(jìn)行計(jì)算，可得采樣點(diǎn)的像素值f(x+i,y+j)：

其中：

柵格地圖中插值函數(shù)的改變會(huì)導(dǎo)致高斯-牛頓法掃描匹配所使用的地圖梯度發(fā)生改變。對(duì)式(2)求X、Y方向的偏導(dǎo)，可得：

將A、B、C代入式(3)中的第1個(gè)方程，化簡(jiǎn)可得：

同理，將式(4)中的i改為j即可求得Y方向的偏導(dǎo)函數(shù)。

1.2 掃描匹配

獲得柵格地圖后，將激光雷達(dá)采集的實(shí)時(shí)數(shù)據(jù)與已知的柵格地圖進(jìn)行對(duì)齊匹配[19]。掃描匹配的原理是利用最小二乘法求解機(jī)器人的最優(yōu)位姿ξ*：

其中：

式中：ξ=[PxPyψ]Τ為機(jī)器人在世界坐標(biāo)系的二維平面XOY內(nèi)的初始位姿，其中Px、Py、ψ分別為機(jī)器人位置的橫、縱坐標(biāo)和旋轉(zhuǎn)角度；M(Sq(ξ))為障礙概率；Sq(ξ)為機(jī)器人在世界坐標(biāo)系中的位置；Sq,x、Sq,y為激光點(diǎn)在激光雷達(dá)坐標(biāo)系中的位置坐標(biāo)。

機(jī)器人在當(dāng)前時(shí)刻的最優(yōu)位姿ξ*與上一時(shí)刻初始位姿ξ存在如下關(guān)系：ξ+Δξ=ξ*。若要減小激光掃描當(dāng)前幀與柵格地圖對(duì)齊過程中的誤差，則要確保所有激光點(diǎn)在柵格地圖中的總占用偏差盡可能小，即Δξ應(yīng)滿足以下條件：

將式(6)在Δξ處泰勒展開并求偏導(dǎo)，可得：

其中：

式中：H為Hessian矩陣；?M(Sq(ξ))為地圖梯度。

1.3 運(yùn)動(dòng)畸變校正

在跟隨機(jī)器人運(yùn)動(dòng)的過程中，激光雷達(dá)每一幀數(shù)據(jù)中的激光點(diǎn)并不是在同一位姿下瞬時(shí)獲得的，即不同激光點(diǎn)的坐標(biāo)不一致。但激光雷達(dá)在封裝每一幀數(shù)據(jù)時(shí)，默認(rèn)該幀數(shù)據(jù)中的所有激光點(diǎn)均是在同一時(shí)刻、同一位姿下采集得到的，這會(huì)導(dǎo)致運(yùn)動(dòng)畸變[20]。當(dāng)激光雷達(dá)的掃描頻率較低時(shí)，激光束之間的誤差較大，點(diǎn)云數(shù)據(jù)會(huì)出現(xiàn)“變形”現(xiàn)象。本文所使用的激光雷達(dá)的掃描頻率為5.5 Hz，每一幀數(shù)據(jù)的掃描用時(shí)約為180 ms。設(shè)置機(jī)器人的線速度為0.5 m/s，每采集一幀數(shù)據(jù)后，機(jī)器人移動(dòng)0.09 m。由此可知，本文所搭載的激光雷達(dá)在實(shí)際運(yùn)動(dòng)過程中的運(yùn)動(dòng)畸變不可忽略。

在機(jī)器人快速移動(dòng)的過程中，里程計(jì)易發(fā)生位置漂移以及無(wú)法有效檢測(cè)滑動(dòng)，而IMU的輸出速率較快，測(cè)量誤差會(huì)隨時(shí)間累積，無(wú)法實(shí)現(xiàn)長(zhǎng)時(shí)間的精準(zhǔn)定位[21]。為了減小累積誤差，采用擴(kuò)展卡爾曼濾波算法對(duì)里程計(jì)和IMU 的數(shù)據(jù)進(jìn)行融合濾波處理。

假設(shè)t時(shí)刻機(jī)器人通過IMU 獲得加速度at，由此可得t時(shí)刻機(jī)器人的速度：

式中：vxt和vyt、axt和ayt分別為t時(shí)刻機(jī)器人沿X、Y方向的速度和加速度。

則在t時(shí)刻，由IMU 獲得的機(jī)器人的狀態(tài)向量X1t可表示為：

式中：ωt為IMU坐標(biāo)系下機(jī)器人的Z向角速度。

在t時(shí)刻，由里程計(jì)獲得的機(jī)器人的狀態(tài)向量X2t可表示為：

式中：xt、yt為t時(shí)刻機(jī)器人沿X、Y方向的位移；θt為t時(shí)刻機(jī)器人的旋轉(zhuǎn)角度。

在利用里程計(jì)和IMU 獲取機(jī)器人的狀態(tài)向量后，將基于里程計(jì)數(shù)據(jù)的機(jī)器人軌跡推導(dǎo)過程視作系統(tǒng)狀態(tài)方程的計(jì)算過程，以IMU和里程計(jì)的輸出結(jié)果作為更新方程的輸入，在擴(kuò)展卡爾曼濾波算法的基礎(chǔ)上進(jìn)行觀測(cè)更新，以實(shí)現(xiàn)對(duì)里程計(jì)與IMU的位姿數(shù)據(jù)的融合。由此可得，t時(shí)刻機(jī)器人的狀態(tài)向量Xt可表示為：

鑒于機(jī)器人所搭載的里程計(jì)為雙輪差速里程計(jì)，在t至t+1 時(shí)刻，機(jī)器人的狀態(tài)轉(zhuǎn)移矩陣可表示為：

式中：W0為機(jī)器人的狀態(tài)噪聲，服從高斯分布，即W0～N(0,Q0)，其中Q0為預(yù)測(cè)狀態(tài)下的協(xié)方差矩陣。

則機(jī)器人的狀態(tài)觀測(cè)模型更新如下：

式中：Zt為機(jī)器人狀態(tài)更新矩陣；Ht為機(jī)器人的狀態(tài)觀測(cè)矩陣；V0為觀測(cè)噪聲，服從高斯分布，即V0～(0,R0)，其中R0為觀測(cè)噪聲的協(xié)方差矩陣。

在此基礎(chǔ)上，按照擴(kuò)展卡爾曼濾波融合流程，計(jì)算卡爾曼增益并更新機(jī)器人狀態(tài)及對(duì)應(yīng)的協(xié)方差矩陣，具體公式如下：

其中：

式中：Ft為雅克比矩陣，Kt為卡爾曼增益，h(Xt)為機(jī)器人狀態(tài)測(cè)量模型，Pt/t為狀態(tài)向量的協(xié)方差矩陣，I為單位矩陣。

通過擴(kuò)展卡爾曼濾波算法對(duì)里程計(jì)和IMU的數(shù)據(jù)進(jìn)行融合后，有效減小了誤差，解決了機(jī)器人在轉(zhuǎn)彎時(shí)檢測(cè)不精準(zhǔn)的問題，但這遠(yuǎn)遠(yuǎn)無(wú)法達(dá)到機(jī)器人定位與導(dǎo)航精度的要求。因此，須結(jié)合里程計(jì)輔助法與PL-ICP配準(zhǔn)法來(lái)對(duì)機(jī)器人的位姿進(jìn)行進(jìn)一步標(biāo)定，以實(shí)現(xiàn)運(yùn)動(dòng)畸變的校正。首先，利用里程計(jì)中的編碼器獲取機(jī)器人在當(dāng)前時(shí)刻與上一時(shí)刻的位姿差，并將該位姿差作為PL-ICP配準(zhǔn)法的輸入初始值，以獲取機(jī)器人在當(dāng)前時(shí)刻的對(duì)應(yīng)位姿。然后，基于當(dāng)前時(shí)刻下里程計(jì)獲取的位姿差和PL-ICP配準(zhǔn)法計(jì)算得到的位姿，構(gòu)建位移誤差方程。最后，將位移誤差均分至下一時(shí)刻的機(jī)器人位姿，以修正其位姿差，并再次使用誤差方程對(duì)修正后的位姿差進(jìn)行多次迭代計(jì)算，不斷縮小機(jī)器人當(dāng)前時(shí)刻與上一時(shí)刻的位移誤差。當(dāng)誤差函數(shù)收斂或達(dá)到最大迭代次數(shù)時(shí)，迭代終止，最終獲得里程計(jì)標(biāo)定的位姿真值。

1.4 地圖重影優(yōu)化

由于機(jī)器人的質(zhì)量較小，其重心在運(yùn)動(dòng)過程中會(huì)因地形變化而上移，使得運(yùn)動(dòng)姿態(tài)不穩(wěn)定，這會(huì)對(duì)激光雷達(dá)的掃描產(chǎn)生干擾，造成地圖重影。另外，由于本文采用的是二維激光雷達(dá)，在構(gòu)建地圖時(shí)將實(shí)際環(huán)境轉(zhuǎn)換到二維平面上時(shí)可能會(huì)出現(xiàn)多平面重疊在同一位置處的情況，同樣會(huì)造成地圖重影。與此同時(shí)，常用的特征匹配算法的魯棒性也會(huì)影響單應(yīng)性矩陣的估計(jì)誤差，導(dǎo)致圖像變換不準(zhǔn)確，使得圖像拼接時(shí)在重疊區(qū)域出現(xiàn)明顯的對(duì)齊誤差，進(jìn)而造成地圖模糊[22]。除此之外，當(dāng)機(jī)器人作旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，由于數(shù)據(jù)更新較慢，掃描匹配時(shí)會(huì)存在時(shí)間誤差，也會(huì)造成地圖重影。針對(duì)上述問題，可通過以下方式來(lái)消除地圖重影：

1）增大機(jī)器人的質(zhì)量，以減少地形對(duì)機(jī)器人運(yùn)動(dòng)姿態(tài)的干擾。

2）融合里程計(jì)和IMU 的數(shù)據(jù)，以減小因機(jī)器人打滑而導(dǎo)致的里程計(jì)測(cè)量誤差，同時(shí)減小機(jī)器人運(yùn)動(dòng)過程中產(chǎn)生的漂移誤差，從而提升定位精度。

3）在IMU 中設(shè)置傾斜角閾值，本文設(shè)傾斜角閾值為45°，當(dāng)傾斜角大于45°時(shí)，系統(tǒng)自動(dòng)忽略激光雷達(dá)采集的數(shù)據(jù)，進(jìn)而過濾在機(jī)器人傾斜角過大情況下所采集的數(shù)據(jù)。

2 AGV驗(yàn)證實(shí)驗(yàn)

基于優(yōu)化的Hector-SLAM算法，設(shè)計(jì)自主導(dǎo)航系統(tǒng)，并以AGV為例，配合Rviz 三維可視化平臺(tái)對(duì)系統(tǒng)的可行性進(jìn)行驗(yàn)證[23]。本次實(shí)驗(yàn)中所使用的AGV的軟硬件設(shè)備如表1所示。

表1 AGV的軟硬件設(shè)備Table 1 Hardware and software equipment of AGV

AGV底盤及其自主導(dǎo)航系統(tǒng)搭載的激光雷達(dá)的實(shí)物分別如圖2和圖3所示。

圖2 AGV底盤實(shí)物圖Fig.2 Physical diagram of AGV chassis

圖3 AGV自主導(dǎo)航系統(tǒng)搭載的激光雷達(dá)Fig.3 Lidar equipped on AGV autonomous navigation system

2.1 建圖實(shí)驗(yàn)

在ROS（robot operating system，機(jī)器人操作系統(tǒng)）中訂閱“/scan”激光雷達(dá)數(shù)據(jù)命令話題，由此發(fā)布AGV的位姿和柵格地圖信息。隨后，啟動(dòng)launch腳本功能，以構(gòu)建相應(yīng)的柵格地圖。優(yōu)化前后Hector-SLAM算法的建圖效果對(duì)比如圖4所示。

圖4 Hector-SLAM 算法優(yōu)化前后的建圖效果對(duì)比Fig.4 Comparison of mapping effects before and after Hector-SLAM algorithm optimization

由于激光雷達(dá)的精度不高，且里程計(jì)和IMU在實(shí)際應(yīng)用時(shí)會(huì)產(chǎn)生漂移誤差，因此激光雷達(dá)的各掃描幀之間偏移量會(huì)不斷增大。觀察圖4(a)可知，采用優(yōu)化前Hector-SLAM算法建圖時(shí)，地圖左上方出現(xiàn)了重影，同時(shí)存在大量噪聲，導(dǎo)致邊緣處出現(xiàn)毛刺現(xiàn)象。觀察圖4(b)可知，Hector-SLAM 算法優(yōu)化后，地圖左上方的重影和邊緣的毛刺現(xiàn)象得到了明顯改善，地圖邊緣更加清晰。由此說明，優(yōu)化后的Hector-SLAM算法有效地提高了建圖精度。

基于優(yōu)化后的Hector-SLAM算法，選取南京林業(yè)大學(xué)教九樓（簡(jiǎn)稱南林教九樓）二樓和逸夫科技實(shí)驗(yàn)樓（簡(jiǎn)稱南林逸夫樓）三樓的走廊為實(shí)驗(yàn)場(chǎng)地，開展建圖實(shí)驗(yàn)。具體流程如下：首先，將AGV放置在走廊地面的某一位置處，以放置位置作為AGV的運(yùn)行起點(diǎn)；然后，調(diào)用“roslaunch bringup.launch”命令，運(yùn)行ROS 中的多個(gè)節(jié)點(diǎn)；接著，調(diào)用“ro‐slaunch lidar_slam.launch”命令，啟動(dòng)激光雷達(dá)；最后，利用鍵盤控制命令來(lái)控制AGV的移動(dòng)。由于AGV速度過快時(shí)，其驅(qū)動(dòng)輪會(huì)出現(xiàn)打滑現(xiàn)象，影響建圖效果，因此設(shè)置AGV的最大線速度為0.5 m/s，角速度為0.3 rad/s，以保證建圖精度。將構(gòu)建好的地圖保存到固態(tài)硬盤中，等待系統(tǒng)調(diào)用。

南林教九樓二樓走廊為封閉的“日”字形區(qū)域，其在百度地圖中的外圍輪廓如圖5所示，對(duì)應(yīng)的自主導(dǎo)航系統(tǒng)建圖結(jié)果如圖6所示。

圖5 百度地圖中的南林教九樓二樓走廊Fig.5 Second floor corridor in Ninth Teaching Build‐ing of NJFU in Baidu map

圖6 南林教九樓二樓走廊建圖結(jié)果Fig.6 Mapping results of second floor corridor in Ninth Teaching Building of NJFU

南林逸夫樓三樓走廊呈扇形，其在百度地圖中的外圍輪廓如圖7所示，對(duì)應(yīng)的自主導(dǎo)航系統(tǒng)建圖結(jié)果如圖8所示。

圖7 百度地圖中的南林逸夫樓三樓走廊Fig.7 Third floor corridor in Yifu Building of NJFU in Baidu map

圖8 南林逸夫樓三樓走廊建圖結(jié)果Fig.8 Mapping results of third floor corridor in Yifu Building of NJFU

為了定量分析基于優(yōu)化Hector-SLAM 算法的AGV自主導(dǎo)航系統(tǒng)在真實(shí)環(huán)境中的映射精度，選取教九樓二樓的東西走廊和南北走廊的長(zhǎng)度為例，來(lái)驗(yàn)證優(yōu)化后Hector-SLAM算法的建圖精度，并與優(yōu)化前Hector-SLAM算法進(jìn)行對(duì)比。

本文實(shí)驗(yàn)采用成都景瑞特科技有限公司研制的高精度激光測(cè)距儀（最大探測(cè)距離為150 m，誤差為3 mm）對(duì)教九樓二樓的走廊長(zhǎng)度進(jìn)行多次測(cè)量，并求平均值作為最終的實(shí)測(cè)值。通過測(cè)量和計(jì)算可得，教九樓二樓東西走廊的長(zhǎng)度約為46 m，南北走廊的長(zhǎng)度約為91 m?；谠摂?shù)據(jù)，使用Visio 軟件等比例繪制教九樓二樓走廊示意圖，各走廊分別用編號(hào)a、b、c、d表示，如圖9所示。

圖9 南林教九樓二樓走廊示意圖Fig.9 Schematic of second floor corridor in Ninth Teaching Building of NJFU

使用Rviz平臺(tái)中的測(cè)量函數(shù)獲取圖6所示地圖中各走廊的長(zhǎng)度（同樣是多次測(cè)量并求平均值），并與利用激光測(cè)距儀獲取的走廊長(zhǎng)度實(shí)測(cè)值進(jìn)行比較，結(jié)果如表2所示。從表2中可以看出，Hector-SLAM算法優(yōu)化前，AGV自主導(dǎo)航系統(tǒng)的平均建圖相對(duì)誤差為0.50%左右；Hector-SLAM 算法優(yōu)化后，系統(tǒng)的平均建圖相對(duì)誤差約為0.44%，最小建圖誤差僅為0.236 m，相較于優(yōu)化前減小了0.041 m，說明建圖精度明顯提升，驗(yàn)證了本文算法的優(yōu)化是可行的。

表2 優(yōu)化前后AGV自主導(dǎo)航系統(tǒng)的建圖誤差對(duì)比Table 2 Comparison of mapping errors of AGV autonomous navigation system before and after optimization

2.2 自主導(dǎo)航實(shí)驗(yàn)

本文分別使用A*算法和DWA（dynamic win‐dow approach，動(dòng)態(tài)窗口法）來(lái)實(shí)現(xiàn)AGV的全局路徑規(guī)劃[24]和局部路徑規(guī)劃[25]。根據(jù)2.1 節(jié)構(gòu)建的地圖，利用Rviz平臺(tái)開展自主導(dǎo)航實(shí)驗(yàn)。由于教九樓的走廊比逸夫樓的長(zhǎng)，且教室和透明玻璃更多，更能考驗(yàn)AGV在復(fù)雜環(huán)境下的自主導(dǎo)航性能，因此在教九樓中開展自主導(dǎo)航實(shí)驗(yàn)。在導(dǎo)航實(shí)驗(yàn)開始前，將AGV放置在教九樓二樓走廊的地面上，并將此時(shí)AGV的位姿作為初始位姿；同時(shí)，根據(jù)AGV在真實(shí)環(huán)境中的位姿，調(diào)整Rviz 平臺(tái)中AGV的初始位姿，使其與實(shí)際位姿一致，如圖10所示。

圖10 Rviz平臺(tái)中AGV自主導(dǎo)航的起點(diǎn)Fig.10 Starting point of AGV autonomous navigation in Rviz platform

為了驗(yàn)證所設(shè)計(jì)的AGV自主導(dǎo)航系統(tǒng)的定位精度和避障準(zhǔn)確度，本次實(shí)驗(yàn)共設(shè)置4 個(gè)目標(biāo)點(diǎn)（第4 個(gè)目標(biāo)點(diǎn)與起點(diǎn)重合）和多個(gè)障礙物，控制AGV從圖10所示的起點(diǎn)出發(fā)，按照A*算法規(guī)劃的路徑前往不同目標(biāo)點(diǎn)，AGV沿地圖運(yùn)行一周，最終返回至起點(diǎn)，具體過程如圖11所示。由圖11可知，AGV能夠按照A*算法規(guī)劃的最優(yōu)路徑準(zhǔn)確前往各個(gè)目標(biāo)點(diǎn)。

圖11 Rviz平臺(tái)中AGV的自主導(dǎo)航過程Fig.11 Autonomous navigation process of AGV in Rviz plat‐form

在實(shí)際場(chǎng)景中，AGV自主導(dǎo)航時(shí)可能會(huì)發(fā)生緊急情況，如出現(xiàn)行人或障礙物。為避免AGV因緊急情況出現(xiàn)而影響正常運(yùn)行，自主導(dǎo)航系統(tǒng)會(huì)調(diào)用DWA來(lái)重新規(guī)劃路徑，以躲避行人和障礙物，保證AGV最終到達(dá)目標(biāo)點(diǎn)。通過2 次實(shí)驗(yàn)來(lái)驗(yàn)證AGV自主導(dǎo)航系統(tǒng)的緊急避障性能，結(jié)果分別如圖12和圖13所示。

圖12 實(shí)際場(chǎng)景中AGV的避障過程Fig.12 Obstacle avoidance process of AGV in actual scenario

圖13 Rviz平臺(tái)中AGV的避障過程Fig.13 Obstacle avoidance process of AGV in Rviz platform

從圖12 和圖13 中可以看出，當(dāng)行人突然出現(xiàn)在AGV的前方時(shí)，自主導(dǎo)航系統(tǒng)能及時(shí)調(diào)用DWA來(lái)重新規(guī)劃AGV的行駛路徑，保證其在躲避行人和障礙物后準(zhǔn)確到達(dá)目標(biāo)點(diǎn)，實(shí)現(xiàn)自主導(dǎo)航功能。

3 總 結(jié)

本文對(duì)搭載低精度激光雷達(dá)的機(jī)器人自主導(dǎo)航系統(tǒng)進(jìn)行了優(yōu)化，解決了機(jī)器人在使用傳統(tǒng)Hector-SLAM算法時(shí)存在的建圖不清晰、定位不精準(zhǔn)等問題。首先，利用雙三次插值法代替雙線性插值法來(lái)提高數(shù)據(jù)的連續(xù)性，使高斯-牛頓法掃描匹配地圖的精準(zhǔn)度得以提升。然后，采用擴(kuò)展卡爾曼濾波算法對(duì)里程計(jì)與IMU的數(shù)據(jù)進(jìn)行了融合；同時(shí)，將里程計(jì)輔助法和PL-ICP配準(zhǔn)法相結(jié)合，以使機(jī)器人的位姿標(biāo)定更加準(zhǔn)確，提高了定位的準(zhǔn)確性。最后，通過設(shè)置IMU傾斜角閾值，消除了機(jī)器人旋轉(zhuǎn)時(shí)產(chǎn)生的地圖重影現(xiàn)象。實(shí)驗(yàn)結(jié)果表明，Hector-SLAM算法優(yōu)化后，自主導(dǎo)航系統(tǒng)的最小建圖誤差相比于優(yōu)化前減小了0.041 m，其平均建圖相對(duì)誤差低于0.50%，建圖精度大大提升。但是，當(dāng)建圖環(huán)境較大時(shí)，所設(shè)計(jì)的自主導(dǎo)航系統(tǒng)的建圖與定位效果仍存在不足，在后續(xù)研究中可考慮使用高精度的激光雷達(dá)來(lái)進(jìn)一步提升系統(tǒng)的有效性。