一種基于計算機視覺的無人機實時三維重建方法

張臻煒，趙 勇，布樹輝

（西北工業大學航空學院，陜西 西安710072）

0 引言

基于計算機視覺的SLAM，即同時定位與地圖構建，由于其重要的理論與應用價值，一直以來都是學者研究的熱點。相比較傳統的測繪技術，視覺SLAM的三維地圖重建，在對傳感器依賴比較少的前提下，最大限度利用獲取到的圖像信息，同時高效的信息處理決定了其能夠實時重建出三維環境。然而，圖像信息采集過程中的噪聲干擾和圖像處理帶來的大計算量一直以來限制該技術應用于實踐中。引入圖優化的思想，來降低傳感器噪聲累積對整個系統的影響。同時由于RGB-D傳感器能同時提供顏色和深度信息的圖像，使得稠密重建的計算復雜度大大降低，提高系統實時運行的能力。

1 視覺SLAM

1.1 運動估計

對于相機采集到的數據，系統用事先測定的相機標定數據進行圖像去畸變處理，得到去畸變后的圖像［1］。通過跟蹤由相鄰的幾個關鍵幀組成的局部地圖上的特征點來估計每個新幀的運動狀態。提取Fast-Corner或者ORB特征用于描述特征點。并且采用運動模型，由于幀率比較高，相鄰幀的差別（時間、空間）不是很大，可以把前一幀的運動狀態當做該幀運動的初始估計。為了提高系統的魯棒性，保留像素深度的不確定性，即引入高斯分布的深度概念，一個像素有深度值的同時還有深度的方差值，并假設其服從高斯分布。然后，通過迭代計算最小化光度和深度殘差函數（通過地圖點和新的幀上的特征點匹配而得到的）來獲取運動估計。提出的方法應用金字塔模型來增強系統的魯棒性，尤其是針對高速運動造成的圖像模糊情況。其中跟蹤的質量是通過計算成功匹配的數量和最小化的殘差值來定義的，同時用于檢測跟蹤丟失和判斷是否要建立新的關鍵幀。由于在實驗中不可避免地會出現跟蹤丟失的情況，因此一旦出現“丟幀”，就必須實施基于關鍵幀匹配的“重定位”，以便估計之前的運動參數。

1.2 坐標定義、轉換與相機投影

相機的姿態通常用矩陣H來表示為：

這里的旋轉矩陣R是3×3的正交陣，R∈SO（3），平移量t是3×1的向量，t∈R3。這個變換矩陣H是過參數的，因為它有12個參數，但是只有6個自由度。因此用向量μ來表示為［2］：v1，v2，v3為平移量；q1，q2，q3為旋轉。μ 與H的相互轉化為H＝exp（μ）。對于一個世界坐標系下的點p＝（X，Y，Z，l）T，可以將其轉化到相機坐標系下為p′＝R［X，Y，Z］T＋t＝HR＝exp（μ）P。將相機坐標系下的點Pc＝（Xc，Yc，Zc，l）T投影到像平面上p＝（x，y，d）T，其中（x，y）T是像素坐標，d是像素深度。標準的針孔相機模型用來表示投影變換為：

fx，fy是焦距；x0，y0是像平面的中心。與之類似，從像平面上往相機坐標系的映射為反投影。

1.3 運動先驗與運動更新

系統可以采用濾波器或者其他傳感器（如IMU）來獲得運動量的“經驗值”。然后針對快速運動，利用這個“經驗值”來增強系統魯棒性和判斷是否需要金字塔模型。如果前一幀跟蹤效果好，那么就可以用前一幀的運動量作為下一幀的“經驗值”，這樣做有利于后續的運動優化。

當地圖初始化完成后，就將地圖上的點投影到當前幀中。然后去尋找最佳的Fast-Corner匹配［3］，用于計算殘差。為了限制全局地圖的復雜度，只投影那些由關鍵幀組成的局部地圖（子地圖）上的點。如果地圖點滿足以下要求就會被丟棄。①算出地圖點在該幀的投影P′，如果P′不在該幀圖像范圍內就舍棄它；②計算該點與相鄰的特征點的ORB距離d，找一個最佳匹配。給距離d設定一個閾值，若超過這個閾值就舍棄它；接著，如果系統獲得了一些三維地圖點和RGB-D圖像的像素點的匹配關系，那么就能估計相機的姿態μ。為了充分利用像素坐標值和深度信息，采用一種魯棒的殘差函數來估計運動位姿，殘差為：

