谷曉杰，卜春光，陳 成，周浚哲

(1.沈陽理工大學(xué) 信息科學(xué)與工程學(xué)院，遼寧 沈陽 110159； 2.中國科學(xué)院沈陽自動化研究所，遼寧 沈陽 110016 )

三維激光測距系統(tǒng)設(shè)計與標(biāo)定方法研究

谷曉杰1，2，卜春光2，陳 成2，周浚哲1

(1.沈陽理工大學(xué) 信息科學(xué)與工程學(xué)院，遼寧 沈陽 110159； 2.中國科學(xué)院沈陽自動化研究所，遼寧 沈陽 110016 )

針對傳統(tǒng)三維激光測距系統(tǒng)標(biāo)定結(jié)果精度低的缺陷，設(shè)計一套由云臺和激光測距裝置組成的三維激光測距系統(tǒng)。提出了采用平面特征的標(biāo)定方法。在初始標(biāo)定值下，對一個已知平面三維掃描后的若干幀，每幀的若干點作為試驗點，用Matlab做非線性優(yōu)化，來找到一個合適的標(biāo)定值，使得試驗點擬合后得到的平面到真實平面的距離平方和最小。該標(biāo)定方法簡單，經(jīng)過試驗證明，此方法能精確地實現(xiàn)激光的標(biāo)定。

激光標(biāo)定；平面擬合；三維激光測距系統(tǒng)；機器人

近年來，隨著科學(xué)技術(shù)的發(fā)展，激光測距儀被越來越多地應(yīng)用于航空航天[1]、機器人[2]等領(lǐng)域。常見的激光測距儀僅能獲取二維的平面數(shù)據(jù)，為了獲取三維環(huán)境信息,通常需要構(gòu)建三維激光測距系統(tǒng)。常見的三維激光測距系統(tǒng)按照其機械結(jié)構(gòu)可以分為通過旋轉(zhuǎn)激光鏡面獲取三維測距信息以及通過選擇激光測距儀本體獲取三維測距信息兩類。旋轉(zhuǎn)鏡面的三維激光測距系統(tǒng)對機械加工精度要求較高，難以設(shè)計實現(xiàn)。市面上已有的旋轉(zhuǎn)鏡面激光測距系統(tǒng)，如Velodyne Lidar系統(tǒng)，成本較高。本文采用將激光測距儀與云臺固連構(gòu)建三維激光測距系統(tǒng)。為了獲取精確的三維測距信息，需要對三維激光測距系統(tǒng)進行標(biāo)定。三維激光測距系統(tǒng)的標(biāo)定主要對激光測距儀相對于基準(zhǔn)坐標(biāo)系的相對位姿進行標(biāo)定。待標(biāo)定的相對位姿包括激光測距儀坐標(biāo)系相對于基準(zhǔn)坐標(biāo)系的三個姿態(tài)角(包括偏航角α，俯仰β角，橫滾γ角)以及三個坐標(biāo)軸偏移量(包括x軸，y軸以及z軸偏移量)。這六個待標(biāo)定參數(shù)記為S=(α,β,γ,xL,yL,zL)，三維激光測距儀標(biāo)定即為通過標(biāo)定算法解算得到這六個參數(shù)的最優(yōu)估計值，通常三維激光測距系統(tǒng)的標(biāo)定方法大都采用標(biāo)準(zhǔn)模板法[3]，即通過標(biāo)準(zhǔn)模板上三維坐標(biāo)已知的被測點將傳感器坐標(biāo)系和測量基準(zhǔn)坐標(biāo)系聯(lián)系起來，進而求得兩個坐標(biāo)系間的變換方程。然而，由于掃描測距系統(tǒng)的激光脈沖是不可見的、而且掃描點是離散的，因此被測點在基準(zhǔn)坐標(biāo)系內(nèi)的三維坐標(biāo)無法精確獲得。為此本文提出了一種新的激光標(biāo)定方法，即在初始標(biāo)定值S0下，以掃描后的若干幀，每幀的若干點作為試驗的空間點(這些點是對真實環(huán)境中的平面的掃描點)，通過非線性優(yōu)化算法來找到一個合適的標(biāo)定值S，使得試驗空間點經(jīng)掃描后得到的點到相應(yīng)的空間擬合平面的距離平方和為最小。

