CN109827577A - 基于运动状态检测的高精度惯性导航定位算法 - Google Patents

Abstract

本发明公开了一种基于运动状态检测的高精度惯性导航定位算法，包括以下步骤：获取加速度计和陀螺仪的采集数据，并进行预处理，以消除加速度计和陀螺仪的系统误差和行人行走摩擦产生的噪声；将行人步态进行分类，获得正常速步态和快速步态；根据行人的不同步态，对任一步态下，每一步幅陀螺仪采集的角速度数据进行偏差矫正；基于INS和误差卡尔曼滤波器，利用加速度数据和角速度数据，根据行人的不同步态，动态判断行人行走的零速时刻；在零速时刻下，对行人姿态、速度、高度信息进行更新；通过得到的行人位置坐标，重建出行人2D运动轨迹，融合高度更新算法后，准确获得行人任一时刻的位置高度，并重建出适用于上下楼时行人的3D运动轨迹。

Description

基于运动状态检测的高精度惯性导航定位算法

技术领域

本发明涉及定位技术领域，尤其是基于运动状态检测的高精度惯性导航定位算法。

背景技术

室内导航系统，是指置身于建筑物内，进行定位自身的位置及建筑物内其他设施的位置的系统，例如停车场车辆定位、医疗设备定位等。在建筑物室内，由于受水泥墙体的阻挡等影响，导航信号衰减太快，全球定位系统(Global Positioning System,GPS)无法使用。目前，常见的室内定位技术分为两类：需要外部设施或先验数据库，不需要外部设施或者先验数据库。其中，需要外部设施或先验数据库的如红外光、超声波和超宽带(UltraWide Band,UWB)等基于基础设施的方法，需要建立相应源的初始条件，成本很高。而成本较低的WIFI指纹定位技术，易受环境干扰，指纹采集工作需要大量的人员来进行，并且要定期进行维护，技术难以扩展。同样地，成本较低的蓝牙和射频识别(Radio FrequencyIdentification，RFID)技术，其传输范围小，限制了其用于定位的有效性。

如今，惯性导航定位技术是一种完全自主式的导航技术，它不依赖于导航卫星、无线基站、电子标签等任何辅助设备和先验数据库，不易受到外界干扰，并且成本低，功耗低，体积小，重量轻，应用场景广泛。惯性导航系统(Inertial Navigation System,INS)是一种利用安装在运载体上的陀螺仪和加速度计来测定运载体位置的一个系统。通过陀螺仪和加速度计的测量数据，可以确定运载体在惯性参考坐标系中的运动，同时也能够计算出运载体在惯性参考坐标系中的位置。但是，导航信息经过积分而产生，定位误差随时间而增大，长期精度差，需要一些方法来抑制由于传感器测量量带来的位置和航向的误差。零速更新技术(Zero Velocity Update,ZUPT)可以减小定位误差。行人步行周期分为站立相和摆动相，即当行人行走时，脚在静止的站立相和动态的摆动相中交替变化。站立相时，将速度归零是零速更新技术的基本思想。如果单独使用ZUPT技术将会造成行程距离误差仍然较大.Foxlin第一次将ZUPT和扩展卡尔曼滤波技术(Extended Kalman Filter，EKF)结合，检测到站立相并且将ZUPT作为伪测量量输入到EKF导航误差矫正系统中，这使得EKF在站立相时矫正速度误差，消除了立方时间累积误差，取而代之的是随步数线性增长的累积误差。同时，ZUPT使得EKF也可以追溯性的矫正在摆动相时产生的漂移。本文把Foxlin提出的基于卡尔曼滤波的框架INS+ZUPT+EKF，称为IEZ框架。但是通过分析发现，仅仅使用这一框架，由于陀螺仪的偏差，会使得航向上有比较大的累积误差，但是，零速更新阶段只更新了速度，航向误差不能得到矫正，这会引起很大的定位误差。同时，不同的行人步态，加速度和角速度变化频率也不同，IEZ并没有区分行人步态，只用阈值法确定零速区间适用性并不高。并且只用这一框架，两步之间也会有明显的高度漂移。

Borenstein等人提出了一种称为启发式漂移(Heuristic Drift Reduction，HDR)技术来减少航向误差，这是基于建筑物内的大多数走廊和路径都是直的假设。HDR算法的思想是检测行人何时直行，直行时对陀螺仪偏差进行补偿，以便减少航向误差。利用零角速度更新(Zero Angular Update,ZARU)消除了站立相阶段的航向误差。但是陀螺仪存在固有误差，如果在使用陀螺仪数据之前，能对数据进行误差补偿，这将使航向误差减少很多。用磁力计数据来校准陀螺仪数据是一种方法，但是磁力计极易受室内磁场环境的干扰，用磁力计数据来校准陀螺仪数据并不总是可靠，同时增大了惯导系统的成本、功耗、计算复杂度。也有人通过建立陀螺仪误差模型，来消除陀螺仪的固有偏差，但是噪声拟合系数的确定，需要大量的实验数据，运算量大运算速度慢，除了较高的计算复杂度，该模型也不能通用于不同的传感器，对于简单的惯导系统来说，代价较大。