被定義為地圖點計算值和預測值的誤差為：

理想情況下，殘差值為零。然而，由于信息采集的噪聲，殘差是基于概率模型p（r1μ）分布的。然后通過最小加權殘差函數來估計相機位姿μ＝。這里的加權誤差項是由誤差的參考值動態確定的。加權項wi用來降低大殘差的影響。

對于估計相機移動而言，以上的處理已經足夠了。但其實還可以通過點的投影殘差來獲得更高的精度。這時候一些對應點的深度值可能無效，那么這些點的權重就很小，甚至被忽略。盡管它的深度方差很大，仍然使用它的RGB值。而這適用于紋理豐富而深度信息很弱的場合。同時應用金字塔模型來避免由于快速運動導致的“丟幀”現象。當用運動模型獲知之前一幀的“運動幅度很大”時，就先在低圖片的分辨率上去做跟蹤：即在底層的金字塔上，在只有較少的幾個特征點，且較大的搜索半徑下去估計位姿μ。然后縮小半徑，提高分辨率，增加匹配的特征點個數，以獲得更加精確地位姿。

1.4 失敗恢復

盡管已經付出最大的努力來讓整個系統盡可能地魯棒，在某些時候跟蹤仍然還是會失敗。用成功匹配的點的數量和最小化的殘差函數值來估計追蹤的質量。如果跟蹤系統沒有獲得足夠多的匹配或者平均的加權半徑超過了允許的范圍，那么就將跟蹤定義為失敗［5］。一旦跟蹤失敗，重定位線程就會將開啟，用來沖失敗中恢復整個系統：選擇一些候選的關鍵幀，然后嘗試去尋找當前幀上的特征點與這些關鍵幀上的點的匹配關系。然后基于這些三維對應點，用SANSAC算法算出一個位姿的估計值。如果對應點足夠的多，那么就用這個估計值來繼續執行后續的程序。

2 關鍵幀處理、圖優化與數據融合

2.1 關鍵幀的建立

為了更好地建立全局地圖，引入關鍵幀的概念。關鍵幀的設立是由于其具有的位姿相對于前一個關鍵幀變化較大，其探測到的三維環境信息相比較于之前的關鍵幀有較大不同，因此將其設為一個標尺，用于擴展全局地圖和檢測后續的幀是否有較大的位姿變化：通過檢測到的特征點進行兩幀之間的特征匹配，然后判斷是否創建新的關鍵幀。這里采用一種新的衡量兩幀之間相似度的標準：用Relij來表示第i幀與第j幀的相似度為：

μij表示第i幀和第j幀的相對位姿；E（di）定義為第i個關鍵幀的平均深度。W 是表示權重的對角矩陣。當關鍵幀間的Relij值比較小時，意味著產生了比較多的新的信息，也就是說這時候有必要增加一個新的關鍵幀了。此外，一個新的關鍵幀必須符合的要求是，①為了確保當前的姿態估計是基于一個良好的重定位的，一個新的關鍵幀應該在前一個全局的重定位之后過15幀以上在添加；②跟蹤的精度應該足夠好，最后被舍棄特征點對應的數目要少于50對。

獲取當前的位姿變化量之后，除了進行前面是否添加關鍵幀的判斷以外，還將該幀新檢測到的特征點同過相機內參和剛才獲得的位姿參數反投影回三維空間，加入全局地圖中，就這樣一步步擴展全局地圖。

2.2 圖優化

由于相機的限制，采集到的圖像信息是包含著噪聲的（無人機對于自身的定位是依賴于所檢測到的圖片信息，而且這個誤差還會累積，最終導致了三維重建的結果由于誤差太大失去實用價值），因此系統需要對生成的地圖進行優化校準，采用圖優化的思想［4］如圖1所示，先進行局部的Bundle校準，先尋找相近的幾個關鍵幀先進行小規模的Bundle校準，等全部的地圖構建完成后再進行一次全局的校準，校準的目的是為了減小累積噪聲的影響。引入圖優化來減小來自傳感器的誤差對最終結果的影響，矯正每一步的位姿，以實現全局優化為：

