CN108592907A - 一种基于双向滤波平滑技术的准实时步进式行人导航方法 - Google Patents

Abstract

本发明属于行人导航定位技术领域，涉及一种基于双向滤波平滑技术的准实时步进式行人导航方法。该方法包括：采集人体行走数据；在第一前向算法通道，对采集到的人体行走数据进行误差校正，利用校正后的人体行走数据进行姿态和速度的捷联惯导解算；同时进行步态时相的检测和步进区间的划分，当检测到步态周期的支撑相时，在误差域进行姿态误差和速度误差的开环卡尔曼滤波估计；在后向算法通道，对第一前向算法通道中估计出的姿态误差和速度误差进行平滑处理；在第二前向算法通道，利用后向算法通道中平滑处理后的误差估计对第一前向算法通道中解算出的姿态和速度进行误差校正，对误差校正后的速度进行积分，解算出人体行走时的位置。

Description

一种基于双向滤波平滑技术的准实时步进式行人导航方法

技术领域

本发明属于行人导航定位技术领域，涉及一种基于双向滤波平滑技术的准实时步进式行人导航方法。

背景技术

目前，市场上基于GPS技术的定位设备发展迅速，市场占有率呈逐年上升趋势。但GPS技术易受工作环境的干扰，只适于室外没有遮挡的开阔环境，在室内、丛林、井下和洞穴等区域存在导航盲区，无法正常工作。因此，基于惯性传感器的行人导航技术得到了快速的发展，主要用于确定行人的实时位置和记录行人的行走轨迹，实现行人定位和导航的功能。

近年来，随着微机电(Micro-Electro-Mechanical Systems，MEMS)技术的不断发展，基于MEMS技术的惯性传感器(Inertial Measurement Unit，IMU)也有了很大的改进。MEMSIMU具有体积小、质量轻、功耗低、响应快、成本低、灵敏度高等优点，成为穿戴式行人导航技术的理想选择。

但是，市场上常见的MEMS IMU的精度大致在商业级和战术级水平，存在多种随机误差和***误差。而惯导技术是一种相对定位技术，导航误差具有随时间不断积累的特点。若单独基于MEMS IMU进行纯惯导解算，导航结果的位置误差会以导航时间三次方的趋势迅速发散，严重偏离实际位置，使***逐渐丧失导航功能，最终导致导航任务失败。这种固有的误差发散特性，使得MEMS IMU在实际的导航应用中受到很大的限制，其应用价值一度受到质疑。因此，在目前MEMS IMU技术的发展水平下，对导航误差的处理就显得异常重要。

基于MEMS IMU的行人导航***(Personal Navigation System，PNS)是在传统捷联惯导***(Strapdown Inertial Navigation System，SINS)的基础上，利用人体行走时足部运动特有的周期特性，通过零速校正(Zero Velocity Updates，ZUPT)算法，实现导航误差的估计和校正。基于卡尔曼滤波器(Kalman Filter，KF)的零速校正算法，充分利用了速度与姿态和位置之间的耦合关系，在消除速度误差的同时，可以估计和校正包含姿态误差和位置误差在内的更多其他导航误差。而基于单独前向卡尔曼滤波器的零速校正算法只在每个步态周期的支撑相内进行误差估计和校正，无法对摆动相内的误差进行及时有效的估计和校正，导致摆动相内***处于自由导航模式，导航误差迅速积累和传播，未得到任何有效地抑制和约束，而且会在***从摆动相过渡到支撑相时引起导航信息的突变，导致导航状态的不连读，尤其是当步态周期的支撑相不能被正确检测时，导航信息的突变更加明显，导致所估计的导航状态偏离其真实值。若加入平滑处理算法，又会引入算法延时，增加算法的时间复杂度，加大对存储空间的需求。

发明内容

由于现有技术中基于单向闭环卡尔曼滤波器的零速校正算法，只在每个步态周期的支撑相内进行误差估计和校正，无法对摆动相内的误差进行估计，在从摆动相过渡到支撑相时会引起导航信息的突变，尤其是当步态周期的支撑相不能被正确检测时，导航信息的突变更加明显，导致行人导航方法所估计的导航状态严重偏离其真实值。因此，本发明提出了一种基于双向滤波平滑技术的准实时步进式行人导航方法，来提行人导航方法的精确性、鲁棒性和平滑性。

双向滤波平滑算法由前向卡尔曼滤波算法和后向固定区间平滑算法构成，在实际的准实时应用中，前向滤波与后向平滑分开执行。以每个步态周期为区间，先执行前向卡尔曼滤波算法，再执行后向固定区间平滑算法。虽然后向平滑算法需要对整个步态周期内的误差进行逐点平滑估计，但为了确保传感器采样数据的连续性，所有误差估计、平滑和校正的操作只能在传感器连续两次数据采样之间的采样间隔内进行，即误差处理时间受限于传感器的采样频率和行人的行走步频。然而后向平滑算法的引入，需要保存前向滤波的状态估计值、状态估计误差协方差矩阵、状态转移矩阵和观测矩阵等，加大了对内存的需求，增加了算法的复杂度和计算量，延长了误差估计和校正的时间。因此，基于MEMS IMU的行人导航方法有三个关键问题亟需解决：

1、引入后向平滑技术进行准实时的误差估计；

2、简化滤波平滑模型匹配传感器的采样频率；

3、划分步态时相确定滤波平滑算法的执行区间。

本发明提供的基于双向滤波平滑技术的准实时步进式行人导航方法综合利用了捷联惯导算法、零速校正算法、卡尔曼滤波算法和固定区间平滑算法，采用前馈加反馈的复合控制策略和基于双向滤波平滑技术的误差估计方法，具有前向、后向、再前向三个步进式算法通道，构成了一个包含开环控制和闭环控制的复合控制***。

