基于激光傳感器的SLAM數(shù)據(jù)關(guān)聯(lián)算法的研究*

李延炬，肖宇峰，古 松，賀 希，郭正平

(1.西南科技大學 特殊環(huán)境機器人技術(shù)四川省重點實驗室，綿陽 四川 621010；2.西南科技大學教務(wù)處，綿陽 四川 621010；3. 蘇州中材建設(shè)有限公司，蘇州 江蘇 215300)

基于激光傳感器的SLAM數(shù)據(jù)關(guān)聯(lián)算法的研究*

李延炬1，肖宇峰1，古 松2，賀 希3，郭正平3

(1.西南科技大學 特殊環(huán)境機器人技術(shù)四川省重點實驗室，綿陽 四川 621010；2.西南科技大學教務(wù)處，綿陽 四川 621010；3. 蘇州中材建設(shè)有限公司，蘇州 江蘇 215300)

移動機器人同時定位與地圖構(gòu)建(SLAM)過程中的難點問題之一即是數(shù)據(jù)關(guān)聯(lián)。結(jié)合獨立兼容最近鄰(ICNN)算法計算復(fù)雜度低和聯(lián)合相容分枝定界(JCBB)算法關(guān)聯(lián)準確度高的優(yōu)點，提出一種基于關(guān)聯(lián)數(shù)據(jù)預(yù)處理的混合數(shù)據(jù)關(guān)聯(lián)方法。首先經(jīng)過數(shù)據(jù)預(yù)處理，選取合適的觀測特征子集和局部地圖特征子集運行ICNN算法進行數(shù)據(jù)關(guān)聯(lián)，若算法失敗，則采用JCBB算法重新計算以保證算法精確度。仿真實驗結(jié)果表明，該算法運行時間短，精確度高，適用于各種復(fù)雜環(huán)境。

同時定位與地圖構(gòu)建；數(shù)據(jù)關(guān)聯(lián)；數(shù)據(jù)預(yù)處理；最近鄰；聯(lián)合相容

0 引言

移動機器人同時定位與地圖構(gòu)建(Simultaneous Localization and Mapping，SLAM)是指機器人在未知環(huán)境中移動時根據(jù)傳感器收集的信息創(chuàng)建環(huán)境地圖，同時利用該地圖進行自身的定位。SLAM中的數(shù)據(jù)關(guān)聯(lián)問題是指建立在不同時刻、不同位置通過傳感器獲得的觀測特征之間和地圖特征之間的對應(yīng)關(guān)系，以確定它們是否來源于實際環(huán)境中的同一物理實體[1]。在SLAM中，數(shù)據(jù)關(guān)聯(lián)的計算復(fù)雜度和準確度直接影響著系統(tǒng)狀態(tài)估計的時間耗度和精確性，對最終建立的地圖有著關(guān)鍵性的影響。故合適的數(shù)據(jù)關(guān)聯(lián)算法在機器人SLAM過程中至關(guān)重要。

目前，機器人SLAM領(lǐng)域中主要應(yīng)用的數(shù)據(jù)關(guān)聯(lián)方法包括獨立兼容最近鄰(Individual Compatibility Nearest Neighbor, ICNN)算法、聯(lián)合相容分枝定界(Joint Compatibility Branch and Bound, JCBB)算法和多假設(shè)跟蹤(Multiple Hypothesis Tracking, MHT)算法等[2]。在不斷優(yōu)化過程中，MULLANE J等人采用了概率假設(shè)密度(Probability Hypothesis Density, PHD)的方法來解決FastSLAM中的數(shù)據(jù)關(guān)聯(lián)問題，得出了Rao-Blackwellised PHD SLAM算法[3]；SEGUNDO P S等人采用了稀疏關(guān)聯(lián)圖的方式來描述各集合之間的關(guān)聯(lián)關(guān)系，從而將數(shù)據(jù)關(guān)聯(lián)問題轉(zhuǎn)化為了求解圖的最大團問題[4]；曾文靜等人采用蟻群算法來搜索量測和特征的關(guān)聯(lián)集合[5]。這些前人的研究工作為數(shù)據(jù)關(guān)聯(lián)算法的優(yōu)化提供了理論依據(jù)，本文將從算法實時性和精確性方面對數(shù)據(jù)關(guān)聯(lián)算法的優(yōu)化進行探討。

