基于智能手機的室內(nèi)外行人定位算法

李冬陽，趙忠華，晏懿琳，盧 菁

(上海交通大學 電子信息與電氣工程學院 儀器科學與工程系，上海 200240)

0 引 言

在室內(nèi)外的環(huán)境下，慣性導航系統(tǒng)[1](inertia navigation system，INS)可以克服全球衛(wèi)星定位系統(tǒng)(global navigation satellite system，GNSS)信號失效的困難。隨著手機傳感器精度的提高，使用手機進行簡單便捷的導航定位也逐漸成為了研究熱點。

行人慣性導航的傳統(tǒng)方法是將慣性測量單元模塊固定在足部，通過對陀螺儀輸出角速度的積分獲得姿態(tài)信息，利用姿態(tài)對加速度二次積分得到位置信息。由于MEMS(micro electro mechanical system)陀螺儀具有零偏和漂移，使得到的航向角出現(xiàn)不可避免的誤差，而且誤差會隨時間推移累積。文獻[2]提出了啟發(fā)式漂移減少的技術(shù)，將航向誤差降低了近一個數(shù)量級。但是這種方法無法修正曲線運動的航向角。文獻[3]利用滑動窗口的方法用三軸磁力計來實時修正航向角，在噪聲環(huán)境顯示出良好的性能。但是磁力計容易受環(huán)境影響，從而降低導航定位穩(wěn)定性。文獻[4]將地圖信息作為輔助，在40 min的步行過程中解算出優(yōu)于5 m的位置精度，但該方法依賴于地圖信息輔助，適用范圍較窄。為使用智能手機這種便捷的設(shè)備在室內(nèi)外都提供較好的定位精度，提出了一種航向修正的方式，利用上一零速時刻的航向角與下一動態(tài)時刻航向角的差值作為觀測來修正航向角，設(shè)計在室內(nèi)的外磁干擾檢測算法，在較強的磁干擾下選擇使用陀螺儀航向角。使用Java語言在Android Studio上開發(fā)手機傳感器采集應(yīng)用來采集傳感器變化值，然后將數(shù)據(jù)拷貝到PC端使用Matlab進行計算。通過實驗驗證，對航向修正和高程修正有一定的效果。

1 行人導航系統(tǒng)原理

用微慣性測量單元來做行人導航系統(tǒng)的方案有兩種：SINS算法和PDR算法。后者主要步驟是步長估計和方向估計，通過將步長投影到前進方向的二位平面上，不能獲得三維的空間信息，適用場景有限。捷聯(lián)慣性導航系統(tǒng)可以獲得的三維的位置信息和姿態(tài)信息，適用場景廣泛，但是由于微慣性測量單元傳感器精度低，必須在定位期間及時修正，否則速度和位置會發(fā)散，導致導航失敗。所以行人導航算法的設(shè)計是關(guān)鍵技術(shù)，本文將微慣性測量單元固聯(lián)在腳尖，利用行人腳部約0.1 s～0.3 s[5]的靜止狀態(tài)來修正速度，從而修正位移，提高定位精度，整體算法結(jié)構(gòu)如圖1所示。

圖1 行人導航解算流程

1.1 行人導航解算

指正本文涉及到兩種坐標系分別是導航坐標系和載體坐標系：

導航坐標系(n系)：本文的導航坐標系為北-東-地(N-E-D)坐標系。導航系的原點設(shè)在載體重心，xn軸與正東方向重合，yn軸與正北方向重合，zn軸符合右手正交定則。

載體坐標系(b系)：本文載體坐標系定義為前-右-下，原點位于載體重心，xb軸與載體縱軸方向重合，yb軸與載體橫軸方向重合，zb軸符合右手正交定則。

行人導航系統(tǒng)解算可以分為4個部分：

(1)利用加速計、陀螺儀和磁力計的輸出解算出方向余弦矩陣，是轉(zhuǎn)換坐標系的基礎(chǔ)；

(1)

(2)

(4)得到導航坐標系的速度的基礎(chǔ)上，積分得到導航系的位移，如式(3)所示。在積分之前根據(jù)零速檢測結(jié)果對速度進行修正，否則結(jié)果誤差很大

(3)

