一種面向移動終端的混合跟蹤定位算法

曹若琛,陳 靖,王涌天,

(北京理工大學 a.計算機科學與技術學院,b.光電學院,北京 100081)

?

一種面向移動終端的混合跟蹤定位算法

曹若琛a,陳 靖b,王涌天a,b

(北京理工大學 a.計算機科學與技術學院,b.光電學院,北京 100081)

通過擴展卡爾曼濾波器(EKF)對手機移動終端的加速度計、陀螺儀以及視覺數據進行融合,實現混合跟蹤。在此基礎上,對基于EKF的數據融合算法進行改進,當失去視覺校正時,利用同步跟蹤的背景區域速度更新目標區域速度,使算法在目標區域提取特征點數不足的情況下可以持續跟蹤;同時引入參考坐標系,定義世界坐標系在參考坐標系中的姿態,校正在基于EKF的數據融合算法中由于光照遮擋等問題出現的角度跳變錯誤；最后,在實際環境中進行測試,實驗結果顯示本混合跟蹤算法相比傳統單一視覺的跟蹤注冊算法和以往基于EKF的數據融合算法具有明顯優勢。

移動增強現實;多傳感器;混合跟蹤;擴展卡爾曼濾波器

隨著移動設備技術的不斷發展,智能手機大多集成了高分辨率的攝像頭、高速處理芯片和多種傳感器,這使得智能手機相比傳統PC,成為更適合增強現實(AR)技術運行的平臺。和傳統增強現實一樣,移動增強現實通過三維圖形技術和視覺跟蹤技術,將虛擬物體與現實景象相結合,使得三維虛擬圖形與現實的周遭環境在視覺上完美地融合在一起,達到增強現實環境的效果,人們可以通過增強現實來與虛擬物體產生互動。

在移動增強現實領域,實現對目標的實時準確跟蹤是一個具有挑戰性的任務。基于視覺的跟蹤注冊算法具有高精度、強魯棒性的特點[1-4]。其中最重要的是文獻[1]提出的一種可運行在手機上的基于SLAM的并行跟蹤系統-PTAM,它在小工作空間中有較高的魯棒性。但是由于自然場景的復雜性和可擴展性,PTAM很難真正在移動設備上滿足實時性要求。文獻[2]提出一種名為monoSLAM的跟蹤系統,它可以在恢復場景的三維結構的同時滿足實時性要求。但是由于monoSLAM是一個基于PC的增強現實系統,過大的內存消耗使它不適合在手機端運行。近期,文獻[3]提出了一種依賴每一個像素的實時跟蹤重建系統。在手機端特征匹配方面,文獻[4]提出一種改良過的SIFT特征,提高了手機端特征匹配的速度和魯棒性。雖然基于視覺的跟蹤注冊算法在圖像清晰的條件下,可以實現穩定、魯棒的實時跟蹤,但當跟蹤目標被遮擋或運動速度較快時,算法將失效。

相比基于視覺的跟蹤注冊算法,混合跟蹤技術可以保障跟蹤系統在快速運動或目標被遮擋等情況下的跟蹤持續性和魯棒性,是增強現實跟蹤注冊系統同時實現高精度和強魯棒性的有效途徑。相比傳統增強現實技術,移動增強現實中的混合跟蹤對跟蹤注冊算法的穩定性、魯棒性以及實時性提出了更高的要求。

自從文獻[5-6]提出將視覺跟蹤與其他跟蹤方法相結合以克服室外視覺跟蹤缺陷的新思路以來,混合跟蹤成為最近十年來增強現實的一個重要研究方向。2006年,文獻[7]的作者利用視覺以及混合重力、慣性、GPS等傳感器在手持設備上開發了一套魯棒的基于模型跟蹤的戶外增強現實系統-GoingOut。2012年,文獻[8]提出了一個混合跟蹤系統,它由擴展的卡爾曼濾波器構成的緊密耦合方法融合慣性測量單元(Inertial measurement unit,IMU)和視覺數據,實現混合跟蹤。對于手機端的數據融合,卡爾曼濾波器仍然是一個有效的常用融合框架[9-12]。其中,文獻[9,10]使用擴展的卡爾曼濾波器(EKF)融合慣性傳感器和視覺的數據。文獻[11]則使用無跡卡爾曼濾波器(UKF)實現數據融合。

