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

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

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

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

　　中圖分類號：TP301.6文獻標識碼：ADOI： 10.19358/j.issn.1674-7720.2017.02.024

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

0引言

　　*基金項目：四川省教育廳重點項目（14ZA0091）；四川省科技支撐計劃項目（2015GZ0035）；四川省科技支撐計劃項目（2015GZ0342）移動機器人同時定位與地圖構(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)的計算復雜度和準確度直接影響著系統(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)精確性較差，容易導致SLAM算法發(fā)散；而JCBB算法雖然關(guān)聯(lián)準確度高，但是計算復雜，實時性較差［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)用中可以滿足較高的實時性要求。

1EKF-SLAM算法原理

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

　　Xk=［XTv,kMT］T(1)

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

　　（1）預測

　?、俳C器人運動模型如下：

　　Xv,k=fv(Xv,k－1,uv,k－1)+ωk－1(2)

　　Xk=f(Xk－1,uk－1)=fv(Xk－1,uk－1)

　　M(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)。

　?、谠趉－1時刻，計算機器人的k時刻狀態(tài)估計值和估計協(xié)方差如下：

　　k,k－1=E［f(Xv,k－1,uk－1)］=fv(v,k－1,uk－1)

　　M(4)

　　Pk,k－1=E［(Xk－k,k－1)(Xk－k,k－1)T］

　　=fxk－1Pk－1fTxk－1+Qk(5)

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

　　fxk－1fx|(xk－1,uk)(6)

　　（2）觀測

　?、俳C器人觀測模型如下：

　　k=h(k,k－1)+nk(7)

　　式中，k表示量測值的預測值，h(·)表示一非線性函數(shù)，nk表示方差為Rk的高斯白噪聲。

　?、谠趉時刻，得到量測值Zk，并計算新息vk及新息協(xié)方差Sk如下：

　　vk=Zk－k(8)

　　Sk=hxkPk,k－1hTxk+Rk(9)

　　式中，Jacobian矩陣hxk表示非線性函數(shù)h(·)在點k,k－1處的一階Taylor展式線性化的結(jié)果。

　　hxk=hX(k,k-1 )(10)

　　(3)更新

　?、儆墒?5)和式(10)計算卡爾曼增益Kk。

　　Kk=Pk,k－1hTxkS－1k(11)

　?、谟嬎愀聶C器人的狀態(tài)估計Xk和協(xié)方差估計Pk。

　　Xk=k,k－1+Kkvk(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表示其為虛警。

　　在關(guān)聯(lián)假設(shè)集Hm下，計算觀測特征Zk,i與每一個地圖特征Fj的預測觀測值k之間的新息及新息協(xié)方差如下：

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

　　D2k,ij=vTkS－1kvk≤χ2d,1－α(16)

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

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

　　Mk=vTkS－1kvk(17)

　　式(17)表示實際觀測值與理論預測值之間的協(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)合觀測方程為：

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

　　vHm=ZHm－h(huán)Hm(k,k－1)(20)

　　SHm=HHmPk,k－1HTHm+RHm(21)

　　式中，ZHm=［Zk,1…Zk,m］，HHm = hHm  X(k,k-1 )。則聯(lián)合相容的檢測準則為下式成立：

　　D2Hm=vTHmS－1HmvHm≤χ2d,1－α(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é)果容易相互沖突，導致關(guān)聯(lián)失敗。JCBB算法將所有的觀測特征與地圖特征進行聯(lián)合關(guān)聯(lián)，在檢驗過程中，一個錯誤匹配的特征可以與其他特征匹配聯(lián)合相容的概率隨著配對個數(shù)的增加而降低，所以，它的魯棒性要強于獨立相容檢驗方法，但是其計算量大，在復雜環(huán)境中實時性較差。

　　為了在SLAM過程中保證精度的同時提高數(shù)據(jù)關(guān)聯(lián)過程的計算效率，提出了局部關(guān)聯(lián)和混合關(guān)聯(lián)相結(jié)合的方法。采用局部地圖區(qū)域內(nèi)的特征集和經(jīng)過預處理分類的觀測特征子集組成關(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)選取局部地圖并分集

　?、龠x取局部地圖

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

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

　?、诜旨?/p>

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

　　D［(xj,yj),(xk,yk)］≤Δd(xk,yk)∈Fk

　　D［(xj,yj),(xk,yk)］>Δd(xk,yk)Fk(24)

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

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

　?、偃ピ?/p>

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

　　D(n,n+i)=(xn－xn+i)2+(yn－yn+i)2(25)

　　f(n,n+i)=0,D(n,n+i)<ΔD

　　1,D(n,n+i)≥ΔD (26)

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

　?、诖_定閾值ΔD

　　二維激光雷達的測量點在極坐標系下表示形式為(θi,ri) ，對應(yīng)的直角坐標系下的參數(shù)xi=ricosθi，yi=risinθi ，位姿描述為ui=(θi,ri,xi,yi)。其中θ表示測量角度，r表示測量距離，i=(1,2,…,n)表示測量點。根據(jù)公式l=|α|r，可以求得相應(yīng)角度在不同極徑的弧長，于是可得ΔD≈l=|α|，其中，α為兩點角度差，為參考點(θi,ri)附近j個點中除去最大極徑和最小極徑的平均值。

　　=∑i+ji=i－jri－max(ri|i+ji=i－j)－min(ri|i+ji=i－j)j－2(27)

　　③分類子集

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

　?。?）關(guān)聯(lián)合適的關(guān)聯(lián)集

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

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

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

4實驗與分析

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

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

　　Xv,k=［xv,kyv,kθv,k］T(28)

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

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

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

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

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

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

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

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

　　從圖4中可以看出，在復雜環(huán)境SLAM過程中，ICNN、JCBB和混合算法實現(xiàn)效果差別巨大；圖5顯示ICNN算法直接失敗，機器人觀測特征與地圖特征匹配結(jié)果錯誤，導致機器人運動路徑與理論路徑相差甚遠，最終無法到達目標點；圖6中，使用JCBB算法，觀測特征與地圖特征關(guān)聯(lián)正確率高，機器人在SLAM過程中，能精確地控制自己的位姿，實際運動路徑接近于理論路徑，但是運算時間很長，執(zhí)行一次仿真要數(shù)個小時以上，混合算法的效果與JCBB算法大致一樣，精度略有降低，但是計算時間少，可以滿足機器人實時SLAM，其詳細數(shù)據(jù)如表2所示。在此，分別比較三種數(shù)據(jù)關(guān)聯(lián)方法在簡單環(huán)境和復雜環(huán)境下SLAM執(zhí)行的有效性，得到了三種算法的計算相對時間和目標位置精度的比較值?？梢钥闯觯N算法中，混合算法運行時間最短，并且保持了相對較好的精度，在機器人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ù)進行預處理，既保證了數(shù)據(jù)精度，又大大減少了運算數(shù)據(jù)，使得算法具有較好的實時執(zhí)行性。實驗結(jié)果表明，所提算法適用于不同的環(huán)境，是實際應(yīng)用中的一種可行方案。

　　參考文獻

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

　?。?］ 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.

　?。?］ 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.

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