基于激光雷達(dá)回波強(qiáng)度的反光板位置擬合方法

方德浩 許 萬 陳幼平 余磊濤 朱 力

1. 湖北工業(yè)大學(xué)機(jī)械工程學(xué)院，武漢，430212

2. 華中科技大學(xué)機(jī)械科學(xué)與工程學(xué)院，武漢，430074

0 引言

自動(dòng)導(dǎo)引車(automated guided vehicle, AGV)是企業(yè)物流系統(tǒng)中的重要設(shè)備。AGV系統(tǒng)中的定位即確定AGV在給定地圖環(huán)境中的全局坐標(biāo)系位姿。在解決該問題時(shí)[1]，2D激光雷達(dá)因其高精度而常作為主要傳感器。以激光雷達(dá)作為傳感器的定位方式主要可分為基于特征的地圖定位[2-3]、基于柵格的地圖定位[4-5]?；诜垂獍宓腁GV定位屬于特征地圖定位[6]。在以反光板為地圖特征的定位系統(tǒng)中，對(duì)反光板進(jìn)行位置擬合為該定位系統(tǒng)的重要環(huán)節(jié)，對(duì)定位的穩(wěn)定性、精度起到至關(guān)重要的作用。

反光板位置擬合主要分兩個(gè)步驟：①對(duì)原始掃描數(shù)據(jù)的分割；②對(duì)分割后點(diǎn)集的擬合。在對(duì)原始數(shù)據(jù)進(jìn)行分割的問題上，DIETMAYER等[7]提出的DIET算法通過設(shè)置兩點(diǎn)間距離的閾值來實(shí)現(xiàn)原始掃描數(shù)據(jù)點(diǎn)的分割；ZHOU等[8]利用改進(jìn)的基于點(diǎn)距離的分割(point-distance-based segmentation，PDBS) 法，使跨越數(shù)據(jù)頭與數(shù)據(jù)尾的環(huán)境特征在分割后保持為同一特征；PREMEBIDA等[9]提出的自適應(yīng)斷點(diǎn)檢測 (adaptive breakpoint detector，ABD)算法通過設(shè)置距離及角度的閾值進(jìn)行數(shù)據(jù)分割；梁雄等[10]采用改進(jìn)ABD算法對(duì)原ABD算法進(jìn)行二次檢測；BORGES 等[11]利用基于卡爾曼斷點(diǎn)檢測器進(jìn)行斷點(diǎn)檢測；ZHAO等[12]利用基于預(yù)測的特征提取 (prediction-based feature extraction，PFE)，通過預(yù)測的方式進(jìn)行數(shù)據(jù)分割；吳波[13]利用截?cái)嚅撝捣▽?duì)掃描數(shù)據(jù)進(jìn)行分割。對(duì)于分割后點(diǎn)集的擬合問題，劉之舟[14]利用回波強(qiáng)度重心法進(jìn)行位置擬合；祖爽等[15]利用平均值法進(jìn)行位置擬合；YUEN等[16]利用霍夫變換提取擬合位置；CHAUDHURI[17]利用最小二乘法 (least squares method，LSM)獲取擬合圓心位置；劉珂等[18]利用多約束下最小二乘法實(shí)現(xiàn)分割后點(diǎn)集位置的擬合。上述分割算法、擬合算法都能夠達(dá)到較好的效果，但分割算法在面對(duì)實(shí)際環(huán)境中產(chǎn)生的各種錯(cuò)誤數(shù)據(jù)時(shí)大多會(huì)產(chǎn)生失效擬合，且擬合精度較低，難以滿足定位算法對(duì)反光板位置擬合精度的需要。

