CN110398245A - 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 - Google Patents

Abstract

本发明提供了一种基于脚戴式惯性测量单元的室内行人导航姿态估计方法，首先，将惯性测量单元安装于行人足部，初始化惯性测量单元在导航坐标系下的初始姿态，然后根据惯性传感器的输出判断行人是否处于静止状态，当行人处于非静止状态时，采用捷联惯导算法进行姿态更新，递推得到当前的姿态，当行人处于静止状态时，根据捷联惯导算法姿态更新方程构造动态模型、加速度计输出与重力关系构造测量模型，在四元数框架下通过卡尔曼滤波器对当前的姿态进行更新。本发明利用静止时刻加速度计的测量构造姿态估计观测量，在四元素框架下执行卡尔曼滤波器进行数据融合得到精确的姿态信息，实现了提供精确的姿态信息的技术效果。

Description

基于脚戴式惯性测量单元的室内行人导航姿态估计方法

技术领域

本发明涉及行人导航技术领域，尤其涉及一种基于脚戴式惯性测量单元的室内行人导航姿态估计方法。

背景技术

“行人导航”是指在地图的基础上，提供必要的位置、方向信息，引导行人到达既定目标。在不断丰富的智能电子设备中，行人导航已成为其一项重要功能(如智能手机中高德地图、百度地图等提供的行人导航服务)，极大地提高了人们的出行效率。

在室外，依靠全球导航卫星***(Global Navigation Satellite System,GNSS)可以获得精确的位置信息，再结合其他方向传感器(如陀螺仪)，便可以实现精确的行人导航。然而在室内，由于建筑结构和墙体的遮挡，GNSS信号被严重衰减，不能估计有效的位置信息。因此，室内行人导航面临困难更多、需要发展专门的技术，是当前国内外定位导航领域研究的热点之一。

现有技术中，可用的室内位置估计技术大致可分为基于视觉的方法、基于无线信号的方法和基于惯性传感器的方法等。考虑到成本、功耗、运算复杂度等问题，基于脚戴式惯性测量单元(Inertial Measurement Unit,IMU)的方法被广泛应用于室内行人导航位置估计。如何获取精确的方向信息的是室内行人导航的主要难点。依靠IMU内置角速度传感器测量进行方向估计是一种经济、简单的方法，但存在方向漂移问题，需要对方向估计进行不断改正，从而限制方向漂移。当前的解决方案包括零加速度更新算法、磁航向辅助算法等。

本申请发明人在实施本发明的过程中，发现现有技术的方法，至少存在如下技术问题：

零加速度更新算法依赖于所使用的姿态误差模型，仅仅在地球坐标系下有效；磁航向辅助算法需要准确的航向观测值，但由于室内地磁干扰严重，通过地磁测量得到的航向估计将存在较大误差。除此之外，这些解决方案需要在地球坐标系下进行，极大地增加了导航***的初始化工作；并且，这些方案普遍只提供航向信息，而不能提供精确的三维方向信息(即姿态)。对于一些特殊的室内行人导航应用，比如消防人员导航，姿态信息有利于监测被导航人员的行走状态(站立、平躺、下蹲等)，从而推测其是否安全。

由此可知，现有技术中的方法存在难以提供精确的姿态信息的技术问题。

发明内容

为解决当前室内行人导航***难以提供精确的姿态信息问题，本发明提供一种基于脚戴式IMU的室内行人导航姿态估计方法，该方法利用行人静止状态的加速度计输出构建方向观测量，然后在四元数框架下执行卡尔曼滤波器对IMU捷联算法的姿态估计进行改正。

本发明提供了基于脚戴式IMU的室内行人导航姿态估计方法，包括：

步骤S1：将惯性测量单元安装于行人足部，确定导航坐标系，对惯性测量单元的零偏误差进行在线标定，初始化惯性测量单元在导航坐标系下的初始姿态，其中，导航坐标系的Z轴与地面垂直；

步骤S2：根据惯性传感器的输出判断行人是否处于静止状态，如果行人处于非静止状态，执行步骤S3，否则执行步骤S4；

步骤S3：根据初始姿态以及三轴陀螺仪的输出，采用捷联惯导算法进行姿态更新，递推得到当前的姿态，其中，将三轴陀螺仪的测量作为传感器坐标系下惯性测量单元相对于导航坐标系的旋转角速度；

