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

摘要

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

主权项

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)：