基于深度相機(jī)的小型無人機(jī)室內(nèi)三維地圖構(gòu)建

張陣委，章 偉，龍 林，顏晨航

(上海工程技術(shù)大學(xué) 機(jī)械與汽車工程學(xué)院，上海 201620)

小型無人機(jī)(Micro Aerial Vehicle，MAV)實(shí)時室內(nèi)三維環(huán)境的構(gòu)建已經(jīng)被廣泛應(yīng)用于無人機(jī)避障領(lǐng)域[1]。深度相機(jī)能夠比較準(zhǔn)確地獲取室內(nèi)三維空間信息，可被運(yùn)用于目標(biāo)識別、定位及路徑規(guī)劃等無人機(jī)室內(nèi)導(dǎo)航任務(wù)中。 相較于雙目視覺系統(tǒng)，深度相機(jī)構(gòu)建的障礙物檢測的三維地圖信息，可以為無人機(jī)的控制器提供更準(zhǔn)確的實(shí)時環(huán)境感知[2]。考慮到建筑物內(nèi)部定位系統(tǒng)(Global Positioning System，GPS)信號缺失、光源缺少以及三原色光(Red Green Blue，RGB)雙目相機(jī)比較依賴光源的特點(diǎn)[3]，基于結(jié)構(gòu)光相機(jī)通過激光掃描獲得三維地圖信息是無人機(jī)在室內(nèi)避障環(huán)境中一個非常合理的選擇。

此前有很多利用RGB相機(jī)獲取無人機(jī)當(dāng)前所在環(huán)境的三維地圖信息[4]。Bachrach使用實(shí)時定位傳感和實(shí)時地圖構(gòu)建算法(Simultaneous Localization and Mapping，SLAM)，使用感光相機(jī)和二維激光信息來計算室內(nèi)的二維地圖[5]，進(jìn)而達(dá)到構(gòu)建室內(nèi)地圖的目的。它生成的二維地圖只能應(yīng)用在簡單場景構(gòu)建，如果室內(nèi)場景有形狀輪廓為長條形的障礙物或者光源比較弱[6]，則容易出現(xiàn)誤差和錯誤。 研究人員也曾使用雙目視覺點(diǎn)云拼接技術(shù)來生成現(xiàn)實(shí)環(huán)境的三維地圖，但是它生成的三維點(diǎn)云信息量太過復(fù)雜[7]，很難對它進(jìn)行數(shù)據(jù)分析，且使用這種拼接方式獲取的點(diǎn)云噪聲很大且難以濾除。

考慮成本和平臺特點(diǎn)以及室內(nèi)光源缺少等各方面因素，研究人員使用高性能、低功耗Pixhawk板和消費(fèi)級別的深度相機(jī)(Red Green Blue Depth Map，RGB-D)構(gòu)建了一套低成本的嵌入式視覺硬件平臺[8]。本文使用多傳感器融合技術(shù)及Linux系統(tǒng)搭建硬件的軟件開發(fā)環(huán)境，利用Linux系統(tǒng)的強(qiáng)大硬件開發(fā)功能，使用機(jī)器人操作系統(tǒng)(Robot Operating System，ROS)開發(fā)，并結(jié)合無人機(jī)的當(dāng)前狀態(tài)信息生成了靈活性和魯棒性都較好的地圖構(gòu)建信息。

1 基于深度相機(jī)的圖像處理

點(diǎn)云圖像是三維圖像信息的一種特殊的表達(dá)形式。與普通平面圖像相比，三維圖像是依靠第三維度的信息實(shí)現(xiàn)環(huán)境背景解耦。激光掃描獲取的點(diǎn)云模型中的每個點(diǎn)的信息都可以通過測量得到，每個點(diǎn)是一個測量點(diǎn)，包含了最大的信息量。這些信息都儲存在測量點(diǎn)中。本文采用的是基于結(jié)構(gòu)光相機(jī)來獲取點(diǎn)云信息進(jìn)行處理。

1.1 基于結(jié)構(gòu)光的深度相機(jī)原理

