基于激光雷達的數(shù)據(jù)處理系統(tǒng)設(shè)計

陳錦旗,陳添丁,林添成,黃 成

（閩南師范大學物理與信息工程學院,福建漳州363000）

三維激光雷達是一種通過發(fā)送激光束探測與目標方位的探測技術(shù)，可實現(xiàn)三維空間的全方位探測[1].三維激光雷達輸出的是被測目標的距離和強度的點云信息，其通過距離與強度點云信息可以構(gòu)建出三維空間的景深模型.通過構(gòu)建模型可應用于自動駕駛、環(huán)境建模和三維成像等領(lǐng)域.本文采用Velodyne 公司的VLP-16 激光雷達，該雷達采用UDP 協(xié)議實時輸出三維點云數(shù)據(jù).雷達通過360°快速的旋轉(zhuǎn)實時采集三維點云其數(shù)據(jù)量很大，輸出的三維數(shù)據(jù)需要進行解析才能轉(zhuǎn)化為三維空間距離與強度信息，再對解析完的數(shù)據(jù)進行顯示與存儲.雷達數(shù)據(jù)需要經(jīng)過采集、解析完成之后才能進行環(huán)境建模、三維成像和自動駕駛等應用，因此，設(shè)計一套可靠的雷達數(shù)據(jù)處理系統(tǒng)對自動駕駛、環(huán)境建模和三維成像等具有重要的意義[2-3].

系統(tǒng)采用Visual C++2015 平臺基于MFC 庫實現(xiàn)了數(shù)據(jù)的高速采集與傳輸、解析、存儲等，并結(jié)合OpenGL 庫實現(xiàn)了三維點云數(shù)據(jù)的顯示.數(shù)據(jù)處理系統(tǒng)可分為參數(shù)設(shè)置模塊、數(shù)據(jù)接收端模塊、數(shù)據(jù)存儲模塊、數(shù)據(jù)處理模塊、顯示模塊與三維顯示控制模塊等.計算機通過網(wǎng)絡(luò)與激光雷達連接，激光雷達采用UDP 協(xié)議把雷達的原始數(shù)據(jù)包傳輸給計算機，計算機接收到激光雷達數(shù)據(jù)需判斷是否為點云數(shù)據(jù)，若是點云數(shù)據(jù)則接收存儲.為了預防系統(tǒng)運行延遲丟失數(shù)據(jù)，數(shù)據(jù)接收端設(shè)計了先進先出的環(huán)形鏈表用于存儲雷達原始數(shù)據(jù)包和已解析的三維點云數(shù)據(jù)包.數(shù)據(jù)處理端從數(shù)據(jù)接收端的環(huán)形鏈表中依次讀取激光雷達原始數(shù)據(jù)包，接收到的數(shù)據(jù)通過計算解析出三維點云數(shù)據(jù),再把解析完成的三維點云數(shù)據(jù)包暫存在數(shù)據(jù)處理模塊的環(huán)形鏈表.系統(tǒng)調(diào)用顯示模塊線程把解析完的三維點云數(shù)據(jù)包進行拼幀操作，每個激光雷達的原始數(shù)據(jù)包只包含某一扇形區(qū)的數(shù)據(jù)，需要提取環(huán)形鏈表中的三維點云數(shù)據(jù)包然后根據(jù)相應的算法拼幀，每幀數(shù)據(jù)包包含激光雷達采集360°的點云數(shù)據(jù).顯示模塊再調(diào)用OpenGL 庫把一幀的三維點云數(shù)據(jù)顯示出來，點動鼠標左鍵、右鍵和滾輪調(diào)用三維顯示控制模塊可實現(xiàn)三維視角切換.

1 激光雷達數(shù)據(jù)解析原理

計算機接收到的雷達數(shù)據(jù)包屬于原始數(shù)據(jù)，需對原始數(shù)據(jù)包進行相應的解算和轉(zhuǎn)換才能獲得三維空間點云數(shù)據(jù).如圖1所示，圖示左邊為激光雷達與目標點三維空間關(guān)系示意圖，其中Y軸正方向為激光雷達掃描的初始位置也就是旋轉(zhuǎn)角度為0°方向，激光雷達水平掃描的方向從俯視圖上看為逆時針方向，當前掃描位置與Y 軸所成的角度為α.從激光雷達側(cè)視圖看激光束與XY 平面的夾角為ω，VLP-16 激光雷達在垂直方向上掃描有16 束激光束，其角度分別為-15°，1°，-13°，-3°，-11°，5°，-9°，7°，-7°，9°，-5°，11°，-3°，13°，-1°，15°，在這垂直面上不同角度的16束激光360°轉(zhuǎn)掃描就可以得到空間三維的點云數(shù)據(jù).已知激光雷達與目標點的距離、垂直方向夾角ω和水平夾角α，通過以下公式（1）至公式（4）即可計算出激光雷達當前位置與目標點的位置關(guān)系.

