,,,

(1.北京自動化控制設(shè)備研究所，北京 100074；2.北京理工大學(xué) 自動化學(xué)院,北京 100081)

基于自包含傳感器的單兵導(dǎo)航系統(tǒng)設(shè)計

田曉春1,2,陳家斌2,尚劍宇2,韓勇強2

(1.北京自動化控制設(shè)備研究所，北京100074；2.北京理工大學(xué) 自動化學(xué)院,北京100081)

針對無衛(wèi)星信號環(huán)境中單兵人員導(dǎo)航定位需求,設(shè)計了一種基于自包含傳感器的單兵導(dǎo)航系統(tǒng),重點研究了慣性傳感器和壓力傳感器組合的零速區(qū)間檢測算法,并通過對單兵導(dǎo)航系統(tǒng)背景磁場誤差進行補償來計算航向角,實現(xiàn)了速度觀測量和航向觀測量的準確提取。在此基礎(chǔ)上,采用Kalman濾波器對系統(tǒng)狀態(tài)誤差進行估計,并對慣性導(dǎo)航解算結(jié)果中的累積誤差進行修正。最后,在實際路線上開展了單兵導(dǎo)航系統(tǒng)定位實驗,實驗結(jié)果表明,行人在矩形路線終點位置處的位置誤差為0.42m,占行走總路程的0.33%,從而證明了零速修正和航向修正能有效提高單兵導(dǎo)航系統(tǒng)的定位精度。

單兵導(dǎo)航系統(tǒng);零速修正;航向修正;誤差補償

0 引言

單兵導(dǎo)航系統(tǒng),又稱行人導(dǎo)航系統(tǒng),能夠為室內(nèi)外的單兵人員提供實時、準確的位置信息及導(dǎo)航服務(wù),從而幫助單兵人員很好地解決“我在哪里”、“我應(yīng)該在哪里”、“如何到達目的地”等問題[1]。單兵導(dǎo)航系統(tǒng)在特種作戰(zhàn)、緊急救援、公共服務(wù)等基于位置服務(wù)需求的軍民領(lǐng)域有著廣闊的應(yīng)用前景。

當(dāng)前用于實現(xiàn)單兵導(dǎo)航的技術(shù)方案主要可分為兩類,一類是基于預(yù)安裝設(shè)備的單兵導(dǎo)航方案[2-5],另一類是基于自包含傳感器的全自主導(dǎo)航定位方案[6-9]?；陬A(yù)安裝設(shè)備的單兵導(dǎo)航方案需要在導(dǎo)航區(qū)域安裝多個信號源,成本較高,只能在特定環(huán)境中使用,不適用于大面積推廣;基于自包含傳感器的單兵導(dǎo)航方案通過在單兵人員身上安裝慣性傳感器來完成自主導(dǎo)航,系統(tǒng)獨立性強且受環(huán)境影響小。受行人負載能力限制,單兵導(dǎo)航系統(tǒng)中的慣性傳感器必須便于攜帶,而傳統(tǒng)的慣性導(dǎo)航系統(tǒng)顯然無法滿足需求,如激光、光纖陀螺儀具有龐大的體積。然而,隨著微機電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)技術(shù)的發(fā)展,慣性傳感器的微小型化變成了可能。由MEMS傳感器構(gòu)成的微慣性測量單元具有體積小、質(zhì)量小、功耗小、成本低等優(yōu)勢,很好地滿足了單兵導(dǎo)航的應(yīng)用需求。但是,隨著傳感器尺寸的減小,MEMS慣性傳感器性能也隨之下降,從而導(dǎo)致單兵導(dǎo)航系統(tǒng)定位誤差隨著時間不斷累積。

行人行走過程中腳部與地面周期性接觸并在步態(tài)周期內(nèi)存在速度為零的時間段,因此可以通過零速修正的方法來抑制導(dǎo)航解算的累積誤差?；诖?本文設(shè)計了一種基于足部安裝自包含傳感器的單兵導(dǎo)航系統(tǒng)。該系統(tǒng)在零速區(qū)間內(nèi)設(shè)計ZUPT/HUPT組合算法,并通過Kalman濾波器對系統(tǒng)的誤差狀態(tài)進行估計,實現(xiàn)了對導(dǎo)航參數(shù)誤差的修正。