它的原理如圖1所示[9]，E為激光發(fā)射裝置；S為激光反射的接收裝置。

圖1 結(jié)構(gòu)光相機(jī)原理Figure 1. Principle of structured light camera

結(jié)構(gòu)光深度相機(jī)克服了雙目視覺相機(jī)依賴光源的缺點(diǎn)。結(jié)構(gòu)光相機(jī)對圖形的獲取不依賴于物體本身的顏色和紋理。使用激光投影障礙物得到圖案信息，不僅可快速獲取障礙物信息，還具有優(yōu)良的魯棒性和高精度，大大擴(kuò)展了適用范圍。

1.2 視覺算法處理框架

整個系統(tǒng)的算法處理流程如圖2所示，首先使用深度相機(jī)進(jìn)行獲取點(diǎn)云數(shù)據(jù)獲取。文中所采用的是基于嵌入式處理器(Advanced RISC Machine，ARM)的相機(jī)，它可自動進(jìn)行圖像預(yù)處理；然后通過一系列的算法進(jìn)行地圖的構(gòu)建，再結(jié)合GPS、光流、超聲波等傳感器獲取的無人機(jī)的姿態(tài)信息進(jìn)行地圖的更新，實(shí)時獲取當(dāng)前地圖信息。

圖2 算法流程圖Figure 2. Flow chart of algorithm

1.3 相機(jī)坐標(biāo)系

深度相機(jī)的視覺系統(tǒng)中主要涉及4個坐標(biāo)系：圖像坐標(biāo)系、相機(jī)坐標(biāo)系、世界坐標(biāo)系和像素坐標(biāo)系。這4個坐標(biāo)系之間的關(guān)系如圖3所示，點(diǎn)P是世界坐標(biāo)系中的某一點(diǎn)。

圖3 四大坐標(biāo)系關(guān)系Figure 3. Four major coordinate system relationships

Ow-XwYwZw是世界坐標(biāo)系，用來描述相機(jī)的位置，單位為m；Oc-XcYcZc是相機(jī)坐標(biāo)系，以光心為原點(diǎn)，單位為m；o-xy為圖像坐標(biāo)系，光心為圖像的中點(diǎn)，單位是mm；uv為像素坐標(biāo)系，原點(diǎn)為圖像左上角，單位為pixel。點(diǎn)P世界坐標(biāo)系中的一點(diǎn)即現(xiàn)實(shí)世界中真實(shí)的一點(diǎn)，點(diǎn)p是P點(diǎn)在圖像焦距中的成像點(diǎn)，在圖像坐標(biāo)系中的坐標(biāo)為(x,y),在像素坐標(biāo)系中的坐標(biāo)為u,v。f代表相機(jī)焦距，等于o與Oc的距離， 即f=o-Oc。

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

從世界坐標(biāo)系變換到相機(jī)坐標(biāo)系是利用剛體變換實(shí)現(xiàn)的，即圖像中的物體不會發(fā)生形變，使用R旋轉(zhuǎn)矩陣和T偏移向量進(jìn)行矩陣的旋轉(zhuǎn)和偏移，旋轉(zhuǎn)矩陣如圖4所示[10]。

圖4 世界坐標(biāo)轉(zhuǎn)換相機(jī)坐標(biāo)Figure 4. World to camera coordinate conversion

它的計算如式(1)所示。

(1)

帶入坐標(biāo)運(yùn)算即為式(2)。

(2)

同理，繞x軸、y軸旋轉(zhuǎn)φ和ω即可得到式(3)和式(4)的矩陣，如下所示。

(3)

(4)

于是可以空過上述計算可得實(shí)驗(yàn)所需的旋轉(zhuǎn)矩陣R=R1R2R3。

所以從世界坐標(biāo)系到相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系如圖5所示[11]。

圖5 矩陣轉(zhuǎn)換Figure 5.Matrix conversion

可以知道坐標(biāo)之間的轉(zhuǎn)換關(guān)系，使用式(5)和式(6)可以得到所需矩陣

(5)

進(jìn)而得出

(6)

