劉 朋，任工昌，何 舟

（陜西科技大學機電學院，西安710021）

2D激光即時定位與構(gòu)圖（Simultaneous local?ization and mapping，SLAM）技術(shù)由于其相對簡單，發(fā)展時間長，技術(shù)較為成熟，目前被廣泛應(yīng)用于物流配送、家庭服務(wù)等類型的自主移動機器人中。該技術(shù)的基本方法可以概括為：機器人通過激光雷達對周圍環(huán)境信息進行觀測，同時利用機器人的控制信息（里程計）實現(xiàn)對機器人的即時定位與地圖構(gòu)建。因為里程計信息具有較大的累積誤差，為了獲得機器人軌跡的長期精確估計，需要將機器人所感知的環(huán)境信息與之前建立并實時更新的地圖進行匹配。所以，匹配是SLAM中的一個關(guān)鍵問題。

在匹配算法中，觀測信息量的大小直接影響著算法的精度和效率。如果能從激光雷達掃描數(shù)據(jù)中提取出較為精確的特征信息，將會大幅減少整個SLAM算法的運算量。在激光雷達數(shù)據(jù)中普遍存在斷點、角點、線段和圓弧段等幾類特征［1］。由于環(huán)境中存在障礙物，造成斷點和線段特征會隨著觀測位置的變化而變化?；【€段特征在室內(nèi)環(huán)境下不多見，且其相對復雜，探測比較困難。鑒于以上原因，本文以提取角點特征進行匹配，可以很好地滿足SLAM算法的需要［2?3］。

目前，對于角點特征的提取，一般都是先提取線段特征，再計算兩條線段的交點以獲得角點特征。線段提取方法中，序慣類的基于點間距離的分割（Point?distance?based segmentation，PDBS）算法和連續(xù)邊沿跟蹤（Successive edge following，SEF）算法［4］，是兩種最簡單的線段類特征提取方法，但是沒有考慮激光雷達以固定間隔進行掃描的特點，效果往往較差。直線跟蹤（Line tracking，LT）算法［5］對閾值敏感，很容易錯誤地將應(yīng)屬于下一條線段上的點合并到當前線段上；遞歸類的迭代適應(yīng)點（Iterative end point fit，IEPF）算法［6?7］和分割與合并（Split?merge，SM）算法［8?9］，也對噪聲和閾值敏感，當采用固定閾值時很容易造成欠分割或過分割現(xiàn)象，而且運算量較大。Hough變換算法［10?11］特征精度較高，但計算量較大，難以保證局部地圖構(gòu)建的實時性。滿增光等［12］提出了一種通過計算角點函數(shù)的方法直接提取角點特征，但該算法在進行角點特征濾波和角點函數(shù)計算時，計算量仍然偏大，并且存在從正反兩個方向提取時得到不同結(jié)果的可能。

綜上所述，目前對于從激光雷達掃描點集中提取特征的方法較多，但是依然缺少一種高效、簡便和準確的算法。因此，本文根據(jù)激光雷達掃描的特點，提出了一種通過點集分割、線段合并和求取交點相結(jié)合的角點特征提取方法。該方法只需要一次計算出相鄰點所構(gòu)成的兩條直線間的斜率差，即可以實現(xiàn)對點集的分割。與IEPF和SM等遞歸類算法相比，不需要進行迭代，在一定程度上降低了計算量。另外，該算法避免了由于掃描點之間的間隔造成提取角點誤差較大的問題；同時，利用兩點擬合直線代替最小二乘法，又可以進一步降低計算量。

1 算法原理

本文提出的算法包括3部分。首先使用激光雷達掃描獲得的點集，通過計算斜率差對其進行初步分割。然后，計算分割后每條線段的斜率，對過分割的線段依據(jù)線段間的連接關(guān)系進行合并，并獲得可能的角點位置。最后，對角點進行修正并計算角點坐標。算法總體框圖如圖1所示。

圖1 算法總體框圖Fig.1 Overall block diagram of algorithm

1.1 線段分割原理