按照上述公式直接解算會得到較大的誤差，所以對誤差模型分析和建立。

1.2 行人導航誤差

行人導航系統(tǒng)算法是在理想條件基礎(chǔ)上建立，而實際情況中有多種多樣的誤差。誤差的主要來源有：傳感器初始的安裝誤差、傳感器本身精度不高帶來的誤差、一些在期望之外的干擾運動所產(chǎn)生的誤差以及數(shù)據(jù)解算的近似誤差。所以對導航誤差模型的準確建立十分重要，下面對系統(tǒng)的姿態(tài)、速度、位置進行誤差建模[6]。

姿態(tài)誤差

(4)

在零速時刻，ωin=ωie，δωin=0；忽略安裝誤差和刻度系數(shù)造成的誤差，不考慮右邊第一項，將上式化簡為

(5)

其中，εn為導航系的等效陀螺漂移，包含白噪聲，一階馬爾可夫還有隨機常值，在零速修正過程可以通過卡爾曼濾波對其估計。

速度誤差方程

(6)

在零速時刻，vn=0，ωen=0，不考慮刻度誤差和安裝誤差，上式可簡化為

(7)

位置誤差方程

(8)

在零速時刻，ωen=0，所以上式簡化為

(9)

對姿態(tài)、速度、位置的誤差模型建立之后，將誤差模型整理到卡爾曼濾波模型里，然后對導航參數(shù)進行修正，在第四節(jié)中詳細介紹卡爾曼濾波模型。

2 零速檢測

如果只按照上述導航方程解算位置，將會有很大的誤差，造成導航定位失敗，所以要利用行人行走的規(guī)律來修正位置。當人在行走的過程中，一只腳處于擺動狀態(tài)，而另一只腳有短暫的靜止狀態(tài)。正是有大約0.1 s～0.3 s的靜止狀態(tài)，可以用來修正速度，將誤差反饋到觀測器中。主要的方法分為兩類，一類是借助外部傳感器來檢測，一類是利用自身的加速度計和陀螺儀來判斷。文獻[7]用傳統(tǒng)的加速度模值和加速度滑動方差來進行零速檢測，具有一定的可行性。文獻[8,9]使用了SVM和馬爾可夫模型將零速區(qū)間分為幾個部分，取得了較好的結(jié)果。文獻[10]提出了自適應(yīng)閾值的零速檢測算法，以適應(yīng)行人不同的運動狀態(tài)，使零速檢測率達99%。

本文使用了多條件判定算法，同時使用加速度計和陀螺儀實現(xiàn)零速檢測，給加速度和角速度加一個窗口為15的中值濾波，使噪聲平滑，提高檢測算法精度。

3個條件分別為C1，C2，C3：

(10)

(2)在零速區(qū)間內(nèi)加速度和處于重力加速度附近，所以加速度矢量和方差值在該區(qū)域趨于零，而在動態(tài)區(qū)域較大

(11)

(12)

(3)在零速區(qū)域內(nèi)角速度矢量和趨于0,根據(jù)經(jīng)驗設(shè)置上限thωmax=1.2 rad/s

(13)

(14)

前3個條件得到的判定結(jié)果都略大于實際的零速區(qū)間，否則取交集之后零速區(qū)間會過小，導致速度發(fā)散，導航失效。取3個條件的交集之后得到圖2中最下面的判定曲線，和加速度模值基本吻合，驗證判定結(jié)果較好。

圖2 零速檢測結(jié)果

3 室內(nèi)外航向修正和高程修正算法

