CN106705968B - 基于姿态识别和步长模型的室内惯性导航算法 - Google Patents

Abstract

一种基于姿态识别和步长模型的室内惯性导航算法，涉及室内惯性导航与定位领域。本发明将惯性传感器固定在行人脚部，使用蓝牙将数据传输至智能手机，根据行人行走时脚部的姿态特点，采用多条件零速度检测法和姿态角检测法计算出行人的步频和步数；对行人每一步的惯导解算结果进行统计归类、建立步长模型，以行人脚部的姿态和步频为参考，实时校准惯导解算的结果；使用基于滑动均值滤波的动态补偿算法的电子罗盘数据为实时航向角，根据行人的步频和航向角变化来判断行人是否处于惯常运动姿态，以此为依据匹配步长模型。本发明解决了室内定位惯性导航技术中二次积分导致的误差累积导致定位失准等问题，保证了行人室内长距离导航的精确性。

Description

基于姿态识别和步长模型的室内惯性导航算法

技术领域:

本发明涉及室内惯性导航与定位领域，尤其是涉及一种利用惯性传感器和智能手机实现的基于行人脚部姿态识别和步长模型的行人室内惯性导航技术。

背景技术:

在室内环境，卫星卫星信号到达地面时信号较弱、不能穿透建筑物、无法发挥精确的定位作用。惯性导航是通过测量载体的加速度，并进行积分运算，获得载体瞬时速度和瞬时位置数据的技术。组成惯性导航系统的设备都安装在载体内，工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰，是一种自主式导航设备。惯性导航技术作为一种自主式导航技术，相对其他室内定位技术具有不需要事先在建筑物内布设接收装置、不受环境变化和信号阻挡等影响等优点，成为室内定位技术的研究热点。由于行人在走动时身体运动情况较复杂，而惯性导航算法存在误差累积，这些问题使得惯性导航应用在行人室内导航时的精确性很低，限制了它的广泛应用。

室内惯性导航技术的研究从上世纪90年代开始出现，Levi和Judd在1996年首次提出了行人航迹推算(Pedestrian Dead Reckoning,PDR)的概念，并在Point Research公司开发的PDR模块中进行了应用。由于人员的运动状态比较复杂，很多科研高校的研究倾向于将惯性传感器佩戴在室内人员身体的一个或多个不同部位，包含头、腿、腰或者脚等，来实现室内行人的导航功能。其中将MEMS(Micro-Electro-Mechanical System)惯性传感器佩戴在室内人员的脚部，通过测量获取室内人员的脚步的运动数据来解算室内人员的导航轨迹的实验效果较为精确。传统的惯性导航算法依靠对传感器加速度和速度的积分来计算运动距离，在应用到行人室内导航时，随着行人移动距离增加和航向角的变化，其累积误差越来越大进而导致精度不高。

发明内容:

针对上述现有技术的不足，本发明提出了一种以行人脚部姿态为判断、以步长为单位划分行人运动的导航算法。

一种基于姿态识别和步长模型的室内惯性导航算法，其特征是，包括以下步骤：

(1)行人惯性数据与智能手机传感器数据的采集、传输、存储以及滤波处理；

(2)多条件零速度判断法和行人脚部姿态判别相结合，解算得出行人的步频与步数特征；

(3)对行人的每一步分别进行惯性导航解算，建立步长模型，作为行人位移的参考值，根据行人运动状态调整步长结果。

进一步，采集行人惯性数据时，将惯性传感器佩戴在行人脚部，对惯性传感器数据进行初始校准，消除零偏，设置采样频率100Hz和波特率特征；使用蓝牙模块将器件与智能手机连接，通过手机端的程序记录蓝牙传输到手机的行人惯性数据和手机自身的电子罗盘数据，并存储在手机中。

进一步，对惯性传感器数据进行低通数字滤波，对电子罗盘数据进行滑动均值滤波，消除器件的噪声。

进一步，对处理后的数据进行多条件零速度检测：包括加速度幅值检测、角速度幅值检测和加速度方差检测，设定阈值，对三个检测条件进行与运算求出零速度时间范围；使用加速度计、角速度计和磁力计解算出器件的姿态角信息，根据姿态角检测在零速度检测结果的基础上得到行人零速度时刻。

进一步，进行惯性导航解算，将零速度时的行人加速度置零，把行人的运动以每一步为分割分别计算；统计每一步的步频和对应的惯性导航位移信息，将步频和姿态稳定时的结果进行分类统计并建立步长模型；

