全思博 李偉光 鄭少華

(華南理工大學(xué) 機(jī)械與汽車工程學(xué)院，廣東 廣州510640)

自動引導(dǎo)小車(AGV)作為制造企業(yè)內(nèi)的現(xiàn)代物流系統(tǒng)的重要組成部分，其應(yīng)用越來越廣泛.除了使用磁導(dǎo)航等方式遵循固定線路運(yùn)行的AGV 之外，許多先進(jìn)制造企業(yè)當(dāng)前迫切需要能在車間內(nèi)自由路徑自主運(yùn)行的智能AGV，這就需要AGV 能夠根據(jù)自帶傳感器感知所處環(huán)境的空間信息，對運(yùn)行場景進(jìn)行地圖構(gòu)建.

AGV 屬于移動機(jī)器人中的一類，關(guān)于移動機(jī)器人二維環(huán)境地圖構(gòu)建方法的研究已有相對豐富的成果.文獻(xiàn)［1］利用聲納創(chuàng)建了室內(nèi)環(huán)境的柵格地圖，文獻(xiàn)［2］從激光雷達(dá)數(shù)據(jù)中提取直線特征作為環(huán)境描述主要特征，并建立了比柵格地圖數(shù)據(jù)量更小的二維特征地圖.但隨著移動機(jī)器人的研究領(lǐng)域從結(jié)構(gòu)化場景向非結(jié)構(gòu)化的復(fù)雜場景拓展，二維特征地圖無法描述不同高度層面的環(huán)境特征，因此三維場景建模成為當(dāng)前自主移動機(jī)器人環(huán)境感知研究的熱點.然而三維環(huán)境信息的獲取、海量數(shù)據(jù)的實時記錄、傳輸和三維重建計算處理等，形成了一系列新問題.

已有的三維場景重建方法主要有基于激光掃描和基于視覺技術(shù)兩類，但是針對室內(nèi)大范圍的三維場景重建問題，這兩類方法都存在很大的局限性.三維激光掃描雖然可以獲得較高精度的幾何模型，但因無法映射紋理而難以生成具有真實感的三維模型［3-4］，同時三維激光掃描設(shè)備價格高昂，尚難大規(guī)模在一般工業(yè)和民用領(lǐng)域普及. 綜合運(yùn)用圖像處理和視覺計算等技術(shù)進(jìn)行非接觸三維測量是計算機(jī)視覺的一個重要研究方向，但是主要適用于小場景固定物體［5-6］，而對于大空間環(huán)境的三維重建尤其是快速實時建模尚無能為力.

近年來，基于以Kinect 為代表的RGB-D 傳感器的出現(xiàn)引起各國學(xué)者的廣泛興趣，并應(yīng)用于三維建模，如Henry 等［7］最早提出了利用RGB-D 傳感器進(jìn)行環(huán)境建模的基本原理，隨后Newcombe 等［8］提出了使用幀到模型配準(zhǔn)策略的KinectFusion 算法，Zeng 等［9］用八叉樹空間表示方法提高了KinectFusion 的空間利用率，使其能夠適應(yīng)更大場景.目前基于RGB-D 傳感器的室外移動機(jī)器人較簡單環(huán)境地形的識別、室內(nèi)單一物體的三維重建的研究已取得了一些成果［10-11］，但在室內(nèi)復(fù)雜場景重建方面的研究尚處于起步階段. 制造車間這樣大空間非結(jié)構(gòu)化復(fù)雜環(huán)境的三維重建是實現(xiàn)AGV 自由路徑自主運(yùn)行的基礎(chǔ)，文中以Kinect 為主要傳感器，結(jié)合機(jī)器人開源操作系統(tǒng)ROS 構(gòu)建了軟硬件集成系統(tǒng)，用于車間環(huán)境自動連續(xù)掃描與三維建模實驗，探討了掃描策略，并分析掃描與重建的耗時與精度.

1 基于RGB-D 相機(jī)的三維重建原理

1.1 Kinect 介紹

圖1 所示的Kinect 本是微軟公司在2010年末為其Xbox 360 游戲機(jī)推出的體感附件，但隨后被世界各地研究者和設(shè)計師用于各類創(chuàng)意開發(fā). 與激光雷達(dá)相比，Kinect 可以方便地對場景同時采集彩色(RGB)和深度(depth)信息，且成本低廉，因此成為當(dāng)前最熱門的3D 傳感設(shè)備［12］.

