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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 三維重建方法

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

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

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

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

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

2.2 三維重建過程

設(shè){L}相對(duì)于{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)矩陣。

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

PR=PZ(τ)PT

(2)

式中τ為云臺(tái)的俯仰角，可以通過讀取云臺(tái)編碼器實(shí)時(shí)在線獲取。

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

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

(3)

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

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

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

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

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

(4)

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

axi+byi+c=zi

(5)

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

(6)

(7)

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

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

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

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

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

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

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

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

3.2 三維激光測(cè)距系統(tǒng)標(biāo)定實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

圖6 偏差代價(jià)值變化過程

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

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

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

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

4 結(jié)束語

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

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

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

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

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

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

[6]南倩，張秦艷，李金泉,等.工業(yè)機(jī)器人的局部標(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資助項(xiàng)目(2011AA040202)

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

1003-1251(2014)05-0010-05

TP391

A

馬金發(fā))