三角測距激光雷達的光斑定位算法研究*

李 高，何元烈，曾 碧

（廣東工業大學計算機學院，廣州510000）

三角測距激光雷達的光斑定位算法研究*

李 高，何元烈*，曾 碧

（廣東工業大學計算機學院，廣州510000）

為了降低激光雷達測距系統的光斑信號噪聲，提高系統的測距精度，對屬性距離加權平均濾波及灰度質心法進行改進，提出一種應用于激光雷達系統的改進算法。以曲率為屬性的屬性距離加權平均濾波在去噪的同時很好地突出了光斑的峰值點。通過多次迭代計算灰度質心的求質心方法，進一步地提高光斑中心的定位精度，還解決了一般平方加權質心法容易受到噪聲影響的問題。在MATLAB仿真環境中對實際采集的數據進行處理，實驗表明本文算法對于4 μs的像素單元可以取得0.05 pix的定位精度，并具有較好的穩定性。

機器人傳感器；二維激光雷達；亞像素定位；屬性距離加權平均濾波；平方加權質心法

激光雷達LIDAR（Laser Radar）［1］是基于激光的距離測量系統，包括單點的測距傳感器、在平面上進行線掃描的二維激光雷達［2］、在空間上進行面掃描的三維激光雷達［3］。作為移動機器人的關鍵組成部分之一，激光雷達在移動機器人中的應用主要有3個方面，即障礙物檢測、路標檢測及地圖匹配、探索行駛時建立地形圖。由于激光雷達能快速提供環境的信息，移動機器人的發展在很大程度上得益于激光雷達的研發。盡管激光雷達的研制已進行多年并逐步實現了商業化，并在一些領域得到了應用，但仍有許多方面需要改進的。

目前，很多機器人都采用三維激光雷達［4］，而且也取得了較好的效果，但是三維激光雷達價格昂貴，體積大而笨重，尤其是成像速度慢的弱點，在很大程度上制約了它的進一步應用。同三維激光雷達相比，二維激光雷達具有測距速度快、系統穩定可靠的特點。所以二維激光雷達室內的移動機器人應用中有明顯的優勢，因為在室內的結構化環境下，地面平坦，所有障礙物又都幾乎與地面垂直，利用二維激光雷達機器人能更迅速地在平行地面上獲取環境導航信息。

國內對于激光雷達的研究相對較少，導致國內許多關于機器人自主導航算法的研究不得不依賴國外的進口傳感器。當前較為成熟的二維激光雷達產品有日本HOKUYO所生產的PBS-03JN、UAM-05LP-T301等，然而這些產品普遍采用的是相位式的測距方式，硬件成本都比較昂貴。本文所述的激光雷達系統基于激光三角測距原理，研究了在快速旋轉運動情況的激光光斑的穩定性，受噪聲影響的問題，及光斑亞像素的定位算法。使二維激光雷達系統達到了3 m量程和1 mm的精度應用需求條件下，在設計成本方面仍然可以得到控制。

激光三角測距系統［5］即激光光源、被測物面、光接收系統三點共同構成一個三角形光路，由激光器發出的光線，經過匯聚透鏡聚焦后入射到被測物體表面上，光接收系統接收來自入射點處的散射光，并將其成像在光電位置探測器敏感面上，通過光點在成像面上的位移來測量被測物面移動距離的一種測量方法。單點式激光三角法測量可分為直射式和斜射式兩種結構［6-7］，本文采用斜射式激光三角測距法，原理如圖1所示。激光器發射的平行光線，經過會聚透鏡聚焦后形成一束光，該光束垂直射到被測物體表面。待測目標面的相對移動或其表面變化都會導致入射光點沿入射光軸前后移動。測量激光的散射光經接收透鏡垂直照射到光電位置探測器上，如果入射光點在光電探測器形成了位移x，則待測目標面沿軸方向的相對位移為d。依據三角測距法的位置關系我們可根據下式計算出d的值：

式中，s為入射光點與接收透鏡入光點的距離，f為接收透鏡入光點到成像面的垂直距離，β為入射激光光軸與接收透鏡光軸的夾角。

圖1 三角測距原理

1 噪聲及去噪分析

