劉奕杉
(廣東工業(yè)大學(xué),廣東 廣州510006)
實時定位和建圖(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é)合改進(jìn)的光流法和IMU 信息,然后基于LM 的非線性優(yōu)化算法,實現(xiàn)了一個精度較高的視覺里程計。本文將從改進(jìn)的光流法、慣性單元預(yù)積分,系統(tǒng)框架三部分對該方案進(jìn)行描述,最后將通過實驗展示該系統(tǒng)的運行效果。
Lucas-Kanade 光流法是Bruce D. Lucas 等人于1981 年在文獻(xiàn)[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 算法進(jìn)行改進(jìn)。為提高光流追蹤的速度,我們采用逆向組合[4]的方法。傳統(tǒng)的LK 算法在更新參數(shù)的過程中需要不斷計算雅可比矩陣和Hessian 矩陣,降低了光流的追蹤效率。而使用逆向組合的方法可以使得算法在迭代過程中只需要計算一次Hessian 矩陣,這有效提高了光流算法的效率。傳統(tǒng)的光流法對光照變化比較敏感,所以使用一種局部差異平方和[5]的形式,將處理單個像素點的形式轉(zhuǎn)化為處理像素塊的形式。我們把相鄰圖像間對應(yīng)像素塊的運動追蹤問題轉(zhuǎn)換為估計變換矩陣T。該變換矩陣T 最小化對應(yīng)像素塊之間的差值,我們將它定義為如下的殘差形式。
圖1 前端流程
由于純視覺的定位方式難以實現(xiàn)魯棒的定位要求,所以采用視覺融合IMU 的方式可以更好的讓系統(tǒng)適應(yīng)現(xiàn)實需求。根據(jù)IMU 模型,我們得到連續(xù)時間下的運動模型。
圖2 不同噪聲條件的參考軌跡和估計軌跡
該系統(tǒng)主要分為基于光流法的前端、基于LM 非線性優(yōu)化的后端、系統(tǒng)初始化部分。首先,光流法前端不斷接收單目相機的圖像數(shù)據(jù),并對圖像進(jìn)行FAST 角點的提取,再使用光流法追蹤相鄰圖像幀間的特征點,從而得到運動軌跡。同時,系統(tǒng)不斷接收IMU 的加速度和角速度的數(shù)據(jù),并對圖像幀間的IMU 數(shù)據(jù)進(jìn)行積分處理。然后進(jìn)行系統(tǒng)初始化,該方法主要是借鑒了VINSMONO 的系統(tǒng)初始化[7],可以恢復(fù)出尺度、重力、速度和IMU 的bias,從而將視覺軌跡和IMU 軌跡對齊。而后,將視覺和IMU 得到的軌跡放入到后端非線性優(yōu)化當(dāng)中,該部分主要使用了LM 算法不斷迭代優(yōu)化相機位姿,從而得到準(zhǔn)確的相機運動軌跡。
該系統(tǒng)的實驗環(huán)境是ubuntu18.04,使用多線程進(jìn)行數(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)還有待進(jìn)一步完善。
表1 不同噪聲條件的絕對軌跡誤差