基于深度學(xué)習(xí)的激光同步定位與地圖構(gòu)建移動(dòng)機(jī)器人可定位性研究

高 揚(yáng)， 張傳璽， 王 晨， 王書(shū)棋

(長(zhǎng)安大學(xué)汽車(chē)學(xué)院， 西安 710064)

定位是機(jī)器人進(jìn)行導(dǎo)航、運(yùn)動(dòng)等任務(wù)的基礎(chǔ)，其定位精度也直接影響相關(guān)任務(wù)的執(zhí)行效果。目前移動(dòng)機(jī)器人領(lǐng)域常用的定位手段包括衛(wèi)星定位、慣性導(dǎo)航、地圖匹配、航跡推算等。可定位性是指移動(dòng)機(jī)器人獲得準(zhǔn)確定位的難易程度，也指定位誤差的大小[1]。可定位性估計(jì)，作為機(jī)器人領(lǐng)域的一個(gè)較新的問(wèn)題，并利用其評(píng)估結(jié)果成功控制機(jī)器人獲得了良好效果。汪志剛等[2]提出了一種緊耦合的視覺(jué)慣性組合定位方法，該方法準(zhǔn)確率較高，但易受室內(nèi)亮度的影響，魯棒性較差。Zhen等[3]提出了一種融合慣性測(cè)量單元(inertial measurement unit，IMU)和旋轉(zhuǎn)激光掃描儀測(cè)量的魯棒定位方法，并提出一種評(píng)估給定三維地圖可定位性的新方法，但該方法計(jì)算復(fù)雜度較高。Ferreira等[4]為了研究水下定位問(wèn)題提出基于啟發(fā)式算法的自適應(yīng)信息融合神經(jīng)網(wǎng)絡(luò)框架(behaviors-prediction reliability-fusion，B-PR-F)，該框架在融合信息之前對(duì)單個(gè)估計(jì)的置信度進(jìn)行評(píng)估。Alomari等[5]介紹了一種用于無(wú)線(xiàn)傳感器中移動(dòng)錨輔助定位的靜態(tài)路徑規(guī)劃模型。與類(lèi)似的靜態(tài)模型相比，該模型能較好地估計(jì)其位置，具有較高的定位精度。周奇勛等[6]設(shè)計(jì)了一種基于被動(dòng)式移動(dòng)架構(gòu)的定位系統(tǒng)，該系統(tǒng)定位精度較高，但須安裝定位基站，實(shí)現(xiàn)較為復(fù)雜。趙妍等[7]提出了一種基于同時(shí)定位與地圖創(chuàng)建技術(shù)的機(jī)器人智能激光定位技術(shù)，該方法收斂性較好，但精度不高。苗潤(rùn)龍等[8]提出了一種基于最小二乘高斯核支持向量機(jī)的單目視覺(jué)定位算法，該算法利用最小二乘高斯核支持向量機(jī),將機(jī)器人在圖片中像素位置與真實(shí)場(chǎng)景中位置間的非線(xiàn)性映射轉(zhuǎn)化為帶高斯核的線(xiàn)性映射，算法精度較優(yōu)，但選參優(yōu)化過(guò)程過(guò)于復(fù)雜。Wu等[9]提出了一種高效的分布式方法，該方法在時(shí)延和定位檢測(cè)精度方面都優(yōu)于現(xiàn)有的算法。Xia等[10]提出了一種基于剛度理論的無(wú)線(xiàn)傳感器網(wǎng)絡(luò)可定位性判斷方法。Li等[11]提出了一種利用地圖輔助水平不確定度(map aided horizontal uncertainty level，MA-HUL)來(lái)測(cè)量定位系統(tǒng)所提供信息置信度的方法。為了提高定位的可靠性，F(xiàn)ankhauser等[12]提出了一種新的地形測(cè)繪方法，但該方法只基于運(yùn)動(dòng)和慣性測(cè)量的主體感知定位。Le等[13]描述了基于梯度下降的運(yùn)動(dòng)規(guī)劃器，試圖優(yōu)化或約束網(wǎng)絡(luò)可定位性變化的機(jī)器人，此外該方法須考慮移動(dòng)機(jī)器人網(wǎng)絡(luò)部署問(wèn)題，較為復(fù)雜。Luo等[14]提出了一種基于網(wǎng)格的室內(nèi)協(xié)同定位跟蹤算法(cooperative localization and tracking algorithm，CLTA)，該算法在定位跟蹤性能上優(yōu)于單網(wǎng)絡(luò)定位算法，精度較高，效率一般。

