基于單目視覺(jué)的四軸飛行器體素地圖重建系統(tǒng)的研究

趙藝博 霍冬浩 陳彥欽 李天城 朱曉佳

（中國(guó)民航大學(xué) 天津市 300300）

同時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping, SLAM)最早源于機(jī)器人領(lǐng)域, 其目標(biāo)是在一個(gè)未知的環(huán)境中實(shí)時(shí)重建環(huán)境的三維結(jié)構(gòu)并同時(shí)對(duì)機(jī)器人自身進(jìn)行定位，并且根據(jù)標(biāo)記點(diǎn)建圖。從SLAM 系統(tǒng)的角度看，“建圖”是服務(wù)于“定位”的；但從應(yīng)用層看，“建圖”有著許多其他的應(yīng)用，這些應(yīng)用大致可以劃歸為：定位、導(dǎo)航、避障、重建、交互。近年來(lái)，隨著自動(dòng)駕駛技術(shù)的興起以及社會(huì)對(duì)有自動(dòng)導(dǎo)航、軌跡規(guī)劃等功能的智能設(shè)備的需求越來(lái)越高，SLAM 問(wèn)題得到了越來(lái)越多的關(guān)注，也使得SLAM 技術(shù)得到快速的發(fā)展。

SLAM 技術(shù)由Smith Self 和Cheeseman 于1986 年首次提出，距今為止已經(jīng)發(fā)展了30 多年。同時(shí)無(wú)人機(jī)也是機(jī)器人技術(shù)應(yīng)用的重要平臺(tái)之一，因此無(wú)人機(jī)視覺(jué)SLAM 也隨之受到了大量的關(guān)注。由于無(wú)人機(jī)自身可以攜帶不同種類的傳感器，大量工作者在無(wú)人機(jī)SLAM 領(lǐng)域做出了優(yōu)異的成果，例如可以適用于裝備有單目、雙目或者深度相機(jī)的無(wú)人機(jī)的ORB-SLAM3算法，專為多旋翼系統(tǒng)設(shè)計(jì)來(lái)實(shí)現(xiàn)無(wú)人機(jī)實(shí)時(shí)建圖的框架OpenREALM，可以用于單目 SLAM無(wú)人機(jī)圖像拼接的Map2DFusion 算法，可以用于無(wú)人機(jī)導(dǎo)航的SLAM 框架openvslam，利用了激光傳感器進(jìn)行稠密重建的RTAB-Map。然而，大多數(shù)無(wú)人機(jī)的應(yīng)用多集中于雙目、深度或是添加激光等其他傳感器的環(huán)境。這是由于單目攝像頭不帶有深度信息，無(wú)法直接應(yīng)用于導(dǎo)航等問(wèn)題。本文就單目攝像頭添加深度信息，在ORB-SLAM2[7]上創(chuàng)新融合，實(shí)現(xiàn)了基于單目視覺(jué)的四軸飛行器體素地圖重建，可用于下一步的路徑規(guī)劃。

1 ORB-SLAM2算法的選擇

比較各種SLAM 優(yōu)勢(shì)與特點(diǎn)，例如基于單目、雙目的ORB-SLAM 系列等算法，融合深度學(xué)習(xí)的TANDEM、MonoRec等 算 法， 基 于 激 光 雷 達(dá) 的range-mcl、MULLS等SLAM 算法，亦或者是多傳感器融合的R3LIVE等算法?？梢钥闯?，基于單目的SLAM 算法的開(kāi)發(fā)成本是很低的，即便是考慮應(yīng)用層面上的硬件設(shè)施，一個(gè)攜帶單目攝像頭的小型四軸無(wú)人機(jī)相較于其他攜帶各種傳感器的設(shè)備也是一種成本極低的工具。

ORB-SLAM2算法主要由三個(gè)線程組成：跟蹤(Tracking)、建圖(Local Mapping)、閉環(huán)檢測(cè)(Loop Closing)。如圖1 所示，在圖片幀傳入后，首先跟蹤圖像，從中提取ORB 特征，并進(jìn)行初始化位姿，優(yōu)化位姿，確定關(guān)鍵幀；之后進(jìn)行關(guān)鍵幀的插入，驗(yàn)證篩選出地圖點(diǎn)，優(yōu)化并完成局部地圖構(gòu)建；最后閉環(huán)檢測(cè)進(jìn)行校正。

