匡 兵，田春月，陳鳳冉，孫毛毛

(桂林電子科技大學(xué)機(jī)械工程學(xué)院，桂林 541000)

在傳統(tǒng)的里程計導(dǎo)航中，通常采用航跡推算法進(jìn)行地圖構(gòu)建與定位，但是此方法會隨時間增長產(chǎn)生累積誤差問題，難以滿足機(jī)器人長時間運動的需求。激光雷達(dá)是一種通過點云數(shù)據(jù)確定環(huán)境信息的主動傳感器，具有較高的距離和角度分辨率，因此增加激光雷達(dá)傳感器可增強(qiáng)建圖時對環(huán)境的探測和感知。同時使用特征提取算法，可以提取環(huán)境特征信息，并在已知機(jī)器人地標(biāo)動態(tài)模型的基礎(chǔ)上，獲取位姿信息用于地圖構(gòu)建[1]。線性距離特征提取算法通過對環(huán)境形狀的簡單假設(shè)，估計所需線條特征，只提取結(jié)構(gòu)化環(huán)境中可用于建圖的特征線，便可以描述所需的環(huán)境結(jié)構(gòu)[2]，可較有效地應(yīng)用于即時定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)中，同時相對于傳統(tǒng)的特征提取算法，此算法與地圖的匹配精度更高。

激光雷達(dá)建圖通常采用基于點云的匹配和基于特征的匹配兩種匹配技術(shù)[3]。傳統(tǒng)的ICP(iterative closest point)算法是基于點云的匹配技術(shù)，首先構(gòu)建兩個點云集合中當(dāng)前幀和之前幀的誤差均方根，通過迭代使得到誤差的非線性方程組的解達(dá)到最小，從而達(dá)到建圖的目的。但是由于加入速度信息后會產(chǎn)生嚴(yán)重的運動畸變，因此ICP算法不適用于機(jī)器人在動態(tài)環(huán)境中的運動?；谔卣鞯钠ヅ浼夹g(shù)需要將二維特征地圖的原始掃描點轉(zhuǎn)換為幾何特征。而傳統(tǒng)的特征匹配算法如Hough變換通常是對匹配的圖像進(jìn)行灰度處理后直接從掃描的圖像中提取直線，但是這種方法在估計直線參數(shù)時沒有考慮噪聲和擬合的不確定性[4]。在此基礎(chǔ)上發(fā)展起來的線性回歸算法，采用的是直接在空間中搜索參數(shù)域，然后在相鄰線段中分布式的分析集群信息，其過程十分復(fù)雜，不利于建圖的實時性。隨機(jī)樣本一致性(random sample consensus,RANSAC)算法，是一種在存在異常值的情況下對模型進(jìn)行穩(wěn)定擬合的算法，可用于多種特征類型的分割，在圖像特征提取中應(yīng)用廣泛[4]，但是在真實環(huán)境中，這種概率算法無法最大限度地利用點云數(shù)據(jù)，同時過度迭代可能會陷入局部極值問題。

采用線性距離特征提取算法，對掃描得到的原始激光點云數(shù)據(jù)進(jìn)行初步的聚類分割，通過選定合適的閾值，對不同區(qū)域的點云數(shù)據(jù)分別進(jìn)行最小二乘擬合。構(gòu)建建圖所需的正交線類別，并使用最近鄰算法(KNN)對正交線進(jìn)行篩選，通過線段合并和偏差的矯正完善特征提取算法，生成環(huán)境地圖。以期較為有效地降低建圖的時間復(fù)雜度同時提高地圖匹配精度。

1 機(jī)器人運動建模

1.1 速度運動模型

圖1 機(jī)器人運動模型

在平面上運動的機(jī)器人，可通過構(gòu)建基于條件概率的運動模型，表示其在環(huán)境地圖中的狀態(tài)變化[5]。其中速度運動模型是通過構(gòu)建旋轉(zhuǎn)和平移的速度矩陣，用于控制機(jī)器人的位姿變化。其運動狀態(tài)用位姿表示時，通常使用x、y、θ三個變量進(jìn)行描述。其中x、y為機(jī)器人所在的二維平面的坐標(biāo)；θ為機(jī)器人航向角；w為運動角速度；v為機(jī)器人運動的切線速度。

如圖1所示，令xt-1=(x,y,θ)T為機(jī)器人在t-1 時刻的狀態(tài)估計，假設(shè)機(jī)器人隨時間的運動是恒定的，則有機(jī)器人在t-1時刻的位姿xc、yc分別為

