基于距離信息的行人協(xié)同導(dǎo)航研究

陳 澤，吳美平，陳昶昊

（國防科學(xué)技術(shù)大學(xué)機電工程與自動化學(xué)院，長沙410073)

基于距離信息的行人協(xié)同導(dǎo)航研究

陳 澤，吳美平，陳昶昊

（國防科學(xué)技術(shù)大學(xué)機電工程與自動化學(xué)院，長沙410073)

針對行人在室內(nèi)導(dǎo)航中GPS信息無法獲取以及純慣導(dǎo)解算結(jié)果發(fā)散嚴(yán)重的問題，提出了通過協(xié)同導(dǎo)航的方式提高行人室內(nèi)導(dǎo)航精度的方法。利用行人間的相對距離約束關(guān)系，抑制純慣導(dǎo)解算結(jié)果的發(fā)散。詳細推導(dǎo)了行人協(xié)同導(dǎo)航的模型，采用信息濾波的方法解算導(dǎo)航結(jié)果，并在理論推導(dǎo)中發(fā)現(xiàn)信息濾波更加適合行人協(xié)同導(dǎo)航的工程實踐。設(shè)計了一種使用微型慣性測量器和超寬帶測距設(shè)備的行人協(xié)同導(dǎo)航系統(tǒng)，進行實際效果驗證。通過數(shù)據(jù)采集與處理，將行人導(dǎo)航軌跡圖輸出，分析協(xié)同導(dǎo)航以及單獨導(dǎo)航的誤差，驗證協(xié)同導(dǎo)航的有效性。實驗結(jié)果表明，協(xié)同導(dǎo)航與單獨導(dǎo)航相比，對導(dǎo)航軌跡和導(dǎo)航誤差有更好的修正效果。

協(xié)同導(dǎo)航；行人導(dǎo)航；距離約束；信息濾波

0 引言

協(xié)同導(dǎo)航（Cooperative Navigation，CN)是各導(dǎo)航平臺在運動過程中通過相互之間的觀測，利用平臺之間的約束關(guān)系，提高系統(tǒng)導(dǎo)航精度的一種導(dǎo)航方式。協(xié)同導(dǎo)航在時空覆蓋域、時間效率、性能魯棒性、故障容錯與系統(tǒng)重構(gòu)等方面具有獨特優(yōu)勢，并且配置靈活，可以根據(jù)平臺能力合理分配工作量，而且具有更強的目標(biāo)分辨力［1］。在協(xié)同導(dǎo)航中，裝備低精度導(dǎo)航設(shè)備的移動平臺可以利用其他高精度平臺的導(dǎo)航信息提高導(dǎo)自身導(dǎo)航精度。同時，只需要部分平臺裝備有界定位誤差的導(dǎo)航系統(tǒng)，系統(tǒng)中每個平臺都可以具有誤差有界的定位能力，解決慣性導(dǎo)航系統(tǒng)的誤差發(fā)散問題［2］。

行人導(dǎo)航（Pedestrian Navigation，PN)是一項應(yīng)用前景十分廣闊的技術(shù)。行人主要通過攜載GPS和微型慣性測量器（Micro Inertial Measurement Unit，MIMU)設(shè)備完成導(dǎo)航和實時定位［3］。但是隨著城市的建設(shè)，越來越高大的建筑物阻礙了GPS信號的接收，使得GPS信息不可用。同時，由于一般MIMU測量精度的局限性，其測量結(jié)果會隨著測量時間增加而發(fā)散，導(dǎo)航精度因此不能滿足要求［4］。

本文主要研究利用距離信息進行行人協(xié)同導(dǎo)航的方法。在詳細推導(dǎo)行人協(xié)同導(dǎo)航模型和算法的基礎(chǔ)上，通過具體的實驗分析利用協(xié)同導(dǎo)航的方式對行人導(dǎo)航結(jié)果的影響，對這種方法的特點做出總結(jié)并得到相應(yīng)的結(jié)論，進一步提高行人導(dǎo)航的精度。

1 行人協(xié)同導(dǎo)航模型

行人運動模型用于描述在導(dǎo)航坐標(biāo)系下行人的位移、速度等運動學(xué)參數(shù)之間的關(guān)系。本文重點研究多行人協(xié)同系統(tǒng)的導(dǎo)航性能，在單個行人運動模型的基礎(chǔ)上，建立多行人協(xié)同導(dǎo)航模型。

建立單個行人Kalman濾波模型［1］，記k（k＝0，1，2，…)時刻第i個行人的狀態(tài)矢量為：