Kinect 主要由彩色攝像頭、紅外攝像頭、紅外散射光斑(微軟稱之為光編碼)發(fā)射器構(gòu)成.紅外攝像頭通過對發(fā)射器投射的光編碼圖像進(jìn)行解碼來獲取場景的三維信息. 彩色攝像頭的分辨率及視場大小分別為640×480 和63°×50°，紅外攝像頭則為320 ×240 和57°×45°，兩者幀率都為30 f/s，深度數(shù)據(jù)的有效范圍為0.8 ～4.0 m.在通信接口方面，Kinect 采用了USB 2.0 進(jìn)行數(shù)據(jù)傳輸.

圖1 Kinect 示意圖Fig.1 Schematic diagram of Kinect

Kinect 可以采用微軟Kinect SDK 或開源的OpenNI 庫進(jìn)行開發(fā)，前者僅適用于Windows 環(huán)境;而后者在Windows 和Linux 環(huán)境下都可以使用. 為了配合后文所述的ROS 框架，本系統(tǒng)采用OpenNI作為開發(fā)中間件.

1.2 三維重建原理和方法

1.2.1 點云數(shù)據(jù)獲取與拼接融合

Kinect 的RGB 攝像頭和紅外攝像頭分別攝取目標(biāo)物體的RGB 圖像和深度圖像.建立深度圖像的圖像坐標(biāo)系xy 與Kinect 攝像頭動態(tài)參考坐標(biāo)系XYZ，將深度圖像的x、y 軸坐標(biāo)轉(zhuǎn)換到參考系的X、Y 軸坐標(biāo)，Z 為Kinect 獲取的深度值，即可獲得三維點云數(shù)據(jù)(X，Y，Z).

上述兩坐標(biāo)系之間的對應(yīng)關(guān)系為［13］

式中，fx、fy分別為紅外相機(jī)對應(yīng)方向上的焦距，cx、cy為紅外相機(jī)主點坐標(biāo). 從上式可推出其逆運(yùn)算關(guān)系式:

隨著傳感器移動而從不同視角掃描，新的數(shù)據(jù)將先前圖像中的空洞逐漸填充. 通過圖像幀間的點云配準(zhǔn)算法計算，可以得到攝像頭移動時的位姿變化軌跡以及當(dāng)前攝像頭相對于起始幀時攝像頭的相對位姿變換關(guān)系.將深度圖像序列進(jìn)行濾波去噪并去除移動物體干擾后根據(jù)相應(yīng)的位姿變換關(guān)系即可融合拼接出整個場景.

在現(xiàn)實空間根據(jù)需要選取適當(dāng)位置為原點建立世界坐標(biāo)系XwYwZw. Kinect 掃描與圖像拼接完成后，將攝像頭XYZ 參考坐標(biāo)系下的點云數(shù)據(jù)通過旋轉(zhuǎn)和平移矩陣轉(zhuǎn)換到XwYwZw世界坐標(biāo)系，最終得到世界坐標(biāo)系下的場景表達(dá).

1.2.2 紋理映射

為了消除由于Kinect 兩個攝像頭位置不同而產(chǎn)生的圖像中心不一致，需要將深度圖像經(jīng)過坐標(biāo)變換與對應(yīng)的RGB 圖像對齊.Kinect 掃描與圖像拼接完成后，從RGB 攝像頭視點位置對重建物體進(jìn)行光線投射，對重建的點陣云產(chǎn)生渲染效果. RGB 攝像頭與紅外攝像頭對應(yīng)點之間的映射關(guān)系為［13］

式中:Xrgb、Yrgb、Zrgb分別是空間某點在RGB 攝像頭坐標(biāo)系下的坐標(biāo);Xir、Yir、Zir分別是該點在紅外攝像頭坐標(biāo)系下的坐標(biāo);R 和T 為RGB 攝像頭和紅外攝像頭之間的變換矩陣.

本研究的目的不是對空間物體表面進(jìn)行精細(xì)識別，而是對車間宏觀三維場景進(jìn)行重建，故可忽略紋理映射細(xì)節(jié)精度.

1.2.3 掃描策略