通常在零速狀態(tài)下可以用ZUPT技術(shù)來修正速度和位移，但是航向角誤差卻無法觀測，這將會導致航向角發(fā)散，無法完成導航定位任務(wù)。同時行人導航系統(tǒng)還伴隨著高度發(fā)散問題，除了ZUPT技術(shù)外，可以通過智能手機的氣壓計來抑制高度發(fā)散。文獻[11]提出了啟發(fā)式航向修正、零角速度修正和磁力計修正的方法。啟發(fā)式航向修正方法利用行人在相鄰兩步的航向角差來修正直線運動，設(shè)立閾值來判斷兩步之間是否發(fā)生轉(zhuǎn)彎，如果判定為轉(zhuǎn)彎則不補償，否則就視為直線運動補償航向誤差。但是該方法只能補償直線運動，對于曲線運動效果有限。磁力計修正的方法依賴于磁力計精度而且易受外界環(huán)境干擾，如果在室內(nèi)導航定位精度會受到較大影響。文獻[12]將相鄰零速時刻的航向角差值作為觀測量，來修正航向角將導航定位精度提高到2%，但是該方法類似于啟發(fā)式航向修正，如果兩個零速時刻之間發(fā)生了轉(zhuǎn)向則會有較大的航向差值，導致航向修正發(fā)散。本文在這3種方法的基礎(chǔ)上提出用上一零速時刻的航向角和下一動態(tài)時刻的航向角差值作為觀測量來修正航向角，在曲線運動中取得了良好的效果。在室內(nèi)環(huán)境有不同大小和方向的外磁干擾，如果只使用純地磁航向角來修正航向角會不可避免地使航向產(chǎn)生誤差，影響導航定位精度。將受到較大磁干擾的區(qū)間檢測出來，在這個區(qū)間內(nèi)使用陀螺儀航向角代替磁航向角，使算法在室內(nèi)也能表現(xiàn)良好。利用手機中的氣壓計來抑制高度發(fā)散，這將在長時間導航任務(wù)中發(fā)揮著重要作用。

3.1 傳統(tǒng)航向修正

(1)啟發(fā)式航向修正

當行人按直線或接近直線的線路行走時，相鄰兩步的航向角變化是非常小的。行人進行曲線行走時，當前這步的航向角相對于前一步的航向角會發(fā)生較大變化，因此可以利用相鄰步之間航向角的突變來檢測行人的曲線運動。為了保證轉(zhuǎn)向檢測的正確率，本文采用3個相鄰步間的航向變化來檢測曲線運動，如式(15)所示

(15)

式中：m表示行走路線狀態(tài)，1代表直線，0代表轉(zhuǎn)彎；φk表示當前步的航向角，φk-1和φk-2分別表示前一步和前兩步的航向角，φth表示偏差閾值。通常情況下，設(shè)定角度值8°作為閾值。當判斷行人走直線時，計算當前航向角與上一時刻航向角的差值，將差值補償?shù)较乱粋€航向值,此處航向角選取靜態(tài)的第一個點

(16)

(2)零角速率修正

當檢測到零速區(qū)間時，真實速度和角速度都為0。零速時刻陀螺儀輸出的角速度值為偏差，可以用來修正角速度，如式(17)所示，從而能修正航向角來提高姿態(tài)解算精度

(17)

(3)磁力計融合

本文所使用的IMU傳感器中包含磁力計，可以用磁力計輸出值來修正航向角。磁力計是一種無源器件，通過測量地球的磁場強度，可以得到載體的絕對航向角。不同環(huán)境對磁力計的準確度影響較大，尤其是室內(nèi)。每次在室內(nèi)使用前要先修正，從而得到更準確的讀數(shù)。

(18)

可以計算出航向角

(19)

其中，φ0為磁偏角，可以通過表查得。

3.2 利用零速時刻的航向角作為動態(tài)的觀測

通常，解算出的航向角在零速時刻比較穩(wěn)定，而在動態(tài)時上下波動較大，因為積分出的速度和位置信息也處在動態(tài)時刻，只利用到了動態(tài)時刻的航向角。在之前的研究者們的工作中，很少有利用零速時刻比較穩(wěn)定的航向角，文獻[12]中利用了相鄰零速時刻的航向角差值作為觀測量來修正航向，該方法更適用于直線運動中，作者的實驗路線選擇了長方形。然而實際情況中曲線和直線運動混雜，所以本文在ZIHR(zero integrated heading rate)修正方法的基礎(chǔ)上提出一種航向修正方法，適用在直線和曲線運動，利用上一時刻零速區(qū)間的航向角和下一動態(tài)時刻航向角的差值作為觀測量，從而來修正航向的漂移，通過實驗驗證提高行人導航定位的精度。

載體坐標系相對于導航坐標系的旋轉(zhuǎn)角速度向量ωnb為

