融合慣導(dǎo)信息的單目視覺室內(nèi)定位方法

李夢(mèng)珍,余敏,俞佳豪

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

0 引 言

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

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

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

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

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

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

1.1 基本流程

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

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

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

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

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

1.2 初始對(duì)準(zhǔn)

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

(1)

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

(2)

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

1.3 平移矩陣解算

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

(3)

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

(4)

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

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

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

1.4.1 積分法

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

(5)

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

1.4.2 互補(bǔ)濾波

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

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

(6)

其中:

(7)

(8)

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

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

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

(9)

式中：x1、y1為特征點(diǎn)在前一時(shí)刻的像素位置;x2、y2為同一特征點(diǎn)在下一時(shí)刻的像素位置;Δx、Δy分別連續(xù)兩場(chǎng)圖像中的平移參數(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)

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

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

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

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

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

3.1 融合模型

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

預(yù)測(cè)模型:

xk+1=f(xk)，

(13)

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

(14)

(15)

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

(16)

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

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

(17)

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

(18)

(19)

修正模型:

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

(20)

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

(21)

(22)

(23)

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

(24)

(25)

由于觀測(cè)向量的設(shè)置不同,針對(duì)于慣性觀測(cè)空間、視覺觀測(cè)空間到狀態(tài)空間的映射也不同．本文中設(shè)置慣導(dǎo)觀測(cè)空間到狀態(tài)空間的映射關(guān)系hi(x)以及視覺觀測(cè)空間到狀態(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ù),首先通過(guò)實(shí)驗(yàn)分析該方法修正后的陀螺儀讀數(shù)積分結(jié)果與直接對(duì)陀螺儀積分結(jié)果之間的差異．

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

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

圖2 角度值波形對(duì)比圖

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

圖3 角度值誤差對(duì)比圖

從圖中可以看出:陀螺儀積分得到的角度隨著時(shí)間的增加積分漂移和溫度漂移帶來(lái)的誤差越來(lái)越大;互補(bǔ)濾波利用加速度計(jì)的讀數(shù)和上一次融合后的角度對(duì)陀螺儀的讀數(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)場(chǎng)所是長(zhǎng)、寬均為6 m的分布式實(shí)驗(yàn)研究室,室內(nèi)有若干L型工作臺(tái)、沙發(fā)、桌子等辦公設(shè)置．實(shí)驗(yàn)路徑規(guī)劃如圖4中的紅線所示,從圓圈處開始到三角形處結(jié)束．

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

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

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

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

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

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

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

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

由圖可知:IMU定位方法在短時(shí)間內(nèi)具有較高的定位精度,但隨著時(shí)間的推移,軌跡偏差越來(lá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é)束語(yǔ)

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