劉 朋 任工昌 楊力鵬 胡小龍

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

0 引言

自主移動(dòng)機(jī)器人近年來(lái)在工業(yè)、農(nóng)業(yè)和服務(wù)業(yè)中得到了廣泛的應(yīng)用，它應(yīng)當(dāng)具備的基本功能就是環(huán)境的感知與定位。激光雷達(dá)具有較高的測(cè)距精度和實(shí)時(shí)性，故普遍被用作自主移動(dòng)機(jī)器人的環(huán)境感知傳感器[1]。利用激光雷達(dá)進(jìn)行環(huán)境特征的提取，關(guān)鍵在于如何從激光雷達(dá)掃描點(diǎn)的距離數(shù)據(jù)中提取出可用于移動(dòng)機(jī)器人定位和構(gòu)建地圖所需的特征線段或特征點(diǎn)[2]。

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

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

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

1 算法原理

本文方法以激光雷達(dá)作為圓心，利用相鄰點(diǎn)的斜率差來(lái)區(qū)分?jǐn)帱c(diǎn)、角點(diǎn)或獨(dú)立點(diǎn)。如圖1所示，從O點(diǎn)向直線L以等角度Δθ的間隔畫(huà)直線，假設(shè)交點(diǎn)依次為P1，P2，…，O點(diǎn)到交點(diǎn)的長(zhǎng)度依次為ρ1，ρ2，…，然后，由P1點(diǎn)向OP2作垂線相交于P′1點(diǎn)，則φ1為直線L與P1P′1的夾角，同理可得φ2、φ3。由幾何關(guān)系可得

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

因?yàn)?/p>

當(dāng)Δθ很小時(shí)，則有

(1)

令第i點(diǎn)處的斜率ki為

(2)

所以

k3-k2≈k2-k1≈0

圖1 掃描點(diǎn)示意圖Fig.1 Scanning point diagram

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

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

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

Δk(i)=ki-ki-1

(3)

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

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

圖4 角點(diǎn)示意圖Fig.4 Angular point diagram

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

某次實(shí)驗(yàn)中激光雷達(dá)部分掃描點(diǎn)數(shù)據(jù)如圖6所示。按照式(3)計(jì)算的Δk分布情況如圖7所示，雖然由于掃描的誤差而導(dǎo)致Δk波動(dòng)，但在角點(diǎn)53處有明顯的峰值。

圖6 實(shí)驗(yàn)中部分掃描點(diǎn)數(shù)據(jù)Fig.6 Some scanning point data in the experiment

圖7 角點(diǎn)處Δk分布Fig.7 The distribution of Δk at the angular point

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)過(guò)的角度和返回的距離。

本算法分割斷點(diǎn)和角點(diǎn)的算法如下：

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

(4)

i=2,3,…,n-1

因?yàn)榧す饫走_(dá)具有環(huán)形掃描的特點(diǎn)，故可將第n點(diǎn)作為第1點(diǎn)的前一個(gè)掃描點(diǎn)，將第1點(diǎn)作為第n點(diǎn)的后一個(gè)掃描點(diǎn)，即

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

圖8 第一類(lèi)孤立點(diǎn)Fig.8 The first kind of isolated point

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

圖9 第二類(lèi)孤立點(diǎn)Fig.9 The second kind of isolated point

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

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

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

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

實(shí)驗(yàn)時(shí)以某樓端側(cè)梯形平臺(tái)為測(cè)試對(duì)象，該平臺(tái)幾何尺寸使用卷尺測(cè)量后如圖10所示。選用SLAMTEC公司的RPLIDAR A2型激光雷達(dá)，在測(cè)試平臺(tái)隨機(jī)選取10個(gè)點(diǎn)進(jìn)行掃描。算法在MATLAB平臺(tái)下實(shí)現(xiàn)。實(shí)驗(yàn)中各參數(shù)如下：θth=2.5°，kth=0.95，α=0.7。

圖10 實(shí)驗(yàn)測(cè)試平臺(tái)Fig.10 Experimental test platform

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

圖11 第1次實(shí)驗(yàn)結(jié)果Fig.11 The result of first experiment

為了進(jìn)一步驗(yàn)證本算法提取線段特征的準(zhǔn)確性，計(jì)算提取線段特征的長(zhǎng)度和相對(duì)角度，并與實(shí)際測(cè)量值進(jìn)行了對(duì)比。相對(duì)角度是在提取的特征線段中，以BA線段為基準(zhǔn)，其他線段與BA線段的角度差作為評(píng)判依據(jù)。由于激光雷達(dá)掃描的特點(diǎn)，刪除了部分因遮擋或距離限制而無(wú)法完整提取的線段特征。實(shí)驗(yàn)結(jié)果見(jiàn)表1和表2，線段編號(hào)與圖10相對(duì)應(yīng)。其中，最大差值百分比依據(jù)提取值和測(cè)量值差值的絕對(duì)值與測(cè)量值的百分比進(jìn)行計(jì)算。

表1 提取特征線段長(zhǎng)度與測(cè)量值對(duì)比

表2 提取特征線段角度與測(cè)量值對(duì)比

由表1和表2中數(shù)據(jù)可以發(fā)現(xiàn)，相對(duì)差值均在3%以?xún)?nèi)。在長(zhǎng)度方面，除第5次測(cè)量的差值稍大以外，其余9次測(cè)量的差值均小于2%。在角度方面，只有線段AN、NM的個(gè)別次測(cè)量相對(duì)差值超過(guò)了2%，而GF和FE兩條線段的差值均在1%以?xún)?nèi)。

4 結(jié)論

在提取特征點(diǎn)時(shí)，本文算法不需要迭代，減小了計(jì)算工作量，而且對(duì)閾值不敏感，也減小了計(jì)算的難度。實(shí)驗(yàn)結(jié)果表明，本文算法在掃描點(diǎn)分割方面可以獲得與手工分割相同的結(jié)果；與測(cè)量值對(duì)比，提取的特征線段也可獲得較高的準(zhǔn)確度。