激光三角測距算法測距中的準確性很大程度依賴于激光光斑定位的精確性［8］，在實際應用中光斑的定位經常會受到噪聲的影響導致定位產生較大的誤差，這種誤差主要受兩方面的噪聲產生［9］，一是由像元的讀出噪聲、暗電流散粒噪聲、光子散粒噪聲、固定模式噪聲和光譜響應不均勻性等造成的隨機噪聲［10-11］，另一方面是由圖像傳感器有限空間采樣寬度帶來的灰度平均效應以及像元填充率、填充形式帶來的系統噪聲。圖2為激光雷達系統在受噪聲影響下所收集的光斑信號，本文依據屬性距離加權平均濾波的原理對這種不可避免的隨機噪聲及系統噪聲加以處理。

圖2 受噪聲的激光光斑

屬性距離加權平均濾波ADWA（Attribute Distance Weighted Average）［12］由基本加權滑動平均濾波改進而來，以不同的距離屬性作為信號的權值。對一維信號的處理，屬性距離加權平均濾波表示為：

式中，Vi+k為采樣信號值，為對應于Vi的濾波結果，ωk=f(Dk)為采樣點Vi+k的權系數，Dk為多元屬性距離。記第i個采樣點的第m個屬性為Ami，則第（i+k）采樣點和第i采樣點之間的M元屬性距離表示為：

式中，dm為屬性基距離，σm為屬性Am的正規化系數，用于調節相應屬性在濾波過程中的作用力度。

屬性距離反映了采樣數據點之間在多個屬性上的差異，實質上也反映了信號和噪聲之間的差異。因此ADWA是從多屬性角度來分離噪聲和信號，克服了其他濾波方法只局限于信號個別屬性的缺點。本文以位置、幅度、曲率作為屬性距離公式的三個元素，曲率反映了曲線在某一點上的彎曲程度，在圖像信號及計算機視覺等領域都有較重要的應用，能在去噪的同時很好地突出光斑的峰值點。則屬性距離計算公式表示為：

式中，L、V、K分別為位置、幅度、曲率值，σL、σV、σK分別為位置、幅度、曲率系數，可以通過這3個參數調整位置、幅度、曲率屬性的影響權重，本文將其統一設置為1。

以Rosenfeld等提出的k余弦曲率計算方法對光斑離散曲線的曲率進行估計［13］，對于給定的參數k，k余弦曲率首先在曲線當前點pi處定義2個矢量：

式（5）、（6）中(xi,yi)、(xi-k,yi-k)、(xi+k,yi+k)都是曲線上的坐標點，通過式（7）計算aik和bik夾角的余弦值cik：

為了選取合適的參數k，首先確定k的最大值，記為m，然后取 k=m,m-1,m-2,…，計算 cim，ci,m-1，…，根據條件 cim＜ci,m-1＜…＜cik≥ci,k-1對k值進行選取，cik即作為當前點pi的離散曲率值。

2 光斑定位算法

理想的光學系統能使平行光束聚焦成像在CCD上的一個像素上，如圖3所示。而實際測量中，由于光學系統的成像誤差、衍射、散射以及CCD器件本身的噪聲影響，會使所成的像變成一個光強按某種分布占據多個像素的光斑，其光斑信號如圖4所示。對于這種光斑信號模型，可以通過算法求出其質心，并以此質心作為光斑中心的準確位置，可以將光斑的定位提高到亞像素級別，從而提高了CCD分辨率。

圖3 理想光斑

圖4 實際光斑

假設成像在CCD的光敏面上的光斑覆蓋光敏面像元個數為m。由于其像素輸出信號的模型近似為高斯分布，所以可以用灰度質心法來求光斑中心，經典灰度質心法［14］可以由式（8）給出：

經典的灰度質心法很容易受到噪聲的影響，在有噪聲的情況下亞像素的定位精度將會嚴重下降，即使在無噪聲影響的情況下的定位精度也只能達到0.1 pix。為了進一步解決亞像素的精確定位問題，本文通過線性插值的平方加權及多次迭代的方法對灰度質心法進行優化，線性插值平方加權后的公式可以由式（9）表示：

式（8）、式（9）中，X為光斑灰度質心的位置，N為光斑正態分布的起始像素坐標，則N+m為結束坐標，xi為第N+i個像素位置，f(xi)為第N+i個像素位置上的灰度值大小。