xk為前面定義的節點（也可以理解為狀態）；zk為邊（也可以理解為約束）；ek為這些節點滿足約束的狀態（如果沒有噪聲及ek＝0）；Ωk為引入的信息矩陣及約束的置信度，如果誤差大則它對應的置信度就小。定義完變量之后，接下來要做的就是使整個誤差函數降到最小，以達到全局最優的目的［6］。將地圖點的三維位置和無人機的位姿定義為節點，將從圖像到地圖點的投影關系和相鄰兩幀之間的SE（3）的變化定義為邊，信息矩陣包含兩方面，地圖點的被關鍵幀觀測次數和圖像中點的灰度梯度。

圖1 圖優化示意

2.3 數據融合

在產生全局地圖點的時候，如果僅由當前幀與當前關鍵幀通過三角化反投影回三維空間產生，會降低程序的魯棒性（往往也不精確）。因此，需要將三角化產生的地圖點投影到幾個相鄰的關鍵幀上（前提是這個地圖點在這些關鍵幀的可見范圍內），在小范圍內進行特征匹配，然后再重新反投影回三維空間，最后對多個值進行加權平均，得到一個更加合理的地圖點。

3 實驗結果

以下將展示重建的結果，包括實時和離線數據的測試結果，并與當前一些主流的視覺SLAM方法作比較。以下所有的測試都在mint 17（基于ubuntu14.04，64bit）上進行，8GRAM，不用 GPU，主頻3.50GHz。說明：圖像數據采集自西北工業大學友誼校區、航空樓A-412民航工程實驗室。以下是實驗截圖，包含實時全局點云圖（白色的點繪制三維地圖；紅色的相框表示當前的實時相機的位姿；藍色的相框代表系統創建的關鍵幀；青色的線代表關鍵幀之間的約束），實時深度估計圖（從紅到藍表示由近到遠），實時深度方差圖（從紅到藍表示方差由小到大）（注：以上說明于彩色版面圖像可見，黑白版面無法識別）和原始圖像數據（1 920×1 024，灰度圖）。

圖2 原始圖像、深度、深度方差

圖3 重建半稠密點云圖

接下來將通過與其他主流SLAM方法比較來評估性能與數據指標如表1所示。通過比較可以發現，

表1 RGB-D基準 測試結果比較

無論是位置漂移量、角度漂移量還是絕對誤差都較低，同時系統實驗精度依然能夠達到實用標準，而且能夠直接在CPU上運行，不用要求GPU。

圖4 ICL公開測試數據

4 結束語

提出一種基于計算機視覺的無人機實時三維重建方法，通過算法實現無人飛行器在未知環境下實時定位和環境感知，針對傳感器帶來的累積噪聲，采用圖優化的思想，達到全局最優的效果。通過當前幀與地圖點特征點匹配，得到當前相機的相對位姿。實驗結果表明本方法在保證重建精度的前提下，能夠實現實時運行。

［1］ Zhenyou Zhang.A flexible new techniquefor camera calibration［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2000，22（11），1330-1334.

［2］ Selig J M.Lie groups and lie algebras in robotics［C］／／Computational Noncommutative Algebra and Applications.Springer，2004.

［3］ Rosten E，Drummond T.Machine learning for highspeed corner detection［J］.Computer Vision-ECCV2006.Springer，2006.

［4］ Kummerle R，Grisetti G，Strasdat H，et.al.G2o：A general framework for graph optimization［C］／／IEEE International Conference on Robotics ＆ Automation，2011：3607-3613.

［5］ Endres F，Hess J，Sturm J，et.al.3-D maping with an RGB-D cameras［J］.IEEE Transaction on Robotics，2014，30（1）：177-187.

［6］ Guivant J E，Nebot E M.Optimization of the simultaneous localization and map-building algorithm for realtimeimplementation［J］.Robotics ＆ Automation IEEE Transactions on，2001，17（3）：242-257.