對于混合跟蹤系統,筆者所在課題組在以前的研究[13]中提出過使用視覺-慣性傳感器用于運動恢復和室外AR系統。該系統基于PC端,使用MTx慣性傳感器。在此工作的基礎上,本文完全在手機端實現混合跟蹤,針對手機內存小、處理速度慢和傳感器誤差大等問題,選用ORB特征點對其進行改進提高了手機端特征點匹配的速度和精度，同時對手機傳感器進行去噪聲,提高了手機傳感器數據的可信度,以保證手機端跟蹤注冊的穩定性和實時性。對傳統基于EKF的視覺-慣性傳感器數據融合算法進行改進,充分利用手機傳感器返回的數據,保證改進后的算法在視覺信息不可靠或視覺信息丟失的情況下,可以繼續準確、穩定跟蹤。

本文主要貢獻如下:1) 采用設備重力方向代替ORB描述符主方向,提高匹配效果;2) 引入Sensors-aware參考坐標系,提出利用參考坐標系校正視覺跟蹤注冊技術返回的旋轉角的方法。3) 提出實時幀背景同步跟蹤方法,利用同步跟蹤的背景區域速度更新目標區域運動速度,解決目標區域視覺丟幀后無法繼續跟蹤的問題。

1 手機坐標系定義

以Iphone手機為例,加速度計和陀螺儀坐標系為右手坐標系。即,將設備正面向上放在一個水平平面上,從左至右為X軸,從下至上為Y軸,與水平面垂直向上為Z軸。在使用過程中傳感器坐標系和手機相對靜止。屏幕坐標系則是以屏幕左上角為原點,從左至右為X軸,從上至下為Y軸。

2 面向移動智能終端的混合跟蹤定位算法

2.1 手眼標定

在姿態融合過程中,由于測量數據來源不同,基于視覺的跟蹤注冊信息和傳感器返回的攝像機姿態信息并不處于統一坐標系下。視覺跟蹤注冊返回的攝像機位姿信息是旋轉矩陣Rcw和平移矩陣Tcw,腳標代表攝像機坐標系相對于世界坐標系。而慣性傳感器返回的數據則是相對于慣性傳感器坐標系的加速度和角速度。因此,在二者數據融合之前,需要標定手機傳感器和攝像機之間的位置、姿態關系,即手眼標定。

設變換矩陣M為4×4方陣:

(1)

式中：R,T為相應旋轉矩陣和平移矩陣。各坐標系變換關系如下:

(2)

式中：Mcw為攝像機坐標系相對世界坐標系的變換矩陣；Msw為傳感器坐標系相對世界坐標系的變換矩陣；Mcs是手眼標定所求的攝像機坐標系相對慣性傳感器坐標系的變換矩陣。

由于在任意時刻,Mcw通過視覺的跟蹤注冊算法很容易測得,而Msw無法直接測得,所以引入慣性傳感器可以直接測得變量ΔMs,即傳感器姿態變化量。

假設，在t=0時刻測得Mcw0，在t=1時刻測得Mcw1，傳感器位置姿態變化量ΔMs:

(3)

(4)

(5)

將式(4)、式(5)帶入式(3)可得:

(6)

2.2 視覺和多傳感器位姿融合算法

本文位姿融合采用EKF,使用陀螺儀和加速度計混合模型,整體流程如圖1所示。

圖1 融合流程圖Fig.1 System framework

其中實時幀背景同步跟蹤方法和Sensors-aware參考坐標系是筆者所在課題組提出的兩個改進措施,將在后文中介紹。在陀螺儀和加速度計混合模型中,假設攝像機的運動模型為勻加速度模型。陀螺儀測量的角速度和加速度計測量的加速度為預測量,視覺跟蹤注冊技術返回的旋轉和平移矩陣為觀測量。在該模型中狀態矢量表示為:

(7)

式(7)中,q是Rsw的四元數表示；ω表示慣性傳感器在世界坐標系下的角速度;p,v,a,Δa分別表示慣性傳感器在世界坐標系下的位置速度、加速度和加速度誤差量。這里的坐標都是慣性傳感器坐標系相對世界坐標系而言的,因此視覺跟蹤注冊返回的位姿信息不能直接使用,要借助2.1中介紹的手眼標定結果Mcs.