1 單兵導(dǎo)航算法架構(gòu)設(shè)計

單兵導(dǎo)航系統(tǒng)算法架構(gòu)如圖1所示,主要包括傳感器信號處理及誤差補償單元、捷聯(lián)慣性導(dǎo)航解算單元、零速區(qū)間檢測單元及誤差估計單元。

圖1 單兵導(dǎo)航系統(tǒng)算法架構(gòu)Fig.1 Algorithm architecture of ISNS

其中,傳感器信號處理及誤差補償單元主要對陀螺儀的零偏和單兵導(dǎo)航系統(tǒng)背景磁場誤差進行補償;捷聯(lián)慣性導(dǎo)航解算單元利用慣性傳感器的輸出來計算導(dǎo)航參數(shù);零速區(qū)間檢測單元用于識別單兵人員步態(tài)中的零速區(qū)間,進而為零速修正提供觸發(fā)條件;誤差估計單元是通過Kalman濾波器來估計系統(tǒng)的狀態(tài)誤差,并對導(dǎo)航解算的結(jié)果進行誤差修正。

2 系統(tǒng)誤差補償

單兵導(dǎo)航系統(tǒng)中慣性傳感器輸出包含零偏和噪聲項,尤其是MEMS陀螺儀的性能相對較低,在導(dǎo)航解算積分過程中誤差隨時間累積較快。同時,對于采用磁傳感器計算航向角的單兵導(dǎo)航系統(tǒng),受周圍環(huán)境中背景磁場的影響,磁傳感器的測量值除了地磁矢量外,還包含周圍環(huán)境中背景磁場矢量。因此,需要對背景磁場誤差進行補償,從而實現(xiàn)航向角的準確計算。本文中誤差補償主要分析陀螺儀的零偏誤差和單兵導(dǎo)航系統(tǒng)的背景磁場誤差。

單兵導(dǎo)航系統(tǒng)中陀螺儀的零偏誤差估計相對簡單,由于MEMS陀螺儀精度較低,無法感知地球自轉(zhuǎn)角速度,因此在單兵導(dǎo)航上電后可以使系統(tǒng)靜止保持一段時間,采集靜止狀態(tài)下陀螺儀輸出并求其均值,即可得到陀螺儀的零偏估計值為

(1)

在實驗過程中,陀螺儀實時敏感的角速率值減去靜止狀態(tài)下的零偏估計值,得到的角速度參數(shù)即可用于導(dǎo)航解算。

根據(jù)Tolles-Lawson理論,背景磁場主要包括固定磁場、感應(yīng)磁場和渦流磁場[10]。由于行人運動屬于低動態(tài)范疇,所以渦流磁場可以忽略不計。同時,由于人體磁場遠低于地球磁場,所以人體磁場對單兵導(dǎo)航系統(tǒng)中磁傳感器輸出的影響也可以忽略不計。單兵人員運動過程中,背景磁場主要包括固定磁場和感應(yīng)磁場,實際環(huán)境中磁傳感器測量值模型為

HM=He+Hf+Hs

(2)

式中,HM表示磁傳感器測量值,He表示地球磁場矢量,Hf表示固定磁場,Hs表示感應(yīng)磁場。

根據(jù)環(huán)境中固定磁場和感應(yīng)磁場的矢量輸出模型,磁傳感器測量值可進一步表示為

HM=(I3×3+kij)(He+Hf)

(3)

He=K-1(HM-Hbias)

(4)

地球上某固定點處,在沒有背景磁場干擾下,磁傳感器的測值量為當(dāng)?shù)氐卮攀噶縃e,滿足

(5)

將式(4)代入式(5),可得

(6)