激光雷達的原始數(shù)據(jù)包格式如圖1所示，圖示右邊為激光雷達的原始數(shù)據(jù)包格式.VLP-16 激光雷達每個數(shù)據(jù)包分為12 數(shù)據(jù)分段Data Block1-Data Block12，每個數(shù)據(jù)分段包含2 組數(shù)據(jù)單元，數(shù)據(jù)包總共包含24組數(shù)據(jù)單元，每組數(shù)據(jù)單元包含16個目標點.每個數(shù)據(jù)分段開頭包含一個標志位0xFFEE，標志位下一個信息為2字節(jié)的當前位置角度值A(chǔ)zimuth N（角度ɑ），當前位置角度值下一個信息為連續(xù)48個字節(jié)的16個目標點信息Channel 0 Data-Channel 15 Data，一個目標點信息占3個字節(jié)，再接下去為下一組16個目標點的信息Channel 0 Data-Channel 15 Data.同一個數(shù)據(jù)分段中只攜帶一個標志位和一個當前位置角度值A(chǔ)zimuth N，那么同一個數(shù)據(jù)分段第2 組單元的當前位置角度值A(chǔ)zimuth N+1 等于當前數(shù)據(jù)分段Azi‐muth N值與下一個數(shù)據(jù)分段Azimuth N+2值求和的平均值.

圖1 激光雷達與目標點三維空間關(guān)系和原始數(shù)據(jù)包格式Fig.1 a)The relationship between LIDAR and 3D spatial of target point;b)The raw packet data of Lidar

2 數(shù)據(jù)處理系統(tǒng)設(shè)計

2.1 拼接雷達數(shù)據(jù)幀算法

計算機接收到激光雷達數(shù)據(jù)包只包含某一扇形區(qū)的數(shù)據(jù)，對激光雷達數(shù)據(jù)處理時需要完整一幀數(shù)據(jù)，這就要求系統(tǒng)需實時對零散的激光雷達數(shù)據(jù)包進行拼接，拼成完整的一幀數(shù)據(jù).具體的算法流程如下圖2所示，首先線程調(diào)用函數(shù)讀取環(huán)形鏈表的激光雷達數(shù)據(jù)包，逐一讀取數(shù)據(jù)包中的單元數(shù)據(jù)判斷角度α是否小于閾值角度β，若小于閾值則可判定為幀頭（VLP-16 激光雷達旋轉(zhuǎn)一周角度變化0°～360°，旋轉(zhuǎn)頻率為10～20 Hz 可調(diào)，每轉(zhuǎn)一圈采集3 600 次可計算得雷達水平分辨率為0.1°～0.2°，閾值角度β根據(jù)設(shè)定的頻率旋轉(zhuǎn)取相應的值）.找到幀頭記錄該幀頭信息把當前數(shù)據(jù)單元存儲到LidarFrame變量中，繼續(xù)從環(huán)形鏈表中讀取數(shù)據(jù)包，并計算最后一個數(shù)據(jù)單元的角度α是否大于360°，若大于則說明幀尾信息在該數(shù)據(jù)包里面，需從該包中獲取幀尾并把剩余的數(shù)據(jù)單元存入TempLidarDataStructure變量中以供查找下一幀的幀頭.若最后一個數(shù)據(jù)單元的角度α介于(360-β)°→360°之間，則該數(shù)據(jù)單元為幀尾信息，到此已完成一個完整數(shù)據(jù)幀的拼接，否則繼續(xù)從環(huán)形鏈表中讀取數(shù)據(jù)包加載到變量LidarFrame中.

圖2 雷達數(shù)據(jù)的拼接算法Fig.2 The reconfiguration algorithm of Lidar data

2.2 激光雷達數(shù)據(jù)存儲結(jié)構(gòu)

