CN104406586B - 基于惯性传感器的行人导航装置和方法 - Google Patents

Abstract

本发明公开了基于惯性传感器的行人导航装置，包括微处理器以及分别与之连接的三轴加速度传感器、三轴陀螺仪、三轴磁力计和无线通信模块，所述微处理器接收三轴加速度传感器、三轴陀螺仪和三轴磁力计采集的数据，并通过无线通信模块将这些数据上传至PC机上。本发明还公开了基于前述装置的行人导航方法，包括步伐阶段检测算法、脚体方位估计算法和扩展卡尔曼滤波算法。本发明在保证精确度的同时极大限度地降低了计算的复杂度，在实际环境中不必消耗大量硬件功耗也能保证实时性的要求。

Description

基于惯性传感器的行人导航装置和方法

技术领域

本发明属于导航技术领域，特别涉及了基于惯性传感器的行人导航装置和方法。

背景技术

随着数据业务与多媒体业务数据的快速增长，高精度行人导航系统的需求日益剧增，拥有这样导航系统的人们可以轻而易举的享受到众多基于位置的服务(Location-Based Services,LBS)。然而，由于城市丛林环境的影响，GPS信号在很大程度上会被摩天大楼阻挡或是进行反射，导致最终不能直接通过GPS进行定位与导航，因此基于低成本微电机系统(Micro Electro-Mechanic System,MEMS)的解决方案渐渐进入了人们的视线。基于MEMS的行人惯性导航系统使用的是包含在自身的一组惯性传感器，如加速度传感器，陀螺仪传感器和磁力传感器，不需要与外界进行通信，就可以估算和推测出行人的运动轨迹。像这样的惯性导航系统拥有体积小、成本低、高隐蔽性和高独立性等优点。因此惯性导航系统可以在GPS信号条件不好的环境中发挥自身的优势。然而，惯性导航系统自身有很多缺点，最严重的就是误差会随着时间累计，例如在用陀螺仪测量角速度时，由于陀螺仪自身的一些特性，存在着零点漂移等现象，所以需要使用各种算法来不断进行修正和处理，消除累计误差。并且由于精度的限制，一般的商业陀螺仪传感器不能测量大范围的角速度值，而当行人在行走时甚至上下楼梯时，瞬时角速度值会远大于其量程。

目前已经有学者对基于MEMS的惯性导航系统进行研究，为了克服系统自身的缺点并提高系统的精度，有些研究者用使用了计步器用于统计步伐数，同时基于这个统计的步伐数再计算出平均步伐长度，就可以推算和估测出行走的距离和轨迹，但是计步器不能区分出不同的步伐状态，比如跑步、踏步等，当行人行走速度快时，该方法会漏估计，当行人行走速度慢时，该方法会多估计，效果不是很理想。有人使用了一种比较复杂的类似之前的计步器的方法，他们将一个两轴加速度传感器和一个两轴陀螺安置在行人的脚上，通过神经网络的方法用加速度传感器估计出步伐长度，并加入改进的卡尔曼滤波技术，来计算出行人行走的轨迹，然而这种方法计算复杂度高，不能进行实时的导航。还有的研究者使用超声波和射频进行辅助导航，然后由于他们要求收端和发端之间的传播路径上不能受到阻挡，所以在地形不好的情况下会受到极大的限制。

因此，在当前国内外的研究结果中，大多数算法的精度仍然不是很高，并且有些并没有考虑到计算复杂度对实时性的影响。

发明内容

为了解决背景技术提出的技术问题，本发明旨在提供基于惯性传感器的行人导航装置和方法，在保证精确度的同时极大限度地降低了计算的复杂度，在实际环境中不必消耗大量硬件功耗也能保证实时性的要求。

为了实现上述技术目的，本发明的技术方案为：

基于惯性传感器的行人导航装置，包括微处理器以及分别与之连接的三轴加速度传感器、三轴陀螺仪、三轴磁力计和无线通信模块，所述微处理器接收三轴加速度传感器、三轴陀螺仪和三轴磁力计采集的数据，并通过无线通信模块将这些数据上传至PC机上。

