智能手機(jī)多傳感器融合的實(shí)時(shí)定位算法

崔思柱，劉豐豪，程石，肖倩

（長(zhǎng)安大學(xué)工程機(jī)械學(xué)院，西安 710064）

0 引言

現(xiàn)代智能手機(jī)配置了各類傳感器，包括全球衛(wèi)星定位系統(tǒng)GPS（Global Position System）、加速度計(jì)、角速度計(jì)、氣壓計(jì)及多個(gè)攝像頭等，這些配置使手機(jī)能夠通過多種方式獲得當(dāng)前的位姿信息[1]。主流的定位方式仍是通過GPS，但是復(fù)雜環(huán)境中會(huì)存在弱GPS甚至無(wú)GPS信號(hào)的地方，例如密集的建筑群街道、室內(nèi)、野外叢林等，此時(shí)GPS信號(hào)無(wú)法滿足精確定位要求。因此需要采用新的方式來(lái)進(jìn)行實(shí)時(shí)定位以獲取實(shí)時(shí)位姿信息。

目前跟蹤定位方式主要有兩種，一是基于視覺傳感器的姿態(tài)跟蹤方式，二是基于慣性傳感器的姿態(tài)跟蹤方式。前者通過對(duì)視覺傳感器獲得的圖像進(jìn)行特征提取、特征匹配、三角化、PNP、最小化重投影誤差，計(jì)算出傳感器的位置，通過坐標(biāo)變換進(jìn)而獲得搭載傳感器的物體瞬時(shí)位置和姿態(tài)。具有實(shí)時(shí)、非接觸等優(yōu)點(diǎn)，但受到光照強(qiáng)度，運(yùn)動(dòng)速度的限制。基于慣性傳感器的跟蹤系統(tǒng)中，采集原始數(shù)據(jù)的硬件為陀螺儀和加速度儀。通過陀螺儀采集的角速度和加速度計(jì)采集的加速度信息，將兩者對(duì)時(shí)間進(jìn)行積分可以粗略獲得搭載慣性傳感器物體的姿態(tài)。由于慣性傳感器的輸出頻率較高，在快速運(yùn)動(dòng)時(shí)仍然適用，物體外觀和外界環(huán)境變化對(duì)計(jì)算結(jié)果不會(huì)產(chǎn)生影響。受噪聲及其自身偏差影響，慣性傳感器數(shù)據(jù)誤差會(huì)隨時(shí)間逐漸累積，其準(zhǔn)確率也會(huì)隨之下降，所以只能適應(yīng)短時(shí)間精確定位要求。本文提出融合視覺和慣性測(cè)量系統(tǒng)的方案，結(jié)合上述兩種定位方式的優(yōu)點(diǎn)以獲取更加精確的位姿。該方案流程如圖1所示。

圖1 傳感器融合定位方式流程圖

1 理論基礎(chǔ)及模型建立

1.1坐標(biāo)系定義

相機(jī)坐標(biāo)系{C}：目前智能手機(jī)相機(jī)模型通常是針孔模型，本文通過將模型中心作為原點(diǎn)建立三維相機(jī)坐標(biāo)系。以相機(jī)的光軸作為坐標(biāo)系的Z軸，對(duì)應(yīng)坐標(biāo)軸Xc、Yc與圖像物理坐標(biāo)系平行。

圖像坐標(biāo)系{P}：圖像像素坐標(biāo)系以圖像左上角為坐標(biāo)原點(diǎn)，橫坐標(biāo)u和縱坐標(biāo)v分別是圖像所在的行和列；圖像物理坐標(biāo)系以圖像的中心為原點(diǎn)，坐標(biāo)軸X和Y分別與像素坐標(biāo)系的u,v對(duì)應(yīng)[2]。

慣性坐標(biāo)系{I}：慣性坐標(biāo)系的Zi軸與相機(jī)坐標(biāo)系的 Zc軸平行且方向相反，Xi、Yi與相機(jī)坐標(biāo)系 Xc、Yc成90°夾角，原點(diǎn)位于相機(jī)坐標(biāo)系原點(diǎn)的正下方。當(dāng)相機(jī)固定在機(jī)器人或其他設(shè)備上時(shí)，慣性坐標(biāo)系即默認(rèn)為運(yùn)動(dòng)載體的體坐標(biāo)系。

世界坐標(biāo)系{W}：全局坐標(biāo)系選取具有任意性，本文設(shè)定Zw垂直向上，將前進(jìn)方向設(shè)定為Xw正方向，通過右手坐標(biāo)系規(guī)則即可確定Yw方向。具體關(guān)系如圖2所示。

圖2 智能手機(jī)相關(guān)坐標(biāo)系

1.2前端數(shù)據(jù)處理

（1）視覺前端處理

