[发明专利]一种基于层高模型的楼层自主识别方法

权利要求书

1.一种基于层高模型的楼层自主识别方法，其特征在于，包括如下方法步骤：

步骤一、在鞋跟位置嵌入MEMS-IMU传感器；

步骤二、采集MEMS-IMU传感器的加速度计和陀螺数据，结合式(1)进行捷联惯性解算，获取行人的姿态，速度和位置信息；

其中，上标n和b分别表示导航坐标系和载体坐标系；Vⁿ和分别表示导航坐标系下的三维速度及其微分；表示导航坐标系下三维位置的微分；和分别表示由b系到n系转换的姿态矩阵及其微分；Ω^b表示由陀螺仪输出角速度构成的反对称矩阵；f^b表示载体坐标系下的比力；gⁿ表示地球重力场矢量；

步骤三、基于加速度计和陀螺仪的输出，进行零速更新检测，即对行走过程中传感器固定脚进行周期性触地检测；

步骤四、基于所述零速更新检测，步态周期序号记作m，将第m个步态周期中的静止态记为Stan_m，在静止态Stan_m内，脚贴地，实际行走速度为零，而基于步骤二解算的三维速度V不为零，实际行走速度与解算的三维速度V之差，即为静止态的速度误差，如下所示：

dV(Stan_m,i)＝[0 0 0] (2)

其中，Stan_m,i是步周期为m时，所处静止态的第i个采样，i取从1到步周期静止态的采样数目last之间的整数，对不同的步周期，last取值不相同；

步骤五、将步骤四中的速度误差作为观测量，采用扩展卡尔曼滤波对步骤二中所求的定位结果三维速度、三维位置、三维姿态进行修正，扩展卡尔曼滤波中的15维误差状态向量定义如下：

其中，δPⁿ、δVⁿ、δφⁿ、ε^b和分别代表为三维位置误差向量、三维速度误差向量、三维姿态误差向量、三维陀螺漂移和三维加速度偏置；

步骤六、第m个步态周期中，行人的航向记作yaw_m，

yaw_m＝-atan2(C₁,C₂) (4)

其中，atan2表示取值范围为[-pi,pi]的反正切函数，其中pi为圆周率，取值3.14，C₁,C₂分别是Stan_m,last采样时刻所对应姿态矩阵中的元素和

步骤七、根据每步的高度变化，判断行人的运动状态为上楼梯或下楼梯或平面行走，依次用+1，-1和0表示，如下所示：

其中，σ_th表示人员运动状态的判断门限值；Cls_m为第m个步周期的人员运动状态类型，δP₃(Stan_m,last)为相邻两个步周期静止态的高度差；

步骤八、行人在Stan_m时所处的高度h_m通过如下方式计算：

其中，K为每个台阶的高度，需要通过激光测距仪或图纸资料来获取；

步骤九、人员所处的楼层用flr表示，行人变换的楼层总数记为flr_total，初始值都设为1；楼层高度记作flr_h(flr)，floor_h(1)取0；当前楼梯段的数量记作str_sec，初值为0，楼间包含两至三个楼梯段；楼梯段的方向记作str_sec_dir(str_sec)；

步骤十、如果Cls_m+Cls_m-1＝1，并且h_m-flr_h(flr_total)γ1，则：

str_sec＝str_sec+1 (7)

str_sec_dir(str_sec)＝yaw_m (8)

其中，γ1为大于零的高度阈值，m为步周期编号，yaw为行人的航向；

步骤十一、如果Cls_m+Cls_m-1＝1，h_m-flr_h(flr_total)γ2，并且α*pi/180|str_sec_dir(str_sec)-str_sec_dir(str_sec-1)|β*pi/180时，则：

flr＝flr+1 (9)

flr_total＝flr_total+1 (10)

flr_h(flr_total)＝h_m (11)

其中，pi取3.14；γ2为大于零的高度阈值；α和β均为大于零的角度阈值。