融合慣導信息的單目視覺室內定位方法

李夢珍,余敏,俞佳豪

(江西師范大學 計算機信息工程學院,江西 南昌 330022)

0 引 言

隨著定位技術的發展,人們的生活也越來越便利．在戶外,諸如車載導航儀、百度地圖、滴滴打車等基于位置服務的產品及應用,已成為人們生活中不可或缺的部分[1];人們對于室內精準位置服務的需求同樣強烈,比如消防救援時的人員定位,大型商場中的導購服務,醫院、養老院的智能看護等[2]．

室內定位技術種類多種多樣,同時定位與制圖(SLAM)以其可在未知環境中自主作業、無需提前布設相關設備、傳感器體積小、功耗低、信息獲取豐富等特點,成為當前研究的熱點．基于慣性測量單元 (IMU)的定位方法以實時性高、抗干擾能力強、成本低、功耗低的特性持續被室內定位領域所關注．

單目視覺SLAM作為視覺SLAM的一個分支,依靠單目攝像機獲取的環境紋理信息作為信息源,其處理效果直接受環境紋理條件影響,難以在紋理貧乏、光線不足或過強的區域中可靠地工作[3]．而IMU具備完全自主的測量特性,可穩定獲取姿態數據,能有效彌補視覺SLAM在紋理貧乏、光線不足或過強的區域中環境信息的缺失,提升系統定位的穩定性,同時高精度的視覺定位結果可以有效修正IMU存在的累計誤差,提升系統定位的精度．因此,針對單目視覺傳感器與IMU的數據融合技術研究成為了新的研究熱點．

近年來,國內外在視覺與慣導的融合定位方面取得了較多的研究進展,其中基于濾波的單目視覺與慣導融合方法越來越多[4]．現有的技術多采用卡爾曼濾波進行數據融合[5],且直接將慣性組件測量的角速度與視覺信息融合．但是陀螺儀積分得到的角度隨著時間的增加，積分漂移和溫度漂移帶來的誤差比較大,且測量數據與預測數據間存在非線性關系．依據單目視覺與IMU融合現狀,本文以單目視覺傳感器與IMU數據為基礎,開展了融合兩種傳感器數據的室內定位方法研究．首先利用四參數擬合模型將單目攝像機得到的圖像數據處理成定位定姿數據;同時,引用互補濾波對陀螺儀讀數進行修正;最后將兩種傳感器處理得到的數據作為觀測值輸入到擴展卡爾曼濾波器中,得到載體最優位置信息．系統架構如圖1所示．

圖1 融合定位方法架構圖

1 慣性導航定位

1.1 基本流程

該定位方法是一種利用上一時刻位置信息與當前時刻位姿變化對當前時刻載體位置信息進行計算的定位方法．定位步驟如下:

1)初始對準獲取載體的初始位姿;

2)通過慣性傳感器測量載體的加速度值和角速度值;

3)對步驟2)中測得數據進行處理獲得載體的速度位移、旋轉角度．

4)利用上一時刻載體位置以及該時刻發生的速度位移、旋轉角度進行航跡推算,獲得載體當前時刻的位置．

1.2 初始對準

本文使用的加速度計、陀螺儀精度較低,對地球重力以及地球自轉角速度不敏感,所以無法使用雙矢量定姿進行對準．若傳感器坐標系與載體坐標系重合,在載體不劇烈運動的情況下,可認為加速度計測出的加速度表示重力加速度,根據這一特性,可解算出俯仰角α和滾轉角γ,計算方法如下:

(1)

式(1)便是利用加速度計解算俯仰角和滾轉角的方法,其中ax、ay、az為三軸方向上加速度計的讀數．對于航偏角β,由于手機中自帶磁力計,可輸出地磁信息,因此本文利用磁力計數據來計算航偏角,計算公式如式(2)所示．

(2)

式中:mx、my、mz分別代表磁傳感器在三個軸上的輸出．通過上述方法可以得到一個粗對準的結果,為了使對準結果更加精確,還要進行精對準,精對準一般是利用卡爾曼濾波實現的,本文參照文獻[6]提出的一種把比力的東向和北向測量值作為觀測量的方法進行了初始精對準．

1.3 平移矩陣解算

本文選用以載體重心為原點的“東北天”世界坐標系作為導航系,為了獲取載體載在導航坐標系中的位置信息,我們首先需要將慣性組件所測量的加速度轉換到導航坐標系中,轉換公式為

(3)

獲取三軸方向上相對位移的方法有很多,本文采用最簡便的方法:對三軸方向上的加速度值進行積分．計算方法為

(4)

式中:Vm、Vn分別表示tm、tn時刻的線速度;at為tm時刻載體在某一個方向上的加速度;Sn為tn時刻某一方向上的位移;Sm為tm時刻對應方向上的位移;gt為對應方向上的重力加速度．

1.4 旋轉矩陣解算

本文采用歐拉法表示旋轉,因此解算出三個歐拉角是獲取旋轉矩陣的關鍵步驟．

1.4.1 積分法

對三軸方向上陀螺儀讀數進行積分是最常用的獲取三個歐拉角的方法,三軸上的角度計算方法為

