柏思琪+梁文海+秦爽

摘 要： 步行慣性導(dǎo)航是一種用于無基礎(chǔ)設(shè)施室內(nèi)定位中的新技術(shù)，但累積誤差效應(yīng)限制了它的使用范圍，為解決此問題，提出一種把步行慣性導(dǎo)航與無線測距定位相融合的定位算法，結(jié)合前者軌跡連續(xù)和后者無累積誤差的優(yōu)點。仿真結(jié)果表明，該融合算法在定位的穩(wěn)定性和精確性方面都表現(xiàn)出良好的性能，驗證了該方案的合理性、有效性。

關(guān)鍵詞： 室內(nèi)定位； 慣性導(dǎo)航； 零速度更新； 卡爾曼濾波； 融合算法

中圖分類號： TN96?34； TP274 文獻標(biāo)識碼： A 文章編號： 1004?373X（2015）0 引 言

室內(nèi)定位系統(tǒng)應(yīng)用前景廣泛，越來越受到研究學(xué)者的重視?？紤]精度、成本、功耗、尺寸及復(fù)雜性等各方面條件，目前還沒有一種普適的室內(nèi)外無縫定位系統(tǒng)。根據(jù)是否需要基礎(chǔ)設(shè)施，室內(nèi)定位系統(tǒng)分為有基礎(chǔ)設(shè)施的定位系統(tǒng)和無基礎(chǔ)設(shè)施的定位系統(tǒng)[1]。前者在搜救、反恐等場合幾乎不適用，后者適用范圍較為靈活。全球定位系統(tǒng)（Global Positioning System，GPS）可用在無基礎(chǔ)設(shè)施定位系統(tǒng)中，在大多數(shù)室外環(huán)境都可以實現(xiàn)較高精度的定位，但在惡劣的室外環(huán)境及室內(nèi)環(huán)境下，需要其他定位系統(tǒng)協(xié)同完成定位，如慣性導(dǎo)航[2]。步行慣性導(dǎo)航模塊非常適合用于無基礎(chǔ)設(shè)施室內(nèi)定位，其優(yōu)點是體積小、成本低、功耗低，但最大的缺點是有累積誤差，其累積誤差隨位移非線性增長，“零速度”更新（Zero Velocity Update，ZUPT）卡爾曼濾波算法[3] 能將累積誤差限制在線性增長范圍內(nèi)，在此基礎(chǔ)上，還可利用陀螺儀、電子羅盤、磁力計的測量值協(xié)助卡爾曼濾波算法消除慣性導(dǎo)航方向上的累積誤差[4]。這些方法都能在一定程度上提高步行慣性導(dǎo)航的準(zhǔn)確性，但對較長時間行走還需要其他消除累積誤差的方法。例如，另一類有效方法是將慣性導(dǎo)航與其他無累積誤差的定位算法相融合。文獻[5]介紹了一種將指紋定位與慣性導(dǎo)航相結(jié)合的經(jīng)驗算法，對精度的提高有一定效果，但指紋定位算法對數(shù)據(jù)庫的建立和維護要求較高，不適用于無基礎(chǔ)設(shè)施定位。針對上述問題，本文提出一種卡爾曼濾波算法，融合了步行慣性導(dǎo)航和基于測距定位信息，該算法計算復(fù)雜度低、速度快、融合效果好。仿真結(jié)果表明，該算法的定位軌跡平滑、誤差小且無累積誤差，對無基礎(chǔ)設(shè)施室內(nèi)定位系統(tǒng)實現(xiàn)具有很好的參考意義和應(yīng)用價值。

1 步行慣性導(dǎo)航

智能手機通常內(nèi)置了慣性測量裝置（Inertial Measurement Unit，IMU），包括加速度傳感器和陀螺儀傳感器等，市場上也很容易購置到IMU模塊。理論上，目標(biāo)的移動距離可通過IMU采集到的加速度通過兩次積分得到，但對于消費級IMU模塊，僅考慮牛頓運動方程時，人行走的位置誤差的標(biāo)準(zhǔn)差正比于時間的三次方，僅僅幾秒時間的行走就會導(dǎo)致幾米的誤差。人在行走時總會有停止?fàn)顟B(tài)，理論上停止?fàn)顟B(tài)的速度應(yīng)該為零，但實際上依靠IMU幾乎無法測到速度為零的情況。ZUPT慣性導(dǎo)航算法的基本思想是采用一些經(jīng)驗檢測方法檢測停止?fàn)顟B(tài)[6?7]，將該狀態(tài)的速度人為置零，這種方法在減小慣性導(dǎo)航累積誤差上取得了很好的效果，例如，瑞典KTH大學(xué)設(shè)計的開源慣性導(dǎo)航系統(tǒng)OpenShoe[8]基于安裝于人腳踝的ADIS16367 IMU（包括加速度和陀螺儀）和Atmel AVR32UC3C微控器，能很好地實現(xiàn)零速度檢測。