由上述計算式可推到得到R:3×3,T:3×1。

相機(jī)坐標(biāo)系得到的圖像坐標(biāo)系，屬于透視投影關(guān)系，從三維轉(zhuǎn)換成為二維圖像，此時投影點(diǎn)p的單位還是mm，并不是pixel，需要進(jìn)一步轉(zhuǎn)換成像素坐標(biāo)系，進(jìn)而由一個像素坐標(biāo)得出現(xiàn)實(shí)中的距離位置坐標(biāo)。如果想精確地將像素坐標(biāo)轉(zhuǎn)換為世界坐標(biāo)準(zhǔn)確并獲得物體的實(shí)際的坐標(biāo)信息，需要對相機(jī)進(jìn)行標(biāo)定。

1.5 相機(jī)標(biāo)定

盡管深度攝像頭與RGB攝像頭的成像效果有較大的差別，但是它們的成像原理相同，所以它們的標(biāo)定方法也是類似的。

對于本文使用的深度相機(jī)而言，它的基本原理為：結(jié)構(gòu)光相機(jī)上有一個紅外散斑激光發(fā)射器，它可以充當(dāng)光源向前方發(fā)射紅外激光束。紅外激光碰到障礙物后反射回來，深度攝像頭開始接收反射回來的光束進(jìn)行成像。通過計算光束的時間差來計算障礙物距離。結(jié)構(gòu)光的深度攝像頭比普通的RGB攝像頭多裝了濾波片，它只允許紅外激光通過，變成了紅外光成像的攝像頭。因此當(dāng)對它進(jìn)行標(biāo)定時，用紅外光源照射標(biāo)定板就可以得到標(biāo)定板的圖片信息。標(biāo)定時，用不透光的黑色物體將結(jié)構(gòu)光的紅外發(fā)射器擋住，避免散斑產(chǎn)生不必要的亮點(diǎn)，以免影響棋盤角點(diǎn)的檢測。如圖6所示，采用這張圖對深度攝像頭計算內(nèi)參。本文是調(diào)用OpenCV標(biāo)定程序進(jìn)行相機(jī)標(biāo)定的，可確保標(biāo)定的精度在允許范圍內(nèi)。

圖6 深度相機(jī)標(biāo)定Figure 6.Depth camera calibration

2 無人機(jī)避障模型的建立

2.1 相機(jī)標(biāo)定反傳感器模型

利用室內(nèi)障礙物測量信息y(k)和無人機(jī)的狀態(tài)x(k)進(jìn)行反傳感器模型構(gòu)建，模中的Pmi|xk,y(k)給出了點(diǎn)云柵格mi處于占有狀態(tài)(occupancy)的概率[12]。本文采用文獻(xiàn)[12]中提到的反傳感器模型進(jìn)行室內(nèi)地圖的構(gòu)建，如式(7)所示，在這個模型中測量到處于擊中狀態(tài)的點(diǎn)云柵格的概率更新為locc，其它沿著傳感器中心到對應(yīng)處于擊中狀態(tài)的點(diǎn)云的射線R之間的點(diǎn)free更新為lfree，即ri=ry(k)。

(7)

其中，ri是沿著射線R在結(jié)構(gòu)光傳感器中心的位置到點(diǎn)云像素點(diǎn)的mi之間的距離。圖7采用的是立體柵格圖的截面圖形式來表達(dá)的，點(diǎn)P是無人機(jī)當(dāng)前的位置信息。一個黑圓和一個黑色的長方形代表障礙物。反傳感器模型中帶有顏色的柵格代表柵格的狀態(tài)，顏色比較深的表示柵格處于occupancy狀態(tài)的柵格，它的概率更新值為locc；顏色比較淺的代表柵格處于free狀態(tài)，概率更新值是lfree。白色的柵格是表示無人機(jī)未知狀態(tài)，它的概率更新值被忽略。其中l(wèi)occ和lfree的取值組合決定了模型的精度與性能的好壞，調(diào)節(jié)它們的取值組合可以加快模型融合現(xiàn)實(shí)環(huán)境的速度以及發(fā)現(xiàn)新障礙物的準(zhǔn)確度，速度越快則模型越優(yōu)[13]。依據(jù)經(jīng)驗(yàn)分別設(shè)置locc和lfree為0.75和0.12。

