室內(nèi)行人航跡推算/超聲波組合定位融合算法

趙延鵬，時偉，艾明曦

（中南大學 航空航天學院，湖南 長沙，410083）

室內(nèi)行人航跡推算/超聲波組合定位融合算法

趙延鵬，時偉，艾明曦

（中南大學 航空航天學院，湖南 長沙，410083）

為提高室內(nèi)定位系統(tǒng)精度和跟蹤性能以及適應復雜環(huán)境，將行人航跡推算與超聲波定位組合，提出基于平方根無跡卡曼濾波的噪聲權(quán)因子輔助協(xié)方差加權(quán)融合算法，并將全局最優(yōu)融合狀態(tài)作為反饋量引入算法。針對超聲波對行人航向角測量困難，采用一種簡單有效的幾何方法。仿真結(jié)果表明：在模擬的室內(nèi)動態(tài)環(huán)境中，包括在多路徑效應和慣性累積誤差的影響下，融合算法始終比單模型定位精度高，并有很好的收斂性、穩(wěn)定性與適應性，對室內(nèi)定位技術(shù)研究與應用具有重要意義。

室內(nèi)定位；平方根無跡卡曼濾波；噪聲權(quán)因子；協(xié)方差加權(quán)融合；航向角測量

近年來，人們對室內(nèi)定位的需求日益增加，例如在城市高樓群、購物商場、火災現(xiàn)場、醫(yī)院病人監(jiān)控等都需要基于個人位置的服務。因此，實現(xiàn)低成本且高精度的室內(nèi)定位具有非常重要的現(xiàn)實意義。在空曠的室外地區(qū)，衛(wèi)星導航定位系統(tǒng)可為用戶提供可靠的位置信息；在復雜的室內(nèi)環(huán)境，衛(wèi)星信號因無法很好地穿透建筑物，不能提供準確的位置信息。為了彌補這一定位盲區(qū)，多種室內(nèi)定位技術(shù)相繼被提出，主要包括：行人基于自包含傳感器的定位技術(shù)（傳統(tǒng)慣性導航機制、行人航跡推算）、AGPS（Assistant-GPS）定位技術(shù)、無線定位技術(shù)（超聲波、紅外線、WLAN信號、射頻無線標簽、UWB等）、其他定位技術(shù)（麥克風陣列、電力線室內(nèi)定位、地球磁場、計算機視覺識別等）［1-3］?；谛腥撕桔E推算（pedestrian dead reckon，PDR）的定位技術(shù)具有導航定位自主性和連續(xù)性優(yōu)勢，但隨著時間有累積誤差缺陷；超聲波定位具有結(jié)構(gòu)簡單、適應能力強、定位精度高等優(yōu)勢，但多路徑效應較強［3-4］。為實現(xiàn)高精度、可靠的室內(nèi)定位系統(tǒng)，本文作者把以上2種獨立定位系統(tǒng)組合，利用多種信息源，互相補充，構(gòu)成一種有多余度和精度更高的多功能PDR/超聲波組合定位系統(tǒng)。融合算法作為組合導航定位的關鍵技術(shù)，已成為當前研究熱點。目前，國內(nèi)外學者對衛(wèi)星導航和慣性導航系統(tǒng)的融合算法做了大量研究，而針對室內(nèi)組合定位系統(tǒng)的融合算法所做的相關研究較少?；?Kalman濾波的信息融合技術(shù)是當前研究的主要方向，本文室內(nèi)組合定位系統(tǒng)狀態(tài)方程為非線性，這給濾波器的設計帶來了一定的困難。針對該問題，部分學者在工作點處做線性化后，采用擴展卡爾曼濾波（簡稱KF）進行狀態(tài)估計，EKF的線性化誤差會降低模型的準確性，隨著時間延長，估計精度難以保證。另外EKF在濾波前必須計算非線性模型的Jacobian矩陣，這一過程非常繁瑣且極容易出錯［5］。自無跡卡曼濾波（簡稱UKF）被提出來后，其應用領域不斷擴展，UKF與EKF相比，能更準確地估計狀態(tài)及其方差，無需對非線性狀態(tài)函數(shù)和觀測函數(shù)求導，具有收斂速度快、估計精度高和易于實現(xiàn)的優(yōu)點［6-7］。但是 UKF會由于計算誤差和噪聲信號等因素引起誤差協(xié)方差矩陣負定，從而導致濾波發(fā)散。平方根UKF（簡稱SRUKF）算法以估計誤差的平方根矩陣進行迭代計算，解決了常規(guī)UKF算法中濾波結(jié)果發(fā)散問題，提高了濾波精度和穩(wěn)定性［8］。本文作者將SRUKF算法與協(xié)方差加權(quán)融合相結(jié)合應用于PDR/超聲波室內(nèi)組合定位系統(tǒng)，根據(jù)2種模型的系統(tǒng)噪聲設置噪聲權(quán)因子輔助協(xié)方差加權(quán)，有效地解決了針對室內(nèi)動態(tài)環(huán)境的實時融合。同時，為了提高系統(tǒng)的跟蹤性與穩(wěn)定性，算法引入反饋機制。通過仿真實驗，驗證算法的融合效果，并對其進行分析比較。

