單兵自主導航技術發展綜述

， ，

(國防科技大學 智能科學學院，長沙410073)

0 引言

行人導航是指對行人進行導航定位的技術，而單兵導航通常是指面向單兵作戰的這類行人導航技術，要求在未知環境下具有無需提前鋪設基礎設施，僅依靠自身傳感器進行自主導航定位的能力。當前常用的單兵導航系統一般以衛星導航為核心，輔以地圖匹配的形式，在室外信號良好的開闊環境下可以實現高精度的導航定位。但衛星導航在室內、地下、城市巷道等信號不良或缺失的環境下難以完成導航任務，且信號易被干擾，是一種非自主導航方式，限制了單兵導航的應用范圍。

室內定位方法有很多種，裴凌對當前主流室內定位技術的原理和方法進行了詳細分析[1]，其中很多室內定位技術可以應用在行人導航上。根據對導航基礎設施的依賴程度不同，可把室內定位技術分為三類：一是需要鋪設專用的導航設施，如無線電、聲學、光學等定位設施，常見的有室內偽衛星、射頻(Radio Frequency Identification，RFID)、超寬帶(Ultra Wide Band，UWB)、藍牙和ZigBee定位等；二是利用環境現有設備作為導航輔助設施，如WLAN指紋、GSM定位、地理信息輔助定位等；三是無任何基礎設施，完全依靠行人自身傳感器進行的導航，代表方式為基于微慣性測量單元(Micro Inertial Measure Unit, MIMU)的慣性導航，并可輔助以視覺、地磁導航等。在消費級的應用領域如倉庫、商超、機場、展覽館等，具備提前鋪設基礎設施的條件，行人導航可直接采用第一或第二類導航方式。而對于消防救援、搶險救災、反恐作戰等往往面臨的是未知環境，不能提前鋪設基礎設施或者現場設施已被損毀，這就對行人導航的自主性提出了更高的要求，也是當前行人導航面臨的最大挑戰，目前還未形成能完全滿足各種應用需求的成熟解決方案。本文討論的單兵導航針對的是第三種場景，即未知環境下依靠單兵自身攜帶的傳感器進行自主導航的技術。

近年來，微慣性技術的快速發展為單兵自主導航的實現創造了條件。慣性導航是一種完全不依賴任何外部信息的自主導航技術，微慣性測量單元MIMU在成本、尺寸、質量、功耗等方面能夠很好地滿足單兵導航系統的應用需求。但MIMU精度較低，直接用慣性導航算法得到的單兵導航結果會迅速發散，不能完成導航任務。因此，在基于MIMU微慣性導航的基礎上，尋求各種主動或被動的方法來抑制誤差發散提高導航精度，成為當前解決單兵自主導航問題的主要思路。

本文首先闡述單兵自主導航技術的發展過程，然后重點討論基于MIMU的微慣性單兵導航技術的原理和方法，隨后針對微慣性單兵導航存在的問題，分析幾種主流的輔助導航方法和單兵協同導航方法，最后探討單兵自主導航技術的未來發展趨勢。

1 單兵自主導航技術發展簡述

單兵定位的傳統方法是采用地圖、羅盤和計步的方式來實現，直到20世紀90年代GPS出現之后，美軍率先把GPS導航應用到陸軍的單兵定位裝備中，基本解決了單兵的室外高精度定位問題。但在室內、叢林等信號缺失環境下的單兵導航依然需要依賴傳統方法。20世紀90年代中期，美軍為陸地勇士 (Land Warrior)單兵裝備研制了導航系統，首次引入了包含加速度計和磁強計的步行航位推算模塊與GPS組合構成單兵導航系統，在衛星信號不良時可切換為航位推算模式[2]。2006年，美軍在新一代單兵裝備未來勇士 (Future Force Warrior)中升級了器件和步行航位推算(Pedestrian Dead Reckoning，PDR)算法，可以適應更復雜的戰場狀況[3]。PDR算法需要對人體進行運動學建模，根據步頻、腿長等參數來計算步長，并結合計步信息及航向信息推算得到位置。其誤差不隨時間發散，僅與累計的步行路程相關，但不同單兵個體的模型參數差異較大，精度提高有限，有一定應用局限性。

PDR算法中MIMU原則上可以安裝于胸、腰、足等不同部位來進行建模和計步。隨著MIMU精度的進一步提高，單兵導航系統出現另外一種方案，把MIMU安裝于足部，利用腳落地時的零速特點作為導航濾波算法中的虛擬觀測，通過周期性的速度約束，達到比PDR算法更好的精度和適應性，這就是單兵的零速修正(Zero Velocity Update，ZUPT)算法。

20世紀90年代末，Elwell等[4]提出了利用行人步態特點設計ZUPT算法的思路。2005年，美國InterSense公司的Foxlin[5]首次設計了足部安裝的單兵慣導系統，用ZUPT算法取得了較好的實驗結果，并于2009年開發了微導航單兵定位模塊NavShoe,測試精度達到了1%里程，優于PDR水平。同時期，Woodman[6-7]、Ojeda[8]、Godha[9]、Borenstein[10]等也對基于MIMU的行人導航方法進行了深入研究。主流的單兵自主導航研究大多基于足部安裝的MIMU，采用慣性導航(Inertial Navigation System,INS)、擴展Kalman濾波(Extend Kalman Filter,EKF)和ZUPT的算法架構，簡稱INS-EKF-ZUPT算法。理論研究主要集中在兩個方面：一是ZUPT算法中的步態檢測方法；二是針對ZUPT算法中航向角不可觀的問題，尋求不同的方法修正航向角漂移誤差。