无论是ZUPT还是ZARU，都需要对站立相进行检测，典型的方法是基于阈值法。大多数文献采用了加速度阈值，也有采用角速度阈值或者是二者结合。此外，还有部分算法采用集成其他可携带传感器的方案，如压力传感器，肌电信号传感器等。而行人行走的周期与行走速度有着密不可分的关系，如何在不同的速度下仍能比较准确的检测到站立相十分重要。在站立相时加入多个观测更新的变量，才能使系统定位更加精确。

现如今，大多数的研究人员只关注到了二维平面的定位，忽略了高度的矫正，尤其是上下楼时，准确和稳健的楼层定位更加有用，并且通常在室内定位中起着重要作用。将气压计与IMU相结合，可以消除纯地惯性元件作为多层建筑中的加速度计和陀螺仪带来的高度漂移。然而，气压计易随着温度变化，强制通风以及突然气压变化等而漂移。气压计的使用也增大了惯导系统的成本和功耗。

因此，急需要提出一种高精度、简化计算工作量的惯性导航定位算法。

发明内容

针对上述问题，本发明的目的在于提供一种基于运动状态检测的高精度惯性导航定位算法，本发明采用的技术方案如下：

基于运动状态检测的高精度惯性导航定位算法，包括以下步骤：

获取加速度计和陀螺仪的采集数据，并进行预处理，以消除加速度计和陀螺仪的系统误差和行人行走摩擦产生的噪声；所述采集数据包括加速度计的加速度数据和陀螺仪的角速度数据。

将行人步态进行分类，获得正常速步态和快速步态。

根据行人的不同步态，对任一步态下，每一步幅陀螺仪采集的角速度数据进行偏差矫正；

基于INS和误差卡尔曼滤波器，利用加速度数据和角速度数据，根据行人的不同步态，动态判断行人行走的零速时刻。

在零速时刻下，对行人姿态、速度、高度信息进行更新。

通过得到的行人位置坐标，重建出行人2D运动轨迹，融合高度更新算法后，准确获得行人任一时刻的位置高度，并重建出适用于上下楼时行人的3D运动轨迹。

进一步地，所述加速度计和陀螺仪的采集数据预处理，包括以下步骤：

加速度校准：校准后的三轴加速度的表达式为：

其中，a_x、a_y、a_z分别为x、y、z轴方向校准后的加速度值，其单位为g； 分别为x、y、z轴方向的初始测量值；S_ax、S_ay、S_az分别为x、y、z轴方向加速度计的灵敏度；b_ax、b_ay、b_az分别为x、y、z轴方向加速度计的偏差值。

陀螺仪校准：校准后的三轴角速度g_x、g_y、g_z的表达式为：

其中，分别为x、y、z轴方向的初始测量值；S_gx、S_gy、S_gz分别为x、y、z轴方向陀螺仪的灵敏度；τ为陀螺仪的测量当前温度；b_gx、b_gy、b_gz分别为x、y、z轴方向在25℃下陀螺仪的偏差值；f_x、f_y、f_z分别为x、y、z轴方向陀螺仪温度偏差敏感度；α_x、α_y、α_z分别为x、y、z轴方向陀螺仪的偏置漂移补偿参数。

分别对所述校准后的角速度和加速度进行滑动均值滤波，其表达式为：

其中，y[n]为滑动均值滤波后的加速度和角速度，x[n]表示经校准后的角速度和加速度，N为均值滤波器中的点数，取值为4。

更进一步地，所述行人步态分类，获得正常速步态和快速步态，包括以下步骤：

以校准和滤波后的角速度数据为参考，设定角速度阀值为0.45°/s；若角速度大于角速度阀值划分为行人的摆动相，否则划分为站立相；

若连续10个角速度采样点均属于摆动相，则将所述10个采样点的第一个采样点，标记为step_s，所述step_s记为摆动相的起点；

若连续10个角速度采样点均属于站立相，则将所述10个采样点的第一个采样点，标记为step_e，所述step_e记为摆动相的终点；

将相邻的step_s和/或step_e之间的间距设定为步幅。

将任一个步幅持续时长设定大于M个采样点，以排除捕捉错误的步幅；所述M为大于100的自然数。

若任一个步幅中站立相小于P个采样点，则该步幅被判断为快速步态，该步幅中每个采样时刻点为快速步态采样点，并标记为1；若任一个步幅中站立相大于或等于P个采样点，该步幅被判断为正常速步态，该步幅中每个采样时刻点为正常速步态采样点，并标记为0；所述P为大于50、且小于100的自然数。

更进一步地，所述任一步态的陀螺仪的采集数据进行偏差矫正，包括以下步骤：

建立陀螺仪的误差模型ω_k，其表达式为：

e_k＝ω_bias+ω_{s_noise} (3-2)

其中，k表示当前采样时刻点，为经校准和滤波处理后的角速度，e_k为根据不同步态所构建的误差值，ω_bias为陀螺仪偏差，ω_{s_noise}为高斯白噪声；

分别设定正常速步态和快速步态的陀螺仪偏差值，并利用构建的误差值e_k对每一步幅陀螺仪采集的角速度数据进行偏差矫正，包括以下步骤：

提取当前步幅，选取站立相的角速度数据的最小值ω_min；