如圖2所示，從O點向直線l以等角度Δθ的間隔畫直線，交點依次為P1、P2、P3、P4、…，O點到交點的長度依次為ρ1、ρ2、ρ3、ρ4、…，然后，由P1點向OP2做垂線相交于P1'點，P2點向OP3做垂線相交于P2'點，P3點向OP4做垂線相交于P3'點，則φ1為直線l與P1P1'的夾角，同理可得φ2、φ3。其中∠P1OP1'=∠P2OP2'=∠P3OP3'=Δθ。

圖2 掃描點示意圖Fig.2 Diagram of scanning points

由幾何關(guān)系可得

φ3=φ2+Δθ=φ1+2?Δθ

因為

當Δθ很小時，令ki=tanφi，則有

所以

tanφ3-tanφ2≈tanφ2-tanφ1≈0

由此可得，當相鄰兩點處的ki值之差很小時，就認為該兩點處于同一條直線上，否則該點為斷點或獨立點。

斜率差計算公式為

如圖3所示，當點Pi和Pi+1分別為直線L1的末點和直線L2的起點時，Δk（i）和Δk（i+1）的值如圖4所示，在直線L1的末點Pi和L2的起點Pi+1對應(yīng)的Δk（i）和Δk（i+1）處峰值非常明顯，且該兩處峰值的符號相反。當|Δk（i）|>dkth，|Δk（i+1）|>dkth，且Δk（i）·Δk（i+1）<0時，第i點為前一條直線的末點，第i+1點為后一條直線的起點。其中dkth為線段分割閾值，該值與激光雷達的測量誤差有關(guān)，可通過實驗的方法選取，同時為了避免出現(xiàn)欠分割的情況，dkth的值在選取時應(yīng)盡量偏小。

圖3 斷點示意圖Fig.3 Breakpoint diagram

圖4 斷點處Δk分布示意圖Fig.4 Distribution diagram ofΔk at the break point

如圖5所示，當點Pi為直線L1和直線L2的交點時，則Pi為兩條直線的角點，Δk（i）的值如圖6所示，在角點Pi處Δk（i）有較明顯的峰值。當|Δk（i）|>α·dkth，|Δk（i）|>|Δk（i-1）|，且|Δk（i）|>|Δk（i+1）|時，第i點為角點，即為前一條直線的末點，同時也是后一條直線的起點。其中α為角點線段分割閾值系數(shù)，該值也可通過實驗的方法選取，一般取值為0.6。

圖5 角點示意圖Fig.5 Diagram of angular points

圖6 角點處Δk分布示意圖Fig.6 Distribution diagram ofΔk at the angular point

假設(shè)激光雷達掃描數(shù)據(jù)每幀有n個掃描點，本方法使用式（2）計算n-2次得到相鄰兩點的Δk，與相應(yīng)的分割閾值進行比較即可獲得較為精確的線段分割結(jié)果和線段連接關(guān)系。而如果使用IEPF算法，由于需要尋找與直線距離最大的點不斷進行迭代，至少需要計算2n次以上點到直線的距離才可得到相同的分割結(jié)果。所以，與IEPF等迭代類算法相比，本算法具有計算簡單、計算量小的優(yōu)點，特別是當掃描點數(shù)或分割線段較多時，這種優(yōu)勢更加明顯。

1.2 線段合并

經(jīng)過初步分割后，得到的線段集L為

L={(Lbi，Lei，Lpi)，i=1，2，…，m}

式中：Lbi表示第i條線段的起點在點集P中對應(yīng)的點數(shù)；Lei表示第i條線段的終點在點集P中對應(yīng)的點數(shù)；Lpi表示第i條線段是否與第i+1條線段相連，Lpi為1表示兩條線段由角點相連，否則為斷點斷開。

為了避免出現(xiàn)欠分割的情況，dkth的值一般選擇可取范圍的下限，所以依據(jù)Δk（i）值進行分割時可能會出現(xiàn)過分割的情況。計算每條線段Li的斜率kLi，如果相鄰的兩條直線相交且其夾角tanθ小于線段合并閾值θth時，即可將此兩條直線進行合并。經(jīng)過實驗分析，θth一般取值為0.3，當θth大于0.4時會造成過多的誤合并，而當θth小于0.2時合并效果不明顯。

如果線段集中Li和Li+1兩條直線相交，其斜率分別為kLi和kLi+1，如圖7所示，則兩條直線的夾角為

