基于深度相機的小型無人機室內三維地圖構建

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

(上海工程技術大學 機械與汽車工程學院，上海 201620)

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

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

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

1 基于深度相機的圖像處理

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

1.1 基于結構光的深度相機原理

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

圖1 結構光相機原理Figure 1. Principle of structured light camera

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

1.2 視覺算法處理框架

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

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

1.3 相機坐標系

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

圖3 四大坐標系關系Figure 3. Four major coordinate system relationships

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

1.4 坐標轉換

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

圖4 世界坐標轉換相機坐標Figure 4. World to camera coordinate conversion

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

(1)

帶入坐標運算即為式(2)。

(2)

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

(3)

(4)

于是可以空過上述計算可得實驗所需的旋轉矩陣R=R1R2R3。

所以從世界坐標系到相機坐標系的轉換關系如圖5所示[11]。

圖5 矩陣轉換Figure 5.Matrix conversion

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

(5)

進而得出

(6)

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

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

1.5 相機標定

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

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

圖6 深度相機標定Figure 6.Depth camera calibration

2 無人機避障模型的建立

2.1 相機標定反傳感器模型

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

(7)

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

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

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

(8)

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

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

(9)

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

(10)

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

(11)

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

2.2 反傳感器模的應用

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

3 實驗結果分析

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

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

圖8 無人機硬件平臺Figure 8. Hardware platform of UAV

3.2 室內3D地圖構建

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

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

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

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

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

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

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

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

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

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

4 結束語

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