基于行人运动模态辨识的室内外无缝导航航向算法研究

黄 欣,许建新,张 苗,熊 智

(南京航空航天大学自动化学院,南京 211100)



基于行人运动模态辨识的室内外无缝导航航向算法研究

黄 欣,许建新,张 苗,熊 智*

(南京航空航天大学自动化学院,南京 211100)

目前行人导航航向解算算法均基于导航传感器在行人身体上的固定安装模式,或者依赖其他射频信息辅助修正陀螺航向,这极大约束了导航传感器的适用条件。为此,利用陀螺对低频噪声的敏感性及加速度计低频的稳定性,提出了解决行人手持手机稳态查看与非稳态摇摆的运动模态辨识算法和基于时域互补滤波器实现姿态的最优融合方法;研究了改进型互补滤波以消除行人的运动加速度对姿态解算的干扰误差,提高了载体姿态的测量精度;此外,利用磁传感器标定后的数据设计了自适应卡尔曼滤波算法,抑制了航向角的误差发散。经实际数据测试验证,室内外行人手持稳态与非稳态下的航向角测量精度提高了80%,同时大大提高了导航传感器的适用性与便携性,满足实际工程的使用需求。

惯性传感器;改进型互补滤波;磁异常辨识;自适应卡尔曼滤波;行人运动模态辨识

目前行人导航已经掀起国内外研究热潮,针对行人导航技术的研究主要集中于以下两类:一类是基于惯性传感器,通过观测惯性传感器固定在行人肩部、腰部、腿部和脚部等部位的运动特性,研究零速修正、零角速修正、步态辨识等技术,实现行人的航迹推算[1-2];一类是基于无线网络,研究利用蓝牙、超声波、WIFI、ZigBee等额外射频设备辅助行人定位[3]。前者极大约束了传感器的适用条件,便携性较差;后者不仅增加了额外成本,同时其适用环境也较为狭窄。现代化社会,加速度计、陀螺仪等导航传感器已普遍嵌入到手机中,而且据统计,除去16 h的睡眠及工作时间,手机人均每日使用时间长达3.7 h[4],已成为人们日常携带的必备产品,因此,利用手机自带导航传感器实现行人定位足以满足日常需求。

但利用手机中的导航传感器实现行人定位时,由于行人对手机的手持方式不同,迫切需要能够解决行人在不同手持状态下的航向解算算法,并尽可能提高航向角的计算精度,这也是限制手持式行人导航定位精度提高的主要因素。陀螺仪作为解算航向角的通用传感器,受限于低成本传感器零偏较大等因素,误差发散较快[6],国内外针对手持式导航传感器这一缺点,主要研究利用建筑楼向信息及射频设备辅助陀螺修正航向[3],其适用条件受到约束;磁航向传感器凭借对地球磁场的测量,可以长期有效地解算出当前载体的航向角,得到了广泛的运用;但是室内环境下,钢筋等铁磁材料对周围磁环境会产生强烈的干扰[6],磁异常辨识及处理不当会极大影响导航系统精度。

针对上述问题,本文以智能手机自带的导航传感器为硬件基础,提出了非固连模式(手持)下高精度航向实时解算算法,主要解决行人手持稳态查看与非稳态摇摆下的室内外无缝导航航向问题。该方法研究了行人运动模态辨识算法以有效区分行人手持稳态查看与非稳态摇摆状态;研究了改进型互补滤波器,从时域角度设计了相关参数,实现了加速度计姿态角与陀螺姿态角的最优融合,同时,消除了行人的运动加速度对姿态解算的干扰,提高了载体姿态的测量精度;此外,利用标定后的磁传感器数据设计自适应卡尔曼滤波算法,抑制了航向角的误差发散。本文在保证高精度的基础上,大大提高了传感器的适用范围与便携性。

1 基于行人运动模态辨识的改进型航向互补滤波器设计

将捷联惯性导航解算算法用于行人导航解算过程中,姿态解算是其重要环节。基于陀螺对低频噪声的敏感性及加速度计低频的稳定性考虑,本文研究了行人运动模态辨识算法以区分行人手持稳态查看与非稳态摇摆状态,并在稳态下构建了互补滤波器[7-9],从时域角度研究利用最优加权平均参数估计完成姿态角的最优融合,由于行人运动会引入额外加速度,本文研究了改进型互补滤波器消除额外加速度对利用加速度计进行姿态解算的干扰,提高了载体姿态的测量精度。

1.1 行人运动模态辨识算法