式(6)所示為二次橢球曲面方程,即三軸磁傳感器的輸出構(gòu)成一個二次橢球曲面。因此,單兵導(dǎo)航系統(tǒng)中對背景磁場的標定就是尋找一組最優(yōu)的橢球參數(shù),使得測量的磁場參量與擬合的橢球之間距離最小。

單兵導(dǎo)航系統(tǒng)進行背景磁場補償時,將系統(tǒng)在空間內(nèi)以較慢的速度轉(zhuǎn)動，使得采集的磁場信號數(shù)據(jù)點能夠盡可能多地覆蓋到橢球面,待不同轉(zhuǎn)動位置對應(yīng)的數(shù)據(jù)點能基本覆蓋橢球面時結(jié)束現(xiàn)場校準。單兵導(dǎo)航系統(tǒng)背景磁場誤差現(xiàn)場校準結(jié)果如圖2所示。

圖2中二次曲面上的曲線即為校準過程中單兵導(dǎo)航鞋的轉(zhuǎn)動軌跡。從圖2中可以看出，旋轉(zhuǎn)過程中磁傳感器輸出的測量數(shù)據(jù)分布在橢球曲面的不同位置,基本實現(xiàn)了對橢球面的覆蓋。從圖2(a)中可以看出,在背景磁場的作用下,三軸磁傳感器實際測量值構(gòu)成了球心偏離坐標原點,形狀近似為橢球的二次曲面;背景磁場補償后得到的三軸磁傳感器測量值則構(gòu)成了一個球心在坐標原點的圓球,如圖2(b)所示。因此，補償后的磁傳感器測量數(shù)據(jù)可用于單兵導(dǎo)航系統(tǒng)航向角的計算。

(a)校準前

(b)校準后圖2 校準前后三軸磁傳感器輸出數(shù)據(jù)Fig.2 Output data of tri-axis magnetic sensor before and after calibration

3 導(dǎo)航解算及誤差估計

單兵導(dǎo)航系統(tǒng)導(dǎo)航解算與傳統(tǒng)捷聯(lián)慣性導(dǎo)航解算類似,本文中不對捷聯(lián)解算過程進行詳細描述,僅對單兵導(dǎo)航系統(tǒng)初始對準過程進行分析。同時對系統(tǒng)的數(shù)據(jù)融合及誤差估計過程進行研究。

3.1 初始對準

單兵導(dǎo)航系統(tǒng)開始導(dǎo)航解算前需進行初始對準。由于MEMS陀螺儀精度較低,所以單兵導(dǎo)航系統(tǒng)不能依靠其自身集成的加速度計和陀螺儀完成自對準。本文采用MEMS加速度計和磁傳感器組合的方式實現(xiàn)單兵導(dǎo)航系統(tǒng)的粗對準。初始對準過程中首先利用慣性測量單元中加速度計的輸出估算水平姿態(tài)角,然后根據(jù)加速度計計算的水平姿態(tài)角并結(jié)合磁傳感器輸出估算方位角進而實現(xiàn)方位對準。

(1)水平對準

(7)

(8)

(2)方位對準

初始對準過程中,磁傳感器測量的是單兵在載體坐標系下的磁場矢量,根據(jù)坐標系之間轉(zhuǎn)換關(guān)系,利用水平對準計算的姿態(tài)角可以得到水平面上地磁場矢量水平分量為:

(9)

相應(yīng)地,根據(jù)地磁矢量水平分量可以計算出載體的航向角,表達式為

(10)

式(10)中磁偏角D可通過查表得到。

3.2 數(shù)據(jù)融合及誤差估計

本文中單兵導(dǎo)航系統(tǒng)通過Kalman濾波技術(shù)進行數(shù)據(jù)融合,估計單兵導(dǎo)航系統(tǒng)的誤差并利用誤差的估計值校正系統(tǒng)。單兵人員的運動屬于低動態(tài)范疇,一般情況下單兵人員行走速度較慢,行走距離較短。同時結(jié)合單兵導(dǎo)航系統(tǒng)中MEMS慣性器件的性能,可得單兵導(dǎo)航系統(tǒng)及慣性器件誤差方程為:

(11)

結(jié)合式(11)中導(dǎo)航系統(tǒng)誤差方程及慣性傳感器誤差方程設(shè)計Kalman濾波器,系統(tǒng)狀態(tài)向量取

(12)

系統(tǒng)離散化狀態(tài)傳輸模型表達式為

δxk+1=Φkδxk+wk

(13)

本文中離散系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣為

(14)

(15)

離散系統(tǒng)的觀測方程為

δzk=Hδxk+vk

(16)

本文中單兵導(dǎo)航系統(tǒng)以速度誤差和航向誤差為觀測量,因此可得系統(tǒng)的量矩陣表達式為

(17)

4 零速區(qū)間檢測

MEMS慣性傳感器存在漂移大、器件精度低的問題,采用SINS算法計算導(dǎo)航參數(shù)時,誤差累積較大。針對行人行走過程中足部周期性觸地的特點,通常采用零速修正的方法對SINS解算的誤差清零,從而有效提高系統(tǒng)的解算精度。常用的零速區(qū)間檢測方法有加速度模值法、加速度滑動方差法、角速率模值法等。本文主要采用加速度模值、角速度模值,并利用本系統(tǒng)中壓力傳感器的測量參數(shù)作為輔助判斷條件,設(shè)計了單兵導(dǎo)航系統(tǒng)零速區(qū)間檢測算法。

1)加速度模值法。在步態(tài)零速區(qū)間內(nèi),單兵人員運動速度近似為零,此時慣性測量單元中三軸加速度計輸出的加速度矢量近似為當(dāng)?shù)刂亓κ噶?零速區(qū)間內(nèi)t時刻對應(yīng)的加速度模值為

(18)

由于零速區(qū)間內(nèi)加速度模值為穩(wěn)態(tài)值,因此可以對加速度模值取邊界閾值來判斷t時刻單兵人員步態(tài)是否處于零速區(qū)間內(nèi)。

2)角速度模值法。與加速度計輸出類似,在步態(tài)零速區(qū)間內(nèi)慣性測量單元中三軸陀螺儀輸出的角速度矢量近似為零,零速區(qū)間內(nèi)t時刻對應(yīng)的角速度模值為

(19)

同理,可以通過對角速度模值設(shè)定閾值來判斷t時刻單兵人員步態(tài)是否處于零速區(qū)間內(nèi)。

3)壓力閾值法。在對加速度和角度速求模值判斷零速區(qū)間的基礎(chǔ)上,為了提高步態(tài)中零速區(qū)間檢測精度,本文在不額外增加系統(tǒng)體積的情況下選用了Interlink Electronics生產(chǎn)的低成本、小尺寸電阻式薄膜壓力傳感器FSR402來輔助測量行人腳部的壓力變化值(見圖3)。FSR402傳感器采用柔性印刷電路工藝制造,厚度僅為0.46mm,圓形感應(yīng)區(qū)域直徑為12.7mm,可檢測的最大壓力值為100N,該壓力傳感器能非常方便地安裝于參考腳鞋底。

圖3 薄膜式壓力傳感器Fig.3 Film pressure sensor

當(dāng)有外力作用時,壓力傳感器輸出對應(yīng)的壓力值;當(dāng)作用力為零時,壓力傳感器輸出近似為零。則對于單兵導(dǎo)航系統(tǒng)在零速區(qū)間內(nèi)足底壓力傳感器承受人體重力,此時壓力傳感器輸出一定數(shù)值的壓力值;當(dāng)行人步態(tài)為除零速區(qū)間以外的其他狀態(tài)時,壓力傳感器承受的外力數(shù)值很小,近似為零?；诖?可以通過設(shè)定閾值對壓力傳感器輸出的幅值進行判斷,得到步態(tài)中的零速區(qū)間。設(shè)t時刻壓力傳感器的測量值為f(t),則單兵人員步態(tài)處于零速區(qū)間的判斷條件為

f(t)>Tf

(20)