圖1： ORB-SLAM2 流程

2 基于ORB-SLAM2算法的稠密重建與保存

2.1 世界坐標(biāo)的獲取

稠密重建需要一種以.pcd 格式存儲(chǔ)的點(diǎn)云圖，點(diǎn)云圖主要包括了點(diǎn)云數(shù)量以及點(diǎn)云的世界坐標(biāo)。因此需要獲取圖像中空間點(diǎn)的世界坐標(biāo)。

2.1.1 關(guān)鍵幀圖像的保存

在實(shí)現(xiàn)稠密建圖之前需要圖像幀與對(duì)應(yīng)的相機(jī)位姿?？梢酝ㄟ^(guò)修改ORB-SLAM2 算法代碼對(duì)關(guān)鍵幀進(jìn)行保存。修改ORB-SLAM2 的源碼System.cc 來(lái)保存關(guān)鍵幀圖片，通過(guò)接口獲取關(guān)鍵幀對(duì)應(yīng)的位姿并保存為文本文件即可。

2.1.2 世界坐標(biāo)的計(jì)算

在針孔相機(jī)模型中，我們可以得到公式：

其中K 為相機(jī)內(nèi)參，R、t 叫做相機(jī)的外參，P為世界坐標(biāo)，P為像素坐標(biāo)，Z 原本為世界坐標(biāo)的深度信息。

但是在單目攝像頭上顯然無(wú)法得到深度信息，所以需要在公式中做歸一化處理，將Z 消除掉，這樣得到像素坐標(biāo)后，通過(guò)計(jì)算就可以得到世界坐標(biāo)下的X、Y 值。但是丟失了深度的地圖是無(wú)法進(jìn)行導(dǎo)航與路徑規(guī)劃的。因此需要做稠密重建來(lái)建立具有導(dǎo)航功能的地圖，通過(guò)在重建中將深度信息計(jì)算并加入，進(jìn)行稠密建圖。

2.2 單目稠密重建

一幅圖像的像素只能提供物體與成像平面的角度以及物體的亮度，無(wú)法提供距離。因此需要通過(guò)立體視覺(jué)的方式來(lái)獲得距離。本文對(duì)于深度估計(jì)分為以下四個(gè)步驟：

Step1 假設(shè)所有像素的深度滿足某個(gè)初始的高斯分布；Step2 讀入當(dāng)前幀，通過(guò)極線搜索和塊匹配確定投影點(diǎn)位置；

Step3 根據(jù)幾何關(guān)系計(jì)算三角化后的深度及不確定性；Step4 將當(dāng)前觀測(cè)融合進(jìn)行上一次的估計(jì)中。若收斂則停止計(jì)算，否則返回Step2。

2.2.1 極線匹配

在稠密圖深度估計(jì)中，是無(wú)法直接將所有特征點(diǎn)拿來(lái)直接做匹配的，所以如何確定第一幅圖的某像素在其他圖的位置是一個(gè)需要解決的問(wèn)題。本文采取極線搜索和塊匹配的方法。

如圖2 所示，左邊相機(jī)觀測(cè)到了某一個(gè)像素p，由無(wú)人機(jī)單目相機(jī)觀測(cè)所得。根據(jù)相機(jī)模型，該地圖點(diǎn)位于從O出發(fā)的射線上的某個(gè)區(qū)間之內(nèi)。從右側(cè)相機(jī)視角來(lái)看，這條射線的在成像平面上的投影叫做極線。顯然，當(dāng)已知兩部相機(jī)的相對(duì)運(yùn)動(dòng)時(shí)，極線是可以確定的。 接下來(lái)，只需要確定p 對(duì)應(yīng)的是極線上的哪個(gè)點(diǎn)即可。而因?yàn)閱蝹€(gè)像素的亮度沒(méi)有區(qū)分性，故選擇比較像素塊。在p 周圍取一個(gè)w×w 的小塊，然后在極線上也取很多相同大小的小塊進(jìn)行比較，即塊匹配。

