蘇志剛，趙世偉，郝敬堂

(1.中國民航大學(xué) 智能信號與圖像處理天津市重點實驗室，天津 300300；2.中國民航大學(xué) 中歐航空工程師學(xué)院，天津 300300)

0 引 言

借助GPS定位功能，移動互聯(lián)網(wǎng)可以提供基于地理定位的信息或娛樂服務(wù)，即基于位置服務(wù)(location based services，LBS)[1]，其被廣泛應(yīng)用于各種場景中。在無GPS信號的環(huán)境中，如室內(nèi)、洞穴等，GPS信號受遮蔽影響，無法提供相應(yīng)的位置服務(wù)[2]。因此，需要不依賴于GPS定位功能的定位技術(shù)。

近年來，隨著智能手機功能逐漸完善，智能手機內(nèi)置的微傳感器使得不依賴于GPS的室內(nèi)定位成為可能?；赪IFI定位技術(shù)利用提前部署的WIFI熱點進行定位[3,4]，此技術(shù)過分依賴熱點的分布情況且存有盲區(qū)?；谒{牙定位技術(shù)同樣需要在室內(nèi)部署網(wǎng)關(guān)，并且傳播距離有限、實時性和穩(wěn)定性較差[5]。利用慣性傳感器進行定位的技術(shù)主要有行人航位推算(pedestrian dead reckoning，PDR)技術(shù)和捷聯(lián)慣性導(dǎo)航(strapdown inertial navigation，SIN)技術(shù)兩類[6]。如何確定行人的步長是PDR技術(shù)的核心問題。學(xué)者通過分析行人步行特點，給出不同的步長估計方案[7-9]。PDR技術(shù)精度通常為0.3%-10%，由于PDR技術(shù)過于依賴步長估計，具有較大的局限性。SIN技術(shù)則是基于慣性數(shù)據(jù)信號積分的方法，其難點在于如何有效地消除累積誤差[10]。Wang Yusheng等通過分析行人步態(tài)特點，將腳觸地時的零速信息作為卡爾曼濾波的狀態(tài)量補償慣性導(dǎo)航的偏差來解決定位系統(tǒng)中累積誤差問題[11]；Zeng Shijie等沒有增加額外的傳感器，提出利用智能手機的加速度峰值確定步數(shù)，并對每步的狀態(tài)進行卡爾曼濾波，從而抑制累積誤差引起的定位誤差發(fā)散[12]；R.Kannan等提出一種基于訓(xùn)練的自適應(yīng)閾值校準(zhǔn)算法，該算法利用自適應(yīng)閾值校準(zhǔn)來糾正慣性傳感器的錯誤從而達到消除誤差的目的[13]；Chen Zhenghua等在利用智能手機慣性傳感器進行人員定位的基礎(chǔ)上融合iBeacons距離測量來校正累積誤差[14]。

本文在未增加額外器件的前提下，通過對實時采集到的數(shù)據(jù)信號進行零加速檢測，并直接在零加速狀態(tài)對定位系統(tǒng)進行狀態(tài)估計與修正，從而在不增加其它額外算法的基礎(chǔ)上完成人員室內(nèi)定位。室內(nèi)外實驗結(jié)果表明該算法可以有效地解決無GPS信號區(qū)域的人員定位問題，并且在實用的基礎(chǔ)上具有較低的實現(xiàn)復(fù)雜度。

1 算法框架

利用智能手機內(nèi)置的慣性傳感器進行定位時，須考慮如圖1所示的3個坐標(biāo)系。以起始點為參考點的東北天(ENU)坐標(biāo)系定義的地理坐標(biāo)系(n系)，以手機中心為參考點建立平行于手機寬邊、長邊及垂直手機面的右手螺旋坐標(biāo)系ObXbYbZb作為載體坐標(biāo)系(b系)，以手機中心為參考點，平行于地理坐標(biāo)系各坐標(biāo)軸建立慣性坐標(biāo)系(i系)ObXiYiZi。 載體坐標(biāo)系與慣性坐標(biāo)系是共原點的，因此兩者間只存在旋轉(zhuǎn)操作，而慣性坐標(biāo)系與地理坐標(biāo)系對應(yīng)軸之間是平行的，因此兩者間存在著平移操作。載體坐標(biāo)系與慣性坐標(biāo)系間的旋轉(zhuǎn)角度用于解算平臺的姿態(tài)，慣性坐標(biāo)系與地理坐標(biāo)系之間的平移用于重構(gòu)平臺的軌跡。

圖1 相關(guān)坐標(biāo)系間關(guān)系

定位算法框架如圖2所示，由數(shù)據(jù)采集、零加速檢測、零加速姿態(tài)估計和位置解算4個模塊構(gòu)成[15]。利用智能手機的內(nèi)置傳感器，主要包括三軸加速度計、三軸陀螺儀和三軸磁力計等，實時地進行數(shù)據(jù)采集。由于各傳感器內(nèi)置于手機內(nèi)，因此，各傳感器輸出的數(shù)據(jù)是載體坐標(biāo)系形式。零加速檢測模塊是利用加速度及角速度是否低于設(shè)定門限來判定平臺是否處于零加速狀態(tài)。若檢測出平臺處于零加速狀態(tài)，將通過零加速估計模塊地對平臺的姿態(tài)進行估計，并將估計出的姿態(tài)信息送達位置解算模塊用于姿態(tài)修正。若零加速檢測模塊給出平臺處于非零加速狀態(tài)，則位置解算模塊不進行平臺姿態(tài)修正，只常規(guī)地進行位置解算。

圖2 算法框架

2 零加速檢測

零加速檢測是對零速檢測的拓展。根據(jù)行人步態(tài)特點，利用額外傳感器獲得腳觸地的零速信息[11]。如果僅利用智能手機的內(nèi)置傳感器無法有效獲得平臺的零速信息。然而，在人行走過程中存在著動平衡狀態(tài)，平動加速度與轉(zhuǎn)動加速度均為零，即零加速狀態(tài)，此時平臺處于相對穩(wěn)定的狀態(tài)，可通過對平臺的姿態(tài)估計實現(xiàn)對累積誤差的修正。

智能手機內(nèi)置的三軸加速度計測量的是平臺的平動加速度與重力加速度的矢量合成。在零加速狀態(tài)時，由于無平動加速度，三軸加速度計反映的是重力加速度。同時，平臺不發(fā)生轉(zhuǎn)動，使得三軸角速度計的輸出應(yīng)為零。因此，平臺零加速狀態(tài)可利用慣性傳感器輸出的三軸加速度和三軸角速度進行判決。

加速度模值檢測(acceleration magnitude detector，AMD)是利用加速度進行門限檢測

(1)

角速度模值檢測(angular rate magnitude detector，ARMD) 是利用三軸陀螺儀輸出的角速度進行檢測

(2)

如前所述，零加速狀態(tài)時，平動加速度和轉(zhuǎn)動角速度均為零，所以零加速檢測可表述為

ZAD(t)=D1(t)×D2(t)

(3)

根據(jù)實驗數(shù)據(jù)分別加速度模值、角速度模值和零加速檢測，結(jié)果如圖3所示。圖3中曲線取1時表示有效檢測狀態(tài)。由圖可見，每種檢測方案均可以獲得相應(yīng)的有效檢測結(jié)果，但ZAD的結(jié)果更能夠準(zhǔn)確地反映零加速狀態(tài)，且檢測出的有效狀態(tài)的數(shù)量相對較少。

圖3 零加速狀態(tài)檢測結(jié)果

3 零加速姿態(tài)估計

(4)

其中，I為三階單位矩陣，Κ(t) 為該單位向量r(t) 的斜對稱矩陣

(5)

(6)

向量gb(t) 和nb(t) 均是向量gi和ni繞單位向量r(t) 逆時針旋轉(zhuǎn)θ(t) 角獲得的，因此單位向量r(t) 在重力加速度矢量gb(t) 和gi的對稱面上，該平面的法向量為

(7)

同理，磁分量矢量nb(t) 和ni的對稱面包含向量r(t)。 相應(yīng)平面的法向量可表示為

(8)

向量r(t) 同時處于式(7)和式(8)所確定的兩平面上，即向量r(t) 位于兩平面的交線上，該交線的方向向量為

rl(t)=m1(t)×m2(t)

(9)

由此可得方向向量rl(t) 在慣性坐標(biāo)系各軸向的分量

rlxt=gyt(nzt-hz)-(gzt+g)(nyt-hy)

(10)

rlyt=(gzt+g)(nxt-hx)-gxt(nzt-hz)

(11)

rlzt=gxt(nyt-hy)-gyt(nxt-hx)

(12)

對方向向量rl(t) 進行歸一化計算

(13)

由式(13)即可得分量rxt，ryt，rzt。

在零加速狀態(tài)下，兩坐標(biāo)系的重力加速度向量間關(guān)系為

(14)

將式(4)、式(5)、式(6)代入式(14)整理得

-gxt/g=[1-cosθ(t)]rxtrzt+rytsinθ(t)

(15)

-gyt/g=[1-cosθ(t)]rytrzt-rxtsinθ(t)

(16)

(17)

由式(17)可得

(18)

將式(18)代入式(16)并整理可得

(19)

由式(18)和式(19)可獲得角度θ(t) 的值。

4 位置解算

在平臺運動過程中，三軸加速度計輸出的載體坐標(biāo)系下的加速度向量ab(t)需要轉(zhuǎn)換為地理坐標(biāo)系下的加速度向量an(t)。 由于地理坐標(biāo)系與慣性坐標(biāo)系下的加速度向量相等，即an(t)=ai(t)， 所以

(20)

顯然，地理坐標(biāo)系下的加速度an(t) 是平臺的平動加速度，已經(jīng)消除了重力加速度的影響。

根據(jù)平動加速度an(t)， 可將速度更新方程表示為

vn(t+dt)=vn(t)+an(t)×dt

(21)

式中：vn(t) 和vn(t+dt) 分別為速度的當(dāng)前值與更新值。當(dāng)零加速狀態(tài)時，式(21)可轉(zhuǎn)化為

vn(t+dt)=vn(t)

(22)

由載體坐標(biāo)系到慣性坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣的更新為

(23)

式中：I為單位矩陣，Β(t) 為斜對稱矩陣，用于定義轉(zhuǎn)動方向

(24)

其中，dt為采樣時間間隔，ωxt、ωyt和ωzt分別為時刻t的3個軸向角速度計分量。在零加速狀態(tài)時，Β(t) 為3×3的零矩陣，即，此時式(23)簡化為

(25)

位置向量的更新方程為

pn(t+dt)=pn(t)+vn(t)×dt

(26)

其中，pn(t) 為當(dāng)前時刻的位置向量，pn(t+dt) 為位置向量的更新值。

5 時間及空間復(fù)雜度分析

本文算法涉及的時間消耗主要發(fā)生于零加速狀態(tài)檢測、姿態(tài)矩陣估計和位置解算3個方面。對于實驗中獲得的m個樣本所能夠檢測到的零加速狀態(tài)有w個。顯然，w≤m。 當(dāng)w

T1=(1+w)α+(β+γ)m

(27)

式中：α為估計姿態(tài)矩陣所消耗的時間，β和γ分別為單次零加速狀態(tài)檢測和位置解算所消耗的時間。由于初始化時也需要對姿態(tài)矩陣進行估計，因此姿態(tài)矩陣的估計次數(shù)為 (1+w)。

將w=m代入式(27)獲得算法的時間復(fù)雜度峰值

T2=α+(α+β+γ)m

(28)

關(guān)于算法的空間復(fù)雜度也可用相同的方式進行分析。算法的空間占用主要涉及原始數(shù)據(jù)及中間變量的存貯，主要有姿態(tài)矩陣、零加速狀態(tài)、速度和位置等，因此，相應(yīng)的空間復(fù)雜度為

S=μ+(μ+ν+τ+ρ)m

(29)

式中：μ為姿態(tài)矩陣消耗的空間，ν、τ和ρ分別為單次零加速檢測、速度和位置瞬時值所消耗的空間。

文獻[12]與文獻[14]均是利用智能手機的內(nèi)置傳感器進行定位的方法，因此，本文算法與上述兩種算法的復(fù)雜度比較見表1。

表1 算法復(fù)雜度對比

文獻[12]中提出的算法在行人運動過程中首先需要對行人的零速狀態(tài)進行檢測，當(dāng)檢測到零速狀態(tài)時，將此時的傳感器輸出作為狀態(tài)量代入卡爾曼濾波算法中來緩解累積誤差，提高導(dǎo)航精度；而本文提出的算法在檢測行人零加速狀態(tài)的基礎(chǔ)上，只需要重新估計此狀態(tài)下的平臺姿態(tài)，無需輔助其它額外算法即可實現(xiàn)累計誤差的消除。由表1可見，本文算法在時間復(fù)雜度和空間復(fù)雜度上明顯優(yōu)于文獻[12]算法。表1還顯示，文獻[14]算法略優(yōu)于本文算法。文獻[14]算法無需進行零速狀態(tài)檢測，但它需要提前部署的ibeacons網(wǎng)關(guān)實現(xiàn)對行人位置距離校準(zhǔn)。通常部署的ibeacons網(wǎng)關(guān)較少，因此校準(zhǔn)的次數(shù)較少，消耗的時間和空間資源相對而言就比較少。然而，文獻[14]算法增大了對環(huán)境要求，提高了實現(xiàn)成本。

6 實驗結(jié)果分析

實驗平臺采用iPhone 6 plus智能手機，其內(nèi)置的三軸加速度計和三軸角速度計是Invensense公司的MPU-6700集成的，三軸磁力計采用的是AKM半導(dǎo)體公司的AK8963C。利用手機端Matlab通過IP與電腦端Matlab互聯(lián)，實現(xiàn)傳感器數(shù)據(jù)的采集與處理。數(shù)據(jù)采樣率設(shè)置為100 Hz。零加速檢測中的加速度檢測閾值為th1min=9.2和th1max=10.4， 角速度檢測閾值為th2max=0.2。

為檢驗定位算法的有效性，首先進行戶外定位實驗，利用GPS定位數(shù)據(jù)檢驗本文算法的有效性。在戶外手持智能手機進行了“Z”字形運動。將零加速修正的軌跡、未進行零加速修正的軌跡和GPS軌跡相比較，如圖4所示。由圖4可見，未進行零加速修正的軌跡很快就發(fā)散，導(dǎo)致定位遠偏離實際位置。當(dāng)采用了零加速修正，相應(yīng)的軌跡能夠較好地保持在真實軌跡附近。

圖4 戶外實驗結(jié)果

對于室內(nèi)實驗，實驗人員沿著室內(nèi)固定軌跡行走，采用零加速修正與否的對比實驗結(jié)果如圖5所示。由圖5可見，零加速修正方法可以有效地避免軌跡發(fā)散現(xiàn)象。

圖5 室內(nèi)實驗結(jié)果

7 結(jié)束語

基于智能手機內(nèi)置傳感器進行慣性定位時，由于累積誤差導(dǎo)致平臺軌跡重現(xiàn)時存在軌跡發(fā)散問題。本文針為此提出一種基于零加速度檢測與姿態(tài)修正的算法，利用檢測出的零加速狀態(tài)實現(xiàn)對平臺姿態(tài)的重新估計與校正，克服慣性傳感器累積誤差持續(xù)增長問題，在未增加額外器件基礎(chǔ)上，實現(xiàn)對人員的自主慣性定位。理論分析與實驗結(jié)果表明該算法具有較低的時間與空間復(fù)雜度，且可以不借助GPS定位信息實現(xiàn)對人員位置的確定。