1 室內(nèi)定位方案

對于室內(nèi)行人定位來說，正常情況下，行人是平穩(wěn)地行走且高度方向上變化較小，因此被認為滾轉(zhuǎn)角和俯仰角接近零，忽略高度通道。本文僅考慮行人航向角φ（本體系X軸與正北方向夾角，逆時針為正）、平面二維位置分量與速度分量。

1.1 行人航跡推算

目前，基于傳統(tǒng)慣性導航機制和基于行人航跡推算的定位技術(shù)取得了一定成果［4，9-13］。

行人航跡推算在已知當前時刻位置條件下，利用慣性測量單元（簡稱 IMU）推算出下一時刻的行人位置，具有完全自主性，不受外界影響，數(shù)據(jù)更新速度快等特點。航跡推算算法的定位精度取決于初始位置和姿態(tài)信息的精確性以及推算過程中速度的航向信息求解。初始位置在實際應用中一般通過在室外由GPS等導航衛(wèi)星定位獲得，在本文仿真研究中將初始位置設置為坐標原點。姿態(tài)信息僅包括航向角 φ，可由陀螺儀與磁羅盤獲得數(shù)據(jù)，用零角速率更新（簡稱ZARU）、濾波融合等算法解算獲得。速度信息是行人航跡推算中主要研究熱點，主要包括步態(tài)檢測即人體步長估計模型以及步頻檢測。為了估計步長 L，目前存在的模型主要包括：常數(shù)/偽常數(shù)步長模型、線性步長模型、非線性步長模型和人工智能步長模型。最新研究表明加速度計或其他IMU固定在人體腳踝部位，步頻探測效果最佳，利用零速更新策略（簡稱 ZUPT）估計誤差并進行補償獲得步頻f信息，降低位置發(fā)散速度，從而得到1個周期T內(nèi)行人步數(shù)C=1/f。得到以上參數(shù)后通過航跡推算算法。

航跡長度為

得到當前時刻位置信息為

行人航跡推算模型圖如圖1所示。

圖1 行人航跡推算模型圖Fig.1 Model of pedestrian dead reckon

1.2 超聲波傳感器定位

超聲波作為一種非接觸的檢測方式，與紅外、激光及無線電測距相比，不受光線、被測對象顏色等影響，在惡劣環(huán)境中具有一定的適應能力，比激光測距更容易獲得近距離的信息，并且具有結(jié)構(gòu)簡單，成本低廉，精度高等優(yōu)點。超聲波測距原理是利用渡越時間法（time of flight，TOF），即檢測超聲波往返的時間，所用時間與超聲波通過距離成正比獲得超聲波所經(jīng)過的距離，常常用于導航定位［3，14］。

室內(nèi)超聲波定位系統(tǒng)中超聲波傳感器網(wǎng)絡布置為：在穿戴于行人后背的定位裝置上安裝2個超聲波發(fā)射器Ut1和Ut2，在天花板上按簡單行列規(guī)律固定安裝若干個全向超聲波接收器Ur1～Urn。

若有n個超聲波接收器接收到Ut1所發(fā)射的超聲波，由極大似然估計可構(gòu)成如下方程組：

將上式方程組從第一個方程組開始依次與最后一個方程相減得

圖2 穿戴式設備超聲波定位模型圖Fig.2 Model of ultrasonic position with wearable device