其中,Tf為壓力閾值,當(dāng)檢測到的壓力值大于設(shè)定閾值時,表示參考腳與地面完全接觸,判定此時運動腳處于靜止狀態(tài)。

當(dāng)t時刻的加速度模值、角速度模值及壓力值都處于設(shè)定的閾值范圍內(nèi)時,認為該時刻即為步態(tài)中的零速時刻點;如果不能同時滿足1)～3)設(shè)定的閾值條件,則認為該時刻行人步態(tài)處于非零速狀態(tài)。

5 實驗驗證

為了驗證本文設(shè)計的單兵導(dǎo)航系統(tǒng)的定位性能,在實際環(huán)境中開展了單兵人員航跡測試實驗。實驗過程中選用Xsens公司生產(chǎn)的MTi微慣性測量單元測量行人運動過程中的步態(tài)參數(shù),慣性測量單元安裝在鞋跟部位,并通過安裝板與鞋體固連。單兵導(dǎo)航系統(tǒng)樣機如圖4所示,慣性測量單元輸出參數(shù)通過數(shù)據(jù)線傳輸給上位機,實驗過程中設(shè)定數(shù)據(jù)采樣率為100Hz。

圖4 單兵導(dǎo)航鞋F(xiàn)ig.4 The navigation shoe of individual soldier

單兵人員航跡測試實驗在一矩形路線上開展。其中矩形路線長50m,寬12.9m,實驗人員從矩形路線的一點出發(fā),行走一圈后回到出發(fā)點。理想情況下,計算得到的行人航跡終點與起始點一致,而實際中由于傳感器誤差、算法及行走過程中的不確定性,往往終點與起始點存在一定位置誤差l,記行人行走路線總長度為L,定義行人導(dǎo)航系統(tǒng)的定位誤差為位置誤差與行走路線總長度的比值l/L。

圖5所示為矩形路線測試實驗過程中部分傳感器輸出及其對應(yīng)的零速區(qū)間檢測結(jié)果。從圖5中可以看出,行人行走過程中加速度計、陀螺儀和壓力傳感器的輸出呈現(xiàn)周期性變化規(guī)律,同時,每一個周期內(nèi)對應(yīng)于零速區(qū)間傳感器的輸出幅值具有顯著的特點。采用第4節(jié)設(shè)計的算法檢測步態(tài)中的零速區(qū)間,結(jié)果如圖5中最下圖所示。從圖5中還可以看出,零速區(qū)間檢測結(jié)果與傳感器測量的步態(tài)參數(shù)相對應(yīng),表明了本文設(shè)計的零速區(qū)間檢測算法能準確檢測行人行走過程中步態(tài)的零速區(qū)間。

圖5 傳感器輸出及零速區(qū)間檢測結(jié)果Fig.5 Sensors output and zero velocity interval detection results

圖6所示為行人繞矩形路線行走一圈的試驗結(jié)果。紅色虛線為參考的幾何路線,藍色實線為采用本文中ZUPT/HUPT組合算法計算的行人航跡曲線,對比可知導(dǎo)航解算的路線與參考路線基本重合。由于磁傳感器可以計算航向信息,捷聯(lián)解算過程中的航向漂移也得到了有效抑制。從圖6中行人位置終點處的局部放大圖可知,終點處位置坐標為(-0.4031,0.1174),位置誤差為0.42m,為行走總路程的0.33%。

圖6 行人繞矩形路線行走一圈航跡曲線Fig.6 Result of walking along a rectangle trajectory

6 結(jié)論

本文設(shè)計了一種基于自包含傳感器的單兵導(dǎo)航系統(tǒng),通過綜合利用慣性傳感器及壓力傳感器輸出參數(shù)的特點,設(shè)計了行人步態(tài)零速區(qū)間檢測算法,并通過零速修正來對系統(tǒng)狀態(tài)誤差進行估計;同時,針對零速修正無法估計航向誤差的問題,本文在對單兵導(dǎo)航系統(tǒng)背景磁場誤差補償?shù)幕A(chǔ)上，借助慣性測量單元中集成的磁傳感器來計算航向角,完成了對航向誤差的修正。在矩形路線測試實驗中,本文設(shè)計的單兵導(dǎo)航算法終點處位置誤差為行走總路程的0.33%,具有較高的定位精度,能夠在無衛(wèi)星信號環(huán)境中對單兵人員進行獨立自主定位。