本发明的技术方案：

一种基于双向滤波平滑技术的准实时步进式行人导航方法，具体步骤如下：

步骤1，采集人体行走数据，人体行走数据包括人体行走时的三维加速度数据和三维角速度数据；人体行走数据通过固联于人体足部的惯性传感器来采集，再通过便携式导航上位机来存储和处理惯性传感器采集的人体行走数据；所述的惯性传感器为微惯性传感器，包括一个三轴加速度计和一个三轴陀螺仪；

步骤2，在第一前向算法通道中，先对采集到的人体行走数据进行误差校正，再对校正后的人体行走数据进行捷联惯导解算，获得人体行走时初步的姿态信息和速度信息；同时进行步态时相的检测和步进区间的划分，当检测到人体足部处于步态周期的支撑相时，在误差域进行姿态误差和速度误差的开环卡尔曼滤波估计；当划分出一个新的步进区间时，第一前向算法通道暂停，运行后向算法通道和第二前向算法通道；

步骤3，在后向算法通道中，对第一前向算法通道中估计出的姿态误差和速度误差进行平滑处理，得到经过平滑处理后的误差估计；

步骤4，在第二前向算法通道中，利用后向算法通道中平滑处理后的误差估计对第一前向算法通道中解算出的姿态信息和速度信息进行误差校正，再对误差校正后的速度信息进行积分，解算出人体行走时的位置信息，并返回步骤2继续运行第一前向算法通道；

所述的步骤2，具体过程如下：

利用公式(1)进行人体行走数据的误差校正，得到误差校正后的人体行走数据：

其中，k表示离散采样点，b表示载体坐标系，表示陀螺仪测量的原始角速度，ε^b表示陀螺仪零偏误差，K_G表示陀螺仪刻度系数误差，表示经过误差校正后的有效角速度；表示加速度计测量的原始加速度，表示加速度计零偏误差，K_A表示加速度计刻度系数误差，表示经过误差校正后的有效加速度；

利用误差校正后的人体行走数据进行姿态和速度的捷联惯导解算，得到人体行走时初步的姿态信息和速度信息，见公式(2)：

其中，n表示导航坐标系，Δt表示采样时间间隔，I表示适当维数的单位矩阵，gⁿ表示重力加速度；表示b系与n系之间的旋转变换矩阵，称为载体的姿态矩阵，Ω_k表示由构成的反对称矩阵，vⁿ表示速度；

在第一前向算法通道中，所述的步态时相的检测和步进区间的划分，包括：

通过零速检测算法对误差校正后的人体行走数据进行计算，获得用于实现零速检测的检验统计量，将检验统计量与阈值参数进行比较，实现步态时相的检测；阈值参数包含检测阈值和修正阈值，其中，检测阈值提前预设，用于步态时相的粗划分，修正阈值在通过聚类分析方法对粗划分结果进行分类的过程中自适应获得，修正阈值包括幅值阈值和时间阈值，用于步态时相的精划分，先进行摆动相的精划分再进行支撑相的精划分，当支撑相的持续时间满足时间阈值时划分出一个新的步进区间；

当检测到人体足部处于步态周期的支撑相时，在误差域进行姿态误差和速度误差的开环卡尔曼滤波估计，步骤如下：

利用公式(3)得到卡尔曼滤波器的状态模型：

其中，b表示载体坐标系，n表示导航坐标系，表示姿态误差，表示姿态误差导数，表示b系与n系之间的旋转变换矩阵，称为载体的姿态矩阵,表示陀螺仪测量噪声，δvⁿ表示速度误差，表示速度误差导数，表示经过误差校正后的有效加速度，表示加速度计测量噪声；

利用得到的卡尔曼滤波器的状态模型，在误差域进行间接滤波，将卡尔曼滤波器的状态向量降维为六维，并建立离散化的卡尔曼滤波状态方程，见公式(4)：

δx_k＝F_kδx_k-1+G_kw_k-1 (4)

其中，k表示离散采样点，δx表示被估计的状态向量，w表示过程噪声，F_k表示状态转移矩阵，G_k表示噪声传递矩阵，状态转移矩阵F_k中的矩阵S_k表示由n系中的向量构成的反对称矩阵，T表示转置矩阵；

当检测到人体足部处于步态周期的支撑相时，人体行走的速度为零，将捷联惯导算法解算出的速度信息作为速度误差的伪观测值提供给卡尔曼滤波器，对姿态误差和速度误差进行估计，建立离散化的卡尔曼滤波观测方程，见公式(5)：

z_k＝Hδx_k+η_k (5)

其中，z_k表示观测量，η_k表示观测噪声，H表示观测矩阵，H＝[0 I 0]；

惯性传感器每采集一次人体行走数据，都要利用捷联惯导算法对采集的人体行走数据进行解算，并执行卡尔曼滤波时间更新；只有当人体足部处于步态周期的支撑相时，才能执行卡尔曼滤波测量更新；

所述的步骤3，具体过程如下：

利用固定区间平滑算法对姿态误差和速度误差进行最优线性平滑处理，见公式(6)：

其中，k表示离散采样点，k∈(FI_end,FI_start]，FI_start表示步进区间的开始时刻，FI_end表示步进区间的结束时刻，表示状态δx_k+1的先验估计，δx_k+1表示点k+1时的状态向量，表示状态δx_k的后验估计，表示状态δx_k的平滑估计，表示状态δx_k+1的平滑估计，P_k+1k表示点k+1的先验估计误差协方差矩阵，P_k表示点k的后验估计误差协方差矩阵，表示点k的平滑估计误差协方差矩阵，表示点k+1的平滑估计误差协方差矩阵，A_k表示固定区间平滑增益矩阵，F_k+1表示点k+1的状态转移矩阵，T表示转置矩阵；