綜上，針對(duì)目前常用的地圖匹配定位方法，現(xiàn)有可定位性估計(jì)方法難以直接、準(zhǔn)確、高效地估計(jì)可定位性，在場(chǎng)景應(yīng)用中與實(shí)際定位誤差相比仍存在一定誤差[15]。因此如何實(shí)現(xiàn)對(duì)可定位性的更準(zhǔn)確評(píng)估仍然是一大難點(diǎn)問(wèn)題。基于激光掃描儀針對(duì)常用的地圖匹配定位算法的可定位性展開(kāi)研究，現(xiàn)提出一種新穎的基于深度學(xué)習(xí)的可定位性估計(jì)方法估計(jì)定位熵評(píng)價(jià)可定位性。

1 方法

經(jīng)典的地圖匹配定位算法中，定位信息來(lái)源于航跡推算與地圖匹配兩個(gè)部分，其中航跡推算部分中關(guān)于位姿估計(jì)的不確定性[15]已進(jìn)行了分析，不再贅述。對(duì)采用激光掃描儀掃描周?chē)h(huán)境實(shí)現(xiàn)定位的地圖匹配部分，現(xiàn)對(duì)其定位誤差分析如下。

ri(s)=f(s,θi)+ε

(1)

式(1)中：ε為一維0均值高斯觀測(cè)噪聲；f為一種函數(shù)映射關(guān)系。

(2)

(3)

(4)

(5)

式中：H(·)為求熵函數(shù)。

由于機(jī)器人的定位過(guò)程實(shí)際上是由一幀激光內(nèi)所有觀測(cè)得到的位姿估計(jì)融合的結(jié)果。故依據(jù)信息融合理論，對(duì)同一狀態(tài)的估計(jì)，假設(shè)各估計(jì)間相互獨(dú)立，則有融合單幀內(nèi)所有觀測(cè)結(jié)果后的熵可表示為

(6)

式(6)中：N為單幀激光探測(cè)值的數(shù)據(jù)點(diǎn)總數(shù)量。

2 多模塊深度神經(jīng)網(wǎng)絡(luò)

估計(jì)定位熵公式[式(5)]仍然過(guò)于復(fù)雜且存在信息壓縮，難以直接計(jì)算融合定位熵，因此本文結(jié)合信息融合理論，提出一種基于深度學(xué)習(xí)的多模塊神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)(multi-module-networks，MMN)以預(yù)測(cè)機(jī)器人定位熵值。

圖2 卷積神經(jīng)網(wǎng)絡(luò)CNN結(jié)構(gòu)Fig.2 CNN structure of convolutional neural network

考慮到定位結(jié)果來(lái)源于地圖匹配與航跡推算結(jié)果的融合，設(shè)計(jì)的MMN(圖1)同樣由3個(gè)模塊構(gòu)成：地圖匹配定位熵估計(jì)模塊，采用卷積神經(jīng)網(wǎng)絡(luò)(convolutional neural network，CNN)；航跡推算定位熵估計(jì)模塊，采用長(zhǎng)短期記憶網(wǎng)絡(luò)(long short-term memory，LSTM)；融合熵估計(jì)模塊，采用多層全連接網(wǎng)絡(luò)(multi-layer full connection，MLFC) 。t時(shí)刻激光探測(cè)值X1(t)輸入地圖匹配定位熵估計(jì)模塊得到輸出特征t時(shí)刻估計(jì)熵值S1(t)，其中X1(t)為t時(shí)刻激光數(shù)據(jù)，大小為1×1 080。激光探測(cè)值X2(t)、X3(t)、S(t-1)輸入航跡推算定位熵估計(jì)模塊得到輸出特征t時(shí)刻估計(jì)熵值S2(t)，X2(t)為t-1～t時(shí)刻機(jī)器人位移；X3(t)為t-1～t時(shí)刻機(jī)器人轉(zhuǎn)過(guò)角度；S(t-1)為t-1時(shí)刻機(jī)器人定位熵值，將兩個(gè)模塊輸出特征S1(t)，S2(t)輸入融合熵估計(jì)模塊輸出t時(shí)刻融合熵值S(t)，S(t)為t時(shí)刻機(jī)器人定位熵值。