采集圖像的視覺傳感器設(shè)備為智能手機(jī)攜帶的后置相機(jī)，常規(guī)后置相機(jī)像素通常為800萬(wàn)或1500萬(wàn)，性能優(yōu)異的高端機(jī)可達(dá)到2000萬(wàn)。對(duì)于具有實(shí)時(shí)要求的定位系統(tǒng)而言，計(jì)算量將會(huì)顯著增加。選取合適的特征處理方式對(duì)于提高位姿估計(jì)精度十分重要。

位姿估計(jì)過程中，對(duì)圖像進(jìn)行特征提取是前端處理的重要部分，特征提取的精確性和穩(wěn)定會(huì)顯著影響位姿估計(jì)。如果在視覺傳感器采集的原始圖像中選中某些具體的物體作為特征，將無(wú)法適應(yīng)復(fù)雜多變的環(huán)境，而通過選定圖像中簡(jiǎn)單、明顯的點(diǎn)特征，不僅能夠增強(qiáng)系統(tǒng)適應(yīng)性而且會(huì)減小計(jì)算量。對(duì)于特征點(diǎn)處理分為以下步驟，檢測(cè)、提取、匹配、剔除離群點(diǎn)，目前視覺導(dǎo)航中對(duì)于特征提取算法有Harris角點(diǎn)，David G.Lowe提出的SIFT算法[3]以及Herbert Bay等人提出的SURF算法[4]等。SURF基于積分圖像提取特征點(diǎn)，通過Haar小波濾波器描述特征點(diǎn)，是一種集特征提取和描述于一體的算法，具有尺度不變性[5]。其抗干擾能力強(qiáng)，運(yùn)算量低于SIFT算法，運(yùn)算速度卻更快，并且結(jié)合了SIFT算法的許多優(yōu)點(diǎn)[6]?？紤]到智能手機(jī)像素通常較高，因此選用SURF算法進(jìn)行圖像的特征處理。

前端處理需要首先對(duì)特征點(diǎn)進(jìn)行高斯濾波，構(gòu)建Hessian矩陣并建立圖像的尺度空間，利用非極大值抑制方式選擇特征點(diǎn)并精確定位，計(jì)算得到每個(gè)特征點(diǎn)的主方向，并構(gòu)造SURF特征點(diǎn)描述子。最后通過判定最近鄰歐氏距離和次近鄰歐氏距離的比值是否小于設(shè)定閾值，進(jìn)行特征匹配。通常SURF特征匹配會(huì)存在誤匹配的現(xiàn)象，本文通過RANSAC算法剔除離群點(diǎn)，以消除誤匹配帶來(lái)的影響。如圖3所示。

圖3 實(shí)驗(yàn)場(chǎng)景特征點(diǎn)匹配

（2）慣性傳感器前端處理

實(shí)時(shí)定位系統(tǒng)中使用關(guān)鍵幀技術(shù)能夠減小后端非線性優(yōu)化求解器的計(jì)算量，然而IMU數(shù)據(jù)輸出頻率通常比視覺傳感器輸出頻率高很多。例如本文使用的小米智能手機(jī)，相機(jī)幀率約23Hz，慣性傳感器頻率約200Hz。通過對(duì)兩幀圖像間隔內(nèi)的IMU數(shù)據(jù)積分成相對(duì)運(yùn)動(dòng)進(jìn)而抑制優(yōu)化變量的快速增長(zhǎng)，避免當(dāng)姿態(tài)變化時(shí)重新傳輸IMU數(shù)據(jù)，實(shí)現(xiàn)實(shí)時(shí)優(yōu)化。

t1、t2為相鄰時(shí)間間隔，P、V、φ分別為慣性坐標(biāo)系相對(duì)全局坐標(biāo)系的位置、速度、旋轉(zhuǎn)矩陣。

對(duì)于IMU的狀態(tài)傳輸需要慣性坐標(biāo)系和世界坐標(biāo)系之間的變換，這個(gè)全局旋轉(zhuǎn)矩陣的求解需要已知初始姿態(tài)。如果將整個(gè)系統(tǒng)的參考坐標(biāo)轉(zhuǎn)變?yōu)槌跏甲鴺?biāo)系I0，并且在IMU傳輸過程中轉(zhuǎn)變?yōu)镮k，則可對(duì)公式中與加速度和陀螺儀相關(guān)的部分進(jìn)行積分。

2 傳感器融合及優(yōu)化

2.1初始化

