基于零加速修正的室內慣性定位算法

蘇志剛，趙世偉，郝敬堂

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

0 引 言

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

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

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

1 算法框架

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

圖1 相關坐標系間關系

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

圖2 算法框架

2 零加速檢測

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

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

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

(1)

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

(2)

如前所述，零加速狀態時，平動加速度和轉動角速度均為零，所以零加速檢測可表述為

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

(3)

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

圖3 零加速狀態檢測結果

3 零加速姿態估計

(4)

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

(5)

(6)

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

(7)

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

(8)

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

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

(9)

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

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。

在零加速狀態下，兩坐標系的重力加速度向量間關系為

(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 位置解算

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

(20)

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

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

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

(21)

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

vn(t+dt)=vn(t)

(22)

由載體坐標系到慣性坐標系的姿態轉換矩陣的更新為

(23)

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

(24)

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

(25)

位置向量的更新方程為

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

(26)

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

5 時間及空間復雜度分析

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

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

(27)

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

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

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

(28)

關于算法的空間復雜度也可用相同的方式進行分析。算法的空間占用主要涉及原始數據及中間變量的存貯，主要有姿態矩陣、零加速狀態、速度和位置等，因此，相應的空間復雜度為

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

(29)

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

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

表1 算法復雜度對比

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

6 實驗結果分析

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

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

圖4 戶外實驗結果

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

圖5 室內實驗結果

7 結束語

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