步骤S4：根据捷联惯导算法姿态更新方程构造动态模型、加速度计输出与重力关系构造测量模型，在四元数框架下通过卡尔曼滤波器对当前的姿态进行更新。

在一种实施方式中，步骤S1具体包括：

步骤S1.1：将惯性测量单元安装于行人足部，自定义三维直角坐标系作为导航坐标系，确保导航坐标系的Z轴与地面垂直；

步骤S1.2：对于三轴陀螺仪，以静止状态下一段预设时间内各轴向输出的平均值作为该轴的零偏误差。对于三轴加速度计，通过静止状态下四个不同位置上一段预设时间内加速度计的输出进行标定，标定公式如式(1)所示，

其中，b_x,b_y,b_z分别为三轴加速度计X、Y、Z轴上零偏误差，x_i、y_i、z_i，i＝1,2,3,4，分别表示第i个位置上X、Y、Z输出的平均值，m_i，i＝1,2,3,4表示第i个位置上各轴输出平均值的平方和；

步骤S1.3：在初始导航时刻，调整惯性测量单元的位置，使惯性测量单元传感器坐标系与导航坐标系对齐，从而获得惯性测量单元在导航坐标系下的初始姿态，其中，初始姿态的形式为q₀＝[1,0,0,0]^T。

在一种实施方式中，步骤S2具体包括：

步骤S2.1：判断当前惯性测量单元输出的序列总长度是否大于或等于预设长度N，如果是，则转至步骤S2.2，判断行人是否处于静止状态；

步骤S2.2：计算当前三轴陀螺仪测量向量的模||y_k-b||，其中，y_k为当前三轴陀螺仪测量向量，b为步骤S1中得到的三轴陀螺仪的零偏向量，并判断||y_k-b||是否大于设定的第一阈值th_m，如果大于，则判定行人在当前时刻处于非静止状态，否则，执行步骤S2.3；

步骤S2.3：进一步计算定长窗口内的陀螺仪测量向量模的方差var，计算公式如式(2)所示：

其中，c代表窗口内N个陀螺仪测量向量模的均值，如果var大于设定的第二阈值th_v，则判定行人在当前时刻处于非静止状态，否则，判定行人在当前时刻处于静止状态。

在一种实施方式中，步骤S3具体包括：

根据传统捷联惯导算法，递推得到当前的姿态，捷联姿态更新方程如下：

其中，w(t)为传感器坐标系下惯性测量单元相对于导航坐标系的旋转角速度，q_k+1表示t_k+1时刻的姿态四元素，q_k+1表示t_k时刻的姿态四元素。

在一种实施方式中，步骤S4具体包括：

步骤S4.1：建立当前传感器坐标系下三轴加速度计真实测量向量与姿态估计之间的关系，

其中，表示当地重力在导航坐标系下投影的四元数形式，q_k+1为时间t_k+1时刻姿态四元素，为时间t_k+1时刻传感器坐标系下三轴加速度计的真实测量向量的四元数形式；⊙为四元数乘法运算符；

步骤S4.2：以捷联姿态更新方程构造动态模型，以式(4)构造测量模型，采用卡尔曼滤波器估计当前的姿态信息。

在一种实施方式中，步骤S4.2具体包括：

步骤S4.2.1：以姿态作为状态变量，根据式(4)和步骤S3中的捷联姿态更新方程建立如下离散四元素卡尔曼滤波模型，

式(5)中，F_k+1表示状态转移矩阵，H_k+1表示状态观测矩阵，K_k+1为***噪声系数矩阵，W_k+1为观测噪声系数矩阵，δ_k+1表示IMU采样间隔内的角增量的噪声，ε_k+1表示三轴加速度计测量噪声，具体定义如下:

式(6)中，Δθ_k+1表示从时间t_k到t_k+1三轴陀螺仪测得的角增量向量，Δθ_k+1为向量Δθ_k+1的模，I_m表示m×m的单位矩阵；gⁿ是当地重力在导航坐标系下的投影；为时间t_k+1时刻载体坐标系下三轴加速度计的输出向量；s_k+1和v_k+1分别是姿态四元素q_k+1的标量部分和矢量部分。

步骤S4.2.2：根据离散四元素卡尔曼滤波模型，得到当前时刻改正后的姿态估计，其中，模型中状态向量X_k+1＝q_k+1，滤波过程中，噪声δ_k+1的协方矩阵Q_k+1和噪声ε_k+1的协方差矩阵R_k+1按如下方式计算：