單目相機(jī)具有尺度不確定性，如果沒有良好的初始估計(jì)值很難直接融合視覺和IMU測(cè)量值。通常會(huì)采用平穩(wěn)假設(shè)，前幾秒IMU測(cè)量值的平均值作為重力向量，但是當(dāng)IMU測(cè)量值受到偏置影響，或在初始時(shí)刻出現(xiàn)加速運(yùn)動(dòng)，這種假設(shè)不成立。該融合算法中采用松耦合傳感器融合的方式來(lái)得到初始值，通過五點(diǎn)法或八點(diǎn)法，得到位姿的初始估計(jì)值（旋轉(zhuǎn)矩陣、平移向量）。將IMU預(yù)積分與視覺SfM結(jié)果對(duì)齊，可粗略地恢復(fù)尺度、重力、速度、加速度計(jì)和陀螺儀的偏置[7]。

首先，檢測(cè)相鄰兩幀對(duì)應(yīng)的特征，如果特征數(shù)量超過35個(gè)，平均視差超過了25像素，通過八點(diǎn)法恢復(fù)相鄰兩幀間的旋轉(zhuǎn)矩陣和平移向量。固定平移矩陣的尺度并三角化兩幀間所有觀測(cè)特征。基于三角化的特征，采用PnP方式估計(jì)其他幀的位姿。最后通過Bundle Adjustment方式最小化所有觀測(cè)特征的重投影誤差[8]。

設(shè)定第一個(gè)相機(jī)坐標(biāo)作為基準(zhǔn)坐標(biāo)系，所有特征的位置和姿態(tài)對(duì)于該坐標(biāo)系。假定已知相機(jī)和IMU的外參,則可以將所有以相機(jī)為基準(zhǔn)的位姿轉(zhuǎn)為以IMU為基準(zhǔn)的位姿。

通過最小化所有IMU和視覺測(cè)量值誤差的范數(shù)之和，即求解最小二乘問題，實(shí)現(xiàn)對(duì)該系統(tǒng)的初始化。

{γ，J}為先驗(yàn)值。MI，Mcjl分別對(duì)應(yīng) IMU 和視覺測(cè)量值模型，前者通過預(yù)積分公式（1）～（3）可求得，后者為第i幀中特征點(diǎn)的像素值對(duì)應(yīng)的第j幀中第l個(gè)特征點(diǎn)的觀測(cè)值。雅克比矩陣JI，Jcj分別對(duì)應(yīng)測(cè)量值模型MI，Mc對(duì)狀態(tài)量 x的偏導(dǎo)。

2.2緊耦合非線性優(yōu)化

初始化后，為了得到更加精確的狀態(tài)估計(jì)值，需要對(duì)初始化結(jié)果進(jìn)行優(yōu)化處理。定義狀態(tài)向量為：

TIc為相機(jī)和IMU的外參。xk為第k幀對(duì)應(yīng)的狀態(tài)量，包括位置、速度、方向以及偏置。最小化先驗(yàn)值和所有測(cè)量值殘差的馬氏范數(shù)之和，以得到最大后驗(yàn)估計(jì)值。

兩個(gè)連續(xù)幀間的IMU測(cè)量值殘差。

兩個(gè)相鄰關(guān)鍵幀的視覺測(cè)量值殘差，即重投影誤差，[Fx，F(xiàn)y，F(xiàn)z]為對(duì) i幀中第 l個(gè)特征點(diǎn)進(jìn)行反投影得到特征點(diǎn)，通過相關(guān)變換求得j幀中相對(duì)應(yīng)的特征點(diǎn)的空間坐標(biāo)。[u，v]為j幀中第l個(gè)特征點(diǎn)對(duì)應(yīng)的歸一化坐標(biāo)。

3 實(shí)驗(yàn)結(jié)果及數(shù)據(jù)分析

實(shí)驗(yàn)中手持小米note智能手機(jī)進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)環(huán)境為室內(nèi)走廊，走廊中線即行走路徑，大小為14.4m×5.6m。實(shí)驗(yàn)結(jié)果如圖4，紅線為行走路線，藍(lán)線為測(cè)量軌跡，以實(shí)驗(yàn)區(qū)域外1.5m處為起始點(diǎn)，紅圈為設(shè)定終點(diǎn)（1.5,0,0），實(shí)際測(cè)量軌跡終點(diǎn)為（2.1044,-0.3480,0.1128）。設(shè)定實(shí)驗(yàn)總長(zhǎng)度為42m，實(shí)際軌跡長(zhǎng)度為44.98m。Z軸在整個(gè)實(shí)驗(yàn)過程隨時(shí)間累積誤差為0.1128m。試驗(yàn)中經(jīng)過了三次轉(zhuǎn)彎，起始端和設(shè)定軌跡基本重合，第一次轉(zhuǎn)彎后出現(xiàn)明顯方向偏差，隨后分離程度增大。

實(shí)驗(yàn)結(jié)果可知隨著時(shí)間增加定位精度會(huì)下降，同時(shí)姿態(tài)角多次變化也會(huì)降低定位精度。

圖4 室內(nèi)走廊實(shí)驗(yàn)