若当前步幅被标记为1，当前步幅的陀螺仪偏差ω_bias为当前站立相角速度最小值ω_min的b₁/e倍；若当前步幅被标记为2，当前步幅的陀螺仪偏差ω_bias为当前站立相角速度最小值ω_min的b₂/e倍；所述步幅被标记为1即当前步幅为正常速步态，所述步幅被标记为2即当前步幅为快速步态；所述e的取值是当前站立相陀螺仪采集角速度数据的量级与陀螺仪静止时采集的角速度数据的量级的比值，一般是10或100；所述b₁、b₂的取值是，以陀螺仪静止时采集的角速度数据的值为参考，b₂/b₁的比值接近于站立相时快速步态与正常速步态陀螺仪数据的比值，一般b₁取1，b₂/b₁为大于1小于2的小数。

优选地，利用根据行人每步的行走步态，偏差矫正后的角速度数据和经过校准且滤波后的加速度数据，求得行人的初始位置坐标。包括以下步骤：

将行人的速度和航向角度设置为零。

根据矫正后的加速度计算初始旋转矩阵C_k，其表达式为：

其中，q₀、q₁、q₂、q₃为四元数，其表达式如下：

其中，pitch为行人行走的俯仰角，yaw为行人行走的偏航角，roll为行人行走的翻滚角；

对公式(6-3)进行两次积分，获得行人的初始位置坐标，其表达式为

其中，a_k表示在参考坐标系下，经过校准且滤波后的三轴加速度数据，C_k表示旋转矩阵，表示在身体坐标系下，经过校准且滤波后的三轴加速度数据，g表示重力单位，表示当前采样时刻的速度，其中上标'-'表示预测值，v_k-1表示上一采样时刻的速度，表示当前采样时刻的位置，p_k-1表示上一采样时刻的位置。

优选地，基于误差卡尔曼滤波器，得到行人位置、姿态的误差向量，更新出准确的位置、姿态，包括以下步骤：

定义行人定位的误差向量δ_xk，其表达式为：

其中，表示行人的姿态角误差，δω_k表示行人的角速度误差，δp_k表示行人的位置误差，δv_k表示行人的速度误差，δa_k表示行人的加速度误差。

预测阶段：建立误差协方差矩阵其表达式为：

其中，Q_K为过程噪声协方差矩阵，F_K为状态传输矩阵；

所述状态传输矩阵F_K的表达式为：

其中，S_K为偏斜对称矩阵，为旋转矩阵，Δt为采样周期，I为3阶单位矩阵；

所述公式(5-3)中，偏斜对称矩阵S_K的表达式如下：

其中，a_k(1),a_k(2),a_k(3)分别为参考坐标系下，经过校准且滤波后的三轴加速度数据；

所述公式(5-3)中，旋转矩阵的表达式为：

所述公式(5-5)中，Ω_k为角速度的偏斜对称矩阵，其表达式为：

其中，w_k(1),w_k(2),w_k(3)分别为根据行人行走步态，偏差矫正后的三轴角速度数据。

判断是否进入更新阶段，具体步骤如下：

设置两个站立相检测器，即快速步态的站立相检测器和正常速步态的站立相检测器；若检测到标记为2的采样点则进入快速步态的站立相检测器；若检测到标记为1的采样点则进入正常速步态的站立相检测器；

任一检测器的逻辑与门D，其表达式为：

其中，a_min表示设定的站立相时的加速度数据的最小值，a_max表示设定的站立相时的加速度数据的最大值，σ_amax表示设定的站立相时的加速度方差的最大值，ω_max表示设定的站立相时的角速度数据的最大值，a_k表示当前站立相时刻参考坐标系下，经过校准且滤波后的加速度数据，表示当前站立相时刻参考坐标系下，经过校准且滤波后的加速度的方差，ω_k表示当前站立相时刻的角速度数据；

当D＝1时，进入更新阶段。

更新阶段：建立站立相时的零速测量的误差向量δ_xk的更新公式，其表达式为：

其中，K为卡尔曼增益，z_k表示为站立相时的测量误差，H表示为测量矩阵；

所述公式(5-8)中，卡尔曼增益K的表达为：

其中，表示误差协方差矩阵，H表示为测量矩阵，R表示测量噪声协方差矩阵，其为8×8的对角矩阵，主对角线元素为：

所述主对角线元素σω_xσω_yσω_z，σp_z，σv_xσv_yσv_z分别为站立相时偏航角、三轴角速度、高度、三轴速度的测量噪声；

所述公式(5-9)中，测量矩阵H的表达式为：

其中，I_3×3表示3阶单位矩阵，0_1×3一行三列零矩阵，0_3×33阶零矩阵；

所述公式(5-8)中，站立相时的测量误差z_k的表达式为

其中，表示启发式航向漂移算法输出值，表示零角速度更新输出角速度误差，p_ZHU表示零高度更新输出的高度误差，表示零速度更新输出的速度误差；所述的零高度更新算法，当行人在走平路时，p_ZHU取当前测得的高度坐标与0的差值为高度误差；当行人在上下楼时，p_ZHU取当前测得的高度坐标与上一时刻高度坐标的差值为高度误差；