隨著傳感器技術(shù)發(fā)展，激光雷達(dá)利用回波波形積分得到回波強(qiáng)度[19-20]，利用該值來輔助反光板進(jìn)行位置擬合可提升擬合效果。筆者提出一種基于回波強(qiáng)度的反光板位置擬合方法，以圓柱形反光板為地圖特征，首先對(duì)原始數(shù)據(jù)進(jìn)行濾波處理，結(jié)合高反光材料產(chǎn)生的較大回波強(qiáng)度[21]與反光板幾何特征進(jìn)行數(shù)據(jù)分割，并結(jié)合回波強(qiáng)度對(duì)分割后數(shù)據(jù)進(jìn)行反光板圓心擬合。

1 基本原理

1.1 基于反光板的激光定位系統(tǒng)

在基于反光板的激光雷達(dá)定位系統(tǒng)中，需要預(yù)先根據(jù)環(huán)境中布置的反光板搭建定位地圖。圖1所示為定位算法流程，其中，掃描數(shù)據(jù)分割及分割后數(shù)據(jù)擬合屬于反光板位置擬合，其主要作用是將原始掃描數(shù)據(jù)轉(zhuǎn)化為反光板極坐標(biāo)。反光板位置匹配是將反光板極坐標(biāo)與定位地圖中反光板數(shù)據(jù)相匹配[22-23]，最后通過卡爾曼濾波、粒子濾波等算法進(jìn)行激光雷達(dá)位姿計(jì)算[24-25]。

圖1 基于反光板的2D激光雷達(dá)定位流程

1.2 回波強(qiáng)度特性分析

激光雷達(dá)發(fā)射激光信號(hào)到目標(biāo)，經(jīng)目標(biāo)散射后再傳回。激光雷達(dá)根據(jù)飛行時(shí)間 (time of flight，TOF)測量該目標(biāo)的距離，同時(shí)根據(jù)接收信號(hào)計(jì)算回波強(qiáng)度?；夭◤?qiáng)度的計(jì)算公式為[19]

(1)

其中，Ei為激光雷達(dá)發(fā)射信號(hào)強(qiáng)度；ηsys為系統(tǒng)參數(shù)；ηatm為大氣影響因子；Dr為激光雷達(dá)接收孔徑；f(θi,φi,θs,φs)為二向性反射率分布函數(shù)(bidirectional reflectance distribution function，BRDF)；θi為激光入射角；φi為入射方位角；θs為激光出射角，激光雷達(dá)中θi=θs；φs為后向散射方位角；Rd為目標(biāo)與激光雷達(dá)系統(tǒng)之間的距離。實(shí)驗(yàn)中，實(shí)驗(yàn)材料系數(shù)

(2)

因式(2)等號(hào)右側(cè)的各個(gè)物理量保持不變，故式(1)可化簡為

(3)

(1)從實(shí)驗(yàn)數(shù)據(jù)中提取距激光雷達(dá)約8.3 m處反光板上的所有掃描點(diǎn)。因掃描點(diǎn)均勻分布于反光板表面，故可用序號(hào)代替入射角度，繪制掃描點(diǎn)回波強(qiáng)度曲線圖(圖2)。

圖2 反光板掃描點(diǎn)回波強(qiáng)度曲線

(2)從實(shí)驗(yàn)數(shù)據(jù)中提取不同反光板的點(diǎn)集，以各點(diǎn)集上最大回波強(qiáng)度點(diǎn)為垂直入射點(diǎn)(入射角θi=θs=0°)，繪制各垂直入射點(diǎn)的距離-回波強(qiáng)度曲線(圖3)。

圖3 垂直入射點(diǎn)的回波強(qiáng)度-距離曲線

同時(shí)，分別使用余弦函數(shù)、冪函數(shù)對(duì)實(shí)驗(yàn)(1)、(2)中的實(shí)際曲線進(jìn)行擬合，得到圖2、圖3中的擬合曲線，可以看出，掃描點(diǎn)回波強(qiáng)度變化符合式(3)的規(guī)律。

2 基于回波強(qiáng)度的反光板位置擬合

2.1 掃描數(shù)據(jù)分割

2.1.1掃描數(shù)據(jù)預(yù)處理

