多傳感器行人航位推算方法和UKF融合算法

漆鈺暉，郭　杭，鄧林坤

(南昌大學信息工程學院，江西 南昌 330031)

隨著基于位置服務的快速興起，準確的位置信息顯得尤為重要。目前，在室外空曠的環(huán)境，依靠GPS、北斗等系統(tǒng)基本可以滿足定位需求；而在室內(nèi)或無衛(wèi)星信號的情況下，則需要利用多種傳感器來輔助行人確定位置，如慣性測量單元、無線網(wǎng)絡傳感器和射頻識別單元等。

隨著慣性測量單元(IMU)硬件水平的不斷提高，基于慣性測量單元的室內(nèi)行人航位推算(PDR)越來越受到重視，通過加速度計和陀螺儀組合磁力計、氣壓計等來提高位置精度。

PDR算法可以分解為航向估算與步長估算兩部分，通過航向估算判定行人行走的方向，通過步長估算在其行走方向的投影，可以獲得行人在二維坐標系下的位置。田國會等[1]提出一種基于小波變換與UKF結(jié)合的濾波方法，利用小波對觀測信號進行處理，降低了傳統(tǒng)UKF由于噪聲而導致濾波偏差的影響，從而提高其定位精度；但其步伐狀態(tài)測定僅利用加速度檢測，倘若受到外界因素干擾，其測定結(jié)果必定產(chǎn)生較大偏差；李金鳳等[2]采用互補濾波融合多傳感器數(shù)據(jù)，修正航向角誤差，獲得了一定的精度，但其步長估計采用簡單的步長模型，倘若行人在行走過程中步頻改變，步長測量將會產(chǎn)生較大誤差，最終將導致定位精度的下降。

本文利用智能移動設備的慣性測量單元和磁力計，通過基于UKF對PDR方法的兩大部分進行誤差修正，步長利用ZUPT進行估計計算，航向角利用ZARU與磁力計融合的方式進行估算，從而提高行人位置精度?；舅悸啡鐖D1所示。

圖1　多傳感器行人航位推算方法框架

1　初始對準

在進行捷聯(lián)導航解算前，慣性導航系統(tǒng)必須要進行初始對準，從而確定初始姿態(tài)等信息，它的結(jié)果將直接影響到慣性導航系統(tǒng)的導航性能。其中，依照運動載體的狀態(tài)，又可以分為靜基座對準和動基座對準兩種，而且都包含粗對準和精對準兩個階段。

粗對準一般利用加速度計來獲取載體的俯仰角和橫滾角，磁力計獲取其航向角；而精對準一般需在粗對準的基礎上使用濾波等算法，處理各傳感器的誤差信息，并且精確估計與真實坐標系的偏差，從而獲得更準確的姿態(tài)及相關(guān)矩陣[3]。本系統(tǒng)采用的是靜基座對準方法，導航坐標系為北西天坐標系。

由于UKF算法濾波效果受濾波初值影響較大，故其初始對準非常重要。

一般情況下，可認為加速度計的輸出為

(1)

(2)

由于慣性測量單元中，低成本陀螺儀性能的限制，計算航向角會產(chǎn)生很大偏差。因此粗對準一般采用磁力計代替陀螺儀獲取相應的磁感應電勢來計算航向角。

(3)

(4)

ψ=arctan(My/Mx)-Md

(5)

Md表示本地磁偏角。

在靜止4 min的情況下，利用加速度計和磁力計的值，計算初始姿態(tài)與加速度計和陀螺儀的初始姿態(tài)誤差。

在此階段，可將UKF的誤差狀態(tài)向量設為[4-5]

X=[δθ,δφ,δψ,δωx,δ,δrU,δvN,δvW,δvU,δax,δay,δaz]T

(6)

式中，導航坐標系為n；δθ、δφ和δψ分別表示姿態(tài)角誤差；δωx、δωy和δωz分別表示在b系下陀螺儀誤差，δrN、δrW和δrU分別表示在n系下北西天方向上的位置誤差；δvN、δvW和δvU分別表示在n系下北西天方向上的速度誤差；δax、δay和δaz分別表示在b系下的加速度誤差。

由于UKF使用的是離散時間上的非線性模型，需要對系統(tǒng)模型進行離散化處理，系統(tǒng)離散化狀態(tài)方程為

Xk+1=f(Xk)+Wk

(7)

系統(tǒng)離散化量測方程為

Zk=HXk+Vk

(8)