(1)

(2)

可以推導(dǎo)出機(jī)器人特征點經(jīng)過Δt時間后的位姿(x′,y′,θ′)，其中x′,y′為機(jī)器人變換后的位姿坐標(biāo)，θ′為變換后的航向角。

(3)

(4)

(5)

(6)

1.2 地標(biāo)測量模型

在機(jī)器人速度模型的基礎(chǔ)上，建立地標(biāo)測量模型并與環(huán)境地圖相關(guān)聯(lián)，為地圖的聚類分割和特征提取提供測量模型。地標(biāo)測量是將機(jī)器人自身局部坐標(biāo)系的方向和距離與環(huán)境特征點的位姿相匹配，從而為機(jī)器人的導(dǎo)航奠定基礎(chǔ)。

一般用三維列向量表征環(huán)境特征，如式(7)所示：

(7)

式(7)中：f(zt)為建立的環(huán)境特征向量組；ft為表征環(huán)境特征的一組列向量；rt為特征點相對于原點的距離；Φ表示方位；st表征環(huán)境地標(biāo)的類型。

假設(shè)特征之間是相互獨立的?？梢越⒃诃h(huán)境地圖m={m1，m2，…}下的條件概率模型p[f(zt)|xt,m]，其公式如式(8)所示：

(8)

式(8)中：xt為當(dāng)前的狀態(tài)估計值和環(huán)境信息，其值可估計對應(yīng)的特征函數(shù)。

由1.1節(jié)可知，機(jī)器人的位姿可以用xt=(x′,y′,θ′)T表示，由此可得一致狀態(tài)量在t-1時刻的概率分布。通過給定t時刻的觀測數(shù)據(jù)可以估計出狀態(tài)量在t時刻的特征向量：

(9)

式(9)中:mj,x、mj,y表示特征點在世界地圖下的坐標(biāo)值;εσr、εΦr、εsr為在存在噪聲條件下的高斯誤差變量。

2 激光點云數(shù)據(jù)聚類與擬合

2.1 點云聚類與分割

設(shè)采集到的激光點云數(shù)據(jù)集合為Ni，其中每一個激光點坐標(biāo)為(x1,y1),(x2,y2),…,(xn,yn)。根據(jù)選定的距離參數(shù)對樣本間的距離D的大小進(jìn)行分類[6]。

首先采用歐式距離計算任一時刻兩個激光樣本的間距：

(10)

其中設(shè)具有同一點云特征的樣本集合為z1=(xi,yi)，為了對點云數(shù)據(jù)進(jìn)行合理的劃分，需要人為的確定點云間的距離參數(shù)Tmax。若Di>Tmax，則需重新確定一個特征點集區(qū)域z2=(xi+1,yi+1)，將樣本點劃分為z2區(qū)域。若Di

2.2 迭代端點匹配及最小二乘擬合

經(jīng)過聚類分割后的點集不能直接生成適用于線性擬合的最小集合，因此需要將各個區(qū)域內(nèi)的樣本點進(jìn)行迭代端點匹配，將樣本集達(dá)到分割最大化，從而進(jìn)行最小二乘擬合[8]。

圖2 點云分割模型

如圖2所示，首先選取分割好的樣本點區(qū)域，在此基礎(chǔ)上進(jìn)行迭代端點擬合(IEPF)。選定集合中的首點ds和末點dm并確定一條線段，人為選定線段的距離參數(shù)Lmax，在距離線段最遠(yuǎn)處選取一點dn，若存在Ln>Lmax則對線段進(jìn)行分割。之后選取分割點與起始終止點組成的線段重新進(jìn)行分割，重復(fù)此過程，直到該點到直線的距離L

在同一區(qū)域中選定較長的基線，并以其為基準(zhǔn)對區(qū)域內(nèi)的線段進(jìn)行基于線性回歸的最小二乘擬合，對其他線段相對基線的距離求解誤差的平方和，從而將激光數(shù)據(jù)的點云信息擬合成所需的特征線段[9]。

3 基于豎直和水平特征線提取算法

在特征提取法中，需要對前文得到特征線段進(jìn)行分類和篩選，可將樣本區(qū)域中擬合的線段分成三類，分別是豎直和水平的特征線，輕微偏移的非正交直線以及其他類型的特征線。提取水平豎直線段，作為維護(hù)重建地圖所需的主要線，同時對輕微偏移的非正交直線進(jìn)行矯正，并將其他類型舍棄[10]，最后通過仿真實驗評估其性能。