1.3 建立模型集

其中Q和R兩者不相關。

2 數(shù)據(jù)融合定位

2.1 SRUKF濾波算法

2.1.1 平方根U變換

U變換的主要思想是“近似概率分布要比近似線性函數(shù)更容易”，它通過設置 Sigma采樣點和相應權(quán)值，逼近樣本非線性變換參量的一階矩和二階矩?？紤]1個L維隨機變量x經(jīng)過非線性變換后得到y(tǒng)=g（x），并假設x的均值和方差分別為和Px， x的Sigma樣點｛χi｝和權(quán)值｛Wi｝可由和 Px得到。利用這些采樣點通過非線性變換得到新的Sigma采樣點集合［15］。

計算變量y的均值和方差為

P通常為非負定矩陣，然而在濾波的迭代過程中，由于計算誤差等因素的影響，致使濾波協(xié)方差不對稱或負定，從而導致濾波發(fā)散，影響濾波算法的收斂性和穩(wěn)定性。因此，將Py分解為

可以通過 Cholesky分解來得到Py的平方根矩陣S，而它在濾波更新算法中的導出可通過QR分解來實現(xiàn)。通過設定縮放參數(shù)，保證粒子權(quán)值為正，可得

其中：qr表示得到QR分解中的R矩陣的函數(shù)。通過使用QR 分解，可以不必先求出樣點的加權(quán)方差，再進行Cholesky分解，即保證了數(shù)值的穩(wěn)定性，又減小了計算量。

在濾波過程中，用 Sy代替 Py參加遞推運算可以保證協(xié)方差陣的非負定性，從而實現(xiàn)有效濾波。

2.1.2 算法步驟

UKF會由于計算誤差和噪聲信號等因素引起誤差協(xié)方差矩陣負定，導致濾波結(jié)果發(fā)散。SRUKF算法以估計誤差的平方根矩陣進行迭代計算，解決了常規(guī)UKF算法中濾波結(jié)果發(fā)散的問題，提高了濾波的精度和穩(wěn)定性［5，16-17］。SRUKF濾波算法具體流程如下：

1） 初始化：

2） 計算2n+1個Sigma點和時間更新：

對每個Sigma點進行f（?）非線性變換：

對變換后的Sigma點集進行加權(quán)處理，從而得到一步預測狀態(tài)：

一步預測方差陣為

3） 測量更新：

預測觀測值為

計算協(xié)方差為

計算濾波增益為

狀態(tài)更新后的濾波值為

狀態(tài)后驗方差陣為

權(quán)重Wi為

2.2 融合算法

在室內(nèi)組合導航系統(tǒng)中，考慮到由于室內(nèi)復雜未知環(huán)境的影響，比如室內(nèi)物品遮擋造成的多路徑效應、慣性器件的累積誤差，造成個別模型或傳感器受到干擾或失去對行人目標的覆蓋。然而，本文研究方案中 PDR/超聲波組合定位使兩模型同時工作，如上文介紹 PDR和超聲波定位方案可獨立的獲得定位信息，系統(tǒng)充分利用各傳感器的測量值，通過數(shù)據(jù)融合進一步提高定位質(zhì)量，能得到足夠的目標信息，從而使系統(tǒng)的可靠性增強，信息置信度提高，時空覆蓋區(qū)域擴大，減少了測量數(shù)據(jù)的模糊性，改進了系統(tǒng)的監(jiān)測性能［18-19］。

針對室內(nèi)動態(tài)環(huán)境，通過噪聲權(quán)因子σ和μ輔助協(xié)方差加權(quán)，以滿足系統(tǒng)的實時融合效果。同時，為提高系統(tǒng)的跟蹤性與穩(wěn)定性，將上一時刻的全局最優(yōu)估計融合狀態(tài)作為反饋量引入算法。

基于以上分析，本文提出了基于SRUKF的噪聲權(quán)因子輔助協(xié)方差加權(quán)融合算法，原理如圖3所示。

圖3 基于SRUKF的噪聲權(quán)因子輔助協(xié)方差加權(quán)融合算法框圖Fig.3 Diagram of noise power factors assisting covariance weighted based on SRUKF

下面介紹噪聲權(quán)因子輔助協(xié)方差加權(quán)融合算法具體步驟與策略：