在行人姿态及步频变化、与模型不匹配时以惯性导航解算为位移结果，当惯性导航结果有累积误差累加的倾向时，把当解算结果不合理的步长舍弃并把该步频对应的步长模型信息作为参考结果，最终将位移及方向信息融合，得到行人导航信息，在手机端显示。

本发明所采用的技术方案是：基于智能手机和MEMS惯性传感器，使用零速度检测方法和脚部姿态判别方法将将行人运动分割成以单步为单位，分别解算单步位移并建立行人惯常步长模型，根据行人步频变化情况判断行人的运动状态实时校正步长算法，通过实时判断行人的运动情况相应匹配相应的步长算法，得到行人精确的运动轨迹。本发明的技术方案是：

(1)零速度检测和姿态角检测相结合的步频检测算法

为了针对行人运动的特点设计导航模型，需要对行人进行脚部姿态判断。行人在行走时，通过鞋底与地面的摩擦力，向运动方向跨出一步。鞋踏向地面、与地面完全接触、又离开地面。根据惯性传感器相对于地面有相对静止的一段时间，我们可以认为此时行人处于零速度状态，两个零速度之间，就是行人行走了一步。在这个“零速度”时刻，行人的加速度、角速度以及磁力计数据都在接近零值的状态，可以通过设置这些数据阈值的方法判断“零速度”点，然而这种判断方法在行人偶尔处于慢速或停顿的时刻会出现无效判定，影响计步精度；而随着脚的抬起和落下，脚部相对于水平面的俯仰角也有着一个循环变化的过程，在脚部处于地面时，俯仰角接近水平，而即使行人慢速行走时其俯仰角判断也不会受影响。所以本发明结合了零速度检测和姿态角检测，能够准确判断出行人行走的姿态，确定行人脚部的落地点，从而确定步数和步频。具体步骤如下：

1)对MEMS惯性传感器采集到的行人运动时的初始数据(包括加速度、角速度、磁力计)进行初始校准处理和滤波处理，滤除器件的漂移误差。

2)对加速度、角速度和磁力计数据进行数据分析，设定阈值，分别判定零速度时刻，建立零速度检测数据。

3)使用姿态解算算法计算行人的姿态角，包括横滚角、偏转角和俯仰角，利用俯仰角数据研究行人抬起和落下的动作。

4)采取零速度检测和姿态角检测双判断方法，当行人的姿态角变化不明显、有多极值出现的时间段，使用零速度检测判断法检测步数，在姿态角极值变化稳定时使用姿态角步态检测方法，并将两者结果结合，确定行人的零速度点。

5)通过零速度点，记录行人每一步的步频、步数，为下一步骤做准备。

(2)基于步长模型的实时校正惯性导航算法

考虑到行人运动的复杂性，本发明对误差累积使用校正机制。实验研究发现，人的步频和步幅之间有着对应关系，行人的步频一定时，其步幅是基本固定的。基于这一研究，本发明对行人的运动进行分割，把位移以每一步为单位分别进行惯性导航解算，并通过统计累积误差并未扩大的前10步的数据结果建立步长模型，在行人以惯常步态行走时以此模型为主要参考。步长的惯性导航算法可以很好的减少累积误差，即使是在长距离行走之后，惯性导航的计算结果仍处于较为精确的范围。基于步长的惯性导航算法具体包括以下步骤：

1)以零速度点划分，两个零速度点之间分别作为起始和终止间隔，通过对加速度的二次积分得出位移信息。

2)统计不同步频对应的步长结果，通过多步训练建立步频与步长对应的步长模型。

3)在行人实际行走过程中，通过步频和航向角判断行人是否处于惯常步态，如果是，则调用步长模型对惯导解算结果进行校正；如果不是，则使用惯导解算的结果。结合这两种结果输出相对精确的行人位置信息。

本发明与现有技术相比，具有的有益效果是：

1)本系统基于MEMS惯性传感器和智能手机，属于自主式导航设备，不需要在建筑物内布设信号接收装置，适应性强、抗干扰能力强。

2)使用多条件零速度检测法与基于姿态检测的姿态角检测法相结合来确定行人运动时的零速度点，计算行人的步频和步数，有效提高了对行人运动步态的判断精度。

3)使用基于步长模型的惯性导航算法，将行人的行走分割成以单步为单位分别计算，并训练出步长模型，在行人以惯常步态行走时参考步长模型的结果，有效避免了惯性导航算法的误差随着时间增加而逐渐累积，提高了定位精度。

附图说明：

图1是本发明系统的总体流程图；

图2是设备佩戴示图；

图3是步频/步数检测方法框图；