(5)

式中:Gx、Gy、Gz為三軸方向上的陀螺儀的讀數;C為采樣頻率．

1.4.2 互補濾波

加速度計的靜態穩定性較好,但在載體運動時所得到的加速度數據相對不可靠,取瞬時值計算傾角誤差比較大且無法測得航偏角;陀螺儀積分得到的角度不受載體加速度的影響,但是隨著時間的增加積分漂移和溫度漂移帶來的誤差比較大[7]．

為了得到更精準的姿態信息,本文利用互補濾波對陀螺儀讀數進行修正．首先利用加速度計的讀數計算出三軸方向上的重力分量;隨后利用上一時刻姿態信息估計該時刻各軸上的重力分量;然后求出估算結果與實際測量的重力分量之間的誤差;最終利用重力分量間的誤差、比例系數對陀螺儀讀數進行修正．計算方法為

(6)

其中:

(7)

(8)

式中:gx、gy、gz為修正后的陀螺儀數據;IntegralX、IntegralY、IntegralZ為三軸方向上陀螺儀的積分結果;ehx、ehy、ehz為利用加速度計讀數估算出的三軸方向上的重力向量和實際測量的重力向量之間的差;sf為采樣頻率；kp為比例系數．對修正后的陀螺儀數據按式(5)進行積分即可獲取三個歐拉角從而得到旋轉矩陣．

2 單目視覺數據解算

根據坐標轉換模型,若已知空間中不共線的3點在兩個坐標系下的坐標,便能唯一確定兩坐標系間的位置關系[9]．單目攝像頭獲取的圖像信息可以構成一個像素坐標系,像素坐標系為二維平面直角坐標系,因此兩個像素坐標系之間的轉換可以用四參數擬合模型表示,其矩陣表示如下:

(9)

式中：x1、y1為特征點在前一時刻的像素位置;x2、y2為同一特征點在下一時刻的像素位置;Δx、Δy分別連續兩場圖像中的平移參數;θ指連續兩像素坐標系間的旋轉參數;k為兩像素坐標系的尺度比．

令C=kcosθ,S=ksinθ,將式(9)展開,可得矩陣的線性方程組如下:

(10)

上述線性方程組可用式(11)的矩陣表達:

A×X=B，

(11)

其中:

利用最小二乘法可解算出矩陣X的值,從而可獲取平移參數Δx、Δy．

隨后利用C、S的計算結果及三角函數誘導公式,可得旋轉參數θ的計算公式如下:

(12)

通過上述方式得到的是在像素坐標系下的位姿變化信息．為了能將導航數據與視覺數據有效融合,我們首先利用“張氏標定法”確立像素坐標系與世界坐標系之間的轉換關系,然后將兩幅圖像中的像素坐標轉換成世界坐標,最后利用上述原理對單目視覺數據進行解算．

3 數據融合

為了充分利用慣導、單目視覺傳感器所得到的信息資源,發揮多個傳感器的聯合優勢,消除單一傳感器的局限性,越來越多的研究人員開始注重數據融合技術的發展．數據融合技術可在一定的準則下對兩者及以上傳感器得到的數據加以自動分析、綜合、支配和使用,從而獲取對被測對象的一致性描述,得到最優估計[10]．

卡爾曼濾波是數據融合常用的方法之一,它是一種利用線性系統狀態方程對系統狀態進行最優估計的算法．考慮到姿態數據及位置數據間的非線性關系,本文采用卡爾曼濾波的非線性版本—擴展卡爾曼濾波融合定位定姿數據．

本文中的多傳感器信息融合基本原理如下:慣性傳感器的觀測方程更新后得到的系統狀態量及系統協方差矩陣,將作為視覺傳感器更新過程中的系統預測狀態量和系統預測協方差矩陣進行姿態更新．將視覺傳感器得到的更新后的系統狀態量及系統協方差矩陣作為下一時刻的預測值進行迭代．

3.1 融合模型

擴展卡爾曼濾波主要分為預測和修正兩個部分．預測模型主要通過上一時刻最優估計以及外部干擾因素來預測下一時刻的狀態和協方差矩陣;修正模型主要通過測量值來修正估計結果和協方差矩陣．本文中所利用的模型公式如下:

預測模型:

xk+1=f(xk)，

(13)

pk+1=F×pk×FT+Q，

(14)

(15)

隨后依據本文算法需求設置狀態向量如下:

(16)

式中:px、py表示載體的位置;vx、vy表示載體的速度．

預測方程設置為:

(17)

由上述預測方程可知:預測過程是在載體做勻速運動的基礎上進行的,因此速度成了本文中的干擾項．速度對應的解算方法如下:

(18)

(19)

修正模型:

xk+1=xk+K×(zk+1-h(xk))，

(20)

pk+1=(I-K×Hj)×pk，

(21)

(22)

(23)

本文中由于兩種傳感器所得觀測數據不同,因此為兩者設置不同的觀測向量,視覺傳感器的觀測向量zm及慣性傳感器的觀測向量zi設置如下:

(24)

(25)