2組定位模型k時刻最優(yōu)航跡狀態(tài)估計值之差為

則d12，k的協(xié)方差矩陣為

當采用SRUKF濾波器對定位模型進行狀態(tài)估計時，互協(xié)方差S，12k可由下式求出：

其中：Kjk，（j=1， 2）為SRUKF濾波器增益；Φ為狀態(tài)轉(zhuǎn)移矩陣；Q為系統(tǒng)過程噪聲方差矩陣；H為觀測矩陣；σ和μ為噪聲權(quán)因子。

融合系統(tǒng)最優(yōu)狀態(tài)估計為

融合系統(tǒng)誤差協(xié)方差為

通過上述分析，可得到噪聲權(quán)因子輔助協(xié)方差加權(quán)融合算法策略如圖4所示。

圖4 噪聲權(quán)因子輔助協(xié)方差加權(quán)融合算法策略Fig.4 strategy of noise power factors assisting covariance weighted

3 仿真實驗與分析

為計算方便，本文統(tǒng)一將定位參數(shù)的采樣周期 T設置為1 s，取α=0.001，κ=0，β=2。為了保證組合定位系統(tǒng)融合精度并控制濾波計算量，仿真時長為 120 s，采用Monte Carlo作M=600次仿真，同時為了驗證算法的有效性，使用均方根誤差（RMSE）作為行人跟蹤性能指標。

仿真環(huán)境：行人初始位置為原點（單位： m），初速度為1 m/s，做近似勻速運動，初始航向角為20°。PDR定位系統(tǒng)采用荷蘭Xsens公司生產(chǎn)的MTi微慣性航姿系統(tǒng)，其性能指標［22］見表 1。通過在地面上選取若干點，測量其至參考點的實際距離，再從定位系統(tǒng)中提取用超聲波測量的距離，兩者比較可得超聲波定位系統(tǒng)的誤差指標見表 1。系統(tǒng)所有誤差均視為零均值高斯白噪聲。

表1 PDR/超聲波組合定位系統(tǒng)仿真參數(shù)設置Table 1 Setting simulation parameters of PDR/Ultrasonic

圖5 融合算法與單模型的位置誤差均方根Fig.5 RMSE of location with fusion algorithm and single model

圖6 融合算法與單模型的速度誤差均方根Fig.6 RMSE of velocity with fusion algorithm and single model

圖7 融合算法與單模型的航向角誤差均方根Fig.7 RMSE of course angle with fusion algorithm and single model

表2 無累積誤差以及多路徑環(huán)境下的誤差均方根Table 2 RMSE without cumulative error and multipath effect

在 PDR與超聲波定位系統(tǒng)各傳感器測量精度一定的情況下，從仿真結(jié)果來看，當慣性累積誤差以及多路徑效應不明顯時，PDR與超聲波單模型定位方案均具有較高的定位精度且誤差收斂在一定范圍內(nèi)，融合算法的定位性能高于單模型，與PDR和超聲波定位相比，位置（x，y）定位精度分別提高27.77%和30.85%，精度＜0.15 m；速度v精度分別提高39.88%和12.57%，精度＜0.12 m/s；航向角 φ精度分別提高 26.86%和10.63%，精度＜0.11°。

融合效果與實驗1中誤差均方根相比較，結(jié)果見表4。

圖8 融合算法與單模型的位置誤差均方根Fig.8 RMSE of location with fusion algorithm and single model

圖9 融合算法與單模型的速度誤差均方根Fig.9 RMSE of velocity with fusion algorithm and single model

圖10 融合算法與單模型的航向角誤差均方根Fig.10 RMSE of course angle with fusion algorithm and single model

表3 無累積誤差以及有多路徑環(huán)境下的誤差均方根Table 3 RMSE without cumulative error but multipath effect

表4 實驗1與實驗2誤差均方根對比Table 4 RMSE of experiment 1 and experiment 2