1 三維激光測距系統(tǒng)

1.1 系統(tǒng)硬件簡介

本文針對由云臺和激光組成的三維激光測距系統(tǒng)設(shè)計控制系統(tǒng)，三維激光測距系統(tǒng)試驗平臺如圖1所示，它由兩自由度的云臺PTU-D47(包括偏轉(zhuǎn)單元和俯仰單元兩部分)和激光UTM-30LX組成。

二維激光測距儀UTM-30LX由HOKUYO公司生產(chǎn)，測距最遠(yuǎn)為30m,掃描范圍為270°，DC12V輸入，25ms掃描時間。云臺裝置PTU-D47有兩個自由度，其中俯仰單元轉(zhuǎn)動范圍+/-159°，偏轉(zhuǎn)單元轉(zhuǎn)動位置范圍為-47°～+31°，俯仰單元最大轉(zhuǎn)速為300°/s,偏轉(zhuǎn)單元最大轉(zhuǎn)速為60°/s,DC12V輸入，內(nèi)部有集成的控制器。通過控制云臺俯仰，二維激光測距儀掃描空間不同平面的環(huán)境信息，從而可實現(xiàn)三維掃描功能。PC機通過COM口與激光通訊。串口參數(shù)為：波特率115200b/s、8位數(shù)據(jù)位、1位停止位、無校驗位。PC機通過RS232串口與云臺進行通訊，串口參數(shù)為：波特率9600b/s、8位數(shù)據(jù)位、1位停止位、無校驗位。

圖1 三維激光測距系統(tǒng)

1.2 系統(tǒng)軟件設(shè)計與實現(xiàn)

利用PC機開發(fā)基于Qt的應(yīng)用程序，首先初始化激光和云臺，若兩者之一初始化失敗則程序退出，如果成功，則GUI界面顯示云臺的基本信息(如云臺最大最小速度，運動的極限范圍等)。然后用戶可以設(shè)定云臺步進大小，隨后開啟云臺線程，云臺每進一步，激光就采集一幀數(shù)據(jù)(步進模式保證測量的數(shù)據(jù)盡可能準(zhǔn)確)。并結(jié)合當(dāng)前的云臺姿態(tài)經(jīng)過坐標(biāo)轉(zhuǎn)換來計算出基于基準(zhǔn)坐標(biāo)系下的激光數(shù)據(jù)的三維表示，將其載入到點云中，用QVTKWidget控件進行顯示。然后云臺繼續(xù)步進，如此反復(fù)，直到云臺達(dá)到極限位置，對點云進行清空，然后云臺繼續(xù)步進，重復(fù)上述步驟，就實現(xiàn)了環(huán)境的實時三維重建[4]。

2 三維重建方法

由于二維激光測距儀僅能獲取二維的測距信息，為了獲取全局一致的三維環(huán)境信息，需要將激光測距信息與云臺位置信息融合。

圖2 系統(tǒng)軟件設(shè)計

2.1 坐標(biāo)系統(tǒng)

該系統(tǒng)中的坐標(biāo)系包括：{R}參考坐標(biāo)系，該坐標(biāo)系與云臺基座固連，為固定的坐標(biāo)系；{T}俯仰坐標(biāo)系，該坐標(biāo)系與云臺俯仰軸和偏轉(zhuǎn)軸交點固連，由于云臺俯仰運動，該坐標(biāo)系與{R}坐標(biāo)系的俯仰角為時變的(該俯仰角可以通過讀取云臺的俯仰角精確獲取)；{L}激光測距儀坐標(biāo)系，該坐標(biāo)系與俯仰擺臂固連，坐標(biāo)系原點為激光測距儀光源光心。三個坐標(biāo)系都為右手坐標(biāo)系，x軸指向正前方、y軸指向左側(cè)、z軸指向上方。如圖1中給出了{(lán)T}坐標(biāo)系和{L}坐標(biāo)系。