圖1 多模塊神經(jīng)網(wǎng)絡(luò)MMN結(jié)構(gòu)Fig.1 Multi-module neural network MMN structure

2.1 地圖匹配定位熵估計(jì)模塊

σ為sigmoid激活函數(shù)，該函數(shù)輸出為0～1；tanh為雙曲正切激活函數(shù)，輸出為-1～1；ht為t時(shí)刻隱層狀態(tài)圖3 長(zhǎng)短期記憶網(wǎng)絡(luò)LSTM結(jié)構(gòu)Fig.3 LSTM structure of long short-term memory network

2.2 航跡推算定位熵估計(jì)模塊和融合熵估計(jì)模塊

由于機(jī)器人的定位熵值與前一時(shí)刻機(jī)器人的角速度，線(xiàn)速度有關(guān)，所以為航跡推算定位熵估計(jì)模塊設(shè)計(jì)了長(zhǎng)短期記憶神經(jīng)網(wǎng)絡(luò)，該網(wǎng)絡(luò)以t-1～t時(shí)刻間機(jī)器人的線(xiàn)位移，角位移與t-1時(shí)刻機(jī)器人定位熵值作為輸入，t時(shí)刻機(jī)器人定位熵作為輸出預(yù)測(cè)機(jī)器人定位熵。如圖3所示，共設(shè)計(jì)10層隱層，前9層為9個(gè)長(zhǎng)短期記憶(long short-term memory，LSTM)層，第一層輸出3×1向量，第2～6層輸出50×1向量，為了防止訓(xùn)練過(guò)程過(guò)擬合，第十層采用丟失率為0.2的Dropout層。最后輸出層采用線(xiàn)性回歸函數(shù)Linear作為激活函數(shù)，得到LSTM網(wǎng)絡(luò)估計(jì)航跡推算定位熵S2。

為融合熵估計(jì)模塊設(shè)計(jì)了多層全連接網(wǎng)絡(luò)MLFC以融合地圖匹配定位熵估計(jì)模塊與航跡推算定位熵估計(jì)模塊的結(jié)果。如圖4所示，S1、S2、S分別為CNN網(wǎng)絡(luò)預(yù)估地圖匹配定位熵，LSTM網(wǎng)絡(luò)估計(jì)航跡推算定位熵，以MLFC估計(jì)前兩者融合后的定位熵，該網(wǎng)絡(luò)設(shè)計(jì)了5個(gè)隱層，前2個(gè)隱含層設(shè)計(jì)256個(gè)神經(jīng)元，第3和第4隱含層各設(shè)計(jì)128個(gè)神經(jīng)元，第5個(gè)隱含層設(shè)計(jì)64個(gè)神經(jīng)元，每層的激活函數(shù)為線(xiàn)性整流函數(shù)，輸出層有一個(gè)神經(jīng)元，激活函數(shù)為線(xiàn)性回歸函數(shù)。

3 實(shí)驗(yàn)研究

3.1 數(shù)據(jù)集說(shuō)明

針對(duì)常見(jiàn)的室內(nèi)外環(huán)境進(jìn)行了數(shù)據(jù)采集以制作樣本庫(kù)，如圖5所示，數(shù)據(jù)采集平臺(tái)采用先鋒P3DX機(jī)器人以及Hokuyo UTM-30型激光雷達(dá)，該激光雷達(dá)的掃描角度范圍為270°，角分辨率大小為0.25°，最大檢測(cè)距離為3 m。激光數(shù)據(jù)分布如表1所示。定位算法采用AMCL(adaptive Monte Carlo localization)算法。每組數(shù)據(jù)內(nèi)容包括時(shí)間戳、激光掃描數(shù)據(jù)、線(xiàn)速度、角速度、定位熵。