RGB-D 攝像頭視野和視距有限，所獲取的單幀圖像只能涵蓋小范圍空間，若進(jìn)行車間大空間場景三維建模，需要對不同位置下所獲取大量圖像場景進(jìn)行配準(zhǔn)拼接，因此產(chǎn)生了掃描規(guī)劃問題，即如何采用合理的掃描策略以獲取覆蓋全場的信息，并滿足大空間場景圖像序列匹配的要求. 特別是在未知環(huán)境中，沒有任何先驗信息，掃描規(guī)劃難度更大.

文中使用移動機(jī)器人搭載Kinect 進(jìn)行自動連續(xù)掃描. 掃描規(guī)劃考慮以下4個方面的約束［14］:①Kinect沿掃描軌跡進(jìn)行圖像采集，其有效視野盡可能覆蓋全場而不留盲區(qū);②機(jī)器人在移動過程中，不能碰撞到障礙物;③掃描路徑要有足夠的閉環(huán)精度以保證從圖像序列進(jìn)行大場景三維建模的精度;④在滿足前三者的前提下，使整個掃描路徑盡可能短，盡量減小圖像處理數(shù)據(jù)量和機(jī)器人執(zhí)行時間.

前面提到RGB 攝像頭與紅外攝像頭的視角并不完全一致，因此計算有效掃描范圍時，以較小的紅外攝像頭視角進(jìn)行計算，即有效水平視角57°，有效垂直視角45°.掃描時機(jī)器人做連續(xù)水平運(yùn)動，選取合適的機(jī)器人移動速度和圖像采集幀率使得相鄰幀圖像在水平方向上有一定重疊區(qū)域.由于AGV 高度通常很低(潛入式AGV 一般只有30 cm 高)，加上承載的物料高度也低于1 m，因此文中規(guī)劃掃描范圍時，只考慮獲取這個高度以下的場景信息，以節(jié)省大量數(shù)據(jù)存儲空間并極大降低計算量. 將Kinect 傳感器向下傾斜安裝，使其垂直視角的上沿與地面平行，其掃描范圍垂直截面如圖2 中陰影部分所示，其中h 為Kinect 的安裝高度，設(shè)為1 m;θ 為Kinect 有效垂直視角，45°;［l，L］為Kinect 有效深度測量范圍，考慮Kinect 的掃描精度隨視距增大而逐漸降低，將掃描截止視距定為5 m，因此有效深度(單位為m)測量范圍為［0.4，5.0］;［z，Z］為Kinect 在地面的掃描區(qū)域范圍，由圖中幾何關(guān)系可算出其值為［1.0，4.9］(單位為m).

圖2 Kinect 的工作俯視角Fig.2 The working depression angle of Kinect

有兩種掃描方式可以實現(xiàn)對室內(nèi)大場景的全覆蓋:一是沿直線移動連續(xù)掃描，二是旋轉(zhuǎn)掃描與移動掃描相結(jié)合.前一種方式移動機(jī)器人沿由多條直線段組成的閉合路徑移動，Kinect 掃描方向保持與移動方向一致或呈固定角度(即Kinect 兩傳感器連線與機(jī)器人移動方向垂直或呈一水平面夾角)，這樣掃描速度快，但是因目標(biāo)物遮擋，固定角度直線掃描所獲得的目標(biāo)物信息量少，尤其是傳感器背面的信息幾乎都缺失.后一種方式移動機(jī)器人先以一定的角速度原地旋轉(zhuǎn)，帶動Kinect 旋轉(zhuǎn)掃描采集圖像，然后機(jī)器人直線移動到下一個旋轉(zhuǎn)掃描點，移動過程中Kinect 也同時采集圖像，以彌補(bǔ)機(jī)器人和傳感器本身占位所造成的旋轉(zhuǎn)圓內(nèi)的信息空白;到達(dá)下一個預(yù)定旋轉(zhuǎn)中心位置后，再次旋轉(zhuǎn)掃描，以此類推，直到全景覆蓋.

2 移動機(jī)器人硬軟件平臺設(shè)計

為了實現(xiàn)Kinect 自動連續(xù)圖像采集，文中設(shè)計了一套具有軟硬件可編程、靈活性強(qiáng)、模塊化、易擴(kuò)展、實時性強(qiáng)的移動機(jī)器人軟硬件平臺.