行人步态的预测的错误状态值为零，即可简化误差向量δ_xk；

更新误差协方差矩阵：

将所述误差向量δ_xk传递到惯性导航系统，校正位置p_k、速度v_k、角速度ω_k和表示姿态的旋转矩阵C_k，其表达式为

所述公式(5-15)中，δΩ_k为姿态角误差的偏斜对称矩阵，其表达式为：

其中，分别表示三轴姿态角误差。

更进一步地，通过得到的行人位置坐标，重建出行人2D运动轨迹，融合高度更新算法后，准确获得行人任一时刻的位置高度，并重建出适用于上下楼时行人的3D运动轨迹，包括以下步骤：

楼梯与平台的识别区分，建立每一个步幅的状态n，其取值为0、1、2；所述步幅的状态n为0表示当前步幅处于平台上；所述步幅的状态n为1表示当前步幅迈了一阶台阶；所述步幅的状态n为2表示当前步幅迈了2阶台阶，步幅的状态n表达式为：

其中，Δh₀表示两步之间的高度差，n₁表示台阶高度下限，n₂表示台阶高度上限。

轨迹重建，建立高度模型，其表达式为：

h＝∑Δh (6-6)

其中，Δh表示每步变化的高度，Δh＝nΔh_stair，n为每一个步幅的状态，Δh_stair为固定值阶的高度。

以行人的初始位置坐标为起点，连续采集步态数据，并叠加高度模型模型，获得行走的高度和轨迹。

进一步地，所述楼梯与平台的识别区分中：

若连续3个步幅状态序列为211，则将连续3步态标记为210或211；

若连续3个步幅状态序列为112，则将连续3步态标记为122；

若连续3个步幅状态序列为212，则将连续3步态标记为222；

若连续3个步幅状态序列为n₁0n₃，则将该步幅后连续3步态分部标记为n₁2n₃；所述n₁和n₃为1或2。

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

本发明仅采用加速度计和陀螺仪进行行人位置定位，无需其他基础设施和数据库，进而降低了设备投入成本。本发明利用行人步态规律，处理陀螺仪的角速度偏差，在提高定位准确率的同时，也无需借助磁力计以简化计算过程。本发明在零速时加入航向和高速的更新，有效提高了定位精度，减小了航向误差，使算法适用性更高。与此同时，高度估计无需使用气压计，定位过程中不受外界环境的干扰。综上所述，本发明具有降低设备投入成本、降低计算复杂度、提高定位精度等优点，在定位技术领域具有很高的实用价值和推广价值。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需使用的附图作简单介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对保护范围的限定，对于本领域技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本发明的流程图。

具体实施方式

为使本申请的目的、技术方案和优点更为清楚，下面结合附图和实施例对本发明作进一步说明，本发明的实施方式包括但不限于下列实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

实施例

如图1所示，本实施例提供了基于运动状态检测的高精度惯性导航定位算法，该算法仅基于加速度计和陀螺仪的采集数据进行定位，具体来说：

第一步，获取加速度计和陀螺仪的采集数据，并进行预处理，以消除加速度计和陀螺仪的系统误差和行人行走摩擦产生的噪声；其中，所述采集数据包括加速度计的加速度数据和陀螺仪的角速度数据。在本实施例中，加速度数据和角速度数据的预处理包括以下步骤：

(11)加速度校准：校准后的三轴加速度的表达式为：

其中，a_x、a_y、a_z分别为x、y、z轴方向校准后的加速度值，其单位为g； 分别为x、y、z轴方向的初始测量值；S_ax、S_ay、S_az分别为x、y、z轴方向加速度计的灵敏度；b_ax、b_ay、b_az分别为x、y、z轴方向加速度计的偏差值。

(12)陀螺仪校准：校准后的三轴角速度g_x、g_y、g_z的表达式为：

其中，分别为x、y、z轴方向的初始测量值；S_gx、S_gy、S_gz分别为x、y、z轴方向陀螺仪的灵敏度；τ为陀螺仪的测量当前温度；b_gx、b_gy、b_gz分别为x、y、z轴方向在25℃下陀螺仪的偏差值；f_x、f_y、f_z分别为x、y、z轴方向陀螺仪温度偏差敏感度；α_x、α_y、α_z分别为x、y、z轴方向陀螺仪的偏置漂移补偿参数。

(13)分别对所述校准后的角速度和加速度进行滑动均值滤波，其表达式为：

其中，y[n]为滑动均值滤波后的加速度和角速度，x[n]表示经校准后的角速度和加速度，N为均值滤波器中的点数，取值为4。

第二步，将行人步态进行分类，获得正常速步态和快速步态，陀螺仪的采样频率为256Hz，根据站立相占整个步幅的比例来确定每一步的步态。在本实施例中分类的具体步骤如下：

(21)以校准和滤波后的角速度数据为参考，设定角速度阀值为0.45°/s；若角速度大于角速度阀值的步态划分为摆动相，否则划分为站立相。

(22)若连续10个步态采集点均为摆动相，则标记step_s；将所述step_s的10个步态采集点的第一个步态采集点记为摆动相的起点。

(23)若连续10个步态采集点均为站立相，则标记step_e；将所述step_e的10个步态采集点的第一个步态采集点，记为摆动相的终点。

