基于斜率差的激光雷達環境特征提取方法

劉 朋 任工昌 楊力鵬 胡小龍

陜西科技大學機電工程學院，西安，710021

0 引言

自主移動機器人近年來在工業、農業和服務業中得到了廣泛的應用，它應當具備的基本功能就是環境的感知與定位。激光雷達具有較高的測距精度和實時性，故普遍被用作自主移動機器人的環境感知傳感器[1]。利用激光雷達進行環境特征的提取，關鍵在于如何從激光雷達掃描點的距離數據中提取出可用于移動機器人定位和構建地圖所需的特征線段或特征點[2]。

在移動機器人領域，研究人員提出了多種利用激光雷達距離數據提取直線特征的算法，這些算法可以大致分為兩類：序慣算法和遞歸算法。序慣方法有基于點間距離的分割(point-distance-based segmentation，PDBS)算法、連續邊緣跟蹤 (successive edge following，SEF)算法和直線跟蹤(line tracking，LT)算法。遞歸方法有迭代適應點 (iterative end point fit，IEPF)算法[3]和分割-合并(split-merge,SM)算法[4]。除此以外，還有不依賴于局部信息的霍夫變換(Hough transform,HT)算法[5]。其中，序慣類算法多存在閾值選取困難、無法對相交直線進行分割等缺點；遞歸類算法對閾值較為敏感，存在過分割或欠分割的現象，而且運算量較大。HT算法不適合實時應用，且由于它采用投票的方式確定直線，提取的直線嚴重依賴于掃描點的空間密集程度。

滿增光等[6]提出了一種分開-合并框架，先對數據集合以某種準則遞歸地分割，分割后再把這些集合以某種準則遞歸地合并。在分開階段，應用 IEPF算法對數據集合遞歸分割；在合并階段，同時采用端點擬合(end point fit,EPF)算法和總體最小二乘法兩種直線擬合誤差作為合并條件的參考量對數據集合遞歸合并。該方法較好地解決了IEPF算法固定閾值所帶來的過分割或欠分割的問題，降低了誤分割率，取得了較好的特征提取效果，但是計算量依然偏大，計算效率較低[7-10]。

綜上所述，目前從激光雷達距離中提出直線特征的方法較多，但普遍采用迭代計算，依然缺少一種高效、簡便、準確率高的算法。本文綜合上述方法的優點，提出了一種利用斜率差進行分割和特征點提取的方法。

1 算法原理

本文方法以激光雷達作為圓心，利用相鄰點的斜率差來區分斷點、角點或獨立點。如圖1所示，從O點向直線L以等角度Δθ的間隔畫直線，假設交點依次為P1，P2，…，O點到交點的長度依次為ρ1，ρ2，…，然后，由P1點向OP2作垂線相交于P′1點，則φ1為直線L與P1P′1的夾角，同理可得φ2、φ3。由幾何關系可得

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

因為

當Δθ很小時，則有

(1)

令第i點處的斜率ki為

(2)

所以

k3-k2≈k2-k1≈0

圖1 掃描點示意圖Fig.1 Scanning point diagram

由此可得，當相鄰兩點處的ki值之差很小時，就認為這兩點處于同一條直線上，否則該點為斷點或獨立點。如圖2所示，由O點分別向直線L1和L2以等角度間隔θ作射線，與直線L1交于點P1,…,Pi-1,Pi，與直線L2交于點Pi+1,…,Pn-1,Pn，其中ρi表示O點至Pi點的距離，Pi和Pi+1分別為直線L1和L2的斷點。

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

如圖3中實線所示，在斷點處k值有明顯的峰值。為了消除相鄰兩點ki值微小的差值，令

Δk(i)=ki-ki-1

(3)

圖3 斷點處Δk分布示意圖Fig.3 The distribution diagram of Δk at the break point

則Δk如圖3中虛線所示，在直線L1的末點Pi和L2的起點Pi+1對應的Δk(i-1)和Δk(i)處峰值非常明顯，且這兩處峰值的符號相反。同理，如圖4所示，直線L1和直線L2相交于點Pi，則Pi為兩條直線的角點。分別按照式(2)和式(3)計算ki和Δk，如圖5所示。在角點Pi處Δk(i)有較明顯的峰值。

圖4 角點示意圖Fig.4 Angular point diagram

圖5 角點處Δk分布示意圖Fig.5 The distribution diagram of Δk at the angular point

某次實驗中激光雷達部分掃描點數據如圖6所示。按照式(3)計算的Δk分布情況如圖7所示，雖然由于掃描的誤差而導致Δk波動，但在角點53處有明顯的峰值。

圖6 實驗中部分掃描點數據Fig.6 Some scanning point data in the experiment

