[发明专利]一种基于自回溯算法的室内行人导航方法

权利要求书

1.一种基于自回溯算法的室内行人导航方法，被导航的行人佩戴有MEMS传感器，其特征在于，所述导航方法的步骤包括：

步骤1：根据MEMS陀螺仪和MEMS加速度计进行行人步态推算得到一段时长行人的行走信息，对该时间段内的行走信息进行直线检测，

步骤2：判定行人在该时间段内的行走路线是否为直线，如果确定行人行走处于直线状态，则执行步骤3，否则返回步骤1，所述行走信息包括行人位置、航向角和步长；

步骤3：根据航向角信息构建伪观测量，利用正反向自适应卡尔曼滤波算法进行导航误差检测与修正补偿；

步骤4：重复步骤1至3，持续进行导航误差检测与修正补偿，同时输出行人室内导航定位结果；

所述构建伪观测量，利用正反向自适应卡尔曼滤波算法进行导航误差检测与修正补偿的步骤是：

第一步，利用PDR推算分别获取时长内每一步的航向角信息：

获取时长内每一步的步长信息：

第二步，构建伪观测序列：

第三步，建立与每一步的航向角信息、每一步的步长信息、伪观测序列信息相关联的表达式模型：

H＝[0 0 0 0 1]，

A(k)为k时刻的系统状态转移矩阵，H为量测矩阵；

第四步，对表达式模型利用自适应卡尔曼滤波算法进行导航误差检测与修正补偿，具体的算法是：

P_k,k-1＝A(k)P_k-1(A(k))^T+Q

P_k＝[I-K_kH]P_k,k-1

其中：

代表滤波结果，代表上一步滤波结果，Q代表模型噪声对应的协方差矩阵，代表滤波结果状态上一步预测，P_k,k-1代表上一步预测滤波误差方差，为主航向，K_k代表滤波增益，P_k代表当前步滤波误差方差，P_k-1代表k-1步的滤波误差方差，I代表对应维度的单位矩阵，代表自适应量测噪声方差矩阵：其中std(Z_pseduo)代表序列Z_pseduo的标准差；其中，会随着行人行走模式、行走状态的变化进行自适应在线调整；

第五步，在第四步的基础上实现正反向自适应卡尔曼滤波算法进行导航误差检测与修正补偿，具体的算法是：

首次正向自适应滤波：

设定正向滤波的初始值为：

开始进行正向自适应滤波：

P_k＝[I-K_kH]P_k,k-1

其中，k由1递增至10；得到正向滤波结果：与

进行反向自适应滤波：

将与作为滤波初始值，

P_k-1,k＝A(k-1)^-1P_k(A(k-1)^-1)^T+Q

P_k-1＝[I-K_kH]P_k-1,k

其中，A(k-1)^-1代表A(k-1)的逆矩阵，k在反向滤波过程中由10递减至1；得到反向滤波结果与P₁²；

此时结束一轮正反向自适应Kalman滤波，下一轮运算开始时，将与P₁²提供给正向自适应滤波算法继续重复上述运算过程，循环迭代两轮后，最终得到正反向自适应滤波算法的结果：与

2.根据权利要求1所述的导航方法，其特征在于，所述时长是行人行走10步的时间长度。

3.根据权利要求1所述的导航方法，其特征在于，所述直线检测及判定行人在该时间段内的行走路线是否为直线是：计算时长起始和终止之间航向角最大值与最小值的差值，如果差值的绝对值小于20*π/180弧度，则认为行人在该时间段内为直线行走，否则不为直线行走。