圖7 反向傳感器模型Figure 7. Reverse sensor model

為了保證障礙物檢測的速度，把點(diǎn)云柵格信息的概率更新值以對數(shù)的形式儲存起來。與它對應(yīng)的對數(shù)機(jī)率反向傳感器模型計算式如式(8)所示。

(8)

相機(jī)每獲取一幀圖像需要對模型中的點(diǎn)云柵格進(jìn)行更新，它的計算式如式(9)所示。

lt-i=lt-1,i+L(mi,xk,yk)-l0

(9)

式中，lt-i表示柵格mi在t時刻的概率對應(yīng)的計算結(jié)果，其表達(dá)式如式(10)所示。

(10)

其中，l0是計算中的初始值，如式(11)所示。

(11)

在所有的計算中，始終認(rèn)為柵格的原始模型概率Pmi=0,i=0.5，它代表初始狀態(tài)的柵格的占有情況未知。

2.2 反傳感器模的應(yīng)用

使用反傳感器模型進(jìn)行進(jìn)行無人機(jī)室內(nèi)障礙物檢測以及路徑規(guī)劃，需要對柵格的狀態(tài)進(jìn)行詳細(xì)的分類。根據(jù)實(shí)際的情況可將它分為兩類，即不穩(wěn)定狀態(tài)和穩(wěn)定狀態(tài)。只有當(dāng)它處于穩(wěn)定狀態(tài)時才能進(jìn)行占有、空閑與未知3種情況的概率計算。利用閾值的方法來區(qū)分這兩種狀態(tài)，即設(shè)置兩個閥值Tocc和Tfree：當(dāng)柵格的數(shù)值達(dá)到其中的任一個數(shù)值時，則認(rèn)為這個點(diǎn)處于穩(wěn)定狀態(tài)[14]，可進(jìn)行節(jié)點(diǎn)判斷。本文采用剪裁柵格占有率的方法，裁剪與父節(jié)點(diǎn)狀態(tài)一致的子節(jié)點(diǎn)。如果剪裁后的信息與之前的節(jié)點(diǎn)有沖突，則創(chuàng)建新的節(jié)點(diǎn)。使用這種方法所構(gòu)建的模型既壓縮不必要的點(diǎn)云信息，又保留必要的接節(jié)信息[15]，加快了模型建立的速度。

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

3.1 系統(tǒng)硬件平臺

本文采用的無人機(jī)實(shí)驗(yàn)平臺如圖8所示。本文采用ARM和可編程門陣列芯片(Field Programmable Gate Array，F(xiàn)PGA)的飛行器控制計算機(jī)，采用結(jié)構(gòu)光相機(jī)進(jìn)行模型搭建的信息采集，利用光流傳感器進(jìn)行無人機(jī)飛行的狀態(tài)監(jiān)測，最后通過Pixhawk的飛控板進(jìn)行控制無人機(jī)的飛行控制。它的結(jié)構(gòu)布局適合在室內(nèi)進(jìn)行各種監(jiān)視偵查救災(zāi)地形探測等任務(wù)。

圖8 無人機(jī)硬件平臺Figure 8. Hardware platform of UAV

3.2 室內(nèi)3D地圖構(gòu)建

針對典型的室內(nèi)環(huán)境案例構(gòu)建實(shí)時地圖。由于室內(nèi)環(huán)境具有一定的封閉性，所以室內(nèi)光線比較依賴光源。圖9是室內(nèi)光源正常狀態(tài)的情況下室內(nèi)環(huán)境和無人機(jī)位置圖。

圖9 無人機(jī)室內(nèi)環(huán)境圖Figure 9. Indoor environment diagram of UAV