當前，SLAM中應(yīng)用最常見的ICNN算法雖然實現(xiàn)簡單、實時性好，但是關(guān)聯(lián)精確性較差，容易導(dǎo)致SLAM算法發(fā)散；而JCBB算法雖然關(guān)聯(lián)準確度高，但是計算復(fù)雜，實時性較差[6]。在此，考慮兩者的優(yōu)缺點，提出將二者混合使用的方法，通過對量測數(shù)據(jù)和地圖數(shù)據(jù)進行分集，縮小數(shù)據(jù)關(guān)聯(lián)空間，再以ICNN算法為基礎(chǔ)，求解最優(yōu)關(guān)聯(lián)解，若關(guān)聯(lián)失敗，則采用JCBB算法重新求解解空間，實現(xiàn)關(guān)聯(lián)校正。該算法在保證魯棒性的同時極大地減少了關(guān)聯(lián)的數(shù)據(jù)量，在應(yīng)用中可以滿足較高的實時性要求。

1 EKF-SLAM算法原理

在SLAM問題中，系統(tǒng)的狀態(tài)向量可以表示為：

(1)

式中：Xv,k=[xv,kyv,kθv,k]T表示機器人的位姿；M=[x1y1…xnyn]T表示建立的地圖特征。EKF(Extended Kalman Filter)在此的運用，就是基于機器人的運動模型和觀測模型遞歸高效地估計機器人的精確位姿和路標特征，其過程服從馬爾科夫過程，分為預(yù)測和更新兩個階段。其步驟如下：

(1)預(yù)測

①建立機器人運動模型如下：

Xv,k=fv(Xv,k-1,uv,k-1)+ωk-1

(2)

(3)

其中，Xv,k表示k時刻機器人位姿狀態(tài)；Xk表示k時刻機器人狀態(tài)向量，包括地圖特征信息；uk-1=[Vk-1αk-1]表示機器人的控制向量，包括機器人速度Vk-1和偏航角度αk-1；ωk-1是方差為Qk-1的高斯白噪聲；fv(·)表示一非線性函數(shù)，用于計算k時刻機器人位姿狀態(tài)。

②在k-1時刻，計算機器人的k時刻狀態(tài)估計值和估計協(xié)方差如下：

(4)

(5)

式中，Jacobian矩陣▽fxk-1表示非線性函數(shù)fv(·)在點xk-1處的一階Taylor展式線性化的結(jié)果。

▽fxk-1

(6)

(2)觀測

①建立機器人觀測模型如下：

(7)

②在k時刻，得到量測值Zk，并計算新息vk及新息協(xié)方差Sk如下：

(8)

(9)

(10)

(3)更新

①由式(5)和式(10)計算卡爾曼增益Kk。

(11)

②計算更新機器人的狀態(tài)估計Xk和協(xié)方差估計Pk。

(12)

Pk=(I-Kk▽hxk)Pk,k-1

(13)

2 數(shù)據(jù)關(guān)聯(lián)

2.1 獨立相容最近鄰(ICNN)算法

假設(shè)k時刻機器人觀測得到M個環(huán)境特征Zk,i(i=1,2,…,m)，地圖中保存N個路標特征Fj(j=1,2,…,n)。數(shù)據(jù)關(guān)聯(lián)的目的就是要確立當前觀測量Zk,i和地圖中已有特征Fj之間的對應(yīng)關(guān)系，并得到關(guān)聯(lián)假設(shè)集Hm(j1,j2,…,jm)，其中jk>0表示第k個觀測特征匹配的地圖特征的編號。jk=0表示觀測特征不是地圖已有特征，為新特征：jk=-1表示其為虛警。

(14)

(15)

獨立相容的檢測準則即滿足如下式：

(16)

此時稱觀測特征Zk,i與地圖特征Fj相容，其中，d表示觀測量維度，α表示置信度，一般選取置信水平1-α為95%。

通過獨立相容檢測標準后，可以得到與觀測特征相容的所有地圖特征，最近鄰算法即為在所相容的地圖特征中選取Mahalanobis距離最短的作為觀測特征的最佳匹配。Mahalanobis距離的定義如下：

(17)

式(17)表示實際觀測值與理論預(yù)測值之間的協(xié)方差距離。則最近鄰算法的選擇標準為：

Nk=min(Mk+ln|Sk|)

(18)

2.2 聯(lián)合相容分枝定界(JCBB)算法

聯(lián)合相容分枝定界算法是采用聯(lián)合相容檢驗方法同時將獲得的所有觀測特征與地圖特征進行聯(lián)合關(guān)聯(lián)，并采用分枝定界準則來搜索關(guān)聯(lián)解空間。