式中，k表示某一時刻；Vk和Wk分別表示量測噪聲矩陣和系統(tǒng)噪聲矩陣，而且它們?yōu)榱憔档陌自肼?，以速度、位置、航向角和角速度為量測值，則相應的量測矩陣為

H=[010×2I10×10010×3]

(9)

利用上述模型及UKF的基本方程對該系統(tǒng)初始狀態(tài)進行估計，利用閉環(huán)形式對系統(tǒng)進行誤差補償，實現(xiàn)精確而快速的初始對準，基本流程如圖2所示。

圖2　精對準流程圖

2　移動測定

行人在行走過程中，其步伐需經(jīng)歷多種狀態(tài)的周期性變化；本試驗采用手托Ipad方式行走采集數(shù)據(jù)，各個傳感器的3個軸均表現(xiàn)周期性的特征。目前大部分試驗都是采用加速度計[6]或陀螺儀[7]來檢測的，本文采用加速度計和陀螺儀聯(lián)合檢測[8]的方式來測定行人行走。

k時刻加速度的振幅及其均值為

(10)

(11)

(12)

角速度的振幅及其均值為

(13)

(14)

(15)

(16)

根據(jù)以上判定條件，行人行走是步伐狀態(tài)如圖3所示。

圖3　步伐狀態(tài)檢測

為了清晰可見，本文僅截取了一段步態(tài)檢測數(shù)據(jù)進行描述與分析，圖中Y軸僅表示數(shù)值的大小，沒有固定的意義，X軸為傳感器采樣樣本數(shù)。由圖3可知，行人行走時步伐狀態(tài)呈現(xiàn)周期性變化，當步伐狀態(tài)為移動時，表明行人處于移動狀態(tài)，相應的靜止則表明行人處于靜止狀態(tài)。

3　無跡卡爾曼濾波

無跡卡爾曼濾波作為一種典型的非線性估計的方法，其核心是利用無跡變換變化處理Sigma點集，使Sigma點集的均值、協(xié)方差與原系統(tǒng)統(tǒng)計特性匹配，再利用Sigma點集進行非線性映射，得到近似化的狀態(tài)概率密度函數(shù)；無跡卡爾曼濾波使用Sigma點計算不同狀態(tài)的更新和量測值的影響[9-10]。

3.1　狀態(tài)傳遞

對于不同時刻k，由具有高斯白噪聲uk的隨機變量X和具有高斯白噪聲vk的觀測變量Z構(gòu)成的非線性系統(tǒng)為

Xk=f(Xk-1,uk)

(17)

Zk=h(Zk-1,vk)

(18)

式中，f為非線性狀態(tài)方程函數(shù)；h為非線性觀測方程函數(shù)；設uk的協(xié)方差矩陣為Q；vk的協(xié)方差矩陣為R。

加速度計及陀螺儀真實輸出為

(19)

(20)

式中，Va和Vω分別代表加速度計和陀螺儀的偏差量。

(21)

(22)

則方程(22)可表示為

(23)

3.2　無跡變換

無跡變換可以用來處理均值和協(xié)方差的非線性傳遞問題，其實現(xiàn)方法為：按一定規(guī)則選取一些采樣點，將這些點代入非線性函數(shù)且得到相應的點集，通過這些點集求取變換后的矩陣和協(xié)方差[11]。采樣點的選取是基于先驗均值和先驗協(xié)方差矩陣的平方根的相關(guān)序列。

計算(2n+1)個Sigma采樣點

(24)

Xk,i=f(Xk-1,i,uk)

(25)

(26)

(27)

采樣點相應的權(quán)重值為

(28)

3.3　量測更新

根據(jù)式(18)得到Sigma點集的觀測預測值，通過加權(quán)求和可得到系統(tǒng)的預測均值和協(xié)方差為

(29)

(30)

(31)

卡爾曼增益矩陣為

(32)

狀態(tài)更新及協(xié)方差更新為

(33)

(34)

4　ZUPT、ZARU與磁力計融合

行人在室內(nèi)行走時，ZUPT和ZARU都是依據(jù)步伐處于靜止狀態(tài)時觸發(fā)，即當行人步伐處于靜止狀態(tài)，理論上該時刻的速度和角速度應該為零，但是由于慣性傳感器的電子噪聲、量測誤差等影響，實際上由加速度計計算得到的速度和陀螺儀獲取的角速度并不為零。因此，可以將該時刻速度和角速度作為UKF的量測值進行誤差修正[12]。

ZUPT的量測值及相應的量測矩陣為

