漆鈺暉，郭　杭，鄧林坤

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

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

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

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

本文利用智能移動(dòng)設(shè)備的慣性測量單元和磁力計(jì)，通過基于UKF對PDR方法的兩大部分進(jìn)行誤差修正，步長利用ZUPT進(jìn)行估計(jì)計(jì)算，航向角利用ZARU與磁力計(jì)融合的方式進(jìn)行估算，從而提高行人位置精度。基本思路如圖1所示。

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

1　初始對準(zhǔn)

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

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

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

一般情況下，可認(rèn)為加速度計(jì)的輸出為

(1)

(2)

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

(3)

(4)

ψ=arctan(My/Mx)-Md

(5)

Md表示本地磁偏角。

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

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

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

(6)

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

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

Xk+1=f(Xk)+Wk

(7)

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

Zk=HXk+Vk

(8)

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

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

(9)

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

圖2　精對準(zhǔn)流程圖

2　移動(dòng)測定

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

k時(shí)刻加速度的振幅及其均值為

(10)

(11)

(12)

角速度的振幅及其均值為

(13)

(14)

(15)

(16)

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

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

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

3　無跡卡爾曼濾波

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

3.1　狀態(tài)傳遞

對于不同時(shí)刻k，由具有高斯白噪聲uk的隨機(jī)變量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ù)；設(shè)uk的協(xié)方差矩陣為Q；vk的協(xié)方差矩陣為R。

加速度計(jì)及陀螺儀真實(shí)輸出為

(19)

(20)

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

(21)

(22)

則方程(22)可表示為

(23)

3.2　無跡變換

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

計(jì)算(2n+1)個(gè)Sigma采樣點(diǎn)

(24)

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

(25)

(26)

(27)

采樣點(diǎn)相應(yīng)的權(quán)重值為

(28)

3.3　量測更新

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

(29)

(30)

(31)

卡爾曼增益矩陣為

(32)

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

(33)

(34)

4　ZUPT、ZARU與磁力計(jì)融合

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

ZUPT的量測值及相應(yīng)的量測矩陣為

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

(35)

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

(36)

ZARU的量測值及相應(yīng)的量測矩陣為

(37)

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

(38)

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

(39)

(40)

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

(41)

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

磁力計(jì)的量測值及相應(yīng)的量測矩陣為

(42)

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

(43)

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

(44)

(45)

5　試驗(yàn)分析

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

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

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

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

定位誤差用試驗(yàn)終點(diǎn)和真實(shí)終點(diǎn)距離的差值與總路程的比值來表示。兩種不同方法的對比見表1，一共進(jìn)行了5次試驗(yàn)，時(shí)間包含初始對準(zhǔn)4 min。

表1　定位誤差分布

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

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

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

6　結(jié)　論

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

參考文獻(xiàn)：

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

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

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

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

[5] 蘇宛新.自適應(yīng)UKF濾波在SINS初始對準(zhǔn)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào),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ù)學(xué)報(bào)，2011，24(5):642 -646.

[11]陳國良,張言哲,汪云甲,等.WiFi-PDR室內(nèi)組合定位的無跡卡爾曼濾波算法[J].測繪學(xué)報(bào),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.