f(xu)為線性插值點u上的灰度值，根據線性插值理論得到 f(xu)的計算公式如下：

式（10）、式（11）中k為斜率，(x,f(x))為曲線上的坐標點。

基于線性插值的平方加權質心法通過線性插值彌補了物理像素點有限性、離散性的缺點，提高了有效的像素點數量，用灰度值的平方作為權值，突出了離中心較近的較大灰度值像素點對中心位置的影響，對系統噪聲起到了很好的去除作用。然而當傳感器處于快速旋轉運動的情況下，很容易造成光斑信號對稱性差的問題，如圖5所示，這樣的問題無法通過簡單地消除系統噪聲的方法來獲得高精度的亞像素點。為了解決這個問題，本文通過多次迭代計算灰度質心可以獲得更好的定位精度及穩定性。

圖5 扭曲的激光光斑

在灰度質心法的基礎上根據質心估計值的偏差進行多次迭代計算，直到偏差小于一定的閾值為止。每次迭代計算前先由式（12）進行質心估計值的偏差衡量：

式中，X為當前計算的光斑中心，N為光斑正態分布的起始像素坐標，光敏面上的光斑覆蓋光敏面像元個數為m，所以上一次的光斑估計值為，通過當前光斑中心的計算值與光斑估計值的差值計算偏差衡量因素σ。如果|σ|大于設定的閾值，則調整正態分布的起始像素為N+σ，用灰度質心法公式重新估計光斑質心。如此迭代，直至|σ|滿足閾值要求。

如圖6為本算法的具體流程圖，通過設置閾值尋找光斑的位置，從而確定光斑在像元的覆蓋范圍m，這個閾值的設定有多種方式，可以是最大值法、均值法等，由于這個光斑位置并非是準確的實際光斑位置，所以在迭代算法中通過重復確定質心，在像元覆蓋個數m確定的情況下調整光斑的位置，通過以上方式可以在初始及迭代過程中確定光斑的正態分布范圍。多次迭代計算灰度質心的求質心方法，不但使光斑中心的定位精度達到了0.05 pix左右，還解決了一般平方加權質心法受噪聲影響大的問題，即使光斑信號出現對稱性差的情況下，依然能較好地獲得光斑中心的定位。

圖6 算法流程

3 實驗

本文所述的激光雷達系統，以DLSI-2K作為線性感光元件，DLIS-2K線陣圖像傳感器靈敏度可達160 V/lux-s，包括4行像素，每行有2 081個光學像素和16黑像素。其中3行為4×4 micron方形像素，另一行為4×32 micron長方形像素。以STM32F103C8T6作為核心處理單元，速度可達72 MHz，具有64 kbyte的程序存儲容量。激光雷達系統結構如圖7所示，測距系統位于一個可連續旋轉的平臺之上，由電機帶動旋轉平臺進行快速旋轉，使測距系統可以對整個水平面進行測距。

圖7 激光雷達系統

由于激光雷達系統工作在一個快速旋轉的狀態中，光斑信號很容易受到各種噪聲的影響，甚至產生變形，不能通過簡單的高斯模型進行模擬，所以本文的實驗將采用實際的光斑數據進行實驗。為了獲得激光雷達系統的光斑信號數據，將系統固定于水平移動平臺之上，只對角度為0°的數據進行輸出，調整激光雷達系統所處的水平位置來獲得不同距離的光斑信號數據。為了直觀的反映算法的性能，本文首先在PC端通過串口收集激光雷達獲取的光斑信號，然后再用MATLAB對數據進行處理。

3.1 亞像素點的精度對比實驗

為了獲取較為理想的光斑估計中心，本文用平均濾波處理后采用灰度質心法計算光斑中心，取50次平均值作為光斑中心的估計值［15］。實驗分別對400 mm、450 mm、500 mm、550 mm、600 mm、650 mm、700 mm、750 mm、800 mm、850 mm、900 mm處獲取的光斑信號進行處理，每個距離都取10組不同的數據進行計算，取誤差值的絕對值計算平均值作為光斑中心的測量平均誤差。

