穆 華，潘獻飛，吳美平，胡小平

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

0 引言

單兵等作戰(zhàn)人員通常在未知環(huán)境中執(zhí)行任務(wù)，用于定位的外界信息(地圖、衛(wèi)星等)可用性受限，單兵如何自主定位是近幾年研究的熱點和難點問題?；谧悴课T性測量單元(Micro Inertial Measurement Unit，MIMU)和單兵間測距的協(xié)同導航技術(shù)是一種解決衛(wèi)星信號受限環(huán)境下單兵自主導航難題的有效途徑。

通過在人體上固聯(lián)MIMU進行慣性導航解算是一種新的單兵導航技術(shù)。該技術(shù)具有隱蔽性、抗干擾性和自主性等優(yōu)勢，具有應用于單兵自主導航系統(tǒng)的潛力。1999年，Elwell首先提出了零速修正(Zero Velocity Update，ZUPT)算法[1]。美國InterSense公司的Foxlin 于2005年首次設(shè)計了足部安裝MIMU的單兵慣導系統(tǒng)[2]，并于2009年開發(fā)了單兵定位模塊NavShoe，測試精度達到1%里程。之后主流的單兵自主導航研究大多基于足部安裝的MIMU，針對ZUPT算法中航向角不可觀的問題，尋求不同的方法修正航向角漂移誤差。一種方法是引入磁羅盤[2]，但是磁羅盤容易受到環(huán)境磁場的干擾；另外一種方法是引入位置參考，如全球定位系統(tǒng)(Global Positioning System，GPS)[3]、建筑物平面圖[4]、事先布設(shè)的電子標簽(Radio Frequen-cy Identification，RFID)[5]以及單兵之間的距離測量[2，6-7]等。在上述手段中，引入單兵間距離測量即在多個單兵間進行協(xié)同導航(Cooperative Navigation，CN)。這種手段可以在多人之間實現(xiàn)導航資源共享，并且不需要事先布設(shè)設(shè)施，也不依賴環(huán)境特征，自主性更好[8]。

2005年 Foxlin提出了單兵協(xié)同導航的思想[2]。之后，一些先進的應急人員定位系統(tǒng)也采用了協(xié)同導航方案，如美國國土安全部資助研發(fā)的先進消防員定位系統(tǒng)GLANSER[9]，以及瑞典相關(guān)機構(gòu)提出的單兵協(xié)同導航系統(tǒng)[6]等。值得注意的是，上述單兵協(xié)同導航方案均采用了新型超寬帶(Ultra-Wideband，UWB)測距技術(shù)。這是由于與傳統(tǒng)射頻測距技術(shù)相比，UWB測距技術(shù)具有測量精確、抗干擾能力強、抗多徑誤差能力強、功耗較低等優(yōu)勢；同時與激光、超聲等距離測量手段相比，UWB測距技術(shù)具有較強的穿透能力。

協(xié)同導航系統(tǒng)是一種分布式系統(tǒng)，協(xié)同導航算法的設(shè)計難點之一在于如何建立合適的協(xié)同導航系統(tǒng)模型，盡量降低系統(tǒng)的通信需求。復雜的通信需求會制約系統(tǒng)的可實現(xiàn)性和可靠性。文獻[10]僅以位置作為狀態(tài)變量，建立協(xié)同導航系統(tǒng)的運動方程，將位置的估計結(jié)果反饋到各單兵進行慣性導航系統(tǒng)誤差修正；實驗數(shù)據(jù)處理結(jié)果驗證了算法的有效性。文獻[7]在足部MIMU模塊采用步進式慣導解算，以位置和航向角作為狀態(tài)變量，建立協(xié)同導航系統(tǒng)的運動方程，無需反饋修正；在模擬UWB測距數(shù)據(jù)的條件下對該算法進行了驗證。

為了促進單兵協(xié)同導航系統(tǒng)的實際應用，本文設(shè)計了一種基于步進式運動模型的協(xié)同導航算法。與文獻[7]不同，本文足部模塊仍然采用一般的ZUPT輔助的慣導解算，而無需采用步進式慣導解算。本文和文獻[10]采用不同的協(xié)同導航系統(tǒng)模型，分析了同一組協(xié)同導航試驗數(shù)據(jù)，試驗結(jié)果可以互相印證各自算法的正確性。文獻[10]僅給出了一種參數(shù)設(shè)置下的導航結(jié)果，而本文分析了不同條件下的導航結(jié)果，更全面地展示了協(xié)同導航在提高定位精度方面的表現(xiàn)。

1 單兵協(xié)同導航系統(tǒng)組成

單兵協(xié)同導航系統(tǒng)是一個包含若干個單兵節(jié)點的網(wǎng)絡(luò)，每個單兵節(jié)點的基本導航設(shè)備包含足部MIMU模塊、進行節(jié)點間距離測量的UWB模塊、進行數(shù)據(jù)收集及計算的處理板，以及在單兵間進行數(shù)據(jù)傳輸?shù)臒o線數(shù)傳模塊。