系統(tǒng)基于Visual C++2015 平臺使用MFC 自帶的Socket 庫編程，實現(xiàn)讀取激光雷達原始數(shù)據(jù)包[4].為了方便后續(xù)對原始數(shù)據(jù)進行處理與解析，系統(tǒng)中定義了兩個結(jié)構(gòu)體分別為組數(shù)據(jù)結(jié)構(gòu)體和包數(shù)據(jù)結(jié)構(gòu)體，如下圖3所示，組數(shù)據(jù)結(jié)構(gòu)體為包數(shù)據(jù)結(jié)構(gòu)體的一個成員.其中組數(shù)據(jù)包_LidarDataMode包含了一個數(shù)據(jù)分組當前位置角度變量Azimuth，數(shù)據(jù)分段的時間戳變量TimeStamp，存儲數(shù)據(jù)分組的二維數(shù)組變量LidarDataAxis[LidarLightBeam][3]，其中LidarLightBeam 是宏定義代表每組16 個目標點數(shù)據(jù)，DataBlock 變量用于標記當前數(shù)據(jù)單元在數(shù)據(jù)包中的位置，angle變量存儲數(shù)據(jù)單元角度α.包數(shù)據(jù)結(jié)構(gòu)體_LidarFrame‐Structure 用于存在一幀激光雷達數(shù)據(jù)，CellSizeLidar 變量用于統(tǒng)計當前幀中數(shù)據(jù)單元的數(shù)目，F(xiàn)rameOrder變量用于累計幀的數(shù)量，DataMode用于標記數(shù)據(jù)類型是否是已解析還是未解析數(shù)據(jù)，ReturnType與Lidar‐Model變量為標記當前雷達的模式和參數(shù)，LidarDataMode為C++容器vector變量用于存儲一幀激光雷達數(shù)據(jù).

圖3 組數(shù)據(jù)結(jié)構(gòu)體與包數(shù)據(jù)結(jié)構(gòu)體Fig.3 Sets of data structures and package data structures

2.3 三維點云顯示

系統(tǒng)基于Visual C++2015 平臺可使用MFC 自帶的CDC 庫繪制三維點云數(shù)據(jù)[5]，但在繪制復雜圖像或者像素點比較多時會出現(xiàn)嚴重的卡頓現(xiàn)象.當VLP-16 激光雷達的旋轉(zhuǎn)速率為10 Hz 輸出的點云最密集，此時，雷達水平方向采集精度為0.1°，轉(zhuǎn)一圈采集3 600 次，每次采集一組點云數(shù)據(jù)，每組有16個點云數(shù)據(jù)每個點云數(shù)據(jù)包含3個維度的數(shù)據(jù)量，每個維度由2個字節(jié)存儲，所以每轉(zhuǎn)一圈雷達輸出的數(shù)據(jù)量為了345 600 B（3 600*16*3*2）字節(jié)信息量.如此大的信息量如果采用傳統(tǒng)的顯示方式必定會出現(xiàn)嚴重卡頓現(xiàn)象，為了解決繪制三維點云數(shù)據(jù)卡頓問題，本文利用多線程結(jié)合VBO 編程方式改善性能，VBO 是OpenGL提供的一種非立即模式下（立即模式使用glBegin/glEnd 方式）更新顯示數(shù)據(jù)的方式，VBO 模式一般在顯卡中開辟的一塊存儲區(qū)域把需要顯示的數(shù)據(jù)存儲在顯卡中，這樣能提高數(shù)據(jù)的刷新速率，使畫面更加流暢，其多線程結(jié)合VBO編程流程如下圖4所示.

圖4 多線程結(jié)合VBO編程Fig.4 Multi-threading combined with VBO programming

2.4 三維點云濾波

激光雷達測距精度比較高，受外界溫度、天氣可見度影響小等.但實際運用中會出現(xiàn)系統(tǒng)噪聲、離散化誤差或激光雷達平臺震動等因素，對數(shù)據(jù)精確度有一定的影響，為了消除影響需對原始數(shù)據(jù)進行濾波.常見的濾波方法有線性最小二乘濾波、均值濾波、移動平均濾波、中值濾波、Savitzky-Golay 濾波和高斯濾波等，依據(jù)文獻[6]的結(jié)論中值濾波方法優(yōu)于其他濾波方法能夠?qū)c云數(shù)據(jù)進行較好的平滑處理，且中值濾波器計算量小有利于是系統(tǒng)的實時性，所以本系統(tǒng)采用中值濾波器對原始數(shù)據(jù)進行平滑處理.