在關(guān)聯(lián)假設(shè)集Hm(j1,j2,…,jm)下，地圖特征的聯(lián)合觀測方程為：

(19)

聯(lián)合新息及聯(lián)合新息協(xié)方差為：

(20)

(21)

(22)

此時稱所有觀測特征和地圖特征是聯(lián)合相容的。

分枝定界準則在數(shù)據(jù)關(guān)聯(lián)中的應(yīng)用主要是遍歷關(guān)聯(lián)解空間，求解最優(yōu)解向量。利用式(22)的聯(lián)合相容條件作為分枝標準，按它們的Mahalanobis距離確定搜索順序，把關(guān)聯(lián)解空間分解成很多小的子集；在每個子集內(nèi)，利用配對數(shù)目的單調(diào)非減規(guī)則作為定界標準，將配對數(shù)最大的關(guān)聯(lián)假設(shè)作為整個解空間的最優(yōu)關(guān)聯(lián)解。

3 優(yōu)化算法

ICNN算法雖然原理簡單、計算量小，但是其關(guān)聯(lián)精度不高，各個觀測特征的關(guān)聯(lián)結(jié)果容易相互沖突，導(dǎo)致關(guān)聯(lián)失敗。JCBB算法將所有的觀測特征與地圖特征進行聯(lián)合關(guān)聯(lián)，在檢驗過程中，一個錯誤匹配的特征可以與其他特征匹配聯(lián)合相容的概率隨著配對個數(shù)的增加而降低，所以，它的魯棒性要強于獨立相容檢驗方法，但是其計算量大，在復(fù)雜環(huán)境中實時性較差。

為了在SLAM過程中保證精度的同時提高數(shù)據(jù)關(guān)聯(lián)過程的計算效率，提出了局部關(guān)聯(lián)和混合關(guān)聯(lián)相結(jié)合的方法。采用局部地圖區(qū)域內(nèi)的特征集和經(jīng)過預(yù)處理分類的觀測特征子集組成關(guān)聯(lián)空間，使得數(shù)據(jù)關(guān)聯(lián)過程的計算量大大減少，SLAM實時性較好；同時，在關(guān)聯(lián)空間內(nèi)，先采用ICNN算法進行數(shù)據(jù)關(guān)聯(lián)，若關(guān)聯(lián)失敗，則采用JCBB算法重新進行數(shù)據(jù)關(guān)聯(lián)以提高算法精確性。算法具體實現(xiàn)如下：

(1)選取局部地圖并分集

①選取局部地圖

根據(jù)實際應(yīng)用需要，選取以機器人為中心，涵蓋半徑略大于激光傳感器有效測距范圍的地圖特征，保證選取的特征少而精，可表示為：

(23)

式中，(xr,yr)和(xj,yj)分別表示機器人和特征點在全局坐標系中的位置，r為選取的有效半徑。如圖1所示，其中，黑色實心圓點表示地圖中已有特征，黑色實心星點表示新觀測的特征點。

圖1 局部地圖選取示意圖

②分集

依次計算局部地圖中的特征點(xk,yk)與其他特征點(xj,yj)(j≠k)之間的相對距離Δd，將距離小于設(shè)定閾值的點劃分為(xk,yk)代表的子集Fk。再從剩余點中隨機抽取一點，繼續(xù)劃分子集，已經(jīng)有歸屬子集的點不重復(fù)參與劃分。

(24)

(2)動態(tài)分類觀測特征子集

在實際機器人導(dǎo)航過程中，并不需要把所有的觀測特征都作為地圖中的特征，一般噪聲和動態(tài)特征是不保留在地圖特征中的，而這些特征量可能很大，既降低了關(guān)聯(lián)的精確度，又大大增加了計算量，影響系統(tǒng)的整體性能。在此，引入預(yù)處理的思想，定義預(yù)處理特征集，對獲得的觀測特征進行去噪分類，保證最后獲得的子集里面的特征少而精確，具體實現(xiàn)如下。

①去噪

將所有的觀測特征加入預(yù)處理特征集，為有效地濾除噪聲點，減少干擾，對特征集里的特征點依次計算相鄰距離D(n,n+i)，與設(shè)置的閾值ΔD進行比較，若滿足D(n,n+i)<ΔD，則認為是特征點，反之，則是噪聲點。

(25)

(26)

式中ΔD依據(jù)激光傳感器的模型特性確定。