2.1 移動機(jī)器人及機(jī)電系統(tǒng)

移動機(jī)器人底盤采用4個獨(dú)立驅(qū)動的麥克納姆輪來實現(xiàn)任意方位的移動，每個電機(jī)都配備了旋轉(zhuǎn)編碼器進(jìn)行脈沖計數(shù)，實現(xiàn)里程計的功能以及閉環(huán)控制.

圖3 給出了移動機(jī)器人硬件構(gòu)成示意圖.其中，移動機(jī)器人內(nèi)部硬件系統(tǒng)由嵌入式主控計算機(jī)、電機(jī)驅(qū)動器及相關(guān)傳感器組成. 主控計算機(jī)采用基于NVIDIA K1 Soc 移動處理器的TK1 嵌入式開發(fā)平臺，整合了四核Cortex-A15 的CPU 與192個CUDA核心的GPU，具有強(qiáng)大的移動計算能力(峰值可達(dá)326 GFLOPS)和良好的硬件靈活性，可滿足移動機(jī)器人視覺計算的需求. Kinect 通過USB 接口連接主控計算機(jī)，負(fù)責(zé)環(huán)境信息的采集.底盤的電機(jī)驅(qū)動器通過串口接收主控計算機(jī)的運(yùn)動控制指令，同時將實時采集得到的電機(jī)編碼器數(shù)據(jù)發(fā)送給主控計算機(jī).IMU 慣性測量單元作為輔助定位傳感器，也通過串口連接至主控計算機(jī).

圖3 移動機(jī)器人硬件構(gòu)成Fig.3 Hardware component of the mobile robot

2.2 ROS 移動機(jī)器人平臺

底層軟件平臺采用開源ROS. 它不是完整的操作系統(tǒng)，而是一個面向機(jī)器人開發(fā)的分布式軟件架構(gòu).由于其開源特性，研究者在這個框架下利用相應(yīng)工具和庫函數(shù)不斷為ROS 社區(qū)貢獻(xiàn)并完善了大量各種機(jī)器人操作和應(yīng)用的程序代碼包. 代碼復(fù)用極大提高了機(jī)器人開發(fā)的效率，使得ROS 已經(jīng)逐步成為機(jī)器人研發(fā)領(lǐng)域的通用性軟件平臺［15-16］.

然而，ROS 的絕大多數(shù)功能原本是基于PC 機(jī)而設(shè)計，在中小型移動機(jī)器人的開發(fā)應(yīng)用中ROS 尚有難度，尤其是在嵌入式系統(tǒng)中使用. 為此，文中將ROS 內(nèi)核移植到一套移動機(jī)器人嵌入式系統(tǒng)中，搭建起一套低成本、高性能的ROS 移動機(jī)器人軟硬件平臺，用于車間環(huán)境的三維信息掃描和重建以及進(jìn)一步的AGV 定位與導(dǎo)航.

圖4 為AGV-ROS 平臺架構(gòu)，該平臺包括遠(yuǎn)程節(jié)點和AGV 節(jié)點.遠(yuǎn)程端節(jié)點負(fù)責(zé)監(jiān)控調(diào)度及信息存儲，其中手持終端主要用于現(xiàn)場調(diào)試;AGV 端節(jié)點負(fù)責(zé)數(shù)據(jù)采集、運(yùn)算和運(yùn)動控制，AGV 端節(jié)點與遠(yuǎn)程端節(jié)點通過無線網(wǎng)絡(luò)進(jìn)行通訊. 軟件系統(tǒng)根據(jù)功能劃分為運(yùn)動控制、信息采集、人機(jī)交互、通信以及數(shù)據(jù)運(yùn)算5個模塊.

該系統(tǒng)基于嵌入式Linux，并對Linux 內(nèi)核添加實時補(bǔ)丁，從而提高ROS 單機(jī)節(jié)點的實時性，并利用DDS 協(xié)議提高ROS 節(jié)點間網(wǎng)絡(luò)消息傳遞的實時性.在數(shù)據(jù)計算方面，使用NVIDIA CUDA 框架，利用TK1 開發(fā)平臺中的192個核心GPU 進(jìn)行硬件加速，實現(xiàn)3D 地圖的在線構(gòu)建，CPU 則負(fù)責(zé)信息采集、運(yùn)動控制與調(diào)度通訊等功能.

