[发明专利]一种人员高度自主定位算法

权利要求书

1.一种人员高度自主定位算法，其特征在于，仅依靠MEMS加速度计和陀螺仪进行定位计算，包括以下步骤：

1)、对惯性系统的MEMS加速度计和MEMS陀螺仪进行数据处理，包括：安装误差、零偏、刻度因子误差校准，温度补偿、以及低通滤波；并将惯性系统安装在行人腰部，获得行人的运动加速度；

2)、对X，Z轴加速度计进行峰值检测，分别表示为Ax_max、Az_min，由于人员行走时身体摆动，导致加速度计在一个周期内出现了两个波峰，通过设定一个峰值检测间隔时间T_c，即当检测到第一个峰值以后，间隔时间T_c后再进行下一次的峰值检测，通过这种时间阈值法滤除加速度计次波峰的干扰，根据X，Z轴加速度计的峰值特征，对人员的上下楼或者平走状态进行判定；

3)、检测人员步态，通过加速度计瞬时零位捕获和陀螺仪三维动态融合算法计算上下楼时的步高，然后进行高度解算；

4)、通过姿态角检测上下楼过程中的转弯点，在转弯点将解算高度修正到半层楼高的整数倍，得到人员位置高度；

所述步骤3)检测人员步态，通过加速度计瞬时零位捕获和陀螺仪三维动态融合算法计算上下楼时的步高，然后进行高度解算，具体包括：

501.捕获加速度计瞬时零位，采集X、Y、Z轴加速度计在当地的重力加速度分量，并求均值，记为g_x，g_y,g_z，读取三轴加速度计值，分别记为Acc_x,Acc_y,Acc_z，并求取模值Acc_norm，具体如下式所示：

采集X、Y、Z轴加速度计在当地的重力加速度分量，并求均值，记为g_x，g_y,g_z

502.对Acc_norm进行峰值检测，波谷值记为Acc_min，波峰值记为Acc_max；

503.计算人员步长DS_L，如下式所示：

504.根据陀螺仪三维动态融合算法得到人员的三维姿态角，读取第i步的航向角和俯仰角，记为yaw_i，pitch_i；根据步长、航向角、俯仰角获得每一步高度值h₀，具体如式(4)所示：

h₀＝DS_L*sin(yaw_i)*tan(pitch_i) (4)

505.对人员进行步态检测；

506.步骤504、505完成以后，检测人员的上下楼状态，如果人员在上楼状态，则进行高度累加；在下楼状态，则进行高度累减；在平走状态，则高度不变，具体如式(5)所示：

其中，H₂为当前时刻解算的高度值，H₁为前一步解算的高度值；

所述步骤4)通过姿态角检测上下楼过程中的转弯点，具体包括：人员上下楼时，在楼梯间会进行一次拐弯，这个弯度通常4～5步走完，利用航向角差值来判断楼层间的转弯点，具体如式(6)所示：

其中，yaw_i为第i步的航向角，i＝1、2、3、4、5；α为设定的转弯判定阈值，该阈值取值为100°；

所述步骤4)在转弯点将解算高度修正到半层楼高的整数倍，修正算法如式(7)所示：

k＝-3，-2，-1，0，1，2,3……，H₂当前时刻解算的高度值，β为高度修正误差阈值，h_c建筑半层楼高；

所述步骤2)检测X，Z轴加速度计峰值特征，根据加速度计的峰值特征，对人员的上下楼或者平走状态进行判定，具体包括：

401.对加速度数据进行峰值检测，X轴加速度计的波峰值，记为Ax_max，Z轴加速度计的波谷值，记为Az_min；

402.人员平走2s，对X、Z轴加速度计数据进行峰值检测，获得X、Z轴加速度计的峰值均值，分别记为av_x，av_z；

403.由于人员行走时身体摆动，导致加速度计在一个周期内出现了两个波峰，通过设定一个峰值检测间隔时间T_c，即当检测到第一个峰值以后，间隔时间T_c后再进行下一次的峰值检测，通过这种时间阈值法滤除加速度计次波峰的干扰；

404.设定上下楼的判定阈值，分别记为D₁,D₂,D₃；

405.人员上下楼以及平走判定的条件如式1所示：