[發明專利]基于稠密視覺SLAM的無人機三維地圖快速重建方法

有效

• 申請號: 201910646511.3
• 申請日: 2019-07-17
• 公開（公告）號: CN110675483B
• 公開（公告）日: 2022-09-09
• 發明（設計）人: 黃方;楊浩;鐵博;陸俊;彭思遠;陳胤杰
• 申請（專利權）人: 電子科技大學
• 主分類號: G06T17/00
• 分類號: G06T17/00;G06T7/246;G06T7/80
• 代理公司: 成都東恒知盛知識產權代理事務所(特殊普通合伙) 51304
• 代理人: 羅江

1.一種基于稠密視覺SLAM的無人機三維地圖快速重建方法，其特征在于：包括無人機平臺三維地圖快速重建、無人機三維地圖快速重建算法、無人機三維地圖快速重建系統、基于ROS的視覺SLAM節點通信系統和基于ROS的視覺SLAM節點通信算法；

所述無人機平臺三維地圖快速重建：

基于ROS的系統硬件底層抽象方法：

接受使用sensor_msgs/Image消息類型的傳感器數據，該消息類型包含RAW圖像和壓縮圖像兩種，RAW圖像就是CMOS或者CCD圖像感應器將捕捉到的光源信號轉化為數字信號的原始數據；

在成功獲取傳感器數據后，ROS在圖像管道提供了相機標定、扭曲校正、顏色解碼，ROS圖像管道通過image_proc功能包運行，提供用于從攝像頭采集的RAW圖像中獲取單色和彩色的轉換功能，在標定完攝像頭，圖像管道就會提取CameraInfo消息并修正圖像的徑向與切向畸變，即在獲取數據的同時能夠完成預處理；

對于復雜的圖像處理任務，使用OpenCV、cv_bridge和image_transport庫對消息進行轉換連接，對訂閱和發布的圖像Topic進行處理，另一方面，OpenCV進行圖像處理時，使用cv_bridge將其轉換為ROSImage消息發布；

在ROS中將執行計算的進程稱為節點，為了在節點中使用OpenCV進行圖像處理，需要安裝獨立的OpenCV庫，然后必須在工作空間下的package.xml文件中指定編譯和運行需要的opencv2依賴包；然后，對于每個使用OpenCV的節點，必須在target_link_libraries中加OpenCV的lib文件；最后，在節點的cpp文件中，加入所需的OpenCV頭文件；

通過上述過程與步驟，實現對硬件的抽象，即通過傳感器獲取數據，將其發布至ROS網絡中，并通過訂閱圖像的RAW消息，使用OpenCV來進行圖像處理；

算法解耦與分布式計算：

三維重建算法運行時，將運行多個節點，每個節點對應實現各自的功能，節點間通過ROS消息進行通信，各個節點可在不同設備上運行，通過ROS構建的網絡，實現算法功能的耦合；

基于CUDA并行計算的算法加速方法：

選擇NVIDIA Jetson TX2嵌入式開發模塊作為實驗的處理平臺，在基于TX2開發模塊的并行程序運行時，每32個并行線程被叫做一個Wrap，GPU中線程的調度執行是以一個Wrap為一個調度單位進行的，每一個SM內有2個線程束調度器，每個線程束調度器內有32個CUDA核，該GPU共256個CUDA核；

基于Topic的消息傳遞與通信：

(1)啟動Talker，Talker通過端口將其信息注冊到Master端，其中包括Talker所發布消息的話題名、節點地址信息等；Master將這些信息加入到一個注冊列表中；

(2)啟動Listener，Listener向Master端注冊，注冊其所需訂閱的話題以及Listener自己的地址信息；

(3)Master對發布者與訂閱者進行信息匹配，如果二者發布/訂閱同一Message則幫其建立連接；如果沒有匹配的則繼續等待；若找到匹配的，且此時Talker在發布Message，便會把Talker的地址發送給Listener；

(4)Listener接收到Master發送的Talker地址后，向Talker發送連接請求，同時將Listener要訂閱的話題名和完成通訊所需的通信協議全發給Talker；

(5)Talker接收到Listener請求后，返還一個確認連接的信息，包括其自身的TCP地址；

(6)Listener接收到Talker的TCP地址后，通過TCP與Talker建立網絡連接；

(7)Talker通過建立的TCP網絡連接將數據發送給Listener；

所述無人機三維地圖快速重建系統：包括視覺傳感器、無人機、嵌入式處理器、GPU，軟件實現視覺SLAM的核心算法；軟硬件通過分布式框架及通信協議相互聯系，組成完整的軟硬件系統；其流程如下：

(1)無人機機載嵌入式搭載視覺傳感器，獲取實時圖像序列；

(2)圖像序列分別傳輸至前端與后端；

(3)前端通過特征提取匹配、關鍵幀提取、位姿估計為后端提供位姿初值；

(4)后端對(2)中接收的圖像進行閉環檢測，并對(3)中獲取的位姿進行優化；

(5)后端基于位姿與圖像進行稠密重建；

(6)由觀測端通過ROS可視化工具進行實時監測；

所述無人機三維地圖快速重建算法：

視覺SLAM前端算法的流程如下：

(1)SLAM系統提供接口，接收由傳感器所獲取的連續的圖像序列，將圖像序列傳入VO端；

(2)判斷SLAM系統是否已初始化完成，若未完成，則進行第(3)步，若已完成初始化，則進行第(4)步；

(3)初始化第一個關鍵幀，并初始化位姿T0；

(4)持續對圖像序列進行ORB特征提取，選取下一個關鍵幀F_i(i＞＝1)；

(5)將F_i與F_i-1的ORB特征進行匹配，并篩選出可靠的匹配；