式(7)中，σ₁代表IMU采样间隔内三轴陀螺仪测得的角增量误差的标准差，σ₂代表三轴加速度计测量噪声的标准差，M_k|k和M_k+1|k为中间变量，按如下方式计算：

式(8)中，q_k|k为卡尔曼滤波上一次姿态估计，q_k+1|k为当前模型的姿态预测，P_k|k和P_k+1/k分别为q_k|k和q_k+1|k的协方差矩阵。

在一种实施方式中，所述方法还包括：

对三轴加速度计进行误差补偿。

本申请实施例中的上述一个或多个技术方案，至少具有如下一种或多种技术效果：

在本发明提供的方法中，首先，将惯性测量单元安装于行人足部，初始化惯性测量单元在导航坐标系下的初始姿态，然后根据惯性传感器的输出判断行人是否处于静止状态，当行人处于非静止状态时，根据初始姿态以及三轴陀螺仪的测量，采用捷联惯导算法进行姿态更新，递推得到当前的姿态，当行人处于静止状态时，根据捷联惯导算法姿态更新方程构造动态模型、加速度计输出与重力关系构造测量模型，在四元数框架下通过卡尔曼滤波器对当前的姿态进行更新。

本发明利用静止时刻加速度计的测量构造姿态估计观测量，在四元素框架下执行卡尔曼滤波器进行数据融合得到精确的姿态信息，从而提供了一种不依赖于外部姿态传感器的、简单可行的行人导航姿态估计方法。解决了现有技术中的方法存在难以提供精确的姿态信息的技术问题。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的一种基于脚戴式惯性测量单元的室内行人导航姿态估计方法的流程图；

图2为本发明一种具体示例中基于脚戴式惯性测量单元的室内行人导航姿态估计方法的流程图；

图3为本发明实施例中四元素卡尔曼滤波流程图。

具体实施方式

本发明实施例提供了一种基于脚戴式惯性测量单元的室内行人导航姿态估计方法，用以改善现有技术中的方法存在难以提供精确的姿态信息的技术问题。

本申请实施例中的技术方案，总体思路如下：

提供一种室内行人导航姿态估计方法，该方法利用行人行走过程中静止状态下的加速度计输出构造姿态观测量，在四元素框架下，通过卡尔曼滤波器对捷联惯导算法姿态估计进行改正，可实现任意导航坐标系下行人姿态的有效估计。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例提供了一种基于脚戴式惯性测量单元的室内行人导航姿态估计方法，请参见图1，该方法包括：

步骤S1：将惯性测量单元安装于行人足部，确定导航坐标系，对惯性测量单元的零偏误差进行在线标定，初始化惯性测量单元在导航坐标系下的初始姿态，其中，导航坐标系的Z轴与地面垂直。

具体来说，导航坐标系是导航参数解算的框架。在室外，通常以东北天地理坐标系作为导航坐标系，在本发明中，采用自定义的三维直角坐标系作为导航坐标系。自定义坐标系的选择，一方面避免了使用地球坐标系作为导航坐标系所涉及复杂的初始化工作，另一方面也简化了将导航参数转化到室内地图坐标系下所需工作。本发明对自定义坐标系具有如下要求：自定义坐标系Z轴与地面垂直。为了省去导航参数与地图坐标之间的转化工作，可将自定义三维直角坐标系的0XY平面与地图坐标系重合。

在一种实施方式中，步骤S1具体包括：

步骤S1.1：将惯性测量单元安装于行人足部，自定义三维直角坐标系作为导航坐标系，确保导航坐标系的Z轴与地面垂直；

步骤S1.2：对于三轴陀螺仪，以静止状态下一段预设时间内各轴向输出的平均值作为该轴的零偏误差。对于三轴加速度计，通过静止状态下四个不同位置上一段预设时间内的加速度输出进行标定，标定公式如式(1)所示，

其中，b_x,b_y,b_z分别为三轴加速度计X、Y、Z轴上零偏误差，x_i、y_i、z_i，i＝1,2,3,4，分别表示第i个位置上X、Y、Z输出的平均值，m_i，i＝1,2,3,4表示第i个位置上各轴输出平均值的平方和；

步骤S1.3：在初始导航时刻，调整惯性测量单元的位置，使惯性测量单元传感器坐标系与导航坐标系对齐，从而获得惯性测量单元在导航坐标系下的初始姿态，其中，初始姿态的形式为q₀＝[1,0,0,0]^T。