激光測距儀獲取的二維測距信息包括若干個量測點，每個量測點由該點P至激光測距儀光心的距離ρ以及該量測點的角度θ構(gòu)成，即P=(ρ,θ)。該量測點在{L}下的三維坐標(biāo)為：PL=[ρcosθρsinθ]T。那么，激光測距點三維重建即為將PL通過旋轉(zhuǎn)平移轉(zhuǎn)換到{R}坐標(biāo)系下，構(gòu)建一致的環(huán)境模型。

2.2 三維重建過程

設(shè){L}相對于{T}的偏轉(zhuǎn)角為α、俯仰角為β、橫滾角為γ。偏移量分別為xL、yL、zL。那么，將PL轉(zhuǎn)換至{T}的過程可以用以下公式描述：

PT=RZ(α)RY(β)RX(γ)PL+TPLORG

(1)

式中:TPLORG=[xLyLzL]T;RZ、RY、RX分別為繞z、y、x的旋轉(zhuǎn)矩陣。

為簡化運算，將{R}坐標(biāo)系原點設(shè)定與{T}坐標(biāo)系原點重合，{R}坐標(biāo)軸與{T}在俯仰角為0時坐標(biāo)軸重合。{T}坐標(biāo)系相對于{R}坐標(biāo)系僅有俯仰角度變化。那么，將{T}下的坐標(biāo)點PT轉(zhuǎn)換至{R}坐標(biāo)系的過程可以用如下公式描述：

PR=PZ(τ)PT

(2)

式中τ為云臺的俯仰角，可以通過讀取云臺編碼器實時在線獲取。

總結(jié)以上轉(zhuǎn)換過程，由激光測距儀量測點P=(ρ,θ)獲取其在參考坐標(biāo)系下的坐標(biāo)可以用以下公式描述：

PR=RZ(τ)(RZ(α)RY(β)RX(γ)PL+TPLORG)=

(3)

式中：ρ、θ分別為P點距離光心距離及對應(yīng)角度，由激光測距儀獲取其量測值；τ為云臺俯仰角，可以通過讀取云臺編碼器獲取；α、β、γ、xL、yL、zL為激光測距儀相對于{T}坐標(biāo)系的安裝位姿。為了獲取精確的環(huán)境模型，需要通過標(biāo)定得到這6個未知變量。

3 三維激光測距系統(tǒng)標(biāo)定

3.1 三維激光測距系統(tǒng)標(biāo)定方法

為了獲取精確的環(huán)境模型，需要對激光測距儀相對于俯仰坐標(biāo)系的位姿參數(shù)S=(α,β,γ,xL,yL，zL)進行標(biāo)定。

常規(guī)的三維激光測距系統(tǒng)標(biāo)定方法與攝像頭標(biāo)定方法類似，即采用標(biāo)定模板通過構(gòu)建真實環(huán)境與三維測距系統(tǒng)的測距點的對應(yīng)關(guān)系，再通過求解轉(zhuǎn)換矩陣得到三維激光測距系統(tǒng)的待標(biāo)定參數(shù)。由于三維激光測距系統(tǒng)的量測點是稀疏的，難以精確構(gòu)建真實環(huán)境與量測環(huán)境的對應(yīng)關(guān)系，最終導(dǎo)致標(biāo)定結(jié)果不夠精確。針對這一問題，本文提出了基于平面特征的標(biāo)定方法。

(4)

本文采用最小二乘法求解平面，設(shè)平面上的N個點分別為{(x1,y1,z1),…,(xN,yN,zN)}，平面方程為

