高 揚， 張傳璽， 王 晨， 王書棋

(長安大學(xué)汽車學(xué)院， 西安 710064)

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

綜上，針對目前常用的地圖匹配定位方法，現(xiàn)有可定位性估計方法難以直接、準(zhǔn)確、高效地估計可定位性，在場景應(yīng)用中與實際定位誤差相比仍存在一定誤差[15]。因此如何實現(xiàn)對可定位性的更準(zhǔn)確評估仍然是一大難點問題?；诩す鈷呙鑳x針對常用的地圖匹配定位算法的可定位性展開研究，現(xiàn)提出一種新穎的基于深度學(xué)習(xí)的可定位性估計方法估計定位熵評價可定位性。

1 方法

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

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

(1)

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

(2)

(3)

(4)

(5)

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

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

(6)

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

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

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

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

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

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

2.1 地圖匹配定位熵估計模塊

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

2.2 航跡推算定位熵估計模塊和融合熵估計模塊

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

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

3 實驗研究

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

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

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

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

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

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

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

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

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

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

圖7 地圖匹配定位熵估計模塊Loss曲線Fig.7 Map matching location entropy estimation module Loss curve

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

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

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

圖9 航跡推算定位熵估計模塊Loss曲線Fig.9 Dead reckoning location entropy estimation module Loss curve

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

融合熵估計模塊訓(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時，網(wǎng)絡(luò)訓(xùn)練集與驗證集Loss趨于穩(wěn)定。圖12為訓(xùn)練集與測試集定位熵的真實值與預(yù)測值的對比，訓(xùn)練集的平均Loss為0.000 27，測試集的平均誤差為0.000 57，訓(xùn)練效果良好。

圖11 融合熵估計模塊 loss曲線Fig.11 Fusion entropy estimates the module loss curve

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

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

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

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

4 結(jié)論

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