其中，ri（k）為k時刻第i個行人的位置信息；vi（k）為k時刻第i個行人的速度。則單個行人非線性運動狀態(tài)空間形式為：

其中，fi是第i個行人k＋1時刻的狀態(tài)矢量與k時刻的狀態(tài)矢量、輸入矢量之間的投影函數(shù)；Ui（k)為k時刻第i個行人的控制輸入矢量，在此選取k時刻第i個行人的加速度作為輸入量；Wi（k)為系統(tǒng)噪聲，方差為Qi（k)。

建立單個行人位置信息觀測方程，將GPS對第i個行人的位置輸出作為其狀態(tài)觀測，得到第i個行人位置信息觀測方程為：

其中，Zi（k)為k時刻第i個行人的觀測矢量；hi為第i個行人k時刻的狀態(tài)矢量和觀測矢量之間的投影函數(shù)；ζi（k)為k時刻第i個行人的觀測噪聲，方差為Ri（k)。

建立行人相對位置信息觀測方程，設(shè)k時刻第i個行人對第j個行人的距離觀測記為Zij（k)，則對應(yīng)的行人間位置信息觀測方程為：

其中，ζij（k)為k時刻第i個行人和第j個行人之間測距的觀測噪聲，方差為Rij（k)；hij為第i個行人和第j個行人k時刻的狀態(tài)矢量和觀測矢量之間的投影函數(shù)。

多行人協(xié)同導(dǎo)航的狀態(tài)矢量由全部行人的狀態(tài)矢量組成，設(shè)k時刻的系統(tǒng)狀態(tài)矢量為X（k)。由N個行人組成的系統(tǒng)狀態(tài)矢量、輸入矢量和系統(tǒng)噪聲為：

則系統(tǒng)的狀態(tài)方程為：

其中，系統(tǒng)噪聲ζ（k)的方差為Q（k)。

對式（6)中的每一個非線性方程進行線性化，得到線性化的系統(tǒng)狀態(tài)空間方程為：

當(dāng)把單個行人和行人間的位置信息觀測方程寫為多行人協(xié)同導(dǎo)航系統(tǒng)的觀測方程時，只需適當(dāng)擴展觀測矩陣。對由N個行人組成的多行人協(xié)同導(dǎo)航系統(tǒng)，k時刻第i個行人位置信息線性化觀測方程為：

其中，

類似地，對由N個行人組成的協(xié)同導(dǎo)航系統(tǒng)，k時刻第i個行人對第j個行人位置信息線性化觀測方程為：

其中，

通過以上推導(dǎo)得到線性化系統(tǒng)模型為：

2 協(xié)同導(dǎo)航信息濾波算法

k時刻狀態(tài)估計值為Xe（k)，狀態(tài)預(yù)測值為Xp（k)，狀態(tài)協(xié)方差矩陣估計值為Pe（k)，狀態(tài)協(xié)方差矩陣預(yù)測值為Pp（k)，則：

信息濾波是用信息狀態(tài)矢量y和信息矩陣Y來代替Kalman濾波的X和P進行處理，即：

信息狀態(tài)和信息矩陣的遞推方程集可直接由Kalman濾波方程獲得。在數(shù)學(xué)上信息濾波與常規(guī)的Kalman濾波結(jié)果是一致的。

將式（13)代入到式（14)中，得：

其中，Yp（k)為預(yù)測信息矩陣；yp（k)為預(yù)測信息向量。

信息濾波更新過程為：

其中，Ye（k)為k時刻的信息矩陣估計值；ye（k)為信息向量估計值，E（k)為觀測更新貢獻的信息更新矩陣，e（k)為觀測更新貢獻的信息更新矢量，即：

對于k時刻行人i的觀測，將式（8)代入式（17)，信息更新矩陣和更新向量為：

對于k時刻行人i、j間的相對觀測，將式（10)代入式（17)，觀測更新的信息更新矩陣為：

由式（18)～式（21)可知，聯(lián)合分布參數(shù)的更新僅改變測量的行人和被測行人之間的信息參數(shù)，具有局部性，更適合運用在行人協(xié)同導(dǎo)航的工程實踐中。

3 實驗驗證

3．1 硬件介紹

