視覺與微慣導系統中多傳感器信息融合導航方法研究

王海，劉峰寰

沈陽理工大學

針對視覺與微慣導系統在無人機等領域應用為研究背景，研究了在視覺微慣導系統中基于多傳感器信息融合的姿態測量方法。測試結果為視覺與微慣性傳感器姿態測量方法融合，以融合測姿結果作為觀測數據，隨后通過卡爾曼濾波器進一步準確估計載體姿態。采用研制的視覺與微慣導組件搭建了實驗平臺，并進行載體姿態測量實驗，最后簡單驗證了融合算法的有效性。

同步定位與地圖構建（SLAM）越來越廣泛地應用到地面移動機器人、水下機器人、無人機乃至行星探測等領域，是當前工業界和學術界備受關注的熱門研究方向。SLAM技術是指某種特定的載體，搭載了一種或多種不同的傳感器，在環境先驗未知的情況下，在運動過程中實現自身的定位以及周圍環境的建圖。

導航的前提就是定位與建圖，在不同應用場景下實現定位與建圖，需要引入不同的傳感器。激光雷達和視覺（Visual）是兩大類根據所用傳感器不同而流行的SLAM主要方法。基于激光雷達的SLAM方法能夠獲取高精度的地圖，但存在價格昂貴、空間分辨率低等缺點。基于視覺的SLAM方法依賴的傳感器具有體積小、功耗低、場最辨識能力強及感知紋理信息豐富等優點（視覺SLAM技術的進展與應用），但對于環境紋理特征不明顯及圖像采集幀率很快的情況，其運動估計能力會降低。

低成本視覺微慣導集成模塊的設計

在特殊環境下，實驗室自主設計了一套完整的視覺與微慣導融合系統的位姿檢測，包括數字信號開發板（Raspberry Pi 4B）、 電 源 模 塊、 攝 像 頭 模 塊、GY-85模塊等。攝像頭模塊使用手機自帶攝像頭及單目攝像頭，MPU6050模塊通過慣導解算得到六自由度的位姿信息。利用擴展卡爾曼濾波算法對這兩個通道的姿態結果進行融合，并得到姿態估計結果。

多傳感器姿態測量方法

視覺傳感器以及陀螺儀、加速度計、磁阻傳感器共同構成了視覺與微慣導融合系統。四元數和歐拉角方法可通過陀螺儀輸出完成姿態更新，求解載體的橫滾、俯仰和偏航角度。解析加速度計的輸出并進行計算，以獲得俯仰和滾轉角的兩個主要情況。在紋理不豐富或高動態視覺丟失的情況下，在單目相機初始化時，由于單目初始化只能得到深度的初值，無法得到絕對尺度信息，只能取到所有特征點的深度中值。不同微慣性器件有著不同噪聲的頻率特點，視覺與微慣導在不同環境有著各自的優勢，多重數據通過互補濾波器的融合，提升了載體姿態測量的精準度。

多傳感器信息融合與濾波算法

通過視覺與微慣導解算均可獲得位姿信息，視覺方法依賴的傳感器具有體積小、功耗低、場最辨識能力強及感知紋理信息豐富等優點，但對于環境紋理特征不明顯及圖像采集幀率很快的情況，其運動估計能力會下降。微慣導解算短時、定位精度高、自主性強，同時可以完全基于加速度計與陀螺儀完成位姿解算，在紋理特征不明顯或快速運動的情況下可以達到良好的測量效果。通過卡爾曼濾波實現兩者的信息融合，一方面，相機在運動過程中可以確定出位姿以及能夠為周圍環境建立出3D模型；另一方面，由于微慣導通過解算可以直接獲得自身的運動信息，與視覺融合時能夠為其提供重力方向，同時也使橫滾與俯仰角可觀，能夠達到更精確及魯棒的運動估計。

圖1 系統框圖。

特征提取、匹配、篩選