圖7 角點處Δk分布Fig.7 The distribution of Δk at the angular point

2 算法詳細描述

激光雷達每掃描環境一次，返回一組有序二維激光雷達數據，將此數據預處理后得到點集

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

式中，θi、ρi分別為掃描第i點時轉過的角度和返回的距離。

本算法分割斷點和角點的算法如下：

(1)根據點集P，利用前述原理計算相鄰掃描點斜率的差值Δk，即

(4)

i=2,3,…,n-1

因為激光雷達具有環形掃描的特點，故可將第n點作為第1點的前一個掃描點，將第1點作為第n點的后一個掃描點，即

如果相鄰的3個掃描點斜率的差值Δki-1、Δki和Δki+1均大于閾值kth，且|Δki-Δki-1|>2kth，|Δki+1-Δki|>2kth，則認為第i點是第一類孤立點(圖8中第28點)。找出所有第一類孤立點，刪除后更新點集P。

圖8 第一類孤立點Fig.8 The first kind of isolated point

(2)更新點集P后，再依次計算第i個掃描點的θi與相鄰兩掃描點的θi-1和θi+1的差值，如果|θi-θi-1|>θth，且|θi+1-θi|>θth(θth為閾值)，則認為第i點為第二類孤立點(圖9中第257點)。找出所有第二類孤立點，刪除后再次更新點集P。

圖9 第二類孤立點Fig.9 The second kind of isolated point

(3)將去除孤立點后的點集P重新利用式(4)計算相鄰掃描點的斜率差值，進行斷點和角點的判斷。判斷規則如下：①如果|θi+1-θi|>θth，則第i點和第i+1點均為斷點，分別為前一條直線上的末點和后一條直線的起點；②如果Δki和Δki+1均大于閾值kth，且ΔkiΔki+1<0，則第i點和第i+1點均為斷點，分別為前一條直線上的末點和后一條直線的起點；③如果|Δki|>αkth(0<α<1)， |Δki+1-Δki|>0， |Δki-Δki-1|>0，且 (Δki+1-Δki)(Δki-Δki-1)<0，則第i點為角點，即前一條直線的終點，同時也是后一條直線的起點，其中，α為角點閾值系數。

(4)根據點集P中各點的屬性，從第1點至最后一點依次分割，并計算每條線段的始末點坐標和線段的斜率，得到線段集L。

(5)由于激光雷達工作的特點，有可能會從某條直線的中間部分開始掃描，所以線段集中的第一條和最后一條線段可能屬于同一條線段。如果第一條線段的始末點均在最后一條線段所在的直線上，則將這兩條線段合并。

3 實驗及結果分析

實驗時以某樓端側梯形平臺為測試對象，該平臺幾何尺寸使用卷尺測量后如圖10所示。選用SLAMTEC公司的RPLIDAR A2型激光雷達，在測試平臺隨機選取10個點進行掃描。算法在MATLAB平臺下實現。實驗中各參數如下：θth=2.5°，kth=0.95，α=0.7。

圖10 實驗測試平臺Fig.10 Experimental test platform

實驗中第1次激光雷達掃描數據按照本文算法進行分割和特征提取的結果如圖11所示。其中，實心圓點表示激光雷達的掃描點，方框表示根據算法提取的角點或斷點特征，直線表示根據算法提取的直線段特征，圓圈表示激光雷達位置。由圖11可以發現，該算法在特征點和特征線段提取方面可以獲得與手工提取相同的結果。其他9次實驗也可以得到相同的結論。

圖11 第1次實驗結果Fig.11 The result of first experiment

為了進一步驗證本算法提取線段特征的準確性，計算提取線段特征的長度和相對角度，并與實際測量值進行了對比。相對角度是在提取的特征線段中，以BA線段為基準，其他線段與BA線段的角度差作為評判依據。由于激光雷達掃描的特點，刪除了部分因遮擋或距離限制而無法完整提取的線段特征。實驗結果見表1和表2，線段編號與圖10相對應。其中，最大差值百分比依據提取值和測量值差值的絕對值與測量值的百分比進行計算。

表1 提取特征線段長度與測量值對比

表2 提取特征線段角度與測量值對比

由表1和表2中數據可以發現，相對差值均在3%以內。在長度方面，除第5次測量的差值稍大以外，其余9次測量的差值均小于2%。在角度方面，只有線段AN、NM的個別次測量相對差值超過了2%，而GF和FE兩條線段的差值均在1%以內。

4 結論

在提取特征點時，本文算法不需要迭代，減小了計算工作量，而且對閾值不敏感，也減小了計算的難度。實驗結果表明，本文算法在掃描點分割方面可以獲得與手工分割相同的結果；與測量值對比，提取的特征線段也可獲得較高的準確度。