圖4 AGV-ROS 平臺架構(gòu)Fig.4 AGV-ROS platform architecture

3 三維掃描和重建實驗

3.1 實驗場地

文中實驗場地選在一個金工實習(xí)車間，實際探測其車床集群局部，與中央通道呈斜角安裝的數(shù)臺車床彼此平行，通道與車床之間豎有教學(xué)指示牌隔離.圖5 為場景照片.

圖5 車間局部實景Fig.5 Local scene of manufacturing plant scene

3.2 掃描方式與重建結(jié)果

前文所述ROS 移動機(jī)器人平臺按照圖2 設(shè)定Kinect 俯視工作角及掃描截面，然后分別采用直線掃描和旋轉(zhuǎn)加直線掃描方式，繞機(jī)床集群進(jìn)行自動掃描，再進(jìn)行三維重建.

首先是直線掃描.圍繞3 臺車床順時針連續(xù)行進(jìn)4 段直線路程，其掃描和重建結(jié)果如圖6 所示(可掃到6 臺機(jī)床). 從圖中可以看出機(jī)床本身以及機(jī)床之間的空間關(guān)系，建模精度誤差為厘米數(shù)量級(圖6 中A 點為機(jī)床側(cè)面一輔助支架下端，實際距地面40 cm，掃描重建測量為41 cm;B、C 為兩機(jī)床底座拐點，兩點實際距離232 cm，掃描重建測量為234 cm).但由于機(jī)床自身遮擋，造成機(jī)床側(cè)后向大塊區(qū)域信息缺失.不過這樣的掃描方式花費(fèi)時間少，完成一圈掃描僅需3 min，并且缺失的信息不會對AGV 運(yùn)行路線范圍的地圖構(gòu)建造成影響.

圖6 圍繞3 臺機(jī)床的直線掃描與重建結(jié)果Fig.6 The result of linear scanning and modeling of three machine tools

在此基礎(chǔ)上，為獲取更豐富的三維信息，適當(dāng)增加旋轉(zhuǎn)掃描. 如圖7 所示，在圖6 場景的直線路徑中，當(dāng)機(jī)器人走到每兩臺機(jī)床之間時插入一個旋轉(zhuǎn)運(yùn)動.很顯然，這樣所得的三維信息比圖6 更完整，不再留下如圖6 中的信息盲區(qū)，如圖7(a)和7(b)所示，不僅機(jī)床這樣的大物體，就連教學(xué)指示牌支腳這樣小的障礙目標(biāo)也能清晰呈現(xiàn).但是，加上旋轉(zhuǎn)掃描后整個行程耗時延長，甚至翻番.

圖7 直線掃描混合旋轉(zhuǎn)掃描的軌跡和重建結(jié)果Fig.7 Track and 3D modeling using linear scanning combined with rotating scanning

實驗過程中，移動機(jī)器人、Kinect 和遠(yuǎn)程PC 通過無線網(wǎng)絡(luò)通訊順暢，機(jī)器人直線運(yùn)行和拐角轉(zhuǎn)向以及旋轉(zhuǎn)控制良好，掃描數(shù)據(jù)讀取和記錄完整，表明所構(gòu)建的ROS 移動機(jī)器人平臺可以很好地協(xié)調(diào)運(yùn)行，并用于車間環(huán)境的三維重建.

4 結(jié)語

面向制造企業(yè)生產(chǎn)車間的大而復(fù)雜的環(huán)境，利用RGB-D 深度傳感器，結(jié)合移動機(jī)器人及開源ROS建構(gòu)的三維掃描和建模系統(tǒng)具有良好的集成性和可操作性，能夠用于車間環(huán)境自動連續(xù)掃描、點云數(shù)據(jù)拼接、三維建模，具有輕巧、快速、成本低的特點，可達(dá)到厘米級重建精度. 該系統(tǒng)在對大場景三維信息自動連續(xù)獲取和重建時，直線掃描方式能快速獲得重建結(jié)果，但是會留下信息盲區(qū);直線掃描加旋轉(zhuǎn)掃描方式可以獲得豐富的信息和良好的重建結(jié)果，但是相對費(fèi)時.文中僅探討了利用單臺AGV 對車間場景三維建模，如何提高建模的魯棒性以及利用多AGV 系統(tǒng)協(xié)作提高建模效率還需要進(jìn)一步的研究.

