蔣玉林, 楊 炯, 梁 杰, 趙建國(guó)

(鄭州大學(xué)機(jī)械與動(dòng)力工程學(xué)院， 鄭州 450001)

近年來(lái)，眾多學(xué)者們對(duì)計(jì)算機(jī)視覺(jué)領(lǐng)域不斷深入研究，基于激光測(cè)量技術(shù)的測(cè)繪方法和三維重建方法成了各行各業(yè)的熱門研究方向。隧道測(cè)量是其中一個(gè)重要的應(yīng)用場(chǎng)景，為了提高測(cè)量效率和精度，一般利用車載激光雷達(dá)對(duì)隧道斷面進(jìn)行多次掃描，建立整個(gè)隧道的三維立體模型，再對(duì)比分析實(shí)際數(shù)據(jù)與測(cè)量的誤差，從而獲得隧道的形變信息[1]。在三維重建領(lǐng)域，如何快速、準(zhǔn)確地獲取目標(biāo)物體的三維數(shù)字化模型是該領(lǐng)域的重要研究任務(wù)，學(xué)者們做了大量的研究，提出了多種重建方法，這些方法中使用的傳感器主要有兩種基于視覺(jué)傳感器[2]和激光雷達(dá)傳感器[3]。視覺(jué)傳感器主要獲取目標(biāo)物體三維深度信息，但數(shù)據(jù)量大、處理復(fù)雜且精度低。激光雷達(dá)傳感器測(cè)量精度高，采集速度快，但是三維激光雷達(dá)價(jià)格昂貴，因此大量研究人員多以二維激光雷達(dá)為基礎(chǔ)，額外增加機(jī)械結(jié)構(gòu)，以實(shí)現(xiàn)三維雷達(dá)的掃描功能。

Liu等[4]提出了利用移動(dòng)設(shè)備上的RGB-D傳感器和慣性測(cè)量單元(inertial measurement unit，IMU)對(duì)動(dòng)態(tài)場(chǎng)景進(jìn)行掃描的方法，將輸入的深度圖像分割成靜態(tài)或動(dòng)態(tài)，并將深度圖像集成到基于體素的表示中，實(shí)驗(yàn)結(jié)果表明該方法動(dòng)態(tài)場(chǎng)景重建效果好。陳穎等[5]以雙目相機(jī)和電動(dòng)云臺(tái)搭建立體視覺(jué)系統(tǒng)，通過(guò)雙目相機(jī)獲取場(chǎng)景的點(diǎn)云信息，然后通過(guò)特征匹配和點(diǎn)云配準(zhǔn)融合實(shí)現(xiàn)對(duì)室內(nèi)全景的重建。由于視覺(jué)方法受環(huán)境影響較大，動(dòng)態(tài)性低，而且容易產(chǎn)生累計(jì)誤差，所以精度較低。鄭道嶺[6]提出了一種基于多傳感器融合的重建方法,利用激光雷達(dá)獲取場(chǎng)景深度信息值，視覺(jué)進(jìn)行運(yùn)動(dòng)估計(jì)和三維建圖，將兩者信息進(jìn)行融合極大地提升了建圖性能，但是傳感器的標(biāo)定和數(shù)據(jù)的融合變得十分困難。段清明等[7]使用2D激光雷達(dá)和數(shù)碼舵機(jī)配合搭建旋轉(zhuǎn)云臺(tái)作為三維數(shù)據(jù)采集裝置，該方法可以實(shí)現(xiàn)對(duì)掃描范圍和精度的控制，但是掃描范圍有限且全局坐標(biāo)標(biāo)定變得困難。