實驗中所用的UWB測距模塊是美國Time Do?main公司的PulsON 440測距模塊。模塊采用超寬帶脈沖射頻信號機制進行雙向飛行時間測距和通信，在高度多徑和高度反射的環(huán)境下，點對點精度可以達到2cm，室內(nèi)穿墻誤差小于0.5m。使用的MIMU為荷蘭Xsens公司的MTi集成傳感器。它內(nèi)部包含有三軸加速度計和三軸陀螺以及三軸磁強計，靜態(tài)測試得到的陀螺3個軸零偏穩(wěn)定性參數(shù)分別為30.7（°)／h，26.8（°)／h，40.5（°)／h。如圖1所示，所用的這2個模塊具有功耗低、體積小、精度高、可靠性強等優(yōu)勢。

圖1 P440測距模塊和MIMUFig．1 P440 ranging module and MIMU

3．2 行人協(xié)同導(dǎo)航實驗

實驗由3人完成，3人均裝備MIMU和P440測距模塊，3人行走路徑均為矩形框，如圖2所示。通過差分GPS的方法得到3個人的行走軌跡，以差分GPS數(shù)據(jù)作為實際值參考。在實驗中，只有第一個人的GPS信息可用，即模擬其余2個人在室內(nèi)無法得到GPS信號的情況。

圖2 實驗路線Fig．2 Experiment route

如圖3所示，實驗中將MIMU固定在實驗者左腳上，將P440測距通信模塊固定在實驗者左肩上，引出USB線連接電腦采集數(shù)據(jù)，MIMU和P440測距模塊輸出頻率均為100Hz。實驗者需要先進行3min左右的靜止準(zhǔn)備，完成MIMU初始對準(zhǔn)。

圖3 設(shè)備安裝示意圖Fig．3 The installation instruction of equipment

3．3 實測導(dǎo)航軌跡結(jié)果

通過對單獨導(dǎo)航得到的行人運動軌跡和協(xié)同導(dǎo)航得到的行人運動軌跡進行比較，分析協(xié)同導(dǎo)航的方法對整體導(dǎo)航結(jié)果的改善。

導(dǎo)航解算結(jié)果如圖4所示，圖中左半部分是3個行人單獨導(dǎo)航的結(jié)果，圖中右半部分是3個行人協(xié)同導(dǎo)航的結(jié)果。其中，第一個行人由于GPS信號可以接收到，在GPS信息的修正下，其單獨導(dǎo)航解算結(jié)果以及協(xié)同導(dǎo)航解算結(jié)果基本與真實軌跡擬合。第二個人和第三個人的GPS信號不可接收到，在只能利用自身MIMU的情況下，導(dǎo)航結(jié)果出現(xiàn)發(fā)散的情況；而進行協(xié)同導(dǎo)航后，導(dǎo)航結(jié)果得到了改善，第二個人和第三個人的運動軌跡基本和真實軌跡擬合。

如圖5所示，通過與GPS測得的數(shù)據(jù)進行比較，計算得到3個行人單獨導(dǎo)航和協(xié)同導(dǎo)航的誤差。其中，CN代表協(xié)同導(dǎo)航，AN代表單獨導(dǎo)航。在導(dǎo)航剛開始時，由于慣導(dǎo)誤差不是特別大，所以協(xié)同導(dǎo)航與單獨導(dǎo)航的誤差基本一致。但是隨著時間推移，在沒有GPS信息下的單獨導(dǎo)航，導(dǎo)航結(jié)果發(fā)散較大，而協(xié)同導(dǎo)航此時的誤差并沒出現(xiàn)較大發(fā)散。因此協(xié)同導(dǎo)航能夠有效地減少行人室內(nèi)導(dǎo)航的漂移誤差，進一步提高在沒有衛(wèi)星條件下行人導(dǎo)航的精度。

圖4 單獨導(dǎo)航與協(xié)同導(dǎo)航軌跡Fig．4 Trajectories of alone navigation and cooperative navigation

圖5 單獨導(dǎo)航與協(xié)同導(dǎo)航誤差Fig．5 Errors of alone navigation and cooperative navigation

4 結(jié)論

本文研究了利用協(xié)同導(dǎo)航的方式對行人導(dǎo)航進行優(yōu)化的方法。針對在GPS信號接收不理想以及MIMU解算結(jié)果發(fā)散的情況，通過對多個行人之間的導(dǎo)航信息進行融合，達到整體導(dǎo)航效果改善的目標(biāo)。利用行人之間較為準(zhǔn)確的距離測量信息，對單個行人的MIMU解算結(jié)果的發(fā)散進行抑制。通過實驗驗證與單個行人獨立導(dǎo)航軌跡進行對比，結(jié)果表明行人協(xié)同導(dǎo)航方法具有更高的導(dǎo)航精度和環(huán)境適應(yīng)能力，進一步提高了行人導(dǎo)航算法的可靠性和實用價值。