本发明还包括基于上述基于惯性传感器的行人导航装置的行人导航方法，将基于惯性传感器的行人导航装置固定在待测行人的脚上，三轴加速度传感器、三轴陀螺仪和三轴磁力计将采集到的数据通过无线通信模块实时上传至PC机上，再将这些数据经数据融合算法计算出行人在三维空间中的运动轨迹，并将其在屏幕上实时显示；所述数据融合算法包括步伐阶段检测算法、脚体方位估计算法以及扩展卡尔曼滤波算法，利用三轴陀螺仪采集的数据检测出步伐阶段，根据步伐阶段以及三轴加速度传感器、三轴陀螺仪、三轴磁力计采集的数据估计出脚体方位，最后经过扩展卡尔曼滤波算法计算出行人的导航数据。

其中，步伐检测算法是将步伐阶段分为依次循环的中间站立阶段、脚尖离地阶段、摆动阶段和脚跟触地阶段；

当第i₁个采样点及其后续τ-1个采样点的和角速度均满足：

ω_i ^b＜ω_TH|i₁≤i≤i₁+τ-1 (1)

则表示从i₁个采样点至其后续τ-1个采样点，行人处于中间站立阶段，其中，ω^b _xi、ω^b _yi、ω^b _zi为第i个采样点三轴上的角速度分量，ω_i ^b为第i个采样点的和角速度，ω_TH为角速度门限值；

当第i₂个采样点及其后续τ-1个采样点的和角速度均满足：

ω_i ^b＞ω_TH|i₂≤i≤i₂+τ-1 (2)

则表示从i₂个采样点至其后续τ-1个采样点，行人处于脚尖离地阶段；

当第i₃个采样点及其后续τ-1个采样点的俯仰角均满足：

θ_i ^b＞θ^b _i-1|i₃≤i≤i₃+τ-1 (3)

则表示从i₃个采样点至其后续τ-1个采样点，行人处于摆动阶段，其中，θ_i ^b表示第i个采样点脚的俯仰角；

当第i₄个采样点及其后续τ-1个采样点的俯仰角均满足：

θ_i ^b＜θ^b _i-1|i₄≤i≤i₄+τ-1 (4)

则表示从i₄个采样点至其后续τ-1个采样点，行人处于脚跟触地阶段。

其中，估计脚体方位的具体方法包括行人处于中间站立阶段时的脚体方位估计和行人处于非中间站立阶段时的脚体方位估计；

所述中间站立阶段时的脚体方位估计的步骤：

(1)利用三轴加速度传感器测得的三轴上的加速度值计算出脚体的俯仰角θⁿ，进而得到脚体围绕Xⁿ轴旋转θⁿ角度的旋转四元数q_θ ⁿ；

(2)利用三轴加速度传感器测得的三轴上的加速度值计算出脚体的横滚角进而得到脚体围绕Zⁿ轴旋转角度的旋转四元数

(3)利用三轴磁力计测得的三轴上的磁场强度值计算出脚体的偏航角ψⁿ，进而得到脚体围绕Yⁿ轴旋转ψⁿ角度的旋转四元数q_ψ ⁿ；

(4)将q_θ ⁿ、q_ψ ⁿ依次相乘得到脚体方位四元数q_foot。

所述非中间站立阶段时的脚体方位估计的步骤：

(1)利用三轴陀螺仪测得三轴上的瞬时角速度值以及前一个采样时刻的脚体方

位四元数根据式(5)求出当前时刻的脚体方位的微分四元数

上式中， 表示四元数乘法运算；

(2)建立微分方程，得到当前时刻的脚体方位四元数q_foot：

上式中，T_S为采样时间。

其中，扩展卡尔曼滤波算法的具体步骤为：