現(xiàn)主要針對(duì)廊道、隧道測(cè)量等工業(yè)應(yīng)用，在前人的研究基礎(chǔ)之上，對(duì)現(xiàn)有方法進(jìn)行改進(jìn)，提出了一種基于單線激光雷達(dá)的數(shù)字重構(gòu)系統(tǒng)。該系統(tǒng)以移動(dòng)機(jī)器人為移動(dòng)平臺(tái)，搭載單線激光雷達(dá)傳感器，實(shí)現(xiàn)三維雷達(dá)掃描功能，可以完成廊道環(huán)境的自動(dòng)化數(shù)據(jù)采集和實(shí)時(shí)場(chǎng)景重構(gòu)。為了保證數(shù)據(jù)采集精度，首先要解決移動(dòng)機(jī)器人運(yùn)動(dòng)偏離的問(wèn)題，本系統(tǒng)采用一種模糊PID閉環(huán)控制，對(duì)移動(dòng)機(jī)器人實(shí)現(xiàn)精確控制。針對(duì)2D點(diǎn)云到3D點(diǎn)云轉(zhuǎn)換問(wèn)題，提出了一種里程計(jì)數(shù)據(jù)和激光雷達(dá)傳感器數(shù)據(jù)融合的方法。最后通過(guò)MATLAB中的點(diǎn)云可視化模塊實(shí)現(xiàn)三維點(diǎn)云的可視化，完成廊道環(huán)境的數(shù)字重構(gòu)。以廊道和管道為實(shí)驗(yàn)場(chǎng)景進(jìn)行自動(dòng)化數(shù)據(jù)采集和數(shù)字重構(gòu)，并與實(shí)驗(yàn)場(chǎng)景進(jìn)行對(duì)比。

1 系統(tǒng)硬件構(gòu)成

本文中設(shè)計(jì)的數(shù)字重構(gòu)系統(tǒng)硬件構(gòu)成示意圖如圖1所示。系統(tǒng)可分為3個(gè)主要模塊：①雷達(dá)掃描模塊；②移動(dòng)機(jī)器人控制模塊；③控制與數(shù)據(jù)采集單元模塊。雷達(dá)掃描模塊主要由一個(gè)單線激光雷達(dá)構(gòu)成。移動(dòng)機(jī)器人控制模塊是以Arduino mega單片機(jī)為主要核心的控制系統(tǒng)組成?？刂婆c數(shù)據(jù)采集模塊通過(guò)串口將運(yùn)動(dòng)指令發(fā)送給單片機(jī)系統(tǒng)，單片機(jī)系統(tǒng)接收數(shù)據(jù)并運(yùn)用下文中的運(yùn)動(dòng)學(xué)模型算法式(1)解算出各個(gè)輪子的轉(zhuǎn)速和方向，然后把解算數(shù)據(jù)以脈沖寬度調(diào)制(pulse width modulation，PWM)信號(hào)的形式發(fā)送給電機(jī)驅(qū)動(dòng)器，控制電機(jī)轉(zhuǎn)速。電機(jī)、電機(jī)驅(qū)動(dòng)器以及編碼器形成了一個(gè)閉環(huán)控制系統(tǒng)，其閉環(huán)控制的反饋量來(lái)自于4個(gè)編碼器測(cè)得的電機(jī)轉(zhuǎn)速?？刂婆c數(shù)據(jù)采集單元模塊由一個(gè)小型筆記本電腦構(gòu)成，搭載在移動(dòng)機(jī)器人上。單片機(jī)和控制與數(shù)據(jù)采集單元模塊之間通過(guò)串口實(shí)現(xiàn)數(shù)據(jù)交流。主要任務(wù)包括：

圖1 系統(tǒng)硬件構(gòu)成示意圖Fig.1 System hardware structure diagram

(1)接收控制與數(shù)據(jù)采集模塊發(fā)送的控制命令，如啟動(dòng)、停止、速度反饋調(diào)節(jié)等等，單片機(jī)系統(tǒng)向控制與數(shù)據(jù)采集模塊實(shí)時(shí)發(fā)送由編碼器采集的機(jī)器人里程，發(fā)送頻率為100 ms/次，大于雷達(dá)掃描模塊轉(zhuǎn)動(dòng)一圈的時(shí)間125 ms。

