劉 朋，任工昌，何 舟

（陜西科技大學(xué)機(jī)電學(xué)院，西安710021）

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

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

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

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

1 算法原理

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

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

1.1 線段分割原理

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

圖2 掃描點(diǎn)示意圖Fig.2 Diagram of scanning points

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

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

因?yàn)?/p>

當(dāng)Δθ很小時(shí)，令ki=tanφi，則有

所以

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

由此可得，當(dāng)相鄰兩點(diǎn)處的ki值之差很小時(shí)，就認(rèn)為該兩點(diǎn)處于同一條直線上，否則該點(diǎn)為斷點(diǎn)或獨(dú)立點(diǎn)。

斜率差計(jì)算公式為

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

圖3 斷點(diǎn)示意圖Fig.3 Breakpoint diagram

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

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

圖5 角點(diǎn)示意圖Fig.5 Diagram of angular points

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

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

1.2 線段合并

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

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

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

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

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

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

1.3 角點(diǎn)定位與特征提取

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

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

圖9 真實(shí)角點(diǎn)附近Δk分布Fig.9 Distribution ofΔk around the real corner point

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

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

圖8 真實(shí)角點(diǎn)與掃描點(diǎn)位置Fig.8 Position of the real corner point and scanning points

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

2 算法詳細(xì)描述

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)與結(jié)果分析

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

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

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

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

圖12 提取角點(diǎn)位置示意圖Fig.12 Location diagram of extracted corner features

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

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

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

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

表3 兩種算法運(yùn)算效率對(duì)比Table 3 Comparison of computational efficiency between the two algor ithms

4 結(jié) 論

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

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