axi+byi+c=zi

(5)

式中a、b、c為平面的參數(shù)。采用最小二乘法擬合平面[5]來求解該平面參數(shù)：

(6)

(7)

激光測距儀安裝位姿的標(biāo)定過程即為調(diào)整待標(biāo)定參數(shù)使得該偏差代價值最小。本文采用非線性優(yōu)化[6]方法求解待標(biāo)定參數(shù)。

該標(biāo)定過程包括以下幾個步驟：

(1)運行三維測距系統(tǒng)采集原始的傳感器數(shù)據(jù)，包括云臺俯仰角，激光量測點；

(2)測量得到待標(biāo)定參數(shù)初始估計值；

(3)采用公式(3)及當(dāng)前標(biāo)定參數(shù)估計值將傳感器量測點轉(zhuǎn)換到參考坐標(biāo)系下；

(4)采用公式(6)求解平面的參數(shù)；

(5)采用公式(7)求解真實平面與量測平面的偏差代價值，若偏差代價值以及參數(shù)滿足要求則停止迭代，否則繼續(xù)步驟(6)；

(6)采用非線性優(yōu)化方法對待標(biāo)定參數(shù)進行調(diào)整，得到新的標(biāo)定參數(shù)，跳轉(zhuǎn)至步驟(3)。

3.2 三維激光測距系統(tǒng)標(biāo)定實驗

為了對三維激光測距系統(tǒng)進行標(biāo)定，本文選擇室內(nèi)的實驗場景進行傳感器數(shù)據(jù)采集。初始的待標(biāo)定參數(shù)為S0=(0.0,0.0,0.0,0.0,0.1,0.05)，由游標(biāo)卡尺測量可得，其中角度單位為弧度，距離單位為米。由初始參數(shù)得到的三維環(huán)境的點云數(shù)據(jù)如圖3所示。

圖3 初始參數(shù)解算得到的三維環(huán)境點云圖

由于該場景包括各種環(huán)境特征，為了方便標(biāo)定，首先手動從中選取出一個平面上的點云量測點，再基于這些平面點進行標(biāo)定實驗。本文選擇的平面點云為第5-160幀激光測距儀量測幀中的第361-801個量測點，如圖4所示。

本文的標(biāo)定過程在CPU為Intel Core 2 Duo CPU E7500 2.93GHz，內(nèi)存為2.0GB的臺式計算機上基于Matlab實現(xiàn)，采用Matlab的非線性優(yōu)化函數(shù)進行參數(shù)求解，其迭代過程如圖5和圖6所示。

圖4 平面點云數(shù)據(jù)

在非線性優(yōu)化的過程中，待標(biāo)定參數(shù)不斷調(diào)整，其(包括偏轉(zhuǎn)角、俯仰角、橫滾角、x軸向距離、y軸向距離、z軸向距離)調(diào)整過程如圖5所示。偏差值，即平面上點到擬合平面距離平方和的變化過程如圖6所示。考慮到通過游標(biāo)卡尺量測得到的初始參數(shù)(即標(biāo)定參數(shù)既要使偏差值盡量小又要接近于初始參數(shù))，最終三維測距系統(tǒng)的標(biāo)定結(jié)果為

S=(0.0026,0.0,0.0017,0.003,0.1,0.0463)

即，激光測距儀相對于俯仰坐標(biāo)系安裝的偏轉(zhuǎn)角為0.0026弧度，俯仰角為0.0弧度，橫滾角為0.0017弧度，x軸向距離為0.003米，y軸向距離為0.1米，z軸向距離為0.0463米。該組參數(shù)使得偏差代價值由最初的0.7573減小到0.7404，考慮到激光測距儀及云臺的量測誤差，該結(jié)果較為合理。

圖5 待標(biāo)定參數(shù)調(diào)整過程

圖6 偏差代價值變化過程