(1)设置状态矢量、卡尔曼增益以及均方差的初值，其中状态矢量为13维，分别为导航坐标系下三个方向的角度、位置、速度的测量值以及脚体方位四元数与它们本身真实值的偏差；

(2)计算13维状态矢量的状态转移矩阵，预测t时刻的状态矢量和先验均方差；

(3)基于零速度更新算法，当行人处于中间站立阶段时，计算出t时刻的卡尔曼增益，并根据卡尔曼增益计算出状态矢量的偏差，根据卡尔曼增益和状态矢量的偏差更新状态矢量和后验均方差。

采用上述技术方案带来的有益效果：

(1)本发明适用在无法接收到GPS信号的室内使用，弥补了GPS定位系统在室内无法工作的缺陷。

(2)本发明仅仅使用了惯性传感器进行导航，没有用到Wifi或电子地图等外界信息，故无需事先知道导航场景的布局，可用于完全陌生的环境。

(3)本发明的精确度较高，可以满足日常生活对定位导航的需求。

(4)本发明的计算复杂度较低，可用于实时的行人导航，实际应用价值较高。

附图说明

图1为本发明的系统结构框图；

图2为本发明数据融合算法的原理框图；

图3为本发明行人行走各个阶段的示意图；

图4为本发明行走各个阶段中脚体俯仰角的变化趋势示意图；

图5为本发明步伐阶段检测算法的实验验证图，包括(a)、(b)两幅图，分别为俯仰角度和步伐阶段趋势图。

图6为本发明扩展卡尔曼滤波算法流程框图；

图7为本发明数据融合算法的精确度结果图；

图8为本发明数据融合算法的时间复杂度结果图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

如图1所示本发明的系统结构框图，基于惯性传感器的行人导航装置，包括微处理器以及分别与之连接的三轴加速度传感器、三轴陀螺仪、三轴磁力计和无线通信模块，所述微处理器接收三轴加速度传感器、三轴陀螺仪和三轴磁力计采集的数据，并通过无线通信模块将这些数据上传至PC机上。在本实施例中，三轴加速度传感器采用Freescale公司的MMA7361，三轴陀螺仪采用InvenSense公司的500系列，三轴磁力计采用Honeywell公司的HMC5842，无线通信模块采用支持ZigBee/IEEE 802.15.4技术的低功耗射频芯片CC2420，发送的频率不得低于20Hz。

本发明还包括基于上述装置的行人导航方法，将基于惯性传感器的行人导航装置固定在待测行人的脚上，三轴加速度传感器、三轴陀螺仪和三轴磁力计将采集到的数据通过无线通信模块实时上传至PC机上，再将这些数据经数据融合算法计算出行人在三维空间中的运动轨迹，并将其在屏幕上实时显示。

其中，数据融合算法为本发明的重点，其过程如图2所示。所述数据融合算法包括步伐阶段检测算法、脚体方位估计算法以及扩展卡尔曼滤波算法，利用三轴陀螺仪采集的数据检测出步伐阶段，根据步伐阶段以及三轴加速度传感器、三轴陀螺仪、三轴磁力计采集的数据估计出脚体方位，最后经过包含零速度更新算法的扩展卡尔曼滤波算法计算出行人的导航数据。

如图3所示行人在正常步行时的阶段示意图。经过详细的分析后发现，正常步行时的步伐可以粗略分为两个阶段：直立阶段和摆动阶段，其中直立阶段可以继续分成三个小阶段，分别是脚跟触地阶段、中间站立阶段和脚尖离地阶段。当走路的时候，步伐周期从中间站立阶段开始，然后进入脚尖离地阶段，接着进入摆动阶段，最后进入脚跟触地阶段，完成了一个步伐周期的行为运动。

经过细致地研究发现，使用陀螺仪来进行步伐阶段的判断比使用加速度传感器更为简单和准确，所以在本发明中依据陀螺仪来判断步伐阶段。为了清晰地理解步伐周期中各个阶段的标识性节点，图4展示了一个步伐周期中在载体坐标系里脚体的俯仰角和步伐事件的关系。

