CN105806343A - 基于惯性传感器的室内3d定位系统及方法 - Google Patents

Abstract

本发明公开了一种基于惯性传感器的室内3D定位系统，它的惯性传感器的信号输出端连接第一控制模块的信号输入端，第一控制模块的信号输出端连接第一无线通信模块的信号输入端，第二无线通信模块的信号输出端连接第二控制模块的信号输入端，第二控制模块的信号输出端通过串口通信模块连接计算机的数据通信端，所述第一无线通信模块和第二无线通信模块之间无线通信连接。本发明能实现室内准确的3D定位。本发明能实现室内准确的3D定位。

Description

基于惯性传感器的室内3D定位系统及方法

技术领域

本发明涉及室内定位技术领域，具体地指一种基于惯性传感器的室内3D定位系统及方法。

背景技术

现代社会中，随着科技不断的发展，基于位置的定位服务越来越多的应用于日常生活中，如现如今生活中常用的高德地图和谷歌地图等，此类的服务的发展都是基于全球定位系统GPS(GlobalPositionSystem)，现如今GPS定位服务已经很成熟，并可以达到很高的定位精度，但是GPS定位服务主要针对的应用场景是在没有遮掩的室外，对于有遮挡的在室内会出现峡谷效应，不能正常的工作，体现在信号的不稳定，定位精度的明显下降，所以GPS存在着定位上的瓶颈，不适合用于室内的定位。

室内定位的问题受到的关注度越来越高，人们对室内定位的需求日益增大，现在已有大量的关于室内定位的技术，如基于红外的室内定位技术、基于超宽带的定位技术、基于RFID的室内定位技术、基于WiFi的室内定位技术，各自有优缺点，总的来说此类的定位技术都是侧重于室内的2D定位服务，但是现在的楼层结构复杂，缺少高度信息的2D室内定位，往往不能满足现在的室内定位需求。如消防人员在火灾现场的定位服务，随着社会的进步发展，城市中的高层建筑彼此起伏，当火灾发生时，产生大量的烟雾，烟雾造成能见度很低，而且环境恶劣，消防人员无法有效的判断自己所处的楼层位置和环境，出现危险时不能及时通过语音汇报，导致指挥工作者不能展开即时有效的营救，从而不能保护消防人员的人身安全；再如医院病人的跟踪定位服务，当病人在医院某处发生突发病情时，可以即时的定位发现，并给予即时有效的治疗，避免错过有效抢救时间。同时在当代人们的日常生活中，大部分时间的活动都在室内，人们消费行为发生在室内的时间比例更高，这使得室内导航蕴含了巨大的商业契机。因此如何解决室内的3D定位，成为众多研究者的共同话题。

发明内容

本发明的目的就是要提供一种基于惯性传感器的室内3D定位系统及方法，该装置和方法能实现室内准确的3D定位。

为实现此目的，本发明所设计的一种基于惯性传感器的室内3D定位系统，它包括惯性数据采集节点、网管节点和计算机，其中，所述惯性数据采集节点包括第一控制模块、惯性传感器和第一无线通信模块，所述网管节点包括第二控制模块、串口通信模块和第二无线通信模块，其中，惯性传感器的信号输出端连接第一控制模块的信号输入端，第一控制模块的信号输出端连接第一无线通信模块的信号输入端，第二无线通信模块的信号输出端连接第二控制模块的信号输入端，第二控制模块的信号输出端通过串口通信模块连接计算机的数据通信端，所述第一无线通信模块和第二无线通信模块之间无线通信连接。

一种利用上述基于惯性传感器的室内3D定位系统进行3D定位的方法，其特征在于，它包括如下步骤：

步骤1：将惯性数据采集节点安装在待定位人体上，惯性数据采集节点中的惯性传感器采集待定位人体实时的加速度数据和角速度数据，并将得到的加速度数据和角速度数据传输给计算机；

步骤2：计算机根据所述加速度数据计算得到待定位人体初始姿态信息，并根据所述角速度数据计算得到待定位人体的运动状态信息，对待定位人体的姿态信息进行基于四元数的姿态初始化，四元数表示为:

Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k

其中，i、j、k为三个虚数单位，q₁表示待定位人体所处空间X轴正向向待定位人体所处空间Y轴正向的旋转值，q₂表示待定位人体所处空间Z轴正向向待定位人体所处空间X轴正向的旋转值，q₃表示待定位人体所处空间Y轴正向向待定位人体所处空间Z轴正向的旋转值，旋转值得大小通过三个轴的角速度值积分得到，利用采集得到的加速度数据和角速度数据计算姿态信息和四元数，设参考坐标系的初始的四元数为q₀＝[1000]，设参考坐标系的重力加速度向量a_R＝[001]^T；

又因为四元数的旋转矩阵为：

$C_b^R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_3{q}_0)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

那么由参考坐标系O到惯性传感器坐标O′的重力加速度可以表示为：

$g_b=C_b^R\·a_R$

$g_b=\left(\begin{array}{c}{g}_{bx}\\ g_{by}\\ g_{bz}\end{array}\right)=\left(\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)$

其中，g_bx、g_by和g_bz分别为惯性传感器坐标O′的重力加速度在三个方向上的值；

实际采样得到的加速度向量可以表示为g_R＝[a_xa_ya_z]^T，实际采样得到加速度向量做归一化处理：

$g_R^{\′}=\frac{g_R}{\sqrt{a_x^2+a_y^2+a_z^2}}$

算出a_R向量与g′_R(R＝1,2,3,···,n)向量的向量积v_n(n＝1,2,3,···,n)：

v_n＝a_R×g′_R

v_n表示参考坐标系的重力加速度与惯性传感器所在坐标系，每个轴角度的差值，也可以理解为经过采样率Δt的时间到R时刻的角度的变化，再解算出惯性传感器所在坐标系的四元数矩阵即姿态，用于下一时刻姿态的估算，假设四元数解算的过程为P，那么解算得到的四元数可以表示为：

Q_n(q_n0+q_n1+q_n2+q_n3)＝v_n·P

将计算得到的四元数Q_n带入到重力加速度的表达式中计算出重力加速度的向量g_b，再次与真实采集到重力加速度的归一化向量算出向量积，完成初始姿态的计算；