(24)将相邻的step_s和/或step_e之间的间距设定为步幅。

(25)将任一个步幅持续时长设定大于110个采样点，以排除捕捉错误的步幅。

在本实施例中，若一个步幅中站立相小于70个采样点，则该步幅被判断为快速步态，该步幅中每个采样时刻点为快速步态采样点，标记为1；若大于等于70个采样点，该步幅被判断为正常速步态，该步幅中每个采样时刻点为正常速步态采样点，标记为0。

第三步，根据区分的步态，任一步态下对每一步幅陀螺仪采集的角速度数据进行偏差矫正，具体如下：

建立陀螺仪的误差模型ω_k，其表达式为：

e_k＝ω_bias+ω_{s_noise} (3-2)

其中，k表示当前采样时刻点，为经校准和滤波处理后的角速度，e_k为根据不同步态所构建的误差值，ω_bias为陀螺仪偏差，ω_{s_noise}为高斯白噪声。

分别设定正常速步态和快速步态的陀螺仪偏差值，根据本文所选传感器，设正常速步态下ω_bias为当前站立相角速度最小值ω_min的1/e倍，快速步态下ω_bias为当前站立相角速度最小值ω_min的1.5/e倍。利用所述误差模值e_k对每一步幅陀螺仪的采集的角速度数据进行偏差校正，将每一个步幅中采样点的角速度值设为ω_k。

第四步，基于INS和误差卡尔曼滤波器，利用加速度数据和角速度数据，根据行人的不同步态，动态判断行人行走的零速时刻。

第五步，在零速时刻下，对行人姿态、速度、高度信息进行更新，具体如下：

(51)定义行人定位的误差向量δ_xk，其表达式为：

其中，表示行人的姿态角误差，δω_k表示行人的角速度误差，δp_k表示行人的位置误差，δv_k表示行人的速度误差，δa_k表示行人的加速度误差。

(52)预测阶段：建立误差协方差矩阵其表达式为：

其中，Q_K为过程噪声协方差矩阵，F_K为状态传输矩阵。

所述状态传输矩阵F_K的表达式为：

其中，S_K为偏斜对称矩阵，为旋转矩阵，Δt为采样周期，I为3阶单位矩阵。

所述公式(5-3)中，偏斜对称矩阵S_K的表达式如下：

其中，a_k(1),a_k(2),a_k(3)分别为参考坐标系下，经过校准且滤波后的三轴加速度数据；

所述公式(5-3)中，旋转矩阵的表达式为：

所述公式(5-5)中，Ω_k为角速度的偏斜对称矩阵，其表达式为：

其中，w_k(1),w_k(2),w_k(3)分别为根据行人行走步态，偏差矫正后的三轴角速度数据。

(53)判断是否进入更新阶段，具体步骤如下：

(531)设置两个站立相检测器，即快速步态的站立相检测器和正常速步态的站立相检测器；若检测到标记为2的采样点则进入快速步态的站立相检测器；若检测到标记为1的采样点则进入正常速步态的站立相检测器；

(532)任一检测器的逻辑与门D，其表达式为：

其中，a_min表示设定的站立相时的加速度数据的最小值，a_max表示设定的站立相时的加速度数据的最大值，σ_amax表示设定的站立相时的加速度方差的最大值，ω_max表示设定的站立相时的角速度数据的最大值，a_k表示当前站立相时刻参考坐标系下，经过校准且滤波后的加速度数据，表示当前站立相时刻参考坐标系下，经过校准且滤波后的加速度的方差，ω_k表示当前站立相时刻的角速度数据；

(533)当D＝1时，进入更新阶段。

(54)更新阶段：建立站立相时的零速测量的误差向量δ_xk的更新公式，其表达式为：

其中，K为卡尔曼增益，z_k表示为站立相时的测量误差，H表示为测量矩阵。

所述公式(5-8)中，卡尔曼增益K的表达为：

其中，表示误差协方差矩阵，H表示为测量矩阵，R表示测量噪声协方差矩阵，其为8×8的对角矩阵，主对角线元素为：

所述主对角线元素σω_xσω_yσω_z，σp_z，σv_xσv_yσv_z分别为站立相时偏航角、三轴角速度、高度、三轴速度的测量噪声。

所述公式(5-9)中，测量矩阵H的表达式为：

其中，I_3×3表示3阶单位矩阵，0_1×3一行三列零矩阵，0_3×33阶零矩阵。

所述公式(5-8)中，站立相时的测量误差z_k的表达式为

其中，表示启发式航向漂移算法输出值，表示零角速度更新输出角速度误差，p_ZHU表示零高度更新输出的高度误差，表示零速度更新输出的速度误差。所述的零高度更新算法，当行人在走平路时，p_ZHU取当前测得的高度坐标与0的差值为高度误差；当行人在上下楼时，p_ZHU取当前测得的高度坐标与上一时刻高度坐标的差值为高度误差；

行人定位预测的错误状态值为零，即可简化误差向量δ_xk；

更新误差协方差矩阵：

将所述误差向量δ_xk传递到惯性导航系统，校正位置p_k、速度v_k、角速度ω_k和表示姿态的旋转矩阵C_k，其表达式为