在 PDR與超聲波定位系統(tǒng)各傳感器測量精度一定的情況下，從仿真結(jié)果來看，PDR、超聲波單模型方案定位誤差均收斂在一定范圍內(nèi)，由于多路徑效應影響了超聲波定位，使得PDR定位精度高于超聲波定位。融合算法的定位性能明顯高于單模型，與PDR、超聲波定位相比，位置（x， y）定位精度分別提升13.35%和38.77%；速度v精度分別提升15.36%和28.04%；航向角φ精度分別提升27.07%和27.13%。如表4所示，盡管室內(nèi)多路徑效應明顯，整體融合效果相比于實驗1略有下降，但定位參數(shù)精度仍較高，定位精度＜0.18 m，速度v精度＜0.16 m/s，航向角φ精度＜0.28°。

圖11 融合算法與單模型的位置誤差均方根Fig.11 RMSE of location with fusion algorithm and single model

圖12 融合算法與單模型的速度誤差均方根Fig.12 RMSE of velocity with fusion algorithm and single model

圖13 融合算法與單模型的航向角誤差均方根Fig.13 RMSE of course angle with fusion algorithm and single model

表5 有累積誤差以及無多路徑環(huán)境下的誤差均方根Table 5 RMSE with cumulative error but multipath effect

融合效果與實驗1中誤差均方根相比較，結(jié)果見表6。

在 PDR與超聲波定位系統(tǒng)各傳感器測量精度一定的情況下，從仿真結(jié)果來看，PDR與超聲波單模型方案定位誤差均收斂在一定范圍內(nèi)，由于慣性器件的累積誤差影響了PDR定位，使得超聲波定位精度高于PDR。融合算法的定位性能明顯高于單模型，與PDR、超聲波定位相比，位置（x， y）定位精度分別提高37.56%和16.10%；速度v精度分別提高41.00%和9.91%；航向角φ精度分別提高56.20%和9.3%。如表6所示，盡管由于系統(tǒng)長時間運行，造成慣性器件累積誤差明顯，整體融合效果相比于實驗1略有下降，但定位參數(shù)精度仍較高，定位精度＜0.19 m，速度v精度＜0.13 m/s，航向角φ精度＜0.12°。

表6 實驗1與實驗3誤差均方根對比Table 6 RMSE of experiment 1 and experiment 3

圖14 融合算法與單模型的位置誤差均方根Fig.14 RMSE of location with fusion algorithm and single model

圖15 融合算法與單模型的速度誤差均方根Fig.15 RMSE of velocity with fusion algorithm and single model

圖16 融合算法與單模型的航向角誤差均方根Fig.16 RMSE of course angle with fusion algorithm and single model

表7 有累積誤差及有多路徑環(huán)境下的誤差均方根Table 7 RMSE with cumulative error and multipath effect

表8 實驗1與實驗4誤差均方根對比Table 8 RMSE of experiment 1 and experiment 4

融合效果與實驗1中誤差均方根相比較，結(jié)果見表8。在PDR與超聲波定位方案中各傳感器測量精度一定的情況下，從仿真結(jié)果來看，PDR與超聲波單模型方案定位誤差均收斂在一定范圍內(nèi)，由于慣性器件累積誤差與多路徑效應造成 PDR與超聲波定位誤差較大，融合算法的定位性能仍然高于單模型，與PDR、超聲波定位相比，位置（x， y）定位精度分別提高29.43%和29.67%；速度v精度分別提高27.11%和24.13%；航向角φ精度分別提高0.26%和4.84%。與實驗1中室內(nèi)環(huán)境相比，無論是單模型還是融合后的定位參數(shù)精度均有一定程度的下降。融合后，定位精度＜0.21 m，速度v精度＜0.14 m/s，航向角φ精度＜0.16°。

4 結(jié)論

1） 2組模型的融合定位與單模型相比，精度更高。

2） 室內(nèi)環(huán)境復雜或系統(tǒng)運行較長時間時，與室內(nèi)無遮擋無累積誤差情況相比，融合定位精度略有下降但仍維持較高的精度，在一定程度上克服PDR的慣性器件誤差隨時間積累的缺陷與超聲波定位由多路徑效應產(chǎn)生的測距誤差，提高系統(tǒng)在復雜環(huán)境下定位的適應性。

3） 融合定位參數(shù)誤差均收斂在一定范圍內(nèi)，具有較強的穩(wěn)定性與跟蹤性能。

4） 引入反饋環(huán)節(jié)勢必會增加融合中心的計算量，但反饋量僅僅是上一時刻的全局最優(yōu)估計融合狀態(tài)，不會給系統(tǒng)增加過多的開銷，也不會影響系統(tǒng)的實時性。

