王 崴，彭勃宇，周 誠，瞿 玨，劉曉衛(wèi)

(1.西安交通大學(xué) 機(jī)械制造系統(tǒng)工程國家重點(diǎn)實(shí)驗(yàn)室，陜西 西安710049;2.空軍工程大學(xué) 防空反導(dǎo)學(xué)院，陜西 西安710051)

0 引 言

增強(qiáng)現(xiàn)實(shí)(augmented reality，AR)的實(shí)質(zhì)是將虛擬信息疊加到真實(shí)視場上并呈現(xiàn)給使用者，而實(shí)現(xiàn)虛擬信息與現(xiàn)實(shí)場景準(zhǔn)確融合的核心是位姿注冊(cè)。注冊(cè)方式可根據(jù)傳感器的不同分為基于視覺、超聲波、電磁和慣性的注冊(cè)。其中，視覺注冊(cè)以其較高的注冊(cè)精度和較少的硬件開銷而被廣泛采用，然而單一的視覺注冊(cè)方式在觀察者位姿快速變化或受到光照和噪聲影響時(shí)會(huì)造成特征點(diǎn)數(shù)目減少，導(dǎo)致注冊(cè)精度降低甚至注冊(cè)失敗。因此，早期的AR 應(yīng)用大多限于動(dòng)作簡單緩慢，對(duì)虛實(shí)配準(zhǔn)的精度要求低的場合。然而，隨著AR 技術(shù)在外科手術(shù)［1］、誘導(dǎo)維修等領(lǐng)域的應(yīng)用，單一視覺注冊(cè)已經(jīng)難以滿足實(shí)時(shí)性和準(zhǔn)確性的需求。

慣性注冊(cè)利用慣性傳感器跟蹤觀察攝像機(jī)，其優(yōu)勢(shì)在于動(dòng)態(tài)性能好，可跟蹤劇烈動(dòng)作且不受外部環(huán)境影響。但通常采用的消費(fèi)級(jí)MEMS 傳感器的零點(diǎn)漂移和隨機(jī)誤差會(huì)隨時(shí)間被積分作用成倍放大［3］，使得慣性方式不適合長時(shí)間跟蹤。考慮到視覺和慣性在特性上的互補(bǔ)，構(gòu)建多傳感器組合注冊(cè)系統(tǒng)是提高AR 注冊(cè)性能的有效途徑。陳靖等人［3］和Oskiper Taragay 等人［4］通過組合注冊(cè)方法構(gòu)建了戶外AR 系統(tǒng)，李薪宇等人［5］通過AR 頭盔和穿戴式終端構(gòu)建了無線組合跟蹤系統(tǒng)，Jonathan K 等人［6］利用視覺—慣性方法實(shí)現(xiàn)了噪聲環(huán)境下攝像機(jī)的精確標(biāo)定。然而，現(xiàn)有的組合注冊(cè)絕大部分采用擴(kuò)展卡爾曼濾波(EKF)融合各傳感器數(shù)據(jù)，但在運(yùn)用EKF 對(duì)模型進(jìn)行線性化處理時(shí)會(huì)導(dǎo)致跟蹤精度的降低甚至濾波器的發(fā)散［7］，因此，不適用于動(dòng)作復(fù)雜和對(duì)跟蹤精度要求高的場合，且現(xiàn)有的融合算法在視覺通道受到遮擋、模糊等作用時(shí)位置和姿態(tài)的跟蹤精度都將受到影響。此外，現(xiàn)有的組合注冊(cè)方法僅在估計(jì)攝像機(jī)位姿時(shí)用濾波算法融合慣性數(shù)據(jù)，造成了資源的浪費(fèi)。

本文提出了一種基于互補(bǔ)濾波(CF)［8，9］和無跡卡爾曼濾波(UKF)［7］的組合注冊(cè)方案，該算法增強(qiáng)了算法的魯棒性，實(shí)現(xiàn)了對(duì)傳感器數(shù)據(jù)的高效利用。

1 多傳感器硬件系統(tǒng)與坐標(biāo)關(guān)系