所述公式(5-15)中，δΩ_k为姿态角误差的偏斜对称矩阵，其表达式为：

其中，分别表示三轴姿态角误差。

第六步，通过得到的行人位置坐标，重建出行人2D运动轨迹，融合高度更新算法后，准确获得行人任一时刻的位置高度，并重建出适用于上下楼时行人的3D运动轨迹，包括以下步骤：

(61)求得行人的初始位置坐标的具体步骤如下：

(611)将行人的速度和航向角度设置为零。

(612)根据矫正后的加速度计算初始旋转矩阵C_k，其表达式为：

其中，q₀、q₁、q₂、q₃为四元数，其表达式如下：

其中，pitch为行人行走的俯仰角，yaw为行人行走的偏航角，roll为行人行走的翻滚角；

(613)对公式(6-3)进行两次积分，获得行人的初始位置坐标，其表达式为

其中，a_k表示在参考坐标系下，经过校准且滤波后的三轴加速度数据，C_k表示旋转矩阵，表示在身体坐标系下，经过校准且滤波后的三轴加速度数据，g表示重力单位，表示当前采样时刻的速度，其中上标'-'表示预测值，v_k-1表示上一采样时刻的速度，表示当前采样时刻的位置，p_k-1表示上一采样时刻的位置。

(62)另外，在本实施例中，基于误差卡尔曼滤波器，得到行人位置、姿态的误差向量，更新出准确的位置、姿态，包括以下步骤：

(621)楼梯与平台的识别区分，建立每一个步幅的状态n，其取值为0、1、2；所述步幅的状态n为0表示当前步幅处于平台上；所述步幅的状态n为1表示当前步幅迈了一阶台阶；所述步幅的状态n为2表示当前步幅迈了2阶台阶，步幅的状态n表达式为：

其中，Δh₀表示两步之间的高度差，n₁表示台阶高度下限，n₂表示台阶高度上限。

(622)逻辑排除器，其具体如下：

(6221)绑了传感器脚步单步移动一阶楼梯后即n＝1时，下一步只可能是在平地上或者是走两阶楼梯即下一个状态只可能是n＝0/n＝2，故检测到连续的状态标签211视为逻辑错误，将其标记为210或221，检测到112，将其标记为122。

(6222)检测到连续的状态标签212视为逻辑错误，将其标记为222。

(6223)检测到两个非0的状态标签之间存在状态标签0，一般将其之间的错误状态标记为2。

(623)轨迹重建，建立高度模型，其表达式为：

h＝∑Δh (6-6)

其中，Δh表示每步变化的高度，Δh＝nΔh_stair，n为每一个步幅的状态，Δh_stair为固定值阶的高度。

(624)以行人的初始位置坐标为起点，连续采集传感器数据，并叠加高度模型模型，获得行走的高度和轨迹。

本发明通过对加速度计的加速度数据和陀螺仪的角速度数据进行预处理，以消除加速度计和陀螺仪的系统误差和行人行走摩擦产生的噪声。同时，根据行人不同的步态，对任一步态行人每一步的角速度数据进行偏差矫正，以提高定位准确率。并且，根据行人不同的步态，动态地确定了不同步态下的零速时刻，使零速更新方法对于行人的适用性更高。提出的高度更新模型，除了在零速时刻，针对上下楼和非上下楼的行人进行零高度更新，更使该发明适用于行人上下楼时的位置高度更新与轨迹重建。本发明无需外设其他基础设施和数据库，无需借助非惯性传感器，降低设备投入成本和计算复杂度，与现有技术相比，具有突出的实质性特点和显著的进步，在室内定位技术领域具有广阔的市场前景。

上述实施例仅为本发明的优选实施例，并非对本发明保护范围的限制，但凡采用本发明的设计原理，以及在此基础上进行非创造性劳动而作出的变化，均应属于本发明的保护范围之内。

Claims (8)

1.基于运动状态检测的高精度惯性导航定位算法，其特征在于，包括以下步骤：

获取加速度计和陀螺仪的采集数据，并进行预处理，以消除加速度计和陀螺仪的系统误差和行人行走摩擦产生的噪声；所述采集数据包括加速度计的加速度数据和陀螺仪的角速度数据；

将行人步态进行分类，获得正常速步态和快速步态；

根据行人的不同步态，对任一步态下，每一步幅陀螺仪采集的角速度数据进行偏差矫正；

基于INS和误差卡尔曼滤波器，利用加速度数据和角速度数据，根据行人的不同步态，动态判断行人行走的零速时刻；

在零速时刻下，对行人姿态、速度、高度信息进行更新；

通过得到的行人位置坐标，重建出行人2D运动轨迹，融合高度更新算法后，准确获得行人任一时刻的位置高度，并重建出适用于上下楼时行人的3D运动轨迹。

2.根据权利要求1所述的基于运动状态检测的高精度惯性导航定位算法，其特征在于，所述加速度计和陀螺仪的采集数据预处理，包括以下步骤：

加速度校准：校准后的三轴加速度的表达式为：

其中，a_x、a_y、a_z分别为x、y、z轴方向校准后的加速度值，其单位为g； 分别为x、y、z轴方向的初始测量值；S_ax、S_ay、S_az分别为x、y、z轴方向加速度计的灵敏度；b_ax、b_ay、b_az分别为x、y、z轴方向加速度计的偏差值；