行人手持载体在复杂运动环境下主要分为两种模态,一种模态为行人行走时人体手臂摆动下的非稳态状态,如图1(a)所示;一种模态为行人行走时的稳态查看状态,如图1(b)所示。

图1 行人运动模态示意图

为有效区分上述的两种模态,通过观测z轴陀螺每秒角速度绝对值积分对两种模态进行判别,仅在稳态下对姿态进行改进型互补滤波估计。模式判别约束如下所示:

(1)

式中:ω为z轴方向陀螺仪输出的角速度,Δt为时间间隔。对陀螺仪输出的角速度绝对值进行积分可以避免人体摆动周期性相消。为避免行人急速转弯时产生误判,则同时针对z轴方向加速度计进行判别约束,当z轴方向加速度计同时大于某值时,判为稳态,否则为非稳态。

因此,当满足

(2)

此时判为手持稳态查看状态,否则为非稳态状态,其中ε、gt为静止稳态下计算出来的参数值,gz为z轴方向加速度计输出,如式(3)所示,其中U代表大小为N的数组,g为当地重力加速度。

(3)

1.2 基于行人运动模态辨识的改进型互补滤波器设计

姿态解算利用陀螺仪与加速度计所测得的数据进行互补滤波获得。加速度计主要用来测量载体坐标系下的线加速度,通过比较加速度计各轴输出与重力加速度的比值可以解算出姿态角,如式(4)所示,其测量误差不会随时间发散,但会受到测量误差和运动状态的影响。

(4)

陀螺仪主要用来测量载体相对于参考坐标系下的角速度,利用陀螺仪输出解算出姿态角目前主要包括欧拉角法、方向余弦法和四元素法,其中欧拉角法每个方程均包含三角函数运算,该姿态算法复杂且不能全姿态工作;方向余弦法微分方程组维数太高且需进行正交化处理;四元素法微分方程只有四维且可以全姿态工作,因此本文选择四元素法解算姿态角[10],如式(5)所示:

(5)

1.2.1 互补滤波器模型设计

鉴于加速度计在低频下的测量稳定性以及陀螺对低频噪声的敏感性,本文从时域角度分析,构建互补滤波器在低频稳态下对两者姿态进行数据融合,提高姿态解算精度,如图2所示。

图2 互补滤波器原理

假设陀螺仪的权重为k1,加速度计的权重为k2,则对真值的估计为:

(6)

且

k1+k2=1

(7)

则估计值的方差为

(8)

对其求偏导,可以求得最小值

(9)

此时,最优解参数为

(10)

则经互补滤波后的最优姿态角为:

η=k1ηw+k2ηf

(11)

式中:η代表横滚角与俯仰角,又因为:

ηwn=ηwn-1+ωT

(12)

则

ηk=k1(ηwk-1+ωT)+k2ηfk

(13)

式中:T为采样周期,则式(13)即为k时刻的姿态角解算方程。

1.2.2 改进的互补滤波器模型设计

由于行人运动环境下,会引入额外加速度,必须对其加以补偿,否则会造成加速度计姿态角解算不准确的问题。本文在常规的互补滤波器的基础上,研究了改进型互补滤波器,消除行人自带加速度对利用加速度计进行姿态解算的干扰,其原理如图3所示。

图3 改进型互补滤波器原理

(14)

(15)

2 基于自适应卡尔曼滤波的惯性/地磁航向最优融合

第1节设计了行人运动模态下的高精度水平姿态解算算法(横滚、俯仰),解决了行人不同运动模态下的地磁投影问题,下面将以此为基础,设计航向和惯性的最优组合算法。

2.1 卡尔曼滤波器状态建模

卡尔曼滤波器作为一种最小方差估计,从概率统计最优的角度估计出系统误差并将其消除。本文选用“东北天”地理坐标系,构建18阶模型,如式(16)所示。其中 包括:3个平台角误差、3个速度误差、3个位置误差;3轴陀螺随机常数、3轴陀螺一阶马尔可夫过程与3轴加速度计一阶马尔可夫过程[11-13]。在扣除加速度计、陀螺仪零偏之后,利用卡尔曼滤波器在稳态下修正陀螺漂移、加速度计漂移及航向角误差;在非稳态下修正陀螺漂移、加速度计漂移、姿态角误差及航向角误差。

(16)

2.2 卡尔曼滤波器量测建模