具体来说，在行人导航中，所采用的惯性测量单元通常为低成本惯性器件，即使在出厂前已经进行标定，但由于三轴加速度计和三轴陀螺仪的零偏误差稳定性差、重复性差，在每次使用前需要对该项误差进行重新在线标定，以提高传感器的测量精度。三轴陀螺仪在线标定方法如下：以静止状态下预设时间内各轴向输出的平均值作为该轴的零偏误差。对于加速度计，由于存在重力的影响不能按以上方法进行标定，可通过静止状态下，四个不同位置上预设时间内加速度计的输出进行标定。其中，预设时间可以根据实际情况和经验进行选择。

在导航坐标系确定后，惯性测量单元在导航坐标系下的初始姿态可按如下方式简单确定：在初始导航时刻，调整惯性测量单元的位置。本发明使惯性测量单元传感器坐标系与导航坐标系对齐，从而初始姿态以四元素形式表示为q₀＝[1,0,0,0]^T。

步骤S2：根据三轴陀螺仪的输出判断行人是否处于静止状态，如果行人处于非静止状态，执行步骤S3，否则执行步骤S4。

在一种实施方式中，步骤S2具体包括：

步骤S2.1：判断当前惯性测量单元输出的序列总长度是否大于或等于预设长度N，如果是，则转至步骤S2.2，判断行人是否处于静止状态；

步骤S2.2：计算当前三轴陀螺仪测量向量的模||y_k-b||，其中，y_k为当前三轴陀螺仪测量向量，b为步骤S1中得到的三轴陀螺仪的零偏向量，并判断||y_k-b||是否大于设定的第一阈值th_m，如果大于，则判定行人在当前时刻处于非静止状态，否则，执行步骤S2.3；

步骤S2.3：进一步计算定长窗口内的陀螺仪测量向量模的方差var，计算公式如式(2)所示：

其中，c代表窗口内N个陀螺仪测量向量模的均值，如果var大于设定的第二阈值th_v，则判定行人在当前时刻处于非静止状态，否则，判定行人在当前时刻处于静止状态。

具体来说，请参见图2，为一种具体示例中室内行人导航姿态估计方法的流程图。本发明根据惯性传感器输出探测行人静止状态，本实施例采用当前三轴陀螺仪输出值幅度和定长窗口内陀螺仪输出值幅度方差进行行人静止状态探测，假定窗口长度为N。如果当前惯性传感器输出序列总长度小于N，无法进行静止探测，则转至步骤S3执行。否则，通过步骤S2.2和步骤S2.3来进行判断。

在步骤S2中，第一阈值th_m和第二阈值th_v按如下方式选择如下：根据步骤S1中为标定三轴陀螺仪零偏误差采集的静态陀螺数据，计算该组数据中所有陀螺仪测量向量的模，并找到最大模值，以其1.5倍作为th_m；从第N个数据开始，计算每个陀螺仪测量向量模值与前N-1个陀螺仪测量向量模值所组成的数据集的方差，找到方差最大值，以其1.5倍作为th_v。

在步骤S2中，窗口长度N的设置可根据行人正常行走过程中一个完整步态期间的静止时长确定。不同学者的大量实验测试结果表明，行人正常行走期间，一个完整步态中静止时长约为0.3-0.5秒。因此，N可设置为0.3F，F表示惯性传感器的输出频率。本申请发明人通过大量的实践与研究发现：N不能设置太大，太大会导致真实静止状态被漏检；N也不易设置太小，太小在一次步态中将探测到多个静止状态，导致运算开销增大。

当然，在具体实施中，静止状态的探测也可采用其他方式来实现，本发明对此不作特别限制。

步骤S3：根据初始姿态以及三轴陀螺仪的测量，采用捷联惯导算法进行姿态更新，递推得到当前的姿态，其中，不考虑地球自转的影响，将三轴陀螺仪的测量作为传感器坐标系下惯性测量单元相对于导航坐标系的旋转角速度。

具体来说，根据传统捷联惯导算法，递推得到当前的姿态。在递推算法中，需要测量传感器坐标系下惯性测量单元相对于导航坐标系的旋转角速度，而惯性测量单元中的三轴陀螺仪的输出测量为传感器坐标系下惯性测量单元相对于导航坐标系的旋转角速度，因此需要从中扣除地球自转的影响。在自定义导航坐标系下，对三轴陀螺仪测量陀螺仪输出进行地球自转角速度补偿非常困难。但地球自转角速度非常小，大约0.004角度每秒，因此可以忽略不计，本发明中，三轴陀螺仪的测量即为传感器坐标系下惯性测量单元相对于导航坐标系的旋转角速度。