當(dāng)關(guān)閉室內(nèi)光源，RGB相機(jī)拍攝的圖像受到光源的影響比較大，當(dāng)光線不足容易出現(xiàn)圖像失真和模糊；當(dāng)光線過強(qiáng)時容易出現(xiàn)圖像過度曝光。因此，在這兩種情況下采集的圖像信息很難進(jìn)行相應(yīng)的視覺算法處理。

如圖10所示，關(guān)閉室內(nèi)光源時，采集的圖像很模糊，難以進(jìn)行相應(yīng)的視覺算法分析[16]。特別是當(dāng)室內(nèi)發(fā)生火災(zāi)時，室內(nèi)環(huán)境負(fù)責(zé)且煙霧多，普通的相機(jī)很難檢測室內(nèi)環(huán)境狀況，因此普通的雙目相機(jī)在這種情況下很難發(fā)揮圖像檢測的功能。

圖10 無光源室內(nèi)環(huán)境Figure 10. No light source indoor environment

當(dāng)采用結(jié)構(gòu)光相機(jī)進(jìn)行障礙物檢測時，它具有紅外激光發(fā)射模塊。利用紅外激光反射原理，相機(jī)可依據(jù)接收到激光反射的信息來進(jìn)行障礙物的檢測，不受室內(nèi)光源的限制[17]。

實(shí)驗(yàn)在Linux系統(tǒng)下使用ROS開發(fā)工具進(jìn)行進(jìn)行深度相機(jī)的視覺算法開發(fā)。ROS中每個軟件包相對獨(dú)立，并且可以自由通信。實(shí)驗(yàn)中采用C++語言進(jìn)行反傳感器模型的編寫，并訂閱深度相機(jī)的服務(wù)節(jié)點(diǎn)來獲取像素信息。最后把構(gòu)建好的地圖信息通過串口通信發(fā)送給Pixhawk飛行控制板來進(jìn)行無人機(jī)的飛行控制實(shí)驗(yàn)。具體步驟如圖11所示。

圖11 無人機(jī)整體控制流程Figure 11. UAV overall control process

利用反傳感器模型算法進(jìn)行室內(nèi)地圖構(gòu)建的實(shí)驗(yàn)結(jié)果如圖11所示。本實(shí)驗(yàn)是在室內(nèi)沒有光源的情況下進(jìn)行的，相機(jī)擺放的位置如圖9中所示。在沒有光源的情況下，深度相機(jī)自帶的激光發(fā)射模塊向周圍發(fā)射激光，深度相機(jī)的一個攝像頭只能接受紅外光線反射的光，可實(shí)現(xiàn)自帶光源進(jìn)行室內(nèi)環(huán)境的物體信息獲取，獲取圖像信息后使用ROS工具生成點(diǎn)云信息。最后使用反傳感器模型進(jìn)行三維地圖構(gòu)建，實(shí)驗(yàn)結(jié)果如圖12所示。

圖12 激光掃描地圖Figure 12. Laser scan map

4 結(jié)束語

本文中運(yùn)用基于嵌入式的結(jié)構(gòu)光相機(jī)來進(jìn)行室內(nèi)無人機(jī)障礙物檢測，并最終獲取室內(nèi)三維環(huán)境地圖信息。其中，所采用的反傳感器模型能夠快速有效地獲取室內(nèi)環(huán)境地圖，并且中央處理器占用較小。同時，結(jié)構(gòu)光相機(jī)自帶激光發(fā)射模塊，不受室內(nèi)光源的影響，即使在黑暗的條件下也能夠準(zhǔn)確地獲取室內(nèi)障礙物信息，可以有效地進(jìn)行室內(nèi)無人機(jī)避障以及路徑規(guī)劃等任務(wù)。由于結(jié)構(gòu)光相機(jī)的障礙物信息的獲取依賴激光反射，且由于物理特性限制，使得該視覺檢測系統(tǒng)的只能檢測0～5 m以內(nèi)的障礙物信息，超出此范圍的障礙物的檢測效果會變差。因此，在之后的實(shí)驗(yàn)中將會考慮光飛行時間法(Time of Flight，ToF)深度相機(jī)進(jìn)行室內(nèi)障礙物的地圖構(gòu)建。