(2)控制與數(shù)據(jù)采集模塊采用多線程并行的方式，一個(gè)線程負(fù)責(zé)通過(guò)串口接收雷達(dá)采集到的角度距離值，另一線程不斷接收單片機(jī)系統(tǒng)發(fā)送的機(jī)器人里程計(jì)，將兩份數(shù)據(jù)同時(shí)存儲(chǔ)起來(lái)，這樣就得到每一幀激光雷達(dá)數(shù)據(jù)和該時(shí)刻對(duì)應(yīng)的機(jī)器人位姿信息[8]，最后使用坐標(biāo)變換公式[式(2)]計(jì)算出所有激光點(diǎn)在三維空間中的坐標(biāo)信息。

2 系統(tǒng)數(shù)字重構(gòu)過(guò)程

如圖2所示為數(shù)字重構(gòu)系統(tǒng)處理過(guò)程流程圖，單片機(jī)系統(tǒng)接收編碼器獲取到的數(shù)據(jù)，并對(duì)其進(jìn)行計(jì)算處理，得到4輪角速度信息，通過(guò)串口將數(shù)據(jù)上傳到控制與數(shù)據(jù)采集單元模塊進(jìn)行運(yùn)動(dòng)解算，得出該時(shí)刻移動(dòng)機(jī)器人的里程信息。上位機(jī)以多線程并存的方式運(yùn)行，一個(gè)線程讀取單線激光雷達(dá)采集到的角度距離信息，另一線程接收單片機(jī)系統(tǒng)上傳的機(jī)器人里程計(jì)?？刂婆c數(shù)據(jù)采集單元模塊接收激光雷達(dá)采集到的環(huán)境角度距離信息并以幀為最小單位存儲(chǔ)，里程計(jì)按照時(shí)間順序存儲(chǔ)容器中。將每一幀原始點(diǎn)云和與之對(duì)應(yīng)的里程計(jì)值進(jìn)行數(shù)據(jù)融合，并運(yùn)用線性插值方法求解該幀點(diǎn)云中的所有數(shù)據(jù)點(diǎn)對(duì)應(yīng)的里程計(jì)值，也就是將每一幀所有激光點(diǎn)轉(zhuǎn)換到里程計(jì)坐標(biāo)系下，再把里程計(jì)坐標(biāo)系下的所有激光點(diǎn)轉(zhuǎn)換到世界坐標(biāo)系下，完成所有2D點(diǎn)云到3D點(diǎn)云的坐標(biāo)轉(zhuǎn)換，最后進(jìn)行場(chǎng)景合成實(shí)現(xiàn)廊道環(huán)境的數(shù)字重構(gòu)。

2.1 移動(dòng)機(jī)器人控制模塊設(shè)計(jì)

2.1.1 機(jī)器人控制模塊運(yùn)動(dòng)學(xué)模型[9-10]分析

(1)

式(1)中：ω1、ω2、ω3、ω4為移動(dòng)機(jī)器人4輪角速度；r為麥克納姆輪半徑；a為兩前輪輪心距離的1/2；b為前輪與后輪輪心距離的1/2；vx、vy分別為移動(dòng)機(jī)器人在平面x、y方向上行駛的速度；ωz為移動(dòng)機(jī)器人繞z軸的轉(zhuǎn)動(dòng)角速度。

2.1.2 模糊PID控制器設(shè)計(jì)

移動(dòng)機(jī)器人控制模塊采用基于參數(shù)分配器的模糊PID控制[11]對(duì)整個(gè)控制模塊形成雙閉環(huán)控制，其結(jié)構(gòu)簡(jiǎn)圖如圖3所示。

圖3 移動(dòng)機(jī)器人控制模塊系統(tǒng)結(jié)構(gòu)簡(jiǎn)圖Fig.3 System structure diagram of mobile robot control module