在一种实施方式中，步骤S3具体包括：

根据传统捷联惯导算法，递推得到当前的姿态，捷联姿态更新方程如下：

其中，w(t)为传感器坐标系下惯性测量单元相对于导航坐标系的旋转角速度，q_k+1表示t_k+1时刻的姿态四元素，q_k表示t_k时刻的姿态四元素。

具体来说，递推姿态更新执行完毕后，判断导航是否结束，如果是，则结束姿态更新，否则返回到步骤S2。

步骤S4：根据捷联惯导算法姿态更新方程构造动态模型、加速度计输出与重力关系构造测量模型，在四元数框架下通过卡尔曼滤波器对当前的姿态进行更新。

具体来说，在四元数框架下通过卡尔曼滤波器进行姿态更新。根据捷联惯导算法可以推得当前的姿态，但是由于传感器误差，姿态存在较大的误差。本发明利用行人静止状态下的三轴加速度计构造姿态观测量对当前姿态进行改正。

其中，步骤S4具体包括：

步骤S4.1：建立传感器坐标系下当前三轴加速度计真实测量向量(不考虑零偏及其他误差)与姿态估计之间的关系，

其中，表示当地重力在导航坐标系下投影的四元数形式，q_k+1为时间t_k+1时刻姿态四元素，为时间t_k+1时刻传感器坐标系下三轴加速度计的真实测量向量的四元数形式；⊙为四元数乘法运算符；

步骤S4.2：以捷联姿态更新方程构造动态模型，以式(4)构造测量模型，采用卡尔曼滤波器估计当前的姿态信息。

具体来说，请参见图3，为四元素卡尔曼滤波流程图。根据步骤S3中的公式可以递推得到当前姿态。但是，但是由于传感器误差，如此得到的姿态估计误差较大，需要利用可靠的外部观测量进行改正。在行人静止状态下，三轴加速度计的测量仅仅反映地球重力影响(也就是测量值等于重力在当前坐标系下的投影)，而重力在导航坐标系下的投影已知，于是可建立当前三轴加速度计测量与姿态估计之间的关系。

为当地重力在导航坐标系下投影的四元数形式，根据导航坐标系的定义，重力在导航坐标系下的投影是已知的。三维向量通过补充一个0元素可转为四元数形式，如假设导航坐标系垂直地面向上，重力在导航坐标系下投影为gⁿ＝[0 0 -g]^T，改写成四元数形式为室内导航应用中，不考虑导航区域内的重力变化，重力g为常数。

在一种实施方式中，步骤S4.2具体包括：

步骤S4.2.1：以姿态作为状态变量，根据式(4)和步骤S3中的捷联姿态更新方程建立如下离散四元素卡尔曼滤波模型，

式(5)中，F_k+1表示状态转移矩阵，H_k+1表示状态观测矩阵，K_k+1为***噪声系数矩阵，W_k+1为观测噪声系数矩阵，δ_k+1表示IMU采样间隔内的角增量的噪声，ε_k+1表示三轴加速度计测量噪声，具体定义如下:

式(6)中，Δθ_k+1表示从时间t_k到t_k+1三轴陀螺仪测得的角增量向量，Δθ_k+1为向量Δθ_k+1的模，I_m表示m×m的单位矩阵；gⁿ是当地重力在导航坐标系下的投影；为时间t_k+1时刻载体坐标系下三轴加速度计的输出向量；s_k+1和v_k+1分别是姿态四元素q_k+1的标量部分和矢量部分。

步骤S4.2.2：根据离散四元素卡尔曼滤波模型，得到当前时刻改正后的姿态估计，其中，模型中状态向量X_k+1＝q_k+1，滤波过程中，噪声δ_k+1的协方矩阵Q_k+1和噪声ε_k+1的协方差矩阵R_k+1按如下方式计算：

式(7)中，σ₁代表IMU采样间隔内三轴陀螺仪测得的角增量误差的标准差，σ₂代表三轴加速度计测量噪声的标准差，M_k|k和M_k+1|k为中间变量，按如下方式计算：