陀螺仪校准：校准后的三轴角速度g_x、g_y、g_z的表达式为：

其中，分别为x、y、z轴方向的初始测量值；S_gx、S_gy、S_gz分别为x、y、z轴方向陀螺仪的灵敏度；τ为陀螺仪的测量当前温度；b_gx、b_gy、b_gz分别为x、y、z轴方向在25℃下陀螺仪的偏差值；f_x、f_y、f_z分别为x、y、z轴方向陀螺仪温度偏差敏感度；α_x、α_y、α_z分别为x、y、z轴方向陀螺仪的偏置漂移补偿参数；

分别对所述校准后的角速度和加速度进行滑动均值滤波，其表达式为：

其中，y[n]为滑动均值滤波后的加速度和角速度，x[n]表示经校准后的角速度和加速度，N为均值滤波器中的点数，取值为4。

3.根据权利要求2所述的基于运动状态检测的高精度惯性导航定位算法，其特征在于，所述行人步态分类，获得正常速步态和快速步态，包括以下步骤：

以校准和滤波后的角速度数据为参考，设定角速度阀值为0.45°/s；若角速度大于角速度阀值划分为行人的摆动相，否则划分为站立相；

若连续10个角速度采样点均属于摆动相，则标记step_s；将所述step_s的10个采样点的第一个采样点，记为摆动相的起点；

若连续10个角速度采样点均属于站立相，则标记step_e；将所述step_e的10个采样点的第一个采样点，记为摆动相的终点；

将相邻的step_s和/或step_e之间的间距设定为步幅；

将任一个步幅持续时长设定大于M个采样点，以排除捕捉错误的步幅；所述M为大于100的自然数；

若任一个步幅中站立相小于P个采样点，则该步幅被判断为快速步态，该步幅中每个采样时刻点为快速步态采样点，并标记为1；若任一个步幅中站立相大于或等于P个采样点，该步幅被判断为正常速步态，该步幅中每个采样时刻点为正常速步态采样点，并标记为0；所述P为大于50、且小于100的自然数。

4.根据权利要求3所述的基于运动状态检测的高精度惯性导航定位算法，其特征在于，所述任一步态的陀螺仪的采集数据进行偏差矫正，包括以下步骤：

建立陀螺仪的误差模型ω_k，其表达式为：

e_k＝ω_bias+ω_{s_noise} (3-2)

其中，k表示当前采样时刻点，为经校准和滤波处理后的角速度，e_k为根据不同步态所构建的误差值，ω_bias为陀螺仪偏差，ω_{s_noise}为高斯白噪声；

分别设定正常速步态和快速步态的陀螺仪偏差值，并利用构建的误差值e_k对每一步幅陀螺仪采集的角速度数据进行偏差矫正，包括以下步骤：

提取当前步幅，选取站立相的角速度数据的最小值ω_min；

若当前步幅被标记为1，当前步幅的陀螺仪偏差ω_bias为当前站立相角速度最小值ω_min的b₁/e倍；

若当前步幅被标记为2，当前步幅的陀螺仪偏差ω_bias为当前站立相角速度最小值ω_min的b₂/e倍；

所述步幅被标记为1即当前步幅为正常速步态，所述步幅被标记为2即当前步幅为快速步态；所述e是当前站立相陀螺仪采集角速度数据的量级与陀螺仪静止时采集的角速度数据的量级的比值，其一般取值为10或100；所述b₁、b₂是以陀螺仪静止时采集的角速度数据的值为参考，b₂/b₁的比值接近于站立相时快速步态与正常速步态陀螺仪数据的比值，其中，b₁一般取值为1，且b₂/b₁为大于1小于2的小数。

5.根据权利要求4所述的基于运动状态检测的高精度惯性导航定位算法，其特征在于，利用根据行人行走步态和时长，偏差矫正后的角速度数据和经过校准且滤波后的加速度数据，求得行人的初始位置坐标，包括以下步骤：

将行人的速度和航向角度设置为零；

根据矫正后的加速度计算初始旋转矩阵C_k，其表达式为：

其中，q₀、q₁、q₂、q₃为四元数，其表达式如下：

其中，pitch为行人行走的俯仰角，yaw为行人行走的偏航角，roll为行人行走的翻滚角；

对公式(6-3)进行两次积分，获得行人的初始位置坐标，其表达式为

其中，a_k表示在参考坐标系下，经过校准且滤波后的三轴加速度数据，C_k表示旋转矩阵，表示在身体坐标系下，经过校准且滤波后的三轴加速度数据，g表示重力单位，表示当前采样时刻的速度，其中上标'-'表示预测值，v_k-1表示上一采样时刻的速度，表示当前采样时刻的位置，p_k-1表示上一采样时刻的位置。

6.根据权利要求5所述的基于运动状态检测的高精度惯性导航定位算法，其特征在于，基于误差卡尔曼滤波器，得到行人位置、姿态的误差向量，更新出准确的位置、姿态，包括以下步骤：

定义行人定位的误差向量δ_xk，其表达式为：

