賈文淵，魏 彤

(北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院，北京100191）

基于計算機視覺/GPS/MG姿態(tài)測量的盲人行走組合導(dǎo)航方法研究

賈文淵，魏 彤

(北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院，北京100191）

電子導(dǎo)盲輔助裝置(ETA）是解決盲人出行困難的重要手段，而導(dǎo)航是ETA的關(guān)鍵技術(shù)。現(xiàn)有的ETA主要用GPS來定位定向，但在城市環(huán)境中經(jīng)常存在GPS信號遮擋導(dǎo)致導(dǎo)航信息丟失的問題。針對該問題，利用視覺導(dǎo)航短時間內(nèi)定位精度高，輸出連續(xù)的優(yōu)點以及MG(Magnetic Gravity）姿態(tài)測量可補償姿態(tài)積累誤差的優(yōu)點，提出一種基于視覺、GPS和MG姿態(tài)測量的盲人行走組合導(dǎo)航算法。該方法構(gòu)建系統(tǒng)誤差模型并以Kalman濾波為框架。仿真和實驗結(jié)果表明，提出的組合導(dǎo)航算法準(zhǔn)確度優(yōu)于單獨的導(dǎo)航算法，滿足盲人戶外安全出行導(dǎo)航的需求。

盲人出行；視覺導(dǎo)航；GPS；加速度計；磁力計；組合導(dǎo)航；Kalman濾波

Abstract：ETA(electronic travel aids）is an important means to solve the difficulties of the blind travel，in which navigation is the crucial technology of ETA.The existing ETA mainly uses the GPS to get the position and the orientation，however，the navigation information is often lost because of occlusion in the urban.Aiming at the problem，this paper propo?ses an integrated navigation method of the blind walking based on vision，GPS and MG attitude measurement.The method takes advantage of the vision navigation which has high positioning accuracy in short time and continuous output，and MG attitude measurement compensated the attitude accumulation errors.The method constructs the system error model and uses Kalman filter as the framework.Simulation and experimental results show that the proposed integrated navigation algorithm is superior to the single navigation algorithm.

Key words：blind travel;visual navigation;GPS;accelerometer;magnetometer;integrated navigation;Kalman filter

0 引言

人類80%的信息都是通過視覺獲得。盲人喪失了視覺，也就喪失了信息獲取的最重要渠道，對盲人日常生活造成極大的障礙，尤其制約了盲人的戶外出行活動［1］。盲人作為社會上的一類特殊群體，如何讓他們安全獨立的行走，成為當(dāng)前研究的重要方向。

電子導(dǎo)盲輔助裝置是幫助盲人安全出行的重要手段，主要解決盲人出行中避障和導(dǎo)航的問題。避障問題主要是盲人能夠通過誘導(dǎo)準(zhǔn)確繞過障礙物，導(dǎo)航問題主要是指盲人知道自身所處的位置以及行走方向。導(dǎo)航的準(zhǔn)確性直接決定盲人能否使用導(dǎo)盲裝置大范圍地安全出行。

目前，導(dǎo)航定位技術(shù)包括絕對定位和相對定位兩種。其中，常用的絕對定位技術(shù)有衛(wèi)星導(dǎo)航定位［2］和無線電定位；相對定位技術(shù)有慣性導(dǎo)航［3］、激光定位、航位推測技術(shù)和視覺導(dǎo)航［4］等。為實現(xiàn)盲人大范圍安全出行，須獲得盲人的全局位置和行走方向。本文采用GPS絕對定位方法，GPS在信號受環(huán)境影響較小的條件下，具有定位精度高、定位全局性和高可靠的優(yōu)點，能滿足盲人基本定位的需求，但對于城市、峽谷等區(qū)域，信號容易受到干擾，GPS定位效果較差［5］，且GPS無法獲得準(zhǔn)確航向信息。相對定位方法具有小范圍內(nèi)定位精度高、輸出連續(xù)性和導(dǎo)航參數(shù)全等優(yōu)點，可實現(xiàn)對GPS的優(yōu)勢互補。廣泛應(yīng)用于盲人行走的相對定位技術(shù)有慣性導(dǎo)航和視覺導(dǎo)航。慣性導(dǎo)航具有短時間定位精度高和輸出航向信息的特點，但其誤差隨時間迅速積累；采用較高精度的慣性器件，可以保證一段時間導(dǎo)航的準(zhǔn)確性，但成本也高。成本較低的MEMS慣性器件則多采用行人航位推算方法，能保證一定程度的導(dǎo)航精度，但受行人行走模型的影響，并不普遍適用于盲人。視覺導(dǎo)航技術(shù)通過剛性掛載在目標(biāo)上的攝像機采集圖像數(shù)據(jù)，根據(jù)圖像特征和前后圖像幀間的運動約束進行位姿估計，具有短距離導(dǎo)航精度高的特性。但該技術(shù)也是通過累積計算的局部導(dǎo)航，故存在誤差隨距離累積的缺陷［6］。因此，需采用一種或多種相對定位技術(shù)來對GPS實現(xiàn)組合。

一般的GPS/INS組合導(dǎo)航方式雖然能在一定時間內(nèi)有效提高定位精度，可是系統(tǒng)誤差隨著時間而逐漸積累，并且在遮擋嚴(yán)重區(qū)域又得不到準(zhǔn)確的GPS定位結(jié)果來修正其初始化，最終導(dǎo)致目標(biāo)定位失敗。

MG姿態(tài)測量是指利用加速度計和磁力計實現(xiàn)載體姿態(tài)測量的方法，測量誤差主要受載體運動狀態(tài)影響，不存在誤差隨時間累積的缺陷?？紤]到盲人用戶的特殊性，本文提出一種基于視覺、GPS 和 MG 姿態(tài)測量的組合導(dǎo)航定位方法［7?8］。GPS具有定位誤差不隨時間積累的優(yōu)點，同樣MG測量姿態(tài)也不隨時間積累，可實現(xiàn)與視覺導(dǎo)航的互補性，能在一定區(qū)域內(nèi)實現(xiàn)盲人準(zhǔn)確導(dǎo)航。

1 坐標(biāo)系定義

本文涉及的坐標(biāo)系均為右手坐標(biāo)系，主要包括地球坐標(biāo)系(e）、地理坐標(biāo)系(t）、導(dǎo)航坐標(biāo)系(n）、載體坐標(biāo)系(b）和相機坐標(biāo)系(c），如圖 1所示。

圖1 坐標(biāo)系定義Fig.1 Coordinate frames

地球坐標(biāo)系(Oe?XeYeZe）以地球質(zhì)心為坐標(biāo)原點，Ze軸指向地球北極，Xe軸指向地球赤道平面與格林尼治子午圈的交點。

地理坐標(biāo)系(Ot?XtYtZt）以盲人所在點為原點，Xt軸沿當(dāng)?shù)鼐暰€指向東，Yt軸沿當(dāng)?shù)刈游缇€指向北，Zt軸沿當(dāng)?shù)氐乩泶咕€指向上。

導(dǎo)航坐標(biāo)系(On?XnYnZn）是導(dǎo)航系統(tǒng)在求解導(dǎo)航參數(shù)時所采用的坐標(biāo)系，通常選取地理坐標(biāo)系。

載體坐標(biāo)系(Ob?XbYbZb）為MEMS慣性器件坐標(biāo)系，主要用于系統(tǒng)初始化。

相機坐標(biāo)系(Oc?XcYcZc）以左相機光心為原點，Xc軸沿像素陣列指向右，Yc軸沿像素陣列指向下，Zc軸沿光軸指向視場方向。

通過確定各坐標(biāo)系之間關(guān)系可實現(xiàn)不同導(dǎo)航方式之間的信息融合。

2 視覺導(dǎo)航原理

視覺導(dǎo)航主要通過特征檢測、特征匹配、特征點跟蹤和運動估計來實現(xiàn)相機坐標(biāo)系相鄰時刻的位姿變換［9］?？紤]兩個相鄰的成像時刻tk－1和tk，時間間隔為Δt。設(shè)tk－1時刻相機載體的速度和角速度分別為。記tk－1到tk時刻匹配且跟蹤成功的N個特征點的相機坐標(biāo)為，tk時刻相機坐標(biāo)系相對于tk－1時刻的旋轉(zhuǎn)矩陣為，平移向量為。相機坐標(biāo)系的旋轉(zhuǎn)和平移運動如圖2所示。

圖2 相機坐標(biāo)系的運動Fig.2 The motion of the camera coordinate system

由前后圖像幀之間特征點關(guān)系可知，對第i個特征點有：

為實現(xiàn)視覺導(dǎo)航和GPS組合定位，需將tk時刻獲得相機位置轉(zhuǎn)換為導(dǎo)航坐標(biāo)系下位置，因此要對初始時刻相機坐標(biāo)系進行初始對準(zhǔn)，可通過標(biāo)定MG載體與相機之間的安裝矩陣以及MG載體的初始捷聯(lián)矩陣來實現(xiàn)。初始對準(zhǔn)矩陣為：

故tk時刻視覺獲得的載體的導(dǎo)航信息如下：

3 MG姿態(tài)測量系統(tǒng)的姿態(tài)解算

MG姿態(tài)測量器件主要包括加速度計和磁力計，只在載體處于勻速或靜止?fàn)顟B(tài)時才能準(zhǔn)確測量，當(dāng)載體具有較大線加速度時，該方法誤差很大［10］。盲人行走過程大部分可以看作是勻速運動狀態(tài)，因此通過載體所在位置的重力場和磁場可解算得到載體當(dāng)前位置的姿態(tài)，轉(zhuǎn)換后可得相機當(dāng)前位置的姿態(tài)。

設(shè)參數(shù)θ、γ、ψ為載體姿態(tài)的俯仰角、滾轉(zhuǎn)角、航向角，導(dǎo)航坐標(biāo)系與載體坐標(biāo)系間的方向余弦矩陣為。

當(dāng)載體處于勻速運動狀態(tài)或靜止?fàn)顟B(tài)時，當(dāng)?shù)刂亓铀俣萭b在載體坐標(biāo)系下的投影可由加速度計三軸輸出直接給出，表示為：

由于導(dǎo)航坐標(biāo)系定義為東北天，所以導(dǎo)航坐標(biāo)系下重力加速度為：

其中，g為載體所在處的重力加速度。在獲得了當(dāng)?shù)刂亓铀俣鹊臏?zhǔn)確值后，有：

由式(8）可解算得到：

式中，ψ為理論值。由于磁北極和真北極不重合，需要通過當(dāng)?shù)卮牌沁M行修正。設(shè)當(dāng)?shù)卮牌菫閐，航向角真值為：

4 視覺/GPS/MG姿態(tài)測量組合導(dǎo)航Kalman濾波設(shè)計

系統(tǒng)整體設(shè)計如圖3所示，通過雙目視覺導(dǎo)航的方式獲得視覺的導(dǎo)航信息，GPS用于獲得較準(zhǔn)確的絕對位置，加速度計和磁力計得到姿態(tài)，在導(dǎo)航濾波器中對導(dǎo)航系統(tǒng)誤差狀態(tài)進行估計，最后對誤差進行補償獲取高精度的導(dǎo)航結(jié)果。

圖3 導(dǎo)航算法方案框圖Fig.3 Diagram of the navigation algorithm

4.1 系統(tǒng)狀態(tài)模型

組合導(dǎo)航系統(tǒng)以地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，設(shè)盲人在導(dǎo)航坐標(biāo)系下的位置誤差為Δp＝［ΔxΔyΔz］，由于計算誤差，用于計算的導(dǎo)航坐標(biāo)系(系）與理想導(dǎo)航坐標(biāo)系(n系）不完全重合，兩個坐標(biāo)系之間存在姿態(tài)角誤差φ，將誤差角寫成φn＝［φEφNφU］T。選擇位置誤差和姿態(tài)誤差作為系統(tǒng)狀態(tài)向量：

系統(tǒng)位置誤差的導(dǎo)數(shù)是速度誤差：

速度誤差可視為白噪聲處理。

其中，φn＝［φEφNφU］T。

計算導(dǎo)航坐標(biāo)系相對于相機坐標(biāo)系的轉(zhuǎn)動角速率，可表示為：

從而姿態(tài)誤差角的微分方程為：

根據(jù)剛體轉(zhuǎn)動定理有：

從而三軸加速度誤差為：

相機姿態(tài)角速度誤差視為白噪聲。

綜上所述，系統(tǒng)狀態(tài)方程可以寫為：

以上即為系統(tǒng)的狀態(tài)方程。

4.2 系統(tǒng)量測模型

由圖3可知，當(dāng)GPS有效時，可以獲取GPS的量測信息，這時導(dǎo)航濾波器進行時間更新和量測更新；當(dāng)GPS無效時，只對導(dǎo)航濾波器進行時間跟新。本文組合導(dǎo)航系統(tǒng)中共有兩組量測值：一組是位置量測值，即GPS給出的絕對位置和視覺導(dǎo)航推算得到的相對位置；另一組是加速度計和磁力計計算的姿態(tài)值和視覺推算得到的姿態(tài)信息。通過對量測值進行誤差分析可以建立量測量和狀態(tài)量之間的關(guān)系，導(dǎo)出量測模型。

(1）位置誤差量測方程

GPS接收機和視覺導(dǎo)航的位置量測信息，可分別表示為：

式中，xr、yr、zr表示真實位置；δxE、δyN、δzU表示GPS的位值誤差；δx、δy、δz表示視覺導(dǎo)航的位值誤差。

位置量測誤差模型為：

(2）姿態(tài)誤差量測方程

視覺導(dǎo)航和MG姿態(tài)測量得到的姿態(tài)值可表示為：

式中，θr、γr、ψr表示真實的姿態(tài)，?x、?y、?z表示視覺導(dǎo)航姿態(tài)誤差，?mx、?my、?mz表示MG測量姿態(tài)的誤差值。

姿態(tài)量測誤差模型為：

通過把位置量測矢量和姿態(tài)量測矢量合并，得到系統(tǒng)觀測方程：

(3）基于加速度計的自適應(yīng)姿態(tài)量測噪聲方差陣

由于利用MG測量姿態(tài)的方法受相機載體線加速度的影響很大，在高動態(tài)下會產(chǎn)生較大的姿態(tài)誤差。為增強導(dǎo)航濾波器的穩(wěn)定性，保證姿態(tài)精度，采用自適應(yīng)的估計算法，利用姿態(tài)量測噪聲矩陣Rφ的變化實時調(diào)整導(dǎo)航濾波器對量測信息的依賴程度［11］。

若系統(tǒng)無加速度，則加速度計的輸出滿足：

此時加速度計的輸出矢量的模值可近似等于重力加速度，以此作為載體運動狀態(tài)的判別依據(jù)，判別過程如下：

5 仿真和實驗驗證

5.1 仿真條件

為驗證本文算法的可靠性，以盲人為對象進行仿真導(dǎo)航實驗。通過軌跡發(fā)生器生成標(biāo)準(zhǔn)軌跡，視覺導(dǎo)航模塊生成視覺導(dǎo)航數(shù)據(jù)，GPS模塊生成仿真GPS數(shù)據(jù)，加速度計和磁力計模塊生成姿態(tài)仿真數(shù)據(jù)。

仿真軌跡時間為400s，累計路程為168m，GPS水平位置均方誤差為2m，加速度計和磁力計模塊生成姿態(tài)角的均方誤差為1°。

5.2 仿真結(jié)果

圖4為仿真模擬實驗的導(dǎo)航位置輸出，視覺導(dǎo)航輸出的位置誤差隨時間積累，而組合導(dǎo)航的位置曲線較接近真實值。

圖4 仿真實驗導(dǎo)航位置Fig.4 Navigation position of simulation experiment

圖5和圖6分別為北向和東向位置誤差誤差曲線，從圖中可以看出單獨的視覺位置輸出存在誤差累積，而組合后位置誤差明顯減小。

圖5 北向位置誤差曲線Fig.5 Error curve of the north position

圖6 東向位置誤差曲線Fig.6 Error curve of the east position

圖7為仿真模擬實驗的姿態(tài)誤差曲線，視覺導(dǎo)航輸出的姿態(tài)誤差逐漸積累并發(fā)散，而組合導(dǎo)航能很好地校正姿態(tài)誤差。

圖7 仿真實驗姿態(tài)誤差Fig.7 Attitude error of simulation experiment

5.3 實驗驗證

為了驗證本文算法的可靠性，采用實驗室自行研制的穿戴式導(dǎo)盲裝置實驗平臺，如圖8所示，該裝置搭載雙目攝像頭、MEMS姿態(tài)測量器件和GPS模塊。

圖8 穿戴式導(dǎo)盲實驗裝置Fig.8 Wearable guide experiment device

其中，MG姿態(tài)測量器件和GPS模塊，采用Xsens公司的 MTi?G?700，該器件集成加速度計、磁力計和GPS。

本次實驗場地為北航圖書館廣場，實驗場景主要是石泥板，實驗路線為從原點向左沿矩形行走，最后回到原點處。實驗結(jié)果如圖9、圖10所示。

圖9 組合導(dǎo)航實驗位置輸出Fig.9 Location of integrated navigation experiment

圖10 組合導(dǎo)航實驗航向角輸出Fig.10 Yaw angle of integrated navigation experiment

從圖9中可以看出，隨著行走距離的增大，視覺導(dǎo)航定位誤差積累明顯，GPS定位誤差受信號穩(wěn)定影響，組合后的導(dǎo)航定位精度較高，精度在5m內(nèi)。圖10表明視覺導(dǎo)航航向角輸出誤差積累發(fā)散，組合后的航向角輸出誤差明顯減小，誤差在10°范圍內(nèi)。

6 結(jié)論

本文提出一種基于視覺、GPS和MG姿態(tài)測量的盲人組合導(dǎo)航算法，該算法利用GPS和視覺導(dǎo)航定位的互補性以及MG姿態(tài)測量不隨時間累積的優(yōu)點，實現(xiàn)組合導(dǎo)航，有效解決了盲人安全出行的導(dǎo)航問題。仿真和實驗結(jié)果表明，本文提出的算法優(yōu)于單獨的GPS和視覺導(dǎo)航系統(tǒng)，定位精度可達5m以內(nèi)，姿態(tài)測量誤差在10°范圍內(nèi)，滿足盲人戶外行走的導(dǎo)航要求。

[1]何婧,聶旻,羅蘭,等.聽覺引導(dǎo)助盲系統(tǒng)[J].生物醫(yī)學(xué)工程學(xué)雜志,2010,27(2）:467?470.HE Jing,NIE Min,LUO Lan,et al.Auditory guidance systems for the visually impaired people[J].Journal of Bi?omedical Engineering,2010,27(2）:467?470.

[2]莊春華,趙治華,張益青,等.衛(wèi)星導(dǎo)航定位技術(shù)綜述[J].導(dǎo)航定位學(xué)報,2014,2(1）:34?40.ZHUANG Chun?hua,ZHAO Zhi?hua,ZHANG Yi?qing,et al.Overview on seamless positioning technologies of satellite navigation[J].Journal of Navigation and Location,2014,2(1）:34?40.

[3]高鐘毓.慣性導(dǎo)航系統(tǒng)技術(shù)[M].北京:清華大學(xué)出版社,2012.GAO Zhong?yu.Technology of inertial navigation system[M].Beijing:Tsinghua University Press,2012.

[4]Lu W,Xiang Z,Liu J.High?performance visual odometry with two?stage local binocular BA and GPU[C].Intelligent Vehicles Symposium（IV）,2013 IEEE,2013:1107?1112.

[5]Helal A S,Moore S E,Ramachandran B.Drishti:an inte?grated navigation system for visually impaired and disabled[C].Wearable Computers,2001 Proceedings 5thInterna?tional Symposium on IEEE,2001:49?156.

[6]李宇波,朱效洲,盧惠民,等.視覺里程計技術(shù)綜述[J].計算機應(yīng)用研究,2012,29(8）:2801?2805＋2810.LI Yu?bo,ZHU Xiao?zhou,LU Hui?min,et al.Review on visual odometry technology[J].Application Research of Computers,2012,29(8）:2801?2805＋2810.

[7]徐勇志,寧曉琳.一種巡視器慣性/視覺組合導(dǎo)航新方法[J].空間科學(xué)學(xué)報,2015,35(6）:721?729.XU Yong?zhi,NING Xiao?lin.A new INS/VNS integrated navigation method for planetary exploration rover[J].Chi?nese Journal of Space Science,2015,35(6）:721?729.

[8]夏凌楠,張波,王營冠,等.基于慣性傳感器和視覺里程計的機器人定位[J].儀器儀表學(xué)報,2013,34(1）:166?172.XIA Ling?nan,ZHANG Bo,WANG Ying?guan,et al.Robot localization algorithm based on inertial sensor and video odometry[J].Chinese Journal of Scientific Instrument,2013,34(1）:166?172.

[9]張洋,王新龍.一種地標(biāo)點修正的高精度雙目視覺導(dǎo)航方法[J].北京航空航天大學(xué)學(xué)報,2014,40(9）:1305?1311.ZHANG Yang,WANG Xin?long.Landmark fixed high?precision binocular visual navigation method[J].Journal of Beijing University of Aeronautics and Astronautics,2014,40(9）:1305?1311.

[10]Bird J,Arden D.Indoor navigation with foot?mounted strapdown inertial navigation and magnetic sensors[J].IEEE Wireless Communications,2011,18(2）:28?35.

[11]劉興川,張盛,李麗哲,等.基于四元數(shù)的MARG傳感器姿態(tài)測量算法[J].清華大學(xué)學(xué)報（自然科學(xué)版）,2012,52(5）:627?631.LIU Xing?chuan,ZHANG Sheng,LI Li?zhe,et al.Quaternion?based algorithm for orientation estimation from MARG sensors[J].Journal of Tsinghua University（Science and Technology）,2012,52(5）:627?631.

Integrated Navigation Method of the Blind Walking Based on Computer Vision/GPS /MG Attitude Measurement

JIA Wen?yuan,WEI Tong
(School of Instrument Science and Optoelectronic Engineering,Beihang University,Beijing 100191）

U666.1

A

1674?5558(2017）01?01336

10.3969/j.issn.1674?5558.2017.05.003

2016?11?21

北京市科技計劃項目(編號：Z151100002115022）

賈文淵，男，碩士，研究方向為計算機視覺和導(dǎo)航定位。