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

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

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

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

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

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

點(diǎn)云圖像是三維圖像信息的一種特殊的表達(dá)形式。與普通平面圖像相比，三維圖像是依靠第三維度的信息實(shí)現(xiàn)環(huán)境背景解耦。激光掃描獲取的點(diǎn)云模型中的每個(gè)點(diǎn)的信息都可以通過(guò)測(cè)量得到，每個(gè)點(diǎn)是一個(gè)測(cè)量點(diǎn)，包含了最大的信息量。這些信息都儲(chǔ)存在測(cè)量點(diǎn)中。本文采用的是基于結(jié)構(gòu)光相機(jī)來(lái)獲取點(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ī)對(duì)圖形的獲取不依賴于物體本身的顏色和紋理。使用激光投影障礙物得到圖案信息，不僅可快速獲取障礙物信息，還具有優(yōu)良的魯棒性和高精度，大大擴(kuò)展了適用范圍。

1.2 視覺算法處理框架

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

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

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

深度相機(jī)的視覺系統(tǒng)中主要涉及4個(gè)坐標(biāo)系：圖像坐標(biāo)系、相機(jī)坐標(biāo)系、世界坐標(biāo)系和像素坐標(biāo)系。這4個(gè)坐標(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)系，用來(lái)描述相機(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)的，即圖像中的物體不會(huì)發(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

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

(1)

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

(2)

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

(3)

(4)

于是可以空過(guò)上述計(jì)算可得實(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)

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

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

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

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

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

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

利用室內(nèi)障礙物測(cè)量信息y(k)和無(wú)人機(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)所示，在這個(gè)模型中測(cè)量到處于擊中狀態(tài)的點(diǎn)云柵格的概率更新為locc，其它沿著傳感器中心到對(duì)應(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采用的是立體柵格圖的截面圖形式來(lái)表達(dá)的，點(diǎn)P是無(wú)人機(jī)當(dāng)前的位置信息。一個(gè)黑圓和一個(gè)黑色的長(zhǎng)方形代表障礙物。反傳感器模型中帶有顏色的柵格代表柵格的狀態(tài)，顏色比較深的表示柵格處于occupancy狀態(tài)的柵格，它的概率更新值為locc；顏色比較淺的代表柵格處于free狀態(tài)，概率更新值是lfree。白色的柵格是表示無(wú)人機(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

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

(8)

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

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

(9)

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

(10)

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

(11)

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

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

使用反傳感器模型進(jìn)行進(jìn)行無(wú)人機(jī)室內(nèi)障礙物檢測(cè)以及路徑規(guī)劃，需要對(duì)柵格的狀態(tài)進(jìn)行詳細(xì)的分類。根據(jù)實(shí)際的情況可將它分為兩類，即不穩(wěn)定狀態(tài)和穩(wěn)定狀態(tài)。只有當(dāng)它處于穩(wěn)定狀態(tài)時(shí)才能進(jìn)行占有、空閑與未知3種情況的概率計(jì)算。利用閾值的方法來(lái)區(qū)分這兩種狀態(tài)，即設(shè)置兩個(gè)閥值Tocc和Tfree：當(dāng)柵格的數(shù)值達(dá)到其中的任一個(gè)數(shù)值時(shí)，則認(rèn)為這個(gè)點(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)硬件平臺(tái)

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

圖8 無(wú)人機(jī)硬件平臺(tái)Figure 8. Hardware platform of UAV

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

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

圖9 無(wú)人機(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)光線過(guò)強(qiáng)時(shí)容易出現(xiàn)圖像過(guò)度曝光。因此，在這兩種情況下采集的圖像信息很難進(jìn)行相應(yīng)的視覺算法處理。

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

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

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

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

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

利用反傳感器模型算法進(jìn)行室內(nèi)地圖構(gòu)建的實(shí)驗(yàn)結(jié)果如圖11所示。本實(shí)驗(yàn)是在室內(nèi)沒(méi)有光源的情況下進(jìn)行的，相機(jī)擺放的位置如圖9中所示。在沒(méi)有光源的情況下，深度相機(jī)自帶的激光發(fā)射模塊向周圍發(fā)射激光，深度相機(jī)的一個(gè)攝像頭只能接受紅外光線反射的光，可實(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ǔ)

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