步骤3：在待定位人体移动的过程中需要消除陀螺仪的测量误差，消除陀螺仪测量误差的具体方法为，通过在姿态的四元数Q_n计算出待定位人体移动时相对于参考坐标系O的实时重力向量g_b，实测加速度的值进行归一化处理变为向量g′_R，计算g_b、g′_R的向量积为角速度漂移误差e＝(e_xe_ye_z)，用来修正陀螺仪的角速度的数据，向量积向量e仍旧位于待定位人体坐标系上，向量积向量e的大小与惯性传感器中陀螺仪的误差成正比，误差越大则向量积e的越大，可以用来纠正陀螺仪的数据，由于角速度数据的积分是对惯性传感器所在的自身坐标系积分，所以误差量包含在对角速度的积分当中，所以有：

$e=g_b\×g_R^{\′}=\left|\begin{array}{ccc}x& y& z\\ g_{Rx}^{\′}& g_{Ry}^{\′}& g_{Rz}^{\′}\\ g_{bx}& g_{by}& g_{bz}\end{array}\right|$

式中，g_bx、g_by和g_bz分别为惯性传感器坐标O′的重力加速度在三个方向上的值，g′_Rx、g′_Ry、g′_Rz分别为向量g′_R在三个方向上的值，x、y、z是惯性传感器所在坐标系的单位向量，经化简可以得到：

$e=\left(\begin{array}{c}{e}_x\\ e_y\\ e_z\end{array}\right)=\left(\begin{array}{c}{g}_{Ry}^{\′}\·g_{bz}-g_{Rz}^{\′}\·g_{by}\\ g_{Rz}^{\′}\·g_{by}-g_{Rx}^{\′}\·g_{bz}\\ g_{Rx}^{\′}\·g_{by}-g_{Ry}^{\′}\·g_{bx}\end{array}\right)$

综合上述，惯性传感器中陀螺仪的角速度可以表示为：

gyro′＝gyro-Kp·e·stationary

式中，gyro是陀螺仪的角速度向量，Kp为惯性传感器的室内3D定位系统的增益误差系数，stationary为人体的步态信号函数，即待定位人体脚落地时刻和迈步时刻组成连续信号；

步骤4:四元数的更新，步骤3根据步态信息消除陀螺仪的测量误差是为了姿态的解算做准备，四元数的更新本质就是求解四元数的微分方程，为了提高计算的精度同时考虑到算法的复杂度，惯性传感器的室内3D定位系统采用四阶龙格库塔求解四元数的微分方程，对一阶的龙格库塔公式进行推导、求解，得出四阶的龙格库塔公式。根据四元数的微分方程定义:

$Q^{\′}=\frac{1}{2}w\left(t\right)*Q\left(t\right)$

其中，w(t)代表角速度的变化，Q(t)表示t时刻的四元数值即待定位人体的姿态信息；

假设t₀时刻

Q(t₀)＝Q₀

Q₀表示t₀时刻的姿态信息即初始时刻；

对应的微分方程为：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\left(t\right)\\ {\stackrel{\·}{q}}_1\left(t\right)\\ {\stackrel{\·}{q}}_2\left(t\right)\\ {\stackrel{\·}{q}}_3\left(t\right)\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -w_x& -w_y& -w_z\\ w_x& 0& w_z& -w_y\\ w_y& -w_z& 0& w_x\\ w_z& w_y& -w_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\left(t\right)\\ q_1\left(t\right)\\ q_2\left(t\right)\\ q_3\left(t\right)\end{array}\right]$

q_o(t)、q₁(t)、q₂(t)和q₃(t)分别为t时刻，四元数的值；

采用四阶的龙格库塔求解四元数的微分方程如下：

$Q_{n+1}=Q_n+\frac{h}{6}(k_1+2k_2+3k_3+k_4)$

式中，Q_n第N点的四元数信息，Q_n+1第N+1点的四元数信息，即待定位人体移动时的姿态信息，

k₁＝f(w_n,Q_n)

$k_2=f(w_n+\frac{h}{2},Q_n+\frac{h}{2}{k}_1)$

$k_3=f(w_n+\frac{h}{2},Q_n+\frac{h}{2}{k}_2)$

k₄＝f(w_n+h,Q_n+hk₃)

$f(w_n,Q_n)=\frac{1}{2}w\left(t\right)*Q\left(t\right)$

其中w_x,w_y,w_z为惯性传感器输出的实际角速度，h为惯性传感器实际的采集间隔时间；

在使用实时计算四元数更新时，会逐渐失去四元数规范化特性，必须对四元数做规范化处理，规范化方程为：

$q_i=\frac{q_i}{\sqrt{{\hat{q}}_0^2+{\hat{q}}_1^2+{\hat{q}}_2^2+{\hat{q}}_3^2}}$

其中i＝1,2,3，为四元数更新所得值，四元数更新所得值通过公式计算得到，Q_n+1包括

步骤5:基于步态的累积误差消除方法实现，经过步骤4得到待定位人体移动时的姿态信息，通过姿态映射采集回的加速度信息，计算待定位人体实时姿态下的3个轴上的加速度数据，加速度数据经过一次积分得到移动的距离信息，利用待定位人体移动时的起点坐标和待定位人体移动时的距离信息根据里程计推算的方式实现待定位人体的室内3D定位。

本发明的有益效果：

1、本发明提供一种低成本的基于惯性传感器的室内3D定位系统，容易推广；

2、本发明结构简单而且适用于室内3D定位，以往的技术方案大部分只是设计到室内的2D定位，如WiFi室内定位、RFID室内定位、红外线室内定位，并没有涉及室内3D定位，而且基于惯性传感器的室内3D定位，往往采取的方式需要借助于额外的传感器(如气压传感器)，辅助得到高度信息，但是结果并不是很理想，而且增加了系统的复杂性；

3、本发明从实现的方法上具有很好的普适性，使用捷联的方式能准确的估算出步长。

4、本发明提出行走状态的判断，并以此消除高度误差，提高了高度信息的准确性。

5、本发明预先只通过三轴加速度计来计算姿态四元数的初始值，结合步态信息和并利用加速计在较长时间静止状态数据漂移的稳定性高于陀螺仪数据的特性，通过四元数微分方程解得运载体的姿态。