首先對(duì)激光雷達(dá)原始掃描數(shù)據(jù)進(jìn)行預(yù)處理，從激光雷達(dá)中獲取的原始掃描數(shù)據(jù)為

D′scan={P′i}i=1，2，…，7200

(4)

P′i=(ρ′i,θ′i,E′s,i)

(5)

其中，D′scan為包含N(N=7200)個(gè)掃描點(diǎn)的一幀原始掃描數(shù)據(jù)。掃描點(diǎn)P′i由掃描距離ρ′i、掃描角度θ′i和回波強(qiáng)度E′s,i表示。反光板位置擬合前，需要針對(duì)下列情況進(jìn)行濾波預(yù)處理：①若原始掃描數(shù)據(jù)中的某些孤立點(diǎn)與其相鄰點(diǎn)的距離較大，則這些點(diǎn)被認(rèn)為是測量噪聲點(diǎn)。②當(dāng)掃描點(diǎn)實(shí)際測量距離大于激光雷達(dá)最大測量距離時(shí)，激光雷達(dá)距離測量值設(shè)為錯(cuò)誤值-1，這些掃描點(diǎn)將不會(huì)被使用。

針對(duì)上述情況，設(shè)計(jì)如下預(yù)處理算法。

輸入: 原始掃描數(shù)據(jù)D′scan={P′i}

初始化：N=7200，最大允許突變量ρerror=0.3，k=1

fori=1 toNdo

ifP′i為唯一孤立點(diǎn)//P′i與周邊點(diǎn)的距離均大于ρerror

elseP′i為非唯一孤立點(diǎn)//P′i與相鄰點(diǎn)的距離小于ρerror即(|ρi-ρi-1|≤ρerror‖|ρi-ρi+1|≤ρerror)&&|ρi-ρi±1|≥ρerror

elseP′i為連續(xù)點(diǎn)//不做處理；

end for

輸出: 預(yù)處理完成后的掃描數(shù)據(jù)Dscan={Pi}

使用濾波算法對(duì)原始掃描數(shù)據(jù)進(jìn)行預(yù)處理前后的結(jié)果如圖4所示。預(yù)處理不僅舍棄了超過激光雷達(dá)使用距離的掃描點(diǎn)，且使受測量噪聲影響的部分掃描點(diǎn)得到修正。

2.1.2掃描數(shù)據(jù)分割算法

預(yù)處理后，需要對(duì)掃描數(shù)據(jù)進(jìn)行分割，以得到不同反光板對(duì)應(yīng)的點(diǎn)集。因周邊環(huán)境與反光板的k′存在較大差異，故可通過式(1)得到的回波強(qiáng)度區(qū)分反光板與周邊環(huán)境。由于測量時(shí)環(huán)境噪聲的影響，若僅采用回波強(qiáng)度進(jìn)行分割預(yù)處理，則掃描數(shù)據(jù)會(huì)錯(cuò)誤地將一個(gè)反光板點(diǎn)集分割為多個(gè)反光板點(diǎn)集，對(duì)后續(xù)算法產(chǎn)生較大影響。因此本文在將回波強(qiáng)度作為掃描數(shù)據(jù)分割依據(jù)的基礎(chǔ)上，把PDBS算法作為輔助分割判據(jù)，從而有效避免因錯(cuò)誤數(shù)據(jù)造成的擬合失效。

(a) 預(yù)處理前

首先針對(duì)不同距離下的反光板進(jìn)行掃描實(shí)驗(yàn)，選取反光板點(diǎn)集中的邊緣點(diǎn)作為該反光板的截?cái)嚅撝迭c(diǎn)，得到圖5。

圖5 邊緣點(diǎn)距離-回波強(qiáng)度曲線