(6)基于可靠的特征點對，通過對極約束求解幀間的運動，計算出該關鍵幀的位姿T_i；

(7)若幀流結束則停止，否則返回第4步；

重建部分算法的流程如下：

(1)選取需要計算深度的目標像素點p；

(2)基于相機運動，即相機光心的平移向量，以及所求關鍵幀相機光心與p的連線的向量，構成的平面交新關鍵幀于極線，計算極線位置；

(3)遍歷所求得的極線，搜索與p匹配的點；

(4)通過三角測量計算出p點實際的空間位置；

(5)更新該像素的深度信息；

SLAM前端特征提取與匹配方法：首先檢測Oriented FAST角點位置，然后基于角點位置，計算其BRIEF描述子；對兩幅圖像中BRIEF描述子進行匹配，記錄其漢明距離，即二進制字符串不同位數的個數；記錄所有匹配之中的最小值d_min，若描述子之間漢明距離大于2倍d_min，則刪除該匹配，反之保留該匹配；

關鍵幀提取方法：首先處理第一張圖像，先檢測FAST特征點和邊緣特征，如果圖像中間的特征點數量超過設定的閾值，就把這張圖像作為第一個關鍵幀，然后處理第一張之后的連續圖像，持續跟蹤特征點；當匹配點的數量大于閾值，如果視差的中位數大于閾值，選擇計算本質矩陣E，否則選擇計算單應矩陣H；如果計算完H或E后，還有足夠的內點，就將其作為關鍵幀；

相機運動估計：相機的運動估計即計算出相鄰關鍵幀之間的平移向量t和旋轉矩陣R，而t與R可通過分解本質矩陣E來獲得，而在關鍵幀相機只有旋轉而無平移的時候，兩視圖的對極約束不成立，基礎矩陣F為零矩陣，這時候需要分解單應矩陣H；分解方法為奇異值(SVD)分解：

tR＝E＝UDV^T (4-1)

其中，U、V均為三階正交矩陣，D為對角陣D＝diag(r,s,t)；分解可得對角矩陣的特征值，該3個特征值中將會有兩個特征值為正，其值相等，另一個則為0；在分解過程中會獲得四組不同的解，分別對應關鍵幀的四種相對位姿；

稠密重建：以相鄰兩幅圖像中估計任一像素的深度來說，對于某一個像素p₁對應的空間點P，相鄰兩幀的相機光心與其連線O₁P、O₂P以及O₁O₂共面，其中O₁P交第一幀于p₁，O₂P交第二幀于p₂，則p₂必然落于面O₁O₂P與第二幀交線上，即p₁在第二幀中的位置在極線上；

由于單個像素的亮度沒有區分性，則考慮對像素塊相似性進行比較，即在像素點p₁周圍取一塊w*w的小塊，然后遍歷極線上各點，與其周圍的w*w的小塊進行匹配，則算法的假設由像素的灰度不變性轉為圖像塊的灰度不變性；

對于兩個小塊的匹配，計算其歸一化互相關(Normalized Cross Correlation，NCC)：

在極線上計算每一個相似性度量之后，將會得到一個沿著極線的NCC分布，即p1像素在第二幀中出現的概率分布，NCC值越高，則該點為p1像素的同名點概率越高；然而僅靠兩張圖像計算某像素的深度值存在一定的偶然性，因此需要多張圖像進行估計；我們假設某個像素點的深度d服從高斯分布：

P(d)＝N(μ,σ²) (4-3)

每當新的數據到來時，觀測其深度也將會是一個高斯分布：

則更新觀測點p1的深度變成一個信息融合問題，而融合后的深度則滿足：

經過多次迭代，則p1在第二幀極線上的概率分布峰值越接近其真實位置；由此，對于稠密重建其完整步驟如下：

(1)假設所有像素的深度滿足某個初始的高斯分布；

(2)當新數據產生時，通過極線搜索與塊匹配確定投影點位置；

(3)根據幾何關系計算三角化后的深度及不確定性；

(4)將當前觀測融合進上一次的估計中，若收斂則停止計算，否則返回第2步；

基于ROS的視覺SLAM節點通信系統：采用ROS分布式計算架構，以降低系統各模塊之間的耦合，提高系統在復雜環境中的可用性與可靠性；將視覺SLAM重建過程中的數據獲取、位姿估計、稠密重建、可視化分別由四個計算節點運行，每個節點各司其職，節點間通信采用Topic訂閱模式；系統工作時共4個節點參與通信，其中，傳感器捕捉數據，其原始數據被發布成話題/sensor_msgs，由視覺傳感器與Jetson TX2通過USB或CSI接口直連，將流數據轉換為圖像序列數據消息，重新發布成話題/usb_cam；

VO節點訂閱/usb_cam，進行位姿估計，并將結果發布至成位姿數據，重建節點同時訂閱/usb_cam與位姿數據，并將處理后得到的深度圖數據與點云數據發布至ROS網絡中，由可視化節點訂閱，實現系統的增量地圖構建的可視化功能；

所述基于ROS的視覺SLAM節點通信算法：SLAM前端，即VO節點，與后端重建節點同時訂閱話題/usb_cam；VO節點將/usb_cam節點所發布的數據通過SLAM前端庫的接口進行位姿解算，然后，將其以消息的形式進行發布，由SLAM后端的重建節點進行訂閱，即重建節點同時訂閱幀數據以及位姿數據；為了能夠實時地對三維地圖重建的結果進行可視化，采用的方法流程如下：

(1)當選取的參照幀其深度圖收斂時，將深度圖結果發布至話題；

(2)可視化節點根據參照幀及其時間戳，獲取由VO節點發布的位姿/pose；

(3)基于初始化結果，計算出該參照幀光心的空間位置以及該幀的法線方向；

(4)基于深度圖逐像素構造點云。