足部MIMU模塊的傳感器包括三軸陀螺、三軸加表、磁力儀和氣壓高度計，進行基于ZUPT的慣導解算，具有獨立的導航定位能力。

本文選用的UWB模塊采用雙向飛行時間法進行高精度距離測量，模塊A向模塊B發(fā)送測距請求信號，模塊B接收并處理后發(fā)送應答信號?？梢姡瑴y距結(jié)果由測距發(fā)起者得到。對于無線電波，天線越高對傳播越有利。因此，UWB模塊安裝在頭部或肩部。

單兵之間采用900MHz頻段的無線數(shù)傳模塊。這個頻段無線電波的穿透能力可以輕松地在室內(nèi)/城市環(huán)境下實現(xiàn)數(shù)百米遠距離傳輸，數(shù)據(jù)速率可達10kbit/s甚至更高。

處理板收集足部慣導解算信息和UWB測距信息，并與無線數(shù)傳模塊之間進行數(shù)據(jù)傳輸。足部慣導解算信息通過藍牙發(fā)送給處理板，UWB信息通過有線連接發(fā)送給處理板，處理板和無線數(shù)傳模塊之間通過Wi-Fi互連。

本文研究的單兵協(xié)同導航系統(tǒng)最小組成如圖1所示。除了上述基本導航設(shè)備外，單兵的其他導航設(shè)備如衛(wèi)星接收機、視覺里程計等都可以納入?yún)f(xié)同導航系統(tǒng)中。

圖1 單兵協(xié)同導航系統(tǒng)硬件配置示意圖Fig.1 Hardware configuration diagram of the cooperative navigation system for soldiers

2 單兵協(xié)同導航算法設(shè)計

單兵協(xié)同導航算法設(shè)計的核心在于單兵運動建模，運動模型決定了是否需要對足部慣導解算進行反饋修正，同時運動模型的維數(shù)直接影響協(xié)同導航系統(tǒng)的通信量和計算量。本文建立了單兵的步進式運動模型，僅以單兵位置和航向角為狀態(tài)變量，且無需對慣導解算進行反饋修正。

2.1 協(xié)同導航系統(tǒng)模型

1)建模思路

足部MIMU模塊采用ZUPT輔助的慣性導航算法。在單兵行走的每一步，存在一個腳部靜止的零速階段。通過零速檢測算法檢測零速階段，并把零速作為虛擬觀測，可以修正足部慣導解算的速度誤差和水平姿態(tài)角誤差，但是不能有效修正航向角誤差。航向角誤差累積會影響定位精度。

以一段連續(xù)ZUPT結(jié)束時刻為一步的開始時刻，此時，足部MIMU的水平姿態(tài)角誤差很小。再加上初始速度為0，所以一步的定位誤差主要由起始時刻的航向誤差引起。對于每一步的慣導解算，可以利用一個中間導航坐標系，將初始航向誤差從導航結(jié)果中分離出來，得到誤差不累積的位移增量和航向增量信息。

以單兵位置和航向為狀態(tài)變量，以單兵每步的位移增量和航向增量為系統(tǒng)輸入，建立單兵的步進式運動模型。所有單兵的步進式運動模型組合成協(xié)同導航系統(tǒng)的狀態(tài)方程，結(jié)合單兵間距離觀測方程，建立協(xié)同導航系統(tǒng)模型。

2)一步慣性導航解算

取北東地坐標系為導航坐標系，并且定義姿態(tài)矩陣

(1)

其中，Mi[·](i=1,2,3)為初等轉(zhuǎn)換矩陣；γ、θ、ψ分別為滾動角、俯仰角和方位角。

考慮一步行走過程，設(shè)一步起始時刻MIMU的姿態(tài)角為(γ0,θ0,ψ0)，一步包含L次慣導解算，第l(l=1,2,…,L)次姿態(tài)解算的結(jié)果為

(2)

(3)

由于一步起始時刻的速度為0，第l次速度解算的結(jié)果為

(4)

第l次位移增量的結(jié)果為

(5)

由于

(6)

并且M3[ψ0]gn=gn，因此在式(3)～式(5)兩端左乘M3[ψ0]，得

(7)

(8)

(9)

式(7)～式(9)相當于在一個中間導航坐標系n′中的慣性導航計算過程，這個坐標系與導航坐標系n之間的關(guān)系為

(10)

在中間導航坐標系n′中獲取的位移增量和航向增量與初始航向ψ0無關(guān)。由式(9)，一步結(jié)束時(l=L)在中間導航坐標系n′和導航坐標系n中位移增量的關(guān)系為

(11)

3)步進式運動模型