目前,国内外针对磁补偿主要包括二维椭圆标定算法和三维椭球标定算法,二维椭圆标定算法利用水平旋转一圈水平磁矢量大小不变的特性进行磁标定,三维椭球标定算法利用三维空间磁矢量大小不变的特性对磁进行补偿[14-16]。鉴于两者标定精度相当及二维标定的便捷性,本文采用二维椭圆标定算法,对磁传感器数据进行误差补偿,消除硬磁、软磁以及自身的非正交安装误差带来的干扰。通过对磁传感器输出数据实时坐标投影,以获得水平方向磁传感器数据,如图4所示。

图4 磁航向示意图

则磁航向角可由如下公式获得:

(17)

本文构建一维量测模型,量测周期为1s,量测方程如下式所示:Zv(t)=[φw-φm]=[δφ+Mφ]=Hv(t)X(t)+Vv(t)

(18)

式中:φw为陀螺解算出的航向角,φm为磁航向角,δφ为姿态误差角。姿态误差角与平台误差角存在如下关系[17]:

(19)

式中:θ为俯仰角,φx、φy、φy为3个平台误差角。因此:

(20)

2.3 基于实时统计信息的磁异常辨识及量测噪声模 型更新方法

针对2.2节量测噪声阵的选取,本文首先采集了室内外环境下相关磁信息,基于统计学相关理论对数据进行了分析与处理,如图5所示,其中图5(a)、图5(b)分别为室外与室内一段路线总磁场强度的变化曲线,图5(c)、图5(d)分别为室外与室内一段路线总磁场强度方差的变化曲线,可以发现室外磁场强度维持在5 μT左右变化,方差大小均在1.4以下,而室内磁场幅度变化较大,利用此相关信息可以有助于区分磁异常情况。

图5 室内外磁传感器信息对比

本文研究利用磁传感器实时统计信息进行磁异常辨识,判别约束条件如下所示:

(21)

(22)

式中:λ为设定的经验值,η为事先人为设定的增益常量,ε3与ε4为通过统计学方法获得的参数值。

3 仿真验证与实验结果分析

本次实验仪器采用华为Mate 8 型号手机,采集频率50 Hz,分别对行人步行稳态查看场景、行人步行稳态与非稳态顺序切换场景进行了室内外测试,室外以GPS航向角作为参考(初始静止状态下,GPS误差较大,不进行误差对比),室内以人为规划路径航向角作为行人行走航向角参考,其中实验航向角每0.02 s输出一次,GPS航向角与标准航向角均为1 s输出一次,仅以整秒时刻量化验证航向角计算精度。

3.1 行人室外步行稳态查看场景

图6 室内外磁二维椭圆标定对比

行人步行稳态查看场景下,手持载体可以轻微抖动,但是不存在大姿态例如手臂摇摆情况。鉴于室外磁异常情况较少,如图6(a)所示,因此室外环境仅用来分析纯陀螺解算航向角与经自适应卡尔曼滤波融合航向角的差异性。纯陀螺解算航向角与GPS航向角对比曲线及手持模式判别曲线如图7所示,组合航向角与GPS航向角对比曲线及手持模式判别曲线如图8所示。

图7 纯陀螺航向与GPS航向对比、模式判别曲线

图8 组合航向与GPS航向对比、模式判别曲线

3.2 行人室外步行稳态与非稳态切换场景

行人步行稳态查看场景下,手持载体可以轻微抖动,但是不存在手臂摇摆情况;行人步行非稳态场景下,手臂摇摆,两者循环交替出现。纯陀螺解算航向角与GPS航向角对比曲线及手持模式判别曲线如图9所示,组合航向角与GPS航向角对比曲线及手持模式判别曲线如图10所示,其中图中绿色为行人运动模态辨识曲线,91 s～128 s、220 s～272 s为非稳态状态,模态置0,其余为稳态,模态置1。

图10 组合航向与GPS航向对比、模式判别曲线

图9 纯陀螺航向与GPS航向对比、模式判别曲线

本文将室外场景下的航向角误差均值做了对比,由于手持摆动情况下,航向角反应的是手持载体的航向角,GPS反应的是行人行走航向角,因此主要观测摆动过后,航向角能否迅速跟踪GPS航向角,如表1所示,可以发现组合航向角精度相对于纯捷联航向角有了很大的提高。

表1 室外不同组合下航向角误差均值