經過Δt時間后,狀態矢量變化方程如下:

(8)

式中：⊙表示四元數乘法；ωk和ak表示傳感器采集的角速度和線性加速度數據；Δt為采樣間隔。式(8)完成擴展卡爾曼濾波器中的預測部分。擴展卡爾曼濾波器中的校正更新部分由視覺跟蹤注冊返回的信息完成,觀測矢量如下:

(9)

式中：q是Rcw的四元數表示；p是攝像機在世界坐標系中的位置。由上述運動模型,擴展卡爾曼濾波器的狀態預測和更新方程設計如下:

(10)

(11)

式(10)中,A為狀態轉移矩陣。這里引入變量Bμ是為了去除重力加速度的影響。其中B=[zero(1,12),1,0,0,0]T,μ=-9.81，引入Wk·aT是為了對加速度測量值進行誤差校正。其中aT=[axayaz]T為加速度計實時測量值,Wk為轉移矩陣。式(11)中,Kk+1為卡爾曼增益,zk+1表示觀測量。

矩陣H取值如下:

(12)

迭代過程中的噪聲協方差矩陣取值如下:

(13)

(14)

通過上述方式實現的混合跟蹤依然存在以下問題:

1) 以視覺跟蹤注冊結果對狀態矢量進行更新,若視覺計算出現明顯錯誤,則算法失效。經過多次實驗我們發現,視覺計算錯誤大多是由于旋轉角計算出現較大偏差。

2) 由于手機中集成的加速度計精度低,穩定性差,在失去視覺信息校正時,融合算法將出現較大的誤差。

3) 為保證算法在移動終端上的運行速度,我們采用ORB特征點提取算法,但是該算法的特征匹配準確度較差,易造成后續的視覺計算偏差。針對以上三個問題,本文提出以下三方面的改進措施。

2.3 改進措施

2.3.1 結合重力方向的ORB特征點

如圖2所示,圖中矩形四個角點特征完全相同或近似相同。圖2左側表示使用傳統ORB算法時,四個角點生成的描述符,顯然四個角點生成的描述符完全相同;圖2右側表示以重力方向作為特征點主方向后生成的描述符,很明顯四個角點所生成的描述符各不相同。所以,引入重力信息后,描述符可區分性明顯增強。本文將改進后的算法稱為GORB。

圖2 傳統方法描述符(左)與重力對齊描述符(右)Fig.2 ORB (left) and GORB (right)

2.3.2 Sensors-aware參考坐標系

經過多次實驗我們發現,在視覺的跟蹤注冊過程中,由于光照、遮擋等原因,會引起姿態計算出現大幅度偏差,這種偏差大多表現為手機和世界坐標系之間的相對旋轉出現錯誤。本文引入姿態固定不變的參考坐標系,通過傳感器計算出世界坐標系和手機在參考坐標系中的姿態,建立起計算手機和世界坐標系之間相對旋轉的另一條途徑,以此校準視覺跟蹤注冊技術返回的旋轉角。

參考坐標系定義如下:重力方向為y軸正方向,垂直于y軸手機朝向角為0時的手機朝向為z軸正方向,垂直yz平面水平向右為x軸正方向。需要注意的是,這個參考坐標系只是用來確定姿態,所以它的原點位置并不需要考慮。

對于第k幀圖像,利用手機傳感器可以得到手機在參考坐標系中的姿態坐標Rsrk,同時利用視覺跟蹤注冊技術可以得到該幀手機在世界坐標系中的姿態坐標Rswk,即手機相對于跟蹤目標的姿態。利用Rsrk和Rswk可以求解出跟蹤目標在參考坐標系下的姿態坐標Rwr.

(15)

由于世界坐標系和參考坐標系姿態均保持不變,所以式(15)中Rwr是一個固定值。在之后的計算姿態過程中,利用Rwr和當前手機在參考坐標系中的姿態Rsr可以計算出參考姿態Rk,即通過傳感器計算出的手機在世界坐標系中的姿態。如式(16)所示。

(16)