OpenShoe方案把人為置零時速度的誤差作為卡爾曼濾波的測量值[mk，]稱為偽測量值，用[vk]表示[k]時刻的速度，則[mk=0-vk，]而不是直接用速度作為測量值，采用偽測量值的原因是IMU并不能真正測量到零速度。系統(tǒng)在“零速度”時刻用卡爾曼濾波跟蹤方向矩陣[Cnb，k]以及各狀態(tài)向量：位置的誤差[δrk，]速度的誤差[δvk，]導(dǎo)航坐標(biāo)系下姿態(tài)的誤差[δφk，]加速度傳感器的測量誤差[δak]和陀螺儀傳感器的測量誤差[δωk，]每個狀態(tài)量均由三個坐標(biāo)軸上的分量構(gòu)成。完整的狀態(tài)向量為[δxk=[δrk，δvk，δφk，δak，δωk]T]。用每次濾波的輸出狀態(tài)[δrk，][δvk，][δφk]更新導(dǎo)航狀態(tài)位置[rk，]速度[vk]和姿態(tài)[φk，]用輸出狀態(tài)[δak]和[δωk]更新加速度和角速度的測量值[ak]和[ωk]。算法處理后的位置誤差的標(biāo)準(zhǔn)差與行走距離基本是線性的，但依然存在累積誤差。因此，本文目的是如何消除步行慣性導(dǎo)航引起的累積誤差。

2 基于測距的定位算法

如果能夠事先通過基于測距的定位算法得到一個粗糙的位置，就能夠利用它來消除累積誤差?；跍y距的定位方案有ZigBee定位、WiFi定位、擴頻頻譜信號（Chirp Spread Spectrum，CSS）定位和超寬帶定位等，其中ZigBee定位和WiFi定位測量的是接收信號強度（Received Signal Strength，RSS），無基礎(chǔ)設(shè)施定位一般選用干擾較小的CSS或UWB定位，測量信息是到達時間（Time of Arrival，ToA）。這些定位方案可采用的定位算法也有很多，如三角測量定位算法、最小二乘定位算法、最大似然估計定位算法等。這些算法中各有優(yōu)缺點，但它們的共同缺點是：受信道不穩(wěn)定因素的影響，定位結(jié)果有跳動現(xiàn)象，跳動的程度由信道環(huán)境和算法的穩(wěn)健性共同決定。例如，Kwang Hyun Lee等提出一種基于CSS的卡爾曼濾波定位系統(tǒng)[9]，其測距精度比ZigBee有明顯提高，但仍然有接近2 m左右的跳動。

為了減小無線測距定位結(jié)果的跳動，增加定位結(jié)果的穩(wěn)定性，可以把基于測距定位的結(jié)果與慣性導(dǎo)航相融合。本文不詳細介紹測距定位算法本身，只利用算法輸出的位置結(jié)果進行融合。

3 融合定位算法

如前所述，慣性導(dǎo)航可產(chǎn)生連續(xù)的軌跡，但會伴隨位移和方向兩方面的累積誤差，因此考慮通過測距定位算法來校正。融合算法的主要思想是逐步將慣性導(dǎo)航的結(jié)果拉向測距定位附近，以消除其累積誤差，如圖1所示，圖中三角形為起點，實曲線為真實運動軌跡，虛線為慣性導(dǎo)航結(jié)果，圓圈為測距定位結(jié)果。

圖1 融合算法原理示意圖

將慣性導(dǎo)航系統(tǒng)與測距定位系統(tǒng)視為一個新的導(dǎo)航定位系統(tǒng)。導(dǎo)航系統(tǒng)的狀態(tài)仍然為[rk，][vk]和[φk，]濾波狀態(tài)中[δrk，][δvk]修改為[rk，][vk，]其他狀態(tài)保持不變。位置的測量值可由測距定位系統(tǒng)得到，而速度的測量值仍然通過“零速度”檢測得到，注意，兩個分系統(tǒng)的采樣頻率可能會不同，卡爾曼濾波在有測量值時進行，所以新系統(tǒng)的測量值有三種取值：

[mk=[pk，0]T，有位置輸出且為“零速度”時pTk，僅有位置輸出但不為“零速度”時0T，”僅為“零速度”但無位置輸出時] （1）

式中：[pTk]代表測距定位系統(tǒng)的位置測量值；[0T]代表“零速度”。

新系統(tǒng)的濾波狀態(tài)為[xkk=xk=[rk，vk，δφk，δak，δωk]T，]狀態(tài)轉(zhuǎn)移方程表示為：