6、本发明提出行走状态判断模型，行走的每一步都可以看作为向量，是由水平平面向量和垂直平面向量组成，利用每一步的向量和垂直平面向量计算出角度的变化，从而可区分平面内移动(正常行走、慢跑)、上楼、下楼三种状态。

7、本发明提出行走状态判断模型，并利用步态信息和此模型消除平面内行走时的垂直距离误差，结合步态信息和行走状态的判断，针对平面内的行走，从距离的层面消除平面内移动时(正常行走、慢跑)垂直距离的误差。

附图说明

图1为本发明的结构示意图；

图2为本发明中加速度误差特性分析曲线图；

图3为本发明中待定位人体的姿态初始化图；

图4为本发明中基于步态的姿态解算图；

图5为本发明中基于步态的累计误差消除过程示意图；

图6为本发明中基于误差线性的特点并结合步态速度修正前后对比图；

图7为本发明中待定位人体下楼和上楼的示意图；

图8为本发明中待定位人体行走状态的角度对比图；

图9为本发明中垂直方向误差累计示意图；

图10为本发明中平面内垂直距离误差消除流程；

图11为本发明中垂直方向距离误差消除对比示意图。

其中，1—惯性数据采集节点、1.1—第一控制模块、1.2—惯性传感器、1.3—第一无线通信模块、2—网管节点、2.1—第二控制模块、2.2—串口通信模块、2.3—第二无线通信模块、3—计算机。

具体实施方式

以下结合附图和具体实施例对本发明作进一步的详细说明：

如图1所述的基于惯性传感器的室内3D定位系统，它包括惯性数据采集节点1、网管节点2和计算机3，其中，所述惯性数据采集节点1包括第一控制模块1.1、惯性传感器1.2和第一无线通信模块1.3，所述网管节点2包括第二控制模块2.1、串口通信模块2.2和第二无线通信模块2.3，其中，惯性传感器1.2的信号输出端连接第一控制模块1.1的信号输入端，第一控制模块1.1的信号输出端连接第一无线通信模块1.3的信号输入端，第二无线通信模块2.3的信号输出端连接第二控制模块2.1的信号输入端，第二控制模块2.1的信号输出端通过串口通信模块2.2连接计算机3的数据通信端，所述第一无线通信模块1.3和第二无线通信模块2.3之间无线通信连接。

上述技术方案中，采集节点1将采集到陀螺仪和加速度数据(通过惯性传感器1.2采集)打包，以无线的方式将数据发送到网管节点2，网管节点2主要实现数据的解析和转发功能，将接解析后的数据包通过串口转发至计算机3，进行进一步的处理，再使用unity3D完成的3D地图上显示位置信息。采集节点1采用鞋绑的方式固定在带定位人体的脚背上，网关节点2通过有限串口的方式与计算机3相连，采集结点1与网关节点2的通信采用无线的方式。

上述技术方案中，第一控制模块1.1和第二控制模块2.1均选用STM32F103处理器，惯性传感器1.2选用MPU6050，第一无线通信模块1.3和第二无线通信模块2.3均选用NRF24L01。

一种利用上述基于惯性传感器的室内3D定位系统进行3D定位的方法，它包括如下步骤：

步骤1：将惯性数据采集节点1安装在待定位人体上，惯性数据采集节点1中的惯性传感器1.2采集待定位人体实时的加速度数据和角速度数据，并将得到的加速度数据和角速度数据传输给计算机3，所述加速度数据和角速度数据需要经过数据的预处理，预处理包括滤波、插值拟合，滤波的方法采用中值滤波；插值拟合的方式采用三次样条插值方法；经过这两种方式处理完得到噪声较小、拟合度高的加速度和角速度数据；

步骤2：基于四元数的姿态初始化(如图3所示)，计算机3根据所述加速度数据计算得到待定位人体初始姿态信息，并根据所述角速度数据计算得到待定位人体的运动状态信息，对待定位人体的姿态信息进行基于四元数的姿态初始化(四元数是简单的超复数)，四元数表示为:

Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k

其中，i、j、k为三个虚数单位，q₁表示待定位人体所处空间X轴正向向待定位人体所处空间Y轴正向的旋转值，q₂表示待定位人体所处空间Z轴正向向待定位人体所处空间X轴正向的旋转值，q₃表示待定位人体所处空间Y轴正向向待定位人体所处空间Z轴正向的旋转值，旋转值得大小通过三个轴的角速度值积分得到，利用采集得到的加速度数据和角速度数据计算姿态信息和四元数，设参考坐标系的初始的四元数为q₀＝[1000]，设参考坐标系的重力加速度向量a_R＝[001]^T；

又因为四元数的旋转矩阵为：

$C_b^R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_3{q}_0)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

那么由参考坐标系O到惯性传感器坐标O′的重力加速度可以表示为：

$g_b=C_b^R\·a_R$

$g_b=\left(\begin{array}{c}{g}_{bx}\\ g_{by}\\ g_{bz}\end{array}\right)=\left(\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)$

其中，g_bx、g_by和g_bz分别为惯性传感器坐标O′的重力加速度在三个方向上的值；

实际采样得到的加速度向量可以表示为g_R＝[a_xa_ya_z]^T，实际采样得到加速度向量做归一化处理：

$g_R^{\′}=\frac{g_R}{\sqrt{a_x^2+a_y^2+a_z^2}}$

算出a_R向量与g′_R(R＝1,2,3,···,n)向量的向量积v_n(n＝1,2,3,···,n)：

v_n＝a_R×g′_R

v_n表示参考坐标系的重力加速度与惯性传感器所在坐标系，每个轴角度的差值，也可以理解为经过采样率Δt的时间到R时刻的角度的变化，再解算出惯性传感器所在坐标系的四元数矩阵即姿态，用于下一时刻姿态的估算，假设四元数解算的过程为P，那么解算得到的四元数可以表示为：

Q_n(q_n0+q_n1+q_n2+q_n3)＝v_n·P

将计算得到的四元数Q_n带入到重力加速度的表达式中计算出重力加速度的向量g_b，再次与真实采集到重力加速度的归一化向量算出向量积，完成初始姿态的计算；