令視覺返回的姿態Rsw與Rk相減得到ΔR.設定閾值D,規定超出閾值為計算錯誤,這一幀的旋轉角使用Rk代替,以此完成對視覺跟蹤注冊技術返回的姿態的校正。如式(17)所示。

(17)

2.3.3 實時幀背景同步跟蹤方法

在混合跟蹤過程中,當跟蹤目標由于遮擋等原因導致提取特征點數不足時,數據融合算法失去視覺校正。此時陀螺儀精度足夠進行短時間的持續預測。但由于手機加速度計精度有限,無法在視覺丟失情況下進行準確地預測跟蹤,造成預測結果出現發散。而此時背景區域大部分在可視范圍內,所以可利用背景區域計算出的速度vb,對目標區域速度vt進行更新,并通過vt更新相應平移矩陣T.同時配合精度較高的陀螺儀計算旋轉矩陣R,實現魯棒的跟蹤定位。

定義sb,st分別為對背景跟蹤和對目標跟蹤時建立的世界坐標系,分別位于背景平面和目標平面上。sr為前文中介紹的參考坐標系,它是固定不動的。vb和vt分別是手機在兩個世界坐標系下的速度。首先,需要求出兩個世界坐標系之間的姿態關系,將vb和vt統一在同一坐標系下。

(18)

(19)

式(18)(19)中,腳標r,t,b分別代表相應坐標在坐標系sr,st,sb中。Rbr,Tbr,Rtr,Ttr分別為坐標系sb至sr和st至sr的旋轉和平移矩陣。Rbr和Rtr由式(15)可以求出。連解式(18)(19)可得:

(20)

(21)

值得注意的是,這種方法有其局限性：

1) 當背景區域與目標區域同時丟失時,該方法將無法進行。

2) 如果背景深度相對手機到背景區域質心距離不能忽略,該方法會失效。

3 實驗驗證

本文實驗平臺為Iphone5手機,該款手機配有意法半導體LIS331DLH超低功耗高性能的三軸線性加速度計以及意法半導體L3G4200D的低功耗三軸陀螺儀,操作系統為IOS7.0.4,圖像分辨率統一設置為640×480。

3.1 手眼標定結果

標定板規格是6×6棋盤格,每格為30 mm×30 mm正方形。通過張正友相機標定法[17]求出攝像機內參為:

通過本文的方法,手眼標定結果為:

3.2 加速度傳感器降噪

本文針對Iphone5手機加速度計的特點,采用4層小波閾值降噪。降噪效果如圖3所示。其中3(a)表示手機靜止不動時的加速度值,3(b)表示用戶手持手機隨機運動時的加速度值。這里取x軸上的變化為例。從圖3可以看出,使用小波閾值降噪對手機加速度數據的降噪效果明顯,顯著提高了手機加速度計測量值的可信度。但是,使用該方法使加速度計對運動的敏感度有少許下降。

圖3 手機加速度降噪效果圖Fig.3 Effect of de-noising

3.3 ORB算法和GORB算法匹配正確率對比

我們設計了實驗對比傳統ORB算法和GORB算法的匹配正確率。取41組實驗數據對比,求其匹配正確率平均值。實驗結果顯示,傳統ORB算法的平均匹配正確率為44.96%,而GORB算法的平均匹配正確率為52.98%。相比傳統的ORB算法,GORB算法的匹配正確率平均提高了8個百分點。可見,GORB算法匹配正確率相比較傳統ORB算法有明顯提升效果。充分說明引入重力信息后增加了描述符的可區分性。

圖4 姿態估計對比(信息可靠時)Fig.4 Comparison of motion estimation results

3.4 混合跟蹤算法精度測試

本節實驗對比了本文提出的改進后的EKF混合跟蹤算法、傳統基于EKF的視覺-慣性傳感器混合跟蹤算法以及基于視覺的跟蹤算法分別在視覺跟蹤注冊技術返回的攝像機姿態信息可靠、不可靠和視覺目標丟失三種情況下的跟蹤精度,通過實驗驗證本文提出的改進后的EKF算法在實際環境中的魯棒性。由于缺少基礎數據(ground truth data),我們用反投影誤差來衡量視覺測量的精度。在本文實驗中我們認為,通過視覺跟蹤注冊技術返回的攝像機姿態信息計算出的反投影誤差小于2像素時,該視覺信息結果可靠;當反投影誤差大于2像素時,該視覺信息不可靠。