在载体坐标系中，当第i₁个采样点及其后续τ-1个采样点的和角速度均满足：

ω_i ^b＜ω_TH|i₁≤i≤i₁+τ-1 (1)

则表示从i₁个采样点至其后续τ-1个采样点，行人处于中间站立阶段，其中，ω^b _xi、ω^b _yi、ω^b _zi为第i个采样点三轴上的角速度分量，ω_i ^b为第i个采样点的和角速度，ω_TH为角速度门限值；

当第i₂个采样点及其后续τ-1个采样点的和角速度均满足：

ω_i ^b＞ω_TH|i₂≤i≤i₂+τ-1 (2)

则表示从i₂个采样点至其后续τ-1个采样点，行人处于脚尖离地阶段；

当第i₃个采样点及其后续τ-1个采样点的俯仰角均满足：

θ_i ^b＞θ^b _i-1|i₃≤i≤i₃+τ-1 (3)

则表示从i₃个采样点至其后续τ-1个采样点，行人处于摆动阶段，其中，θ_i ^b表示第i个采样点脚的俯仰角；

当第i₄个采样点及其后续τ-1个采样点的俯仰角均满足：

θ_i ^b＜θ^b _i-1|i₄≤i≤i₄+τ-1 (4)

则表示从i₄个采样点至其后续τ-1个采样点，行人处于脚跟触地阶段。

如图5所示本发明中提出的步伐阶段检测算法的实验验证图，包括(a)、(b)两幅图，分别为俯仰角度和步伐阶段趋势图。图中显示了脚体的俯仰角与步伐阶段的关系，可以很清晰地看出步伐阶段检测算法是比较准确可靠的，判断的步伐阶段基本正确。

在正确判断出行人的不同步伐阶段后，需要正确的使用惯性传感器来对脚体在不同阶段下的方位进行正确的估计，在得到脚体的方位四元数后，进而得到从载体坐标系到导航坐标系的转换，并以此计算出在导航坐标系中的加速度数值再加以分析。

本发明使用三轴加速度传感器、三轴陀螺仪和三轴磁力计共同进行对脚体的方位估计，当行人处于中间站立阶段时，脚上传感器节点的线性加速度值很小，所以此时加速度传感器测得的加速度值可以近似认为能代表当地的重力加速度值，由于偏航角的改变不会影响加速度传感器的读数，此时根据加速度值可以计算出脚体在导航坐标系中的俯仰角θⁿ和横滚角并利用加速度值对磁力计测出的当地磁力值进行修正，进而得出偏航角ψⁿ。而当行人处于非中间站立阶段时，加速度传感器测得的就不再仅是当地的重力加速度值，还包括脚体三个方向上的线性加速度值，这时本发明使用陀螺仪以及上一个采样点脚体的方位，利用四元数微分方程来进一步对当前时刻脚体的方位进行估计。

一、中间站立阶段时脚体方位估计：

根据刚体旋转的性质，一次三个方向上的旋转可以视为三次各个方向上的旋转合成而来，利用这个性质，我们分别求出脚体绕着导航坐标系的Xⁿ、Zⁿ、Yⁿ旋转的角度俯仰角θⁿ、横滚角和偏航角ψⁿ，得到三次旋转的四元数，最后将它们依次相乘，便可得出完整的脚体方位四元数。

假设行人处于中间站立状态时，加速度传感器在三个方向上测得的加速度数值是根据立体几何的知识，可以求得导航坐标系中用来表示的俯仰角θⁿ的值：

所以可以得出脚体围绕Xⁿ轴旋转θⁿ角度的旋转四元数q_θ ⁿ：

在正确的估计出脚体在导航坐标系中的俯仰角θⁿ后，在X^b轴和Z^b轴方向上，加速度传感器测量得到的和值可以表示为：

其中，gⁿ表示当地重力加速度值。现将两式相除，进而可以得到：