步骤3：基于步态的AHRS陀螺仪误差消除实现，在待定位人体移动的过程中需要消除陀螺仪的测量误差(因为陀螺仪是引起数据漂移的主要原因，修正陀螺仪的误差是提供定位准确度的关键。如图4为基于步态的姿态解算图。当人体行走的时候，脚落地的时刻，加速度接近为一个相对恒定的值，相似于惯性传感器静止的时刻，相对于角速度，加速度数据是比较稳定的，所以可以利用加速度来修正角速度的误差，人在迈步的过程中，加速度数据是一个变化很剧烈的数据，不具有作为参考点的性质，不适合用于做为角速度数据修正的依据)，消除陀螺仪测量误差的具体方法为，通过在姿态的四元数Q_n计算出待定位人体移动时相对于参考坐标系O的实时重力向量g_b，实测加速度的值进行归一化处理变为向量g′_R，计算g_b、g′_R的向量积为角速度漂移误差e＝(e_xe_ye_z)，用来修正陀螺仪的角速度的数据，向量积向量e仍旧位于待定位人体坐标系上，向量积向量e的大小与惯性传感器1.2中陀螺仪的误差成正比，误差越大则向量积e的越大，可以用来纠正陀螺仪的数据，由于角速度数据的积分是对惯性传感器1.2所在的自身坐标系积分，所以误差量包含在对角速度的积分当中，所以有：

$e=g_b\×g_R^{\′}=\left|\begin{array}{ccc}x& y& z\\ g_{Rx}^{\′}& g_{Ry}^{\′}& g_{Rz}^{\′}\\ g_{bx}& g_{by}& g_{bz}\end{array}\right|$

式中，g_bx、g_by和g_bz分别为惯性传感器坐标O′的重力加速度在三个方向上的值，g′_Rx、g′_Ry、g′_Rz分别为向量g′_R在三个方向上的值，x、y、z是惯性传感器1.2所在坐标系的单位向量，经化简可以得到：

$e=\left(\begin{array}{c}{e}_x\\ e_y\\ e_z\end{array}\right)=\left(\begin{array}{c}{g}_{Ry}^{\′}\·g_{bz}-g_{Rz}^{\′}\·g_{by}\\ g_{Rz}^{\′}\·g_{by}-g_{Rx}^{\′}\·g_{bz}\\ g_{Rx}^{\′}\·g_{by}-g_{Ry}^{\′}\·g_{bx}\end{array}\right)$

综合上述，惯性传感器1.2中陀螺仪的角速度可以表示为：

gyro′＝gyro-Kp·e·stationary

式中，gyro是陀螺仪的角速度向量，Kp为惯性传感器的室内3D定位系统的增益误差系数，stationary为人体的步态信号函数，即待定位人体脚落地时刻和迈步时刻组成连续信号，当脚落地的时刻利用加速度修正陀螺仪的数据的误差，迈步过程中不修正，系统中更加相信陀螺仪的本身的角速度数据；

步骤4:四元数的更新，步骤3根据步态信息消除陀螺仪的测量误差是为了姿态的解算做准备，四元数的更新本质就是求解四元数的微分方程，为了提高计算的精度同时考虑到算法的复杂度，惯性传感器的室内3D定位系统采用四阶龙格库塔求解四元数的微分方程，对一阶的龙格库塔公式进行推导、求解，得出四阶的龙格库塔公式。根据四元数的微分方程定义:

$Q^{\′}=\frac{1}{2}w\left(t\right)*Q\left(t\right)$

其中，w(t)代表角速度的变化，Q(t)表示t时刻的四元数值即待定位人体的姿态信息；

假设t₀时刻

Q(t₀)＝Q₀

Q₀表示t₀时刻的姿态信息即初始时刻；

对应的微分方程为：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\left(t\right)\\ {\stackrel{\·}{q}}_1\left(t\right)\\ {\stackrel{\·}{q}}_2\left(t\right)\\ {\stackrel{\·}{q}}_3\left(t\right)\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -w_x& -w_y& -w_z\\ w_x& 0& w_z& -w_y\\ w_y& -w_z& 0& w_x\\ w_z& w_y& -w_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\left(t\right)\\ q_1\left(t\right)\\ q_2\left(t\right)\\ q_3\left(t\right)\end{array}\right]$

q_o(t)、q₁(t)、q₂(t)和q₃(t)分别为t时刻，四元数的值；

采用四阶的龙格库塔求解四元数的微分方程如下：

$Q_{n+1}=Q_n+\frac{h}{6}(k_1+2k_2+3k_3+k_4)$

式中，Q_n第N点的四元数信息，Q_n+1第N+1点的四元数信息，即待定位人体移动时的姿态信息，

k₁＝f(w_n,Q_n)

$k_2=f(w_n+\frac{h}{2},Q_n+\frac{h}{2}{k}_1)$

$k_3=f(w_n+\frac{h}{2},Q_n+\frac{h}{2}{k}_2)$

k₄＝f(w_n+h,Q_n+hk₃)

$f(w_n,Q_n)=\frac{1}{2}w\left(t\right)*Q\left(t\right)$

其中w_x,w_y,w_z为惯性传感器(1.2)输出的实际角速度，h为惯性传感器(1.2)实际的采集间隔时间；

在使用实时计算四元数更新时，会逐渐失去四元数规范化特性，必须对四元数做规范化处理，规范化方程为：

$q_i=\frac{q_i}{\sqrt{{\hat{q}}_0^2+{\hat{q}}_1^2+{\hat{q}}_2^2+{\hat{q}}_3^2}}$

其中i＝1,2,3，为四元数更新所得值，四元数更新所得值通过公式计算得到，Q_n+1包括

步骤5:基于步态的累积误差消除方法实现，经过步骤4得到待定位人体移动时的姿态信息，通过姿态映射采集回的加速度信息，计算待定位人体实时姿态下的3个轴上的加速度数据，加速度数据经过一次积分得到移动的距离信息，利用待定位人体移动时的起点坐标和待定位人体移动时的距离信息根据里程计推算的方式实现待定位人体的室内3D定位。

