基于自包含傳感器的單兵導航系統設計

,,,

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

基于自包含傳感器的單兵導航系統設計

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

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

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

單兵導航系統;零速修正;航向修正;誤差補償

0 引言

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

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

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

1 單兵導航算法架構設計

單兵導航系統算法架構如圖1所示,主要包括傳感器信號處理及誤差補償單元、捷聯慣性導航解算單元、零速區間檢測單元及誤差估計單元。

圖1 單兵導航系統算法架構Fig.1 Algorithm architecture of ISNS

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

2 系統誤差補償

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

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

(1)

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

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

HM=He+Hf+Hs

(2)

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

根據環境中固定磁場和感應磁場的矢量輸出模型,磁傳感器測量值可進一步表示為

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

(3)

He=K-1(HM-Hbias)

(4)

地球上某固定點處,在沒有背景磁場干擾下,磁傳感器的測值量為當地地磁矢量He,滿足

(5)

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

(6)

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

單兵導航系統進行背景磁場補償時,將系統在空間內以較慢的速度轉動，使得采集的磁場信號數據點能夠盡可能多地覆蓋到橢球面,待不同轉動位置對應的數據點能基本覆蓋橢球面時結束現場校準。單兵導航系統背景磁場誤差現場校準結果如圖2所示。

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

(a)校準前

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

3 導航解算及誤差估計

單兵導航系統導航解算與傳統捷聯慣性導航解算類似,本文中不對捷聯解算過程進行詳細描述,僅對單兵導航系統初始對準過程進行分析。同時對系統的數據融合及誤差估計過程進行研究。

3.1 初始對準

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

(1)水平對準

(7)

(8)

(2)方位對準

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

(9)

相應地,根據地磁矢量水平分量可以計算出載體的航向角,表達式為

(10)

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

3.2 數據融合及誤差估計

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

(11)

結合式(11)中導航系統誤差方程及慣性傳感器誤差方程設計Kalman濾波器,系統狀態向量取

(12)

系統離散化狀態傳輸模型表達式為

δxk+1=Φkδxk+wk

(13)

本文中離散系統的狀態轉移矩陣為

(14)

(15)

離散系統的觀測方程為

δzk=Hδxk+vk

(16)

本文中單兵導航系統以速度誤差和航向誤差為觀測量,因此可得系統的量矩陣表達式為

(17)

4 零速區間檢測

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

1)加速度模值法。在步態零速區間內,單兵人員運動速度近似為零,此時慣性測量單元中三軸加速度計輸出的加速度矢量近似為當地重力矢量,零速區間內t時刻對應的加速度模值為

(18)

由于零速區間內加速度模值為穩態值,因此可以對加速度模值取邊界閾值來判斷t時刻單兵人員步態是否處于零速區間內。

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

(19)

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

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

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

當有外力作用時,壓力傳感器輸出對應的壓力值;當作用力為零時,壓力傳感器輸出近似為零。則對于單兵導航系統在零速區間內足底壓力傳感器承受人體重力,此時壓力傳感器輸出一定數值的壓力值;當行人步態為除零速區間以外的其他狀態時,壓力傳感器承受的外力數值很小,近似為零。基于此,可以通過設定閾值對壓力傳感器輸出的幅值進行判斷,得到步態中的零速區間。設t時刻壓力傳感器的測量值為f(t),則單兵人員步態處于零速區間的判斷條件為

f(t)>Tf

(20)

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

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

5 實驗驗證

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

圖4 單兵導航鞋Fig.4 The navigation shoe of individual soldier

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

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

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

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

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

6 結論

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

[1] 田曉春, 陳家斌, 韓勇強, 等.多條件約束的行人導航零速區間檢測算法[J].中國慣性技術學報, 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] 徐海剛, 吳亮華, 楊軍, 等.單兵自主導航技術研究[J].導航定位與授時, 2014, 1(1): 13-17.

[7] 張金亮, 秦永元, 梅春波.基于MEMS慣性技術的鞋式個人導航系統[J].中國慣性技術學報, 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

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

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

10.19306/j.cnki.2095-8110.2017.05.009

U666.1

：A

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