固定区间平滑算法得到的为平滑处理后的误差估计，包含姿态误差的和速度误差的平滑估计，见公式(7)：

其中，为姿态误差的平滑估计，为速度误差的平滑估计；

所述的步骤4，具体过程如下：

利用公式(8)进行姿态误差和速度误差校正，得到人体行走时最终的姿态信息和速度信息：

其中，k表示离散采样点，b表示载体坐标系，n表示导航坐标系，I表示适当维数的单位矩阵，表示误差校正后的姿态矩阵，表示捷联惯导算法解算出的初步姿态矩阵，Θ_k表示由姿态误差构成的反对称矩阵，其中表示固定区间平滑算法得到姿态误差的平滑估计；表示误差校正后的速度，表示捷联惯导算法解算出的初步速度，表示固定区间平滑算法得到速度误差的平滑估计；

将经过误差校正后的速度对时间进行积分，得到人体行走时最终的位置信息，见公式(9)：

其中，Δt表示采样时间间隔，表示位置。

本发明的有益效果：

本发明充分利用了人体行走时足部运动特有的周期特性，以步进的方式进行导航信息的解算和导航误差的估计、平滑和校正。该方法以每个步态周期为步进区间，第一前向算法通道进行姿态和速度的惯导解算及姿态误差和速度误差的开环卡尔曼滤波估计，后向算法通道进行姿态误差和速度误差的固定区间平滑处理，第二前向算法通道进行姿态误差和速度误差的校正以及位置信息的解算。通过引入一个步态周期的最大延时，使得步态周期中支撑相内的伪观测值在整个步态周期中都可得，不仅能够有效地消除测量值波动对步态时相检测的影响，避免由测量值波动引起的步态时相的误检测和漏检测，而且能够及时地估计和校正摆动相内的导航误差，实现导航过程中摆动相与支撑相之间的平滑过渡，避免基于单向闭环卡尔曼滤波器的导航方法在从摆动相过渡到支撑相时所引起的导航信息突变的问题，使得导航方法所估计的行人状态更加贴近真实状态，提高行人导航***的精确性和鲁棒性以及导航定位结果的平滑性。

同时，将原始捷联惯导***方程分解为不含噪声的确定性标称子***和含有随机噪声的误差子***，在误差域中通过误差子***方程进行导航误差的间接滤波。经过***分解之后，误差子***的非线性减弱，有利于降低卡尔曼滤波算法在进行泰勒级数展开时的高阶截断误差，从而提高行人状态的估计精度。引入间接滤波，允许将位置误差从开环卡尔曼滤波状态中排除，以简化双向滤波平滑模型，从而降低算法的时间复杂度、缩短计算时间并减少对存储空间的需求，确保传感器采样数据的连续性，再下一个采样数据到来之前完成所有导航信息的捷联惯导解算以及导航误差的估计、平滑和校正操作，并输出一组描述整个步态周期中行人状态的导航定位数据。

附图说明

图1为本发明实施例提供的基于双向滤波平滑技术的步进式行人导航方法的实现流程图；

图2为本发明实施例提供的步进式行人导航方法中步态时相划分方法的示意图；

图3为本发明实施例提供的步进式行人导航方法中步进区间划分方法的示意图；

图4为本发明实施例提供的步进式行人导航方法中由测量波动引起的伪支撑相以及伪支撑消除前后的速度误差估计曲线；

图5为本发明实施例提供的步进式行人导航方法中基于闭环卡尔曼滤波、开环卡尔曼滤波和固定区间平滑技术的速度误差估计曲线；

图6为本发明实施例提供的步进式行人导航方法中基于单向闭环卡尔曼滤波和双向滤波平滑技术的速度误差估计协方差曲线；

图7为本发明实施例提供的步进式行人导航方法中三个步进式算法通道在数据域和时间域上的作用范围示意图。

具体实施方式

以下结合附图和技术方案，进一步说明本发明的具体实施方式。

图1为本发明实施例提供的基于双向滤波平滑技术的准实时步进式行人导航方法的实现流程图。如图1所示，本发明实施例提供的基于双向滤波平滑技术的准实时步进式行人导航方法包括：

步骤1，采集人体行走数据。

其中，所述人体行走数据包括人体行走时的三维加速度数据和三维角速度数据。通过固联于人体足部的惯性传感器来采集人体行走数据，通过便携式导航上位机来存储和处理惯性传感器采集的人体行走数据。便携式导航上位机可以提供行人的姿态、速度和位置信息，实现行人定位和导航的功能。具体的，所述惯性传感器为微惯性传感器(MEMSIMU传感器)，所述微惯性传感器在封装时集成了一个三轴加速度计和一个三轴陀螺仪，可以同时测量物体在三维空间中的加速度和角速度信息，通过捷联惯导解算，实现三维空间内的导航和定位功能。为了充分利用人体行走时足部运动特有的周期特性，MEMS IMU传感器固联在行人的足部。便携式上位机包括便携式PC、PDA、智能手机等终端设备，具有简洁直观的人机界面，可以支持行人导航软件的安装和应用，主要用于存储和处理惯性传感器采集的人体行走数据，确定行人的实时位置和记录行人的行走轨迹，实现行人定位和导航的功能。MEMS IMU传感器与上位机之间可以通过RS232、RS422、RS485和RS644等串行通信方式进行有线连接，也可以通过蓝牙、Zigbee、WIFI和GPRS等无线通信方式进行无线连接。便携式上位机的数据传输模块可将上述导航信息通过无线通讯的方式传输给现场或远程的监控中心，使现场或远程的监控中心能够实时监测行人的速度和位置变化情况。

