CN111024126A - 一种行人导航定位中的自适应零速修正方法 - Google Patents

Abstract

本发明一种行人导航定位中的自适应零速修正方法，在行人足跟部穿戴导航模块，所述导航模块由陀螺仪和加速度计组成，本发明方法使用MEMS单元获得的信息，根据阈值判定行走零速状态。在零速状态时，构成全维度状态观测量，设计卡尔曼滤波器，得到行人更精确的速度位置信息。在缺少外部导航信息的情况下，消除惯性导航累积误差，提高定位精度。

Description

一种行人导航定位中的自适应零速修正方法

技术领域

本发明涉及一种行人导航定位中的自适应零速修正方法，属于惯性导航技术领域。

背景技术

个人导航系统能够实时跟踪人员的速度、姿态、位置等信息，通过无线通讯传递给终端进行显示，实时监控人员运动状态和位置变化。能够应用于战场作战指挥、人员搜救、战场侦察，提高作战实时反应，增加作战成功率。

目前的行人导航系统主要依赖卫星、UWB、WIFI等外部导航信息来实现，卫星定位能实现较好的定位精度，但在有遮挡的室内情况下定位失效。UWB、WIFI等定位方法虽然能够实现室内定位，但是需要预先铺设基站，使定位区域受到限制。基于MEMS惯性器件的个人导航系统体积小、自主性高、价格低、适合规模量产，在军事行动、反恐、消防等领域有很好的应用。基于惯性导航的人员定位系统存在定位误差随时间累积的缺点。本发明提出了一种零速修正方法，能够更精准的判别人行走中的零速状态，通过卡尔曼滤波消除累积误差，实现利用自身传感器信息的自主行人导航。

惯性导航系统技术已经非常成熟，广泛地应用飞机、车辆、船舶等大型载体，由于传统惯性器件的体积较大、成本较高，不能够应用于人员的定位跟踪。现阶段由于MEMS技术的突飞猛进，解决了设备重量、体积和造价的问题，使人员定位惯性导航系统成为了可能。同时惯性导航特有的自主性克服了卫星导航易受遮挡干扰的缺点。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提出一种行人导航零速修正的方法，有效地抑制了惯性导航系统导航信息的累积误差，实现各种环境下人员的精确定位。

本发明解决技术的方案是：

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

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)；

7)根据步骤6)所述更新后的速度、位置和姿态角，确定行人当前k+1时刻的位置相对于起始点的位移

作为最终结果输出给显示终端。

步骤2)所述确定角速度补偿量ε和加速度补偿量

具体如下：

ε＝a_gT_ε ²+b_gT_ε+c_g；

步骤3)所述确定补偿后的角速度ω′和补偿后的加速度f′，具体为：

ω′＝ω-ε；

本发明与现有技术相比的有益效果是：

本发明使用行走时的陀螺仪和加速度计输出信息检测零速状态，并采用零速修正算法消除惯性导航单元的累积误差，在不使用GPS等外界导航信息辅助的条件下进行完全自主导航。设备采用MEMS器件具有便携、低成本的优势，适合批量地应用于行人导航。

1)采用零速卡尔曼滤波技术手段，起到了抑制惯性误差累积作用，提高了连续导航的精度；

2)采用自适应零速阈值判定技术手段，使零速状态的判断更加准确，减少了零速状态的误判，提高导航精度；

附图说明

图1为本发明惯性导航系统的系统工作原理图。

具体实施方式

下面结合实施例对本发明作进一步阐述。

本发明的MEMS模块包括：三轴加速度计、三轴陀螺仪、三轴磁力计、气压计。通过陀螺仪和加速度计的输出检测行走过程的零速状态，进行零速修正。以速度作为观测量设计卡尔曼滤波器对速度误差、位置误差、姿态误差进行估计，补偿到惯性导航单元的导航信息提升精度。

系统的工作原理图如图1所示，通过三轴陀螺仪测量的角速度积分得到载体的姿态角，通过姿态角计算捷联矩阵将加速度计测量的比力信息投影到导航坐标系下进行积分，获得导航坐标系下的速度，再次积分得到行人的位置信息。该部分为纯导航下的导航信息输出，根据行走时的步态特征，结合陀螺仪和加速度计的输出信息判定脚部零速状态，进行卡尔曼滤波，获得纠正后的姿态、速度、位置信息。

所述的零速修正卡尔曼滤波算法，根据零速时脚部速度为零的特点，选取速度误差作为观测量，设计简化的全维状态方程，对速度误差、位置误差、姿态误差进行估计，反馈补偿给惯性导航单元对累积误差进行消除。

导航模块系统运行分为初始化和正常工作两个模式，系统上电后，静止一段时间等待系统实现初始化，然后进入正常工作模式。

初始化要完成对系统的初始姿态和初始位置的确定，以及温度检测和温度补偿，包括对MEMS陀螺仪进行温度补偿，对加速度计进行温度补偿，根据加速度计输出计算姿态角。其中陀螺仪和加速度计温度补偿采用二阶多项式拟合