國外很多研究團隊在單兵導航領域開展了大量深入細致的研究。瑞典皇家理工大學(KTH)的Skog和Nilsson等自2010年起在行人導航領域進行了系統性研究[11-17]，深入研究了步行導航算法，對多種步態檢測算法和ZUPT算法進行定量分析，設計實現了步行導航模塊OpenShoe,固定在鞋上進行導航，短距離測試精度優于1%里程。同時開展了雙腳MIMU導航、多人協同導航、MIMU傳感器陣列等單兵導航優化方法的研究。加拿大卡爾加里大學(University of Calgary)的導航定位實驗室(PLANS)長期從事行人導航方面的研究[18-21]。從2010年開始，研究了地磁信息輔助方法，采用多傳感器、多MIMU融合方式改善精度，研究了地圖、視覺輔助及場景約束下的多源融合單兵導航算法。美國密歇根大學的Borenstein于2009年提出了利用建筑物方向對單兵航向進行約束的方法[10]。英國諾丁漢大學的Moore教授團隊深入研究了單兵的建筑物航向約束方法，在單兵室內定位、無衛星環境下的導航技術等方面都開展了深入的研究[22-28]。西班牙自動化和機器人研究中心的Jiménez等也在單兵EKF算法、ZUPT算法、單兵航向約束算法等方面進行了深入研究[29-34]。德國航空航天研究中心的Robertson團隊提出了一種新的僅用MIMU數據的即時定位與地圖構建(Simultaneous Localization and Mapping, SLAM)算法，命名為Foot-SLAM算法，能夠粗略生成室內地圖并實現人員的實時室內定位[35-37]。

在國內，北京自動化控制設備研究所、國防科技大學、清華大學、北京理工大學、南京航空航天大學、西北工業大學、東南大學等多個研究團隊近年來也在單兵自主導航領域開展了系統性的研究工作，取得了諸多研究成果[38-45]。

2 基于MIMU的單兵慣性導航方法

基于MIMU的慣性導航是完全自主的導航方法。雖然純慣性導航位置誤差發散很快，但利用慣性器件可以測量單兵運動的特點，把運動規律作為虛擬觀測設計導航濾波算法，同時結合慣性器件的誤差估計和補償方法，可以有效抑制導航誤差發散，一定條件下可以獨立完成單兵導航任務。

2.1 PDR算法

PDR算法的基本思路是通過加速度信息或計步測量裝置對行走時的步數和步長進行估計，結合磁敏感器或陀螺獲得的航向信息逐步推算出行人的位置、行走距離和方向信息，如圖1所示。算法可以分為4個階段：步態探測、步長估計、航向估計和位置推算。

圖1 PDR算法過程示意圖Fig.1 The procedure of PDR algorithm

步態檢測是通過MIMU識別人在行走過程中的步伐狀況，主要包括步頻檢測、步伐判斷。步態檢測主要完成對步行期間內的步態周期進行劃分，輸出一個完整的步態周期的起始和結束時間，用于后續的步長估計和航向估計。步態檢測一般通過陀螺和加速度計輸出數據進行判別，也可以采用支持向量機( Support Vector Machine，SVM)方法對動作進行識別。步態檢測的準確性直接影響到PDR算法的精度。

步長估計是指估算單兵每一步的長度。步長指同一只腳相鄰兩次觸地所邁過的距離，也就是一個步態周期走過的長度。步長估計一般通過步長建模完成。常用的步長模型有偽常數步長模型、線性步長模型、非線性步長模型和基于神經網絡的步長模型等多種。一般步長模型都需要事先標定，模型參數隨使用者的身體特征、步行習慣而存在差異。

航向估計是估算行人的實時航向。步行推算中的航向估計至關重要，航向角的準確性直接關系到最終導航軌跡與實際運動軌跡的差異。航向角的初始對準一般基于磁羅盤完成，步行過程中，航向角依靠陀螺儀和磁羅盤共同維持。

位置推算是基于步長和航向遞推行人的位置坐標。步行推算主要依靠每一步估算出的步長和航向角遞推更新完成，導航精度受步長估計和航向估計的精度影響。定位誤差隨著步行的距離即步數的增加而逐漸累積。

PDR算法實現簡單，安裝應用方便，早期只需要磁羅盤和加速度計即可完成，后來隨著MIMU技術的發展，一般直接用集成的MIMU模塊實現，可安裝于人體腰部、背部、腳上等不同位置。但其步長模型與人體的身體特征和運動習慣等相關性較大，精度有限。

2.2 零速修正算法

1)基本原理

人在步行過程中，腳部會往復出現離地、擺動、觸地、靜止4個階段，且呈周期性變化，如圖2所示。固定在鞋上的MIMU器件能夠敏感到腳部的運動狀態。當步態檢測模塊檢測到腳部處于落地、靜止狀態時，此時固定在腳部的MIMU單元的速度理論上應該為零，因此該階段也被稱作零速階段。把速度為零作為導航濾波算法的虛擬觀測，可以建立Kalman濾波方程，即為ZUPT算法。

圖2 步行周期性變化規律圖Fig.2 The cyclical action of pedestrian step

根據慣性導航的誤差方程建立INS-EKF-ZUPT單兵導航算法，EKF濾波方程可設計不同的濾波狀態。以常用的15狀態濾波器為例，設該模型的系統狀態為導航系統誤差δx，如式(1)所示。

δx=[δφδνδrΔbηb]T

(1)

式中，δφ=[φNφEφD]為姿態角誤差，δν=[δvNδvEδvD]為速度誤差，δr=[rNrErD]為位置誤差，Δb=[ΔbxΔbyΔbz]為加速度計零偏，ηb=[ηbxηbyηbz]為陀螺零偏。

Kalman濾波狀態方程和量測方程分別為：

(2)

z=Hx+v

(3)

系統的觀測量z如下

z=[δνk]=νk-[0,0,0]

(4)

式中，δνk為第k時刻的速度誤差，νk為第k時刻的速度。

則觀測矩陣為

H=[03×3I3×303×303×303×3]

(5)

這就是ZUPT算法的基本原理，而其中零速狀態的檢測是影響算法精度的主要因素之一。

2)零速檢測