步骤2，在第一前向算法通道中，先对采集到的人体行走数据进行误差校正，再对校正后的人体行走数据进行捷联惯导解算，获得人体行走时初步的姿态信息和速度信息；同时进行步态时相的检测和步进区间的划分，当检测到人体足部处于步态周期的支撑相时，在误差域进行姿态误差和速度误差的开环卡尔曼滤波估计；当划分出一个新的步进区间时，第一前向算法通道暂停，运行后向算法通道和第二前向算法通道。

其中，通过零速检测算法实现步态时相的检测是为了确定卡尔曼滤波测量更新的执行区间，将捷联惯导算法在支撑相中解算出的速度作为速度误差的“观测值”，用于构建卡尔曼滤波观测方程，执行卡尔曼滤波测量更新。所谓“误差域”是指通过误差子***进行误差状态的滤波估计，即针对姿态误差和速度误差建模，而不是针对姿态和速度的建模，这种间接估计***状态的方法为间接滤波。其中，步态时相的检测是步进区间划分的前提，当检测到支撑相的持续时间满足时间阈值时便可划分出一个新的步进区间，并在此步进区间内进行误差的平滑和校正。为了简化所述双向滤波平滑模型以减少计算量和缩短计算时间，确保传感器采样数据的连续性，第一前向算法通道不进行位置信息的捷联惯导解算和位置误差的卡尔曼滤波估计。

其中，捷联惯导算法是一种根据加速度数据和角速度数据解算载体姿态、速度及位置的算法。零速检测算法是一种进行步态时相的检测和步进区间的划分的算法。卡尔曼滤波算法是通过***输入输出观测数据，对***状态进行最优估计的算法，本发明中的卡尔曼滤波算法以开环的方式运行。所述开环卡尔曼滤波指第一前向算法通道中捷联惯导算法完全自治，不受卡尔曼滤波算法的影响，不进行误差的反馈与校正。前向算法通道即按照采样序列号递增的顺序进行计算，前后算法通道即按照采样序列号递减的顺序进行计算。

a)、进行导航信息的捷联惯导解算：

所述行人导航方法在进行捷联惯导解算之前，需要通过以下公式进行人体行走数据的误差校正：

其中，k表示离散采样点，b表示载体坐标系，表示陀螺仪测量的原始角速度，ε^b表示陀螺仪零偏误差，K_G表示陀螺仪刻度系数误差，表示经过误差校正后的有效角速度；表示加速度计测量的原始加速度，表示加速度计零偏误差，K_A表示加速度计刻度系数误差，表示经过误差校正后的有效加速度。

利用误差校正后的人体行走数据进行姿态和速度的捷联惯导解算，得到人体行走时初步的姿态信息和速度信息：

其中，n表示导航坐标系；Δt表示采样时间间隔，I表示适当维数的单位矩阵，gⁿ表示重力加速度；表示b系与n系之间的旋转变换矩阵，称为载体的姿态矩阵，采用矩阵指数函数的一阶Padé逼近法进行姿态更新；Ω_k表示由构成的反对称矩阵，其中表示b系中经过误差校正后的有效角速度；表示b系中经过误差校正后的有效加速度，通过旋转变换矩阵将其转换到n系中进行重力加速度的补偿，表示速度。所述初步的姿态信息和速度信息指纯惯导解算出的姿态和速度，未进行任何形式的误差估计和校正。

b)、进行导航误差的卡尔曼滤波估计：

在第一前向算法通道中，当检测到人体足部处于步态周期的支撑相时，将捷联惯导算法计算出的速度信息作为速度误差的观测值，在误差域进行姿态误差和速度误差的开环卡尔曼滤波估计，卡尔曼滤波器的状态模型由姿态误差方程和速度误差方程组成：

其中，表示姿态误差，表示姿态误差导数，δvⁿ表示速度误差，表示速度误差导数，表示陀螺仪测量噪声，表示加速度计测量噪声。

为了简化所述双向滤波平滑模型，卡尔曼滤波器的状态向量降维为六维，即被估计的状态向量T表示转置矩阵，***噪声为建立线性离散卡尔曼滤波状态方程如下：

δx_k＝F_kδx_k-1+G_kw_k-1 (13)

其中，状态转移矩阵F_k和噪声传递矩阵G_k分别为：

状态转移矩阵F_k中的矩阵S_k表示由n系中的向量构成的反对称矩阵。

卡尔曼滤波算法分为时间更新和测量更新两个阶段。惯性传感器每进行一次数据采样，捷联惯导算法都要对采集到的数据进行解算，每一次惯导解算后都要执行卡尔曼滤波时间更新。而只有在步态周期的支撑相中，即足部与地面相接触的静止时间片段内，才能将当前时刻捷联惯导算法解算出的速度信息作为速度误差的伪观测值提供给卡尔曼滤波器，对姿态误差和速度误差进行估计，执行卡尔曼滤波测量更新。

建立离散化的卡尔曼滤波观测方程如下：

z_k＝Hδx_k+η_k (14)

其中，z_k表示观测量，η_k表示观测噪声，H表示观测矩阵，H＝[0 I 0]。

卡尔曼滤波器采用“预测-校正”的算法框架，针对所描述的误差状态模型，卡尔曼滤波器的递推公式总结如下：

时间更新(预测)：

测量更新(校正)：

其中，表示状态δx_k的先验估计，表示状态δx_k的后验估计；P_k|k-1表示先验估计误差协方差矩阵，P_k表示后验估计误差协方差矩阵；Q_k-1表示***噪声协方差矩阵，即w_k-1～(0,Q_k-1)；R_k表示观测噪声协方差矩阵，即η_k～(0,R_k)；K_k表示卡尔曼滤波增益矩阵，T表示转置矩阵。