［1］ 張凡， 陳典鋮， 楊杰. 室內(nèi)定位技術(shù)及系統(tǒng)比較研究［J］. 廣東通信技術(shù)， 2012（11）： 2-5. ZHANG Fan， CHEN Diancheng， YANG Jie. Comparative study on indoor positioning technology and system［J］. Guangdong Communication Technology， 2012（11）： 2-5.

［2］ KUNDRA L， EKLER P. The summary of indoor navigation possibilities considering mobile environment［C］// European Regional Conference on the Engineering of Computer Based Systems. Budapest： IEEE， 2013： 165-166.

［3］ 趙銳， 鐘榜， 朱祖禮， 等. 室內(nèi)定位技術(shù)及應用綜述［J］. 電子科技.2014， 27（3）： 154-157. ZHAO Rui， ZHONG Bang， ZHU Zuli， et al. Overview of indoor location techniques and application［J］. Electronic Sci， 2014，27（3）： 154-157.

［4］ 陳偉. 基于GPS和自包含傳感器的行人室內(nèi)外無縫定位算法研究［D］. 合肥： 中國科學技術(shù)大學信息科學技術(shù)學院， 2010：6-10. CHEN Wei. Research on GPS/self-contained sensors based seamless outdoor/indoor pedestrian positioning algorithm［D］. Hefei： University of Science and Technology of China. College of Information Science and Technology， 2010： 6-10.

［5］ 金宏斌， 戴林燕， 徐毓， 等. 基于無味卡爾曼濾波的多雷達方位配準算法［J］. 數(shù)據(jù)采集與處理， 2006， 21（1）： 29-33. JIN Hongbin， DAI Linyan， XU Yu， et al. Algorithm for multi-radar azimuth registration based on unscented Kalman filter［J］. Journal of Data Acquisition Proceeding， 2006， 21（1）：29-33.

［6］ SHEN Zhong， YU Wenbo， FANG Jiancheng. Nonlinear algorithm based on UKF for low cost SINS/GPS integrated navigation system［J］. System Engineering and Electronica， 2007，29（30）： 408-411.

［7］ 張志鑫， 張峰. 組合濾波器在組合導航中的應用設計［J］. 中國慣性技術(shù)學報， 2008， 16（2）： 188-191. ZHANG Zhixin， ZHANG Feng. Design of combined filter for integrated navigation system［J］. Journal of Chinese Inertial Technology， 2008， 16（2）： 188-191.

［8］ 司學慧， 李小兵， 張彥， 等. 基于平方根 UKF的多傳感器融合再入段目標跟蹤研究［J］. 系統(tǒng)工程與電子技術(shù)， 2012， 34（2）：303-306. SI Xuehui， LI Xiaobing， ZHANG Yan， et al. Multi-sensor fusion target tracking of reentry phase based on square-root unscented Kalman filte［J］. Systems Engineering and Electronics， 2012，34（2）： 303-306.

［9］ ASANO S， WAKUDA Y， KOSHIZUKA N， et al. Pedestrian dead-reckoning unit for navigation system using mobile device［C］// Global Conference on Consumer Electronics. Tokyo：IEEE， 2012： 530-534.

［10］ LAN K， SHIH W. Using smart-phones and floor plans for indoor location tracking［C］// IEEE Transactions on Human-Machine Systems. New Jersey： IEEE， 2014： 1-11.

［11］ FUJII M， OGAWARA R， HATANO H， et al. A study on position tracking system for pedestrian navigation using location and sensor information［C］// ITS Telecommunications （ITST）. Tampere：IEEE， 2013： 344-349.

［12］ FELIZ R， ZALAMA E， GARCIA-BERMEJO J G. Pedestrian tracking using inertial sensors［J］. Journal of Agents， 2009， 31（5）：35.

［13］ LEI Fang， ANTSAKLIS P J， MONTESTRUQUE L A. Design of a wireless assisted pedestrian dead reckoning system-the Nav Mote experience［J］. Instrumentation and Measurement， 2005，54（6）： 2342-2358.