(20)

將旋轉(zhuǎn)角速率分解到載體坐標系上

(21)

展開成矩陣形式為

(22)

對上式求逆可得

(23)

(24)

(25)

(26)

(27)

(28)

將航向角整理到觀測方程中

(29)

將上式整合到觀測矩陣里為

航向角可觀測性分析：對于腳綁式行人慣性導航系統(tǒng)，航向角在靜態(tài)時比較穩(wěn)定，而在動態(tài)時航向角變化較大。按照大多數(shù)人的行走習慣，行人在靜態(tài)時候的航向也是下一時刻踢腳的方向。當IMU在靜態(tài)時，由于沒有動態(tài)誤差，解算出來的航向角的誤差也更小，利用在靜態(tài)時的航向信息來修正下一時刻的動態(tài)航向角，提高行人導航定位精度。

3.3 檢測磁力計是否受到外磁干擾

對于室外空曠的地方，磁力計能提供持續(xù)穩(wěn)定的航向信息，然而室內(nèi)容易受到磁干擾，會嚴重影響到解算結(jié)果。為了解決這個問題，在文獻[13]中提出地磁航向角與陀螺儀航向角差值的閾值條件，檢測是否受到硬磁場的干擾。但是這種檢測外磁干擾的方法過于單一，假如陀螺儀航向角解算誤差過大導致系統(tǒng)錯誤檢測了外磁干擾區(qū)間。所以在此方法基礎(chǔ)上再使用磁力計模值閾值條件和磁力計模值方差檢測條件共同判斷受外磁干擾的區(qū)間

(30)

式中：ξM為行走前靜止狀態(tài)磁力計模值均值，m為方差窗口大小，ξgc為磁航向角與陀螺儀航向角差值閾值。同時滿足上式，說明磁力計在此刻受到外磁干擾，此時磁力計提供穩(wěn)定可靠的航向角。如圖3所示，磁力計測量值模值局部變化較為劇烈，但是整體來看是一個緩變量，行走到不同的室內(nèi)環(huán)境就會受到不同大小的外磁干擾。圖3是磁力計在零速時刻的模值，將動態(tài)時刻磁力計置為0。在15 s～17 s，84 s～86 s等區(qū)間檢測到磁力計受到外磁干擾，大幅度超出穩(wěn)定時的磁力計測量模值，如圓圈所示。此時使用陀螺儀航向角來代替地磁航向角，以免解算結(jié)果漂移。

圖3 磁力計在零速時刻的模值變化

3.4 利用氣壓計修正高程誤差

由于解算軌跡在高度方向難免會產(chǎn)生發(fā)散問題，使得行人導航系統(tǒng)隨著時間而發(fā)散，除了零速修正之外，需要引入氣壓計作為高度觀測值來修正高程誤差。在0 m～20 000 m 之間有兩個高度層，即對流層和同溫層，對流層的海拔范圍為0 m～11 000 m，同溫層的海拔范圍為11 000 m～20 000 m。由于本文實驗場地約為5 m左右，屬于同溫層，標準氣壓高度公式為

(31)

式中：hbm為氣壓計測量高度，hs為捷聯(lián)慣導求得的高度信息，Ps為氣壓計測量值。

將上式整合到觀測矩陣里為

經(jīng)過氣壓計的高程修正，較好抑制住了高程誤差。

4 行人導航系統(tǒng)自適應(yīng)卡爾曼濾波模型

經(jīng)過上文的分析，將誤差模型、零速修正和航向修正公式整理到自適應(yīng)卡爾曼濾波中。當檢測到零速區(qū)間時，將這個區(qū)間的速度置零，利用速度誤差修正動態(tài)時刻速度，利用上一零速時刻航向角與下一動態(tài)時刻的航向角差值來修正航向角。狀態(tài)方程由誤差模型方程組成，分別是姿態(tài)誤差、速度誤差、位置誤差、陀螺偏移誤差、加速度偏移誤差，觀測模型由ZUPT、航向修正和氣壓計修正組成。使用自適應(yīng)濾波對系統(tǒng)和觀測噪聲進行估計和修正，增強行人定位算法的適應(yīng)性。

系統(tǒng)的狀態(tài)變量為