所述步骤5中，得到的人体移动时的距离信息存在着距离上的估算误差，如图5为基于步态的累计误差消除过程，人体的步态信号函数不仅可以当作角速度数据的参考量，用于陀螺仪误差消除依据，同时也是加速度累计误差的参考点，根据人体行走的特点，当脚落地的时候，此时的速度接近为零，加速度数据经过两次积分得到距离，根据惯性传感器1.2的定位分析，加速度的误差经过一次积分得到速度，即加速度的误差传播到速度上，而且在速度上的误差体现为速度近似成线性增长，连续时间下待定位人体的移动速度可以表示为：

$v\left(T\right)=v\left(t_0\right)+{\∫}_0^{T}acc\left(t\right)dt$

式中，v(t₀)为带定位人体的初始速度状态；

在离散时间下待定位人体的移动速度可以表示为：

$v\left(T\right)=v\left(0\right)+\underset{i=0}{\overset{T\·k}{\Σ}}\frac{1}{k}\·{\mathrm{acc}}_x\left(i\right)$

式中的k表示为基于惯性传感器的室内3D定位系统的采样率，为采样的时间间隔，acc_x(i)表示x轴加速度在i点的值，i点为跟时间相关的坐标点，1秒包含100个点，设初始的速度为零，那么某一时刻的速度是初始时刻t₀到T时刻的加速度的累加之和，如图6为基于误差线性的特点并结合步态速度修正前后对比图，在t_a时刻到t_b时刻的时间片段上速度上的误差为：

v_e＝Δv₂-Δv₁

Δv₁为t_a时刻速度修正前和速度修正后的速度差，Δv₂为t_b时刻速度修正前和速度修正后的速度差；

那么这段时间的内每一刻时间的速度误差斜率为：

$\tilde{e}=\frac{{\mathrm{\Δv}}_2-{\mathrm{\Δv}}_1}{t_b-t_a}$

在这一步中可以认为误差的斜率是固定的，那么在待定位人体迈步的过程中某一个时刻的速度可以表示为：

$V\left(T\right)=V(a\·k-1)+\underset{i=a\·k}{\overset{b\·k}{\Σ}}\frac{1}{k}\·{\mathrm{acc}}_x\left(i\right)-\frac{{\mathrm{\Δv}}_2-{\mathrm{\Δv}}_1}{t_b-t_a}\·\frac{i-a\·k}{k}$

当脚落地的时刻速度为零，则

V(a·k-1)＝0

那么人体正常行走的过程中速度可以表示为：