［1］穆華.多運動平臺協(xié)同導(dǎo)航的分散式算法研究［D］.國防科學(xué)技術(shù)大學(xué)，2010. MU Hua.Decentralized algorithms of cooperative navi?gation for mobile platforms［D］.National University of De?fense Technology，2010.

［2］張金亮，秦永元，梅春波.基于MEMS慣性技術(shù)的鞋式個人導(dǎo)航系統(tǒng)［J］.中國慣性技術(shù)學(xué)報，2011，19（3)：253?256. ZHANG Jin?liang，QIN Yong?yuan，MEI Chun?bo.Shoe?mounted personal navigation system based on MEMS inertial technology［J］.Journal of Chinese Inertial Tech?nology，2011，19（3)：253?256.

［3］練軍想，唐康華，潘獻飛，等.GNSS與慣性及多傳感器組合導(dǎo)航系統(tǒng)原理（第二版)［M］.北京：國防工業(yè)出版社，2015. LIAN Jun?xiang，TANG Kang?hua，PAN Xian?fei，et al. Principles of GNSS inertial and multisensor integrated nav?igation systems（2nd)［M］.Beijing：National Defense Industry Press，2015.

［4］Nilsson J O，Zachariah D，Skog I，et al.Cooperative lo?calization by dual foot?mounted inertial sensors and inter?agent ranging［J］.Journal on Advances in Signal Process?ing，2013，164（1)：1?17.

［5］王文佳，孫睿智，高偉，等.基于通信延遲誤差補償?shù)膮f(xié)同導(dǎo)航算法［J］.火力與指揮控制，2014，39（12)：27?30. WANG Wen?jia，SUN Rui?zhi，GAO Wei，et al.Coopera?tive navigation based on error compensation of communica?tion delays［J］.Fire Control＆Command Control，2014，39（12)：27?30.

［6］錢偉行，彭晨，田恩剛，等.基于導(dǎo)航信息雙向融合的行人／移動機器人協(xié)同導(dǎo)航方法［J］.中國慣性技術(shù)學(xué)報，2014，22（1)：74?78. QIAN Wei?xing，PENG Chen，TIAN En?gang，et al.Pe?destrian／mobile robot cooperative navigation method based on navigation information bidirectional fusion［J］.Journal of Chinese Inertial Technology，2014，22（1)：74?78.

［7］Frankz K，Robertsonz P，Angermann M.Reliable real?time recognition of motion related human activities using MEMS inertial sensors［C］.Proceedings of the ION GNSS，Potland，2010：2919?2932.

［8］萬駿煒，曾慶化，陳磊江，等.行人慣性導(dǎo)航系統(tǒng)平臺設(shè)計與實現(xiàn)［J］.計算機應(yīng)用與軟件，2015，32（2)：45?47＋170. WAN Jun?wei，ZENG Qing?hua，CHEN Lei?jiang，et al. Design and implementation of pedestrian inertial navigation system［J］.Computer Applications and Software，2015，32（2)：45?47＋170.

The Research of Cooperative Pedestrian Navigation Based on Distance Information

CHEN Ze，WU Mei?ping，CHEN Chang?hao
（College of Mechatronic Engineering and Automation，National University of Defense Technology，Changsha 410073)

In order to solve the problem of GPS information unavailable and the serious divergence of inertial naviga?tion in the indoor pedestrian navigation，the method is proposed to improve the accuracy of the indoor pedestrian navigation by cooperative navigation.By utilizing the relative distance constraint between pedestrians，the divergence of the inertial navigation result is restrained.This paper deduces the model of cooperative pedestrian navigation in detail.The navigation result is worked out by the method of information filter which is more suitable for cooperative pedestrian navigation in the engineering practice by deducing the theory.The paper designs a cooperative pedestrian navigation system，using a micro inertial measurement unit and ultra wideband measurement equipment，collecting and processing the data，analysing out?puts，finding errors，verifying the effectiveness of the algorithm.The experimental results show that the cooperative naviga?tion has the advantage of fixing errors，compared to the alone navigation.

cooperative navigation;pedestrian navigation;distance constraint;information filter

U666.1

A

1674?5558（2017)01?01263

10.3969／j.issn.1674?5558.2017.01.002

陳澤，男，碩士，研究方向為慣性導(dǎo)航。

2016?04?01

國家自然科學(xué)基金（編號：61203200)