［14］ 原新， 王東陽， 嚴勇杰. 基于推算定位的超聲波定位融合的機器人自主定位技術(shù)［J］. 海軍工程大學學報， 2009， 21（5）：67-72. YUAN Xin， WANG Dongyang， YAN Yongjie. Self-positioning of robot based on dead reckoning and ultrasonic data fusion［J］.Journal of Naval University of Engineering， 2009， 21（5）： 67-72.

［15］ 成蘭， 謝愷. 迭代平方根 UKF［J］. 信息與控制， 2008， 37（4）：439-444. CHENG Lan， XIE Kai. Iterated square root unscented Kalman filter［J］. Information and Control， 2008， 37（4）： 439-444.

［16］ 劉錚. UKF算法及其改進算法的研究［D］. 長沙： 中南大學信息科學與工程學院， 2009： 16-19. LIU Zheng. Research of UKF algorithm and improved algorithm［D］. Changsha： Central South University. School of Information Science Engineering， 2009： 16-19.

［17］ MERVE R V D， WAN E. The square-root unscented Kalman filter for state and parameter-estimation［C］// Acoustics， Speech，and Signal Processing. Salt Lake City， UT： IEEE， 2001：3461-3464.

［18］ 張偉， 王澤陽， 張珂. 基于多模型航跡質(zhì)量的融合算法［J］. 計算機科學， 2013， 40（2）： 65-69. ZHANG We， WANG Zeyang， ZHANG Ke. Track-to-track fusion algorithm based on track quality with mutiple model［J］. Computer Science， 2013， 40（2）： 65-69.

［19］ ZHANG Yu， RAN Jinhe. Dynamic weighted track fusion algorithm based on track comparability degree［C］// Information Theory and Information Security （ICITIS）. Beijing： IEEE， 2010：710-713.

［20］ 崔波. 多傳感器目標跟蹤數(shù)據(jù)融合關鍵技術(shù)研究［D］. 成都：西南交通大學信息科學與技術(shù)學院， 2012： 48-57. CUI Bo. Key technologies research on multi-sensor data fusion for target tracking［D］. Chengdu： Southwest Jiaotong University. School of Information Science and Technology， 2012： 48-57.

［21］ 田雪怡， 李一兵， 李志剛. 航跡融合算法在多傳感器融合中的應用［J］. 計算仿真， 2012， 29（1）： 53-56. TIAN Xueyi， LI Yibing， LI Zhigang. Application of the track fusion in multi-sensor fusion［J］. Computer Simulation. 2012，29（1）： 53-56.

［22］ HAYWARD R， MARCHICK A， POWELL J D. Single baseline GPS based attitude heading reference system （AHRS） for aircraft applications［C］// Proceedings of the American Control Conference 5. San Diego， CA： IEEE， 1999： 3655-3659.

（編輯 陳愛華）

Fusion algorithm in indoor integrated position system for pedestrian dead reckoning/ultrasonic

ZHAO Yanpeng， SHI Wei， AI Mingxi

（School of Aeronautics and Astronautics， Central South University， Changsha 410083， China）

To improve the precision and tracking performance of indoor position system and adapt to the complex indoor environment， the position systems of pedestrian dead reckoning and ultrasonic transducer were integrated. What’s more，the fusion algorithm of noise power factors assisting covariance weighted fusion based on Square Root Unscented Kalman Filter was proposed， and the globally optimal state estimation as feedback was introduced into the algorithm too. To solve the problem of measuring pedestrian’s course angle with ultrasonic， a simple and effective geometry method was proposed. The simulation result shows that in the simulative and dynamic indoor environment including the influence of multipath effect and cumulative error of inertia device， the position accuracy of fusion algorithm is always better than that of single position model’s. Moreover the fusion algorithm has good convergence performance， stability and adaptation performance. So the study has positive effect on indoor position research and application.

indoor position； Square Root Unscented Kalman Filter； noise power factors； covariance weighted fusion；course angle measurement

V249.3

A

1672-7207（2016）05-1588-11

10.11817/j.issn.1672-7207.2016.05.019

2015-06-04；

2015-08-28

國家自然科學基金資助項目（61174002） （Project（61174002） supported by the National Natural Science Foundation of China）

時偉，博士，副教授，從事室內(nèi)導航與定位、飛行器導航與控制技術(shù)研究；E-mail： ahshw@csu.edu.cn