姿态角的计算公式如下

其中，f为加速度计三轴输出的平均值，由于MEMS器件的精度有限，航向角无法确定，本案采取初始方位朝东放置。零速判定的阈值所需的值也需要在初始化时刻确定。取初始化时间内的加速度计和陀螺仪输出的方差作为零速判定的条件。使用RF算法对运动状态进行分类并根据运动状态自适应地调整判定阈值。

对于单兵导航系统，位置变化对状态方程的影响可以忽略，为了提高导航的实时性我们采用简化的误差方程。

离散化的状态方程和量测方程为

δx_k|k-1＝A_kδx_k-1|k-1+w_k-1

Z_k＝H_kδx_k|k+n_k

卡尔曼滤波的过程可表示为

以上五个方程组成一个循环递推最优估计，通过递推估计能够消除惯性导航计算随时间的积累的误差。

本发明中的MEMS惯性器件安装在鞋底，通过捷联解算计算行人姿态速度位置信息，并使用步态检测的算法检测零速，进行卡尔曼滤波估计消除累积误差，使用气压计控制速度发散。

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

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)；

7)根据步骤6)所述更新后的速度、位置和姿态角，确定行人当前k+1时刻的位置相对于起始点的位移

作为最终结果输出给显示终端。

步骤2)所述确定角速度补偿量ε和加速度补偿量

具体如下：

ε＝a_gT_ε ²+b_gT_ε+c_g；

步骤3)所述确定补偿后的角速度ω′和补偿后的加速度f′，具体为：

ω′＝ω-ε；

步骤5)所述确定行人当前k+1时刻的位置相对于起始点的位移

的方法，具体为：

其中，

为k+1时刻行人位置相对起始点的位移，

为k+1时刻导航模块的速度，R_k为k时刻根据陀螺仪的输出确定的导航模块的姿态矩阵；k＝1时，R_k由初始时刻的姿态角确定，

为重力加速度，

为导航坐标系相对于惯性坐标系的旋转矢量，ω′＝[ω_x，ω_y，ω_z]为陀螺仪的补偿后的三轴输出的角速度。

步骤4)所述当前导航模块是否进入零速状态的方法，具体为：

41)按100HZ频率分别采集K组静止、行走、奔跑、上下楼梯、乘电梯五个运动状态时加速度计和陀螺仪的输出，对采集到的数据进行滑动窗体平滑后按固定形式排列得到5*K个序列A_m×n，其中，m为采集数据帧数，n＝8；

A_m×n＝(a_x，a_y，a_z，ω_x，ω_y，ω_z，a，ω)

式中a_x，a_y，a_z为加速度计的三轴输出，ω_x，ω_y，ω_z为陀螺仪的三轴输出，a为加速度计的三轴输出的矢量和，ω为陀螺仪的三轴输出的矢量和；

42)从5*K个A_m×n样本数据中获取每列数据的特征向量，构成特征向量矩阵，得到5*K个特征向量矩阵，特征向量矩阵的获得方法如下：

T＝f(A_m×n)＝(μ，σ，s_max，s_min，s_k，r_xy，r_yz，r_xz)

其中，T为8×8维的特征向量矩阵，f(A)为矩阵A的函数，μ为样本算数平均值，σ为样本标准差，s_max为样本最大值，s_min为样本最小值，s_k为样本偏度，r_xy为样本x轴和y轴数据之间的相关系数，r_yz为样本y轴和z轴数据之间的相关系数，r_xz为样本x轴和z轴数据之间的相关系数；

43)定义一个由一组决策分类器{h(T,K)，k＝1，2，…，n_tree}，n_tree＝50-100，组成的集成分类器；将采集的5*K个特征向量矩阵作为训练集，训练模型；

44)模块进入导航模式时，以实时采集数据提取的特征向量作为测试集，使用分类器进行分类，得到人员的实时运动状态；

如果对应状态下的判定值小于对应状态下的阈值，则判定为模块处于零速状态；

d1(k₀)<r1且d2(k₀)<r2

为当前k时刻前L个加速度计输出的加速度采样点的均方差，L的取值范围为50～100，r1在静止、行走、奔跑、上下楼梯的取值0.008～0.02之间；

为当前k时刻前L个陀螺仪输出的角速度采样点的均方差，L的取值范围为50～100，r2在静止、行走、奔跑、上下楼梯的取值在0.5～0.8。

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

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

其中，ε_k为陀螺常值误差，

为陀螺漂移误差，

为姿态角误差，

为速度误差，

为位置误差；

简化后的状态方程为：

δx_k|k-1＝A_kδx_k-1|k-1+w_k-1

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

其中，R_k-1为姿态矩阵，

为导航坐标系下加速度计实时输出的加速度的反叉乘矩阵，t_s为采样时间，I₁₅为15为单位矩阵,I₃为三维单位矩阵；

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

Z_k＝H_kδx_k|k+n_k

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

H_k＝[0 0 0 I₃ 0]