圖8為使用平均濾波、中值濾波及本文所述的ADWA濾波進行濾波處理，并用灰度質心法獲取的光斑中心誤差值對比。圖9為用平均濾波處理后，分別用灰度質心法、線性插值灰度質心法及本文算法獲取的光斑中心誤差值對比。

圖8 濾波算法對比

圖9 亞像素定位算法精度對比

通過圖8、圖9的對比結果可知，本文所述的ADWA濾波對處于快速旋轉的激光雷達的光斑信號的處理明顯好于平均濾波及中值濾波，使光斑定位計算值依然可以達到0.1 pix的精度要求。同樣，對于本文的亞像素定位算法，處理后的光斑信號誤差基本在0.05 pix左右，明顯優于普通質心法及線性插值的質心法。由于三角測距方式本身特性，各算法隨著距離的增加誤差也會不同程度增大，從整體數據來分析，ADWA濾波及本文所提的迭代質心法依然有較明顯的優勢。

3.2 穩定性對比實驗

以500 mm的距離處獲得的光斑信號對算法的穩定性進行實驗，在500 mm處光斑中心的估計值為612.12 pix，通過本文所述的ADWA濾波算法處理后，分別用灰度質心法、線性插值灰度質心法及本文算法計算的光斑中心計算值。如表1所示，取10組不同的數據分別用不同的定位算法計算光斑中心計算值，取誤差值的絕對值計算平均值作為光斑中心的測量平均誤差，并計算光斑中心計算值標準差以反映算法的穩定性。

表1 亞像素定位算法穩定性對比

通過表1的對比可以發現，在測量誤差方面，質心法，線性插值質心法和本文算法的測量平均誤差逐漸減小，本文算法相對與傳統的質心法有3倍以上的提高，相對與改進的線性插值質心法也有1倍以上的提高。在穩定性方面，從標準差的結果可以得出，本文算法為0.053，相對與傳統質心法提升了320%，而相對于線性插值質心法也有180%的提升。

本文的亞像素定位算法不管在定位誤差方面還穩定性方面都是比普通質心法及線性插值法要好。

3.3 濾波效果對比實驗

分別通過5×1的中值濾波、均值濾波、ADWA濾波對圖2中的光斑信號進行處理，處理結果如圖10所示。

圖10 濾波效果對比

圖10中，3種濾波都在一定程度上降低的信號的噪聲，但中值濾波對于光斑峰值處噪聲的處理明顯是不足的，均值濾波雖然能較好地處理了峰值處的噪聲，但處理后光斑的峰值明顯小于原始信號峰值，而ADWA濾波可以在較好地保留了峰值特征的情況下起到去噪的作用。以均方根誤差作為去噪效果的評估標準，ADWA濾波的去噪效果約為傳統方式的0.5倍。

4 結論

本文主要是針對激光雷達系統所獲取的激光光斑信號進行濾波算法及亞像素定位算法的研究，對于這種在快速旋轉的狀態中獲取的激光光斑信號進行了噪聲分析，并提出了相應的濾波進行去噪，并用改進的灰度質心法對亞像素進行計算。通過實驗的對比，證明本文所述算法具有很好的效果，對于只有4 μs的像素單元仍能取得0.05 pix的定位精度，并在穩定性上明顯優于其他算法，滿足了激光雷達用于室內機器人環境感知的設計要求。

［1］Zhang J，Mao X.Optimal Bandwidth Design of Laser Radar Re?ceiver［C］//Optical Communications and Networks（ICOCN），2015 14th International Conference on.IEEE，2015.

［2］Fong E H L，Adams W，Crabbe F L，et al.Representing a 3-D En?vironment with a 21/2-D Map Structure［C］//Intelligent Robots and Systems，2003.（IROS 2003）.Proceedings.2003 IEEE/RSJ International Conference on.IEEE，2003：2986-2991.

［3］Vandapel N，Huber D F，Kapuria A，et al.Natural Terrain Classifi?cation Using 3-D Ladar Data［C］//IEEE International Conference on Robotics&Automation，2004：5117-5122.

［4］Brosed F J，Santolaria J，Aguilar J J，et al.Laser Triangulation Sen?sor and Six Axes Anthropomorphic Robot Manipulator Modelling for the Measurement of Complex Geometry Products［J］.Robotics and Computer-Integrated Manufacturing，2012，28（6）：660-671.