其中，表示行人的姿态角误差，δω_k表示行人的角速度误差，δp_k表示行人的位置误差，δv_k表示行人的速度误差，δa_k表示行人的加速度误差；

预测阶段：建立误差协方差矩阵其表达式为：

其中，Q_K为过程噪声协方差矩阵，F_K为状态传输矩阵；

所述状态传输矩阵F_K的表达式为：

其中，S_K为偏斜对称矩阵，为旋转矩阵，Δt为采样周期，I为3阶单位矩阵；

所述公式(5-3)中，偏斜对称矩阵S_K的表达式如下：

其中，a_k(1),a_k(2),a_k(3)分别为参考坐标系下，经过校准且滤波后的三轴加速度数据；

所述公式(5-3)中，旋转矩阵的表达式为：

所述公式(5-5)中，Ω_k为角速度的偏斜对称矩阵，其表达式为：

其中，w_k(1),w_k(2),w_k(3)分别为根据行人行走步态，偏差矫正后的三轴角速度数据；

判断是否进入更新阶段，具体步骤如下：

设置两个站立相检测器，即快速步态的站立相检测器和正常速步态的站立相检测器；若检测到标记为2的采样点则进入快速步态的站立相检测器；若检测到标记为1的采样点则进入正常速步态的站立相检测器；

任一检测器的逻辑与门D，其表达式为：

其中，a_min表示设定的站立相时的加速度数据的最小值，a_max表示设定的站立相时的加速度数据的最大值，σ_amax表示设定的站立相时的加速度方差的最大值，ω_max表示设定的站立相时的角速度数据的最大值，a_k表示当前站立相时刻参考坐标系下，经过校准且滤波后的加速度数据，表示当前站立相时刻参考坐标系下，经过校准且滤波后的加速度的方差，ω_k表示当前站立相时刻的角速度数据；

当D＝1时，进入更新阶段；

更新阶段：建立站立相时的零速测量的误差向量δ_xk的更新公式，其表达式为：

其中，K为卡尔曼增益，z_k表示为站立相时的测量误差，H表示为测量矩阵；

所述公式(5-8)中，卡尔曼增益K的表达为：

其中，表示误差协方差矩阵，H表示为测量矩阵，R表示测量噪声协方差矩阵，其为8×8的对角矩阵，主对角线元素为：

所述主对角线元素σ_ωxσ_ωyσ_ωz，σ_pz，σ_vxσ_vyσ_vz分别为站立相时偏航角、三轴角速度、高度、三轴速度的测量噪声；

所述公式(5-9)中，测量矩阵H的表达式为：

其中，I_3×3表示3阶单位矩阵，0_1×3一行三列零矩阵，0_3×33阶零矩阵；

所述公式(5-8)中，站立相时的测量误差z_k的表达式为

其中，表示启发式航向漂移算法输出值，表示零角速度更新输出角速度误差，p_ZHU表示零高度更新输出的高度误差，表示零速度更新输出的速度误差；所述的零高度更新算法，当行人在走平路时，p_ZHU取当前测得的高度坐标与0的差值为高度误差；当行人在上下楼时，p_ZHU取当前测得的高度坐标与上一时刻高度坐标的差值为高度误差；

行人定位的预测的错误状态值为零，即可简化误差向量δ_xk；

更新误差协方差矩阵：

将所述误差向量δ_xk传递到惯性导航系统，校正位置p_k、速度v_k、角速度ω_k和表示姿态的旋转矩阵C_k，其表达式为

所述公式(5-15)中，为姿态角误差的偏斜对称矩阵，其表达式为：

其中，分别表示三轴姿态角误差。

7.根据权利要求6所述的基于运动状态检测的高精度惯性导航定位算法，其特征在于，通过得到的行人位置坐标，重建出行人2D运动轨迹，融合高度更新算法后，准确获得行人任一时刻的位置高度，并重建出适用于上下楼时行人的3D运动轨迹，包括以下步骤：

楼梯与平台的识别区分，建立每一个步幅的状态n，其取值为0、1、2；所述步幅的状态n为0表示当前步幅处于平台上；所述步幅的状态n为1表示当前步幅迈了一阶台阶；所述步幅的状态n为2表示当前步幅迈了2阶台阶，步幅的状态n表达式为：

其中，Δh₀表示两步之间的高度差，n₁表示台阶高度下限，n₂表示台阶高度上限；

轨迹重建，建立高度模型，其表达式为：

h＝∑Δh (6-6)

其中，Δh表示每步变化的高度，Δh＝nΔh_stair，n为每一个步幅的状态，Δh_stair为固定值阶的高度；

以行人的初始位置坐标为起点，连续采集传感器数据，并叠加高度模型，获得行走的高度和轨迹。

8.根据权利要求7所述的基于运动状态检测的高精度惯性导航定位算法，其特征在于，所述楼梯与平台的识别区分中：

若连续3个步幅状态序列为211，则将连续3步态标记为210或211；

若连续3个步幅状态序列为112，则将连续3步态标记为122；

若连续3个步幅状态序列为212，则将连续3步态标记为222；

若连续3个步幅状态序列为n₁0n₃，则将该步幅后连续3步态分部标记为n₁2n₃；所述n₁和n₃为1或2。