,,,

(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