圖4 多層全連接神經(jīng)網(wǎng)絡(luò)MLFC結(jié)構(gòu)Fig.4 Multi-layer fully connected neural network MLFC structure

圖5 數(shù)據(jù)采集平臺(tái)Fig.5 Data collection platform

將整個(gè)數(shù)據(jù)集分成A、B兩個(gè)子數(shù)據(jù)集，分別用于卷積神經(jīng)網(wǎng)絡(luò)和循環(huán)神經(jīng)網(wǎng)絡(luò)的訓(xùn)練。子數(shù)據(jù)集A由激光掃描數(shù)據(jù)和定位熵組成，分別作為地圖匹配定位熵估計(jì)模塊的輸入和輸出。子數(shù)據(jù)集B由線(xiàn)位移、角位移和定位熵組成。以當(dāng)前線(xiàn)位移、角位移及上一時(shí)刻定位熵為長(zhǎng)短期記憶神經(jīng)網(wǎng)絡(luò)輸入，當(dāng)前時(shí)間戳定位熵為該網(wǎng)絡(luò)輸出。

整個(gè)數(shù)據(jù)集定位熵值域范圍為[-11.57，10.46]，以間隔1將值域劃分為23個(gè)區(qū)間，則數(shù)據(jù)集中定位熵在各區(qū)間的分布如圖6所示。將整個(gè)數(shù)據(jù)集劃分為訓(xùn)練集占比64%，驗(yàn)證集16%，測(cè)試集占比20%。

表1 激光數(shù)據(jù)分布Table 1 Laser data distribution

X為熵值；Y為該區(qū)間內(nèi)熵值數(shù)量圖6 熵值分布Fig.6 Entropy distribution

3.2 訓(xùn)練及測(cè)試結(jié)果

為提高多模塊神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)對(duì)定位熵估計(jì)的準(zhǔn)確性，分別訓(xùn)練地圖匹配定位熵估計(jì)模塊，航跡推算定位熵估計(jì)模塊，其次在兩個(gè)網(wǎng)絡(luò)權(quán)重基礎(chǔ)之上進(jìn)行總體網(wǎng)絡(luò)的訓(xùn)練。采用Nvidia公司GTX2080TI顯卡進(jìn)行訓(xùn)練與測(cè)試。

圖7 地圖匹配定位熵估計(jì)模塊Loss曲線(xiàn)Fig.7 Map matching location entropy estimation module Loss curve

X為AMCL算法測(cè)得的真值；Y為本文算法預(yù)測(cè)所得的熵值圖8 地圖匹配定位熵估計(jì)模塊訓(xùn)練集和測(cè)試集結(jié)果Fig.8 Map matching location entropy estimation module training and testing set results

地圖匹配定位熵估計(jì)模塊訓(xùn)練參數(shù)設(shè)置為：batch_size為20，epoch為3 000，learning_rate為0.002，損失函數(shù)Loss采用均方誤差，優(yōu)化器選擇adamax。圖7為地圖匹配定位熵估計(jì)模塊訓(xùn)練Loss曲線(xiàn)，可以看出，當(dāng)epoch數(shù)值為500時(shí)，網(wǎng)絡(luò)訓(xùn)練集與驗(yàn)證集loss趨于穩(wěn)定。圖8為子集A中訓(xùn)練集與測(cè)試集定位熵的真實(shí)值與預(yù)測(cè)值的對(duì)比。訓(xùn)練集的平均Loss為0.000 26，測(cè)試集的平均誤差為0.002 38，訓(xùn)練效果良好。

