基于步進(jìn)式運(yùn)動(dòng)模型的單兵協(xié)同導(dǎo)航算法設(shè)計(jì)

穆 華，潘獻(xiàn)飛，吳美平，胡小平

(國(guó)防科技大學(xué)智能科學(xué)學(xué)院，長(zhǎng)沙 410073)

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

2 單兵協(xié)同導(dǎo)航算法設(shè)計(jì)

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

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

1)建模思路

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

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

以單兵位置和航向?yàn)闋顟B(tài)變量，以單兵每步的位移增量和航向增量為系統(tǒng)輸入，建立單兵的步進(jìn)式運(yùn)動(dòng)模型。所有單兵的步進(jìn)式運(yùn)動(dòng)模型組合成協(xié)同導(dǎo)航系統(tǒng)的狀態(tài)方程，結(jié)合單兵間距離觀測(cè)方程，建立協(xié)同導(dǎo)航系統(tǒng)模型。

2)一步慣性導(dǎo)航解算

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

(1)

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

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

(2)

(3)

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

(4)

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

(5)

由于

(6)

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

(7)

(8)

(9)

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

(10)

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

(11)

3)步進(jìn)式運(yùn)動(dòng)模型

利用n′系中一步位移增量dp′s，可得n系下的步進(jìn)式運(yùn)動(dòng)模型

(12)

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

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

(13)

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

設(shè)N個(gè)單兵進(jìn)行協(xié)同導(dǎo)航,協(xié)同導(dǎo)航系統(tǒng)的狀態(tài)方程由N個(gè)單兵的步進(jìn)式運(yùn)動(dòng)模型組成

(14)

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

協(xié)同導(dǎo)航系統(tǒng)的觀測(cè)方程為

(15)

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

2.2 步進(jìn)式協(xié)同導(dǎo)航算法

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

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

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

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

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

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

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

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

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

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

3 單兵協(xié)同導(dǎo)航試驗(yàn)分析

3.1 試驗(yàn)條件

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

表1 MIMU的性能指標(biāo)

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

3.2 定位精度分析

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

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

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

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

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

表2 ZUPT參數(shù)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4)不同試驗(yàn)條件下的定位精度

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

(16)

表4 不同試驗(yàn)條件的定位精度

4 結(jié)論

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

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

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

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