利用n′系中一步位移增量dp′s，可得n系下的步進式運動模型

(12)

其中，變量xs表示第s步結(jié)束時MIMU在n系下的位置；變量χs表示第s步結(jié)束時MIMU的航向角；ws表示第s步的位移增量誤差和航向增量誤差，其方差根據(jù)第s步慣性導航解算結(jié)果計算。

Xs=f(Xs-1,us)+ws

(13)

4)協(xié)同導航系統(tǒng)狀態(tài)模型

設(shè)N個單兵進行協(xié)同導航,協(xié)同導航系統(tǒng)的狀態(tài)方程由N個單兵的步進式運動模型組成

(14)

其中，上標i表示單兵編號，下標si表示單兵i的第si步(i=1,2,…,N)。注意，除了齊步行進，相同時刻不同單兵的步數(shù)通常是不一樣的。

協(xié)同導航系統(tǒng)的觀測方程為

(15)

式(14)和式(15)組成協(xié)同導航系統(tǒng)模型。距離觀測使得各單兵導航狀態(tài)不再獨立，而是成為了相關(guān)變量。

2.2 步進式協(xié)同導航算法

建立協(xié)同導航系統(tǒng)模型之后，采用Kalman濾波進行協(xié)同導航計算，信息流程如圖2所示。

圖2 步進式協(xié)同導航算法信息流程圖Fig.2 Information flow chart of the cooperative navigation algorithm based on stepwise localization

考慮將某個單兵作為協(xié)同中心，涉及的通信如下：

1)足部模塊將每步增量及方差陣發(fā)送給處理板，再通過無線數(shù)傳模塊發(fā)送到協(xié)同中心的處理板，發(fā)送頻率為每步1次，發(fā)送數(shù)據(jù)為一個4維向量和一個4×4的對稱矩陣；

2)測距數(shù)據(jù)由協(xié)同中心的UWB模塊收集，并傳輸?shù)絽f(xié)同中心的處理板。

2.3 協(xié)同導航算法比較

比較本文、文獻[7]以及文獻[10](算法信息流程見圖3)三種算法。從算法結(jié)構(gòu)上看，文獻[10]的算法是帶反饋修正的閉環(huán)結(jié)構(gòu)，本文和文獻[7]則是開環(huán)結(jié)構(gòu)。

圖3 反饋式協(xié)同導航算法信息流程圖Fig.3 Information flow chart of the cooperative navigation algorithm with feedback

本文算法與文獻[7]中的算法相比，通信量是一樣的，區(qū)別在于本文算法中足部模塊并不進行步進式慣導解算，協(xié)同導航中心步進式運動模型的輸入(每步在n′坐標系的位移增量和航向增量)是根據(jù)足部慣導解算結(jié)果轉(zhuǎn)換得到的。

可見，本文步進式協(xié)同導航算法的優(yōu)點是無需改變足部慣導算法，且無需進行反饋修正，這對系統(tǒng)模塊化設(shè)計及系統(tǒng)工程實現(xiàn)具有重要意義。

3 單兵協(xié)同導航試驗分析

3.1 試驗條件

3名試驗人員，分別標記為100、101和102，在校園馬路上開展協(xié)同導航試驗。3人行走在一條馬路的兩側(cè)，100走外側(cè)，101和102走內(nèi)側(cè)，且101在102的前面。100和102行走時長約1400s，行走1圈回到各自的起點；101行走時長約1000s，中途退出協(xié)同導航。每人足部安裝MIMU模塊，肩部安裝UWB模塊和衛(wèi)星接收機。MIMU模塊中慣性測量單元的主要性能指標如表1所示。試驗人員101的差分GPS軌跡見圖4，圖中紅點是GPS輸出的定位結(jié)果，藍線是沒有GPS輸出的地方擬合的路線。

表1 MIMU的性能指標

圖4 試驗人員101的差分GPS軌跡Fig.4 Pedestrian 101 trajectories given by differential GPS

3.2 定位精度分析

對試驗數(shù)據(jù)進行事后處理，對比分析不同條件下獨立導航和協(xié)同導航的定位精度。

1)足部慣導模塊定位結(jié)果

足部慣導模塊進行ZUPT輔助的慣導解算，可以獨立給出單兵軌跡。慣導軌跡精度受兩方面的影響，一是初始對準的航向精度，二是ZUPT參數(shù)(如零速檢測閾值[11]和零速觀測噪聲方差)。采用三軸磁力儀確定初始對準的航向，其精度取決于磁場環(huán)境以及磁力儀的磁干擾補償精度。ZUPT參數(shù)對軌跡形狀影響較大。

參數(shù)設(shè)置一：

在足部定位算法中，零速檢測算法和器件噪聲參數(shù)設(shè)置是相同的。在進行事后處理時，通過調(diào)整ZUPT參數(shù)和初始航向，可以使解算軌跡更接近參考軌跡。采用表2所示ZUPT參數(shù)，得到軌跡如圖5所示。