首先控制與數(shù)據(jù)采集單元模塊以報(bào)文的形式將機(jī)器人的運(yùn)動(dòng)控制參數(shù)發(fā)送給單片機(jī)系統(tǒng)，單片機(jī)系統(tǒng)經(jīng)過(guò)分析處理得到相應(yīng)的控制參數(shù)vx、vy、ωz，然后使用式(1)計(jì)算得出機(jī)器人四輪轉(zhuǎn)動(dòng)的角速度，傳送至PID控制器，PID控制器輸出規(guī)定的PWM控制信號(hào)，通過(guò)參數(shù)分配器將各輪轉(zhuǎn)速分配給與其對(duì)應(yīng)的電機(jī)驅(qū)動(dòng)器，進(jìn)行PID調(diào)節(jié)控制，使各輪的轉(zhuǎn)速與預(yù)期速度保持一致。在電機(jī)轉(zhuǎn)動(dòng)的同時(shí)，其尾部安裝的編碼器可以實(shí)時(shí)測(cè)得各輪轉(zhuǎn)速反饋到PID模糊控制器，通過(guò)模糊控制器對(duì)PID控制參數(shù)kp、ki、kd形成不斷的動(dòng)態(tài)修正，對(duì)機(jī)器人控制模塊進(jìn)行PID控制，整個(gè)控制過(guò)程形成了可以進(jìn)行實(shí)時(shí)反饋的雙閉環(huán)控制系統(tǒng)，保證了機(jī)器人在數(shù)據(jù)采集過(guò)程中保持直線運(yùn)動(dòng)狀態(tài)，提高了數(shù)據(jù)采集的準(zhǔn)確性。

2.2 坐標(biāo)轉(zhuǎn)換

因?yàn)閱尉€激光雷達(dá)采集到的數(shù)據(jù)是極坐標(biāo)[12]形式，所以就要把這些數(shù)據(jù)點(diǎn)轉(zhuǎn)換為直角坐標(biāo)形式，設(shè)空間中點(diǎn)云坐標(biāo)形式為(d，α，ρ)，其中d為里程計(jì)傳感器獲得的當(dāng)前激光點(diǎn)在水平方向的里程值，α為單線激光雷達(dá)當(dāng)前的測(cè)量角度，ρ為當(dāng)前激光點(diǎn)到雷達(dá)中心的距離，于是當(dāng)前激光點(diǎn)的三維空間坐標(biāo)為(x,y,z)。

(2)

2.3 數(shù)據(jù)融合

數(shù)字重構(gòu)系統(tǒng)在工作的狀態(tài)下，移動(dòng)機(jī)器人控制模塊由車輪上的編碼器獲取里程計(jì)數(shù)據(jù)，雷達(dá)掃描模塊采集廊道環(huán)境豎直截面的角度距離信息，本文提出了一種里程計(jì)數(shù)據(jù)和激光雷達(dá)傳感器數(shù)據(jù)融合的方法[13]。假設(shè)知道激光雷達(dá)采集到的某一幀數(shù)據(jù)點(diǎn)的開(kāi)始時(shí)刻和結(jié)束時(shí)刻分別為tm、tn，相鄰的兩束激光時(shí)間差值為Δt，單片機(jī)系統(tǒng)上傳的里程計(jì)數(shù)據(jù)根據(jù)時(shí)間的先后順序存儲(chǔ)到容器中，采集到的第一個(gè)里程計(jì)值的時(shí)間戳≤tm，最后一個(gè)里程計(jì)值的時(shí)間戳≥tn。數(shù)據(jù)融合的過(guò)程，就是將該幀的激光數(shù)據(jù)點(diǎn)中所有的激光點(diǎn)找到一個(gè)與之對(duì)應(yīng)的里程計(jì)數(shù)值，也就是計(jì)算{tm,tm+Δt,…,tn}時(shí)刻系統(tǒng)對(duì)應(yīng)的里程數(shù)值，按照里程計(jì)數(shù)據(jù)把采集到的所有數(shù)據(jù)點(diǎn)統(tǒng)一到世界坐標(biāo)系下，最終實(shí)現(xiàn)了二維點(diǎn)云到三維點(diǎn)云的轉(zhuǎn)換，最后利用MATLAB中的點(diǎn)云可視化模塊[14]將坐標(biāo)轉(zhuǎn)換后的三維點(diǎn)云顯示出來(lái)，實(shí)現(xiàn)環(huán)境的數(shù)字重構(gòu)。

