基于激光雷達回波強度的反光板位置擬合方法

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

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

2. 華中科技大學機械科學與工程學院，武漢，430074

0 引言

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

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

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

1 基本原理

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

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

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

1.2 回波強度特性分析

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

(1)

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

(2)

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

(3)

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

圖2 反光板掃描點回波強度曲線

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

圖3 垂直入射點的回波強度-距離曲線

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

2 基于回波強度的反光板位置擬合

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

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

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

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

(4)

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

(5)

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

針對上述情況，設計如下預處理算法。

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

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

fori=1 toNdo

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

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

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

end for

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

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

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

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

(a) 預處理前

首先針對不同距離下的反光板進行掃描實驗，選取反光板點集中的邊緣點作為該反光板的截斷閾值點，得到圖5。

圖5 邊緣點距離-回波強度曲線

從圖5可以看出，隨著距離增大，邊緣點回波強度變化較大，單一截斷閾值難以滿足要求，故需設置閾值截斷函數(shù)對各距離下的截斷閾值進行選取。以回波強度為應變量，距離作為自變量，使用MATLAB工具進行擬合，得到回波強度擬合曲線：

(6)

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

將回波強度擬合曲線沿Y軸向下平移c個單位得到截斷函數(shù)曲線，其方程為

(7)

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

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

初始化：N=7200，e=0，當前待分割點集隊列Gm，當前分割完成反光板隊列Gall

forj= 1 toNdo

if (Es,j

then Break;

end for

fori=jtoNdo

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

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

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

then 將Pi置于隊列Gm中，令e=0

else thene=e+1

ife≥3

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

end for

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

輸出:當前分割完成反光板隊列Gall={G1,G2,…,GN}

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

2.2 圓柱形反光板擬合

圖6 激光雷達掃描點分布示意圖

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

圖7 激光雷達實際掃描點分布圖

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

3 實驗與分析

3.1 實驗平臺

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

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

(a) 實驗場景

3.2 實驗與結果分析

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

3.2.1數(shù)據(jù)容錯能力實驗

表1所示的原始掃描數(shù)據(jù)包含錯誤的數(shù)據(jù)幀，第120號數(shù)據(jù)的回波強度顯著小于其周圍掃描點回波強度，疑似為錯誤數(shù)據(jù)。

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

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

圖9 基于回波強度的分割法效果圖

圖10 融合分割法效果圖

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

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

3.2.2靜態(tài)擬合精度實驗

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

圖12 擬合算法距離誤差對比圖

圖13 擬合算法角度誤差對比圖

表2 擬合算法精度對比表

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

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

3.2.3動態(tài)擬合精度實驗

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

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

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

圖16 兩反光板間距離曲線

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

4 結論

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