航跡推算定位熵估計(jì)模塊訓(xùn)練參數(shù)設(shè)置為：batch_size為20，epoch為3 000，learning_rate為0.002，損失函數(shù)Loss采用均方誤差，優(yōu)化器選擇adamax。圖9為航跡推算定位熵估計(jì)模塊Loss曲線(xiàn)，可以看出,當(dāng)epoch數(shù)值為500時(shí)，網(wǎng)絡(luò)訓(xùn)練集與驗(yàn)證集Loss趨于穩(wěn)定。圖10為訓(xùn)練集與測(cè)試集定位熵的真實(shí)值與預(yù)測(cè)值的對(duì)比，訓(xùn)練集的平均Loss為0.000 72，測(cè)試集的平均誤差為0.000 67，訓(xùn)練效果良好。

圖9 航跡推算定位熵估計(jì)模塊Loss曲線(xiàn)Fig.9 Dead reckoning location entropy estimation module Loss curve

X為由AMCL算法測(cè)得的真值；Y為由本文算法預(yù)測(cè)所得的熵值圖10 航跡推算定位熵估計(jì)模塊訓(xùn)練集、測(cè)試集結(jié)果Fig.10 Dead reckoning location entropy estimation module training and testing set results

融合熵估計(jì)模塊訓(xùn)練參數(shù)設(shè)置如下：batch_size為20，epoch為3 000，learning_rate為0.002，損失函數(shù)Loss采用均方誤差，優(yōu)化器選擇adamax，加載權(quán)重為CNN與LSTM訓(xùn)練權(quán)重。由圖11可以看出，當(dāng)epoch數(shù)值為500時(shí)，網(wǎng)絡(luò)訓(xùn)練集與驗(yàn)證集Loss趨于穩(wěn)定。圖12為訓(xùn)練集與測(cè)試集定位熵的真實(shí)值與預(yù)測(cè)值的對(duì)比，訓(xùn)練集的平均Loss為0.000 27，測(cè)試集的平均誤差為0.000 57，訓(xùn)練效果良好。

圖11 融合熵估計(jì)模塊 loss曲線(xiàn)Fig.11 Fusion entropy estimates the module loss curve

X為由AMCL算法測(cè)得的真值；Y為本文算法預(yù)測(cè)所得的熵值圖12 融合熵估計(jì)模塊訓(xùn)練集、測(cè)試集結(jié)果Fig.12 Fusion entropy estimation module training and testing set results

為進(jìn)一步驗(yàn)證多模塊網(wǎng)絡(luò)MMN的準(zhǔn)確性與魯棒性，在圖13占據(jù)柵格地圖所示的新室內(nèi)環(huán)境中進(jìn)行實(shí)際實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)結(jié)果如圖14所示，橫坐標(biāo)表示由AMCL算法實(shí)際測(cè)得的真值，縱坐標(biāo)表示由本文算法預(yù)測(cè)所得的熵值，兩者平均誤差為0.003 5，預(yù)測(cè)熵值與實(shí)測(cè)熵值相比誤差小于5%，結(jié)果表明所設(shè)計(jì)MMN模塊在新環(huán)境下估計(jì)的定位熵值與真實(shí)值較為接近。

紅色部分為機(jī)器人在6 min內(nèi)的運(yùn)動(dòng)軌跡圖13 實(shí)驗(yàn)環(huán)境Fig.13 Experimental environment

圖14 新環(huán)境實(shí)驗(yàn)結(jié)果Fig.14 Experimental results of new environment

4 結(jié)論

基于地圖匹配的定位算法中定位誤差來(lái)源的基礎(chǔ)上，提出以定位熵作為可定位性評(píng)價(jià)指標(biāo)，設(shè)計(jì)了一種新穎的多模塊神經(jīng)網(wǎng)絡(luò)模型估計(jì)定位熵值，該方法通過(guò)融合多模塊熵值結(jié)果可以對(duì)定位熵進(jìn)行高效、準(zhǔn)確估計(jì)，完成機(jī)器人在復(fù)雜環(huán)境下自定位，對(duì)未來(lái)機(jī)器人領(lǐng)域乃至自動(dòng)駕駛領(lǐng)域可定位性估計(jì)具有一定的參考價(jià)值。