一个完整的卡尔曼滤波器，作用相当于一个误差估计器，在每个步态周期的支撑相内估计出整个步态周期中积累的导航误差。为了在前向滤波算法的基础上直接执行后向平滑算法，导航阶段中采用开环卡尔曼滤波算法进行导航误差估计。

c)、进行步态时相的划分：

本发明提出的基于双向滤波平滑技术的零速校正算法，其精度依赖于步态时相的正确检测和步态周期的正确划分，核心是支撑相检测的精确性和可靠性。前向卡尔曼滤波算法将支撑相内解算的速度作为速度误差的伪观测值引入卡尔曼滤波观测方程，后向固定区间平滑算法充分利用支撑相内的伪观测值对整个步态周期内的误差进行反向平滑估计。

本发明提供的行人导航方法，根据步态周期中支撑相的特点，采用零速检测算法实现步态时相的检测。在步态周期的支撑相内，足底与地面保持相对静止，传感器的对地绝对速度趋于零。安装在人体足部的惯性传感器能精确地测量到足跟与地面的每一次触碰以及随后的静止时间片段。零速检测是行人导航***的关键技术之一，零速检测的可靠性直接关系到零速校正算法的正确性，是进行误差估计、平滑和校正的必要前提。步态时相的误检测和漏检测，都将引入不同程度的导航误差，降低行人导航方法的精确性和可靠性。

图2为本发明实施例提供的步进式行人导航方法中步态时相划分方法的示意图。通过零速检测算法对误差校正后的人体行走数据进行计算，获得用于实现零速检测的检验统计量，将所述的检验统计量与阈值参数进行比较，实现步态时相的检测。

所述零速检测方法的阈值参数包含检测阈值和修正阈值。检测阈值提前预设，通过将所述检验统计量与检测阈值进行比较实现步态时相的粗划分，当所述检验统计量大于检测阈值时，检测时刻的步态时相为摆动相；当所述检验统计量小于检测阈值时，检测时刻的步态时相为支撑相。

修正阈值分为幅值阈值和时间阈值，幅值阈值用于对所述摆动相进行精划分，时间阈值用于对所述支撑相进行精划分，先进行摆动相的精划分再进行支撑相的精划分。步态时相的精划分是对粗划分结果的修正，用以消除由测量波动引起的步态时相的误检测和漏检测。由于待分类的数目确定为真伪两类，选用简单易行、快速有效的K-均值聚类分析方法，根据步态时相粗划分的结果自适应获取相关修正阈值，无需人工设定和修改。

当所述摆动相的摆动幅值小于幅值阈值时，该摆动相为伪摆动相，将所述伪摆动相与相邻的支撑相融合形成新的支撑相；当所述支撑相的持续时间小于时间阈值时，该支撑相为伪支撑相，将所述伪支撑相还原到摆动相。只有当所述支撑相的持续时间大于时间阈值时，才可在该支撑相内执行卡尔曼滤波测量更新。

在行人的行走过程中，只有步态时相发生状态转移时，才能对上一个完整的步态时相进行摆动幅值或者持续时间的验证，因此步态时相的精划分具有一个步态时相的最大延时。所述准实时步进式行人导航方法，本身就存在一个步态周期的最大延时，因此，步态时相的精划分不会引入新的时间延时。

d)、进行步进区间的划分：

本发明提供的步进式行人导航方法，充分利用了人体行走时足部运动特有的周期特性，以每个步态周期为区间，以步进的方式进行导航信息的解算和导航误差的校正。行人每走一步，执行一次基于双向滤波平滑技术的零速校正算法，输出一次描述整个步态周期的导航定位数据，***具有一个步态周期的最大延时。零速校正的执行频率和定位数据的输出频率与行人的行走步频相关，正常行走时，每隔一秒左右就会进行一次误差估计、平滑和校正，能给及时有效地消除速度误差并约束位置误差。经过零速校正后，导航结果的位置误差不再以导航时间三次方的趋势发散，而是与行走的步数成正比例关系。

本发明提供的准实时步进式行人导航方法，为了充分利用步态周期中支撑相内的有效伪观测值，步进区间的分界点在支撑相中期(Midstance)内选取，分界点距离摆动相开始时刻的时间阈值与步态时相精划分中的时间阈值相同。

图3为本发明实施例提供的步进式行人导航方法中步进区间划分方法的示意图。当前步进区间开始时刻用FI_start表示，当零速检测方法检测到步态周期支撑相的开始时刻后，就需要对支撑相的持续时间进行判断，当支撑相的持续时间满足时间阈值时，当前时刻的采样点选取为步进区间的分界点，确定出当前步进区间的结束时刻，用FI_end表示，从而划分出一个新的步进区间

[FI_start FI_end]，并在此区间内进行导航误差的后向固定区间平滑处理。

本发明提供的步进式行人导航方法，由于延时的存在，允许进行步态时相的精划分，第一前向算法通道中可以灵活地进行卡尔曼滤波测量更新的挂起和补充执行，能够有效地消除测量值波动对步态时相检测的影响，避免由测量值波动引起的步态时相的误检测和漏检测，提高步态时相检测的鲁棒性。

当步态时相从摆动相转移到支撑相后，如果所述摆动相的摆动幅值大于幅值阈值，则该摆动相内不执行卡尔曼滤波测量更新，且所述支撑相内也暂时不执行卡尔曼滤波测量更新，而是将其挂起，直至该支撑相的持续时间大于时间阈值后，再从该支撑相的开始时刻起补充执行卡尔曼滤波测量更新，并划分出一个新的步进区间。如果该支撑相的持续时间小于时间阈值时便转移到了新的摆动相，且新摆动相的摆动幅值大于幅值阈值，则该支撑相为伪支撑相，无需补充执行卡尔曼滤波测量更新。

