文偉松， 譚 彧， 朱 凱， 梁盛好

(中國農(nóng)業(yè)大學(xué) 工學(xué)院，北京 100083)

基于激光掃描的移動(dòng)機(jī)器人行走環(huán)境直線檢測*

文偉松， 譚 彧， 朱 凱， 梁盛好

(中國農(nóng)業(yè)大學(xué) 工學(xué)院，北京 100083)

提出一種移動(dòng)機(jī)器人行走環(huán)境直線檢測算法；對激光傳感器采集的環(huán)境信息作預(yù)處理，設(shè)計(jì)分割、提取規(guī)則將離散距離信息轉(zhuǎn)化為具備明顯特征的直線段序列，考慮傳感器誤差用最小二乘法擬合直線段；計(jì)算擬合誤差作為直線分割提取的閾值自動(dòng)調(diào)整條件，實(shí)現(xiàn)閾值自動(dòng)調(diào)整。基于極坐標(biāo)內(nèi)直線協(xié)方差矩陣計(jì)算Mahalanobis距離，實(shí)現(xiàn)直線合并，不丟失環(huán)境信息同時(shí)降低其直線存儲量。與傳統(tǒng)的Split-and-Merge算法相比，解決了對分割閾值參數(shù)敏感的問題。經(jīng)過實(shí)驗(yàn)證明:直線檢測算法能夠有效檢測出直線特征。

移動(dòng)機(jī)器人; 直線檢測; 閾值

0 引 言

移動(dòng)機(jī)器人自主導(dǎo)航已成為智能機(jī)器人領(lǐng)域研究熱點(diǎn)，移動(dòng)機(jī)器人的智能作業(yè)顯著提高農(nóng)業(yè)水平；農(nóng)業(yè)機(jī)器人智能導(dǎo)航中環(huán)境特征檢測是當(dāng)前的研究重點(diǎn)，普通工作環(huán)境一般可通過直線段描述，直線段具有理解簡易，存儲容易的特點(diǎn)；當(dāng)前的機(jī)器人導(dǎo)航技術(shù)主要有視覺導(dǎo)航、激光導(dǎo)航、GPS加慣性導(dǎo)航等；其中激光導(dǎo)航通過檢測環(huán)境直線，基于卡爾曼濾波等方法實(shí)現(xiàn)機(jī)器人實(shí)時(shí)自定位和地圖創(chuàng)建(SLAM)。SLAM技術(shù)的出現(xiàn)意味著機(jī)器人智能導(dǎo)航進(jìn)入新階段[1～4]，通過激光測距等傳感器持續(xù)獲取環(huán)境特征并關(guān)聯(lián)數(shù)據(jù)，得到環(huán)境的實(shí)時(shí)地圖并實(shí)現(xiàn)實(shí)時(shí)導(dǎo)航，直線檢測在農(nóng)業(yè)機(jī)器人智能導(dǎo)航中尤為重要。文獻(xiàn)[5]中論述了幾種提取直線的算法，其中，IEPF算法和Split-and-Merge[6]算法在提取激光傳感器數(shù)據(jù)中直線特征中性能好，但對于環(huán)境特征原始模型過多時(shí)，算法空間復(fù)雜度o(f(m))快速上升。其次，IEPF算法在提取直線過程中，對閾值敏感，單一固定的閾值可能導(dǎo)致提取誤差超出合理值。其中，Incremental算法簡單快速，但對算法內(nèi)部參數(shù)值敏感；Hough變換提取直線最初用于圖像處理，優(yōu)勢是算法穩(wěn)定但對提取復(fù)雜環(huán)境直線特征效率低；Line regression算法在文獻(xiàn)[7]中被實(shí)驗(yàn)驗(yàn)證，缺點(diǎn)是算法復(fù)雜；隨機(jī)抽樣一致性(RANSAC)算法通用性強(qiáng)，可同時(shí)用于提取圓弧，直線等特征，可以在存在非直線內(nèi)點(diǎn)情況下做粗略的特征擬合，易于實(shí)現(xiàn)但環(huán)境特征提取精度低；最大期望(EM)算法在視覺圖像中直線特征提取中廣泛應(yīng)用[8,9]，缺點(diǎn)是算法的初始參數(shù)不易獲得。為了降低對環(huán)境特征的存儲量，有多種方法計(jì)算直線參數(shù)之間的統(tǒng)計(jì)學(xué)距離作為直線合并的判斷條件[10,11]，以實(shí)現(xiàn)直線合并，以快速實(shí)現(xiàn)對環(huán)境特征的感知與理解；有效的直線檢測算法在機(jī)器人自主規(guī)劃實(shí)時(shí)性方面具有重要作用[12,13]。