零速檢測是步態檢測中的一個核心環節。步態檢測主要包括步頻檢測、步態判斷和零速檢測。當MIMU被固定在鞋上時，內置的加速度計和陀螺儀能夠敏感腳部的變化狀況，實時檢測抬起、邁步、落地、靜止等周期性的動態變化。而加速度和角速度是直接反映腳部動態性的量值，大多數步態檢測算法借助了它們的統計學特征，利用其周期性變化規律完成。基于MIMU的步態檢測算法大致可以分為三類：基于加速度信息的步態檢測、基于角速度信息的步態檢測和融合了加速度和角速度信息的步態檢測算法[46]。這些算法都是基于數據的模值、方差和周期性變化特征，通過設定閾值，判斷當前時刻處于零速時刻(落地、靜止)或抬起邁步時刻，而閾值一般要根據使用者的體態特征和步行習慣事先設定。測試表明，在參數設計得當的條件下，同時融合陀螺和加速度計輸出的角速度和加速度信息的檢測模型精度一般要優于單獨使用角速度或加速度的檢測模型，可靠性更高。

為了提高零速檢測精度，除了用MIMU直接檢測外，也可以通過附加其他傳感器來提高測量精度，如在鞋底增加壓力傳感器，通過測量鞋底與地面的壓力變化來更加精確地判斷腳步落地時刻。Ozkan Bebek[47]在鞋上安裝了一組地面反作用力感應器，通過壓力數據分析行人的步行狀態，進而更加精準地找到了ZUPT時刻，使得校正過程更加及時和準確。田曉春等也通過在鞋底安裝薄膜壓力傳感器的方法提高了零速檢測精度[38]。相比MIMU檢測，壓力傳感器檢測精度更高，但需要在鞋底額外安裝壓力傳感器，增加了系統復雜性和應用難度。

通過零速檢測，可以在慣性導航解算過程中，在每一步的腳落地時刻施加ZUPT算法，對速度誤差、位置誤差和水平姿態角誤差進行修正，提高導航定位精度。但是，ZUPT算法中航向角是不可觀的，意味著算法并不能對航向角漂移進行修正。因此，由于陀螺漂移誤差，單兵航向角誤差逐漸積累，成為單兵導航的主要誤差源。

2.3 運動約束算法

ZUPT算法是單兵導航中最基本的一種運動約束算法，實際上，為了進一步提高精度，單兵的一些其他規律性運動也可以利用來設計為運動約束算法。

類似于零速檢測，在設計運動約束算法之前，首先要準確檢測和識別單兵的運動模式，總結運動規律。單兵的運動模式，除了步頻、步長等步態動作外，還有諸如走、跑、轉彎等行進動作，上下坡、上下樓梯等與環境相關的動作，立、坐、趴和躺等一系列與身體姿態相關的動作等。單兵運動模式識別，是指從MIMU采集到的數據中，處理分析得到當前單兵的動作狀態。基于單兵不同運動模式下的人體運動規律，根據識別出的步態、動作及姿勢等信息研究設計運動學約束條件，利用有效的非完整約束作為虛擬觀測，通過EKF組合導航算法來提高導航精度，并能對導航誤差和器件誤差進行在線估計和修正。常用的運動約束有航向約束和高度約束方法。

1)航向約束

如前所述，零速修正算法并不能對航向角漂移進行修正，考慮到大多數建筑物的走廊是直線的和直角彎的，在室內步行時，對于較長距離的直行和轉彎路線，一般對應的是樓道直線和樓道拐角。通過算法中的運動判斷模塊識別每一步的航向角，基于連續步伐的航向角變化序列判斷單兵的行走狀態如直行和轉彎等，如圖3所示。當判斷狀態為直線行走時，可認為此段路線理想航跡角均值不變；當判斷狀態為轉彎(90°或180°)時，可認為航跡角理想變化量應為90°或180°，把輸出航跡角與理想航跡角的偏差作為觀測量用于Kalman濾波中，起到基于運動特征進行航向約束的作用[10,40]。

圖3 直行/轉彎示意圖Fig.3 The steps of going straight and turning

2)高度約束

慣性導航的高度通道是發散的，如果在室內需要提供樓層信息，常用的方法是利用氣壓高度計輸出作為高度約束。但在一些特殊情況如消防救火、士兵作戰等條件下，現場的大火或爆炸可能瞬間改變氣壓，影響氣壓與高度的關系。此時可以利用上下樓梯的運動規律進行高度和樓層估計。

每一級臺階的高度在建筑規范里大多為定值，而每一步所踏臺階的數量，決定著當前這一步高度更新的大小。因此，動作的準確識別和誤判的及時排除，決定著高度方向上的位置估計精度。固定在鞋上的MIMU能夠有效地感知高度變化引起的加速度和角速度變化，通過動作判斷一步的臺階數量來估算高度變化量。為了減小誤判，可用一系列高度變化量建立高度序列，建立隱藏馬爾科夫模型(Hidden Markov Model, HMM)生成概率最大的動作序列，排除動作誤判后被用于高度誤差修正算法中。谷陽[48]、陳昶昊[40]等基于此思路設計高度約束算法，測試得到較好的樓層判定效果。

2.4 多MIMU信息融合算法

針對MIMU普遍精度不高的問題，可以采用2個或多個MIMU共同導航的方式來提高導航的精度和可靠性。即在人體安裝多個MIMU，每個MIMU都進行導航解算，然后對多個傳感器信息或導航結果進行信息融合和優化，得到更優的導航解。