從圖5可以看出，隨著距離增大，邊緣點(diǎn)回波強(qiáng)度變化較大，單一截?cái)嚅撝惦y以滿足要求，故需設(shè)置閾值截?cái)嗪瘮?shù)對(duì)各距離下的截?cái)嚅撝颠M(jìn)行選取。以回波強(qiáng)度為應(yīng)變量，距離作為自變量，使用MATLAB工具進(jìn)行擬合，得到回波強(qiáng)度擬合曲線：

(6)

式中，a、b為擬合函數(shù)參數(shù)。

將回波強(qiáng)度擬合曲線沿Y軸向下平移c個(gè)單位得到截?cái)嗪瘮?shù)曲線，其方程為

(7)

將掃描點(diǎn)的距離帶入上述截?cái)嗪瘮?shù)求得當(dāng)前距離的回波強(qiáng)度截?cái)嘀?，若掃描點(diǎn)回波強(qiáng)度大于該截?cái)嘀?，則表示該點(diǎn)為反光板點(diǎn)。當(dāng)受環(huán)境因素影響，導(dǎo)致部分掃描點(diǎn)回波強(qiáng)度小于該點(diǎn)截?cái)嘀禃r(shí)，反光板點(diǎn)集中會(huì)產(chǎn)生環(huán)境點(diǎn)，出現(xiàn)點(diǎn)集內(nèi)部分割，導(dǎo)致擬合失效。因此本文融合回波強(qiáng)度分割法與PDBS算法進(jìn)行數(shù)據(jù)分割，該融合分割算法偽代碼如下。

輸入: 預(yù)處理完成后的掃描數(shù)據(jù)Dscan={Pi}

初始化：N=7200，e=0，當(dāng)前待分割點(diǎn)集隊(duì)列Gm，當(dāng)前分割完成反光板隊(duì)列Gall

forj= 1 toNdo