本文以移動(dòng)機(jī)器人為研究對象，以Robopeak公司的Rplidar激光傳感器獲取機(jī)器人行走環(huán)境信息，考慮以上論述的算法局限性，研究一種直線檢測算法，自動(dòng)調(diào)整直線分割、提取的閾值，實(shí)現(xiàn)移動(dòng)機(jī)器人行走環(huán)境直線檢測。

1 直線描述與直線檢測算法

激光傳感器讀取環(huán)境信息得到環(huán)境點(diǎn)云地圖W={(ρj,θj)|j=1,…,N)},以激光傳感器中心為原點(diǎn)坐標(biāo)，其中,(ρj,θj)為對應(yīng)坐標(biāo)系內(nèi)環(huán)境點(diǎn)，直線特征描述如下[14]

(1)

式中 ρj為環(huán)境點(diǎn)距離激光原點(diǎn)距離,θj為測距點(diǎn)相對于激光朝向夾角, -π≤αj≤π,其中環(huán)境數(shù)據(jù)預(yù)處理、分割和提取過程如圖1。

圖1 激光數(shù)據(jù)處理過程

2)最小二乘擬合區(qū)域包A中每個(gè)區(qū)域，求取直線擬合誤差，若某條直線擬合誤差超過閾值EκMax，則重新設(shè)置搜索閾值ΔκA，重復(fù)步驟⑴，直到滿足直線擬合誤差要求。擬合完成后得到直線特征S{T,S1,S2,…,ST}，其中Sl為環(huán)境直線的五維特征向量sl(θl,θsl,θel,dl,ll,κl,bl)，其中直線法向角θl、直線起始點(diǎn)角度θsl、終點(diǎn)角度θel、激光原點(diǎn)到直線垂線長度dl、直線長度ll、直線斜率κl和直線截距bl。

3)合并直線:提取完直線后對端點(diǎn)相距較近且平行的直線合并成一條直線，不丟失主要環(huán)境信息的前提下減小了環(huán)境特征存儲量。

1.1 直線檢測

本文中研究的算法根據(jù)環(huán)境距離數(shù)據(jù)自動(dòng)調(diào)整閾值，克服IEPF算法等閾值參數(shù)值敏感的問題，并對直線段序列做合并處理減小直線存儲量；環(huán)境特征提取速度直接影響機(jī)器人導(dǎo)航實(shí)時(shí)性，在不丟失環(huán)境主要信息同時(shí)減小環(huán)境特征數(shù)量能有效提高導(dǎo)航算法速度。環(huán)境直線提取如圖2，以激光內(nèi)部坐標(biāo)原點(diǎn)作為圖2中坐標(biāo)系原點(diǎn)，X，Y坐標(biāo)則確定了每一個(gè)環(huán)境點(diǎn)到激光原點(diǎn)幾何距離。

算法實(shí)現(xiàn)包括環(huán)境數(shù)據(jù)點(diǎn)預(yù)處理、直線提取、直線擬合和直線合并四部分。首先算法在逆時(shí)針讀取原始距離的同時(shí)，按照連續(xù)直線模式，線段確認(rèn)模式、直線膨脹模式、轉(zhuǎn)折點(diǎn)處理模式和分割點(diǎn)處理模式四種模式處理環(huán)境點(diǎn)與激光原點(diǎn)距離值Ce(e=1,…,n)。當(dāng)兩個(gè)相鄰環(huán)境點(diǎn)e和e+1之間幾何距離超過閾值 ，則標(biāo)記e點(diǎn)為分割點(diǎn)。

1.1.1 數(shù)據(jù)點(diǎn)處理

