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

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

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

　　關鍵詞：同時定位與地圖構建；數據關聯；數據預處理；最近鄰；聯合相容

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

　　引用格式：李延炬，肖宇峰，古松，等.基于激光傳感器的SLAM數據關聯算法的研究［J］.微型機與應用，2017,36（2）：78-82.

0引言

　　*基金項目：四川省教育廳重點項目（14ZA0091）；四川省科技支撐計劃項目（2015GZ0035）；四川省科技支撐計劃項目（2015GZ0342）移動機器人同時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）是指機器人在未知環境中移動時根據傳感器收集的信息創建環境地圖，同時利用該地圖進行自身的定位。SLAM中的數據關聯問題是指建立在不同時刻、不同位置通過傳感器獲得的觀測特征之間和地圖特征之間的對應關系，以確定它們是否來源于實際環境中的同一物理實體［1］。在SLAM中，數據關聯的計算復雜度和準確度直接影響著系統狀態估計的時間耗度和精確性，對最終建立的地圖有著關鍵性的影響。故合適的數據關聯算法在機器人SLAM過程中至關重要。

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

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

1EKF-SLAM算法原理

　　在SLAM問題中，系統的狀態向量可以表示為：

　　Xk=［XTv,kMT］T(1)

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

　　（1）預測

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

　　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時刻機器人位姿狀態；Xk表示k時刻機器人狀態向量，包括地圖特征信息；uk－1=［Vk－1αk－1］表示機器人的控制向量，包括機器人速度Vk－1和偏航角度αk－1；ωk－1是方差為Qk－1的高斯白噪聲；fv(·)表示一非線性函數，用于計算k時刻機器人位姿狀態。

　　②在k－1時刻，計算機器人的k時刻狀態估計值和估計協方差如下：

　　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表示非線性函數fv(·)在點xk－1處的一階Taylor展式線性化的結果。

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

　　（2）觀測

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

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

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

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

　　vk=Zk－k(8)

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

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

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

　　(3)更新

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

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

　　②計算更新機器人的狀態估計Xk和協方差估計Pk。

　　Xk=k,k－1+Kkvk(12)

　　Pk=(I－Kkhxk)Pk,k－1(13)

2數據關聯

　　2.1獨立相容最近鄰（ICNN）算法

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

　　在關聯假設集Hm下，計算觀測特征Zk,i與每一個地圖特征Fj的預測觀測值k之間的新息及新息協方差如下：

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

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

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

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

　　Mk=vTkS－1kvk(17)

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

　　Nk=min(Mk+ln|Sk|)(18)

　　2.2聯合相容分枝定界（JCBB）算法

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

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

　　聯合新息及聯合新息協方差為：

　　vHm=ZHm－hHm(k,k－1)(20)

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

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

　　D2Hm=vTHmS－1HmvHm≤χ2d,1－α(22)

　　此時稱所有觀測特征和地圖特征是聯合相容的。

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

3優化算法

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

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

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

　　①選取局部地圖

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

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

　　②分集

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

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

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

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

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

　　①去噪

　　將所有的觀測特征加入預處理特征集，為有效地濾除噪聲點，減少干擾，對特征集里的特征點依次計算相鄰距離D(n,n+i)，與設置的閾值Δ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依據激光傳感器的模型特性確定。

　　②確定閾值ΔD

　　二維激光雷達的測量點在極坐標系下表示形式為(θi,ri) ，對應的直角坐標系下的參數xi=ricosθi，yi=risinθi ，位姿描述為ui=(θi,ri,xi,yi)。其中θ表示測量角度，r表示測量距離，i=(1,2,…,n)表示測量點。根據公式l=|α|r，可以求得相應角度在不同極徑的弧長，于是可得Δ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)依次與本區域點{(xi+2,yi+2),(xi+3,yi+3),…,(xi+j,yi+j)}進行閾值比較，判斷各點是否和首點屬于同一子集，直到該區域最后一點。

　　（3）關聯合適的關聯集

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

　　（4）使用ICNN算法求關聯解

　　（5）若步驟（4）求解失敗，則使用JCBB算法求關聯解

4實驗與分析

　　4.1實驗模型

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

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

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

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

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

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

　　4.2實驗結果與分析

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

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

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

　　從圖4中可以看出，在復雜環境SLAM過程中，ICNN、JCBB和混合算法實現效果差別巨大；圖5顯示ICNN算法直接失敗，機器人觀測特征與地圖特征匹配結果錯誤，導致機器人運動路徑與理論路徑相差甚遠，最終無法到達目標點；圖6中，使用JCBB算法，觀測特征與地圖特征關聯正確率高，機器人在SLAM過程中，能精確地控制自己的位姿，實際運動路徑接近于理論路徑，但是運算時間很長，執行一次仿真要數個小時以上，混合算法的效果與JCBB算法大致一樣，精度略有降低，但是計算時間少，可以滿足機器人實時SLAM，其詳細數據如表2所示。在此，分別比較三種數據關聯方法在簡單環境和復雜環境下SLAM執行的有效性，得到了三種算法的計算相對時間和目標位置精度的比較值。可以看出，三種算法中，混合算法運行時間最短，并且保持了相對較好的精度，在機器人SLAM過程中應用效果良好。

5結論

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

　　參考文獻

　　［1］ 周武．面向智能移動機器人的同時定位與地圖創建研究［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數據關聯方法［J］.計算機應用,2009,29(1):136-148.

　　［6］ 郭劍輝,趙春霞,石杏喜.一種改進的聯合相容SLAM數據關聯方法［J］.儀器儀表學報,2008,29(11):2260-2265.