同样地，可以得出脚体围绕Zⁿ轴旋转角度的旋转四元数

假设表示磁力计在载体坐标系三个轴向上读取的数据，即磁力计在三个方向上测得的磁场强度值，一般情况下，当磁力计水平放置时，偏航角ψⁿ可以用以下公式计算得到：

其中，D表示当地的地磁偏角。

但是当磁力不是水平放置的时候，再利用式(10)得出的偏航角就不再是正确的偏航角了，所以需要利用加速度传感器得到的数据对三轴磁力计数据进行矫正。使用上文中已经得出的q_θ ⁿ、将三维中的磁场强度值转换到导航坐标系中水平面上的磁场强度值：

其中，mⁿ和m^b分别是导航坐标系和载体坐标系中三个轴向的地磁场强度，和分别是q_θ ⁿ和的共轭四元数，在公式计算时，标量部分被视为0，即仅包含矢量部分的四元数，符号表示四元数乘法。

经过式(11)计算后，套用式(10)，便可以得到脚体经过加速度传感器矫正后的偏航角ψⁿ：

利用四元数的定义，便可以求出脚体绕着Yⁿ轴旋转ψⁿ角度的旋转四元数

最后将求出的三次旋转四元数依次相乘，便可以得到行人处于中间站立阶段时脚体的方位四元数q_foot:

非中间站立阶段时脚体方位估计：

当行人离开中间站立阶段进入脚尖离地阶段时，并直到下一次进入中间站立阶段之前，脚体的线性加速度可能瞬时达到很大值，所以此时再使用加速度传感器来进行方位姿态的估计就不够准确。因此，在非中间站立阶段时，本发明使用四元数微分方程，来对脚体的方位进行估计。

假设ω^b _x、ω^b _y、ω^b _z分别为陀螺仪在载体坐标系中三个方向上瞬时角速度的值，根据前一个时刻估计的四元数利用四元数的微分方程可以得到当前时刻的微分四元数

其中，

那么经过采样时间T_S后，利用微分方程，得到该时刻的方位四元数q_foot：

利用式(14)和(16)便可以得到行人在完整步伐周期中脚体的方位姿态四元数q_foot。

根据计算得出的脚体方位姿态四元数，得到用四元数表示的从载体坐标系到导航坐标系的变换矩阵C_b2n：

其中，rⁿ、r^b只是为了说明导航坐标系下和载体坐标系下的两个方向向量之间的对应关系，q*是四元数q的共轭四元数。

由于变换矩阵C_b2n是酉矩阵，所以变换矩阵C_b2n又可以写成：

所以对加速度值从载体坐标系到导航坐标系有如下变换：

aⁿ＝C_b2n·a^b (19)

本发明的扩韩卡尔曼滤波算法如图6所示，在设置了状态矢量、卡尔曼增益、均方差的初值后就可以根据观测值推算出当前时刻的状态矢量。将导航坐标系下的加速度值积分得到速度的值，再进行积分得到位置的值。因此在本发明中，使用状态矢量大小为13维的扩展卡尔曼滤波：

其中，13个状态变量分别为在导航坐标系下的三个方向的角度值φⁿ、速度值vⁿ、位置值pⁿ、表示脚体方向的四元数q_foot与各自真实值的偏差。

对于t时刻下导航坐标系中的位置与真实值的偏差和上一时刻的偏差的状态转移方程可以表示为：

对于t时刻下导航坐标系中的速度与真实值的偏差和上一时刻的偏差的状态转移方程可以表示为：

其中，S表示加速度aⁿ的反对称矩阵，用作和φⁿ进行差乘运算，将脚体因旋转而产生的加速度考虑进卡尔曼滤波器里：

其中，分别表示在导航坐标系下三个方向上的加速度值。

接着，得到微分四元数和载体坐标系中载体在三个方向上角速度的关系：

所以得到四元数与真实值的偏差的状态方程为：

其中，

因此，得到所有13维状态矢量的状态转移矩阵Φ：

