利用步行慣性導(dǎo)航的室內(nèi)定位融合算法研究.doc

利用步行慣性導(dǎo)航的室內(nèi)定位融合算法研究 柏思琪1，梁文海1，2，秦爽1，2 （1.四川師范大學(xué)物理與電子工程學(xué)院，四川成都610100；2.無線傳感器網(wǎng)絡(luò)四川省高校重點實驗室，四川成都610101） 摘要：步行慣性導(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：A：1004?373X（xx）15?0001?04 ：xx?02?06 基金項目：四川省教育廳重點項目（12ZA133） 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)（GlobalPositioningSystem，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)點是體積小、成本低、功耗低，但最大的缺點是有累積誤差，其累積誤差隨位移非線性增長，“零速度”更新（ZeroVeloc?ityUpdate，ZUPT）卡爾曼濾波算法3能將累積誤差限制在線性增長范圍內(nèi)，在此基礎(chǔ)上，還可利用陀螺儀、電子羅盤、磁力計的測量值協(xié)助卡爾曼濾波算法消除慣性導(dǎo)航方向上的累積誤差4。這些方法都能在一定程度上提高步行慣性導(dǎo)航的準(zhǔn)確性，但對較長時間行走還需要其他消除累積誤差的方法。例如，另一類有效方法是將慣性導(dǎo)航與其他無累積誤差的定位算法相融合。文獻(xiàn)5介紹了一種將指紋定位與慣性導(dǎo)航相結(jié)合的經(jīng)驗算法，對精度的提高有一定效果，但指紋定位算法對數(shù)據(jù)庫的建立和維護(hù)要求較高，不適用于無基礎(chǔ)設(shè)施定位。針對上述問題，本文提出一種卡爾曼濾波算法，融合了步行慣性導(dǎo)航和基于測距定位信息，該算法計算復(fù)雜度低、速度快、融合效果好。仿真結(jié)果表明，該算法的定位軌跡平滑、誤差小且無累積誤差，對無基礎(chǔ)設(shè)施室內(nèi)定位系統(tǒng)實現(xiàn)具有很好的參考意義和應(yīng)用價值。 1步行慣性導(dǎo)航 智能手機通常內(nèi)置了慣性測量裝置（InertialMea?surementUnit，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)OpenShoe8基于安裝于人腳踝的ADIS16367IMU（包括加速度和陀螺儀）和AtmelAVR32UC3C微控器，能很好地實現(xiàn)零速度檢測。 OpenShoe方案把人為置零時速度的誤差作為卡爾曼濾波的測量值mk，稱為偽測量值，用vk表示k時刻的速度，則mk=0-vk，而不是直接用速度作為測量值，采用偽測量值的原因是IMU并不能真正測量到零速度。系統(tǒng)在“零速度”時刻用卡爾曼濾波跟蹤方向矩陣C?nb,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,kT。用每次濾波的輸出狀態(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定位、擴頻頻譜信號（ChirpSpreadSpectrum，CSS）定位和超寬帶定位等，其中ZigBee定位和WiFi定位測量的是接收信號強度（ReceivedSignalStrength，RSS），無基礎(chǔ)設(shè)施定位一般選用干擾較小的CSS或UWB定位，測量信息是到達(dá)時間（TimeofArrival，ToA）。這些定位方案可采用的定位算法也有很多，如三角測量定位算法、最小二乘定位算法、最大似然估計定位算法等。這些算法中各有優(yōu)缺點，但它們的共同缺點是：受信道不穩(wěn)定因素的影響，定位結(jié)果有跳動現(xiàn)象，跳動的程度由信道環(huán)境和算法的穩(wěn)健性共同決定。例如，KwangHyunLee等提出一種基于CSS的卡爾曼濾波定位系統(tǒng)9，其測距精度比ZigBee有明顯提高，但仍然有接近2m左右的跳動。 為了減小無線測距定位結(jié)果的跳動，增加定位結(jié)果的穩(wěn)定性，可以把基于測距定位的結(jié)果與慣性導(dǎo)航相融合。本文不詳細(xì)介紹測距定位算法本身，只利用算法輸出的位置結(jié)果進(jìn)行融合。 3融合定位算法 如前所述，慣性導(dǎo)航可產(chǎn)生連續(xù)的軌跡，但會伴隨位移和方向兩方面的累積誤差，因此考慮通過測距定位算法來校正。融合算法的主要思想是逐步將慣性導(dǎo)航的結(jié)果拉向測距定位附近，以消除其累積誤差，如圖1所示，圖中三角形為起點，實曲線為真實運動軌跡，虛線為慣性導(dǎo)航結(jié)果，圓圈為測距定位結(jié)果。 將慣性導(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)的采樣頻率可能會不同，卡爾曼濾波在有測量值時進(jìn)行，所以新系統(tǒng)的測量值有三種取值： 式中：S(akn)為與加速度值相關(guān)的對稱陣，其取值可參見文獻(xiàn)4。新系統(tǒng)的測量矩陣為： 新系統(tǒng)的測量方程為： 狀態(tài)更新方程與經(jīng)典卡爾曼濾波一致： 其中卡爾曼增益Kk也按照經(jīng)典卡爾曼公式更新： Pk|k-1為預(yù)測狀態(tài)協(xié)方差矩陣，根據(jù)k-1時刻的測量來計算： Pk-1|k-1的計算方程為： 用每次濾波的輸出狀態(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ù)大約為15000次，選擇合適的DSP器件需要運行的時間僅為毫秒級。 4算法仿真 人行走時的加速度和角加速度值可以通過程序模擬產(chǎn)生，產(chǎn)生的方法見文獻(xiàn)10。本文加速度和陀螺儀的仿真數(shù)據(jù)公開數(shù)據(jù)庫：/.openshoe/。仿真數(shù)據(jù)分別是直線行走300步和沿固定軌道走10圈兩種情況，人的行走速度為1.2m/s，采樣頻率為100Hz，加速度和陀螺儀數(shù)據(jù)的噪聲假設(shè)為高斯白噪聲，標(biāo)準(zhǔn)差分別為0.012m/s2和0.0087rad/s，它們還分別有1個y軸上的偏差0.1m/s2和0.05rad/s，偏差的噪聲也為高斯白噪聲，標(biāo)準(zhǔn)差分別為0.04m/s2和0.012rad/s。假設(shè)測距定位系統(tǒng)的輸出頻率為IMU模塊的15，20Hz，其測量噪聲為高斯白噪聲，在3個軸上的標(biāo)準(zhǔn)差都為1m。兩種情況下融合算法與Openshoe慣性導(dǎo)航算法定位的軌跡比較圖如圖2，圖3所示。 從圖2可以看出，由于傳感器的偏差和噪聲的存在，慣性導(dǎo)航的軌跡大約在y軸方向100m處就開始偏離真實行走軌跡，并且偏離程度隨著行走距離變遠(yuǎn)而越來越大，而融合定位軌跡始終在真實軌跡附近，且比基于測距的定位結(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所示。無論是走直線還是繞圈，融合算法的均方誤差始終最小，進(jìn)一步說明融合算法有更好的性能。 5結(jié)語 無基礎(chǔ)設(shè)施室內(nèi)定位技術(shù)在救援、反恐等特殊場合有重要意義，步行慣性導(dǎo)航是可用于無基礎(chǔ)設(shè)施室內(nèi)定位的重要技術(shù)之一，因此，研究如何消除慣性導(dǎo)航的累積誤差非常具有實際價值。本文提出的融合定位算法在穩(wěn)定性和精確性兩方面都表現(xiàn)出較為優(yōu)越的性能。下一步的工作是搭建實測嵌入式定位平臺以進(jìn)一步驗證算法的實用性，在實測定位平臺基礎(chǔ)上融合更多定位方法，如GPS、UWB、計算機視覺等，以實現(xiàn)更靈活、更精確的室內(nèi)外無縫定位。 參考文獻(xiàn) 1NOHY，YAMAGUCHIH，LEEU，etal.CLIPS：infrastruc?ture?freecollaborativeindoorpositioningschemefortime?criti?calteamoperationsC/xxIEEEInternationalConferenceonPervasiveComputingandCommunications.SanDiego：IEEE，xx：172?178. 2LEES，BYOUNGGEUNK，HOONK，etal.Inertialsensor?basedindoorpedestrianlocalizationwithminimum802.15.4aconfigurationJ.IEEETransactionsonIndustrialInformatics，xx，7（3）：455?466. 3FOXLINE.Pedestriantrackingwithshoe?mountedinertialsen?sorsJ.IEEEComputerGraphicsandApplications，xx，25（6）：38?46. 4JIMENEZA，SECOF，PRIETOJ，etal.IndoorpedestriannavigationusinganINS/EKFframeworkforyawdriftreductionandafoot?mountedIMUC/xxIEEEWorkshoponPosi?tioning，NavigationandCommunication.Dresden，Germany：IEEE，xx：11?12. 5周亮，付永濤，李廣軍.無線定位與慣性導(dǎo)航結(jié)合的室內(nèi)定位系統(tǒng)設(shè)計J.電子技術(shù)應(yīng)用，xx，40（4）：73?76. 6ZAMPELLAFJ，JIMENEZAR，SECOF，etal.Simulationoffoot?mountedIMUsignalsfortheevaluationofPDRalgo?rithmsC/xxIEEEInternationalConferenceonIndoorPo?sitioningandIndoorNavigation.Guimaraes：IEEE，xx：1?7. 7SKOGI，HANDELP，NILSSONJO，etal.Zero?velocityde?tection?analgorithmevaluationJ.IEEETransactionsonBio?medicalEngineering，xx，57（11）：2657?2666. 8NILSSONJO，SKOGI，HANDELP，etal.Foot?mountedINSforeverybody?anopen?sourceembeddedimplementationC/xxIEEEInternationalConferenceonPositionLocationandNavigationSymposium.MyrtleBeach：IEEE，xx：140?145. 9LEEKH，CHOSH.CSSbasedlocalizationsystemusingKal?manfilterformulti?cellenvironmentC/IEEEInternationalConferenceonAdvancedTechnologiesforCommunications.Ha?noi：IEEE，xx：293?296. 10ZAMPELLAFJ，JIMENEZAR，SECOF，etal.Simulationoffoot?mountedIMUsignal