当步态时相从摆动相转移到支撑相后，如果所述摆动相的摆动幅值小于幅值阈值，则该摆动相为伪摆动相，并将其融合到相邻的支撑相中。如果相邻支撑相由于持续时间小于时间阈值而处于卡尔曼滤波测量更新的挂起状态时，则该伪摆动相内继续保持卡尔曼滤波测量更新的挂起状态，重新进行支撑相持续时间的判断。如果相邻支撑相未处于卡尔曼滤波测量更新的挂起状态时，则从该摆动相的开始时刻起补充执行卡尔曼滤波测量更新。

在行人导航过程中，以摆动相内测量值的局部下降波动为例，图4为本发明实施例提供的步进式行人导航方法中由测量波动引起的伪支撑相以及伪支撑消除前后的速度误差估计曲线。从图4可以看出，在伪支撑相内进行基于零速校正的卡尔曼滤波测量更新，不仅导致导航误差的错误估计引起导航信息的突变，而且造成对行走速度和行走步幅的估计不足，降低导航***的导航精度。从上图也可以看出，伪支撑相的持续时间较短，通过进行时间阈值的验证可以将其消除。

步骤3，在后向算法通道中，对第一前向算法通道中估计出的姿态误差和速度误差进行平滑处理，得到经过平滑处理后的误差估计。

在行人导航方法在后向算法通道中，根据人体行走时足部运动特有的周期特性，最优线性平滑算法选用以每个步态周期为区间的固定区间平滑算法。固定区间平滑算法的步进区间为[FI_start,FI_end]，其中FI_start＜FI_end，固定区间长度N为步态周期的长度，即N＝FI_end-FI_start。其中，所述后向算法通道是指按照采样序列号递减的顺序进行计算，即从FI_end到FI_start。最优线性平滑问题就是基于已知的N个观测值{y_N,y_N-1,…,y₀}，求状态x_k的最优线性估计其中k＜N。所述行人导航方法中采用的后向平滑算法是由Rauch等人在卡尔曼滤波算法的基础上提出的Rauch-Tung-Striebel(RTS)固定区间平滑算法。不同步态周期所对应的步进区间长度是可变的，而单个步态周期所对应的步进区间长度是固定的，即固定区间平滑算法中观测区间是固定的。

前向卡尔曼滤波算法和后向固定区间平滑算法相结合构成双向滤波平滑算法，前向滤波与后向平滑分开进行，先执行前向滤波，再执行后向平滑，两者交替迭代，后向固定区间平滑算法是在前向卡尔曼滤波算法的基础上进行的反向滤波。基于双向滤波平滑技术的零速校正算法作为准实时步进式的误差处理方法引入，以每个步态周期为区间以步进的方式进行导航误差的估计、平滑与校正。每个步态周期中，前向滤波和后向平滑各进行一次，并以平滑估计值作为最终的误差估计结果，以提高数据处理的精度。

后向固定区间平滑算法的引入，需要保存前向卡尔曼滤波的先验和后验状态估计和先验和后验状态估计误差协方差矩阵P_k|k-1和P_k以及状态转移矩阵F_k。在步态周期的摆动相中，由于只执行卡尔曼滤波时间更新，有P_k＝P_k|k-1。后向固定区间平滑算法是在前向卡尔曼滤波算法的基础上进行的反向滤波，递推公式总结如下：

其中，k∈(FI_end,FI_start]，表示状态δx_k的先验估计，表示状态δx_k的后验估计，表示状态δx_k的平滑估计，P_k|k-1表示先验估计误差协方差矩阵，P_k表示后验估计误差协方差矩阵，表示平滑估计误差协方差矩阵，A_k表示固定区间平滑增益矩阵，初始边界条件由前向卡尔曼滤波器提供，即T表示转置矩阵。

图5为本发明实施例提供的步进式行人导航方法中基于闭环卡尔曼滤波、开环卡尔曼滤波和固定区间平滑技术的速度误差估计曲线。从图5可以看出，单向闭环卡尔曼滤波器只有在从摆动相过渡到支撑相后才进行导航误差的估计和校正，不但未对摆动相内的导航误差进行估计和校正，而且还会引起导航信息的突变。后向固定区间平滑算法的引入，能够及时地校正摆动相内的导航误差，避免单向卡尔曼滤波器只在从摆动相过渡到支撑相后才进行误差校正而引起导航信息的突变，实现导航过程中摆动相与支撑相之间的平滑过渡，从而使得误差滤波曲线更加平滑。

图6为本发明实施例提供的步进式行人导航方法中基于单向闭环卡尔曼滤波和双向滤波平滑技术的速度误差估计协方差曲线。从图6可以看出，单向闭环卡尔曼滤波器的估计误差协方差在摆动相内一直单调递增，而在从摆动相过渡到支撑相后突然大幅降低。采用双向滤波平滑技术后，不但降低了估计误差协方差的幅值，而且协方差在整个步态周期中对称分布，协方差幅值出现在摆动相中期，距离两侧支撑相最远，协方差变化曲线更符合人体行走时足部运动的周期特性。

步骤4，在第二前向算法通道中，利用后向算法通道中平滑处理后的误差估计对第一前向算法通道中解算出的姿态信息和速度信息进行误差校正，再对误差校正后的速度信息进行积分，解算出人体行走时的位置信息。其中，所述第二前向算法通道是指按照采样序列号递增的顺序进行计算，即从FI_start到FI_end。

在行人导航方法的每个步进区间内，后向算法通道中采用固定区间平滑算法平滑处理后的误差中包含两个误差向量，即

姿态误差的平滑估计：

速度误差的平滑估计：

利用平滑处理后的误差估计和对第一前向算法通道中捷联惯导算法解算出的姿态矩阵和速度进行误差校正，即可得到最终的姿态信息和速度信息。姿态和速度的误差校正公式如下：