综合上述室外结果可以发现,利用改进型互补滤波器及卡尔曼滤波器实现航向角的最优融合有助于提高其精度。鉴于行人速度下,GPS航向角精度难以保证,同时室内缺乏GPS信息,因此室内环境下,人为规划方形标准路径,如图11所示。路径初始航向58°,每经过2号位、3号位、4号位时刻,航向偏移90°,并利用计时器精准记录当前时刻以保证后续精度的对比。观测图6(b),室内磁异常较为严重,在验证磁异常检测及自适应滤波的同时,对比组合航向角与标准航向角误差。

图11 室内路径示意图

3.3 行人室内步行稳态查看场景

行人步行稳态查看场景下,手持载体可以轻微抖动,但是不存在手臂摇摆情况。未加自适应滤波的组合航向角与标准航向角对比曲线及手持模式判别曲线如图12所示,加入自适应滤波的组合航向角与标准航向角对比曲线及手持模式判别曲线如图13所示。

3.4 行人室内步行稳态与非稳态切换场景

行人步行稳态查看场景下,手持载体可以轻微抖动,但是不存在手臂摇摆情况;行人步行非稳态场景下,手臂摇摆,两者循环交替出现,未加自适应滤波的组合航向角与标准航向角对比曲线及手持模式判别曲线如图14所示,加入自适应滤波的组合航向角与标准航向角对比曲线及手持模式判别曲线如图15所示,其中图中绿色为行人运动模态辨识曲线,56 s～81 s、125 s～154 s为非稳态状态,模态置0,其余为稳态,模态置1。

图12 航向对比、模式判别曲线

图13 航向对比、模式判别曲线

图14 航向对比、模式判别曲线

图15 航向对比、模式判别曲线

由于标准路径航向角仅与人体运动方向相对应,手持摇摆情况下,由于左右的摆动幅度角,曲线组合航向角反应的仅是手机的航向,并非行人行走的航向,因此仅以稳态对应路段下行人行走的航向角作对比,其结果如表2所示。

表2 室内不同情景下航向角误差均值

观测上述数据,本文提出的基于实时统计信息进行磁异常判断及量测噪声阵修正具有明显的效果,有效的剔除了磁异常航向角,同时,当行人从非稳态进入稳态状态下,航向角能精准而又迅捷的跟踪行人行驶航向角,算法精度较高。

综述上述实验,本文提出的基于行人运动模态辨识的室内外无缝导航航向算法,无论是步行稳态查看状态,还是步行稳态与非稳态切换状态;无论是室外磁正常环境,亦或是室内磁异常环境,其均有较高的航向精度,算法适应性较高。

4 仿真验证与实验结果分析

本文以智能手机自带导航传感器为硬件基础,结合行人复杂运动环境下不同模态特性,设计了一种非固连模式(手持)下高精度航向实时解算算法。该方法研究了运动模态辨识算法有效区分出行人手持稳态查看与非稳态摇摆状态。同时,基于互补滤波器,从时域角度设计了相关参数,实现了加速度计姿态角与陀螺姿态角的最优融合,在此基础上研究了改进型互补滤波器,消除了行人运动加速度对利用加速度计进行姿态解算的干扰误差,提高了载体姿态的测量精度。此外,本文还研究了低精度导航传感器误差的实时最优估计算法并研究了自适应卡尔曼滤波器实现陀螺航向角与磁航向角的最优融合,利用实时统计信息实现磁异常辨识及组合滤波量测噪声阵的自适应调整。本算法能够保证室内外行人手持传感器复杂运动环境下的航向角精度,提高了传感器的适用性,具有足够的工程应用价值。

[1] 钱伟行,曾庆化,万骏炜,等. 基于人体下肢运动学机理的行人导航方法[J]. 中国惯性技术学报,2015(1):24-28.

[2] Stirling R G. Development of a Pedestrian Navigation System Using Shoe Mounted Sensors[J]. 2005.

[3] Antonio Ramón Jiménez Ruiz,Fernando Seco Granja,José Carlos Prieto Honorato,et al. Accurate Pedestrian Indoor Navigation by Tightly Coupling Foot-Mounted IMU and RFID Measurements.[J]. IEEE T Instrumentation and Measurement,2011,61:178-189.

[4] 贺艳. 试析移动互联网背景下新媒体剧的叙事变化与价值指向[J]. 中国电视,2015(4):65-69.

[5] Afzal M H,Renaudin V,Lachapelle G. Magnetic Field Based Heading Estimation for Pedestrian Navigation Environments[C]//International Conference on Indoor Positioning and Indoor Navigation. IEEE,2011:1-10.

[6] 王昌刚,刘强,韩瑜. 姿态航向测量与误差补偿方法研究[J]. 传感器与微系统,2014,33(11):37-40.

