[发明专利]一种行人导航定位中的自适应零速修正方法

权利要求书

1.一种行人导航定位中的自适应零速修正方法，行人足跟部穿戴有导航模块，所述导航模块由陀螺仪和加速度计组成，其特征在于，包括步骤如下：

1)以5℃为间隔，选取-40-+60℃温度段中20个温度点；在每个温度点下，以100Hz为采样频率，持续采集陀螺仪输出的角速度和加速度计输出的加速度数据5min，根据每个温度点下陀螺仪输出的角速度和加速度计输出的加速度，确定每个温度点对应的角速度均值和加速度均值；以各温度点的温度值为输入，以各温度点对应的角速度均值和加速度均值为输出进行二阶拟合，获得陀螺仪的拟合方程参数a_g、b_g、c_g和加速度计的拟合方程参数a_a、b_a、c_a；

2)根据步骤1)获得的拟合方程参数a_g、b_g、c_g、a_a、b_a、c_a；并根据行人在导航定位过程中，当前陀螺仪实时的温度T_ε和当前加速度计实时的温度确定角速度补偿量ε和加速度补偿量

3)根据步骤2)所述的角速度补偿量ε，补偿当前行人在导航定位过程中，陀螺仪输出的角速度ω，获得补偿后的角速度ω′；同时，根据步骤2)所述的加速度补偿量补偿当前行人在导航定位过程中，加速度计输出的加速度f，获得补偿后的加速度f′；

4)根据当前实时陀螺仪输出的角速度和加速度输出角加速度，判定当前导航模块是否进入零速状态，如果未进入零速状态，则执行步骤5)，如果进入零速状态，则执行步骤6)；

5)根据步骤3)计算的当前实时补偿后的角速度ω′和加速度f′，确定行人当前k+1时刻的位置相对于起始点的位移作为最终结果输出给显示终端

6)根据当前k+1时刻导航模块的速度确定速度误差以速度误差作为输入，通过卡尔曼滤波计算位置误差、姿态误差，根据速度误差、位置误差和姿态误差，获得更新后的速度、位置和姿态角，然后进入步骤7)；

所述通过卡尔曼滤波计算位置误差、姿态误差的方法，具体为：

使用15维的状态量构造状态方程，状态量我们选取：

其中，ε_k为陀螺常值误差，为陀螺漂移误差，为姿态角误差，为速度误差，为位置误差；

简化后的状态方程为：

δxk|k-1＝Akδxk-1|k-1+wk-1

其中，Ak为状态转移矩阵，wk-1为系统噪声，为均值为0.02的白噪声；

其中，Rk-1为姿态矩阵，为导航坐标系下加速度计实时输出的加速度的反叉乘矩阵，ts为采样时间，I15为15为单位矩阵，I3为三维单位矩阵；

选取速度误差为输入观测量，构建观测方程，具体为：

Zk＝Hkδxk|k+nk

其中，Hk为量测矩阵，nk为量测噪声，为均值为0.3的白噪声；

Hk＝[000I30]

根据状态方程和观测方程进行卡尔曼滤波得到位置误差和姿态误差得到修正后的位置信息和姿态角具体为：

由可确定姿态矩阵Rk；

7)根据步骤6)所述更新后的速度、位置和姿态角，确定行人当前k+1时刻的位置相对于起始点的位移作为最终结果输出给显示终端。

2.根据权利要求1所述的一种行人导航定位中的自适应零速修正方法，其特征在于，步骤2)所述确定角速度补偿量ε和加速度补偿量具体如下；

ε＝a_gT_ε²+b_gT_ε+c_g；

3.根据权利要求1所述的一种行人导航定位中的自适应零速修正方法，其特征在于，步骤3)所述确定补偿后的角速度ω′和补偿后的加速度f′，具体；

ω′＝ω-ε；