OFAST相比較其他算法，擁有尺度的旋轉和描述。類似計算重心指向更亮的區域，計算角度、圖像梯度。搭建圖像金字塔并在金字塔不同層的每一層分別提取一個FAST，可以得到多種尺度下的FAST特征。在這種情況下，可以認為FAST具有一定程度上的尺度不變性。根據特征點的方向計算旋轉后的BRIEF描述子，使其具有旋轉不變性。

特征匹配是對兩幀圖像間的描述子進行匹配，提供給后續位姿估計。假設分別在圖像1、2中提取到相同個數的ORB特征點，然后計算對應的BRIEF描述子。ORB的特征匹配常用暴力匹配法進行，即對第一個圖中的一個特征點與第二個圖中的每一個特征點測量BRIEF描述子的漢明距離，對這些漢明距離進行排序，設定小于一定閾值的點對作為匹配點。

圖2 經過篩選后的匹配。

暴力匹配的操作簡單，但是結果會存在大量誤匹配，誤匹配影響著后續的位姿估計的速度以及精度。使用暴力匹配，然后根據距離應相等或相近的正確匹配點對進行提純，提高匹配的準確度，以減少運行時間及提高相機運動解算的精度；再使用Lowe’s算法，獲取第一個圖中有誤匹配的關鍵點，并找到第二個圖中在暴力匹配時得到的誤匹配的兩個關鍵點中歐式距離最近的那個，如果最近的關鍵點的歐式距離與另外一個關鍵點的歐式距離之比小于預設的閾值，那么接受這一對匹配點。最后利用篩選后的特征點對構建非線性優化問題，求解變換矩陣。

位姿估計

單目視覺里程計中，常通過經過篩選后的匹配并根據對極幾何以及本質矩陣等理論，估計出粗略的運動。為了提高這種粗略運動估計的準確性，基于將優化問題表現成圖的形式的理論進行幀間運動估計，在幀間運動估計問題中，可將不同時刻的相機位姿和路標點視為頂點，不同位姿之間的關系當成邊。對于幀間的運動估計和尺度未知的特征點位置解算問題，可基于圖優化理論進行解算。

圖3 運行過程。

假設幀間運動R，t，相機內參矩陣為K，特征點的世界坐標為Xi，兩相機坐標系下的相機坐標分別為Ci1、Ci2，對應的深度值分別為S1、S2，相機1的位姿為單位矩陣，根據投影關系，有：

然而由于噪聲影響，投影關系不能準確成立，用二范數量化每一個特征點存在的誤差，并對所有特征點的誤差項進行求和，可得目標優化問題如下。

也稱最小化重投影誤差問題，為非線性、非凸的優化問題。實際的求解過程中是調整每個Xi，使得每個誤差項都盡量地小，因此也可稱為捆集調整（bundle adjustment，BA）。

圖4 軌跡圖。

求解兩幀之間的運動，需設置相機位姿結點、特征點的空間位置結點，以及重投影誤差邊。優化求解前，需要選擇求解器和迭代算法：①選擇線性方程求解器；②選擇塊求解器；③選擇列文伯格-馬夸爾特（Levenburg-Marquadt，LM）迭代算法。

為解算各幀的估計位姿，幀間運動估計過后，根據數據集中的連續幀，首先固定第一幀的位姿，利用估計出的運動進而推算出接下來各幀的估計位姿。

圖5 方向示意圖。

實驗結果

本文試驗采用了kitti數據集進行測試和評估，它包含了市區、鄉村和高速公路等場景采集的真實圖像數據，每張圖像中最多達15輛車和30個行人，還有各種程度的遮擋與截斷。以10Hz的頻率采樣及同步。

結論

本文以無人機在無衛星條件下的位姿估計為背景，設計了一個多傳感器組合定位系統，其中包括單目視覺傳感器、IMU，實現了視覺與慣導的融合定位。提出了基于最小二乘法實現單目視覺尺度的最優估計; 基于擴展卡爾曼濾波實現視覺慣導融合定位。

本文設計實現的系統還可進一步完善，包括增加傳感器的融合數量以及采集精度可以獲得更高的精度，以及減少視覺丟失的情況。