if (Es,j

then Break;

end for

fori=jtoNdo

ifEs,i>Es(ρi)&&Gm≠?

thenPstart=Pi并且將Pi置于隊(duì)列Gm中，令e=0

elseEs,i>Es(ρi) &&D(Pi,Pstart)≤2R//D為點(diǎn)Pi、Pstart間的歐氏距離，R為反光板半徑

then 將Pi置于隊(duì)列Gm中，令e=0

else thene=e+1

ife≥3

then 將隊(duì)列Gm放入Gall中，并令i=0,Gm≠?;

end for

fori=1 tojdo 執(zhí)行上述算法的6～18行

輸出:當(dāng)前分割完成反光板隊(duì)列Gall={G1,G2,…,GN}

上述偽代碼1～5行的主要作用是使一幀數(shù)據(jù)以小回波強(qiáng)度點(diǎn)為起始點(diǎn)，從而保證跨越數(shù)據(jù)頭與數(shù)據(jù)尾的特征在分割后仍為同一特征。融合分割算法的主要目的是將屬于反光板的掃描數(shù)據(jù)點(diǎn)提取出來，并分割成多組數(shù)據(jù)集Gall={G1,G2, …,GN}，其中，Gm={P1,P2, …,Pi} (m=1，2，…，N)為屬于某一反光板的掃描點(diǎn)集合。

2.2 圓柱形反光板擬合

圖6 激光雷達(dá)掃描點(diǎn)分布示意圖

執(zhí)行融合分割算法后，需要對(duì)Gall中的點(diǎn)集Gm進(jìn)行擬合計(jì)算。圖6為激光雷達(dá)掃描圓柱形反光板的示意圖，其中，O1O為實(shí)際距離，Pi為掃描點(diǎn)。通常，Gm中的掃描點(diǎn)在圓柱形反光板上呈現(xiàn)沿O1O的非對(duì)稱分布。這會(huì)導(dǎo)致在不同觀測位姿時(shí)，反光板擬合位置發(fā)生變化，AGV動(dòng)態(tài)運(yùn)行狀態(tài)下的擬合精度難以保證。

圖7 激光雷達(dá)實(shí)際掃描點(diǎn)分布圖

為避免上述情況，根據(jù)圖2中反光板內(nèi)部點(diǎn)回波強(qiáng)度變化曲線，本文提出一種結(jié)合回波強(qiáng)度的擬合算法。以回波強(qiáng)度為權(quán)值，對(duì)反光板點(diǎn)集Gm中各點(diǎn)的角度進(jìn)行加權(quán)來減小因掃描點(diǎn)Pi在圓柱形反光板上分布不均而造成的擬合誤差。設(shè)Pf為反光板擬合位置，ρf為擬合距離(極徑)，θf為擬合角度(極角)，則有Pf=(ρf,θf)。

3 實(shí)驗(yàn)與分析

3.1 實(shí)驗(yàn)平臺(tái)

本實(shí)驗(yàn)中激光雷達(dá)為倍加福公司生產(chǎn)的R2000HD，其有效輸出距離為0.1～30 m，輸出回波強(qiáng)度為32～4095；實(shí)驗(yàn)中使用的反光板材料為3M公司生產(chǎn)的V級(jí)白色鉆石級(jí)反光膜。該反光材料為全棱鏡結(jié)構(gòu)，理論反射率達(dá)100%；移動(dòng)平臺(tái)使用的是江淮重工三自由度堆垛式叉車。

實(shí)驗(yàn)環(huán)境為AGV工廠測試環(huán)境，實(shí)驗(yàn)環(huán)境四周墻壁為彩鋼瓦，在墻壁上安裝半徑55 mm的圓形反光板，實(shí)驗(yàn)地面為平坦水泥地面。實(shí)驗(yàn)場景、叉車尺寸以及激光雷達(dá)安裝位置如圖8所示。

(a) 實(shí)驗(yàn)場景

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

本文設(shè)計(jì)的反光板位置擬合方法主要用于AGV激光定位系統(tǒng)，作為激光定位的重要步驟之一，該方法的擬合精度及數(shù)據(jù)容錯(cuò)能力顯得尤為重要。為了驗(yàn)證本文算法的數(shù)據(jù)容錯(cuò)能力及擬合精度，設(shè)計(jì)了以下實(shí)驗(yàn)。

3.2.1數(shù)據(jù)容錯(cuò)能力實(shí)驗(yàn)

表1所示的原始掃描數(shù)據(jù)包含錯(cuò)誤的數(shù)據(jù)幀，第120號(hào)數(shù)據(jù)的回波強(qiáng)度顯著小于其周圍掃描點(diǎn)回波強(qiáng)度，疑似為錯(cuò)誤數(shù)據(jù)。

為驗(yàn)證本文融合分割算法的數(shù)據(jù)容錯(cuò)能力，使用基于回波強(qiáng)度的分割法與本文融合分割算法對(duì)表1中的數(shù)據(jù)進(jìn)行分割處理，得到圖9、圖10，其中不同顏色的點(diǎn)來自不同的擬合點(diǎn)集。

表1 錯(cuò)誤數(shù)據(jù)幀部分信息

圖9 基于回波強(qiáng)度的分割法效果圖

圖10 融合分割法效果圖

對(duì)比圖9、圖10可知，使用基于回波強(qiáng)度的分割法會(huì)在錯(cuò)誤數(shù)據(jù)(120號(hào)數(shù)據(jù))處將表1中的數(shù)據(jù)分割為兩組點(diǎn)集Gm、Gm+1，因點(diǎn)集數(shù)量等于后續(xù)將擬合的反光板數(shù)量，故該方法將會(huì)在這個(gè)位置的一個(gè)反光板擬合“兩個(gè)”反光板，對(duì)后續(xù)算法產(chǎn)生巨大影響。融合分割算法加入PDBS分割判據(jù)，保證了待分割點(diǎn)集Gm前后的連續(xù)性和該位置反光板的唯一性，提高了數(shù)據(jù)容錯(cuò)能力。圖11所示為其余錯(cuò)誤數(shù)據(jù)幀的擬合情況。

(a) 錯(cuò)誤數(shù)據(jù)幀一

3.2.2靜態(tài)擬合精度實(shí)驗(yàn)

為驗(yàn)證本文擬合算法的靜態(tài)精度，在采集的原始掃描數(shù)據(jù)中，取距激光雷達(dá)8389.1 mm的反光板掃描數(shù)據(jù)為樣本數(shù)據(jù)。分別使用霍夫變換法、最小二乘法(使用ceres庫求解)、本文算法進(jìn)行擬合計(jì)算，結(jié)果如圖12、圖13所示。表2所示為不同算法的距離標(biāo)準(zhǔn)差σρ、角度標(biāo)準(zhǔn)差σθ、距離極差Rρ、角度極差Rθ。由圖12、圖13以及表2可看出，本文算法擬合精度高、波動(dòng)幅度小、穩(wěn)定性好。

圖12 擬合算法距離誤差對(duì)比圖

圖13 擬合算法角度誤差對(duì)比圖

表2 擬合算法精度對(duì)比表

為獲得各距離下本文算法的靜態(tài)擬合精度，在采集的原始掃描數(shù)據(jù)中選取不同距離的反光板進(jìn)行多組重復(fù)試驗(yàn)，記錄其距離和角度的誤差，如圖14、圖15所示。選用距離誤差、角度誤差的標(biāo)準(zhǔn)差為精度標(biāo)準(zhǔn)，表3為多組不同距離反光板的擬合精度。從圖14、圖15和表3可以看出，隨距離的增大，擬合精度并沒有明顯下降，本文方法具有波動(dòng)幅度小、穩(wěn)定性好等特性。

表3 不同距離下的擬合精度

3.2.3動(dòng)態(tài)擬合精度實(shí)驗(yàn)

AGV通常處于動(dòng)態(tài)運(yùn)行狀態(tài)，為驗(yàn)證本文算法在動(dòng)態(tài)運(yùn)行下的擬合精度，使AGV從(0,0)點(diǎn)沿x軸正向117.4392°行駛8.9 m，車速為AGV正常行駛車速0.5 m/s，環(huán)境中兩反光板直角坐標(biāo)分別為(-1.5818 m,0.5524 m)、(-0.6693 m,8.7974 m)，通過兩反光板的擬合數(shù)據(jù)推算得到反光板的間距(以此來表示動(dòng)態(tài)擬合精度)。繪制間距隨掃描幀數(shù)變化的曲線，如圖16所示，其中，兩反光板的實(shí)際間距為9.39 m。

圖14 不同距離反光板角度擬合誤差圖

圖15 不同距離反光板距離擬合誤差圖

圖16 兩反光板間距離曲線

從350幀實(shí)驗(yàn)數(shù)據(jù)可得兩反光板之間距離誤差的標(biāo)準(zhǔn)差為2.9 mm，極差為15.1 mm。動(dòng)態(tài)擬合精度相對(duì)于靜態(tài)擬合精度并未明顯降低，觀測位姿變化造成的實(shí)際擬合位置變化較小，故本文算法在動(dòng)態(tài)運(yùn)行環(huán)境下也具有較高的擬合精度。

4 結(jié)論

不同算法對(duì)相同錯(cuò)誤數(shù)據(jù)處理效果的對(duì)比證明了本文算法具有一定的容錯(cuò)能力；使用不同算法分別對(duì)原始掃描數(shù)據(jù)進(jìn)行擬合，證明了本文算法較高的擬合精度；對(duì)動(dòng)態(tài)運(yùn)行下的擬合精度統(tǒng)計(jì)，證明了本算法可以在AGV各種運(yùn)行狀態(tài)下均保持較高的擬合精度。綜上所述，本文提出的算法可以滿足基于反光板的激光雷達(dá)定位系統(tǒng)中對(duì)反光板位置擬合要求，具有較高的實(shí)用性。