②確定閾值ΔD

(27)

③分類子集

將特征集里特征點按角度順序依次排列，將首點(xi,yi)依次與本區(qū)域點{(xi+2,yi+2),(xi+3,yi+3),…,(xi+j,yi+j)}進行閾值比較，判斷各點是否和首點屬于同一子集，直到該區(qū)域最后一點。

(3)關(guān)聯(lián)合適的關(guān)聯(lián)集

將每個觀測特征子集的首點與局部地圖特征各個子集的首點依次做聯(lián)合相容檢驗，取檢驗結(jié)果最好的關(guān)聯(lián)解所在的子集作為關(guān)聯(lián)對象進行關(guān)聯(lián)。

(4)使用ICNN算法求關(guān)聯(lián)解

(5)若步驟(4)求解失敗，則使用JCBB算法求關(guān)聯(lián)解

4 實驗與分析

4.1 實驗?zāi)Ｐ?/p>

機器人在k時刻的位姿為：

Xv,k=[xv,kyv,kθv,k]T

(28)

假設(shè)機器人在k+1時刻相對k時刻位姿的變化量為ΔXv,k=[△xv,k△yv,k△θv,k]T, 可以由模擬里程計獲得。

建立輪式機器人運動方程為：

Xv,k+1=Xv,k+

(29)

式中(Ωx(k),Ωy(k),Ωθ(k))表示系統(tǒng)過程噪聲。

建立二維激光雷達的觀測方程為：

(30)

其中，輸入(xi,yi)為第i個特征的位置坐標，(ωr(k),ωθ(k))表示傳感器量測噪聲。

激光雷達選用的有效測量距離設(shè)為0.1～10m，誤差為0.1m，角度測量范圍為[-3π/4,3π/4]，誤差為0.1°，系統(tǒng)過程噪聲初始值設(shè)置為diag(0.09,0.09,180/π)，觀測噪聲初始值設(shè)置為diag(0.01,180/π)，機器人行駛速度為0.4m/s，地圖面積為200m×200m。

4.2 實驗結(jié)果與分析

圖2 簡單環(huán)境全局地圖示意圖

在地圖環(huán)境相對良好、特征信息比較清晰、機器人運動路徑規(guī)劃相對簡單的條件下，驗證混合算法的實現(xiàn)效果，結(jié)果如圖2和圖3所示。

圖3 簡單環(huán)境SLAM過程示意圖

從圖2中可以看出，機器人在環(huán)境狀況良好的情況下，SLAM過程中數(shù)據(jù)關(guān)聯(lián)效果較好，激光觀測特征與地圖路標特征基本重合，機器人實際運動路徑遵從設(shè)定的目標路線行駛；如圖3所示，在SLAM過程中，機器人到達點與各目標點基本重合，且計算迅速，在機器人速度設(shè)定在1.5 m/s的情況下，也可以很好地完成SLAM任務(wù)，其詳細數(shù)據(jù)如表1所示。

表1 簡單環(huán)境SLAM 下ICNN、JCBB與混合算法性能比較

在地圖環(huán)境相對復(fù)雜、特征點排列雜亂無序、機器人路徑規(guī)劃復(fù)雜曲折的情況下，驗證ICNN、JCBB和混合算法的實現(xiàn)效果,如圖4～圖7所示。

圖4 復(fù)雜環(huán)境全局地圖示意圖

圖5 復(fù)雜環(huán)境SLAM(ICNN)過程示意圖

圖6 復(fù)雜環(huán)境SLAM(JCBB)過程示意圖

圖7 復(fù)雜環(huán)境SLAM(混合)過程示意圖

從圖4中可以看出，在復(fù)雜環(huán)境SLAM過程中，ICNN、JCBB和混合算法實現(xiàn)效果差別巨大；圖5顯示ICNN算法直接失敗，機器人觀測特征與地圖特征匹配結(jié)果錯誤，導(dǎo)致機器人運動路徑與理論路徑相差甚遠，最終無法到達目標點；圖6中，使用JCBB算法，觀測特征與地圖特征關(guān)聯(lián)正確率高，機器人在SLAM過程中，能精確地控制自己的位姿，實際運動路徑接近于理論路徑，但是運算時間很長，執(zhí)行一次仿真要數(shù)個小時以上，混合算法的效果與JCBB算法大致一樣，精度略有降低，但是計算時間少，可以滿足機器人實時SLAM，其詳細數(shù)據(jù)如表2所示。