(32)

系統(tǒng)的觀測方程為

δZk=Hδxk+vk

(33)

其中，δZk是時間k的誤差測量值，H是測量矩陣，vk是測量噪聲。

觀測方程由3部分組成，第一部分是零速區(qū)間末的速度值，來修正動態(tài)時刻的速度；第二部分是上一零速時刻航向角與下一時刻動態(tài)時刻航向角的差值，作為觀測量來修正動態(tài)時刻的航向角；第三部分是利用氣壓計測量的高度來修正行人軌跡的高程誤差。

(1)將狀態(tài)方程和觀測方程離散化

δXk+1=AkδXk+WkδZk=HXk+Vk

(34)

狀態(tài)轉(zhuǎn)移矩陣

其中，τg和τa分別是陀螺儀和加速度計的相關(guān)時間，Δt代表采樣時間間隔，fn×是導航系中的各加速度構(gòu)成的偏斜對稱矩陣。當檢測到零速區(qū)間時，用零速區(qū)間上一時刻的速度減零即為速度誤差，然后將速度誤差輸入卡爾曼濾波器進行觀測更新,利用上一零速時刻航向角與下一動態(tài)時刻航向角的差值作為觀測量，來修正航向角。

對于基于ZUPT和航向修正的卡爾曼濾波器，測量矩陣H的表達式如下

(2)預測及觀測量

狀態(tài)單步預測為

(35)

單步預測誤差協(xié)方差為

(36)

(3)求速度、航向角和高程的零偏估計

狀態(tài)估計

(37)

誤差協(xié)方差更新

(38)

濾波增益更新

(39)

(4)計算加權(quán)系數(shù)并更新相關(guān)參數(shù)

(40)

(41)

在導航參數(shù)修正完成后，由于δφn、δVn和δPn不屬于零偏誤差，該誤差已經(jīng)在這次修正階段修正完成，所以要將這些參數(shù)置0。

5 實驗與結(jié)果分析

為了檢驗本文行人導航系統(tǒng)的準確性，驗證航向修正和高程修正算法的可行性，分別設(shè)計了室內(nèi)外3組行人軌跡實驗。

實驗一的場地在上海交通大學南體體育場，自西南角足球場角球位置為起點，逆時針方向，沿規(guī)則足球場邊線行走一圈。

實驗二的實驗場地在上海交通大學南體體育場，自西南角100 m跑道終點為起點，逆時針方向，沿400 m跑道第一賽道行走一圈。

實驗三的實驗場地在上海交通大學電子信息與電氣工程學院2號樓與3號樓之間的二樓走廊，順時針行走一圈。

5.1 實驗條件及數(shù)據(jù)采集

本文使用的傳感器數(shù)據(jù)均來源于華為Mate9，使用Java語言在Android Studio上開發(fā)傳感器數(shù)據(jù)采集app，將傳感器采集頻率參數(shù)設(shè)置為SENSOR_DELAY_FASTEST，頻率最高可到500 Hz。通過時間戳來控制采樣周期，每個采樣周期存儲一次傳感器數(shù)據(jù)從而精準控制采樣頻率。實驗中將手機固定在行人右腳腳踝，如圖4所示，實驗結(jié)束后點擊保存按鈕存儲各傳感器數(shù)據(jù)，最后將數(shù)據(jù)拷貝到PC端使用Matlab進行計算。

圖4 載體坐標系

采集數(shù)據(jù)的傳感器包括三軸陀螺儀、三軸加速度計、三軸磁力計和氣壓計，符合實驗的要求。表1是智能手機各傳感器技術(shù)指標。lsm6 dsm是三軸加速度計和三軸陀螺儀合二為一的傳感器，磁力計是akm09918，氣壓計是airpress_lps22bh。

表1 Mate9各傳感器指標

5.2 實驗結(jié)果和分析

實驗設(shè)計共分為兩類，分別是室外曲線和室內(nèi)曲線。室外實驗分別是直線閉合曲線和直線曲線混合曲線，設(shè)置直線路徑和曲線路徑對照，對比使用與未使用該航向修正算法之間的差別和關(guān)系。由于室外操場的實驗磁力計不易受到外磁干擾，可以不使用檢測外磁干擾模塊。