圖7 線段夾角示意圖Fig.7 Angular diagram of two intersecting lines

1.3 角點定位與特征提取

激光雷達在對環(huán)境進行掃描時，獲得的是離散的掃描點。因此，從這些掃描點中提取出的角點特征位置與真實的物理角點位置之間存在一定的誤差，特別是物理角點離激光雷達較遠的情況，這種誤差將會很大。為了減少提取出的角點特征位置與真實物理角點位置之間的差值，需要根據(jù)初步線段分割后獲得的可能角點進行精確定位。

如圖8所示，58～63點在一條直線上，64～70點在另一條直線上，“★”位置為該兩條直線相交的真實角點位置。按照前述分割方法，64點處的Δk最大（圖9），是兩條線段相交的角點，很明顯此位置與真實角點位置之間差值較大，所以不能直接使用線段分割時獲得的可能角點位置直接作為角點特征。

圖9 真實角點附近Δk分布Fig.9 Distribution ofΔk around the real corner point

出現(xiàn)此種情況的主要原因在于掃描點是離散的，而真實的角點出現(xiàn)在相鄰兩個掃描點之間。如圖8所示，64點應(yīng)在第2條直線上，但是分割后其作為第1條直線的末點，同時作為第2條直線起點，因此導致角點位置的差異。

進一步分析Δk發(fā)現(xiàn)，當Δk（i）符合角點特征，出現(xiàn)峰值時，如果|Δk（i）-Δk（i-1）|<|Δk（i）-Δk（i+1）|，則角點應(yīng)在第i-1和第i點之間，相反，角點則在第i點和第i+1點之間。如圖8、9所示，真實角點位置在第63點和第64點之間，第63點為前一條線段末點，第64點為后一條線段起點。分別擬合出兩條直線，求出交點即為角點特征。

圖8 真實角點與掃描點位置Fig.8 Position of the real corner point and scanning points

在進行直線擬合時，目前普遍采用的方法是最小二乘法，此方法的優(yōu)點是擬合時考慮到了每一個點，盡量減小了由激光雷達掃描誤差而引起的擬合誤差，但該方法的缺點是運算量相對偏大。為了減小運算量，本算法已經(jīng)剔除掉了不在一條直線上的點，在待擬合的點集中找出起點、末點和中間點，將點集分為前后兩段，然后算出前后兩部分點集坐標的平均值作為新點，最后以此兩點來擬合直線，這樣可以大幅減小直線擬合時的計算量。

2 算法詳細描述

激光雷達每掃描環(huán)境一次，返回一組有序二維激光雷達數(shù)據(jù)，將此數(shù)據(jù)預處理后得到點集為

P={(θi，ρi)，i=1，2，…，n}

其中θi和ρi分別為掃描第i點時轉(zhuǎn)過的角度和返回的距離。

步驟1 根據(jù)點集P，利用前述原理中式（2）計算相鄰掃描點斜率的差值Δk，即

式中i=2，3，…，n-1。然后，根據(jù)Δki、Δki+1與dkth的關(guān)系對點集P進行分割，獲取初步分割后的線段集L。其中

L={(Lbi，Lei，Lpi)，i=1，2，…，m}

步驟2 如果線段集中Lpi=1，則取出Lbi、Lei、Lbi+1、Lei+1點對應(yīng)坐標，計算線段Li和Li+1的斜率kLi、kLi+1；然后依據(jù)式（3）計算tanθ，如果tanθ<θth則線段Li和Li+1合并，將線段Li中的末點Lei修正為Lei+1，并將線段Li+1從線段集L中刪除。

步驟3 在合并后的線段集L中，當?shù)趇條線段的Lpi=1時，則在點集P中取出該線段末點Lei的對應(yīng)點Pq及其前后兩點Pq-1和Pq+1，并依據(jù)式（2）計算出相應(yīng)的Δk（q-1）、Δk（q）、Δk（q+1）；如果|Δk（q）-Δk（q-1）|<|Δk（q）-Δk（q+1）|，則將線段Li的末點Lei修正為點Pq-1，否則將線段Li+1的起點Lbi+1修正為Pq+1。

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