多MIMU數據融合技術研究可以追蹤到1991年，McMillan為解決水下探測器的長時間穩定導航問題，建立了雙IMU的Kalman濾波模型，進行雙導航系統數據的融合[49]，隨后被廣泛地應用到其他各種設備的導航系統上。2003年，Brand提出將雙MIMU模型應用于行人導航上，利用雙MIMU間的空間距離約束實現了導航誤差的抑制[50]。加拿大卡爾加里大學導航定位實驗室的Bancroft[51]，瑞典皇家理工大學的Nilsson和Skog[52-53]等也分別針對行人導航的背景，對多MIMU數據融合進行了更為細致的研究，并通過實驗論證了多MIMU在行人導航系統中的可行性和有效性。研究成果表明，雙MIMU數據融合可以降低MIMU噪聲，提高導航定位結果的精度和穩定性。國內的孟祥賓[41]、陳昶昊[40]、時偉[54]等對雙腳不等式約束算法也進行了分析和研究。錢偉行[55]還通過多個MIMU在人體下肢分布式安裝，設計了基于人體下肢運動學機理構建的虛擬足部慣性測量組件來實現單兵導航定位的方法。

1)雙腳MIMU距離約束

當2個或多個MIMU安裝于人體不同位置時，任何一個MIMU都可以進行導航解算，得到導航結果，每個導航結果代表的是安裝位置的導航信息。雖然人體不是剛體，不同位置的MIMU之間的距離可能是相對變化的，但人體的高度和四肢伸展范圍是有限的，即不同位置的MIMU之間的距離是有上限的。在多個MIMU導航濾波模型中，加入距離上限約束，可以優化導航結果。其中最常用的是雙腳分別安裝MIMU進行雙腳距離約束，如圖4所示。

圖4 雙腳MIM距離約束關系示意圖Fig.4 The range constraint of two MIMUs

雙腳MIMU距離約束算法是將每個MIMU子系統獨立進行慣性導航解算和零速檢測，再經過ZUPT和距離最大值約束分別對兩MIMU系統間的位置關系、速度和角速率進行觀測，針對雙MIMU系統設計帶不等式約束的EKF算法，對導航解算的結果進行優化。實際上，除了雙腳距離約束外，安裝于身體其他部位如肩部、腰部等的MIMU也可以用于距離約束。

2) MIMU陣列

另外一種多MIMU優化方法是MIMU陣列，即多個MIMU按陣列方式固聯排列虛擬為一個MIMU。多個MIMU誤差相對獨立，可以平滑器件漂移和噪聲，提高數據采樣率和系統可靠性，甚至多個MIMU中的磁場傳感器可以建立一個磁場指紋，從而提高系統性能。瑞典皇家理工大學的Nilsson和Skog[16,56]，國防科技大學的馬明等[57]對此作了分析研究。

3 單兵常用輔助導航方法

為了進一步提高精度和環境適應性，單兵導航可以借助現場一些實時可用的導航信息源，如磁航向、建筑物方向、視覺信息等來輔助導航。地磁和視覺信息可通過單兵隨身攜帶的磁傳感器和視覺傳感器獲取，建筑物方向信息可在現場作為參數提前配置。雖然利用了外部環境信息，但并不需要提前鋪設基礎設施，也不需要在導航過程中接收外部信號，故符合導航自主性要求。同時，作為一種可選的輔助導航手段，在導航過程中僅在條件具備時使用，條件不具備時在算法中可自動失效，不影響導航自主性。

3.1 地磁航向輔助方法

由于MIMU精度較低不能實現自尋北，一般單兵導航直接用地磁傳感器完成尋北和定向，磁航向也可在導航解算中作為航向外部觀測量抑制航向角漂移。由于磁強計存在零偏誤差、標度因數誤差和非正交誤差等，在使用磁強計計算磁航向角之前，需要對磁強計誤差進行校準。對于足部安裝的單兵導航系統，動態環境下磁傳感器的輸出穩定性較差，一般僅在腳部的零速區間內采用磁航向角輔助，以減小動態誤差的影響。地磁信息最主要的問題在于環境的磁場干擾，特別是有些室內環境磁場干擾嚴重，此時可結合慣性傳感器信息對磁航向角的置信度進行評估，當判斷磁航向角干擾較大時，減小磁航向角的約束權值。只要得到可信的磁航向，就可以作為導航濾波算法的外部航向觀測量，起到修正航向漂移的效果。

另外地磁匹配的方法也被用于行人導航，但需要預先測量環境磁場建立磁場分布圖，不適用于未知環境下的自主導航。

3.2 樓向信息航向輔助方法

單兵的活動范圍一般較小，在一個新的未知環境下很難有足夠詳細的數字地圖，特別是室內地圖，故采用地圖匹配導航往往是不現實的。但一些簡單的環境信息如建筑物形狀、方向等信息是現場可立即觀測到的，可被作為導航的先驗信息來輔助導航。2009年，美國密歇根大學的Borenstein提出了一種利用建筑物方向約束的啟發式誤差抑制(Heuristic Drift Reduction, HDR)方法[10]。2011年，英國諾丁漢大學的Abdulrahim基于類似的思路，提出了一種基于EKF框架的建筑物樓向修正方法[22]，西班牙的Jiménez基于EKF算法框架對樓向約束方法進行了進一步的分析和優化，可在室內應用中顯著提高導航定位精度[31]。李楊寰[45]、陳昶昊[40]，張新喜[58]等把樓向約束與運動約束相結合，對航向約束算法做了進一步優化，在單兵室內導航試驗中得到了較好的測試效果。

樓向修正的基本原理是利用已知的建筑物方向信息作為航向的外部觀測量，把樓向信息與運動約束的航向修正算法相結合。考慮到大部分建筑物為規則形狀，走廊是直的，轉彎是直角，行人一般會沿大樓走廊方向行進和轉彎，這就是一種天然的場景約束條件。當判斷行人當前處于直線行走或轉彎時，默認其行走的方向與樓向平行或垂直，此時用樓向信息作為導航濾波算法的航向觀測量，可以修正航向角漂移。樓向按照所處象限劃分，可被劃分為東南西北4個方向區間，兩兩互為90°角關系。也可劃分為更細的區間如8個方向，兩兩互為45°角。當前所處的樓向可以作為航向角場景約束加入濾波算法，修正慣導解算的航向角誤差。