其中k∈[FI_start,FI_end]，Θ_k表示由姿态误差构成的反对称矩阵，表示误差校正后的姿态矩阵，表示误差校正后的速度。

在行人导航***的导航阶段中，在n系中将经过误差校正后的速度对时间进行积分，即可得到人体行走时最终的位置信息。位置的捷联惯导解算公式如下：

其中，表示位置。

图7为本发明实施例提供的步进式行人导航方法中三个步进式算法通道在数据域和时间域上的作用范围示意图。如图7所示，在实际的准实时应用中，每个步进区间的结束时刻后，从数据轴而言，后向算法通道需要对整个步态周期内的导航误差进行逐点平滑，第二前向算法通道需要对整个步态周期内的导航误差进行逐点校正，二者在数据域的运行区间为k∈[FI_start,FI_end]，其中FI_start＜FI_end，且FI_end-FI_start＝N，N表示固定区间长度；从时间轴而言，为了确保传感器采样数据的连续性，需要在下一个步进区间的开始时刻前，完成所有导航信息的捷联惯导解算以及导航误差的估计、平滑和校正操作，并步进式地输出一次描述整个步态周期的导航定位数据，后向算法通道和第二前向算法通道在时间域的运行区间为k∈(FI_end,FI_start)，其中FI_end＜FI_start，FI_start为下一个步进区间的开始时刻，且FI_start-FI_end＝Δt，Δt表示采样时间间隔。可见，行人导航过程中，所有导航误差的估计、平滑和校正操作只能在传感器相邻两次数据采样之间的采样间隔内进行，即三个算法通道的运行时间受限于传感器的采样频率和行人的行走步频，尤其是后向算法通道和第二前向算法通道。而后向固定区间平滑算法的引入，加大了对内存的需求，增加了算法的复杂度和计算量，延长了误差估计和校正的时间。因此，需要简化所述双向滤波平滑模型以减少计算量和缩短计算时间，本发明先进行姿态和速度的捷联惯导解算，再采用六维的双向滤波平滑算法对姿态误差和速度误差进行估计、平滑和校正，并将经过误差校正后的速度对时间进行积分直接解算出行人的位置信息。双向滤波平滑模型的状态变量从常用的十五维或九维降维到六维中，减少了算法的复杂度和计算量，缩短了算法的运行时间，确保了传感器采样数据的连续性。

本发明的准实时步进式行人导航方法，利用基于双向滤波平滑技术的零速校正算法作为准实时步进式的误差处理方法引入，以每个步态周期为区间以步进的方式进行导航误差的估计、平滑与校正。每个步态周期中，前向滤波和后向平滑各进行一次，并以平滑估计值作为最终的误差估计结果，提高数据处理的精度。该方法通过引入一个步态周期的最大延时，不仅能够有效地消除测量值波动对步态时相检测的影响，避免由测量值波动引起的步态时相的误检测和漏检测，提高步态时相检测的鲁棒性，而且可以充分利用步态周期中未来支撑相内的伪观测值信息对摆动相内的导航误差进行估计，并及时地校正摆动相内的导航误差，避免单向卡尔曼滤波器只在从摆动相过渡到支撑相后才进行误差校正而引起导航信息的突变，实现导航过程中摆动相与支撑相之间的平滑过渡，从而使得误差滤波曲线更加平滑，导航定位精度也随之提高。

以上是行人导航方法在导航阶段的工作原理，即根据行人的初始位置、初始方位以及惯性传感器采集的人体行走数据进行行人的定位和导航，确定行人的实时位置和记录行人的行走轨迹。在导航模式开始之前，行人导航设备处于启动模式。启动模式下，行人导航设备处于静止状态，在一个开环的前向算法通道中，根据惯性传感器采集的人体静态数据进行捷联惯导的初始对准和惯性传感器的误差估计。

本实施例提供的基于双向滤波平滑技术的准实时步进式行人导航方法，充分利用人体行走时足部运动特有的周期特性，以步进的方式进行导航信息的解算和导航误差的估计、平滑和校正。该方法以每个步态周期为步进区间，第一前向算法通道进行姿态和速度的惯导解算及姿态误差和速度误差的开环卡尔曼滤波估计，后向算法通道进行姿态误差和速度误差的固定区间平滑处理，第二前向算法通道进行姿态误差和速度误差的校正以及位置信息的解算。通过引入一个步态周期的最大延时，不仅能够有效地消除测量值波动对步态时相检测的影响，避免由测量值波动引起的步态时相的误检测和漏检测，而且能够及时地校正摆动相内的导航误差，实现导航过程中摆动相与支撑相之间的平滑过渡，提高行人导航方法的精确性、鲁棒性和平滑性。

Claims (1)

1.一种基于双向滤波平滑技术的准实时步进式行人导航方法，其特征在于，具体步骤如下：

步骤1，采集人体行走数据，人体行走数据包括人体行走时的三维加速度数据和三维角速度数据；人体行走数据通过固联于人体足部的惯性传感器来采集，再通过便携式导航上位机来存储和处理惯性传感器采集的人体行走数据；所述的惯性传感器为微惯性传感器，包括一个三轴加速度计和一个三轴陀螺仪；

步骤2，在第一前向算法通道中，先对采集到的人体行走数据进行误差校正，再对校正后的人体行走数据进行捷联惯导解算，获得人体行走时初步的姿态信息和速度信息；同时进行步态时相的检测和步进区间的划分，当检测到人体足部处于步态周期的支撑相时，在误差域进行姿态误差和速度误差的开环卡尔曼滤波估计；当划分出一个新的步进区间时，第一前向算法通道暂停，运行后向算法通道和第二前向算法通道；

