1.一种人员高度自主定位算法,其特征在于,仅依靠MEMS加速度计和陀螺仪进行定位计算,包括以下步骤:
1)、对惯性系统的MEMS加速度计和MEMS陀螺仪进行数据处理,包括:安装误差、零偏、刻度因子误差校准,温度补偿、以及低通滤波;并将惯性系统安装在行人腰部,获得行人的运动加速度;
2)、对X,Z轴加速度计进行峰值检测,分别表示为Axmax、Azmin,由于人员行走时身体摆动,导致加速度计在一个周期内出现了两个波峰,通过设定一个峰值检测间隔时间Tc,即当检测到第一个峰值以后,间隔时间Tc后再进行下一次的峰值检测,通过这种时间阈值法滤除加速度计次波峰的干扰,根据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步的航向角和俯仰角,记为yawi,pitchi;根据步长、航向角、俯仰角获得每一步高度值h0,具体如式(4)所示:h0=DS_L*sin(yawi)*tan(pitchi) (4)
505.对人员进行步态检测;
506.步骤504、505完成以后,检测人员的上下楼状态,如果人员在上楼状态,则进行高度累加;在下楼状态,则进行高度累减;在平走状态,则高度不变,具体如式(5)所示:其中,H2为当前时刻解算的高度值,H1为前一步解算的高度值;
所述步骤4)通过姿态角检测上下楼过程中的转弯点,具体包括:人员上下楼时,在楼梯间会进行一次拐弯,这个弯度通常4~5步走完,利用航向角差值来判断楼层间的转弯点,具体如式(6)所示:其中,yawi为第i步的航向角,i=1、2、3、4、5;α为设定的转弯判定阈值,该阈值取值为
100°;
所述步骤4)在转弯点将解算高度修正到半层楼高的整数倍,修正算法如式(7)所示:k=‑3,‑2,‑1,0,1,2,3……,H2当前时刻解算的高度值,β为高度修正误差阈值,hc建筑半层楼高;
所述步骤2)检测X,Z轴加速度计峰值特征,根据加速度计的峰值特征,对人员的上下楼或者平走状态进行判定,具体包括:
401.对加速度数据进行峰值检测,X轴加速度计的波峰值,记为Axmax,Z轴加速度计的波谷值,记为Azmin;
402.人员平走2s,对X、Z轴加速度计数据进行峰值检测,获得X、Z轴加速度计的峰值均值,分别记为av_x,av_z;
403.由于人员行走时身体摆动,导致加速度计在一个周期内出现了两个波峰,通过设定一个峰值检测间隔时间Tc,即当检测到第一个峰值以后,间隔时间Tc后再进行下一次的峰值检测,通过这种时间阈值法滤除加速度计次波峰的干扰;
404.设定上下楼的判定阈值,分别记为D1,D2,D3;
405.人员上下楼以及平走判定的条件如式1所示:
2.根据权利要求1所述的一种人员高度自主定位算法,其特征在于,所述步骤1)对惯性系统的MEMS加速计和MEMS陀螺仪进行包括误差校准、温度补偿、低通滤波在内的数据处理步骤,具体包括:
201.选定一个集成三轴MEMS加速度计和陀螺仪的惯性系统;
202.选定二自由度及以上的旋转控制系统;
203.对步骤102的旋转控制系统的主轴和俯仰轴进行归零操作;
204.将惯性系统垂直放置在旋转控制系统上,对加速度计和陀螺仪进行校准工作,校准参数包括:安装误差、刻度因子误差、零位偏移;
205.选定温箱,将步骤104校准处理后的数据进行温度补偿;
206.将步骤105温度补偿以后的数据进行低通滤波处理。
3.根据权利要求2所述的一种人员高度自主定位算法,其特征在于,所述步骤2)将惯性系统安装在被测人员身上,获得行人的运动加速度,具体包括:
301.人员将惯性系统置于腰部,采集误差校准、温度补偿、低通滤波后的加速度计数据,人员静止1秒,采集X、Y、Z轴加速度计在当地的重力加速度分量,并求均值,记为g_x,g_y,g_z;
302.将人员行走过程中的X、Z轴加速度计数据减去静止状态的重力加速度均值,得到人员的运动加速度数据分别记为Ax,Az,并将此运动加速度数据作为研究参考量。