一種基于改進光流法的視覺慣性狀態估計器

劉奕杉

（廣東工業大學，廣東 廣州510006）

1 概述

實時定位和建圖(SLAM)起源于20 世紀80 年代，它廣泛應用在移動機器人、無人機、虛擬現實、無人駕駛等領域。它解決的主要問題是機器人在未知環境的定位和建圖問題。傳統的定位方式多以GPS 為主，然而這種定位方式難以適應復雜陌生環境，所以SLAM 技術應運而生。SLAM 根據后端方法的差異，可以分為基于濾波和基于優化這兩大類，根據傳感器差異也可以分為激光SLAM 和視覺SLAM[1]。然而，單一傳感器難以滿足高精度的定位效果，所以在實際應用中，常見融合多種傳感器的信息用于機器人定位，常見的有視覺和慣性單元的融合。視覺傳感器可以提取現實場景豐富的信息，但它受環境影響大，當面對光照變化大、紋理缺失、快速運動等情形，無法有效追蹤物體運動。慣性單元可以得到加速度和角速度，且不受圖像質量的影響、采樣頻率較高，但存在長時間累計誤差的問題。所以視覺傳感器和IMU 具有優劣互補的特點。

本文研究基于光流法的視覺慣性SLAM 系統。我們結合改進的光流法和IMU 信息，然后基于LM 的非線性優化算法，實現了一個精度較高的視覺里程計。本文將從改進的光流法、慣性單元預積分，系統框架三部分對該方案進行描述，最后將通過實驗展示該系統的運行效果。

2 正文

2.1 光流法

Lucas-Kanade 光流法是Bruce D. Lucas 等人于1981 年在文獻[2][3]提出來的。它被廣泛地應用到圖像配準、運動追蹤，目標檢測等方面。根據計算圖像的部分像素或是全部像素的運動，可以將它分為稀疏光流法和稠密光流法。光流法的原理依賴于三個假設。第一個是灰度不變假設 I ( x + dx,y + dy,t + dt ) =I ( x, y , t )，它要求t 時刻在坐標(x,y)處的灰度值 I ( x, y , t)和t+1 時刻的灰度值I ( x + dx, y + dy, t +dt )相等。第二個是要求相鄰圖像幀是小運動。第三個是空間一致性，它假定相鄰像素之間具有相同的運動。光流法在視覺SLAM 中的前端被廣泛使用，它具有響應速度快、計算量小等特點。但同時光流法受光照影響大、相機發生大尺度運動容易跟丟、圖像的單個像素點沒有區分度等缺點。

為了更好地適應實際環境，需要對傳統的LK 算法進行改進。為提高光流追蹤的速度，我們采用逆向組合[4]的方法。傳統的LK 算法在更新參數的過程中需要不斷計算雅可比矩陣和Hessian 矩陣，降低了光流的追蹤效率。而使用逆向組合的方法可以使得算法在迭代過程中只需要計算一次Hessian 矩陣，這有效提高了光流算法的效率。傳統的光流法對光照變化比較敏感，所以使用一種局部差異平方和[5]的形式，將處理單個像素點的形式轉化為處理像素塊的形式。我們把相鄰圖像間對應像素塊的運動追蹤問題轉換為估計變換矩陣T。該變換矩陣T 最小化對應像素塊之間的差值，我們將它定義為如下的殘差形式。

圖1 前端流程

2.2 IMU 預積分

由于純視覺的定位方式難以實現魯棒的定位要求，所以采用視覺融合IMU 的方式可以更好的讓系統適應現實需求。根據IMU 模型，我們得到連續時間下的運動模型。

圖2 不同噪聲條件的參考軌跡和估計軌跡

2.3 系統框架

該系統主要分為基于光流法的前端、基于LM 非線性優化的后端、系統初始化部分。首先，光流法前端不斷接收單目相機的圖像數據，并對圖像進行FAST 角點的提取，再使用光流法追蹤相鄰圖像幀間的特征點，從而得到運動軌跡。同時，系統不斷接收IMU 的加速度和角速度的數據，并對圖像幀間的IMU 數據進行積分處理。然后進行系統初始化，該方法主要是借鑒了VINSMONO 的系統初始化[7]，可以恢復出尺度、重力、速度和IMU 的bias，從而將視覺軌跡和IMU 軌跡對齊。而后，將視覺和IMU 得到的軌跡放入到后端非線性優化當中，該部分主要使用了LM 算法不斷迭代優化相機位姿，從而得到準確的相機運動軌跡。

3 實驗與分析

該系統的實驗環境是ubuntu18.04，使用多線程進行數據傳輸。同時，使用了pangolin 庫可視化相機的運動軌跡。此外，生成自定義軌跡測試系統，并設置了三種噪聲情況，分別是噪聲軌跡1: acc-n: 0.02,gyr-n: 0.015,acc-w: 0.0001,gyr-w: 1.0e-5；噪聲軌跡2: acc-n: 0.04,gyr-n: 0.03,acc-w: 0.0002,gyr-w: 2.0e-5；噪聲軌跡3: acc-n: 0.2,gyr-n: 0.15,acc-w: 0.001,gyr-w: 1.0e-4。我們使用evo 評估系統在三種噪聲情況下的運動誤差，得到了不同情況下參考軌跡與估計軌跡的絕對誤差，具體數值如表1所示。不同的噪聲情況得到的參考軌跡和估計軌跡如圖2 所示。實驗結果表明，系統估計的軌跡能較好擬合參考軌跡，證明了系統對不同噪聲環境的有效性和魯棒性。但是，從表1 的數據分析，該系統會出現殘差平方和較大的情況，而本次實驗的場景不夠豐富，且穩定性不足，因此，該系統還有待進一步完善。

表1 不同噪聲條件的絕對軌跡誤差