式(8)中，q_k|k为卡尔曼滤波上一次姿态估计，q_k+1|k为当前模型的姿态预测，P_k|k和P_k+1/k分别为q_k|k和q_k+1|k的协方差矩阵。

具体来说，在该滤波器中***模型为非线性化方程，要使用传统卡尔曼滤波器进行数据融合非常复杂，因此本发明在四元素框架下执行卡尔曼滤波器。以姿态四元素为状态量，建立离散四元素卡尔曼滤波模型。滤波执行完毕后，重新回到步骤S2。

本发明中四元素框架下执行卡尔曼滤波器与普通卡尔曼滤波器相比，不同之处是：二者在不同的框架下执行的，一个是普通的三维向量框架，一个是四元素框架下，不同框架下的运算法则不一样，四元素卡尔曼滤波可以有效避免传统卡尔曼滤波的复杂线性化问题。

为了获得准确可靠的三轴加速度测量，在一种实施方式中，所述方法还包括：对三轴加速度计进行误差补偿。

具体来说，本发明考虑了三轴加速度计误差随时间的变化。具体实施过程中，可以采用任意可行方法进行三轴加速度计误差补偿，本发现对此不作特别限制。在本实施例中，提供如下补偿方法。将三轴加速度计各项误差影响统一到零偏误差中，建立零偏误差随机游走模型，

b_k+1＝b_k+η_k+1 (9)

式(9)中，b_k+1和b_k代表当前时刻和上一时刻三轴加速度计的零偏误差，η_k+1为当前噪声。采用零速更新方法，将行人静止状态下的速度作为观测量，行人速度和三轴加速度计偏差作为状态量，可建立如下离散卡尔曼滤波模型估计三轴加速度计的零偏大小，

式(10)中，x_k是由行人当前速度和三轴加速度计偏差组成的状态向量，A_k+1是状态转移矩阵，B_k+1是状态观测矩阵，ω_k+1和分别为***噪声和观测噪声。A_k+1和B_k+1定义如下，

式中，T是惯性传感器单元输出时间间隔，表示传感器坐标系到导航坐标系的方向余弦矩阵，I_3×1和0_3×1分别代表元素全为1的三维列向量和元素全为0的三维列向量。四元素卡尔曼滤波执行完毕，判断导航是否结束，如是则结束姿态更新，如否则返回到步骤S2。

本申请实施例中的上述一个或多个技术方案，至少具有如下一种或多种技术效果：

本发明利用静止时刻加速度计的测量构造姿态估计观测量，在四元素框架下执行卡尔曼滤波器进行数据融合得到精确的姿态信息，从而提供了一种不依赖于外部姿态传感器的、简单可行的行人导航姿态估计方法。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明实施例进行各种改动和变型而不脱离本发明实施例的精神和范围。这样，倘若本发明实施例的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (7)

1.基于脚戴式惯性测量单元的室内行人导航姿态估计方法，其特征在于，包括：

步骤S1：将惯性测量单元安装于行人足部，确定导航坐标系，对惯性测量单元的零偏误差进行在线标定，初始化惯性测量单元在导航坐标系下的初始姿态，其中，导航坐标系的Z轴与地面垂直；

步骤S2：根据惯性传感器的输出判断行人是否处于静止状态，如果行人处于非静止状态，执行步骤S3，否则执行步骤S4；

步骤S3：根据初始姿态以及三轴陀螺仪的输出，采用捷联惯导算法进行姿态更新，递推得到当前的姿态，其中，将三轴陀螺仪的测量作为传感器坐标系下惯性测量单元相对于导航坐标系的旋转角速度；

步骤S4：根据捷联惯导算法姿态更新方程构造动态模型、加速度计输出与重力关系构造测量模型，在四元数框架下通过卡尔曼滤波器对当前的姿态进行更新。

2.如权利要求1所述的方法，其特征在于，步骤S1具体包括：

步骤S1.1：将惯性测量单元安装于行人足部，自定义三维直角坐标系作为导航坐标系，确保导航坐标系的Z轴与地面垂直；

步骤S1.2：对于三轴陀螺仪，以静止状态下一段预设时间内各轴向输出的平均值作为该轴的零偏误差；对于三轴加速度计，通过静止状态下四个不同位置上一段预设时间内的加速度输出进行标定，标定公式如式(1)所示，