图4是以步长为单位的行人惯性导航算法框图；

图5是手机端行人轨迹显示软件界面；

具体实施方式：

下面结合附图和实例对本发明做进一步说明。本发明的系统的总体流程图见图1。

本发明使用了集成MPU6050惯性模块、AK8963磁力计模块以及蓝牙HC-06模块的惯性传感器，惯性传感器件通过蓝牙与安卓智能手机无线连接并且实时的将传感器测得的行人运动数据传输到手机中，同时调用手机内置的加速度传感器、电子罗盘数据，在智能手机中实现数据存储、处理以及导航输出。行人佩戴设备如图2所示。本发明针对MEMS惯性传感器本身精度不高、存在漂移误差导致惯导解算算法的结果存在累积误差的问题，根据行人运动时脚部的姿态特点，将行人位移分割以单步长为单位，通过建立步长模型来实时校正惯导解算结果的准确性。通过多条件零速度检测和行人姿态角检测算法相结合的步频算法计算出行人的步频和步数；通过惯导解算和实时建立步长模型并根据行人的步频、姿态变化特性实时校正步长结果、计算出行人的位移情况；结合基于滑动均值滤波的航向角算法判断行人的实时航向角，从而计算得出行人完整的运动轨迹；最终在手机上显示行人的运动轨迹，实现行人的室内导航功能。具体实施过程如下：

(1)基于加速度计、角速度计和磁力计的行人步频与步数算法。

步频/步数检测方法框图如图3所示。步骤如下：

1)对MPU6050传感器进行初始校准和低通滤波处理，滤除高于5Hz的高频信号，消除传感器的零偏和高频噪声，降低传感器自身的漂移误差。

2)使用多条件判断法判定行人的零速度点，分别是加速度幅值、加速度方差和角速度幅值三种检测方法，公式如下：

其中，

k为当前数据点，b表示载体坐标系b系，c₁、c₂和c₃取逻辑值0或1，

是三个轴向上的合加速度的幅值，其阈值th_amtr和th_amax分别设为0.1和1.0，若

在此范围内，则将c₁置1，表示此时为合加速度幅值判定的“零速度”时刻，否则置0，表示此时为零速度时刻；

是合加速度的方差，s是计算方差的数据间隔，一般设为3或4，其阈值th_a的大小设为0.5，若

在此范围内，则将c₂置1，表示此时为加速度方差判定的“零速度”时刻，否则置0；

是三个轴向上的合角速度的幅值，其阈值th_ω的大小设为6，若

在此范围内，则将c₃置1，表示此时为角速度判定的“零速度”时刻，否则置0；以上阈值的设定是根据实验行人的具体行走情况，经过大量的实验测试与统计，确定的最优零条件判断数值。在不同的行人及硬件情况下，可以重新设定其最优阈值。最后，将单个条件结果进行“与”运算，求出脚着地，即“零速度”的时刻：

c＝c₁*c₂*c₃

上式的c是逻辑“与”运算的结果。当c取值为1时，说明此刻是“零速度”时刻；当取值为0时，说明是行人运动的“非零”速度时刻。

多条件零速度检测判断法的优点是集合了多种行人瞬时的速度数据并求交集，可以有针对性的选择出各条件判断都符合的观察点；其缺点在于需要设立合理的阈值范围才能更准确的判断，如果阈值范围很小，会出现漏判零速度点的情况；而零速度范围比较大，则会出现零速度时刻比实际时间长的情况，影响下一步的惯导解算。针对这一问题，本发明又提出了姿态角检测以确定零速度点。

3)通过佩戴在行人脚部的惯性传感器解算行人行走时脚部的姿态数据，方法如下：

行人处于静止状态时测得加速度信息中的重力场分量在载体坐标系b系下的表达为

而重力场在导航坐标系下的分量为gⁿ＝[0 0 g]^T，则有转换关系为：

其中，

是由导航坐标系n系到载体坐标系b系下的转换矩阵，

分别是重力在b系下x、y、z三轴的分量。由上式可以计算获得横滚角γ和俯仰角θ：

此公式只适用于在静止状态计算姿态角，当传感器处于运动状态时，由于传感器本身存在加速度，与重力产生的加速度重合，而单纯依靠加速度计无法分离出重力分量，所以使用加速度计的作用是获得初始姿态角和姿态角校正，而运动时的姿态角需要通过陀螺仪计算得出。

一个动坐标系相对参考坐标系的方位可以完全由动坐标系依次绕3个不同的轴转动的3个角度来确定。如把载体坐标系作为动坐标系，把导航坐标系作为参考坐标系，则姿态角即为一组欧拉角，按一定的转动顺序得到导航坐标系到载体坐标系的关系。