正确预测出下一时刻的状态矢量：

x_t ^-＝Φ_t-1x_t-1+W_t-1 (27)

对先验均方差进行预测：

其中，W_t-1是t-1时刻的过程噪声，Q_t-1是t-1时刻的过程激励噪声协方差。

下面将零速度更新算法应用到卡尔曼滤波器中，正如之前讨论的，当行人处于中间站立阶段时，绑有传感器节点的那只脚的速度为0，并且认为角速度也为0，即在导航坐标系中的三个方向的角度不会改变，同时认为此时脚体的四元数以用加速度传感器和磁力传感器测的数据计算得到的四元数为准。那么得到观测方程中的测量矩阵Η：

当检测到行人处于中间站立阶段时，应用卡尔曼滤波，计算t时刻的卡尔曼增益：

然后计算出t时刻的状态矢量：

其中，R_t是t时刻的观测噪声协方差，z_t和分别是观测值的真值和估计值。然后计算出状态矢量的偏差ε_t：

得到状态矢量的偏差ε_t后，便可对状态矢量进行更新。首先对三个方向角度进行更新：

φⁿ＝φⁿ+ε_φ (33)

接着对速度和位置进行更新：

vⁿ＝vⁿ+ε_v (34)

pⁿ＝pⁿ+ε_p (35)

再利用之前得到的角度偏差ε_φ，计算出旋转四元数q_ε,对四元数进行更新：

q_foot＝q_ε·q_foot (36)

然后将新的脚体四元数q_foot利用式(18)对变换矩阵C_b2n进行更新。至此，完成了对状态矢量的更新，最后用式(37)对后验均方差进行更新：

重复上面的步骤，即可完成整个卡尔曼滤波算法，算法计算得出滤波后的脚体角度、位置、速度与脚体四元数的估计。

在实际验证中，测试场景选择为矩形的区域，总长为206米，由于步行者行走的路线是闭合的，所以如果计算准确，那么将得到终点和起点完全重合的路径，因此在本实验中，将计算得出的终点与起点的距离和计算的路径长度与实际行走的路径长度的差值作为两个定量评价指标。一共做了三组对比实验，将使用扩展卡尔曼滤波算法的结果与未使用扩展卡尔曼滤波算法的结果进行比较，从图7可以看出使用扩展卡尔曼滤波算法之后准确性比较高，误差可以基本控制在1％以内，该系统可以满足一般行走情况下的轨迹追踪需求。

接着又进行了三组时间复杂度的实验，我们选取了当前比较流行的两种算法与本发明提出的方法进行比较，分别是A.R.Jimenez等人提出的IEZ算法和Foxlin提出的PDR算法。三种算法共同计算同一组实验数据，结果如图8所示，可以发现，本发明提出的算法在计算速度上最快，比IEZ算法平均快22％。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (3)

1.基于惯性传感器的行人导航装置的行人导航方法，所述基于惯性传感器的行人导航装置包括微处理器以及分别与之连接的三轴加速度传感器、三轴陀螺仪、三轴磁力计和无线通信模块，所述微处理器接收三轴加速度传感器、三轴陀螺仪和三轴磁力计采集的数据，并通过无线通信模块将这些数据上传至PC机上；其特征在于：将基于惯性传感器的行人导航装置固定在待测行人的脚上，三轴加速度传感器、三轴陀螺仪和三轴磁力计将采集到的数据通过无线通信模块实时上传至PC机上，再将这些数据经数据融合算法计算出行人在三维空间中的运动轨迹，并将其在屏幕上实时显示；所述数据融合算法包括步伐阶段检测算法、脚体方位估计算法以及扩展卡尔曼滤波算法，利用三轴陀螺仪采集的数据检测出步伐阶段，根据步伐阶段以及三轴加速度传感器、三轴陀螺仪、三轴磁力计采集的数据估计出脚体方位，最后经过扩展卡尔曼滤波算法计算出行人的导航数据；所述步伐检测算法是将步伐阶段分为依次循环的中间站立阶段、脚尖离地阶段、摆动阶段和脚跟触地阶段；