初始樓向信息的獲取是樓向修正算法的前提。簡單的方法是作為先驗知識在導航開始前作為初始參數人工輸入。另一種方法是通過圖像處理算法，在大尺度的數字地圖上僅對建筑物輪廓特征進行特征提取，計算得到建筑物的樓向信息。

樓向信息輔助是一種典型的地理幾何信息輔助導航方法，在室內應用可明顯改善單兵自主導航性能。對于室外場景，則需靈活利用環境特征信息，如道路方向、隧道方向、區域幾何形狀等場景信息來輔助導航，而在人可以任意行走的開闊地帶如操場、草坪等場景則不適用。

3.3 視覺信息輔助導航方法

隨著攝像設備的微小型化，單兵隨身裝備攝像頭在很多條件下是必要的且是容易實現的。而視覺信息在記錄影像的同時，也可以同時用作導航輔助信息。攝像設備可以實時獲取環境影像信息并進行特征提取，基于影像信息設計視覺導航算法對慣性導航進行姿態或位置的輔助觀測，以修正慣導誤差。

20世紀60年代，以圖像跟蹤技術為代表的視覺導航技術就被應用在了軍事領域上。20世紀70年代末，MIT的Marr教授創立了視覺計算理論，從視覺傳感器獲取的圖像中提取有效信息，包括載體的導航信息。如今視覺導航技術已經被廣泛地應用于移動機器人、無人機、交通運輸、農業生產和工業生產等多領域中。

20世紀90年代就出現了將視覺輔助導航技術應用于行人導航背景。Aoki等通過處理接收到的圖片和GPS定位信息，能夠將行人的絕對坐標在室內平面圖直接顯示出來[59]。近年來，視覺陀螺儀的概念也被提了出來，基于視覺傳感器獲得的圖像信息，監測連續圖片上的消影點、消影線變化狀況，由此設計算法估計相機的位姿角。該算法早先被用于機器人、無人機系統，隨后也被改進應用在行人導航上，并取得了較好的成效[60]。Laura等在研究了連續圖像定位技術的基礎上，通過視覺陀螺儀算法有效地估計當前航向變化，有效地抑制了步行導航系統的誤差發散[61]。

當前視覺導航技術發展迅速，針對單兵導航的應用特點，可用的視覺輔助方法主要包括：慣性/立體視覺測姿與定向方法，視覺里程計/慣性組合導航方法、基于圖像特征的場景識別方法及圖像匹配定位方法、基于視覺SLAM的圖節點遞推與閉環優化方法等。隨著視覺傳感器的小型化、精密化，基于視覺輔助的單兵導航定位系統可在不顯著增加導航系統成本的前提下，進一步提升導航定位精度。基于視覺輔助的導航技術正成為國內外研究熱點，其在單兵導航上的應用還需要進一步深入研究。

4 單兵協同導航方法

協同導航需要發送或接收單兵之間的相對觀測信息，嚴格意義上不屬于自主導航。但從單兵分隊整體來看，每個單兵隨身攜帶相對測距設備，內部相互測距并交互信息，而不需要與外界信息交換，也不需要外界其他輔助設施的支持，故整體上符合自主性要求，在單兵分隊導航應用中可以作為一種補充的自主導航手段。

4.1 發展概述

協同導航(Cooperative Navigation, CN)是指多運動體在各自的導航信息基礎上，通過平臺間相互觀測所構成的約束關系,綜合其他平臺的導航信息進行數據融合，實現更優的導航效果。協同導航技術在地面機器人領域取得了良好的導航效果，目前已拓展到水下潛航器、無人機、衛星、導彈等領域。

單兵通常以分隊形式作戰，單兵作戰系統的通信功能使得單兵分隊成為一個通信網絡，為單兵協同導航提供了網絡基礎。通過增加單兵間相對觀測(距離、相對方位等)，實現單兵導航信息共享，是提高單兵作戰系統導航精度的一種理想方式。

2005年，美國InterSense公司的Foxlin在實現ZUPT方法的同時，也提出了利用團隊成員之間的測距信息進行協同導航的思想[5]。之后，一些先進的應急人員定位系統也采用了協同導航方案，美國國土安全部先后資助研發了針對消防員或者警察室內定位的先進定位系統A3DL、針對單兵的定位系統M3DL以及先進消防員定位系統GLANSER。A3DL系統包含足部慣導、GPS接收機和測距模塊，其主要特點是利用小組成員之間、不同小組之間、成員與基站之間的距離測量和通信實現室內精確定位。通過替換部分傳感器(如采用軍用GPS接收機、軍用電臺等)，可以將A3DL系統改造為針對單兵的定位系統M3DL。GLANSER系統提出了利用消防員之間以及消防員和地面基站(消防車等)之間的距離測量進行協同導航的方案。2011年，針對士兵和應急響應人員室內定位問題，瑞典多家機構聯合提出了多傳感器融合的方案，指出協同定位是提高定位精度和適用性的關鍵，圖5所示為城市作戰中利用協同定位的場景[62]。2013年，Nilsson提出了一種分層式多人協同導航算法結構，底層進行ZUPT輔助的慣導解算；上層利用行人間的距離數據，僅對行人的位置和航向狀態進行估計。實驗結果表明，協同導航可以有效抑制微慣性件導航誤差的發散，改善導航精度[14]。錢偉行將行人與移動機器人進行協同導航也得到了較好的效果[63]。

圖5 單兵協同導航系統示意圖[62]Fig.5 The concept of cooperative positioning for soldiers

值得注意的是，A3DL、GLANSER等行人協同導航方案均采用了UWB測距技術。激光雷達、相機等方位測量手段和激光、超聲等距離測量手段要求兩者之間通視(沒有遮擋物)，在室內環境中可應用場景受限較多。Wi-Fi、超高頻等傳統射頻測距技術和UWB新興測距技術可以在非通視條件下進行。與傳統射頻測距技術相比，UWB測距技術具有測量精確、抗干擾能力強、抗多徑誤差能力強、功耗較低等優勢，因此成為目前行人協同導航方案中的主流相對觀測方式。同時，在非通視條件下，UWB測距噪聲呈現明顯的非高斯特征，是協同導航數據融合算法需要重點解決的難點問題。