(1)計(jì)算tm、tn時(shí)間點(diǎn)機(jī)器人對(duì)應(yīng)的里程計(jì)數(shù)值。如果里程計(jì)容器和雷達(dá)掃描模塊采集的數(shù)據(jù)剛好時(shí)間同步，設(shè)第i和第j個(gè)數(shù)據(jù)點(diǎn)對(duì)應(yīng)的時(shí)刻分別為tm、tn，于是可以求出m、n時(shí)刻對(duì)應(yīng)的里程計(jì)值，可計(jì)算得到

pm=DataContainer(i)

(3)

pn=DataContainer(j)

(4)

(2)里程計(jì)容器與雷達(dá)掃描模塊采集的數(shù)據(jù)不同步，可以設(shè)tm時(shí)刻里程計(jì)隊(duì)列中找不到與之對(duì)應(yīng)的里程計(jì)值，那么我們就需要進(jìn)行線性插值，假設(shè)在里程計(jì)容器中可以找到e、f時(shí)刻的里程值，且e

pe=DataContainer(e)

(5)

pf=DataContainer(f)

(6)

(7)

(8)

(9)

(4)已知pm、ps、pn,可以插值一條二次曲線，即

P(t)=At2+Bt+C,tm≤t≤tn

(10)

(5)接下來(lái)，用分段線性函數(shù)對(duì)二次曲線進(jìn)行近似。如果分段數(shù)不低于3時(shí)，產(chǎn)生的誤差則可忽略。在tm、tn時(shí)間段內(nèi)，共有K個(gè)位姿{(lán)pm,pm+1,…,pm+k-2,pn}，每個(gè)數(shù)據(jù)點(diǎn)對(duì)應(yīng)的里程值可以通過(guò)線性插值的方法計(jì)算?？梢栽O(shè)在pm～pm+1范圍內(nèi)共有N個(gè)里程數(shù)值，可以得到

(11)

(6)每個(gè)激光數(shù)據(jù)對(duì)應(yīng)的里程計(jì)值{p1,p2,…,pn}通過(guò)上述步驟插值得到，將所得到的數(shù)據(jù)代入式(2)，計(jì)算出的數(shù)據(jù)點(diǎn)就是融合后的三維空間點(diǎn)云，最后重新封裝并發(fā)布出去。

3 實(shí)驗(yàn)結(jié)果

3.1 實(shí)驗(yàn)條件

數(shù)字重構(gòu)系統(tǒng)的掃描頻率為8 Hz,即每秒重構(gòu)出8個(gè)豎直的截面。雷達(dá)掃描模塊以2 000次/s的測(cè)距頻率工作，機(jī)器人的移動(dòng)速度為3 m/s。

表1為實(shí)驗(yàn)軟硬件詳細(xì)介紹。為了驗(yàn)證本文設(shè)計(jì)搭建的數(shù)字重構(gòu)系統(tǒng)的可靠性和穩(wěn)定性，分別在兩種實(shí)驗(yàn)環(huán)境中進(jìn)行測(cè)試。

表1 實(shí)驗(yàn)軟硬件詳細(xì)介紹

