基于步進式運動模型的單兵協同導航算法設計

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

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

0 引言

單兵等作戰人員通常在未知環境中執行任務，用于定位的外界信息(地圖、衛星等)可用性受限，單兵如何自主定位是近幾年研究的熱點和難點問題。基于足部微慣性測量單元(Micro Inertial Measurement Unit，MIMU)和單兵間測距的協同導航技術是一種解決衛星信號受限環境下單兵自主導航難題的有效途徑。

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

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

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

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

1 單兵協同導航系統組成

單兵協同導航系統是一個包含若干個單兵節點的網絡，每個單兵節點的基本導航設備包含足部MIMU模塊、進行節點間距離測量的UWB模塊、進行數據收集及計算的處理板，以及在單兵間進行數據傳輸的無線數傳模塊。

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

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

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

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

本文研究的單兵協同導航系統最小組成如圖1所示。除了上述基本導航設備外，單兵的其他導航設備如衛星接收機、視覺里程計等都可以納入協同導航系統中。

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

2 單兵協同導航算法設計

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

2.1 協同導航系統模型

1)建模思路

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

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

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

2)一步慣性導航解算

取北東地坐標系為導航坐標系，并且定義姿態矩陣

(1)

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

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

(2)

(3)

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

(4)

第l次位移增量的結果為

(5)

由于

(6)

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

(7)

(8)

(9)

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

(10)

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

(11)

3)步進式運動模型

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

(12)

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

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

(13)

4)協同導航系統狀態模型

設N個單兵進行協同導航,協同導航系統的狀態方程由N個單兵的步進式運動模型組成

(14)

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

協同導航系統的觀測方程為

(15)

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

2.2 步進式協同導航算法

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

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

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

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

2)測距數據由協同中心的UWB模塊收集，并傳輸到協同中心的處理板。

2.3 協同導航算法比較

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

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

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

可見，本文步進式協同導航算法的優點是無需改變足部慣導算法，且無需進行反饋修正，這對系統模塊化設計及系統工程實現具有重要意義。

3 單兵協同導航試驗分析

3.1 試驗條件

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

表1 MIMU的性能指標

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

3.2 定位精度分析

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

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

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

參數設置一：

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

表2 ZUPT參數

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

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

參數設置二：

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

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

2)無GPS條件下的協同導航結果

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

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

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

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

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

3)有GPS條件下的協同導航結果

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

表3 參與協同導航的GPS觀測數據

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

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

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

(16)

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

4 結論

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

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

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

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