4.2 單兵協同導航算法

單兵協同導航系統是一個網絡，單兵及其他載體平臺都是網絡中的節點。單兵協同導航是在單節點導航的基礎上，利用網絡節點之間的數據傳輸能力，通過在節點之間增加相對觀測(距離、方位等)來實現的，如圖6所示。單節點導航信息是以MIMU模塊為核心的多源信息融合的結果。分隊成員間導航信息的交互是分隊協同導航的關鍵。

(a)多節點協同導航 (b)單節點導航信息圖6 協同導航系統信息融合示意圖Fig.6 The information fusion of cooperative navigation system

單兵協同導航問題的特點和難點在于參與協同導航的單兵的狀態是相關的。現有的協同導航算法種類各式各樣，其核心算法主要有迭代貝葉斯估計方法(Kalman濾波、信息濾波、粒子濾波等)和優化方法兩類。按照數據融合結構，則可以分為集中式、分層式、分散式算法三類。集中式是將數據直接通過中央處理單元進行處理，簡單直觀但是數據量和運算量巨大，適應能力和可靠性差。分層式是在每一個層級進行處理，最終由中央處理單元進行全局估計，中央處理單元的存在使得系統的可擴展性受到限制并且魯棒性降低。分散式是每個系統中的成員都可以進行信息處理，成員間進行信息交互，是一種不依賴中心節點的結構，具有很好的靈活性和適應性，單個成員故障不會對系統產生影響，但對系統通信和協調能力有較高要求。

Kalman濾波以及各種改進算法在求解協同導航問題中比較常見。采用Kalman濾波要實現最優估計，就要保持對所有運動體的狀態協方差矩陣的更新；為減小計算量或通信量而對協方差矩陣近似就不會得到一致估計。對于非高斯噪聲，如UWB測量誤差，可對測量誤差進行簡單的偏差校正后，再采用Kalman濾波來求解。信息濾波與Kalman濾波在數學上是等價的，在計算上的不同特點使得信息濾波具有分布式特征，因此在分散式協同導航算法設計上得到了應用。在信息濾波中，雖然觀測更新可以利用每個運動體的局部信息完成，但是具有物理意義的均值和方差的計算仍然要用到所有運動體的信息。考慮到Kalman濾波、信息濾波在處理非高斯噪聲上的局限性，可尋求非高斯濾波來求解協同導航問題。如將UWB測量噪聲建模為柯西分布，采用基于確定性采樣的高斯濾波方法融合UWB測量數據[62]。

5 單兵自主導航技術的發展趨勢

經過多年發展，雖然取得了很大的研究進展，但單兵自主導航技術還未形成成熟的解決方案，未知環境下的單兵自主導航問題并未完全解決。當前基于MIMU的單兵導航技術在一定環境條件下可以滿足導航需求，但其導航性能依然受到外部環境條件和單兵運動條件的影響，在復雜條件下的導航普適性和可靠性還有待加強。單兵自主導航技術未來的發展主要集中在以下兩個方向。

一是從根本上大幅度提高微慣性器件和微慣性導航的水平，使得僅用慣性導航就能實現單兵的高精度導航，不再需要其他輔助信息，這是最直接和最根本的解決方法。2010年，美國國防高級研究計劃局(DARPA)啟動了Micro-PNT項目，目標是實現體積在10mm3以內，質量不超過2g，功率控制在1W以內的微型導航定位終端的制造。項目主要面對24h以內的慣性導航需求，用于單兵作戰系統、小型無人機和小型無人潛航器等小型作戰平臺在GPS受限條件下的導航。其中單兵自主導航是其主要目標需求之一。如果微導航系統達到Micro-PNT的性能指標，則只需要MIMU即可滿足單兵導航的性能要求。

二是在僅用慣性導航不能滿足要求時，能更充分靈活地利用多種導航信息，即在單兵自主導航技術的基礎上，拓展研究單兵全源導航方法。2011年，同樣是美國國防高級研究計劃局首次提出一種全新的導航信息融合概念-全源導航(All Source Positioning and Navigation, ASPN)。全源導航是針對衛星受限條件下的導航定位需求提出的，其思路是充分利用導航過程中可能存在的多種導航信息源，設計一套支持導航單元實時配置和即插即用功能的算法架構。如前所述，單兵導航針對未知環境，故只能選用自主導航技術，不能依賴不確定的外部信息。但實際上，單兵面臨的環境復雜多樣，每種環境下都可能已經存在一些非自主但有效的導航信息源，如果單兵導航系統能充分利用這些信息則可以大大提高導航性能。其難點在于不同環境下信息源的不確定性，可能存在不同的信號形式、不同的更新率、不同的定位解算方法，這對信息融合算法提出了很高的自適應要求，而這正是全源導航技術需要解決的問題。相比普通行人導航，單兵導航面臨更復雜的動作模式和行進模式，更復雜多變的外部環境，更多臨時的意外狀態。單兵全源導航技術能在自主導航的基礎上，自適應地利用現場已有的多種導航信息源，是未來進一步提高單兵導航精度、適應性和可靠性的有效途徑。

[1] 裴凌, 劉東輝, 錢久超. 室內定位技術與應用綜述[J]. 導航定位與授時, 2017, 4(3): 1-10.

[2] Marth R B, Levi R, Durboraw I N, et al. The integrated navigation capability for the Force XXI Land Warrior[C]// IEEE Position Location and Navigation Symposium. IEEE, 1998:193-200.

[3] Dorneich M C, Ververs P M, Whitlow S D, et al. Evaluation of a tactile navigation cueing system and real-time assessment of cognitive state[C]// Proceedings of the Human Factors and Ergonomics Society Annual Meeting, 2006: 2600-2604.