由于觀測向量的設置不同,針對于慣性觀測空間、視覺觀測空間到狀態空間的映射也不同．本文中設置慣導觀測空間到狀態空間的映射關系hi(x)以及視覺觀測空間到狀態空間的映射關系hm(x)如下:

(26)

(27)

式中,Ra、Rb的值為

(28)

4 數據融合

完成了前期的一系列研究工作后,根據姿態解算方法及數據融合方法,將單目視覺傳感器和慣導的融合定位方法應用于室內定位中進行實驗驗證．

4.1 互補濾波算法驗證

本文采用互補濾波修正陀螺儀讀數,首先通過實驗分析該方法修正后的陀螺儀讀數積分結果與直接對陀螺儀積分結果之間的差異．

實驗之前利用六位置標定法對所用到的加速度計與陀螺儀進行標定,標定后的加速度計與陀螺儀的輸出模型如下:

實驗利用華為MATE 8內置的加速度計、陀螺儀采集行人在變速行走過程中的加速度值與角速度值,之后利用采集到的數據計算行人航偏角,200 ms內的計算結果如圖2所示,實線為加速度與陀螺儀互補之后得到的角度值,虛線為陀螺儀積分得到的角度值．從圖中可以看出,兩者得到的角度值存在一定的差異．

圖2 角度值波形對比圖

為了更直觀地對比兩者的差異,實驗將手機水平保持固定角度為0°,隨后進行變速運動．實驗結束后分別求互補濾波得到的角度值、陀螺儀積分得到的角度值與實際角度之間的差值,最終得到的結果如圖3所示.

圖3 角度值誤差對比圖

從圖中可以看出:陀螺儀積分得到的角度隨著時間的增加積分漂移和溫度漂移帶來的誤差越來越大;互補濾波利用加速度計的讀數和上一次融合后的角度對陀螺儀的讀數進行了修正,有效地抑制了陀螺儀的溫漂,提高了行人航偏角計算的準確度．

4.2 融合定位方法驗證

4.2.1 實驗環境設置

實驗選擇在江西師范大學瑤湖校區先骕樓進行,實驗場所是長、寬均為6 m的分布式實驗研究室,室內有若干L型工作臺、沙發、桌子等辦公設置．實驗路徑規劃如圖4中的紅線所示,從圓圈處開始到三角形處結束．

圖4 實驗路徑選擇圖

4.2.2 融合定位方法驗證

實驗按圖4所示的路徑行走,在行走的同時利用華為MATE 8內置的加速度計、陀螺儀與單目視覺相機進行數據采集,在利用攝像頭拍攝視頻的同時,以0.01 s的頻率收集手機加速度計與陀螺儀數據,數據采集過程用時約為1.5 min,總位移約14 m．

為了明確各種定位方法定位結果的差異,接下來分別采用IMU定位算法、單目視覺定位算法與融合定位算法進行了對比實驗．圖5為使用IMU定位過程中在27.05 s、47.50 s產生的效果圖,圖6為使用單目視覺定位過程中角點提取與軌跡繪制的效果圖．

(a)27.05 s 定位效果圖 (b)47.50 s 定位效果圖圖5 慣性導航定位實驗

(a)特征提取效果圖 (b)視覺定位建圖過程圖6 單目視覺定位實驗

將IMU與單目相機所得數據利用擴展卡爾曼濾波及matlab進行融合仿真實驗,得到行人最優位置估計,最后將三種定位結果繪制成如圖7所示的軌跡對比圖．

圖7 實驗軌跡對比圖

由圖可知:IMU定位方法在短時間內具有較高的定位精度,但隨著時間的推移,軌跡偏差越來越大;單目視覺定位方法定位精度較高,但由于單目攝像頭易受環境特征影響,定位軌跡在局部發生偏移;融合定位方法最終所得軌跡與實際行走軌跡重合度最高．

為進一步分析融合定位方法的有效性,求出1.5 min內各定位方法的最大定位誤差,結果如表1所示．相比較于IMU定位方法與單目視覺定位方法,融合定位方法最大定位誤差有所降低．

表1 定位誤差 m

因此,融合定位方法能有效地融合IMU與單目視覺傳感器所得數據,減少IMU定位所產生的累計誤差以及單目視覺定位因環境特征變換較快而產生的跟蹤丟失等不利影響,使得室內定位的準確性和穩定性得到有效提升．

5 結束語

本文利用擴展卡爾曼濾波方法對單目視覺傳感器與慣性傳感器數據進行融合以提高室內定位的準確性和穩定性．首先利用四參數擬合模型將圖像數據轉換成姿態數據,然后引入互補濾波對陀螺儀數據進行修正,濾除傳感器溫漂,提高數據源的質量,最后將姿態數據作為觀測值輸入到擴展卡爾曼濾波器中,得到載體的最優位置．實驗結果表明,相較于IMU定位方法與單目視覺定位方法,融合定位方法能有效地降低室內定位誤差,使得室內定位的準確性和穩定性得到有效提升．但本文在融合算法上,主要采用擴展卡爾曼濾波技術,擴展卡爾曼濾波在利用非線性關系時會引入線性誤差,且計算復雜度比較高．因此,引入更好的傳感器數據融合是下一步需要解決的重要問題之一．