［5］Herrmann M，Otesteanu M，Otto M A.A Method to Determine the Extrinsic Parameter of Laser Triangulation Sensors，With Restrict?ed Mobility［C］//Electronics and Telecommunications（ISETC），2014 11th International Symposium on.IEEE，2014.

［6］Breier M，Moller P，Li W，et al.Accurate laser Triangulation Us?ing a Perpendicular Camera Setup to Assess the Height Profile of PCBs［C］//Industrial Technology（ICIT），2015 IEEE International Conference on.IEEE，2015：1613-1618.

［7］Ferreira Barreto S V，Eskinazi Sant’Anna R，Feitosa M A F.A Method for Image Processing and Distance Measuring Based on Laser Distance Triangulation［C］//Electronics，Circuits，and Sys?tems（ICECS），2013 IEEE 20th International Conference on.IEEE，2013：695-698.

［8］Lefebvre D，Doucet M，Duval Y，et al.3D Laser Triangulation Compensation for Non-Uniform Surfaces Reflectivity［C］//Applied Industrial Optics：Spectroscopy，Imaging and Metrology.Optical Society of America，2013.

［9］馬玉良，許明珍，佘青山，等.基于自適應閾值的腦電信號去噪方法［J］.傳感技術學報，2014，27（10）：1368-1372.

［10］紀峰，李翠，常霞，等.基于改進閾值函數的自適應圖像去噪方法［J］.傳感技術學報，2014，27（3）：351-354.

［11］張輝，袁家虎，劉恩海.CCD噪聲對星敏感器星點定位精度的影響［J］.紅外與激光工程，2006，35（5）：629-633.

［12］熊剛，丁天懷，王鵬.應用屬性距離加權平均濾波提高CCD光斑的亞像素定位精度［J］.光學精密工程，2012，20（5）：1102-1109.

［13］郭娟娟，鐘寶江.U弦長曲率：一種離散曲率計算方法［J］.模式識別與人工智能，2014（8）：683-691.

［14］陳曉東，李為民.利用重心法求光斑信號位置的誤差分析［J］.光學技術，2000（1）：5-608.

［15］陳運錦，馮瑩，魏立安，等.光斑質心亞像素定位誤差的實驗研究［J］.光電工程，2010，37（2）：80-84.

李 高（1992-），男，碩士研究生，主要研究方向為機器人傳感器、機器人，lg16@qq.com；

何元烈（1976-），男，工學博士，廣東工業大學計算機學院副教授，碩士研究生導師，研究方向為計算機視覺、機器人、物聯網等，heyuanlie@163.com；

曾 碧（1963-），女，工學博士，廣東工業大學計算機學院教授，主要研究方向為智能信息處理、智能機器人等。

Spot Positioning Algorithm for Laser Radar Based on Triangulation*

LI Gao，HE Yuanlie*，ZENG Bi
（School of Computer Science Guangdong University of Technology，Guangzhou 510000，China）

In order to reduce the noise of light spot of laser radar system and improve the precision of the distance measurement for the system，an advanced algorithm based on attribute distance weighted average filter algorithm and gray-gravity method algorithm was proposed.The attribute distance weighted average filter with the property of curvature can not only reduce the noise of light spot，but also can highlight the peak spot of the laser facula.While the method by using iterative calculations for gray centroids would further improve the positioning accuracy of light spot and it can also solve the problem of susceptible to noise by using the square weighted gray-gravity method.Ex?periments have dealt with the collected practical data in MATLAB simulation environment，and the results of the ex?periments show that the algorithm can achieve positioning accuracy with even 0.05 pix for 4 μs pixel unit.Many re?peated experiments indicated that the experimental results had a good stability.

robot sensor；2-D laser radar；sub pixel positioning；attribute distance weighted average filter；square weighted gray-gravity method algorithm

TP212.6

A

1004-1699（2016）11-1692-06

EEACC：6320；4360B 10.3969/j.issn.1004-1699.2016.11.011

項目來源：廣東省產學研合作專項資金項目（2014B090904080）；廣東省應用型科技研發專項資金項目（2015B090922012）；東莞市產學研合作項目（2015509109107）

2016-03-17 修改日期：2016-07-05