實驗一：圖5為操場的標準足球場邊線，長105 m寬68 m，總長346 m。從西南角角球位置開始(圖中圓點)逆時針沿著邊線行走一圈，只用ZUPT約束解算的定位誤差為34.5 m，誤差為8.62%,結(jié)合ZUPT與航向修正算法之后解算的定位誤差為2.64 m，誤差為0.76%。較好限制了航向角的漂移。利用氣壓計來修正高程誤差，將高程誤差限制在0.3%以內(nèi)。

圖5 標準346 m足球場邊線行走

實驗二：圖6為操場的400 m的跑道內(nèi)道，從西南角(圖中圓點)出發(fā)逆時針行走一圈，只用ZUPT約束解算的定位誤差為38.6 m，誤差為9.7%,結(jié)合ZUPT與該航向修正算法之后解算的定位誤差為2.9 m，誤差為0.72%。較好限制了航向角的漂移。利用氣壓計來修正高程誤差，將高程誤差限制在0.4%以內(nèi)。

圖6 操場400 m閉合路徑行走

實驗三：圖7為上海交通大學電子信息與電氣工程學院2號樓與3號樓之間的二樓辦公室過道和走廊，從2號樓西側(cè)樓梯口為起點順時針行走一圈，只用ZUPT約束解算的定位誤差為46.7 m，誤差為15.3%,結(jié)合ZUPT與該航向修正算法之后解算的定位誤差為14.4 m，誤差為4.8%，較好限制了航向角的漂移。但是還存在一些外磁干擾點，經(jīng)過外磁干擾檢測模塊后，對修正航向有一定的幫助，定位誤差為3.5 m，使定位誤差小于1.2%。利用氣壓計來修正高程誤差，將高程誤差限制在0.3%以內(nèi)。

圖7 室內(nèi)路徑行走

本文分別設(shè)置室內(nèi)外實驗，對僅使用ZUPT、ZUPT+航向修正算法、ZUPT+航向修正算法+磁異常檢測修正算法效果進行對比，如圖5～圖7所示。僅使用ZUPT缺少對陀螺儀的零偏估計，航向角會不斷發(fā)散，產(chǎn)生較大誤差。ZUPT結(jié)合航向修正算法可以對航向角誤差進行觀測，能有效改善解算結(jié)果。分別設(shè)置直線路徑與曲線路徑進行對比，驗證算法的適應(yīng)性。但是在室內(nèi)環(huán)境下地磁容易受到磁干擾，不去加以檢測和修正同樣會使航向角發(fā)散。在室內(nèi)環(huán)境下同時使用ZUPT、航向修正算法以及外磁干擾檢測修正算法，在室內(nèi)出現(xiàn)強磁干擾后會檢測并暫時放棄使用地磁航向角，以免使航向角發(fā)散。從結(jié)果來看，基本與實際路線吻合，能夠在磁干擾條件下表現(xiàn)穩(wěn)定。

同時，進行了多組實驗驗證，并將綜合誤差統(tǒng)計在表2中。根據(jù)數(shù)據(jù)顯示，僅使用ZUPT誤差最大，經(jīng)過航向修正算法后大大降低了室外的誤差，在室內(nèi)通過使用磁異常檢測修正后降低了室內(nèi)行走的誤差。

表2 實驗誤差

6 結(jié)束語

本文在零速修正和捷聯(lián)慣性導航算法的基礎(chǔ)上，根據(jù)行人的行走習慣：腳在零速時刻的航向和下一時刻踢出的方向是一致的，微慣性器件在靜態(tài)時解算出的誤差更小的性質(zhì)，提出了一種航向修正的方法，用上一零速時刻的航向角與下一動態(tài)時刻航向角的差值作為觀測來修正航向，擴充了一維觀測器。經(jīng)過對比實驗驗證，發(fā)現(xiàn)這種方法對航向抑制有效。針對室內(nèi)存在磁干擾的問題提出了磁干擾檢測的方法，較好修正了航向角。本文在一定程度上解決了微慣性陀螺在行人導航系統(tǒng)中航向漂移難以抑制的問題，擴展了適用場景，提高了行人定位精度。