從環(huán)境獲取得到距離信息數(shù)據(jù)包{N,L1,L2,…，LN}，激光傳感器采集環(huán)境點(diǎn)及距離表示如圖2所示，設(shè)環(huán)境點(diǎn)Lj與Lk之間距離djk。從第一個(gè)點(diǎn)L1開始計(jì)算與下一個(gè)相鄰點(diǎn)距離,當(dāng)兩點(diǎn)之間距離小于ΔκA時(shí)，將點(diǎn)1存入數(shù)據(jù)包A1，依次向前計(jì)算相鄰點(diǎn)之間距離，當(dāng)環(huán)境點(diǎn)Lj與Lk之間距離djk。從第一個(gè)點(diǎn)L1開始計(jì)算與下一個(gè)相鄰點(diǎn)距離,當(dāng)兩點(diǎn)之間距離小于ΔκA時(shí)，將點(diǎn)1存入數(shù)據(jù)包A1，依次向前計(jì)算相鄰點(diǎn)之間距離，當(dāng)環(huán)境點(diǎn)Lj與Lk之間距離djk>ΔκA時(shí)，將Lk存入一個(gè)新的數(shù)據(jù)包Ak，直到遍歷結(jié)束所有環(huán)境點(diǎn)，按此方法將圖2中所有環(huán)境點(diǎn)分成兩個(gè)數(shù)據(jù)包A1,A2。其中Lx與Ln兩點(diǎn)為數(shù)據(jù)包A1，A2分界點(diǎn)，Lx與Ln之間距離dxn>ΔκA。對激光采集的原始數(shù)據(jù)預(yù)處理之后，數(shù)據(jù)包內(nèi)數(shù)據(jù)為連續(xù)直線上點(diǎn)，可能包含具有轉(zhuǎn)折點(diǎn)的折線。

圖2 激光傳感器采集環(huán)境距離數(shù)據(jù)模型

1.1.2 直線提取

環(huán)境數(shù)據(jù)點(diǎn)預(yù)處理后，對數(shù)據(jù)包內(nèi)環(huán)境點(diǎn)數(shù)據(jù)進(jìn)一步處理，依次從各個(gè)數(shù)據(jù)包提取直線;設(shè)定轉(zhuǎn)折點(diǎn)判斷閾值δline。提取規(guī)則如下：

1.1.3 直線段擬合

直線提取結(jié)束后得到子數(shù)據(jù)包子Aend{(A11,A12,…)(A21,A22,…)(Ak1,Ak2,…)}，每一個(gè)子數(shù)據(jù)包對應(yīng)一條直線。通過整體最小二乘擬合和計(jì)算直線相關(guān)的參數(shù)得到直線的五維特征Sl(θl,θsl,θel,dl,ll,κl,bl)，在擬合過程中轉(zhuǎn)換成卡迪爾坐標(biāo)系內(nèi)坐標(biāo)做運(yùn)算；區(qū)域包內(nèi)每個(gè)擬合點(diǎn)擬合誤差vyl如下

(2)

得到誤差方程矩陣

AδX=H+V

(3)

其中

1.1.4 直線合并

由于傳感器的偶然誤差，在采集環(huán)境點(diǎn)過程中可能丟失部分點(diǎn)，導(dǎo)致實(shí)際上連續(xù)的環(huán)境輪廓在分割結(jié)束后得到多條鄰近的不同直線。故提出，在擬合結(jié)束后，對于接近平行且相距較近的直線則對其做合并處理，對直線做合并處理可在不丟失主要環(huán)境信息的條件下，減少環(huán)境特征的存儲量。通過計(jì)算直線參數(shù)(ρi,θi)協(xié)方差矩陣cov(ρ,θ)，基于統(tǒng)計(jì)學(xué)方法計(jì)算每條直線之間馬氏距離，若任何兩條直線間馬氏距離小于給定閾值δMah,則合并兩條直線，并計(jì)算得到新直線及五維特征向量Sl。直線協(xié)方差矩陣cov(ρ,θ)

(4)

由前面提取得直線及直線上的點(diǎn)，可得直線協(xié)方差矩陣cov(ρ,θ)各個(gè)值

(5)

(6)

(7)

其中

(8)

(9)

(10)

(11)

式中wi,wj為環(huán)境距離數(shù)據(jù)點(diǎn)權(quán)值，實(shí)驗(yàn)用激光每次采樣輸出環(huán)境點(diǎn)距離數(shù)據(jù)時(shí)也輸出采樣點(diǎn)信號質(zhì)量qc，該參數(shù)由激光輸出數(shù)據(jù)中解析得到；取wi=qc,wj=qc;(ρi,θi)為環(huán)境點(diǎn)距離及角度，(r,α)為直線參數(shù)，其中,ρ,θ可認(rèn)為是兩個(gè)統(tǒng)計(jì)學(xué)上相互獨(dú)立的兩個(gè)變量。