圖4對比了當視覺跟蹤注冊技術返回的攝像機姿態信息可靠時,不同跟蹤方法返回的攝像機姿態信息,這里以x軸上數據為例。從圖中可以看出,由于視覺信息可靠,三種算法返回的攝像機姿態信息較為接近,相比傳統基于EKF的混合跟蹤算法,本文所提出的混合跟蹤算法對其進行了改進,返回的姿態信息更加平滑,減少了傳感器數據波動和姿態估計帶來的抖動。

圖5 姿態估計對比(信息不可靠時)Fig.5 Comparison of motion estimation results

圖6 丟失目標時跟蹤對比圖Fig.6 Comparison of motion estimation results

圖5對比了當視覺跟蹤注冊技術返回的攝像機姿態信息不可靠時,不同跟蹤方法返回的攝像機姿態(旋轉)數據,這里以x軸上數據為例。從圖中可以看出,由于視覺信息不可靠,傳統EKF算法的視覺校正出現誤差。而本文提出的混合跟蹤算法,由于使用前文中提出的參考坐標系對視覺返回的旋轉數據進行校正,在視覺校正(旋轉)出現誤差時仍能返回較為準確的攝像機姿態(旋轉)信息。

圖6對比了當視覺目標丟失時,不同跟蹤方法返回的攝像機姿態(平移)信息,其中曲線數值為0表示視覺目標丟失,這里以x軸上數據為例。從圖中可以看出,從33幀開始,視覺目標丟失，視覺跟蹤算法失效。由于失去視覺信息校正,單純使用視覺信息作為校正的混合跟蹤算法很快發散,而本文提出的混合跟蹤算法可以在短時間內(圖6中大約10幀左右)持續穩定跟蹤。

4 結語

本文在利用擴展卡爾曼濾波器實現面向移動智能終端的視覺和多傳感器混合跟蹤的基礎上,對其進行改進。提出實時幀背景同步跟蹤方法,解決當目標區域提取特征點數不足時無法準確跟蹤的問題;引入參考坐標系,定義世界坐標系在參考坐標系中的姿態,校正跟蹤過程中由于光照、遮擋等問題造成的視覺姿態計算錯誤。另外,使用手機重力方向代替ORB描述符主方向,提高特征匹配準確度。在實際環境中進行實驗,結果顯示,相比傳統基于EKF的視覺和多傳感器混合跟蹤算法,本文提出的改進EKF混合跟蹤算法在視覺姿態角出現錯誤和視覺目標丟失后仍能持續較為準確的跟蹤。

本文提出的方法仍有很多不足和局限性:要求背景最好為平面或背景深度相對景深可以忽略，背景區域圖像需紋理豐富。這需要在下一步的工作中繼續研究。

[1] KLEIN G,MURRAY D.Parallel tracking and mapping for small AR workspaces[C]∥IEEE.Proceedings of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality (ISMAR’07),Nara,Japan,2007:225-234.

[2] DAVISON A J,REID I D,MOLTON N D,et al.MonoSLAM: real-time single camera SLAM[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2007,29(6)：1052-1067.

[3] NEWCOMBE R A,LOVEGROVE S J,DAVISON A J.DTAM:Dense tracking and mapping in real-time[C]∥IEEE.Proceedings of 2011 IEEE International Conference on Computer Vision (ICCV),Barcelona,Spain,2011:2320-2327.

[4] WAGNER D,REITMAYR G,MULLONI A,et al.Real-time detection and tracking for augmented reality on mobile phones[J].IEEE Transactions on Visualization and Computer Graphics,2010,16(3)：355-368.

[5] AZUMA R T,HOFF B R,NEELY III H E,et al.Making augmented reality work outdoors requires hybrid tracking[C]∥IEEE.The first International Workshop on Augmented Reality(IWAR′98)，San Francisco,CA,1998:219-224.

[6] AZUMA R T.The challenge of making augmented reality work outdoors[J].Mixed reality:Merging real and virtual worlds,1999:379-390.

[7] REITMAYR G,DRUMMOND T W.Going out:robust model-based tracking for outdoor augmented reality[C]∥ACM. International Symposium on Mixed and Augmented Reality,Santa Barbara,2006:109-118.

[8] OSKIPER T,SAMARASEKERA S,KUMAR R.Multi-sensor navigation algorithm using monocular camera,IMU and GPS for large scale augmented reality[C]∥IEEE.IEEE International Symposium on Mixed and Augmented Reality (ISMAR),Atlanta,USA,2012:71-80.

[9] HILSENBECK S,MOLLER A,HUITL R,et al.Scale-preserving long-term visual odometry for indoor navigation[C]IEEE.∥2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN),Sydney,Australia,2012:1-10.