[7] 万晓凤,康利平,余运俊,等. 互补滤波算法在四旋翼飞行器姿态解算中的应用[J]. 测控技术,2015(2):8-11.

[8] Calvin Coopmans,Austin M Jensen,Yangquan Chen. Fractional-Order Complementary Filters for Small Unmanned Aerial System Navigation[J]. Journal of Intelligent and amp;Robotic Systems,2014,731-4:.

[9] Tian Y,Wei H,Tan J. An Adaptive-Gain Complementary Filter for Real-Time Human Motion Tracking With MARG Sensors in Free-Living Environments.[J]. IEEE Transactions on Neural Systems and Rehabilitation Engineering,2013,21(2):254-264.

[10] 丁君,赵忠华. AHRS航姿解算中的两种滤波方法的比较研究[J]. 计算机仿真,2013(9):60-65.

[11] 钱伟行,彭晨,田恩刚,等. 基于导航信息双向融合的行人/移动机器人协同导航方法[J]. 中国惯性技术学报,2014(1):74-78.

[12] 代金华,张丽杰. 多准则MEMS陀螺随机误差在线建模与实时滤波[J]. 传感技术学报,2016,29(1):75-79.

[13] 吴涛,白茹,朱礼尧,等. 基于卡尔曼滤波的航姿参考系统设计[J]. 传感技术学报,2016,29(4):531-535.

[14] 李艳,管斌,王成宾,等. 电子磁罗盘航向角误差推导及分析[J]. 仪器仪表学报,2013(7):127-133.

[15] 袁智荣. 磁航向传感器使用中的误差补偿[J]. 测控技术,2001(1):58-59.

[16] 张樨,李杰,蒋窍,等. 基于椭球拟合的三轴磁传感器快速标定补偿方法[J]. 机械工程与自动化,2012(4):163-165.

[17] 丁涛,赵忠. 捷联惯导系统的误差角关系与推导[J]. 兵工自动化,2007,26(12):66-67.

Research on Indoor and Outdoor Seamless Navigation Heading Algorithm Based on Pedestrian Modal Identification

HUANG Xin,XU Jianxin,ZHANG Miao,XIONG Zhi*

(College of Automation Engineering,Nanjing University of Aeronautics and Astronautics,Nanjing 211100,China)

Nowadays,heading algorithm in pedestrian navigation is mostly based on the fixed mode of navigation sensor on the pedestrian body,or relies on other auxiliary radio frequency information,which greatly reduces the portability of sensors. Hence,based on the sensitivity of the gyroscope to the low frequency noise and the stability of the accelerometer,this paper puts forward one solution to determine the steady-state and non-steady state of the hand held mobile based on the modal identification and proposes complementary filter to achieve optimal attitude based on time domain. This paper uses modified complementary filter to weaken the interference caused by pedestrian movement and improves the accuracy of the carrier attitude measurement. In addition,the adaptive kalman filter algorithm is designed by using the magnetic sensor calibration data to restrain the divergence of heading angle error. The test results show that this algorithm can ensure that the accuracy of heading angle measurement in indoor and outdoor pedestrians can be improved by 80% and greatly improve the adaptability and portability of sensors simultaneously,which meets the demand of practical engineering.

inertial sensors;modified complementary filter;identification of magnetic anomaly;adaptive Kalman filter;model identification of pedestrian movement

黄 欣(1993-),男,汉族,江苏省常州市人,2015年获南京航空航天大学自动化专业学士学位,同年进入南航导航研究中心读研,研究方向是惯性技术及组合导航研究,huangxin@nuaa.edu.cn;

熊 智(1976-),男,汉族,四川人,研究员,博士研究生导师,主要研究领域为惯性技术、景象匹配辅助导航、天文导航及惯性组合导航等领域。

项目来源:国家自然科学基金项目(61533008,61673208,61533009,61374115);江苏省六大人才高峰项目(2013-JY-013);江苏高校优势学科建设工程项目和江苏省“物联网与控制技术”重点实验室基金项目;中央高校基本科研业务费专项资金项目(NZ2016104,NS2017016);江苏省“333工程”科研立项项目(BRA2016405);留学人员择优项目(2016年);航空科学基金项目(20165552043);江苏省普通高校研究生科研创新计划项目(KYLX15_0264);江苏省自然科学基金项目(BK20141453)

2016-11-11 修改日期:2017-01-15

V249.32+8

A

1004-1699(2017)06-0878-08

C:7230

10.3969/j.issn.1004-1699.2017.06.013