馬氏距離是一種有效計(jì)算兩個(gè)未知樣本集的相似度的方法。與歐氏距離不同的是,它考慮到各種特性之間的聯(lián)系，針對前面求取的直線參數(shù)協(xié)方差矩陣M。提取后的直線可看成是關(guān)于(ρi,θi)的樣本，假設(shè)兩個(gè)直線L1和L2，則兩直線樣本之間馬氏距離D(L1,L2)

(12)

計(jì)算直線樣本之間馬氏距離，若樣本點(diǎn)Li和Lj馬氏距離D(Li,Lj)<δMah,則合并兩個(gè)直線，得到新直線，在得到直線在極坐標(biāo)表示后，同時(shí)為了直線的直觀理解，將直線參數(shù)轉(zhuǎn)換成直角坐標(biāo)表示。

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

2.1 實(shí)驗(yàn)結(jié)果

本實(shí)驗(yàn)以Rplidar激光傳感器在實(shí)驗(yàn)室走廊做實(shí)驗(yàn)，檢測走廊拐角及一個(gè)障礙的輪廓直線。Rplidar傳感器每幀得到360個(gè)數(shù)據(jù)，激光采集的數(shù)據(jù)通過無線通信發(fā)送到PC實(shí)現(xiàn)文中直線檢測算法。圖3(a)中包含了激光傳感器直接采集的環(huán)境距離點(diǎn)，原點(diǎn)為激光傳感器中心位置，網(wǎng)格分度值900 mm；L1,L2,L3,L4,L5為檢測的環(huán)境直線輪廓，其中L3和L4之間連接處對應(yīng)圖(b)中一個(gè)墻角拐點(diǎn)，L5和L6之間連接處對應(yīng)圖(b)中另一個(gè)墻角拐點(diǎn)，由于激光傳感器量程限制，只提取量程范圍內(nèi)環(huán)境數(shù)據(jù)點(diǎn)。表1中分析了6條直線擬合點(diǎn)平均誤差，由擬合總誤差除以直線內(nèi)數(shù)據(jù)點(diǎn)個(gè)數(shù)。

圖3 直線檢測試驗(yàn)結(jié)果

線段序號包含點(diǎn)數(shù)Pi直線斜率k直線截距b擬合點(diǎn)平均誤差SM=Ek/Pi/mmL189-1.650-37501.40L2190.08012.020.44L39-1.6301636.01.99L4250.6101454.61.56L5290.5601927.21.47L69-1.6401646.13.05

2.2 實(shí)驗(yàn)分析

結(jié)合圖3(b)，采用文中研究的移動(dòng)機(jī)器人行走環(huán)境直線檢測算法檢測環(huán)境直線，包括直線L1,L2,L3,L4,L5，L6，各條直線相關(guān)參數(shù)如表1。其中,直線L2擬合平均誤差最小，環(huán)境點(diǎn)最大擬合誤差均在5 mm范圍內(nèi)。且對墻角拐彎處特征提取有效；同時(shí)采用直線合并在不丟失主要環(huán)境信息條件下降低直線段存儲量。誤差分析方面，從表1求得直線參數(shù)表可知，直線擬合誤差均較小，也表明擬合程度高。環(huán)境數(shù)據(jù)點(diǎn)在卡迪爾坐標(biāo)系內(nèi)擬合如圖4，直觀得到直線L1點(diǎn)數(shù)最多，驗(yàn)證了文中研究的直線檢測算法的有效性。

圖4 數(shù)據(jù)點(diǎn)擬合結(jié)果

3 結(jié) 論

1)通過激光傳感器采集環(huán)境距離數(shù)據(jù)，采用文中提出的直線檢測算法能有效檢測出直線，如圖3中近距離的直線L3,L4和遠(yuǎn)距離的直線L1，通過自動(dòng)調(diào)整閾值的方法能有效消除環(huán)境直線欠分割問題。