[4] Elwell J. Inertial navigation for the urban warrior[C]// Proceedings of the SPIE, 1999: 196-204.

[5] Foxlin E. Pedestrian tracking with shoe-mounted inertial sensors[J]. IEEE Computer Graphics and Applications, 2005, 25(6): 38-46.

[6] Woodman O, Harle R. Pedestrian localisation for indoor environments[C]// Proceedings of the 10thInternational Conference on Ubiquitous Computing, 2008: 114-123.

[7] Woodman O. Pedestrian localisation for indoor environments[D]. University of Cambridge, 2010.

[8] Ojeda L, Borenstein J. Non-GPS navigation for security personnel and first responders[J]. Journal of Navigation, 2007, 60(3): 391.

[9] Godha S, Lachapelle G. Foot mounted inertial system for pedestrian navigation[J]. Measurement Science and Technology, 2008, 19(7): 075202.

[10] Borenstein J, Ojeda L, Kwanmuang S. Heuristic reduction of gyro drift for personnel tracking systems[J]. Journal of Navigation, 2009, 62(1): 41-58.

[11] 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.

[12] Colomar D S, Nilsson J O, H?ndel P. Smoothing for ZUPT-aided INSs[C]// 2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2012: 1-5.

[13] Nilsson J O, Skog I, H?ndel P, et al. Foot-mounted INS for everybody-an open-source embedded implementation[C]// 2012 IEEE/ION Position Location and Navigation Symposium (PLANS). IEEE, 2012: 140-145.

[14] Nilsson J O, Zachariah D, Skog I, et al. Cooperative localization by dual foot-mounted inertial sensors and inter-agent ranging[J]. EURASIP Journal on Advances in Signal Processing, 2013, 2013(1): 164.

[15] Nilsson J O, Skog I, H?ndel P. Aligning the forces—Eliminating the misalignments in IMU arrays[J]. IEEE Transactions on Instrumentation and Measurement, 2014, 63(10): 2498-2500.

[16] Skog I, Nilsson J O, H?ndel P, et al. Inertial sensor arrays, maximum likelihood, and cramér-rao bound[J]. IEEE Transactions on Signal Processing, 2016, 64(16): 4218-4227.

[17] Pasku V, De Angelis A, Moschitta A, et al. A magnetic ranging-aided dead-reckoning positioning system for pedestrian applications[J]. IEEE Transactions on Instrumentation and Measurement, 2017, 66(5): 953-963.

[18] Du S. Integration of precise point positioning and low cost MEMS IMU[D]. University of Calgary, 2010.

[19] Afzal M H. Use of Earth's magnetic field for pedestrian navigation[M]. Lap Lambert Academic Publishing, 2011.

[20] Saeedi S. Context-aware personal navigation services using multi-level sensor fusion algorithms[D]. University of Calgary, 2013.

[21] Dhital A. Reliability improvement of sensors used in personal navigation devices[D]. University of Calgary, 2015.

[22] Abdulrahim K, Hide C, Moore T, et al. Using constraints for shoe mounted indoor pedestrian navigation[J]. Journal of Navigation, 2011, 65(1): 15-28.

[23] Abdulrahim K, Hide C, Moore T, et al. Increased error observability of an inertial pedestrian navigation system by rotating IMU[J]. Journal of Engineering and Technological Sciences, 2014, 46(2): 211-225.

[24] Jing H, Bonenberg L K, Pinchin J, et al. Detection of UWB ranging measurement quality for collaborative indoor positioning[J]. Journal of Location Based Services, 2015, 9(4): 296-319.

[25] Kealy A, Retscher G, Toth C, et al. Collaborative navigation as a solution for PNT applications in GNSS challenged environments-report on field trials of a joint FIG/IAG working group[J]. Journal of Applied Geodesy, 2015, 9(4): 244-263.

[26] Basiri A, Amirian P, Winstanley A, et al. Seamless pedestrian positioning and navigation using landmarks[J]. Journal of Navigation, 2016, 69(1): 24-40.

[27] Jing H, Pinchin J, Hill C, et al. An adaptive weighting based on modified DOP for collaborative indoor positioning[J]. Journal of Navigation, 2016, 69(2): 225-245.

[28] Peltola P, Hill C, Moore T. Particle filter for context sensitive indoor pedestrian navigation[C]// 2016 International Conference on Localization and GNSS (ICL-GNSS). IEEE, 2016: 1-6.

[29] Jimenez A R, Seco F, Prieto C, et al. A comparison of pedestrian dead-reckoning algorithms using a low-cost MEMS IMU[C]// IEEE International Symposium on Intelligent Signal Processing. IEEE, 2009: 37-42.

[30] Jiménez A R, Seco F, Prieto J C, et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU[C]// 2010 7thWorkshop on Positioning Navigation and Communication (WPNC). IEEE, 2010: 135-143.

[31] Jiménez A R, Seco F, Zampella F, et al. Improved Heuristic Drift Elimination (iHDE) for pedestrian navigation in complex buildings[C]// 2011 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2011: 1-8.

[32] Jiménez A R, Seco F, Zampella F, et al. Improved heuristic drift elimination with magnetically-aided dominant directions (MiHDE) for pedestrian navigation in complex buildings[J]. Journal of Location Based Services, 2012, 6(3): 186-210.

[33] Jimenez A R, Zampella F, Seco F. Improving inertial pedestrian dead-reckoning by detecting unmodified switched-on lamps in buildings[J]. Sensors (Basel), 2014, 14(1): 731-769.

[34] Jiménez A, Seco F. Comparing Decawave and Bespoon UWB location systems: Indoor/outdoor performance analysis[C]// 2016 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2016: 1-8.

[35] Robertson P, Angermann M, Khider M. Improving simultaneous localization and mapping for pedestrian navigation and automatic mapping of buildings by using online human-based feature labeling[C]// 2010 IEEE/ION Position Location and Navigation Symposium (PLANS). IEEE, 2010: 365-374.

