劉奕杉

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

1 概述

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

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

2 正文

2.1 光流法

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

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

圖1 前端流程

2.2 IMU 預(yù)積分

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

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

2.3 系統(tǒng)框架

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

3 實(shí)驗(yàn)與分析

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

表1 不同噪聲條件的絕對(duì)軌跡誤差