上式中左侧，是本次更新后的欧拉角，对应横滚角γ、俯仰角θ和偏转角ψ。右侧，是上个周期测算出来的角度，以及三轴陀螺仪在周期t内测得的角速度。因此求解这个微分方程就能解算出当前的欧拉角。

其中

代表需要解算的欧拉角，k指当前数据点，t是惯性传感器的采样周期，取为0.01s，e_t为陀螺仪的零偏值。计算出陀螺仪三个轴的角度变化，再以加速度计算出的初始姿态角为基准，就能得出载体的姿态角数据。

行人在行走时其俯仰角有着周期性的变化，其变化幅度在-10度到90度之间，研究行人的步态可以发现，其极大值和极小值分别是在踏向地面时和离开地面时的姿态角，由此可以判断出行人的步数间隔。利用姿态角的判别方法的优点是可以根据行人运动姿态判断步数，判定结果更加直观、准确，缺点是可能出现多极大值、极小值情况，影响判断精度。所以在多条件零速度检测法的检测出零速度时间段的基础上，再利用姿态角检测准确判断出零速度点，经过试验验证，单纯使用多条件零速度判断法，步频检测结果的正确率在90％至95％之间，而使用姿态角与零速度判断结合算法的正确率可以达到98％以上，该算法有效提高了步频和步数的检测精度。

(2)基于滑动均值滤波的航向角更新算法

实验中模块被平稳固定在行人脚面，行人手持手机正对行走方向。在个人航迹推算导航系统中，对运动方向的估算主要是使用智能手机中的电子罗盘数据，电子罗盘在静止状态下航向角非常准确，可以作为参考；然而，当行人手持手机处于运动状态时，由于行人重心的移动和手部的抖动，电子罗盘的输出数据也相应的有一些偏移。我们采用滑动均值滤波算法对电子罗盘输出数据进行校正。滑动均值滤波器公式如下：

其中，k为当前数据点，为智能手机电子罗盘的输出原始数据，j取当前数据点以及当前数据点最近的m组数据，为滤波输出结果；m为一次进行滤波处理的数据个数，也称滑动滤波器阶数，本发明根据行人的步频特征和实验数据，将m取值为100。我们将m个连续的采样信号看成一个长度为m队列，随着滤波运算的进行，一个新数据获得后立即放入队列末尾，并抛弃以前队首的一个数据，这样不断地把获得新数据放队首，并且淘汰以前旧的数据，这样可以利用最新数据信息进行更新并且保证了运算的实时性。经实验验证，电子罗盘使用航向角更新算法处理后的数据保持了较好的平滑性，消除了由于设备在行人运动中产生的偏移误差。

(3)以步长为单位的行人惯性导航算法

行人惯性导航算法框图所示，步骤如下：

1)加速度积分及位移计算：

MEMS惯性传感器获得的是沿载体坐标系b系下三个轴向加速度数据a^b：

通过已经结算出的姿态角求出的坐标转换矩阵

通过

可以获得沿n系(即东北天坐标系，x轴以地理北向为正、y轴以地理北向为正、z轴以垂直于地理东向和北向的天向为正)下的输出数据aⁿ:

然后再将求得的aⁿ减去重力加速度，即可以获得运动物体在n系下的加速度数据aⁱⁿ。在本系统中，采用的MPU6050惯性传感器的采样频率设置为100Hz，能够得到采样间隔Δt＝0.01s，由于Δt较短，在十分短的时间内我们可以认为运动载体在做匀速直线运动。由牛顿第二定律可知，在导航坐标系下，速度变化量等于加速度值对极短时间的积分，即：

则，载体在导航坐标系下的速度为上一时刻的速度与瞬时加速度积分的累加：

再由位移公式，可得到n系下的运动物体位移的变化量：

最后，可以得到运动物体在n系下的位置为：

在确定了零速度点之后，两个零速度点之间就是每一步的步长，通过上文中的惯导解算算法可以计算出每一步的步长。

2)训练步长模型：

在步频稳定时，惯导解算计算出的步长在真实值之间振荡，其偏移大概在20％以内，随着时间的积累，这些误差会累积，导致导航出现较大偏差。根据在初始阶段误差并未累积时解算的步长计算结果训练出行人以固定步频运动的步长模型，取同一步频(步频差在5步/min范围以内)的前10步的步长为模型参考数据，以此模型作为长距离行走时步长的参考值。下表为一个步长模型的示例：