傳感器系統(tǒng)由攝像頭、陀螺儀、加速度計(jì)和地磁感應(yīng)計(jì)組成。攝像頭為Logitech C920，可拍攝60 fps 分辨率為1270P 的視頻。設(shè)計(jì)了慣性/地磁模塊，將陀螺儀、加速度計(jì)、地磁感應(yīng)計(jì)集成在一起。慣性傳感器選用整合了16 位三軸陀螺儀和三軸加速度計(jì)的MPU6050，采樣更新速率為8 kHz，保證了系統(tǒng)測(cè)量的實(shí)時(shí)性。三軸地磁感應(yīng)計(jì)選用16 位的HMC5883，采樣更新速率為80 Hz。選用基于CortexM3 內(nèi)核的ARM 處理器STM32F103U8T6 進(jìn)行數(shù)據(jù)采集，其主頻達(dá)72 MHz，具有1.25 DMIPS/MHz 的計(jì)算能力，有硬件單周期乘法器，以保證姿態(tài)更新的實(shí)時(shí)性。利用中斷使處理器能在傳感器在完成A/D 轉(zhuǎn)換后的第一時(shí)間讀取更新數(shù)據(jù)。圖1 為慣性/地磁模塊硬件框圖。

圖1 慣性/地磁模塊硬件框圖Fig 1 Hardware block diagram of inertial/geomagnetic module

攝像頭和慣性/地磁模塊固連在自制的AR 頭盔上，如圖2 所示。

圖2 AR 頭盔Fig 2 AR helmet

系統(tǒng)各坐標(biāo)系關(guān)系如圖3 所示，W 為世界坐標(biāo)系;C 為攝像機(jī)坐標(biāo)系，其原點(diǎn)與攝像頭光心重合;U 為圖像坐標(biāo)系，將光心O 在其上的投影設(shè)為原點(diǎn);慣性/地磁模塊固連在慣性坐標(biāo)系I 上。

圖3 多傳感器系統(tǒng)坐標(biāo)關(guān)系Fig 3 Relationship between multi-sensor coordinate systems

空間一點(diǎn)P 在C 系下坐標(biāo)為P(xC，yC，zC)，則由透視投影原理和攝像機(jī)針孔模型可得到點(diǎn)在圖像坐標(biāo)系下坐標(biāo)p(xU，yU)與之的關(guān)系

其中，α，β 為像平面上橫縱坐標(biāo)方向的尺度因子，λ 為比例因子。矩陣M1為攝像機(jī)內(nèi)參矩陣，將其視為不變量，通過張正友的方法［10］事先標(biāo)定。設(shè)P 在世界坐標(biāo)系下坐標(biāo)為P(xW，yW，zW)，則其與之在攝像機(jī)坐標(biāo)系下坐標(biāo)P(xC，yC，zC)有如下關(guān)系

其中，RCW是3×3 正交單位陣，反映W 系到C 系的旋轉(zhuǎn)變換，RCW是3×1 列向量，反映W 系到C 系的平移變換，M2即外參矩陣，求得M2便可根據(jù)式(3)將世界坐標(biāo)系中的點(diǎn)與其在圖像平面上的投影點(diǎn)對(duì)應(yīng)從而實(shí)現(xiàn)注冊(cè)

系統(tǒng)中慣性坐標(biāo)系和攝像機(jī)坐標(biāo)系固連在一起，其變換RCI，TCI可事先標(biāo)定。

2 基于CF 的姿態(tài)估計(jì)

陀螺儀動(dòng)態(tài)性能好，能抵抗瞬時(shí)波動(dòng)，但在姿態(tài)解算時(shí)會(huì)產(chǎn)生累積誤差。加速度計(jì)和地磁傳感器可利用重力和地磁場可精確測(cè)量靜態(tài)姿態(tài)，但動(dòng)態(tài)性能差，測(cè)量精度會(huì)受速度變化影響。為獲得精度高、響應(yīng)快的姿態(tài)數(shù)據(jù)，利用傳感器在頻域上的互補(bǔ)特性，采用互補(bǔ)濾波融合陀螺儀、加速度計(jì)和地磁感應(yīng)計(jì)的數(shù)據(jù)。

設(shè)置參考坐標(biāo)系N，其原點(diǎn)和I 系重合，姿態(tài)與W 系保持一致。N～I(xiàn) 的旋轉(zhuǎn)矩陣表示如下

測(cè)得。設(shè)計(jì)互補(bǔ)濾波器的傳遞函數(shù)為

通過CF 得到的姿態(tài)數(shù)據(jù)具有精度高、響應(yīng)好的特點(diǎn)，且不受任何遮擋和所處環(huán)境的影響，增強(qiáng)了AR 系統(tǒng)的靈活性和魯棒性，實(shí)現(xiàn)了對(duì)攝像機(jī)姿態(tài)的持續(xù)穩(wěn)定跟蹤。

3 基于UKF 的位置估計(jì)

