李夢珍,余敏,俞佳豪

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

0 引 言

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

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

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

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

圖1 融合定位方法架構(gòu)圖

1 慣性導(dǎo)航定位

1.1 基本流程

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

1)初始對準(zhǔn)獲取載體的初始位姿;

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

3)對步驟2)中測得數(shù)據(jù)進(jìn)行處理獲得載體的速度位移、旋轉(zhuǎn)角度．

4)利用上一時刻載體位置以及該時刻發(fā)生的速度位移、旋轉(zhuǎn)角度進(jìn)行航跡推算,獲得載體當(dāng)前時刻的位置．

1.2 初始對準(zhǔn)

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

(1)

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

(2)

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

1.3 平移矩陣解算

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

(3)

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

(4)

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

1.4 旋轉(zhuǎn)矩陣解算

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

1.4.1 積分法

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

(5)

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

1.4.2 互補(bǔ)濾波

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

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

(6)

其中:

(7)

(8)

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

2 單目視覺數(shù)據(jù)解算

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

(9)

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

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

(10)

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

A×X=B，

(11)

其中:

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

隨后利用C、S的計(jì)算結(jié)果及三角函數(shù)誘導(dǎo)公式,可得旋轉(zhuǎn)參數(shù)θ的計(jì)算公式如下:

(12)

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

3 數(shù)據(jù)融合

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

卡爾曼濾波是數(shù)據(jù)融合常用的方法之一,它是一種利用線性系統(tǒng)狀態(tài)方程對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法．考慮到姿態(tài)數(shù)據(jù)及位置數(shù)據(jù)間的非線性關(guān)系,本文采用卡爾曼濾波的非線性版本—擴(kuò)展卡爾曼濾波融合定位定姿數(shù)據(jù)．

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

3.1 融合模型

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

預(yù)測模型:

xk+1=f(xk)，

(13)

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

(14)

(15)

隨后依據(jù)本文算法需求設(shè)置狀態(tài)向量如下:

(16)

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

預(yù)測方程設(shè)置為:

(17)

由上述預(yù)測方程可知:預(yù)測過程是在載體做勻速運(yùn)動的基礎(chǔ)上進(jìn)行的,因此速度成了本文中的干擾項(xiàng)．速度對應(yīng)的解算方法如下:

(18)

(19)

修正模型:

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

(20)

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

(21)

(22)

(23)

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

(24)

(25)

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

(26)

(27)

式中,Ra、Rb的值為

(28)

4 數(shù)據(jù)融合

完成了前期的一系列研究工作后,根據(jù)姿態(tài)解算方法及數(shù)據(jù)融合方法,將單目視覺傳感器和慣導(dǎo)的融合定位方法應(yīng)用于室內(nèi)定位中進(jìn)行實(shí)驗(yàn)驗(yàn)證．

4.1 互補(bǔ)濾波算法驗(yàn)證

本文采用互補(bǔ)濾波修正陀螺儀讀數(shù),首先通過實(shí)驗(yàn)分析該方法修正后的陀螺儀讀數(shù)積分結(jié)果與直接對陀螺儀積分結(jié)果之間的差異．

實(shí)驗(yàn)之前利用六位置標(biāo)定法對所用到的加速度計(jì)與陀螺儀進(jìn)行標(biāo)定,標(biāo)定后的加速度計(jì)與陀螺儀的輸出模型如下:

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

圖2 角度值波形對比圖

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

圖3 角度值誤差對比圖

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

4.2 融合定位方法驗(yàn)證

4.2.1 實(shí)驗(yàn)環(huán)境設(shè)置

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

圖4 實(shí)驗(yàn)路徑選擇圖

4.2.2 融合定位方法驗(yàn)證

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

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

(a)27.05 s 定位效果圖 (b)47.50 s 定位效果圖圖5 慣性導(dǎo)航定位實(shí)驗(yàn)

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

將IMU與單目相機(jī)所得數(shù)據(jù)利用擴(kuò)展卡爾曼濾波及matlab進(jìn)行融合仿真實(shí)驗(yàn),得到行人最優(yōu)位置估計(jì),最后將三種定位結(jié)果繪制成如圖7所示的軌跡對比圖．

圖7 實(shí)驗(yàn)軌跡對比圖

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

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

表1 定位誤差 m

因此,融合定位方法能有效地融合IMU與單目視覺傳感器所得數(shù)據(jù),減少IMU定位所產(chǎn)生的累計(jì)誤差以及單目視覺定位因環(huán)境特征變換較快而產(chǎn)生的跟蹤丟失等不利影響,使得室內(nèi)定位的準(zhǔn)確性和穩(wěn)定性得到有效提升．

5 結(jié)束語

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