表2 復(fù)雜環(huán)境SLAM 下ICNN、JCBB與混合算法性能比較

在此，分別比較三種數(shù)據(jù)關(guān)聯(lián)方法在簡單環(huán)境和復(fù)雜環(huán)境下SLAM執(zhí)行的有效性，得到了三種算法的計算相對時間和目標位置精度的比較值?？梢钥闯?，三種算法中，混合算法運行時間最短，并且保持了相對較好的精度，在機器人SLAM過程中應(yīng)用效果良好。

5 結(jié)論

針對二維激光雷達在移動機器人SLAM過程中數(shù)據(jù)關(guān)聯(lián)問題，提出一種基于ICNN算法和JCBB算法的混合算法，在關(guān)聯(lián)空間內(nèi)，采用JCBB算法對ICNN算法的結(jié)果進行校正補充，提高了算法的精確性；同時，結(jié)合二維激光雷達的數(shù)據(jù)特點，對觀測數(shù)據(jù)和地圖數(shù)據(jù)進行預(yù)處理，既保證了數(shù)據(jù)精度，又大大減少了運算數(shù)據(jù)，使得算法具有較好的實時執(zhí)行性。實驗結(jié)果表明，所提算法適用于不同的環(huán)境，是實際應(yīng)用中的一種可行方案。

[1] 周武．面向智能移動機器人的同時定位與地圖創(chuàng)建研究[D]．南京：南京理工大學，2009.

[2] DALLIL A, OUSSALAH M, OULDALI A. Sensor fusion and target tracking using evidential data association[J]. Sensors Journal, IEEE, 2013,13：285-293.

[3] MULLANE J, VO B N, ADAMS M D. Rao-Blackwellised PHD SLAM [C]. IEEE International Conference on Robotics and Automation,2010：5410-5416.

[4] SEGUNDO P S, RODRIGUEZ-LOSADA D. Robust global feature based data association with a sparse bit optimized maximum clique algorithm[J].IEEE Transactions on Robotics, 2013, 29：1332-1339.

[5] 曾文靜,張鐵棟,徐玉如,等．一種基于蟻群算法的SLAM數(shù)據(jù)關(guān)聯(lián)方法[J].計算機應(yīng)用,2009,29(1):136-148.

[6] 郭劍輝,趙春霞,石杏喜.一種改進的聯(lián)合相容SLAM數(shù)據(jù)關(guān)聯(lián)方法[J].儀器儀表學報,2008,29(11):2260-2265.

Research on data association in SLAM based laser sensor

Li Yanju1，Xiao Yufeng1，Gu Song2，He Xi3，Guo Zhengping3

(1.Sichuan Province Key Laboratory of Robot Technology Used for Special Environment, Southwest University of Science & Technology, Mianyang 621010, China; 2. Academic Affairs Office, Southwest University of Science & Technology, Mianyang 621010, China; 3. Suzhou Zhong Cai Construction Co., Ltd., Suzhou 215300, China)

Data association is one of the difficult problems in simultaneous localization and mapping (SLAM) for mobile robot. This paper presents a hybrid approach of data association based on data preprocessing by combining the advantages of individual compatibility nearest neighbor (ICNN) algorithm’s low computation complexity and Joint Compatibility Branch and Bound (JCBB) algorithm’s high accuracy. Firstly, ICNN is used for data association in a subset of localmap and a subset of measurement data which has been data-preprocessed. If its result is incorrect, JCBB is used to assure the accuracy of the algorithm. The experimental results show that this method has shorter running time and higher association precision, and adapts to complex environments.

simultaneous localization and mapping(SLAM); data association; data preprocessing; neatest neighbor; joint compatibility

TP301.6

A

10.19358/j.issn.1674- 7720.2017.02.024

李延炬，肖宇峰，古松，等.基于激光傳感器的SLAM數(shù)據(jù)關(guān)聯(lián)算法的研究[J].微型機與應(yīng)用，2017,36(2)：78-82.

四川省教育廳重點項目(14ZA0091)；四川省科技支撐計劃項目(2015GZ0035)；四川省科技支撐計劃項目(2015GZ0342)

2016-08-27)

李延炬(1991-)，男，碩士研究生，主要研究方向：機器人定位、嵌入式技術(shù)與應(yīng)用。

肖宇峰(1978-)，男，博士，副研究員，主要研究方向：機器人通信、嵌入式技術(shù)及應(yīng)用、機器人信息系統(tǒng)。