一種基于改進光流法的視覺慣性狀態(tài)估計器

劉奕杉

（廣東工業(yè)大學(xué)，廣東 廣州510006）

1 概述

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

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

2 正文

2.1 光流法

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

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

圖1 前端流程

2.2 IMU 預(yù)積分

由于純視覺的定位方式難以實現(xiàn)魯棒的定位要求，所以采用視覺融合IMU 的方式可以更好的讓系統(tǒng)適應(yīng)現(xiàn)實需求。根據(jù)IMU 模型，我們得到連續(xù)時間下的運動模型。

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

2.3 系統(tǒng)框架

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

3 實驗與分析

該系統(tǒng)的實驗環(huán)境是ubuntu18.04，使用多線程進行數(shù)據(jù)傳輸。同時，使用了pangolin 庫可視化相機的運動軌跡。此外，生成自定義軌跡測試系統(tǒng)，并設(shè)置了三種噪聲情況，分別是噪聲軌跡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 評估系統(tǒng)在三種噪聲情況下的運動誤差，得到了不同情況下參考軌跡與估計軌跡的絕對誤差，具體數(shù)值如表1所示。不同的噪聲情況得到的參考軌跡和估計軌跡如圖2 所示。實驗結(jié)果表明，系統(tǒng)估計的軌跡能較好擬合參考軌跡，證明了系統(tǒng)對不同噪聲環(huán)境的有效性和魯棒性。但是，從表1 的數(shù)據(jù)分析，該系統(tǒng)會出現(xiàn)殘差平方和較大的情況，而本次實驗的場景不夠豐富，且穩(wěn)定性不足，因此，該系統(tǒng)還有待進一步完善。

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