[1] 田曉春, 陳家斌, 韓勇強, 等.多條件約束的行人導(dǎo)航零速區(qū)間檢測算法[J].中國慣性技術(shù)學(xué)報, 2016, 24(1): 1-5.

[2] Schatzberg U, Banin L, Amizur Y.Enhanced wifi tof indoor positioning system with mems-based ins and pedometric information[C]//2014 IEEE/ION Position, Location and Navigation Symposium-PLANS 2014.IEEE, 2014: 185-192.

[3] Zhuang Y, El-sheimy N.Tightly-coupled integration of WiFi and MEMS sensors on handheld devices for indoor pedestrian navigation[J].IEEE Sensors Journal, 2016, 16(1): 224-234.

[4] Hol J D, Dijkstra F, Luinge H, et al.Tightly coupled UWB/IMU pose estimation[C]//IEEE International Conference on Ultra-Wideband.IEEE, 2009: 688-692.

[5] Zihajehzadeh S, Yoon P K, Park E J.A magnetometer-free indoor human localization based on loosely coupled IMU/UWB fusion[C]//2015 37thAnnual International Conference of the Engineering in Medicine and Biology Society(EMBC).IEEE, 2015: 3141-3144.

[6] 徐海剛, 吳亮華, 楊軍, 等.單兵自主導(dǎo)航技術(shù)研究[J].導(dǎo)航定位與授時, 2014, 1(1): 13-17.

[7] 張金亮, 秦永元, 梅春波.基于MEMS慣性技術(shù)的鞋式個人導(dǎo)航系統(tǒng)[J].中國慣性技術(shù)學(xué)報, 2011, 19(3): 253-256.

[8] 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]//IEEE 7thWorkshop on Positioning Navigation and Communication (WPNC).IEEE, 2010: 135-143.

[9] Abdulrahim K, Hide C, Moore T, et al.Aiding low cost inertial navigation with building heading for pedestrian navigation[J].Journal of Navigation, 2011, 64(2): 219-233.

[10] Tolles W E.Compensation of induced magnetic fields in mad equipped aircraft[J].Airborne Instruments Lab., OSRD, 1943.

DesignofIndividualSoldierNavigationSystemBasedonSelf-ContainedSensors

TIANXiao-chun1，2,CHENJia-bin2,SHANGJian-yu2,HANYong-qiang2

(1.BeijingInstituteofAutomaticControlEquipment,Beijing100074,China；
2.SchoolofAutomation,BeijingInstituteofTechnology,Beijing100081,China)

Aiming at the requirements of individual soldier positioning in the environment without satellite signal, an individual soldier navigation system (ISNS) based on self-contained sensors is designed in this paper and external measurement extraction algorithm is studied.The zero velocity interval in human gait is detected by combining the output of inertial sensors and force sensor.Meanwhile, the heading angle can be calculated according to the magnetic sensor output after compensating the background magnetic field error of ISNS.On this basis, the state error of the system is estimated by adopting Kalman filter and the cumulative error of SINS is corrected.Finally, positioning experiment is carried out in actual experimental course, the results show that the position error at the end point of the rectangular route is0.42m, accounting for0.33% of the total walking distance, which proves that the zero velocity update (ZUPT) and heading update (HUPT) can effectively improve the positioning accuracy of ISNS.

Individual soldier navigation system; ZUPT; HUPT; Error compensation

2017-05-17；

：2017-06-16

：國防預(yù)研基金項目資助(9140A09050313BQ01127)

：田曉春(1986-),男,博士研究生,主要從事單兵導(dǎo)航技術(shù)研究。E-mail:tianxiaochunno1@126.com

10.19306/j.cnki.2095-8110.2017.05.009

U666.1

：A

：2095-8110(2017)05-0054-06