[10] KUNDRA L,EKLER P,CHARAF H.Improving orientation estimation in mobiles with built-in camera[C]∥IEEE.2013 IEEE 4th International Conference on Cognitive Infocommunications (CogInfoCom),Budapest,Hungary,2013:765-770.

[11] HARTMANN G,HUANG F,KLETTE R.Landmark initialization for unscented kalman filter sensor fusion in monocular camera localization[J].International Journal of Fuzzy Logic and Intelligent Systems,2013,13(1)：1-10.

[12] LI M,KIM B H,MOURIKIS A.Real-time motion tracking on a cellphone using inertial sensing and a rolling-shutter camera[C]∥IEEE.IEEE International Conference on Robotics and Automation (ICRA),Karlsruhe,Germany,2013:4712-4719.

[13] 陳靖,王涌天,劉越，等.適用于戶外增強現實系統的混合跟蹤定位算法[J].計算機輔助設計與圖形學學報,2010,22(2):204-209.

[14] 沈曉衛,姚敏立,常瑞花.模糊邏輯提升小波在慣性傳感器去噪中的應用[J].振動與沖擊,2011,30(12):226-229.

[15] 姚麗慧,高井祥,王堅.不同小波函數對粗差識別效果的比較[J].北京測繪,2011,3:008.

[16] DANIILIDIS K.Hand-eye calibration using dual quaternions[J].The International Journal of Robotics Research,1999,18(3):286-298.

[17] ZHANG Z.Flexible camera calibration by viewing a plane from unknown orientations[C]∥IEEE.The Seventh IEEE International Conference on Computer Vision,1999,1:666-673.

[18] ERDEM A T,ERCAN A O.Fusing inertial sensordata in an extended kalman filter for 3D camera tracking[J].IEEE Transactions on Image Processing,2015,24(2):538-548.

(編輯：賈麗紅)

Hybrid Tracking Algorithm for Mobile Terminals

CAO Ruochena,CHEN Jingb,WANG Yongtiana,b

(a.School of Computer Science and Technology,b.SchoolofOptoelectronics,BeijingInstituteofTechnology,Beijing100081,China)

In the Mobile Augmented Reality field, accurate tracking is a challenging task. Hybrid tracking based on Visual and Multi-sensor generally provides more stable results than single visual tracking. This paper fuses the data from visual and multi-sensor using Extended Kalman Filter (EKF). On this basis, to improve the algorithm of data fusion based on EKF, a way to update the velocity of the target using background is presented, which makes tracking to work well when visible feature points are not enough;the reference coordinate system is introduced as well,the attitude of world coordinate system is defined in the reference coordinate system,and the error caused by lighting, shelter and so on is corrected.Experimental results show that the mobile terminal hybrid tracking presented in the paper is advantageous.

mobile augmented reality;multi-sensor;hybrid tracking;extended kalman filter

1007-9432(2016)04-0506-07

2016-01-25

國家高科技863計劃項目:基于內容理解的高融合度增強現實技術(2013AA013802); 國家自然科學基金資助項目：面向運動目標檢測識別的高分辨率光學壓縮成像理論與技術(61271375)

曹若琛(1990-),男,太原人，碩士生,主要從事虛擬現實與增強現實,移動應用的研究,(E-mail)517296205@qq.com

陳靖,博士,副研究員,主要從事虛擬現實與增強現實,計算機視覺的研究,(E-mail)chen74jing@bit.edu.cn

TN912.34

A

10.16355/j.cnki.issn1007-9432tyut.2016.04.014