[36] Robertson P, Puyol M G, Angermann M. Collaborative pedestrian mapping of buildings using inertial sensors and FootSLAM[C]// ION GNSS DLR, 2011:1366-1377.

[37] Puyol M G, Bobkov D, Robertson P, et al. Pedestrian simultaneous localization and mapping in multistory buildings using inertial sensors[J]. IEEE Transactions on Intelligent Transportation Systems, 2014, 15(4): 1714-1727.

[38] 田曉春, 陳家斌, 尚劍宇, 等. 基于自包含傳感器的單兵導航系統設計[J]. 導航定位與授時, 2017, 4(5): 54-59.

[39] 張新喜, 張嶸, 郭美鳳, 等. 基于足綁式 INS 的行人導航三軸磁強計在線校準[J]. 清華大學學報 (自然科學版), 2016, 56(2): 211-217.

[40] 陳昶昊. 單兵自主導航算法優化及誤差修正方法研究[D]. 長沙: 國防科技大學, 2016.

[41] 孟祥賓. 基于運動/場景約束的單兵導航算法研究[D]. 長沙: 國防科技大學, 2015.

[42] 徐元, 陳熙源, 王宜敏, 等. 一種采用足部航姿參考和肩部航向參考的室內個人導航方法[J]. 中國慣性技術學報, 2016, 24(3): 325-329.

[43] Zhang X, Zeng Q, Meng Q, et al. Implementation of smartphone seamless positioning system based on mobile navigation electronic map[C]// 2016 4thInternational Conference on Ubiquitous Positioning, Indoor Navigation and Location Based Services (UPINLBS). IEEE, 2016: 89-93.

[44] Xu Y, Chen X, Cheng J, et al. Improving tightly-coupled model for indoor pedestrian navigation using foot-mounted IMU and UWB measurements[C]// 2016 IEEE International Instrumentation and Measurement Technology Conference Proceedings (I2MTC). IEEE, 2016: 1-5.

[45] Li Y-H, Song Q, Ma M, et al. Near Real Time Heading Drift Correction for indoor pedestrian tracking based on sequence detection[C]// 2016 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2016: 1-4.

[46] Chen C, Chen Z, Pan X, et al. Assessment of zero-velocity detectors for pedestrian navigation system using MIMU[C]// 2016 IEEE Chinese Guidance, Navigation and Control Conference (CGNCC). IEEE, 2016: 128-132.

[47] Bebek ?, Suster M A, Rajgopal S, et al. Personal navigation via high-resolution gait-corrected inertial measurement units[J]. IEEE Transactions on Instrumentation and Measurement, 2010, 59(11): 3018-3027.

[48] Gu Y, Ma M, Li Y-H, et al. Accurate height estimation based on apriori knowledge of buildings[C]// 2013 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2013: 1-7.

[49] Mcmillan J C, Ramotaur R. Development environment for DIINS (Dual Inertial Integrated Navigation System)[R]. Defence Research Establishment Ottawa(ONTARIO), 1991.

[50] Brand T J, Phillips R E. Foot-to-foot range measurement as an aid to personal navigation[C]// Proceedings of the 59thAnnual Meeting of the Institute of Navigation and CIGTF 22ndGuidance Test Symposium. 2001: 113-121.

[51] Bancroft J B, Lachapelle G, Cannon M E, et al. Twin IMU-HSGPS integration for pedestrian navigation[C]// ION GNSS, 2008: 1377-1387.

[52] Skog I, Nilsson J O, Zachariah D, et al. Fusing the information from two navigation systems using an upper bound on their maximum spatial separation[C]// 2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2012: 1-5.

[53] Prateek G V, Girisha R, Hari K, et al. Data fusion of dual foot-mounted INS to reduce the systematic heading drift[C]// 2013 4thInternational Conference on Intelligent Systems Modelling & Simulation (ISMS). IEEE, 2013: 208-213.

[54] 時偉, 王陽. 基于不等式約束卡爾曼濾波的雙 MIMU 導航位置校正方法[J]. 中國慣性技術學報, 2017, 25(1): 11-16.

[55] 錢偉行, 曾慶化, 萬駿煒, 等. 基于人體下肢運動學機理的行人導航方法[J]. 中國慣性技術學報, 2015, 23(1):24-28.

[56] Nilsson J O, Skog I. Inertial sensor arrays—A literature review[C]// 2016 European Navigation Conference (ENC). IEEE, 2016: 1-10.

[57] Ma M, Song Q, Li Y H, et al. A heading error estimation approach based on improved quasi-static magnetic field detection[C]//2016 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2016: 1-8.

[58] 張新喜, 張嶸, 郭美鳳, 等. 足綁式行人導航偏航角誤差自觀測算法[J]. 中國慣性技術學報, 2015, 23(4): 457-466.

[59] Aoki H, Schiele B, Pentland A. Realtime personal positioning system for a wearable computer[C]// The 3rdInternational Symposium on Wearable Computers, Digest of Papers. IEEE, 1999: 37-43.

[60] Xu C, Zhang L, Cheng L, et al. Pose estimation from line correspondences: A complete analysis and a series of solutions[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2017, 39(6): 1209-1222.

[61] Laura R, Heidi K, Ruizhi C, et al. Heading change detection for indoor navigation with a Smartphone camera[C]// 2011 International Conference on Indoor Positioning and Indoor Navigation(IPIN). IEEE, 2011: 1-7.

[62] Rantakokko J, Rydell J, Str?mb?ck P, et al. Accurate and reliable soldier and first responder indoor positioning: multisensor systems and cooperative localization[J]. IEEE Wireless Communications, 2011, 18(2): 10-18.

[63] 錢偉行, 彭晨, 田恩剛, 等. 基于導航信息雙向融合的行人/移動機器人協同導航方法[J]. 中國慣性技術學報, 2014, 22(1): 74-78.