為了驗證該標(biāo)定結(jié)果，本文采集了多組平面環(huán)境的數(shù)據(jù)，多次重復(fù)進行標(biāo)定實驗，并將該標(biāo)定結(jié)果帶入進行驗證。標(biāo)定前后擬合平面的距離平方和如表1所示。

由于受論文篇幅限制，該表僅列出了實驗中的8個平面的初始代價值以及標(biāo)定后的代價值。不同平面的代價值的差異是由平面大小以及距離激光測距儀的距離不同導(dǎo)致的。從表1中可以發(fā)現(xiàn)，標(biāo)定得到的參數(shù)能夠有效的減小該代價值，盡

可能得到真實的平面。因此，該標(biāo)定結(jié)果可以有效地提高三維激光測距系統(tǒng)的測量精度。

表1 多組平面標(biāo)定前后代價值對比

4 結(jié)束語

針對由二維激光測距系統(tǒng)和云臺組成的三維激光測距系統(tǒng)，介紹了其硬件組成、軟件設(shè)計與實現(xiàn)、三維重建方法，重點研究了基于平面特征的標(biāo)定方法。經(jīng)實驗證明，標(biāo)定得到的參數(shù)能夠有效減小代價值，該標(biāo)定方法能較為精確地實現(xiàn)掃描系統(tǒng)的標(biāo)定。

[1]武漢華工激光工程有限責(zé)任公司.激光技術(shù)在航空航天領(lǐng)域的應(yīng)用[J].光電產(chǎn)品與資訊,2012，3(5)：27-28.

[2]楊洗陳.激光加工機器人技術(shù)及工業(yè)應(yīng)用[J].中國激光，2009,36(11)：2780-2798.

[3]余祖俊，楊婭楠，朱力強.三維激光掃描測量系統(tǒng)標(biāo)定方法研究[J].電子測量與儀器學(xué)報，2007，21(6)：31-35.

[4]項志宇，李斌.基于2D激光雷達(dá)的快速3D測距系統(tǒng)的設(shè)計與實現(xiàn)[J].傳感技術(shù)學(xué)報，2006,19(6)：2638-2642.

[5]劉志平，石林英.最小二乘法原理及其Matlab實現(xiàn)[J]．中國西部科技，2008，17(7)：33-34.

[6]南倩，張秦艷，李金泉,等.工業(yè)機器人的局部標(biāo)定[J].現(xiàn)代電子技術(shù)，2012，2(35)：81-83.

Design,ImplementationandCalibrationof3-DLIDARScanningSystem

GU Xiaojie1，2,BU Chunguang2,CHEN Cheng2,ZHOU Junzhe1

(1.Shenyang Ligong University,Shenyang 110159,China;2.Shenyang Institute of Automation,Chinese Academy of Sciences,Shenyang City,Shenyang 110016,China)

3D lidar scanning system has been widely used and calibration usually adopts standard template method,namely,sensor coordinate can be connected with the reference coordinate according to the known three-dimensional coordinates of the measured points,but low accuracy is its drawback.With regard to a 3D lidar scanning system composed of pan-tilt and lidar,the system components,software design and implementation as well as 3D reconstruction methods are introduced,which focus on the calibration based on plane.The method is to choose a number of points from a plane as the test points to give an initial calibration value,and it aims at finding a suitable calibration value，which makes sure that the distance from true plane to the fitting plane is shortest according to nonlinear optimization based on Matlab simulation.The calibration method is simple，which proves to be able to achieve precise calibration of the 3D lidar scanning systemt.

lidar calibration;plane-fitting;three-dimensional scanning system;robot

2013-11-11

國家863資助項目(2011AA040202)

谷曉杰(1987—)，女，碩士研究生；通訊作者：卜春光(1971—)，男，副研究員，研究方向：自主移動機器人控制與系統(tǒng).

1003-1251(2014)05-0010-05

TP391

A

馬金發(fā))