如圖3所示，首先通過一次掃描確定世界坐標(biāo)(xe,ye)的方向，并以此為基準(zhǔn)，在各區(qū)域擬合好的線段中定義出相對于世界坐標(biāo)方向的角度參數(shù)，選定正交線允許的最大角度公差Δφ，作為篩選特征線的最大范圍。若擬合線段相對于基準(zhǔn)坐標(biāo)的角度參數(shù)大于最大角度公差Δφ，則剔除該擬合線段。通過剔除角度偏差過大的線段，最終篩選出用于構(gòu)造結(jié)構(gòu)化環(huán)境地圖的正交或輕微正交的直線。

圖3 水平豎直線提取

在特征線搜索過程中，為了簡化搜索樣本，采用KNN算法將搜索簡化。若兩個樣本特征足夠相似，則有更高的概率為同一類別。根據(jù)上述篩選后的環(huán)境特征，判斷樣本的角度公差，作為其類型標(biāo)簽，繼而判斷待篩選的線段是否為水平或豎直線段[11]。

選擇樣本最近的k個鄰近值，記作l1，l2，…，lk，計算所有訓(xùn)練樣本的距離，其對應(yīng)的類標(biāo)簽為u1，u2，…，uk，則樣本整體可記為{li,ui}。此時k所對應(yīng)的訓(xùn)練樣本lk返回的類標(biāo)簽為最大角度公差，即u=Δφ。當(dāng)搜索到正交線的鄰近值線段時，可以基于鄰近值線段的類標(biāo)簽來判斷此線段的類別。若其鄰近值線段與樣本值線段的角度公差相同，均小于最大角度公差，則劃分為同一類。最終，篩選出具有相同角度公差的類標(biāo)簽線段，完成特征線段的篩選。

4 正交線矯正與線段合并

4.1 正交線矯正

為避免提取的線段受到噪聲或動態(tài)環(huán)境變化的影響，導(dǎo)致定位參考坐標(biāo)系和全局坐標(biāo)系方向不一致造成線段微偏，如圖4所示，需要矯正所提取的線段中輕微非正交部分，以得到正交線[12]。

圖4 正交線矯正

如式(11)所示，選取樣本線段l1、l2，通過計算水平直線的角度加權(quán)平均值w1、w2、w3確定水平參考方向xz，減去π/2即可得到垂直的參考方向yz。由于線段的中點處特征提取較完整，受噪聲干擾較小，因此，需要將觀察的線段圍繞其中點旋轉(zhuǎn)后與參考的方向(xh,yh)對齊，直到xh和xz之間的相對角正交。

(11)

4.2 正交線段合并

在實現(xiàn)正交特征線段矯正后，為了簡化圖形的復(fù)雜程度，避免線條過于雜亂，需將觀測到的水平和豎直線中最長的一段作為基線輸入[13]。計算周圍線段相對基線的距離，并求出其距離的線性方程組的解，最終將求得的解分別估計水平和豎直線。

如圖5所示，觀察到的垂直線的集合為xd={l12,l13,l14,l15,l12,l35,l45,l56,l57}。假設(shè)xd中的所有元素并不相關(guān)，則特征線之間的距離關(guān)系可以用連續(xù)時間內(nèi)的線性方程組l13+l35-l15=0,l14+l45-l15=0描述，其中u1,u2,…,u7為特征線之間的距離。使用卡爾曼濾波對此進(jìn)行狀態(tài)估計，求出其最小解的集合即為線段合并后的最小距離。

圖5 特征線合并

最終得到整體算法流程圖如圖6所示。

圖6 線性距離特征提取算法流程圖

5 實驗評估

5.1 環(huán)境地圖對比

為驗證算法的有效性，使用2D思嵐激光雷達(dá)進(jìn)行試驗，其幀率為10 Hz，測量半徑為10～12 m，將算法在MATLAB平臺中分析。首先選擇可用來實驗的室內(nèi)結(jié)構(gòu)化環(huán)境。之后使用2D思嵐激光雷達(dá)對環(huán)境進(jìn)行掃描并采集數(shù)據(jù)，采用特征提取算法構(gòu)建環(huán)境地圖。同時在相同的實驗環(huán)境下，與傳統(tǒng)的ICP點云匹配算法對比，分析兩種算法在建圖完整性上的優(yōu)劣。首先獲取來自激光雷達(dá)的原始點云數(shù)據(jù)，并求出相鄰點云距離，使用聚類分割算法選定合適的閾值Tmax進(jìn)行特征區(qū)域分割。如圖7所示，紫色部分為原始點云信息，綠色部分為采用特征提取算法聚類分割后的部分點云擬合線段。