根据状态方程和观测方程进行卡尔曼滤波得到位置误差

和姿态误差

得到修正后的位置信息

和姿态角

具体为：

由

可确定的姿态矩阵R_k。

实施例

将行人导航模块安装固定在温箱转台中，在-20℃到50℃，间隔温度为10℃，选取8个温度点，以100HZ的频率采集数据，进行温度补偿。使用标定补偿后的模块在没有GPS信号的楼宇内部进行行走测试，将模块放置于定制鞋垫的脚跟部位，朝东上电静止3s后，开始行走，使用手台接受模块输出位置信息进行显示，进行25min的平面行走，5min的上下楼梯行走，回到起始位置。最终水平误差1.3m，高度误差小于1m。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

本发明说明书中未作详细描述的内容属本领域专业技术人员的公知技术。

Claims (6)

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)；

7)根据步骤6)所述更新后的速度、位置和姿态角，确定行人当前k+1时刻的位置相对于起始点的位移

作为最终结果输出给显示终端。

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

具体如下：

ε＝a_gT_ε ²+b_gT_ε+c_g；

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

ω′＝ω-ε；

4.根据权利要求1所述的一种行人导航定位中的自适应零速修正方法，其特征在于，步骤5)所述确定行人当前k+1时刻的位置相对于起始点的位移

的方法，具体为：

其中，

为k+1时刻行人位置相对起始点的位移，

为k+1时刻导航模块的速度，R_k为k时刻根据陀螺仪的输出确定的导航模块的姿态矩阵；k＝1时，R_k由初始时刻的姿态角确定，

为重力加速度，

为导航坐标系相对于惯性坐标系的旋转矢量。

5.根据权利要求1～4任意之一所述的一种行人导航定位中的自适应零速修正方法，其特征在于，步骤4)所述当前导航模块是否进入零速状态的方法，具体为：

41)按100HZ频率分别采集K组静止、行走、奔跑、上下楼梯、乘电梯五个运动状态时加速度计和陀螺仪的输出，对采集到的数据进行滑动窗体平滑后按固定形式排列得到5＊K个序列A_m×n，其中，m为采集数据帧数，n＝8；

A_m×n＝(a_x，a_y，a_z，ω_x，ω_y，ω_z，a，ω)

式中a_x，a_y，a_z为加速度计的三轴输出，ω_x，ω_y，ω_z为陀螺仪的三轴输出，a为加速度计的三轴输出的矢量和，ω为陀螺仪的三轴输出的矢量和；

42)从5＊K个A_m×n样本数据中获取每列数据的特征向量，构成特征向量矩阵，得到5＊K个特征向量矩阵，特征向量矩阵的获得方法如下：

T＝f(A_m×n)＝(μ，σ，s_max，s_min，s_k，r_xy，r_yz，r_xz)

其中，T为8×8维的特征向量矩阵，f(A)为矩阵A的函数，μ为样本算数平均值，σ为样本标准差，S_max为样本最大值，S_min为样本最小值，s_k为样本偏度，r_xy为样本x轴和y轴数据之间的相关系数，r_yz为样本y轴和z轴数据之间的相关系数，r_xz为样本x轴和z轴数据之间的相关系数；

43)定义一个由一组决策分类器{h(T，K)，k＝1，2，...，n_tree}，n_tree＝50-100，组成的集成分类器；将采集的5＊K个特征向量矩阵作为训练集，训练模型；

44)模块进入导航模式时，以实时采集数据提取的特征向量作为测试集，使用分类器进行分类，得到人员的实时运动状态；

如果对应状态下的判定值小于对应状态下的阈值，则判定为模块处于零速状态；

d1(k₀)＜r1且d2(k₀)＜r2

为当前k时刻前L个加速度计输出的加速度采样点的均方差，L的取值范围为50～100，r1在静止、行走、奔跑、上下楼梯的取值0.008～0.02之间；

为当前k时刻前L个陀螺仪输出的角速度采样点的均方差，L的取值范围为50～100，r2在静止、行走、奔跑、上下楼梯的取值在0.5～0.8。

6.根据权利要求1～4任意之一所述的一种行人导航定位中的自适应零速修正方法，其特征在于，步骤6)所述通过卡尔曼滤波计算位置误差、姿态误差的方法，具体为：

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

其中，ε_k为陀螺常值误差，

为陀螺漂移误差，

为姿态角误差，

为速度误差，

为位置误差；

简化后的状态方程为：

δx_k|k-1＝A_kδx_k-1|_k-1+w_k-1

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

其中，R_k-1为姿态矩阵，

为导航坐标系下加速度计实时输出的加速度的反叉乘矩阵，t_s为采样时间，I₁₅为15为单位矩阵，I₃为三维单位矩阵；

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

Z_k＝H_kδx_k|k+n_k

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

H_k＝[0 0 0 I₃ 0]

根据状态方程和观测方程进行卡尔曼滤波得到位置误差

和姿态误差

得到修正后的位置信息

和姿态角

具体为：

由

可确定的姿态矩阵R_k。

Images