CN106767790B - 人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的方法 - Google Patents

Abstract

本发明一种人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的方法，针对将传感器放置于人体上半身的情况下，提出运动模型与卡尔曼滤波相结合的方法，通过运动模型方程映射出加速度或者速度的竖直分量与水平分量之间的关系，卡尔曼滤波器根据此关系将对竖直分量的修正量反馈到水平分量中，实现对水平分量的间接修正，最终得到修正后的竖直方向速度和水平方向速度；本发明利用运动模型体现的运动特征有效地抑制传感器的累积误差，使得在长时间的室内定位中，依然能够获得相对准确的行人位移信息。

Description

人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的 方法

技术领域

本发明属于行人室内定位领域，通过惯性传感器在行人室内定位中准确估计步长，尤其是一种人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的方法。

背景技术

近年来，基于可穿戴传感器设备的室内定位技术的研究已经越来越多，由于惯性传感器本身受到材料和工艺等因素的限制，无法做到百分百的准确，并且传感器设备的测量值也存在一定的误差，在定位计算过程中，往往需要对传感器测量数据在时间上进行积分，因此，即使是较小的传感器测量误差，在通过长时间的积分累积之后都将对定位结果造成较大的影响。在这种情况下，如果没有一个较好的手段对传感器的输出误差进行修正，则我们就无法使用传感器数据进行相对准确的室内定位。

在大部分研究中，主要是将可穿戴设备置于人体的下半身，例如脚踝处，小腿处等，这是因为在人体行走过程中，下半身往往最能体现出行走的特征，但是，如果从实际应用考虑，将这些惯性设备置于脚部或许并不是最好的选择，一方面，其佩戴起来并不方面，而且很有可能会影响美观，另一方面，由于脚部的运动轨迹较大，并且时常与地面或其他物体存在碰撞，这就很有可能对测量设备造成磨损。因此，如果可以选择该传感器设备的佩戴位置，并且不影响定位精度，人们会更愿意将其佩戴于上半身，例如胸前，腰部位置等。

发明内容

为了解决上述问题，本发明的目的在于提供一种人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的方法，在将传感器设备安置在人体上半身的前提下，仍然能够获得准确的行人位移信息。

本发明一种人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的方法，包括如下步骤：

步骤1、用户佩戴惯性传感器设备在室内环境中行走，通过惯性传感器设备采集用户行走过程中的三轴加速度数据；

步骤2、将采集到的三轴加速度数据分解为竖直方向加速度a_z与水平方向加速度a_x，并计算得到相应的竖直方向速度v_z和水平方向速度v_x；

步骤3、使用零速检测算法对步骤2分解出的竖直方向加速度a_z进行检测，标记出人体行走过程中的各个零速点；

步骤4、使用卡尔曼滤波器对步骤3标记的各零速点的竖直分量x_z进行修正，其中x为加速度或者速度：

将运动模型方程f(x_z)代入到卡尔曼滤波器中，该运动模型方程映射出加速度或者速度的竖直分量与水平分量之间的关系，卡尔曼滤波器根据此关系将对竖直分量的修正量反馈到水平分量中，实现对水平分量的间接修正，最终得到修正后的竖直方向速度v_nz和水平方向速度v_nx；

步骤5、计算每一步的步长：

(1)使用修正后的竖直方向速度v_nz计算每一步水平位移S₁(k)：

S₁(k)＝f₁(v_nz)

(2)使用修正的水平方向速度v_nx计算每一步水平位移S₂(k)：

S₂(k)＝f₂(v_nx)

(3)融合两种方法计算出的步长，得到最终的步长结果：

S(k)＝f₃(S₁(k),S₂(k))

步骤6、利用陀螺仪测得的角速度数据计算得到每个时刻点k的航向角Yaw(k)，根据步骤5得到的每一步步长S(k)，将上一步的坐标位置作为下一步的起始点，便能估计出最终的行人定位结果(x,y)：

本发明针对将传感器放置于人体上半身的情况下，提出运动模型与卡尔曼滤波相结合的方法，通过运动模型方程映射出加速度或者速度的竖直分量与水平分量之间的关系，卡尔曼滤波器根据此关系将对竖直分量的修正量反馈到水平分量中，实现对水平分量的间接修正，最终得到修正后的竖直方向速度和水平方向速度；本发明利用运动模型体现的运动特征有效地抑制传感器的累积误差，使得在长时间的室内定位中，依然能够获得相对准确的行人位移信息。

附图说明

图1是本发明的系统框图；

图2是人行走时质心运动的轨迹；

图3是倒立摆模型中水平速度与竖直速度关系示意图；

图4是航向角与坐标的关系示意图。

以下结合附图和实施例对本发明作进一步详述。

具体实施方式

如图1所示，本发明一种人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的方法，包括如下步骤：

步骤1、用户佩戴惯性传感器设备在室内环境中行走，通过惯性传感器设备采集用户行走过程中的三轴加速度数据；

步骤2、将采集到的三轴加速度数据分解为竖直方向(重力加速度方向)加速度a_z与水平方向加速度a_x，并计算得到相应的竖直方向速度v_z和水平方向速度v_x；

步骤3、由于人在行走过程中，当人体质心处于最高点时刻(脚掌与地面平行)，人体竖直方向的加速度值达到向下的最大值，当人体质心处于最低点时刻(脚跟着地)，人体竖直方向的加速度值达到向上的最大值，这两个时刻人体竖直方向上的速度都为0；根据这一特点，使用零速检测算法对步骤2分解出的竖直方向加速度a_z进行检测，标记出人体行走过程中的各个零速点；