圖7 聚類分割算法

如圖8所示，為了避免區(qū)域分割后點云數(shù)據(jù)較為離散，使線段更加完整，需在此基礎(chǔ)上進(jìn)行最小二乘擬合，得到連續(xù)的地圖邊界，如圖8中紅色部分所示。

圖8 線段擬合

對已提取的水平豎直特征線，采用KNN算法進(jìn)行豎直和水平特征線段的分類，得到與標(biāo)簽類型相似的正交線段。同時，通過濾波可濾除與建圖無關(guān)的非正交線段。對雜亂的線段進(jìn)行合并處理，使離散的線段與基線合并，得到如圖9所示的較為完整的地圖邊界。

圖9 正交線合并

如圖10所示，在同一環(huán)境下，采用點云匹配算法(ICP)進(jìn)行對比實驗。相比特征提取算法，ICP點云匹配算法所建立的地圖在邊界處較為模糊，同時實驗中噪聲點過多，因此需要進(jìn)一步進(jìn)行高斯濾波處理，此方法計算負(fù)荷過大，會導(dǎo)致內(nèi)存消耗增大，同時對算法進(jìn)行進(jìn)一步優(yōu)化，加大了計算復(fù)雜度。

圖10 ICP-SLAM點云匹配算法

5.2 地圖匹配精度分析

衡量地圖構(gòu)建精度的常規(guī)方式有兩種：第一是對比實時建圖的質(zhì)量，第二是通過算法生成的地圖邊界計算平均定位誤差的大小[14]。

將豎直和水平線的特征提取算法和目前ICP點云匹配算法進(jìn)行對比。如圖11所示，試驗使用KITTI數(shù)據(jù)集，調(diào)用兩種算法并建圖。對比運動過程中機(jī)器人實時建圖的質(zhì)量。由圖11可知，所提出的特征提取算法(藍(lán)色)與地圖(虛線)的匹配程度更好。

如圖12可知，兩種算法分別在x、y兩個位姿方向上相對于真實軌跡坐標(biāo)的定位誤差。特征線提取算法(藍(lán)色線段)明顯更接近于真實軌跡部分(虛線)。因此，特征點匹配法在相對結(jié)構(gòu)化的環(huán)境中地圖的匹配度較好。

虛線為數(shù)據(jù)集的真值；藍(lán)色線段為ICP-SLAM算法生成的軌跡；綠色線段為特征線提取算法(ORTH-SLAM)生成的軌跡

圖12 算法位姿偏差比較

如表1所示，為了對比平均定位誤差的大小，選取相同的數(shù)據(jù)集，測試得到線性距離特征提取算法的占用率較低，為39.57%，表明此算法在一定程度上可降低計算的復(fù)雜度。相同情況下，平均定位誤差為2.7 m，可滿足工業(yè)生產(chǎn)中的定位精度要求，同時與地圖的匹配程度較高。因此本文算法可較為有效地減少內(nèi)存的占用率，適用環(huán)境范圍更廣。

表1 地圖匹配結(jié)果分析

6 結(jié)論

提出了一種用于機(jī)器人在結(jié)構(gòu)化環(huán)境中建圖的特征提取算法，并得到以下結(jié)論。

(1)該算法首先建立了機(jī)器人的動態(tài)地標(biāo)模型，得到機(jī)器人的實時位姿。

(2)通過聚類分割算法，對原始的激光點云數(shù)據(jù)進(jìn)行預(yù)處理，并將區(qū)域分割后的點云數(shù)據(jù)進(jìn)行最小二乘擬合。

(3)建立線段相對于世界坐標(biāo)的方向基準(zhǔn)，通過對比最大角度公差，篩選出豎直和水平的直線用于地圖的創(chuàng)建。

(4)最后使用最近鄰(KNN)濾波法對擬合的線段進(jìn)行處理，以快速篩選出具有豎直和水平特征的線段。

該算法通過與傳統(tǒng)的(ICP)點云匹配算法比較，其優(yōu)點在于在動態(tài)的建圖環(huán)境中較少受到運動畸變的影響，同時建圖較完整。同時與傳統(tǒng)點云(ICP)匹配算法相比，該算法一定程度上降低了運算的復(fù)雜度，使地圖輪廓清晰度提高，同時與地圖匹配度較好，可用于解決優(yōu)化激光點云在邊緣處點云稀疏的問題。