實驗數(shù)據(jù)來自Cartographer ROS提供的德意志博物館Deutsches Museum的2D激光SLAM數(shù)據(jù)集［13］。算法使用Matlab2016b，計算機CPU為Intel Core i5?6300U，內(nèi)存8G。實驗中各參數(shù)為：θth=0.3，dkth=1.0，α=0.6。從數(shù)據(jù)集中選取了10幀數(shù)據(jù)進行角點特征提取和直線擬合。由于篇幅限制，圖10和圖11給出了第5幀和第10幀的點集數(shù)據(jù)和特征提取結(jié)果，其中圓點表示激光雷達掃描點，直線為擬合的線段，“*”位置表示提取的角點特征位置。

圖10 第5幀點集數(shù)據(jù)和提取的角點特征Fig.10 Scanning point data and extracted corner features from frame 5

圖11 第10幀點集數(shù)據(jù)和提取的角點特征Fig.11 Scanning point data and extracted corner features from frame 10

為了驗證算法的正確性，從結(jié)果中挑選了一處具有明顯角點特征的環(huán)境掃描數(shù)據(jù)進行分析，如圖12所示。其中A、B、C為提取的角點位置，線段AB和BC為擬合的直線。由于移動機器人在不同位置提取的特征在激光雷達坐標系下具有不同的位置，但是兩個特征的相對位置卻保持不變。因此，為了衡量提取角點特征的精度，本文以兩角點之間的距離和角點兩直線的夾角作為評判依據(jù)，即線段AB和BC的長度及∠A、∠B和∠C的大小。

圖12 提取角點位置示意圖Fig.12 Location diagram of extracted corner features

如表1所示，根據(jù)10幀數(shù)據(jù)提取的結(jié)果，線段長度與均值之間的差值較大的是線段BC，差值范圍為［-0.009，0.011］，提取的角度與均值之間差值較大的是∠A，差值范圍為［-1.01，0.86］。由此可見，本文提出的角點特征提取算法具有較高的定位精度，可以滿足自主機器人的建圖與定位需要。

表1 角點特征提取結(jié)果Table 1 Results of extracting corner features

為了進一步驗證本文算法在直線擬合方面的精度和計算效率，從上述10幀數(shù)據(jù)中選取了5幀，分別采用本文直線擬合方法和最小二乘法對分割后的線段進行擬合，獲得角點B和B'點坐標，結(jié)果如表2所示。由此對比結(jié)果可得，本文算法計算的B點與最小二乘法計算的B'點之間的距離范圍為［0.003，0.007］，所以使用本文算法可以得到與最小二乘法基本相同的結(jié)果。表3為本算法與使用最小二乘法進行擬合時計算效率的對比，通過對比發(fā)現(xiàn)，本算法在直線擬合時相較最小二乘法有較為明顯的優(yōu)勢，單幀掃描數(shù)據(jù)提取特征角點的用時均小于10 ms。

表2 特征點提取結(jié)果對比Table 2 Comparison of the results extracting feature point between the two algorithms

表3 兩種算法運算效率對比Table 3 Comparison of computational efficiency between the two algor ithms

4 結(jié) 論

本文提出了一種利用激光雷達掃描數(shù)據(jù)直接提取角點特征的算法。該算法使用激光雷達中獲得的掃描點對應(yīng)矢徑長度和角度，計算相鄰點的斜率差對點集進行初始分割。然后，計算分割后每部分點集對應(yīng)線段的斜率，對過分割的點集進行合并。最后，通過計算相鄰兩直線的交點對角點特征進行定位和提取。在進行點集分割時，本算法只需要一次計算出相鄰點的斜率差，與其他迭代類算法相比，減小了計算量。在對直線進行擬合求取角點時，已根據(jù)角點處斜率差的分布特點對角點進行了定位，實現(xiàn)對點集分割結(jié)果的修正，所以使用兩點擬合直線代替?zhèn)鹘y(tǒng)的最小二乘法，在滿足精度的前提下，也在一定程度上提高了計算效率。通過實驗表明，本算法能夠準確獲得角點位置，并且精度較高，特別適用于使用嵌入式系統(tǒng)開發(fā)的自主機器人SLAM算法中。

本算法僅完成了角點特征的提取，如果需要可以在此基礎(chǔ)上進一步提取線段、斷點或其他特征。