2)文中算法求出了環(huán)境特征直線在激光傳感器坐標(biāo)系內(nèi)幾何參數(shù)，為移動(dòng)機(jī)器人在導(dǎo)航提供了環(huán)境特征，同時(shí)為移動(dòng)機(jī)器人智能導(dǎo)航中姿態(tài)求解建提供理論基礎(chǔ)。

[1]BaileyT,Durrant-WhyteH.Simultaneouslocalizationandmapping(SLAM):PartII[J].Robotics&AutomationMagazine,IEEE,2006,13(3):108-117.

[2]DissanayakeMWMG,NewmanP,ClarkS,etal.Asolutiontothesimultaneouslocalizationandmapbuilding(SLAM)proble-m[J].IEEETransactionsonRobotics&Automation,2001,17(3):229-241.

[3]BaileyT,NietoJ,NebotE.ConsistencyoftheFastSLAMalgorithm[C]∥IEEEInternationalConferenceonRobotics&Automation,2006:424-429.

[4]NguyenV,MartinelliA,TomatisN,etal.Acomparisonoflineextractionalgorithmsusing2Dlaserrangefinderforindoormobilerobotics[C]∥ConferenceonintelligentRobotsandSystems(IROS),2005.

[5]BorgesGA,AldonMJ.Asplit-and-mergesegmentationalgorithmforlineextractionin2Drangeimages[C]∥The15thInternationalConferenceonPattenRecognition,2000.

[6]SiadatA.Anoptimizedsegmentationmethodfora2Dlaser-scannerappliedtomobilerobotnavigation[C]∥Proceedingsofthe3rdIFACSymposiumonIntelligentComponentsandInstrumentsforControlApplications,1997.

[7]ArrasKO,SiegwartR.Featureextractionandsceneinterpretationformap-basednavigationandmapbuilding[C]∥ProceedingsoftheSymposiumonIntelligentSystemsandAdvancedManufac-turing,1997.

[8]BorgesGA,AldonMJ.Lineextractionin2Drangeimagesformobilerobotics[J].JournalofIntelligentandRoboticsSystems,2004,40:267-297.

[9] 丁克良,沈云中,歐吉坤.整體最小二乘法直線擬合[J].遼寧工程技術(shù)大學(xué)學(xué)報(bào):自然科學(xué)版,2010(1):44-47.

[10] 嚴(yán)春美,呂曉榮,許云紅.移動(dòng)醫(yī)療服務(wù)技術(shù)研究進(jìn)展與發(fā)展前景[J].傳感器與微系統(tǒng),2013,32(2):1-3,7.

[11] 鮑慶勇,李舜酩,沈 峘,等.自主移動(dòng)機(jī)器人局部路徑規(guī)劃綜述[J].傳感器與微系統(tǒng),2009,28(9):1-4，11.

[12] 趙 鵬.子母式機(jī)器人系統(tǒng)設(shè)計(jì)與協(xié)調(diào)控制研究[D].北京：中科院自動(dòng)化研究所，2015:41-47.

譚 彧，女，通訊作者，教授，主要從事機(jī)器人技術(shù)、嵌入式技術(shù)的研究工作,E—mail:tanyu@cau.edu.cn。

Straight line detection of walking environment of mobile robot based on laser scanning*

WEN Wei-song, TAN Yu, ZHU Kai, LIANG Sheng-hao

(College of Engineering,China Agricultural University,Beijing 100083,China)

A walking environment line detection algorithm for mobile robot is proposed;preprocessing on environment information is performed,segmentation and extraction regulation to transform discrete distance information into line segments with distinct feature is designed.Fit straight line segments by least square method considering sensor errors.Compute fitting error as criteria of threshold automatic adjustment.Computing Mahalanobis distance between lines in polar coordinates,achieve line merging to reduce storage contents of line features under the circumstance of retaining all the environment information.Comparing to the Split-and-Merge algorithm,solve the problem of sensitivity of threshold parameters.It is proved by experiments that the proposed algorithm can effectively detects line features of environment.

mobile robot; straight line detection; threshold

10.13873/J.1000—9787(2017)03—0120—04

2016—04—21

國家科技支撐計(jì)劃項(xiàng)目(2014BAD08B10—0102)

TP 242

A

1000—9787(2017)03—0120—04

文偉松(1994-)，男，碩士研究生，主要研究方向?yàn)闇y控技術(shù)、嵌入式技術(shù)。