基于距離信息的行人協同導航研究

陳 澤，吳美平，陳昶昊

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

基于距離信息的行人協同導航研究

陳 澤，吳美平，陳昶昊

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

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

協同導航；行人導航；距離約束；信息濾波

0 引言

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

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

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

1 行人協同導航模型

行人運動模型用于描述在導航坐標系下行人的位移、速度等運動學參數之間的關系。本文重點研究多行人協同系統的導航性能，在單個行人運動模型的基礎上，建立多行人協同導航模型。

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

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

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

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

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

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

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

多行人協同導航的狀態矢量由全部行人的狀態矢量組成，設k時刻的系統狀態矢量為X（k)。由N個行人組成的系統狀態矢量、輸入矢量和系統噪聲為：

則系統的狀態方程為：

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

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

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

其中，

類似地，對由N個行人組成的協同導航系統，k時刻第i個行人對第j個行人位置信息線性化觀測方程為：

其中，

通過以上推導得到線性化系統模型為：

2 協同導航信息濾波算法

k時刻狀態估計值為Xe（k)，狀態預測值為Xp（k)，狀態協方差矩陣估計值為Pe（k)，狀態協方差矩陣預測值為Pp（k)，則：

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

信息狀態和信息矩陣的遞推方程集可直接由Kalman濾波方程獲得。在數學上信息濾波與常規的Kalman濾波結果是一致的。

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

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

信息濾波更新過程為：

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

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

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

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

3 實驗驗證

3．1 硬件介紹

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

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

3．2 行人協同導航實驗

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

圖2 實驗路線Fig．2 Experiment route

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

圖3 設備安裝示意圖Fig．3 The installation instruction of equipment

3．3 實測導航軌跡結果

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

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

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

圖4 單獨導航與協同導航軌跡Fig．4 Trajectories of alone navigation and cooperative navigation

圖5 單獨導航與協同導航誤差Fig．5 Errors of alone navigation and cooperative navigation

4 結論

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

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

［2］張金亮，秦永元，梅春波.基于MEMS慣性技術的鞋式個人導航系統［J］.中國慣性技術學報，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與慣性及多傳感器組合導航系統原理（第二版)［M］.北京：國防工業出版社，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］王文佳，孫睿智，高偉，等.基于通信延遲誤差補償的協同導航算法［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］錢偉行，彭晨，田恩剛，等.基于導航信息雙向融合的行人／移動機器人協同導航方法［J］.中國慣性技術學報，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］萬駿煒，曾慶化，陳磊江，等.行人慣性導航系統平臺設計與實現［J］.計算機應用與軟件，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

陳澤，男，碩士，研究方向為慣性導航。

2016?04?01

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