一種基于結(jié)構(gòu)化環(huán)境的線性距離特征提取算法

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

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

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

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

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

1 機(jī)器人運(yùn)動(dòng)建模

1.1 速度運(yùn)動(dòng)模型

圖1 機(jī)器人運(yùn)動(dòng)模型

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

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

(1)

(2)

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

(3)

(4)

(5)

(6)

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

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

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

(7)

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

假設(shè)特征之間是相互獨(dú)立的。可以建立在環(huán)境地圖m={m1，m2，…}下的條件概率模型p[f(zt)|xt,m]，其公式如式(8)所示：

(8)

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

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

(9)

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

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

2.1 點(diǎn)云聚類與分割

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

首先采用歐式距離計(jì)算任一時(shí)刻兩個(gè)激光樣本的間距：

(10)

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

2.2 迭代端點(diǎn)匹配及最小二乘擬合

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

圖2 點(diǎn)云分割模型

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

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

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

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

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

圖3 水平豎直線提取

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

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

4 正交線矯正與線段合并

4.1 正交線矯正

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

圖4 正交線矯正

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

(11)

4.2 正交線段合并

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

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

圖5 特征線合并

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

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

5 實(shí)驗(yàn)評(píng)估

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

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

圖7 聚類分割算法

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

圖8 線段擬合

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

圖9 正交線合并

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

圖10 ICP-SLAM點(diǎn)云匹配算法

5.2 地圖匹配精度分析

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

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

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

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

圖12 算法位姿偏差比較

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

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

6 結(jié)論

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

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

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

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

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

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