2.5 系統(tǒng)調(diào)試

系統(tǒng)調(diào)試需要通過網(wǎng)線把VLP-16激光雷達接到計算機.打開數(shù)據(jù)處理軟件之后具體操作：點擊菜單欄“設(shè)置”按鈕彈出對話框，并在該話框中設(shè)置雷達的IP 地址，可選擇手動輸入雷達IP 地址或自動獲取雷達IP 地址，然后點擊“啟動”按鈕，系統(tǒng)啟動接收雷達數(shù)據(jù).軟件菜單欄中的“設(shè)置”按鈕就是軟件框架中的“參數(shù)設(shè)置模塊”，該模塊主要用于設(shè)置雷達IP地址、雷達端口編號、雷達旋轉(zhuǎn)頻率和啟動雷達等.

系統(tǒng)啟動之后可以看到軟件界面掃描出當前環(huán)境的三維點云數(shù)據(jù)如下圖5所示：圖中黑色網(wǎng)格表示地面，雷達數(shù)據(jù)的高度信息以網(wǎng)格為參照，綠色箭頭表示空間Z 軸坐標，紅色箭頭表示空間Y 軸坐標，藍色箭頭表示空間X 軸坐標.通過操作鼠標的左鍵、滾輪、右鍵等實現(xiàn)三維空間任意視角觀察雷達數(shù)據(jù)，按住鼠標左鍵并上下拖動鼠標可實現(xiàn)改變俯仰視角若左右拖動鼠標則可實現(xiàn)視角的旋轉(zhuǎn)，按住鼠標右鍵并上下方向拖動鼠標可實現(xiàn)Z 軸方向視角調(diào)整若左右方向拖動鼠標則可實現(xiàn)Y 軸方向視角調(diào)整，按住鼠標滾輪并上下拖動可實現(xiàn)X軸方向視角調(diào)整.該系統(tǒng)中三維點云數(shù)據(jù)的誤差主要取決于激光雷達自身的精度，VLP-16 雷達官方手冊提供的精度為±3 cm.圖中紅色的點即為激光雷達掃描周圍環(huán)境的三維點云數(shù)據(jù).

圖5 激光雷達數(shù)據(jù)采集軟件界面Fig.5 Software interface of data collection of Lidar

2.6 數(shù)據(jù)采集測試

為了測試該系統(tǒng)數(shù)據(jù)處理的精度.本文采用1 個VLP-16 激光雷達、筆記本電腦和相機支架等設(shè)備，在大約寬為4 m，長度為8 m 的封閉實驗室內(nèi)利用相機支架架設(shè)激光雷達，然后在筆記本電腦上安裝數(shù)據(jù)處理系統(tǒng)對雷達的數(shù)據(jù)進行采集與處理.激光雷達架設(shè)在室內(nèi)8個不同位置，在這8個位置上實時對室內(nèi)環(huán)境進行數(shù)據(jù)采集，然后通過采集出來的數(shù)據(jù)計算激光雷達與室內(nèi)環(huán)境的空間位置關(guān)系，并與實際量測的數(shù)據(jù)進行分析得出采集數(shù)據(jù)的誤差，如下圖6所示.圖中橫坐標表示激光雷達與室內(nèi)某一目標的距離，縱坐標記錄激光雷達掃描該目標數(shù)據(jù)存在的方差，從該實驗中可以得出數(shù)據(jù)處理系統(tǒng)對環(huán)境掃描誤差控制在3 cm左右.

圖6 量測距離與誤差Fig.6 Measuring distance and error

3 結(jié)語

本文設(shè)計了激光雷達的數(shù)據(jù)處理系統(tǒng)，數(shù)據(jù)處理系統(tǒng)可分為參數(shù)設(shè)置模塊、數(shù)據(jù)接收端模塊、數(shù)據(jù)存儲模塊、數(shù)據(jù)處理模塊、顯示模塊與三維顯示控制模塊等.該系統(tǒng)采用Visual C++2015 平臺基于MFC 庫實現(xiàn)了數(shù)據(jù)的高速采集與傳輸、解析、存儲，并結(jié)合OpenGL庫實現(xiàn)了點云數(shù)據(jù)的顯示，實驗表明系統(tǒng)能夠采集復雜環(huán)境的三維點云數(shù)據(jù).激光雷達數(shù)據(jù)處理系統(tǒng)對后續(xù)研究自動駕駛、環(huán)境建模和三維成像等有著重要的意義.