通過CF 可得較為精確的姿態(tài)，但無法獲得精確的位置信息，因此，使用UKF 融合加速度計(jì)和視覺數(shù)據(jù)進(jìn)行位置估計(jì)。UKF 無需線性化處理和計(jì)算雅克比矩陣，易于計(jì)算機(jī)實(shí)現(xiàn)且不存在在高階截?cái)嗾`差，在計(jì)算量與EKF 相當(dāng)?shù)那闆r下，均值和方差的精度可達(dá)三階泰勒級(jí)數(shù)水平［7］。

3.1 系統(tǒng)建模

根據(jù)AR 系統(tǒng)特點(diǎn)，設(shè)攝像頭光心加速度a(ax，ay，az)為常數(shù)，用高斯白噪聲模擬其波動(dòng)。系統(tǒng)狀態(tài)向量為xk=［pk，vk，ak］T，其中，pk，vk，ak分別表示光心的位置、速度和加速度矢量。設(shè)采樣時(shí)間為τ，系統(tǒng)由的狀態(tài)更新方程為

其中，wk為高斯白噪聲，且服從wk～N(0，Qk)。

系統(tǒng)觀測(cè)量來源于攝像頭和慣性/地磁模塊，因此，將觀測(cè)方程分為個(gè)通道。視覺觀測(cè)量為圖像特征點(diǎn)，觀測(cè)向量為，由n 個(gè)特征點(diǎn)坐標(biāo)構(gòu)成，因此，視覺通道的觀測(cè)方程為:其中，ηv為圖像噪聲。慣性觀測(cè)量為為加速度計(jì)輸出，和攝像頭光心加速度ak存在如下關(guān)系使用慣性數(shù)據(jù)更新狀態(tài)時(shí)，先用式(9)對(duì)觀測(cè)值進(jìn)行變換。慣性通道觀測(cè)方程為其中為觀測(cè)噪聲

3.2 UKF 算法流程

其中，κ 為比例參數(shù)，用于調(diào)節(jié)δ 點(diǎn)和ˉx 的距離，一般取κ=0;α 為縮放因子，控制δ 點(diǎn)的分布范圍，取(0，1)中較小的值;β 為引入狀態(tài)方程f(·)高階信息的參數(shù)，通常取β=2。融合視覺慣性的UKF 算法步驟如下

1)初始化狀態(tài)向量xk及其誤差方差矩陣

2)由式(10)計(jì)算ξi(k|k)，取Px為為xk－1。

3)由下式計(jì)算δ 點(diǎn)經(jīng)f(·)的非線性變換，并預(yù)測(cè)系統(tǒng)狀態(tài)和誤差協(xié)方差

4)根據(jù)當(dāng)前特征點(diǎn)采集匹配情況選擇采用視覺或慣性通道進(jìn)行預(yù)測(cè)更新。若特征點(diǎn)對(duì)數(shù)量大于n 則使用視覺通道進(jìn)行預(yù)測(cè)更新;否則，使用慣性通道進(jìn)行預(yù)測(cè)更新。根據(jù)攝像頭性能和匹配算法確定n 的值，本文取50。預(yù)測(cè)更新的具體步驟如下:

(1)計(jì)算預(yù)測(cè)觀測(cè)δ 點(diǎn)和預(yù)測(cè)觀測(cè)量

(2)計(jì)算預(yù)測(cè)觀測(cè)量對(duì)應(yīng)的協(xié)方差和觀測(cè)量與狀態(tài)向量的交互協(xié)方差

(3)當(dāng)k+1 時(shí)刻傳感器觀測(cè)量為z(k+1)，則狀態(tài)更新和更新協(xié)方差表示如下

4 慣性輔助的圖像匹配

利用圖像信息對(duì)狀態(tài)向量xk 進(jìn)行更新時(shí)，通過圖像平面間單應(yīng)性關(guān)系推導(dǎo)得出RCW，TCW［11］。但傳統(tǒng)的圖像匹配過程會(huì)消耗大量系統(tǒng)資源，制約了系統(tǒng)的實(shí)時(shí)性，為提高系統(tǒng)效率。本文在采用SURF［12］算法進(jìn)行匹配的同時(shí)利用姿態(tài)數(shù)據(jù)預(yù)估特征點(diǎn)位置，以縮小匹配范圍提高匹配效率減少誤匹配點(diǎn)。首先，由互補(bǔ)濾波得到慣性/地磁模塊從k－1 幀到k 幀的姿態(tài)的變化數(shù)據(jù)，右乘RIC將其轉(zhuǎn)換到I 下，表示為AR 應(yīng)用中用戶頭部平移速度較小，因此，在估計(jì)特征點(diǎn)位置時(shí)認(rèn)為采樣周期(1/60 s)內(nèi)攝像頭光心位移為0。然后，利用和估算第k－1 幀中的特征點(diǎn)在第k 幀中的坐標(biāo)位置，并以之為中心劃定圓形匹配區(qū)域，具體方法如下:

設(shè)點(diǎn)P 在k－1 幀和k 幀時(shí)刻在I 系中坐標(biāo)分別為Pk－1(xI(k－1)，yI(k－1)，zI(k－1))和Pk(xI(k)，yI(k)，zI(k))。點(diǎn)P所對(duì)應(yīng)的特征點(diǎn)在k－1 幀和k 幀圖像中坐標(biāo)分別為pk－1(xU(k－1)，yU(k－1))和pk(xU(k)，yU(k))，根據(jù)式(2)可得

合并以上兩式得

由對(duì)應(yīng)項(xiàng)求得pk－1和pk坐標(biāo)關(guān)系

利用慣性數(shù)據(jù)縮小匹配范圍，在節(jié)約系統(tǒng)資源的同時(shí)能夠大幅減少誤匹配的概率，提高了AR 系統(tǒng)的速度和精度。圖4 為注冊(cè)系統(tǒng)的總體結(jié)構(gòu)框圖。

圖4 注冊(cè)系統(tǒng)總體框圖Fig 4 Overall block diagram of registration system

5 實(shí)驗(yàn)結(jié)果與分析

在I7 3.4 GHz CPU，4GB RAM 的Windows7 微機(jī)上使用VS2010 和Matlab 混合編程實(shí)現(xiàn)了本文算法。采用第二節(jié)介紹的傳感器硬件系統(tǒng)，各傳感器間相對(duì)位姿已精確標(biāo)定。實(shí)驗(yàn)中將人工標(biāo)識(shí)至于W 系xoy 面，標(biāo)識(shí)中心點(diǎn)與W 系原點(diǎn)重合，固定標(biāo)識(shí)，使AR 頭盔做出一系列規(guī)定的平移旋轉(zhuǎn)動(dòng)作，對(duì)頭盔位姿進(jìn)行跟蹤。為保證實(shí)驗(yàn)環(huán)境的一致和可控，頭盔被固定在可控導(dǎo)軌上，用電機(jī)控制頭盔做出平移和旋轉(zhuǎn)以模擬用戶頭部動(dòng)作。用圖5 所示PST IRIS 高精度紅外跟蹤器對(duì)頭盔進(jìn)行跟蹤，將得到的位姿數(shù)據(jù)作為真值。

圖5 PST IRIS 紅外跟蹤器Fig 5 PST IRIS infrared tracker

圖6 為傳統(tǒng)EKF 算法［4］和本文算法在x，y，z 軸方向上跟蹤位置的誤差曲線;圖7 為滾轉(zhuǎn)、俯仰、偏航角的跟蹤誤差曲線。為驗(yàn)證算法的魯棒性，在穩(wěn)定跟蹤后對(duì)標(biāo)識(shí)進(jìn)行遮擋并對(duì)比兩種算法跟蹤誤差，圖中虛線所截時(shí)間為遮擋時(shí)間。由圖6、圖7 看出，和傳統(tǒng)EKF 算法相比本文算法在跟蹤精度和穩(wěn)定性上有明顯優(yōu)勢(shì)，收斂速度也快于傳統(tǒng)EKF 算法。當(dāng)對(duì)標(biāo)識(shí)進(jìn)行遮擋時(shí)，傳統(tǒng)EKF 算法在對(duì)位置和姿態(tài)的跟蹤上都出現(xiàn)發(fā)散，跟蹤精度快速下降造成目標(biāo)丟失。本文算法在對(duì)頭盔位置進(jìn)行跟蹤時(shí)，將視覺和慣性通道分開，當(dāng)攝像機(jī)無法獲取足夠數(shù)目特征點(diǎn)時(shí)采用慣性通道更新狀態(tài)，在一段時(shí)間內(nèi)時(shí)間內(nèi)仍能夠較好跟蹤目標(biāo)，算法整體上實(shí)現(xiàn)了持續(xù)穩(wěn)定的跟蹤注冊(cè)。

圖6 x，y，z 軸估計(jì)誤差對(duì)比Fig 6 Comparison of x，y，z-axis estimated error

圖7 滾轉(zhuǎn)角、俯仰角、偏航角估計(jì)誤差對(duì)比Fig 7 Comparison of roll，pitch，yaw angles estimated error

圖8 統(tǒng)計(jì)了使用傳統(tǒng)EKF 算法和本文算法的AR 系統(tǒng)輸出視頻幀速率。由于引入慣性模塊的姿態(tài)信息輔助進(jìn)行匹配，大幅減少了特征匹配和誤點(diǎn)剔除的計(jì)算量，使得AR系統(tǒng)輸出視頻的幀速率獲得了20.6%的提升。

6 結(jié)束語

本文提出了基于CF 和UKF 的多傳感器組合位姿注冊(cè)算法。通過互補(bǔ)濾波融合陀螺儀、加速度計(jì)和地磁感應(yīng)計(jì)的數(shù)據(jù)對(duì)觀察者頭部姿態(tài)進(jìn)行估計(jì);通過UKF 融合攝像頭和加速度計(jì)數(shù)據(jù)對(duì)頭部位置進(jìn)行估計(jì);在對(duì)攝像頭采集到的圖像進(jìn)行處理時(shí)，引入慣性傳感器測(cè)得的姿態(tài)信息以輔助特征點(diǎn)匹配。該算法解決了現(xiàn)有AR 系統(tǒng)中注冊(cè)算法的四個(gè)問題:1)動(dòng)作復(fù)雜的情況下難以穩(wěn)定注冊(cè);2)動(dòng)態(tài)環(huán)境下注冊(cè)精度低;3)數(shù)據(jù)融合效率低;4)算法魯棒性差，對(duì)遮擋、圖像噪聲等干擾抵抗能力弱。實(shí)驗(yàn)證明:該算法是一種魯棒高效的跟蹤注冊(cè)方法，能夠?qū)崿F(xiàn)AR 系統(tǒng)持續(xù)、穩(wěn)定、實(shí)時(shí)的注冊(cè)。

圖8 輸出視頻幀速率對(duì)比Fig 8 Comparison of output video frame rate

［1］ Hideyuki Suenaga，Huy Hoang Tran，Hongen Liao.Real－time in situ three－dimensional integral videography and surgical navigation using augmented reality:A pilot study［J］.International Journal of Oral Science，2013，5:98－102.

［2］ 王新龍.慣性導(dǎo)航基礎(chǔ)［M］.西安:西北工業(yè)大學(xué)出版社，2013:11－12.

［3］ 陳 靖，王涌天，劉 越，等.適用于戶外增強(qiáng)現(xiàn)實(shí)系統(tǒng)的混合跟蹤定位算法［J］.計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào)，2010，22(2):204－209.

［4］ Oskiper T，Chiu H P，Zhu Z W，et al.Stable vision－aided navigation for large－area augmented reality［C］∥Proceedings of the 2011 IEEE Virtual Reality Conference，Singapore:IEEE，2011:63－70.

［5］ 李薪宇，陳東義.基于混合跟蹤的增強(qiáng)現(xiàn)實(shí)系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)［J］.計(jì)算機(jī)應(yīng)用，2009，29(10):2853－2858.

［6］ Jonathan K，Gaurav S.Visual－inertial sensor fusion:Localization，mapping and sensor－to－sensor self－calibration［J］.The International Journal of Robotics Research，2010，30(1):56－79.

［7］ 郭軍海.彈道測(cè)量數(shù)據(jù)融合技術(shù)［M］.北京:國防工業(yè)出版社，2012:169－174.

［8］ 胡少興，劉東昌，張愛武，等.一種基于MEMS 傳感器的無人飛艇航姿測(cè)量系統(tǒng)［J］.傳感器與微系統(tǒng)，2014，33(3):58－61.

［9］ 劉輝邦，褚金奎，支 煒，等.基于STM32 的無人機(jī)姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)［J］.傳感器與微系統(tǒng)，2013，32(8):108－110.

［10］Zhang Z Y.A flexible new technique for camera calibration［J］.IEEE Transactions on Pattem Analysis and Machine Intelligence，2000，22(11):1330－1334.

［11］Zhang Z Y.Determining the epipolar geometry and its uncertainty:A review［J］.Int’l J of Computer Vision，1998，27(2):161－198.

［12］Bay H，Tuvtellars T，Gool L Van.SURF:Speeded up robust features［C］∥Proceedings of the European Conference on Computer Vision，2006:404－417.