当第i₁个采样点及其后续τ-1个采样点的和角速度均满足：

ω_i ^b＜ω_TH|i₁≤i≤i₁+τ-1 (1)

则表示从i₁个采样点至其后续τ-1个采样点，行人处于中间站立阶段，其中，ω^b _xi、ω^b _yi、ω^b _zi为第i个采样点三轴上的角速度分量，ω_i ^b为第i个采样点的和角速度，ω_TH为角速度门限值；

当第i₂个采样点及其后续τ-1个采样点的和角速度均满足：

ω_i ^b＞ω_TH|i₂≤i≤i₂+τ-1 (2)

则表示从i₂个采样点至其后续τ-1个采样点，行人处于脚尖离地阶段；

当第i₃个采样点及其后续τ-1个采样点的俯仰角均满足：

θ_i ^b＞θ^b _i-1|i₃≤i≤i₃+τ-1 (3)

则表示从i₃个采样点至其后续τ-1个采样点，行人处于摆动阶段，其中，θ_i ^b表示第i个采样点脚的俯仰角；

当第i₄个采样点及其后续τ-1个采样点的俯仰角均满足：

θ_i ^b＜θ^b _i-1|i₄≤i≤i₄+τ-1 (4)

则表示从i₄个采样点至其后续τ-1个采样点，行人处于脚跟触地阶段。

2.根据权利要求1所述基于惯性传感器的行人导航装置的行人导航方法，其特征在于：估计脚体方位的具体方法包括行人处于中间站立阶段时的脚体方位估计和行人处于非中间站立阶段时的脚体方位估计；

所述中间站立阶段时的脚体方位估计的步骤：

(1)利用三轴加速度传感器测得的三轴上的加速度值计算出脚体的俯仰角θⁿ，进而得到脚体围绕Xⁿ轴旋转θⁿ角度的旋转四元数q_θ ⁿ；

(2)利用三轴加速度传感器测得的三轴上的加速度值计算出脚体的横滚角进而得到脚体围绕Zⁿ轴旋转角度的旋转四元数

(3)利用三轴磁力计测得的三轴上的磁场强度值计算出脚体的偏航角ψⁿ，进而得到脚体围绕Yⁿ轴旋转ψⁿ角度的旋转四元数q_ψ ⁿ；

(4)将q_θ ⁿ、q_ψ ⁿ依次相乘得到脚体方位四元数q_foot；

所述非中间站立阶段时的脚体方位估计的步骤：

(1)利用三轴陀螺仪测得三轴上的瞬时角速度值以及前一个采样时刻的脚体方位四元数根据式(5)求出当前时刻的脚体方位的微分四元数

$\stackrel{\·}{q}=\frac{1}{2}\hat{q}\⊗{\mathrm{\ω}}^b---\left(5\right)$

上式中， 表示四元数乘法运算；

(2)建立微分方程，得到当前时刻的脚体方位四元数q_foot：

$q_{foot}=\hat{q}+\stackrel{\·}{q}\·Ts---\left(6\right)$

上式中，T_S为采样时间。

3.根据权利要求2所述基于惯性传感器的行人导航装置的行人导航方法，其特征在于：扩展卡尔曼滤波算法的具体步骤为：

(1)设置状态矢量、卡尔曼增益以及均方差的初值，其中状态矢量为13维，分别为导航坐标系下三个方向的角度、位置、速度的测量值以及脚体方位四元数与它们本身真实值的偏差；

(2)计算13维状态矢量的状态转移矩阵，预测t时刻的状态矢量和先验均方差；

(3)基于零速度更新算法，当行人处于中间站立阶段时，计算出t时刻的卡尔曼增益，并根据卡尔曼增益计算出状态矢量的偏差，根据卡尔曼增益和状态矢量的偏差更新状态矢量和后验均方差。