步骤3，在后向算法通道中，对第一前向算法通道中估计出的姿态误差和速度误差进行平滑处理，得到经过平滑处理后的误差估计；

步骤4，在第二前向算法通道中，利用后向算法通道中平滑处理后的误差估计对第一前向算法通道中解算出的姿态信息和速度信息进行误差校正，再对误差校正后的速度信息进行积分，解算出人体行走时的位置信息，并返回步骤2继续运行第一前向算法通道；

所述的步骤2，具体过程如下：

利用公式(1)进行人体行走数据的误差校正，得到误差校正后的人体行走数据：

其中，k表示离散采样点，b表示载体坐标系，表示陀螺仪测量的原始角速度，ε^b表示陀螺仪零偏误差，K_G表示陀螺仪刻度系数误差，表示经过误差校正后的有效角速度；表示加速度计测量的原始加速度，▽^b表示加速度计零偏误差，K_A表示加速度计刻度系数误差，表示经过误差校正后的有效加速度；

利用误差校正后的人体行走数据进行姿态和速度的捷联惯导解算，得到人体行走时初步的姿态信息和速度信息，见公式(2)：

其中，n表示导航坐标系，Δt表示采样时间间隔，I表示适当维数的单位矩阵，gⁿ表示重力加速度；表示b系与n系之间的旋转变换矩阵，称为载体的姿态矩阵，Ω_k表示由构成的反对称矩阵，vⁿ表示速度；

在第一前向算法通道中，所述的步态时相的检测和步进区间的划分，包括：

通过零速检测算法对误差校正后的人体行走数据进行计算，获得用于实现零速检测的检验统计量，将检验统计量与阈值参数进行比较，实现步态时相的检测；阈值参数包含检测阈值和修正阈值，其中，检测阈值提前预设，用于步态时相的粗划分，修正阈值在通过聚类分析方法对粗划分结果进行分类的过程中自适应获得，修正阈值包括幅值阈值和时间阈值，用于步态时相的精划分，先进行摆动相的精划分再进行支撑相的精划分，当支撑相的持续时间满足时间阈值时划分出一个新的步进区间；

当检测到人体足部处于步态周期的支撑相时，在误差域进行姿态误差和速度误差的开环卡尔曼滤波估计，步骤如下：

利用公式(3)得到卡尔曼滤波器的状态模型：

其中，b表示载体坐标系，n表示导航坐标系，表示姿态误差，表示姿态误差导数，表示b系与n系之间的旋转变换矩阵，称为载体的姿态矩阵,表示陀螺仪测量噪声，δvⁿ表示速度误差，表示速度误差导数，表示经过误差校正后的有效加速度，表示加速度计测量噪声；

利用得到的卡尔曼滤波器的状态模型，在误差域进行间接滤波，将卡尔曼滤波器的状态向量降维为六维，并建立离散化的卡尔曼滤波状态方程，见公式(4)：

δx_k＝F_kδx_k-1+G_kw_k-1 (4)

其中，k表示离散采样点，δx表示被估计的状态向量，w表示过程噪声，F_k表示状态转移矩阵，G_k表示噪声传递矩阵，状态转移矩阵F_k中的矩阵S_k表示由n系中的向量构成的反对称矩阵，T表示转置矩阵；

当检测到人体足部处于步态周期的支撑相时，人体行走的速度为零，将捷联惯导算法解算出的速度信息作为速度误差的伪观测值提供给卡尔曼滤波器，对姿态误差和速度误差进行估计，建立离散化的卡尔曼滤波观测方程，见公式(5)：

z_k＝Hδx_k+η_k (5)

其中，z_k表示观测量，η_k表示观测噪声，H表示观测矩阵，H＝[0 I 0]；

惯性传感器每采集一次人体行走数据，都要利用捷联惯导算法对采集的人体行走数据进行解算，并执行卡尔曼滤波时间更新；只有当人体足部处于步态周期的支撑相时，才能执行卡尔曼滤波测量更新；

所述的步骤3，具体过程如下：

利用固定区间平滑算法对姿态误差和速度误差进行最优线性平滑处理，见公式(6)：

其中，k表示离散采样点，k∈(FI_end,FI_start]，FI_start表示步进区间的开始时刻，FI_end表示步进区间的结束时刻，表示状态δx_k+1的先验估计，δx_k+1表示点k+1时的状态向量，表示状态δx_k的后验估计，表示状态δx_k的平滑估计，表示状态δx_k+1的平滑估计，P_k+1|k表示点k+1的先验估计误差协方差矩阵，P_k表示点k的后验估计误差协方差矩阵，表示点k的平滑估计误差协方差矩阵，表示点k+1的平滑估计误差协方差矩阵，A_k表示固定区间平滑增益矩阵，F_k+1表示点k+1的状态转移矩阵，T表示转置矩阵；

固定区间平滑算法得到的为平滑处理后的误差估计，包含姿态误差的和速度误差的平滑估计，见公式(7)：

其中，为姿态误差的平滑估计，为速度误差的平滑估计；

所述的步骤4，具体过程如下：

利用公式(8)进行姿态误差和速度误差校正，得到人体行走时最终的姿态信息和速度信息：

其中，k表示离散采样点，b表示载体坐标系，n表示导航坐标系，I表示适当维数的单位矩阵，表示误差校正后的姿态矩阵，表示捷联惯导算法解算出的初步姿态矩阵，Θ_k表示由姿态误差构成的反对称矩阵，其中表示固定区间平滑算法得到姿态误差的平滑估计；表示误差校正后的速度，表示捷联惯导算法解算出的初步速度，表示固定区间平滑算法得到速度误差的平滑估计；

将经过误差校正后的速度对时间进行积分，得到人体行走时最终的位置信息，见公式(9)：

其中，Δt表示采样时间间隔，表示位置。