Zk=Δvk=vk,k-1-[0,0,0]

(35)

H=[03×3,03×3,03×3,I3×3,03×3]

(36)

ZARU的量測值及相應的量測矩陣為

(37)

H=[03×3,I3×3,03×3,03×3,03×3]

(38)

由于行人在室內(nèi)環(huán)境下，極易受到人工設施的干擾而導致磁極偏離地磁場，從而使磁場感應電勢產(chǎn)生很大的偏差。

(39)

(40)

Δψk=ψk-ψmag(k)

(41)

式中，ψk為由修正后k時刻的姿態(tài)矩陣求得；ψmag(k)為由磁力計計算得到；Δψk為航向角偏差。

磁力計的量測值及相應的量測矩陣為

(42)

H=[[001],01×3,01×3,01×3,01×3]

(43)

單純的利用磁力計或陀螺儀都很難獲得較高精度的方向信息，針對這種問題本文建立了方向估計模型，將每一時刻的感應電勢由載體坐標系轉(zhuǎn)換為導航坐標系。利用ZUPT、ZARU與磁力計融合，構(gòu)建相應的航向角誤差模型。根據(jù)式(36)、式(38)和式(43)可以得到融合后相應的量測值和量測矩陣為

(44)

(45)

5　試驗分析

本文提出了一種基于UKF低成本多傳感器融合行人航位推算方法，試驗設備采用蘋果公司iPad air 2，此設備含有IMU、磁力計和氣壓計等傳感器。通過SensorLog軟件獲取行人室內(nèi)行走的數(shù)據(jù)，試驗場地為長方形會議廳，設置采樣頻率為100 Hz，總測試路線長度為50.4 m，圖4為使用Matlab軟件仿真某一次試驗方法對比圖。

為了驗證本文方法的優(yōu)越性，同時引入了基于擴展卡爾曼濾波(EKF)的多傳感器組合行人航位推算方法進行對比試驗，從圖4中可以看出，基于EKF利用ZUPT與磁力計融合的方法，在航向角方面，一開始直線行走行人航向角與實際偏差不大，但當經(jīng)過第一個直角轉(zhuǎn)彎時，由于設備傳感器反應不靈敏且精度低，利用陀螺儀與磁力計組合計算航向角也會產(chǎn)生很大誤差，而且基于EKF的方法將非線性問題線性化不可避免地產(chǎn)生線性誤差，即一遇到轉(zhuǎn)彎，該方法的航向角出現(xiàn)明顯的波動；同樣基于EKF的ZUPT未能有效地對速度誤差修正，造成相鄰步伐之間距離差別較大。

采用本文基于UKF利用ZUPT、ZARU與磁力計融合的方法，針對上述基于EKF和低成本傳感器等問題，利用基于UKF的ZARU與磁力計融合部分，由步伐為靜止狀態(tài)觸發(fā)角速度修正并融合磁力計對航向角偏差進行修正，有效地校正了行人轉(zhuǎn)彎時航向角誤差較大的問題；在速度誤差方面，基于UKF的ZUPT部分同樣在步伐處于靜止狀態(tài)觸發(fā)速度修正，以貝葉斯理論和無跡變換為基礎，有效地削弱了傳感器誤差累積，修正了行人行走時線性速度及位置漂移。從圖4可以看出，基于UKF的方法在方向和相鄰步伐的距離方面，兩者表現(xiàn)得都非常平穩(wěn)。

圖4　試驗測試對比結(jié)果

定位誤差用試驗終點和真實終點距離的差值與總路程的比值來表示。兩種不同方法的對比見表1，一共進行了5次試驗，時間包含初始對準4 min。

表1　定位誤差分布

從表1中結(jié)果對比可知，基于UKF的ZUPT、ZARU與磁力計融合的方法，利用ZARU和磁力計構(gòu)建修正航向角誤差模型，有效地修正了航向角誤差；利用ZUPT對速度誤差修正，降低加速度計偏差及漂移，從而減小了相鄰步伐間的誤差，結(jié)合上述修正后的數(shù)據(jù)，最終提高室內(nèi)行人位置精度。

由于本試驗設備反應不靈敏且精度低等問題，慣性測量單元中的陀螺儀精度較低，而且磁力計易受外界干擾。故本試驗使用智能移動設備的多傳感器，采用基于UKF的ZUPT、ZARU與磁力計組合的方法同樣達到了較好的精度。

上述試驗結(jié)果表明：基于UKF的ZUPT、ZARU與磁力計組合的方法，使用低成本多傳感器進行室內(nèi)行人定位，同樣可以獲得較高的定位精度，驗證了該方法的實用性和通用性。