［1］李潤偉.基于聲納的室內(nèi)環(huán)境柵格地圖創(chuàng)建方法的研究［D］.長沙:中南大學(xué)計算機(jī)科學(xué)與技術(shù)系，2008.

［2］朱建國，高峻峣，李科杰，等. 室內(nèi)未知環(huán)境下移動機(jī)器人特征地圖創(chuàng)建研究［J］. 計算機(jī)測量與控制，2011，19(12):3044-3046.Zhu Jian-guo，Gao Jun-yao，Li Ke-jie，et al. Research on feature map building of mobile robot in indoor unknown environment［J］.Computer Measurement ＆ Control，2011，19(12):3044-3046.

［3］林鴻生. 成像激光雷達(dá)與攝像機(jī)的圖像融合研究［D］.長沙:國防科學(xué)技術(shù)大學(xué)光學(xué)工程系，2007.

［4］Burcin B G，Jazizadeh F，Kavulya G.Assessment of target types and layouts in 3D laser scanning for registration accuracy［J］. Automation in Construction，2011，20(5):649-658.

［5］楊明.基于雙目立體視覺的三維重建研究［D］.武漢:武漢工程大學(xué)智能科學(xué)與技術(shù)系，2012.

［6］章秀華，白浩玉，李毅.多目立體視覺三維重建系統(tǒng)的設(shè)計［J］.武漢工程大學(xué)學(xué)報，2013，35(3):70-74.Zhang Xiu-hua，Bai Hao-yu，Li Yi. Design of 3D reconstruction system based on multi-view stereo vision ［J］.Journal of Wuhan Institute of Technology，2013，35(3):70-74.

［7］Henry P，Krainin M，Herbst E，et al. RGB-D mapping:using Kinect-style depth cameras for dense 3D modeling of indoor environments［J］. The International Journal of Robotics Research，2012(5):647-663.

［8］Newcombe R A，Davison A J，Izadi S，et al.KinectFusion:real-time dense surface mapping and tracking［C］∥Proceedings of the 2011 IEEE International Symposium on Mixed and Augmented Reality.Basel:IEEE，2011:127-136.

［9］Zeng M，Zhao F，Zheng J，et al. A memory-efficient KinectFusion using octree［C］∥Proceedings of the 1st International Conference on Computational Visual Media.Beijing:［s.n.］，2012:234-241.

［10］李國鎮(zhèn).基于Kinect 的三維重建方法的研究與實現(xiàn)［D］.北京:北京交通大學(xué)軟件工程系，2012.

［11］陳曉明，蔣樂天，應(yīng)忍冬. 基于Kinect 深度信息的實時三維重建和濾波算法研究［J］.計算機(jī)應(yīng)用研究，2013，30(4):1216-1218.Chen Xiao-ming，Jiang Le-tian，Ying Ren-dong.Research of 3D reconstruction and filtering algorithm based on depth information of Kinect［J］.Application Research of Computers，2013，30(4):1216-1218.

［12］El-Laithy R A，Huang J D，Yeh M. Study on the use of Microsoft Kinect for robotics applications［C］∥Proceedings of the 2012 IEEE/ION Position，Location and Navigation Symposium.Myrtle Beach:IEEE，2012:1280-1288.

［13］Bethencourt A，Jaulin L.3D reconstruction using interval methods on the Kinect device coupled with an IMU［J］.International Journal of Advanced Robotic Systems，2013，10(2):1-10.

［14］王鋒.面向服務(wù)機(jī)器人的室內(nèi)語義地圖構(gòu)建的研究［D］.合肥:中國科學(xué)技術(shù)大學(xué)計算機(jī)科學(xué)與技術(shù)學(xué)院，2013.

［15］Cousins S.Exponential growth of ROS［J］.IEEE Robotics and Automation Magazine，2011，18(1):19-20.

［16］Reid R，Cann A，Meiklejohn C. Cooperative multi-robot navigation，exploration，mapping and object detection with ROS ［C］∥Proceedings of 2013 IEEE Intelligent Vehicles Symposium.Gold Coast:IEEE，2013:1083-1088.