[发明专利]一种基于改进式HDE的室内行人导航航向修正方法及系统

权利要求书

1.一种基于改进式HDE的室内行人导航航向修正方法，至少包括下列步骤：

S1,结合典型建筑结构定义主方向，进行运动类型分类；

S2,扩展卡尔曼滤波；

所述S1包括：S11，结合典型建筑结构定义主方向，本步骤中，基于启发式偏移消除算法为中心，设定8个主方向，主方向间的角度间隔Δ为45°；以及

S12，运动类型分类，本步骤中，根据行人行走的方向，分为三种运动类型：非直线运动、非主方向的直线运动以及沿主方向的直线运动；

所述运动类型分类包括

S121,行人航向判定；

S122,陀螺仪角速率判定；

S123,行人位置判定；以及

S124,主方向判定；

所述S121中，行人航向判定的方法为：

ψ_k代表当前k时刻陀螺仪解算的航向，ψ_j代表前第j时刻陀螺仪解算的航向，Δψ_k代表当前时刻陀螺仪解算的航向与历史平均值的差值，n代表选取的步数，代表航向差值的阈值；

当航向差值小于阈值时，判定行人沿直线行走，进行下一步判定；反之，由于在连续区间的航向发生较大变化，判定为非直线运动；

S122中，陀螺仪角速率判定的方法为：

观察在直行、转圈、左转弯、右转弯下，可发现行人行走过程中的角速率具有明显的变化；

根据角速率变化规律，进行类型判定；

m代表加权滑动均值的窗口大小，代表窗口第j个陀螺角速率值，α_j代表所对应的权重因子，ΔWar_m代表相邻两个窗口的差值，代表所取阈值；

当角速率差值小于阈值时，判定行人沿直线行走，进行下一步判定；反之，由于在2个相邻区间的角速率发生较大变化，判定为非直线运动；

S123,行人位置判定的方法为：

为了判断执行，采用前3步的位置和当前位置检测直线运动；基于线性回归通过4步位置的垂直偏移拟合直线，设R_k、R_k-1、R_k-2、R_k-3分别为当前位置和前3步的位置，拟合直线为y＝a+bx；

垂直偏移量的平方和：

由函数D得到a的偏导：

由函数D得到b的偏导：

得到函数D的最小值：

选择函数D最小化的b值：

其中，

得到一条最佳拟合直线，利用最小函数值D来判定；当D小于阈值时，判定行人沿直线行走，进行下一步判定；

S124,主方向判定的方法为：

根据8个主方向，使用MOD函数得到离最接近主方向的偏移差值；

当时，Δψ_k-1^*＝Δψ_k-1^*-Δ

代表取整数的函数，输出结果为小于的最大整数值，即此算法中MOD函数的取值区间为[0，Δ]，Δψ_M代表MOD函数取余差值；

当偏移差值小于阈值时，判定行人沿主方向直线行走；反之，则为沿非主方向直线行走。

2.根据权利要求1所述的基于改进式HDE的室内行人导航航向修正方法，其特征在于：所述S2中，构建主方向航向偏移差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波(EKF)，得到航向的最优估计从而修正航向；

首先构建9维状态向量：x＝[δψⁿ,δωⁿ,δpⁿ]；

其中，δψ＝[δpitch,δroll,δyaw]为3轴姿态角误差，δω＝[δω_x,δω_y,δω_z]为3轴角速率误差，δp＝[δx,δy,δh]为位置误差；量测矩阵为H＝[0,0,1,0,0,0,0,0,0]，由扩展卡尔曼滤波得到最优估计结果可得航向信息

该输出航向作为下一时刻的初始值，参与到下一时刻的姿态角结算和行人航迹推算算法中，达到对航向的修正作用。