圖2： 極線匹配

其中 表示圖像塊中像素點(diǎn) 的灰度值，通過(guò)去均值可有效改善圖像塊由于光照變亮或變暗導(dǎo)致的相似性降低。相關(guān)性越高，則圖像塊間相似度越高，取極線上相關(guān)性最高的搜索點(diǎn)作為匹配點(diǎn)。極線匹配算法具體過(guò)程如表1 所示。

表1：

2.2.2 三角測(cè)量

在得到兩幀上的一對(duì)匹配點(diǎn)P，P后，可以通過(guò)三角測(cè)量來(lái)計(jì)算空間點(diǎn)的深度。如圖3 所示，根據(jù)其觀測(cè)角度及平移距離可構(gòu)建三角并相交于空間點(diǎn)P，但由于噪聲的影響通常會(huì)存在些許偏差。設(shè)空間點(diǎn)P 在兩幀相機(jī)坐標(biāo)系下的深度分別為d，d，其歸一化圖像平面坐標(biāo)分別為P，P，則根據(jù)位姿變化可得：

圖3： 三角測(cè)量

3 地圖重建

3.1 八叉樹(shù)地圖

在導(dǎo)航問(wèn)題中，對(duì)地圖建模一般采用一種本身有較好的壓縮性能的地圖形式：八叉樹(shù)地圖，它是一種體素地圖。如圖4 所示，如果把一個(gè)小方塊的每個(gè)面平均切成兩塊，那么這個(gè)小方塊就會(huì)變成同樣大小的八個(gè)方塊，直到最后的方塊大小達(dá)到建模的最高精度。這個(gè)從最大空間細(xì)分到最小空間的過(guò)程，就是一顆八叉樹(shù)。在八叉樹(shù)中，當(dāng)我們由下一層節(jié)點(diǎn)往上走一層時(shí)，地圖就能擴(kuò)大為原來(lái)的八倍。從存儲(chǔ)層面來(lái)講，可以用0 表示空白，1 表示被占據(jù)。這種0-1 的表示可以用一個(gè)比特來(lái)存儲(chǔ)，節(jié)省空間。

圖4： 八叉樹(shù)地圖

3.2 地圖可視化

通過(guò)安裝octomap 庫(kù)，可實(shí)現(xiàn)pcd 點(diǎn)云圖轉(zhuǎn)化為八叉樹(shù)地圖，再利用rvis 可視化工具可觀察生成的八叉樹(shù)地圖。建立地圖的流程如圖5 所示。

圖5： 建圖流程

如圖6 所示，場(chǎng)景1、2 為使用特洛無(wú)人機(jī)攜帶的單目相機(jī)所拍攝的數(shù)據(jù)集場(chǎng)景以及使用重建系統(tǒng)轉(zhuǎn)化后的體素地圖。場(chǎng)景1中相機(jī)位于場(chǎng)景前方，場(chǎng)景2中相機(jī)位于場(chǎng)景上方。

圖6： 轉(zhuǎn)化出的體素地圖

4 結(jié)束語(yǔ)

本文對(duì)基于單目相機(jī)的ORB-SLAM 算法進(jìn)行了改進(jìn)，使得無(wú)人機(jī)通過(guò)單目相機(jī)能夠建立可用于導(dǎo)航的八叉樹(shù)地圖。但是仍然存在不足之處，建立的體素地圖細(xì)節(jié)不夠豐富，且存在深度錯(cuò)誤估計(jì)的問(wèn)題。并且只對(duì)單一場(chǎng)景進(jìn)行重建是不足以應(yīng)用于導(dǎo)航的。下一步，筆者將深入學(xué)習(xí)視覺(jué) SLAM框架以及點(diǎn)云庫(kù)，尋找更加合適的深度估計(jì)方法，并研究如何建立維護(hù)全局地圖。在保證低成本傳感器的同時(shí)，盡可能建立高質(zhì)量的地圖，提高單目ORB-SLAM2 建圖算法的實(shí)用性。