一种基于磁干扰检测的步行者轨迹推算方法及装置与流程
本发明涉及探测技术领域，尤其涉及一种基于磁干扰检测的步行者轨迹推算方法及装置。
背景技术：
步行者轨迹推算是导航领域研究的热点和难点之一，在消防救援、单兵定位、商业化位置服务、行人导航等领域具有广泛的应用前景。在室内、森林、矿井等环境中，卫星定位系统因信号被遮挡而无法实现有效定位；超宽带、射频识别等定位方案需在定位环境预先部署定位信标，无法应对陌生环境或突发状况，且成本高；因此，基于惯性测量单元的步行者轨迹推算方案由于不需外部辅助设施且在卫星信号被遮挡环境能实现自主实时定位，而受到广泛关注。在基于惯性测量单元的步行者轨迹推算方案中，零速修正方案因不需对人体运动进行建模，能实现复杂运动状态下步行者的准确定位，而成为步行者轨迹推算方案研究的热点。该方案利用步行者脚落地时存在零速的运动规律，建立零速观测信息对低成本低精度的mems捷联惯导随时间而累积的定位误差进行修正，从而实现步行者运动轨迹的准确实时自主推算。高精度的姿态角信息是步行者轨迹推算的关键。目前一般利用地磁矢量信息构建航向角观测量对轨迹推算系统的姿态角进行校正。地磁场观测信息的准确与否直接关系到航向角的准确与否，对轨迹推算精度影响大。磁传感器所测磁场信息不仅包括轨迹推算所需的地磁场信息，而且包括载体自身干扰、软硬铁效应、外部磁场等干扰信息，如电磁干扰、建筑物内部钢筋、道路上铁质井盖、汽车、外部磁源等。因此，如何对外部磁干扰进行检测，对于高精度步行者轨迹推算系统而非常重要。
目前，现有技术对磁力计测量误差的校正与补偿一般利用校正时刻和地点的磁场信息，通过基于椭球假设的最小二乘法估计磁力计校正参数，整个行走过程采用该校正参数。如果行走中外部磁场环境不发生变化，则该方法可达到较高精度，如果外部磁场环境是变化的，则原先的校正参数将失效。实际行走中，由于建筑物内部钢筋、道路上铁质井盖、汽车、外部磁源等会导致外部磁场环境发生变化，若仍采用原先的校正参数，则姿态角估计效果变差，将严重影响轨迹推算精度。
技术实现要素：
鉴于上述的分析，本发明旨在提供一种基于磁干扰检测的步行者轨迹推算方法及装置，用以完全或者至少部分地解决上述问题。
为解决上述问题，本发明主要是通过以下技术方案实现的：
本发明一方面提供了一种基于磁干扰检测的步行者轨迹推算方法，该方法包括：获取步行者的加速度、角速度，以及步行者所处的地磁矢量；
根据卡尔曼偏差估计结果对获取的步行者的加速度、角速度进行偏差补偿，对获取的所述地磁矢量进行校正，判断所述地磁矢量是否存在干扰，确定当前时刻的磁场干扰检测结果；
根据加速度和角速度数据零速检测方法对步行者的运动状态进行判断；
在地理坐标系下利用已经进行偏差补偿的角速度进行姿态矩阵递推；
去除重力加速度影响，进行速度递推和位置递推；
根据磁场干扰检测结果和步行者的运动状态确定步行者的轨迹。
进一步地，对获取的所述地磁矢量进行校正，具体包括：
建立磁力计测量的通用误差模型
其中，
进一步地，所述判断所述地磁矢量是否存在干扰，具体包括：
条件1，当前时刻的磁场强度判定，判定模型为：
其中，||·||2为向量的二范数，表示磁场强度大小，flag条件1为磁干扰判定标志位，等于1时为有磁干扰，br为步行者活动区域的参考地磁场强度大小，根据活动区域的经纬度，由世界地磁场模型查询获得，参数α和参数β预设的经验参数；
和/或，
条件2，当前时刻磁场强度的滑动方差判定，判定模型为：
其中，参数s为正整数，决定滑动窗口大小，由经验给定，flag条件2为条件2的磁干扰判定标志位，等于1时为有磁干扰，
当条件1或条件2判定存在有磁干扰，则判定为有磁干扰。
进一步地，所述根据磁场干扰检测结果和步行者的运动状态确定步行者的轨迹，具体包括：
当判定存在外部磁干扰且步行者为非静止状态，则直接通过系统状态方程进行状态传播获得下一时刻的轨迹状态；
当判定存在外部磁干扰且步行者为静止状态，通过零速校正原理对步行者的轨迹进行修正，卡尔曼滤波的观测量为零速状态量，观测方程为：
yk＝hzk+υk；
其中，h为观测矩阵，υk为噪声矩阵；
观测模型观测量
当判定无外部磁干扰且步行者为非静止状态，通过磁力计测量地磁场所得航向角信息对步行者的轨迹进行卡尔曼滤波修正，卡尔曼滤波的观测量为k时刻姿态矩阵所得航向角与由地磁场所得航向角的差量，观测模型的观测量为：
zk＝δψ；
其中，
当判定无外部磁干扰且步行者为静止状态，通过磁力计测量地磁场所得航向角信息与零速校正原理对步行者的轨迹进行卡尔曼滤波修正，观测模型的观测量为：
相应的观测矩阵为：
进一步地，所述卡尔曼滤波修正，具体包括：
通过卡尔曼误差估计结果对位置和速度进行误差补偿；
vk|k＝vk|k-1-δvk
rk|k＝rk|k-1-δrk
其中δvk为速度误差卡尔曼估计结果，δrk为位置误差卡尔曼估计结果；
根据卡尔曼误差估计结果对姿态矩阵进行更新
其中，
进行卡尔曼误差估计：
设状态误差向量为
δxk，k-1＝φkδxk-1，k-1+wk-1
其中，
本发明另一方面还提供了一种基于磁干扰检测的步行者轨迹推算装置，该装置包括：
获取单元，用于获取步行者的加速度、角速度，以及步行者所处的地磁矢量；
处理单元，用于根据卡尔曼偏差估计结果对获取的步行者的加速度、角速度进行偏差补偿，对获取的所述地磁矢量进行校正，判断所述地磁矢量是否存在干扰，确定当前时刻的磁场干扰检测结果；根据加速度和角速度数据零速检测方法对步行者的运动状态进行判断；在地理坐标系下利用已经进行偏差补偿的角速度进行姿态矩阵递推；去除重力加速度影响，进行速度递推和位置递推；根据磁场干扰检测结果和步行者的运动状态确定步行者的轨迹
进一步地，所述处理单元还用于，建立磁力计测量的通用误差模型
进一步地，所述处理单元还用于，条件1，当前时刻的磁场强度判定，判定模型为：
其中，||·||2为向量的二范数，表示磁场强度大小，flag条件1为磁干扰判定标志位，等于1时为有磁干扰，br为步行者活动区域的参考地磁场强度大小，根据活动区域的经纬度，由世界地磁场模型查询获得，参数α和参数β预设的经验参数；
条件2，当前时刻磁场强度的滑动方差判定，判定模型为：
其中，参数s为正整数，决定滑动窗口大小，由经验给定，flag条件2为条件2的磁干扰判定标志位，等于1时为有磁干扰，
进一步地，所述处理单元还用于，当判定存在外部磁干扰且步行者为非静止状态，则直接通过系统状态方程进行状态传播获得下一时刻的轨迹状态；
当判定存在外部磁干扰且步行者为静止状态，通过零速校正原理对步行者的轨迹进行修正，卡尔曼滤波的观测量为零速状态量，观测方程为：
yk＝hzk+υk；
其中，h为观测矩阵，υk为噪声矩阵；
观测模型观测量
当判定无外部磁干扰且步行者为非静止状态，通过磁力计测量地磁场所得航向角信息对步行者的轨迹进行卡尔曼滤波修正，卡尔曼滤波的观测量为k时刻姿态矩阵所得航向角与由地磁场所得航向角的差量，观测模型的观测量为：
zk＝δψ；
其中，
当判定无外部磁干扰且步行者为静止状态，通过磁力计测量地磁场所得航向角信息与零速校正原理对步行者的轨迹进行卡尔曼滤波修正，观测模型的观测量为：
相应的观测矩阵为：
进一步地，所述处理单元还用于，通过卡尔曼误差估计结果对位置和速度进行误差补偿；
vk|k＝vk|k-1-δvk
rk|k＝rk|k-1-δrk
其中δvk为速度误差卡尔曼估计结果，δrk为位置误差卡尔曼估计结果；
根据卡尔曼误差估计结果对姿态矩阵进行更新
其中，
进行卡尔曼误差估计：设状态误差向量为
其中，
本发明有益效果如下：
本发明可利用磁干扰实时检测方法，结合零速校正原理，从而实现在外部磁干扰场景中的进行高精度稳定轨迹推算。
本发明的其他特征和优点将在随后的说明书中阐述，并且部分的从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
附图说明
图1为本发明实施例的一种基于磁干扰检测的步行者轨迹推算方法的流程示意图；
图2为本发明实施例的磁干扰检测的流程示意图；
图3为本发明实施例的步行者轨迹推算流程示意图；
图4为应用本发明实施例的方法得到的一种试验效果图；
图5为应用本发明实施例的方法得到的另一种试验效果图；
图6为本发明实施例的一种基于磁干扰检测的步行者轨迹推算装置的结构示意图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。为了清楚和简化目的，当其可能使本发明的主题模糊不清时，将省略本文所描述的器件中已知功能和结构的详细具体说明。
本发明提供了一种基于磁干扰检测的步行者轨迹推算方法及装置，本发明可利用磁干扰实时检测方法，结合零速校正原理，从而实现在外部磁干扰场景中的进行高精度稳定轨迹推算。以下结合附图以及几个实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不限定本发明。
本发明实施例提供了一种基于磁干扰检测的步行者轨迹推算方法，参见图1，该方法包括：
s101、获取步行者的加速度、角速度，以及步行者所处的地磁矢量；
s102、根据卡尔曼偏差估计结果对获取的步行者的加速度、角速度进行偏差补偿，对获取的所述地磁矢量进行校正，判断所述地磁矢量是否存在干扰，确定当前时刻的磁场干扰检测结果；
s103、根据加速度和角速度数据零速检测方法对步行者的运动状态进行判断；
s104、在地理坐标系下利用已经进行偏差补偿的角速度进行姿态矩阵递推；
s105、去除重力加速度影响，进行速度递推和位置递推，得到步行者的轨迹；
s106、根据磁场干扰检测结果和步行者的运动状态确定步行者的轨迹。
也就是说，本发明可利用磁干扰实时检测方法，结合零速校正原理，从而实现在外部磁干扰场景中的进行高精度稳定轨迹推算。
进一步地，本发明所述对获取的所述地磁矢量进行校正，具体包括：
建立磁力计测量的通用误差模型
其中，
误差校正系数阵a和b可通过传统的基于椭球假设的最小二乘法获得，由步行者在行走前或者开始时完成。一旦获得误差校正系数阵a和b后，每时刻的磁力计测量值可通过误差模型进行校正，如k时刻校正后的磁场矢量为
所述对获取的所述地磁矢量进行校正的步骤之后，所述根据加速度和角速度数据通过零速检测方法对步行者的运动状态进行判断的步骤之前，还包括：
判断当前是否存在磁场干扰，并确定当前时刻的磁场干扰检测结果。
具体实施时，本发明所述判断当前是否存在磁场干扰，具体包括：
条件1，当前时刻的磁场强度判定，判定模型为：
其中，||·||2为向量的二范数，表示磁场强度大小，flag条件1为磁干扰判定标志位，等于1时为有磁干扰，br为步行者活动区域的参考地磁场强度大小，根据活动区域的经纬度，由世界地磁场模型查询获得，参数α和参数β预设的经验参数；
和/或，
条件2，当前时刻磁场强度的滑动方差判定，判定模型为：
其中，参数s为正整数，决定滑动窗口大小，由经验给定，flag条件2为条件2的磁干扰判定标志位，等于1时为有磁干扰，
当条件1或条件2判定存在有磁干扰，则判定为有磁干扰。
详细来说，本发明对外部磁干扰实时检测方法具体包括：
外部磁干扰实时检测方法如图2所示，首先对磁力计测量的原始数据进行基于椭球假设的最小二乘法校正，再对校正后磁场矢量进行磁场强度(条件1)和滑动方差(条件2)判定，最后综合两者判定结果输出当前时刻是否有外部磁干扰的检测结果。外部磁干扰检测的原理为：(1)根据科学家的探测结果，地球磁场强度约为0.3～0.6高斯，全球各地地磁场强度可由已公开的最新世界地磁场模型获得，而步行者的活动区域一般不大，相应的活动区域地磁场强度也变化不大。在磁力计所测磁场数据正确校正的基础上，若出现磁场强度远大于或远小于当地磁场实际水平的情况，则显然该处存在外部磁干扰。基于这样的事实，本发明提出了磁干扰判定条件1。(2)步行者由无外部磁干扰环境进入磁干扰环境或由磁干扰环境进入无磁干扰环境时，磁场强度的离散程度会发生变化。基于这样的事实，本发明提出了磁干扰判定条件2，以弥补判定条件1的不足。外部磁干扰检测的具体实施步骤如下：
步骤1：对k时刻的磁力计测量数据进行校正获得载体坐标系下的磁场矢量
可建立磁力计测量的通用误差模型
步骤2：本发明提出2个判定条件对磁场干扰进行实时检测。设置磁干扰检测结果标志位flag，令flag＝1为有磁干扰，flag＝0为无磁干扰。当检测出有外部磁干扰时，则在进行步行者轨迹推算时不利用该时刻的磁力计所测航向角信息。
条件1：当前时刻的磁场强度判定。判定模型为
其中，||·||2为向量的二范数，表示磁场强度大小。flag条件1为条件1的磁干扰判定标志位，等于1时为有磁干扰。br为步行者活动区域的参考地磁场强度大小，根据活动区域的经纬度，由最新的世界地磁场模型查询获得。实际上，受测量噪声等多种因素影响，磁力计所测磁场强度在世界地磁场模型给出的参考地磁场强度附近波动，为此设计参数α和β限定合理的磁场强度范围，其取值根据实际情况由经验给定。
条件2：当前时刻磁场强度的滑动方差判定。判定模型为
其中，参数s为正整数，决定滑动窗口大小，由经验给定。flag条件2为条件2的磁干扰判定标志位，等于1时为有磁干扰。
步骤3：k时刻磁干扰判定结果。判定模型为
flag＝flag条件1|flag条件2
其中，符号|为逻辑或，即条件1和条件2只要有个标志位为1，则判定为有磁干扰。
具体实施时，所述根据磁场干扰检测结果和步行者的运动状态确定步行者的轨迹，具体包括：
当判定存在外部磁干扰且步行者为非静止状态，则直接通过系统状态方程进行状态传播获得下一时刻的轨迹状态；
当判定存在外部磁干扰且步行者为静止状态，通过零速校正原理对步行者的轨迹进行修正，卡尔曼滤波的观测量为零速状态量，观测方程为：
yk＝hzk+υk；
其中，h为观测矩阵，υk为噪声矩阵；
观测模型观测量
当判定无外部磁干扰且步行者为非静止状态，通过磁力计测量地磁场所得航向角信息对步行者的轨迹进行卡尔曼滤波修正，卡尔曼滤波的观测量为k时刻姿态矩阵所得航向角与由地磁场所得航向角的差量，观测模型的观测量为：
zk＝δψ；
其中，
当判定无外部磁干扰且步行者为静止状态，通过磁力计测量地磁场所得航向角信息与零速校正原理对步行者的轨迹进行卡尔曼滤波修正，观测模型的观测量为：
相应的观测矩阵为：
本发明实施例所述卡尔曼滤波修正，具体包括：通过卡尔曼误差估计结果对位置和速度进行误差补偿；
vk|k＝vk|k-1-δvk
rk|k＝rk|k-1-δrk
其中δvk为速度误差卡尔曼估计结果，δrk为位置误差卡尔曼估计结果；
根据卡尔曼误差估计结果对姿态矩阵进行更新
其中，
进行卡尔曼误差估计：
设状态误差向量为
δxk，k-1＝φkδxk-1，k-1+wk-1
其中，
详细来说，本发明对步行者轨迹推算方法包括：
步行者轨迹推算的流程如图3所示。传感器数据采集模块利用加速度计采集加速度数据、陀螺仪采集角速度数据及磁力计采集磁场数据。测量数据补偿模块完成对加速度和角速度数据的偏差补偿，并对磁力计数据进行校正。根据磁干扰检测和零速检测模块的结果动态选择观测模型，可分为如下四种情况：
情况1：有外部磁干扰+非静止状态。直接利用系统状态方程进行状态传播获得下一时刻的轨迹状态，而不进行卡尔曼滤波。
情况2：有外部磁干扰+静止状态。仅可利用零速校正原理对步行者的轨迹进行修正，此时卡尔曼滤波的观测量为零速状态量，观测方程为：
yk＝hzk+υk
其中，h为观测矩阵，υk为噪声矩阵。
观测模型1的观测量为
其中，
情况3：无外部磁干扰+非静止状态。仅可利用磁力计测量地磁场所得航向角信息对步行者的轨迹进行卡尔曼滤波修正。卡尔曼滤波的观测量为k时刻姿态矩阵所得航向角与由地磁场所得航向角的差量。观测模型2的观测量为
zk＝δψ
其中，
h＝[[0，0，1]01×301×301×301×3]
情况4：无外部磁干扰+静止状态。可同时利用磁力计测量地磁场所得航向角信息与零速校正原理对步行者的轨迹进行卡尔曼滤波修正。观测模型3的观测量为
相应的观测矩阵为
下面将通过一个具体的例子对本发明所述的方法进行详细的解释和说明：
本实例提供一种基于磁干扰检测的步行者轨迹递推系统，该系统利用固定于步行者脚部的mems传感器，包括三轴加速度计、三轴陀螺仪和三轴磁力计。加速度计测量步行者加速度，陀螺仪测量步行者运动角速度，磁力计测量地磁矢量。具体实施步骤如下：
步骤1运用传统的基于椭球假设的最小二乘法对磁力计测量数据进行校正，获得校正误差系数阵a和b后，k时刻校正后的磁场矢量为
步骤2利用卡尔曼偏差估计结果对陀螺仪和加速度计测量的原始角速度和加速度进行偏差补偿。
其中，
步骤3利用本发明提供的磁干扰检测方法对外部磁干扰进行检测。
步骤4利用加速度和角速度数据对步行者的运动状态进行判断，判断方法为传统的零速检测方法。
步骤5在地理坐标系下利用已经进行偏差补偿的角速度进行姿态矩阵递推。
其中，
步骤6去除重力加速度影响，进行速度递推和位置递推。
rk|k-1＝rk-1|k-1+vk|k-1δt
其中，g为当地重力加速度大小。vk-1|k-1为k-1时刻的速度估计矢量，rk-1|k-1为k-1时刻的位置估计矢量，δt为时间步长。
步骤7利用卡尔曼误差估计结果对位置和速度进行误差补偿
vk|k＝vk|k-1-δvk
rk|k＝rk|k-1-δrk
其中δvk为速度误差卡尔曼估计结果，δrk为位置误差卡尔曼估计结果。
步骤8利用卡尔曼误差估计结果对姿态矩阵进行更新
其中，
步骤9卡尔曼误差估计
设状态误差向量为
δxk，k-1＝φkδxk-1，k-1+wk-1
其中，
根据外部磁干扰和零速检测结果进行判断，当为情况1时，则步行者轨迹递推不进行步骤7、8、9，当为情况2、3、4时，分别选择相应的观测模型，进行步骤7、8、9。
本发明采用了荷兰xsens公司的mti100模块进行了两组步行实验。第1组实验的场地为威海北洋电气集团股份有限公司的篮球场，沿篮球场白线行走，总路程约130米，回到起始点，始终点误差0.75米，定位精度0.58％。第2组实验的场地为威海街道，沿“火炬路-吉林路-文化西路-山大路-火炬路”行走一圈，回到起点，总路程＞1800米，始终点误差11.06米，定位精度＜0.61％，具体如图4和图5所示。
相应于图1所示的方法，本发明实施例还提供了一种基于磁干扰检测的步行者轨迹推算装置，参见图6，该装置包括：
获取单元，用于获取步行者的加速度、角速度，以及步行者所处的地磁矢量；
处理单元，用于根据卡尔曼偏差估计结果对获取的步行者的加速度、角速度进行偏差补偿，对获取的所述地磁矢量进行校正，判断所述地磁矢量是否存在干扰，确定当前时刻的磁场干扰检测结果；根据加速度和角速度数据零速检测方法对步行者的运动状态进行判断；在地理坐标系下利用已经进行偏差补偿的角速度进行姿态矩阵递推；去除重力加速度影响，进行速度递推和位置递推；根据磁场干扰检测结果和步行者的运动状态确定步行者的轨迹。
具体实施时，本发明实施例所述处理单元还用于，建立磁力计测量的通用误差模型
进一步地，本发明实施例所述处理单元还用于，条件1，当前时刻的磁场强度判定，判定模型为：
其中，||·||2为向量的二范数，表示磁场强度大小，flag条件1为磁干扰判定标志位，等于1时为有磁干扰，br为步行者活动区域的参考地磁场强度大小，根据活动区域的经纬度，由世界地磁场模型查询获得，参数α和参数β预设的经验参数；
条件2，当前时刻磁场强度的滑动方差判定，判定模型为：
其中，参数s为正整数，决定滑动窗口大小，由经验给定，flag条件2为条件2的磁干扰判定标志位，等于1时为有磁干扰，
具体实施时，
当判定存在外部磁干扰且步行者为非静止状态，则直接通过系统状态方程进行状态传播获得下一时刻的轨迹状态；
当判定存在外部磁干扰且步行者为静止状态，通过零速校正原理对步行者的轨迹进行修正，卡尔曼滤波的观测量为零速状态量，观测方程为：
yk＝hzk+υk；
其中，h为观测矩阵，υk为噪声矩阵；
观测模型观测量
当判定无外部磁干扰且步行者为非静止状态，通过磁力计测量地磁场所得航向角信息对步行者的轨迹进行卡尔曼滤波修正，卡尔曼滤波的观测量为k时刻姿态矩阵所得航向角与由地磁场所得航向角的差量，观测模型的观测量为：
zk＝δψ；
其中，
当判定无外部磁干扰且步行者为静止状态，通过磁力计测量地磁场所得航向角信息与零速校正原理对步行者的轨迹进行卡尔曼滤波修正，观测模型的观测量为：
相应的观测矩阵为：
10、根据权利要求9所述的装置，其特征在于，
所述处理单元还用于，通过卡尔曼误差估计结果对位置和速度进行误差补偿；
vk|k＝vk|k-1-δvk
rk|k＝rk|k-1-δrk
其中δvk为速度误差卡尔曼估计结果，δrk为位置误差卡尔曼估计结果；
根据卡尔曼误差估计结果对姿态矩阵进行更新
其中，
进行卡尔曼误差估计：设状态误差向量为
其中，
本发明装置实施例的相关内容可参见方法实施例部分进行理解，在此不再详细赘述。
本发明至少能够达到以下的有效效果：
本发明可利用磁干扰实时检测方法，结合零速校正原理，动态选择步行者轨迹推算系统的观测模型，实现在外部磁干扰场景中的高精度稳定轨迹推算。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。
- 还没有人留言评论。精彩留言会获得点赞！