表2 ZUPT參數(shù)

圖5 3人足部慣導模塊解算軌跡(參數(shù)設(shè)置一)Fig.5 The trajectories of three pedestrians given by the foot-mounted MIMU(Parameter setting 1)

可見，100和102的慣導解算定位精度較高，101的航向在行走早期階段出現(xiàn)了較大偏差，但是軌跡形狀并不失真。

參數(shù)設(shè)置二：

對于在線實現(xiàn)，無法保證初始對準的航向精度及采用最優(yōu)的ZUPT參數(shù)，此時，足部慣導模塊定位精度會降低。當3人都采用100的ZUPT參數(shù)設(shè)置，并且100采用磁力儀確定的初始航向(與參數(shù)設(shè)置一中的航向相差5°)，得到軌跡如圖6所示。此時，101的定位結(jié)果沒有顯著變化，而102的定位精度顯著降低。

圖6 3人足部慣導模塊解算軌跡(參數(shù)設(shè)置二)Fig.6 The trajectories of three pedestrians given by the foot-mounted MIMU(Parameter setting 2)

2)無GPS條件下的協(xié)同導航結(jié)果

分別采用參數(shù)設(shè)置一和參數(shù)設(shè)置二，僅利用足部慣導模塊和UWB測距模塊，采用本文算法進行協(xié)同導航，定位軌跡分別如圖7和圖8所示。

圖7 3人協(xié)同導航軌跡(參數(shù)設(shè)置一，無GPS)Fig.7 Cooperative navigation trajectories of three pedestrians (Parameter setting 1 without GPS)

圖8 3人協(xié)同導航軌跡(參數(shù)設(shè)置二，無GPS)Fig.8 Cooperative navigation trajectories of three pedestrians (Parameter setting 2 without GPS)

對于參數(shù)設(shè)置一，協(xié)同后，101的航向偏差得到了有效修正，3人的定位精度都有提高。

對于參數(shù)設(shè)置二，在前期3人協(xié)同階段，3人協(xié)同定位的精度都有提升；而在后期僅有100和102協(xié)同的階段，102的自定位誤差影響了協(xié)同導航的效果，在102定位誤差降低的同時，100的定位誤差反而有所增加。

3)有GPS條件下的協(xié)同導航結(jié)果

對于參數(shù)設(shè)置二，將100在3個時刻的GPS觀測數(shù)據(jù)(見表3)加入?yún)f(xié)同導航，定位軌跡如圖9所示。可見，單個試驗人員少量的GPS位置修正可以有效提高每個人員的定位精度。

表3 參與協(xié)同導航的GPS觀測數(shù)據(jù)

圖9 3人協(xié)同導航軌跡(參數(shù)設(shè)置二，帶GPS)Fig.9 Cooperative navigation trajectories of three pedestrians (Parameter setting 2 with GPS)

4)不同試驗條件下的定位精度

為了定量評估不同試驗條件下的定位精度，以差分GPS輸出的定位結(jié)果為參考值，按照式(16)計算不同試驗條件下的均方根誤差(Root Mean Square Error,RMSE)

(16)

表4 不同試驗條件的定位精度

4 結(jié)論

本文利用足部慣性導航解算每步步長和航向增量誤差不累積的特點，建立了單兵的步進式運動模型，由此設(shè)計的協(xié)同導航算法無需反饋修正，具有模塊化和低通信量的優(yōu)勢，易于工程實現(xiàn)。對一次3人協(xié)同導航的試驗數(shù)據(jù)進行了詳細分析，結(jié)果表明：

1)在每個成員初始對準誤差小、ZUPT參數(shù)優(yōu)化的情況下，僅利用測距的協(xié)同導航可以使每個成員都獲得高精度的定位結(jié)果；

2)當初始對準和ZUPT無法確保最優(yōu)時，僅利用測距的協(xié)同導航可以提高整體的平均定位精度，但不一定能提高每個成員的定位精度，增加單個成員少量的衛(wèi)星觀測則可以顯著提高每個成員的定位精度。

單兵協(xié)同導航技術(shù)未來的發(fā)展主要集中在兩個方向：一是提高足部慣導模塊的導航性能，這一方面可以通過提高器件精度來實現(xiàn)，另一方面可以通過引入學習算法實現(xiàn)零速檢測參數(shù)的在線優(yōu)化；二是充分利用實際環(huán)境中可能存在的多種導航信息源，將UMB基站、衛(wèi)星接收機、視覺/慣性組合導航系統(tǒng)、隨機信號導航系統(tǒng)等納入?yún)f(xié)同導航系統(tǒng)中，以提高協(xié)同導航系統(tǒng)的可靠性和可用性。