$V\left(T\right)=\left\{\begin{array}{cc}\underset{i=a_n\·k}{\overset{b_n\·k}{\Σ}}\frac{1}{k}\·{\mathrm{acc}}_x\left(i\right)-\hat{e}\·\frac{i-a_n\·k}{k}& stationary=1\\ 0& stationary=0\end{array}\right.$

式中stationary表示步态信息，stationary＝0表示脚落地的时刻，stationary＝1表示人体在迈步过程中，为每一步速度的误差斜率，a_n为一步的起始时刻，b_n为一步的结束时刻前一个采样点时刻，a为某一个时间点，修正后的速度V(T)，在经过一次积分得到距离，假设初始的距离为零，那么某一个轴的距离可以表示为：

$S\left(T\right)=\underset{i=0}{\overset{T\·K}{\Σ}}\frac{1}{k}\·V\left(i\right)$

V(i)为i坐标点在三个方向的速度值向量；

步骤5中得到从速度层面消除的累积误差的速度信息，并对速度信息进行一次积分得到距离信息，但是在平面行走时垂直方向还是存在着距离误差，需要进一步消除垂直方向上的距离误差；

首先，利用如下方法区分判断出待定位人体行走的状态(如图7为上楼下楼示意图，从加速度上观测，很难区分出是上楼还是下楼。主要可以根据每一步z轴上变化区分。但是当在水平面行走的时候，z轴上也存在着误差，虽不明显，但是会影响到行走状态的判断，为了可以准确的判断出行走状态)，正常行走的每一步可以看作一个向量那么向量为水平平面内行走的向量和z轴行走的向量之和，待定位人体在水平平面内行走时，由于z轴上的距离存在着一定的偏差，而且方向是不确定的，θ可以表示为：

$\mathrm{\θ}=arccos\frac{\stackrel{\→}{Z}}{\left|\stackrel{\→}{S}\right|}$

θ反映出了随机某一步中，z轴距离的变化，但是行走的每一步大小是不定的，那么θ的值变化范围受到人的行走个有特性的影响，对上楼和下楼的判断有影响，为了更好的区别上楼和平面内行走，或者是下楼和平面内行走，设计如下数学模型：

${\mathrm{\θ}}^{\′}=\frac{\mathrm{\θ}}{\left|\stackrel{\→}{H}\right|}$

θ′表示单位水平距离θ角变化；使用此方法对行走状态的判断有着很高的准确性，如图8为三种状态的角度对比图；

其次，借助步态信息的判断进一步消除平面移动时垂直距离方向上的误差，图9为每一步上的垂直方向的误差可以近似认为成线性，借助于行走状态判断模型和步态信息，每一步上的垂直方向的误差认为成线性，借助于行走状态判断模型和步态信息，按照每一步消除高度信息的偏差，可以消除在平面移动时的垂直方向的误差，消除误差的流程如图10所示，根据步态信息得到每一步起始位置和终止位置，根据行走状态判断模型判断出是否在平面内移动，从而执行误差修正的方法，借助于本发明的行走状态判断的模型，可以按步态消除垂直方向的累计的误差，主要针对的是平面内移动的场景，如图10为垂直方向距离误差消除对比图。；

如图11所述，在t_a时刻到t_b时刻的时间片段上待定位人体垂直运动距离的误差为：

s_e＝Δs₂-Δs₁

式中Δs₁为t_a时刻的待定位人体垂直运动误差距离，Δs₂为t_b时刻的待定位人体垂直运动误差距离；

那么这段时间的内每一刻时间的距离误差斜率为：

${\tilde{e}}_s=\frac{{\mathrm{\Δs}}_2-{\mathrm{\Δs}}_1}{t_b-t_a}$

在这一步中可以认为误差的斜率是固定的，那么在迈步的过程中某一个时刻的垂直方向距离可以表示为：

$S_z\left(T\right)=S_z(a\·k-1)+\underset{i=a\·k}{\overset{b\·k}{\Σ}}(\frac{1}{k}\·V_z(i)-{\tilde{e}}_s\·\frac{i-a\·k}{k})$

那么人体正常行走的过程中垂直方向的距离可以表示为：

$S_z\left(T\right)=\left\{\begin{array}{cc}{S}_z(a_n\·k-1)+\underset{i=a_n\·k}{\overset{b_n\·k}{\Σ}}(\frac{1}{k}\·V_z(i)-{\tilde{e}}_s\·\frac{i-a_n\·k}{k})& stationary=1\\ S_z(a_n\·k-1)& stationary=0\end{array}\right.$

式中stationary表示步态信息，stationary＝0表示脚落地的时刻，stationary＝1表示待定位人体在迈步过程中，为每一步距离的误差斜率，a_n为一步的起始时刻，b_n为一步的结束时刻前一个采样点时刻，S_z(a_n·k-1)为迈步前一刻的移动的距离值，V_z(i)为待定位人体在z轴i点时刻的速度，表示时刻值。

本发明根据数据特性设计出行走状态判断模型，主要可以区分平面移动(正常行走、慢跑)、上楼、下楼三种状态。本发明助于行走状态判断模型，设计了平面移动时高度信息误差消除策略，从而提高定位的精度。最后，设计了基于惯性传感器的室内3D定位系统。设计的思路和算法的实现建立在鞋绑式的捷联式惯性导航的系统上，并使用Unity3D绘制楼层的3D地图，在地图中映射出待定位人体的3D位置，完成室内的3D定位。

本发明使用四元数结合步态的信息解算传感器姿态，为了使四元数的计算误差更小而且在算法复杂度比较低的情况下，本发明中使用四阶龙格库塔进行四元数的更新；根据姿态的信息，将采集得到的加速度信息映射到传感器坐标系中；各个轴上的加速度分别积分求得速度，利用步态的信息修正速度的累计漂移；再利用人体在平面行走特点，消除位置上z轴的漂移。最后将得到的位置信息映射到室内的3D地图中，从而实现室内的3D定位。如图2(a)，原始的加速度数据波形，在短时间内可以看出加速度是恒定不变的，加速度的有一定的偏置，图2(b)为原始的加速度数据经过一次积分得的速度数据波形，从图中可以看出，速度的变化是近似一条直线，为定位误差的消除打下基础。

本说明书未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (3)

1.一种基于惯性传感器的室内3D定位系统，它包括惯性数据采集节点(1)、网管节点(2)和计算机(3)，其中，所述惯性数据采集节点(1)包括第一控制模块(1.1)、惯性传感器(1.2)和第一无线通信模块(1.3)，所述网管节点(2)包括第二控制模块(2.1)、串口通信模块(2.2)和第二无线通信模块(2.3)，其中，惯性传感器(1.2)的信号输出端连接第一控制模块(1.1)的信号输入端，第一控制模块(1.1)的信号输出端连接第一无线通信模块(1.3)的信号输入端，第二无线通信模块(2.3)的信号输出端连接第二控制模块(2.1)的信号输入端，第二控制模块(2.1)的信号输出端通过串口通信模块(2.2)连接计算机(3)的数据通信端，所述第一无线通信模块(1.3)和第二无线通信模块(2.3)之间无线通信连接。

2.一种利用权利要求1所述基于惯性传感器的室内3D定位系统进行3D定位的方法，其特征在于，它包括如下步骤：

步骤1：将惯性数据采集节点(1)安装在待定位人体上，惯性数据采集节点(1)中的惯性传感器(1.2)采集待定位人体实时的加速度数据和角速度数据，并将得到的加速度数据和角速度数据传输给计算机(3)；

步骤2：计算机(3)根据所述加速度数据计算得到待定位人体初始姿态信息，并根据所述角速度数据计算得到待定位人体的运动状态信息，对待定位人体的姿态信息进行基于四元数的姿态初始化，四元数表示为:

Q(q₀,q₁,q₂,q₃)＝q₀+q₁i+q₂j+q₃k

其中，i、j、k为三个虚数单位，q₁表示待定位人体所处空间X轴正向向待定位人体所处空间Y轴正向的旋转值，q₂表示待定位人体所处空间Z轴正向向待定位人体所处空间X轴正向的旋转值，q₃表示待定位人体所处空间Y轴正向向待定位人体所处空间Z轴正向的旋转值，旋转值得大小通过三个轴的角速度值积分得到，利用采集得到的加速度数据和角速度数据计算姿态信息和四元数，设参考坐标系的初始的四元数为q₀＝[1000]，设参考坐标系的重力加速度向量a_R＝[001]^T；

又因为四元数的旋转矩阵为：

$C_b^R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_3{q}_0\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

那么由参考坐标系O到惯性传感器坐标O′的重力加速度可以表示为：

$g_b=C_b^R\·a_R$

$g_b=\left(\begin{array}{c}{g}_{bx}\\ g_{by}\\ g_{bz}\end{array}\right)=\left(\begin{array}{c}2\left(q_1{q}_3-q_0{q}_2\right)\\ 2\left(q_2{q}_3+q_0{q}_1\right)\\ q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)$

其中，g_bx、g_by和g_bz分别为惯性传感器坐标O′的重力加速度在三个方向上的值；

实际采样得到的加速度向量可以表示为g_R＝[a_xa_ya_z]^T，实际采样得到加速度向量做归一化处理：

$g_R^{\′}=\frac{g_R}{\sqrt{a_x^2+a_y^2+a_z^2}}$

算出a_R向量与g′_R(R＝1,2,3,…,n)向量的向量积v_n(n＝1,2,3,…,n)：

v_n＝a_R×g′_R

v_n表示参考坐标系的重力加速度与惯性传感器所在坐标系，每个轴角度的差值，也可以理解为经过采样率Δt的时间到R时刻的角度的变化，再解算出惯性传感器所在坐标系的四元数矩阵即姿态，用于下一时刻姿态的估算，假设四元数解算的过程为P，那么解算得到的四元数可以表示为：

Q_n(q_n0+q_n1+q_n2+q_n3)＝v_n·P

将计算得到的四元数Q_n带入到重力加速度的表达式中计算出重力加速度的向量g_b，再次与真实采集到重力加速度的归一化向量算出向量积，完成初始姿态的计算；

步骤3：在待定位人体移动的过程中需要消除陀螺仪的测量误差，消除陀螺仪测量误差的具体方法为，通过在姿态的四元数Q_n计算出待定位人体移动时相对于参考坐标系O的实时重力向量g_b，实测加速度的值进行归一化处理变为向量g′_R，计算g_b、g′_R的向量积为角速度漂移误差e＝(e_xe_ye_z)，用来修正陀螺仪的角速度的数据，向量积向量e仍旧位于待定位人体坐标系上，向量积向量e的大小与惯性传感器(1.2)中陀螺仪的误差成正比，误差越大则向量积e的越大，可以用来纠正陀螺仪的数据，由于角速度数据的积分是对惯性传感器(1.2)所在的自身坐标系积分，所以误差量包含在对角速度的积分当中，所以有：

$e=g_b\×g_R^{\′}=\left|\begin{array}{ccc}x& y& z\\ g_{Rx}^{\′}& g_{Ry}^{\′}& g_{Rz}^{\′}\\ g_{bx}& g_{by}& g_{bz}\end{array}\right|$

式中，g_bx、g_by和g_bz分别为惯性传感器坐标O′的重力加速度在三个方向上的值，g′_Rx、g′_Ry、g′_Rz分别为向量g′_R在三个方向上的值，x、y、z是惯性传感器(1.2)所在坐标系的单位向量，经化简可以得到：

$e=\left(\begin{array}{c}{e}_x\\ e_y\\ e_z\end{array}\right)=\left(\begin{array}{c}{g}_{Ry}^{\′}\·g_{bz}-g_{Rz}^{\′}\·g_{by}\\ g_{Rz}^{\′}\·g_{by}-g_{Rx}^{\′}\·g_{bz}\\ g_{Rx}^{\′}\·g_{by}-g_{Ry}^{\′}\·g_{bx}\end{array}\right)$

综合上述，惯性传感器(1.2)中陀螺仪的角速度可以表示为：

gyro′＝gyro-Kp·e·stationary

式中，gyro是陀螺仪的角速度向量，Kp为惯性传感器的室内3D定位系统的增益误差系数，stationary为人体的步态信号函数，即待定位人体脚落地时刻和迈步时刻组成连续信号；

步骤4:四元数的更新，步骤3根据步态信息消除陀螺仪的测量误差是为了姿态的解算做准备，四元数的更新本质就是求解四元数的微分方程，为了提高计算的精度同时考虑到算法的复杂度，惯性传感器的室内3D定位系统采用四阶龙格库塔求解四元数的微分方程，对一阶的龙格库塔公式进行推导、求解，得出四阶的龙格库塔公式。根据四元数的微分方程定义:

$Q^{\′}=\frac{1}{2}w\left(t\right)*Q\left(t\right)$

其中，w(t)代表角速度的变化，Q(t)表示t时刻的四元数值即待定位人体的姿态信息；

假设t₀时刻

Q(t₀)＝Q₀

Q₀表示t₀时刻的姿态信息即初始时刻；

对应的微分方程为：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\left(t\right)\\ {\stackrel{\·}{q}}_1\left(t\right)\\ {\stackrel{\·}{q}}_2\left(t\right)\\ {\stackrel{\·}{q}}_3\left(t\right)\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -w_x& -w_y& -w_z\\ w_x& 0& w_z& -w_y\\ w_y& -w_z& 0& w_x\\ w_z& w_y& -w_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\left(t\right)\\ q_1\left(t\right)\\ q_2\left(t\right)\\ q_3\left(t\right)\end{array}\right]$

q_o(t)、q₁(t)、q₂(t)和q₃(t)分别为t时刻，四元数的值；

采用四阶的龙格库塔求解四元数的微分方程如下：

$Q_{n+1}=Q_n+\frac{h}{6}(k_1+2k_2+3k_3+k_4)$

式中，Q_n第N点的四元数信息，Q_n+1第N+1点的四元数信息，即待定位人体移动时的姿态信息，

k₁＝f(w_n,Q_n)

$k_2=f(w_n+\frac{h}{2},Q_n+\frac{h}{2}{k}_1)$

$k_3=f(w_n+\frac{h}{2},Q_n+\frac{h}{2}{k}_2)$

k₄＝f(w_n+h,Q_n+hk₃)

$f(w_n,Q_n)=\frac{1}{2}w\left(t\right)*Q\left(t\right)$

其中w_x,w_y,w_z为惯性传感器(1.2)输出的实际角速度，h为惯性传感器(1.2)实际的采集间隔时间；

在使用实时计算四元数更新时，会逐渐失去四元数规范化特性，必须对四元数做规范化处理，规范化方程为：

$q_i=\frac{q_i}{\sqrt{{\hat{q}}_0^2+{\hat{q}}_1^2+{\hat{q}}_2^2+{\hat{q}}_3^2}}$

其中i＝1,2,3，为四元数更新所得值，四元数更新所得值通过公式计算得到，Q_n+1包括

步骤5:基于步态的累积误差消除方法实现，经过步骤4得到待定位人体移动时的姿态信息，通过姿态映射采集回的加速度信息，计算待定位人体实时姿态下的3个轴上的加速度数据，加速度数据经过一次积分得到移动的距离信息，利用待定位人体移动时的起点坐标和待定位人体移动时的距离信息根据里程计推算的方式实现待定位人体的室内3D定位。

3.根据权利要求2所述的3D定位方法，其特征在于：所述步骤5中，得到的人体移动时的距离信息存在着距离上的估算误差，人体的步态信号函数不仅可以当作角速度数据的参考量，用于陀螺仪误差消除依据，同时也是加速度累计误差的参考点，根据人体行走的特点，当脚落地的时候，此时的速度接近为零，加速度数据经过两次积分得到距离，根据惯性传感器(1.2)的定位分析，加速度的误差经过一次积分得到速度，即加速度的误差传播到速度上，而且在速度上的误差体现为速度近似成线性增长，连续时间下待定位人体的移动速度可以表示为：

$v\left(T\right)=v\left(t_0\right)+{\∫}_0^{T}acc\left(t\right)dt$

式中，v(t₀)为带定位人体的初始速度状态；

在离散时间下待定位人体的移动速度可以表示为：

$v\left(T\right)=v\left(0\right)+\underset{i=0}{\overset{T\·k}{\Σ}}\frac{1}{k}\·{\mathrm{acc}}_x\left(i\right)$

式中的k表示为基于惯性传感器的室内3D定位系统的采样率，为采样的时间间隔，acc_x(i)表示x轴加速度在i点的值，i点为跟时间相关的坐标点，1秒包含100个点，设初始的速度为零，那么某一时刻的速度是初始时刻t₀到T时刻的加速度的累加之和，在t_a时刻到t_b时刻的时间片段上速度上的误差为：

v_e＝Δv₂-Δv₁

Δv₁为t_a时刻速度修正前和速度修正后的速度差，Δv₂为t_b时刻速度修正前和速度修正后的速度差；

那么这段时间的内每一刻时间的速度误差斜率为：

$\tilde{e}=\frac{{\mathrm{\Δv}}_2-{\mathrm{\Δv}}_1}{t_b-t_a}$

在这一步中可以认为误差的斜率是固定的，那么在待定位人体迈步的过程中某一个时刻的速度可以表示为：

$V\left(T\right)=V(a\·k-1)+\underset{i=a\·k}{\overset{b\·k}{\Σ}}\frac{1}{k}\·{\mathrm{acc}}_x\left(i\right)-\frac{{\mathrm{\Δv}}_2-{\mathrm{\Δv}}_1}{t_b-t_a}\·\frac{i-a\·k}{k}$

当脚落地的时刻速度为零，则

V(a·k-1)＝0

那么人体正常行走的过程中速度可以表示为：

$V\left(T\right)=\left\{\begin{array}{cc}\underset{i=a_n\·k}{\overset{b_n\·k}{\mathrm{\Σ}}}\frac{1}{k}\·{\mathrm{acc}}_x\left(i\right)-\hat{e}\·\frac{i-a_n\·k}{k}& stationary=1\\ 0& stationary=0\end{array}\right.$

式中stationary表示步态信息，stationary＝0表示脚落地的时刻，stationary＝1表示人体在迈步过程中，为每一步速度的误差斜率，a_n为一步的起始时刻，b_n为一步的结束时刻前一个采样点时刻，a为某一个时间点，修正后的速度V(T)，在经过一次积分得到距离，假设初始的距离为零，那么某一个轴的距离可以表示为：

$S\left(T\right)=\underset{i=0}{\overset{T\·K}{\Σ}}\frac{1}{k}\·V\left(i\right)$

V(i)为i坐标点在三个方向的速度值向量；

步骤5中得到从速度层面消除的累积误差的速度信息，并对速度信息进行一次积分得到距离信息，但是在平面行走时垂直方向还是存在着距离误差，需要进一步消除垂直方向上的距离误差；

首先，利用如下方法区分判断出待定位人体行走的状态，正常行走的每一步可以看作一个向量那么向量为水平平面内行走的向量和z轴行走的向量之和，待定位人体在水平平面内行走时，由于z轴上的距离存在着一定的偏差，而且方向是不确定的，θ可以表示为：

$\mathrm{\θ}=\arccos \frac{\stackrel{\→}{Z}}{\left|\stackrel{\→}{S}\right|}$

θ反映出了随机某一步中，z轴距离的变化，但是行走的每一步大小是不定的，那么θ的值变化范围受到人的行走个有特性的影响，对上楼和下楼的判断有影响，为了更好的区别上楼和平面内行走，或者是下楼和平面内行走，设计如下数学模型：

${\mathrm{\θ}}^{\′}=\frac{\mathrm{\θ}}{\left|\stackrel{\→}{H}\right|}$

θ′表示单位水平距离θ角变化；

其次，借助步态信息的判断进一步消除平面移动时垂直距离方向上的误差，每一步上的垂直方向的误差认为成线性，借助于行走状态判断模型和步态信息，按照每一步消除高度信息的偏差，可以消除在平面移动时的垂直方向的误差，根据步态信息得到每一步起始位置和终止位置，根据行走状态判断模型判断出是否在平面内移动，从而执行误差修正的方法；

在t_a时刻到t_b时刻的时间片段上待定位人体垂直运动距离的误差为：

s_e＝Δs₂-Δs₁

式中Δs₁为t_a时刻的待定位人体垂直运动误差距离，Δs₂为t_b时刻的待定位人体垂直运动误差距离；

那么这段时间的内每一刻时间的距离误差斜率为：

${\tilde{e}}_s=\frac{{\mathrm{\Δs}}_2-{\mathrm{\Δs}}_1}{t_b-t_a}$

在这一步中可以认为误差的斜率是固定的，那么在迈步的过程中某一个时刻的垂直方向距离可以表示为：

$S_z\left(T\right)=S_z(a\·k-1)+\underset{i=a\·k}{\overset{b\·k}{\Σ}}(\frac{1}{k}\·V_z(i)-{\tilde{e}}_s\·\frac{i-a\·k}{k})$

那么人体正常行走的过程中垂直方向的距离可以表示为：

$S_z\left(T\right)=\left\{\begin{array}{cc}{S}_z\left(a_n\·k-1\right)+\underset{i=a_n\·k}{\overset{b_n\·k}{\mathrm{\Σ}}}\left(\frac{1}{k}\·V_z\left(i\right)-{\tilde{e}}_s\·\frac{i-a_n\·k}{k}\right)& stationary=1\\ S_z\left(a_n\·k-1\right)& stationary=0\end{array}\right.$

式中stationary表示步态信息，stationary＝0表示脚落地的时刻，stationary＝1表示待定位人体在迈步过程中，为每一步距离的误差斜率，a_n为一步的起始时刻，b_n为一步的结束时刻前一个采样点时刻，S_z(a_n·k-1)为迈步前一刻的移动的距离值，V_z(i)为待定位人体在z轴i点时刻的速度，表示时刻值。