6　結(jié)　論

本試驗提出一種基于智能移動設備的多傳感器行人航位推算方法，該方法不僅可以實現(xiàn)在室內(nèi)連續(xù)自主的定位，而且通過引入ZARU與磁力計融合構(gòu)建航向角估計模型，有效地修正行人行走時航向角的偏差，ZUPT對速度誤差的修正減少了加速度計偏差及漂移，有效地修正了行人步伐的誤差。與目前常用的行人航位推算系統(tǒng)比較，該系統(tǒng)使用低成本的傳感器，獲得了相當?shù)亩ㄎ痪?。在一定時間內(nèi)，該系統(tǒng)可以滿足室內(nèi)行人的定位需求。但是，由于試驗設備傳感器性能比較差，造成采集到的數(shù)據(jù)包含了許多其他誤差與噪聲，從而導致了時間越長，位置誤差越大的結(jié)果。下一步需要以與視覺定位結(jié)合的方式，實現(xiàn)在室內(nèi)高精度、長時間、長距離和實時定位的PDR系統(tǒng)。

參考文獻：

[1] 田國會,張慶賓,丁娜娜.基于WT-UKF的PDR/GPS組合定位算法[J].控制與決策,2015，30(1):86-90.

[2] 李金鳳,王慶輝,劉曉梅,等.基于MEMS慣性傳感器的行人航位推算系統(tǒng)[J].傳感器與微系統(tǒng),2014,33 (12):85-90.

[3] 馬宏陽,程鵬飛,王潛心.一種改進的UKF算法在捷聯(lián)慣導初始對準中的應用[J].測繪通報,2015(7):18-22.

[4] 門愛東,姜竹青,王宇鵬,等.雙重自適應UKF在SINS初始對準中的應用[J].北京郵電大學學報,2014,37(1):11-15.

[5] 蘇宛新.自適應UKF濾波在SINS初始對準中的應用[J].中國慣性技術(shù)學報,2011,19(5):533-536.

[6] JIMENEZ A R，SECO F，PRIETO C，et al.A Comparison of Pedestrian Dead-Reckoning Algorithms Using a Low-cost MEMS IMU [C]∥The 6th IEEE International Symposium on Intelligent Signal Processing.Budapest：IEEE，2009:37-42.

[7] RAUL F，EDUARDO Z，JAIME G.Pedestrian Tracking Using Inertial Sensors[J].Journal of Physical Agents,2009,3(1):35-42.

[8] ZHENG X，YANG H，TANG W，et al.Indoor Pedestrian Navigation with Shoe-mounted Inertial Sensors[C]∥Multimedia and Ubiquitous Engineering.Berlin：Springer，2014:67-73.

[9] FRANCISCO Z，MOHAMMED K，PATRICK R，et al.Unscented Kalman Filter and Magnetic Angular Rate Update (MARU) for an Improved Pedestrian Dead-reckoning [C]∥Position Location and Navigation Symposium.[S.l.]：IEEE,2012:129-139.

[10]趙鶴，王喆垚.基于UKF的MEMS傳感器姿態(tài)測量系統(tǒng)[J].傳感技術(shù)學報，2011，24(5):642 -646.

[11]陳國良,張言哲,汪云甲,等.WiFi-PDR室內(nèi)組合定位的無跡卡爾曼濾波算法[J].測繪學報,2015,44(12):1314-1321.

[12]BORENSTEIN J，OJEDA L，KWANMUANG S.Heuristic Reduction of Gyro Drift in IMU-based Personnel Tracking Systems[J].Journal of Navigation，2009，62(1):41-58.

[13]JIMENEZ A R，SECO F，PRIETO J C，et al.Indoor Pedestrian Navigation Using an INS/EKF Framework for Yaw Drift Reduction and a Foot-mounted IMU [C]∥Proc.of 7th Workshop on Positioning，Navigation and Communication (WPNC’10).Dresden：[s.n.]，2010:1-9.

[14]WEINBERG H.Using the ADXL202 in Pedometer and Personal Navigation Applications[EB/OL].[2017-01-02].www.BDTIC.com/ADI.

[15]JIMENEZ A R，SECO F，PRIETO C，et al.A Comparison of Pedestrian Dead-reckoning Algorithms Using a Low-cost MEMS IMU [C]∥The 6th IEEE International Symposium on Intelligent Signal Processing.Budapest：IEEE，2009:37-42.