其中，b_x,b_y,b_z分别为三轴加速度计X、Y、Z轴上零偏误差，x_i、y_i、z_i，i＝1,2,3,4，分别表示第i个位置上X、Y、Z输出的平均值，m_i，i＝1,2,3,4表示第i个位置上各轴输出平均值的平方和；

步骤S1.3：在初始导航时刻，调整惯性测量单元的位置，使惯性测量单元传感器坐标系与导航坐标系对齐，从而获得惯性测量单元在导航坐标系下的初始姿态，其中，初始姿态的形式为q₀＝[1,0,0,0]^T。

3.如权利要求1所述的方法，其特征在于，步骤S2具体包括：

步骤S2.1：判断当前惯性测量单元输出的序列总长度是否大于或等于预设长度N，如果是，则转至步骤S2.2，判断行人是否处于静止状态；

步骤S2.2：计算当前三轴陀螺仪测量向量的模||y_k-b||，其中，y_k为当前三轴陀螺仪测量向量，b为步骤S1中得到的三轴陀螺仪的零偏向量，并判断||y_k-b||是否大于设定的第一阈值th_m，如果大于，则判定行人在当前时刻处于非静止状态，否则，执行步骤S2.3；

步骤S2.3：进一步计算定长窗口内的陀螺仪测量向量模的方差var，计算公式如式(2)所示：

其中，c代表窗口内N个陀螺仪测量向量模的均值，如果var大于设定的第二阈值th_v，则判定行人在当前时刻处于非静止状态，否则，判定行人在当前时刻处于静止状态。

4.如权利要求1所述的方法，其特征在于，步骤S3具体包括：

根据传统捷联惯导算法，递推得到当前的姿态，捷联姿态更新方程如下：

其中，w(t)为传感器坐标系下惯性测量单元相对于导航坐标系的旋转角速度，q_k+1表示t_k+1时刻的姿态四元素，q_k表示t_k时刻的姿态四元素。

5.如权利要求1所述的方法，其特征在于，步骤S4具体包括：

步骤S4.1：建立当前传感器坐标系下加速度真实测量向量与姿态估计之间的关系，

其中，表示当地重力在导航坐标系下投影的四元数形式，q_k+1为时间t_k+1时刻姿态四元素，为时间t_k+1时刻传感器坐标系下三轴加速度计的真实测量向量的四元数形式；⊙为四元数乘法运算符；

步骤S4.2：以捷联姿态更新方程构造动态模型，以式(4)构造测量模型，采用卡尔曼滤波器估计当前的姿态信息。

6.如权利要求5所述的方法，其特征在于，步骤S4.2具体包括：

步骤S4.2.1：以姿态作为状态变量，根据式(4)和步骤S3中的捷联姿态更新方程建立如下离散四元素卡尔曼滤波模型，

式(5)中，F_k+1表示状态转移矩阵，H_k+1表示状态观测矩阵，K_k+1为***噪声系数矩阵，W_k+1为观测噪声系数矩阵，δ_k+1表示IMU采样间隔内的角增量的噪声，ε_k+1表示三轴加速度计测量噪声，具体定义如下:

式(6)中，Δθ_k+1表示从时间t_k到t_k+1三轴陀螺仪测得的角增量向量，Δθ_k+1为向量Δθ_k+1的模，I_m表示m×m的单位矩阵；gⁿ是当地重力在导航坐标系下的投影；为时间t_k+1时刻载体坐标系下三轴加速度计的输出向量；s_k+1和v_k+1分别是姿态四元素q_k+1的标量部分和矢量部分；

步骤S4.2.2：根据离散四元素卡尔曼滤波模型，得到当前时刻改正后的姿态估计，其中，模型中状态向量X_k+1＝q_k+1，滤波过程中，噪声δ_k+1的协方矩阵Q_k+1和噪声ε_k+1的协方差矩阵R_k+1按如下方式计算：

式(7)中，σ₁代表惯性测量单元采样间隔内三轴陀螺仪测得的角增量误差的标准差，σ₂代表三轴加速度计测量噪声的标准差，M_k|k和M_k+1|k为中间变量，按如下方式计算：

式(8)中，q_k|k为卡尔曼滤波上一次姿态估计，q_k+1|k为当前模型的姿态预测，P_k|k和P_k+1/k分别为q_k|k和q_k+1|k的协方差矩阵。

7.如权利要求1-6任一项权利要求所述的方法，其特征在于，所述方法还包括：

对三轴加速度计进行误差补偿。