(1)實(shí)驗(yàn)環(huán)境1為室外的管道，管道內(nèi)壁的長(zhǎng)寬高分別為2.38 m×0.72 m×0.72 m，其測(cè)試實(shí)驗(yàn)場(chǎng)景如圖4(a)所示。

(2)實(shí)驗(yàn)環(huán)境2為室內(nèi)的走廊，廊道環(huán)境的長(zhǎng)寬高分別為7 m×2.22 m×2.53 m，其測(cè)試實(shí)驗(yàn)場(chǎng)景如圖4(c)所示。首先對(duì)數(shù)字重構(gòu)系統(tǒng)進(jìn)行啟動(dòng)通電完成系統(tǒng)初始化，系統(tǒng)開(kāi)始自動(dòng)采集實(shí)驗(yàn)環(huán)境的三維點(diǎn)云數(shù)據(jù)，再使用MATLAB軟件處理采集到數(shù)據(jù)點(diǎn)實(shí)現(xiàn)點(diǎn)云可視化，完成實(shí)驗(yàn)場(chǎng)景的數(shù)字重構(gòu)，最后與實(shí)際實(shí)驗(yàn)場(chǎng)景進(jìn)行對(duì)比。

3.2 實(shí)驗(yàn)結(jié)果分析

數(shù)字重構(gòu)系統(tǒng)在實(shí)驗(yàn)環(huán)境1和實(shí)驗(yàn)環(huán)境2下的重構(gòu)結(jié)果分別如圖4(b)、圖4(d)所示。從圖中可以直觀地看出，實(shí)驗(yàn)環(huán)境1的紋理信息重構(gòu)清晰，實(shí)驗(yàn)環(huán)境2中的門框、消防栓、燈等結(jié)構(gòu)特征信息明顯，重構(gòu)效果好。

圖4 實(shí)際實(shí)驗(yàn)場(chǎng)景和數(shù)字重構(gòu)結(jié)果Fig.4 Actual experimental scenario and digital reconstruction result

如圖表2所示，對(duì)比實(shí)驗(yàn)場(chǎng)景原始數(shù)據(jù)和數(shù)字重構(gòu)結(jié)果可以得出，本文設(shè)計(jì)的數(shù)字重構(gòu)系統(tǒng)實(shí)現(xiàn)了兩種實(shí)驗(yàn)場(chǎng)景的重構(gòu)，數(shù)字重構(gòu)的結(jié)果與實(shí)際數(shù)據(jù)進(jìn)行比較，誤差在2～6 mm，重構(gòu)特征明顯，精度高。

表2 實(shí)驗(yàn)場(chǎng)景原始數(shù)據(jù)與重構(gòu)結(jié)果對(duì)比

4 結(jié) 論

(1)基于單線激光雷達(dá)和移動(dòng)機(jī)器人，設(shè)計(jì)了一個(gè)廊道環(huán)境的數(shù)字重構(gòu)系統(tǒng)。系統(tǒng)以自行設(shè)計(jì)搭建的移動(dòng)機(jī)器人為移動(dòng)平臺(tái)，搭載單線激光雷達(dá)傳感器。介紹了系統(tǒng)軟硬件模塊，提出了一種里程計(jì)數(shù)據(jù)和激光雷達(dá)傳感器數(shù)據(jù)融合的方法，并詳細(xì)介紹了系統(tǒng)數(shù)字重構(gòu)過(guò)程、移動(dòng)機(jī)器人控制模塊、坐標(biāo)轉(zhuǎn)換以及數(shù)據(jù)融合過(guò)程。

(2)通過(guò)實(shí)驗(yàn)測(cè)試，結(jié)果表明，該系統(tǒng)可以實(shí)現(xiàn)對(duì)廊道環(huán)境的數(shù)字重構(gòu)且能較為準(zhǔn)確的重構(gòu)出一般結(jié)構(gòu)的幾何特征，重構(gòu)特征明顯、重構(gòu)精度高且穩(wěn)定性強(qiáng)。