[xkk-1=Fkxk-1k-1+wk-1] （2）

式中：[xkk-1]為預(yù)測狀態(tài)；[xk-1k-1]為[k-1]時刻濾波器的輸出狀態(tài)；[wk-1]是過程噪聲，其協(xié)方差矩陣為[Qk=E（wkwTk）]。 轉(zhuǎn)移矩陣為[4]：

[Fk=IΔt?I0000I-Δt?S（a′kn）Δt?Cnb，kk-1000I0Δt?Cnb，kk-1000I00000I] （3）

式中：[S（a′kn）]為與加速度值相關(guān)的對稱陣，其取值可參見文獻[4]。新系統(tǒng)的測量矩陣為：

[H=I00000I000，有位置輸出且為“零速度”時[0I000]，僅有位置輸出但不為“零速度”時[I0000]，僅為“零速度”但無位置輸出時] （4）

新系統(tǒng)的測量方程為：

[zk=Hxkk+nk] （5）

狀態(tài)更新方程與經(jīng)典卡爾曼濾波一致：

[xkk=xkk-1+Kk?[mk-Hxkk-1]] （6）

其中卡爾曼增益[Kk]也按照經(jīng)典卡爾曼公式更新：

[Kk=Pkk-1HT（HPkk-1HT-Rk）-1] （7）

[Pkk-1]為預(yù)測狀態(tài)協(xié)方差矩陣，根據(jù)[k-1]時刻的測量來計算：

[Pkk-1=Fk-1Pk-1k-1FTk-1+Qk-1] （8）

[Pk-1k-1]的計算方程為：

[Pkk=（I-KkH）Pkk-1（I-KkH）T+Rk] （9）

用每次濾波的輸出狀態(tài)[rk，][vk，][δφk]更新導(dǎo)航狀態(tài)位置[rk，]速度[vk]和姿態(tài)[φk，]用輸出狀態(tài)[δak]和[δωk]更新加速度和角速度的測量值[ak]和[ωk]。

融合算法的計算復(fù)雜度由慣性導(dǎo)航方程與卡爾曼濾波方程共同決定，前者為牛頓運動方程，復(fù)雜度低，所以總的計算復(fù)雜度由卡爾曼濾波部分決定?？柭鼮V波的算法復(fù)雜度為：

[3n3+3mn2+2m2n+mn+A+B]

式中：[n]表示狀態(tài)向量維數(shù)，本文算法即為15；[m]表示測量向量維數(shù)，本文算法為6或3；[A]為狀態(tài)一步預(yù)測的乘法次數(shù)，本文算法為225；[B]為測量一步預(yù)測的乘法次數(shù)，本文算法為30。這樣，程序運行一次總共所需執(zhí)行的乘法次數(shù)大約為15 000次，選擇合適的DSP器件需要運行的時間僅為毫秒級。

4 算法仿真

人行走時的加速度和角加速度值可以通過程序模擬產(chǎn)生，產(chǎn)生的方法見文獻[10]。本文加速度和陀螺儀的仿真數(shù)據(jù)來自公開數(shù)據(jù)庫http：//www.openshoe.org/。仿真數(shù)據(jù)分別是直線行走300步和沿固定軌道走10圈兩種情況，人的行走速度為1.2 m/s，采樣頻率為100 Hz，加速度和陀螺儀數(shù)據(jù)的噪聲假設(shè)為高斯白噪聲，標(biāo)準(zhǔn)差分別為0.012 m/s2和0.008 7 rad/s，它們還分別有1個[y]軸上的偏差0.1 m/s2和0.05 rad/s，偏差的噪聲也為高斯白噪聲，標(biāo)準(zhǔn)差分別為0.04 m/s2和0.012 rad/s。假設(shè)測距定位系統(tǒng)的輸出頻率為IMU模塊的[15，]20 Hz，其測量噪聲為高斯白噪聲，在3個軸上的標(biāo)準(zhǔn)差都為1 m。兩種情況下融合算法與Openshoe慣性導(dǎo)航算法定位的軌跡比較圖如圖2，圖3所示。

圖2 直線行走300步仿真結(jié)果

圖3 固定軌道走10圈仿真結(jié)果

