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

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

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

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

1 ORB-SLAM2算法的選擇

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

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

圖1： ORB-SLAM2 流程

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

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

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

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

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

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

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

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

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

2.2 單目稠密重建

一幅圖像的像素只能提供物體與成像平面的角度以及物體的亮度，無法提供距離。因此需要通過立體視覺的方式來獲得距離。本文對于深度估計分為以下四個步驟：

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

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

2.2.1 極線匹配

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

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

圖2： 極線匹配

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

表1：

2.2.2 三角測量

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

圖3： 三角測量

3 地圖重建

3.1 八叉樹地圖

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

圖4： 八叉樹地圖

3.2 地圖可視化

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

圖5： 建圖流程

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

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

4 結(jié)束語

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