当行人当前步的步频在步长模型所包含的步频范围内时，将惯导解算的不合理结果阈值范围设置为该步步频对应步长模型结果的0.5倍以下或2倍以上，在这个范围内解算结果误差较大，可以认为这一步解算出的步长结果是不合理的，将其舍弃，解算结果出现不合理结果时，采用步长模型的数据作为参考结果；而当行人步频和航向角变化、即行人在做变向或变速运动时，将步频变化阈值设为0.2s、航向角变化阈值设为20度，超过这个阈值范围，认为行人较上一步其行走状态发生了变化，这时依然采用瞬时值更加精确的惯性导航计算结果。这两种匹配策略的目的是将惯导解算过程中出现明显误差的不合理数据进行纠正，根据行人行走时的运动特点实时校正惯导结算结果，保证导航算法始终处于一个比较精确的范围。

最后，在获得了步频、步长以及航向角的数据之后，经过每一步的累加，即可获得完整的位移数据。行人轨迹显示软件如图5所示。经过大量试验验证，传统惯性导航算法在行走距离增加时累积误差较大，在100m行走之后其解算结果可靠性变差，位置误差超过50％的情形频繁出现，而采取本文的基于姿态识别和步长模型的室内惯性导航算法，解算的准确性并没有随着距离的增加出现衰减，在100m行走之后惯导解算依然保持了95％以上的精确度。

Claims (3)

1.一种基于姿态识别和步长模型的室内惯性导航算法，其特征是，包括以下步骤：

(1)行人惯性数据与智能手机传感器数据的采集、传输、存储以及滤波处理；

(2)多条件零速度判断法和行人脚部姿态判别相结合，解算得出行人的步频与步数特征；

(3)对行人的每一步分别进行惯性导航解算，建立步长模型，作为行人位移的参考值，根据行人运动状态调整步长结果；

进行惯性导航解算，将零速度时的行人加速度置零，把行人的运动以每一步为分割分别计算；统计每一步的步频和对应的惯性导航位移信息，将步频和姿态稳定时的结果进行分类统计并建立步长模型；

在行人姿态及步频变化、与模型不匹配时以惯性导航解算为位移结果，当惯性导航结果有累积误差累加的倾向时，把当解算结果不合理的步长舍弃并把该步频对应的步长模型信息作为参考结果，最终将位移及方向信息融合，得到行人导航信息，在手机端显示。

2.根据权利要求1所述的基于姿态识别和步长模型的室内惯性导航算法，其特征在于，步骤(3)中的行人惯性导航算法，步骤如下：

1)加速度积分及位移计算：

MEMS惯性传感器获得的是沿载体坐标系b系下三个轴向加速度数据a^b：

通过已经解 算出的姿态角求出的坐标转换矩阵

通过

获得沿n系下的输出数据aⁿ:

然后再将求得的aⁿ减去重力加速度，即获得运动物体在n系下的加速度数据a^′n；通过惯性传感器的采样频率得到采样间隔Δt＝0.01s，由牛顿第二定律可知，在导航坐标系下，速度变化量等于加速度值对极短时间的积分，即：

则，载体在导航坐标系下的速度为上一时刻的速度与瞬时加速度积分的累加：

再由位移公式，得到n系下的运动物体位移的变化量：

最后，得到运动物体在n系下的位置为：

在确定了零速度点之后，两个零速度点之间就是每一步的步长，通过上文中的惯导解算算法计算出每一步的步长。

3.根据权利要求1所述的基于姿态识别和步长模型的室内惯性导航算法，其特征在于，步骤(3)具体为：

根据在初始阶段误差并未累积时解算的步长计算结果训练出行人以固定步频运动的步长模型，取步频差在5步/min范围以内的前10步的步长为模型参考数据，以此模型作为长距离行走时步长的参考值；

当行人当前步的步频在步长模型所包含的步频范围内时，将惯导解算的不合理结果阈值范围设置为该步步频对应步长模型结果的0.5倍以下或2倍以上，在这个范围内解算结果误差较大，认为这一步解算出的步长结果是不合理的，将其舍弃，解算结果出现不合理结果时，采用步长模型的数据作为参考结果；而当行人步频和航向角变化、即行人在做变向或变速运动时，将步频变化阈值设为0.2s、航向角变化阈值设为20度，超过这个阈值范围，认为行人较上一步其行走状态发生了变化，这时依然采用瞬时值更加精确的惯性导航计算结果；

在获得了步频、步长以及航向角的数据之后，经过每一步的累加，获得完整的位移数据。

Images