從圖2可以看出，由于傳感器的偏差和噪聲的存在，慣性導(dǎo)航的軌跡大約在[y]軸方向100 m處就開始偏離真實行走軌跡，并且偏離程度隨著行走距離變遠而越來越大，而融合定位軌跡始終在真實軌跡附近，且比基于測距的定位結(jié)果更穩(wěn)健、更準(zhǔn)確。對于沿固定軌道繞圈的仿真結(jié)果也同樣如此，如圖3所示，慣性導(dǎo)航與真實軌跡越來越不重合，而融合定位軌跡的結(jié)果始終在真實軌跡附近，同樣比基于測距的定位結(jié)果更穩(wěn)健、準(zhǔn)確。造成慣性導(dǎo)航偏離真實軌跡的主要原因是傳感器（加速度和陀螺儀）的噪聲和偏差對時間的兩次積分，其中偏差對慣性導(dǎo)航的影響更為明顯，在無偏差的情況下，融合算法對慣性導(dǎo)航結(jié)果的改善程度比有偏差的情況小，但仍有提高，限于篇幅在此不給出仿真結(jié)果。沿直線行走是慣性導(dǎo)航的最壞情況，均方根誤差隨著行走距離的增大不斷增加，如圖4所示。當(dāng)繞圈時，由于加速度存在一個正向的偏差，慣性導(dǎo)航的速度比人的真實行走速度快，均方根誤差總體呈現(xiàn)一個增大的趨勢，當(dāng)慣性導(dǎo)航結(jié)果超過人的真實位置半圈時，均方根誤差增加到最大，隨后呈現(xiàn)減小趨勢，直到超過真實位置一圈時，誤差降到最小，然后按照同樣的趨勢增大，如圖5所示。無論是走直線還是繞圈，融合算法的均方誤差始終最小，進一步說明融合算法有更好的性能。

圖4 直走均方根誤差

圖5 繞圈均方根誤差

5 結(jié) 語

無基礎(chǔ)設(shè)施室內(nèi)定位技術(shù)在救援、反恐等特殊場合有重要意義，步行慣性導(dǎo)航是可用于無基礎(chǔ)設(shè)施室內(nèi)定位的重要技術(shù)之一，因此，研究如何消除慣性導(dǎo)航的累積誤差非常具有實際價值。本文提出的融合定位算法在穩(wěn)定性和精確性兩方面都表現(xiàn)出較為優(yōu)越的性能。下一步的工作是搭建實測嵌入式定位平臺以進一步驗證算法的實用性，在實測定位平臺基礎(chǔ)上融合更多定位方法，如GPS、UWB、計算機視覺等，以實現(xiàn)更靈活、更精確的室內(nèi)外無縫定位。

參考文獻

[1] NOH Y， YAMAGUCHI H， LEE U， et al. CLIPS： infrastructure?free collaborative indoor positioning scheme for time?critical team operations [C]// 2013 IEEE International Conference on Pervasive Computing and Communications. San Diego： IEEE， 2013： 172?178.

[2] LEE S， BYOUNGGEUN K， HOON K， et al. Inertial sensor?based indoor pedestrian localization with minimum 802.15.4a configuration [J]. IEEE Transactions on Industrial Informatics， 2011， 7（3）： 455?466.

[3] FOXLIN E. Pedestrian tracking with shoe?mounted inertial sensors [J]. IEEE Computer Graphics and Applications， 2005， 25（6）： 38?46.

[4] JIMENEZ A， SECO F， PRIETO J， et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot?mounted IMU [C]// 2010 IEEE Workshop on Positioning， Navigation and Communication. Dresden， Germany： IEEE， 2010： 11?12.

[5] 周亮，付永濤，李廣軍.無線定位與慣性導(dǎo)航結(jié)合的室內(nèi)定位系統(tǒng)設(shè)計[J].電子技術(shù)應(yīng)用，2014，40（4）：73?76.

[6] ZAMPELLA F J， JIMENEZ A R， SECO F， et al. Simulation of foot?mounted IMU signals for the evaluation of PDR algorithms [C]// 2011 IEEE International Conference on Indoor Positioning and Indoor Navigation. Guimaraes： IEEE， 2011： 1?7.

[7] SKOG I， HANDEL P， NILSSON J O， et al. Zero?velocity detection?an algorithm evaluation [J]. IEEE Transactions on Biomedical Engineering， 2010， 57（11）： 2657?2666.

[8] NILSSON J O， SKOG I， HANDEL P， et al. Foot?mounted INS for everybody?an open?source embedded implementation [C]// 2012 IEEE International Conference on Position Location and Navigation Symposium. Myrtle Beach： IEEE， 2012： 140?145.

[9] LEE K H， CHO S H. CSS based localization system using Kalman filter for multi?cell environment [C]// IEEE International Conference on Advanced Technologies for Communications. Hanoi： IEEE， 2008： 293?296.

[10] ZAMPELLA F J， JIMENEZ A R， SECO F， et al. Simulation of foot?mounted IMU signals for the evaluation of PDR algorithms [C]// 2011 IEEE International Conference on Indoor Positioning and Indoor Navigation. Guimaraes： IEEE， 2011： 1?7.