步骤4、若使用ZUPT算法对竖直方向速度v_z进行一定的修正，却无法对水平方向速度v_x进行修正，从而无法采用传统的牛顿运动学定律直接对行走的水平位移进行估计。在人体行走过程中，人体质心实际上是在循环进行一种是近似圆弧的运动，如图2所示。根据这一特点，本实施方式以构建如图3所示的倒立摆运动模型为例，对于圆弧上的每一个点，其水平方向速度v_x与竖直方向速度v_z的关系为v_x＝v_zcotθ，θ为0时，v_x即为和速度，将这一运动关系作为运动模型方程f(v_z)＝v_zcotθ代入到卡尔曼滤波器中，对竖直方向速度v_z进行修正，利用修正后的竖直方向速度对水平方向速度做出修正，从而消除水平方向速度的累积误差。即，将运动模型方程f(v_z)＝v_zcotθ代入到卡尔曼滤波器中，通过在步骤3标记的零速点处对竖直方向速度v_z进行修正，该运动模型方程映射出加速度或者速度的竖直分量与水平分量之间的关系，卡尔曼滤波器根据此关系将对竖直分量的修正量反馈到水平分量中，实现对水平分量的间接修正，最终得到修正后的竖直方向速度v_nz和水平方向速度v_nx；

本实施方式使用以下方法计算当前时刻倒立摆运动模型方程中的θ值：

其中，l代表腿长，h代表当前时刻人体的竖直方向位移，可以通过对竖直方向加速度az二次积分得到；

本实施方式采用的卡尔曼滤波器的最优估计方程如下：

V(k|k)＝V(k|k-1)+Kg(k)*(Z(k)-H*V(k|k-1))

其中，V(k|k)为卡尔曼滤波估计出的最优水平方向速度，V(k|k-1)为对水平方向加速度ax积分得到的水平方向速度，Kg(k)为当前时刻的卡尔曼滤波增益，运动模型方程f(v_z)＝v_zcotθ作为测量值Z(k)传入，H为测量系统的参数；

步骤5、计算每一步的步长：

(1)使用修正后的竖直方向速度v_nz计算每一步水平位移S₁(k)：

其中，l为行人的腿长；

(2)使用修正的水平方向速度v_nx计算每一步水平位移S₂(k)：

(3)融合两种方法计算出的步长，得到最终的步长结果：

S(k)＝f₃(S₁(k),S₂(k))＝k₁*S₁(k)+k₂*S₂(k)

其中，k₁和k₂分别为两种步长结果各自的权重；

步骤6、利用陀螺仪测得的角速度数据计算得到每个时刻点k的航向角Yaw(k)，根据步骤5得到的每一步步长S(k)，将上一步的坐标位置作为下一步的起始点，如图4所示，便能估计出最终的行人定位结果(x,y)：

以上所述，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (1)

1.一种人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的方法，其特征在于包括如下步骤：

步骤1、用户佩戴惯性传感器设备在室内环境中行走，通过惯性传感器设备采集用户行走过程中的三轴加速度数据；

步骤2、将采集到的三轴加速度数据分解为竖直方向加速度a_z与水平方向加速度a_x，并计算得到相应的竖直方向速度v_z和水平方向速度v_x；

步骤3、使用零速检测算法对步骤2分解出的竖直方向加速度a_z进行检测，标记出人体行走过程中的各个零速点；

步骤4、使用卡尔曼滤波器对步骤3标记的各零速点的竖直分量v_z进行修正：

在人体行走过程中，人体质心是在循环进行一种是近似圆弧的运动，根据这一特点，构建倒立摆运动模型，对于圆弧上的每一个点，其水平方向速度v_x与竖直方向速度v_z的关系为v_x＝v_zcotθ，θ为0时，v_x即为和速度，将运动模型方程f(v_z)＝v_zcotθ代入到卡尔曼滤波器中，通过在步骤3标记的零速点处对竖直方向速度v_z进行修正，该运动模型方程反映了速度的竖直分量与水平分量之间的关系，卡尔曼滤波器根据此关系将对竖直分量的修正量反馈到水平分量中，实现对水平分量的间接修正，最终得到修正后的竖直方向速度v_nz和水平方向速度v_nx；

计算当前时刻倒立摆运动模型方程中的θ值：

其中，l代表腿长，h代表当前时刻人体的竖直方向位移，可以通过对竖直方向加速度a_z二次积分得到；

采用的卡尔曼滤波器的最优估计方程如下：

V(k|k)＝V(k|k-1)+Kg(k)*(Z(k)-H*V(k|k-1))

其中，V(k|k)为卡尔曼滤波估计出的最优水平方向速度，V(k|k-1)为对水平方向加速度a_x积分得到的水平方向速度，Kg(k)为当前时刻k的卡尔曼滤波增益，运动模型方程f(v_z)＝v_zcotθ作为测量值Z(k)传入，H为测量系统的参数；

步骤5、计算每一步的步长：

(1)使用修正后的竖直方向速度v_nz计算每一步水平位移S₁(k)：

其中，l为行人的腿长；

(2)使用修正的水平方向速度v_nx计算每一步水平位移S₂(k)：

(3)融合两种方法计算出的步长，得到最终的步长结果：

S(k)＝f₃(S₁(k),S₂(k))＝k₁*S₁(k)+k₂*S₂(k)

其中，k₁和k₂分别为两种步长结果各自的权重；

步骤6、利用陀螺仪测得的角速度数据计算得到每个时刻点k的航向角Yaw(k)，根据步骤5得到的每一步步长S(k)，将上一步的坐标位置作为下一步的起始点，便能估计出最终的行人定位结果(x,y)：