基于图优化框架的行人惯性导航方法和装置

技术领域

本申请涉及行人导航技术领域，特别是涉及一种基于图优化框架的行人惯性导航方法和装置。

背景技术

行人导航是指对行人进行导航定位的技术。在城市、室内、地下等卫星拒止环境中，行人导航已成为导航定位领域一个活跃的分支，且随着技术的发展，Wi-Fi指纹、超宽带、惯性、视觉、射频识别、蓝牙等多种传感器都被用来对行人进行导航定位。而相对于其他导航定位手段，基于MEMS惯性器件的行人导航技术不需要事先布设基站，仅依靠自身携带的陀螺仪、加速度计和磁强计等传感器就可以完成行人的导航定位，能够实现导航定位的自主性和连续性，且其在成本、尺寸、重量、功耗等方面能够很好地满足行人导航系统的应用需求。目前，常见的行人自主定位技术多以惯性传感器为基础，融合Wi-Fi指纹、超宽带、惯性、视觉、射频识别、蓝牙等其他信息源进行定位。

行人捷联惯导解算是一种典型的行人惯性导航算法，其在传统SINS算法的基础上，结合足部运动特有的步态特性，利用零速修正方法，周期性地实现导航误差的校正。零速修正在惯导系统中占据着至关重要的地位，它是进行误差控制一种廉价而有效的手段。在行人行走的每个步态周期内，当足部与地面接触时，足底与地面保持相对静止，传感器对地的速度为零，这一步态信息可以被零速修正方法充分地加以利用，来进行导航误差的估计和校正。因此，准确的零速区间判断对提高捷联惯导解算算法的适应性和鲁棒性至关重要。现有的基于惯性技术的零速检测方法，检测结果易受测量值波动和检测参数设置的影响。一方面，无论是采用单一检测阐值进行检测，还是增加固定时间阀值进行检测，都无法准确地检测零速区间；另一方面，检测参数设置具有手动修改和单独调节的缺点，主观性较强，而且一旦参数设定完成，则在整个导航过程中都会保持固定不变，灵活性较差。

为实现准确的零速区间检测，近年来，国内外对该算法研究主要集中在三个方面：一是基于速度或运动模式分类的阈值调整，检测器根据惯性数据为该特定速度或运动等级选择一个已优化的阈值。M. Mäkelä设计了零速度探测器，该探测器使用隐马尔可夫模型（HMM）表示步态周期的不同阶段；R. Zhang在脚载惯性传感器的基础上增加胸部加速度计，综合二者信息更新零速检测的相应阈值；B. Wagstaff训练SVM分类模型对运动类型进行分类，进而更新阈值；K. Pan允许阈值随加速度计测量值的时间变化而变化。二是不借助阈值，训练神经网络直接判断是否零速。J. Wahlstrom用LSTM网络代替传统的零速检测算法，提出一种新的零速修正算法。三是从分析数据特征出发，采取其他方法进行零速检测。S.K.Park提出一种不需要任何的阈值评估，根据峰值以及两峰值之间的最低处的加速度值进行零速检测和修正的方法。 J. Zhang提出一种基于依然比测试的贝叶斯自适应阈值检测模型，通过量化零速检测的先验概率和错误检测的成本来进行零速检测。陈泽提出一种基于最优区间估计的行人导航零速检测阈值自适应算法。

基于因子图的多传感器融合导航将所有的观测信息表示为一个非线性的误差能量函数，通过优化的方法求解该函数的极小值得到状态估计。与滤波过程递推地估计当前的状态分布不同，基于优化的方法批量地估计状态变量，精度和鲁棒性更高。但该方法只是在惯性导航的基础上，融合利用其他传感器对行人位姿进行优化，并未对行人惯性导航的参数进行优化更新。

发明内容

基于此，有必要针对上述技术问题，提供一种能够对惯性导航进行参数优化的基于图优化框架的行人惯性导航方法和装置。

一种基于图优化框架的行人惯性导航方法，所述方法包括：

对获取的MIMU导航数据和辅助导航数据进行数据处理，确定变量节点，以及得到变量节点位置中MIMU导航数据对应的位置信息和辅助导航数据对应的全局因子；

根据所述位置信息通过卡尔曼滤波进行导航解算，并且根据所述MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量；

根据所述虚拟观测量对卡尔曼滤波的导航解算进行误差修正，得到惯导解算结果；

根据所述全局因子，以运动约束和场景约束为因子，构建因子图；

根据所述因子图和所述惯导解算结果进行全局位姿优化，得到全局优化结果；

根据所述全局优化结果和所述惯导解算结果进行惯导参数优化。

在其中一个实施例中，还包括：基于加速度数据对获取的MIMU导航数据和辅助导航数据进行数据处理，得到位置信息和姿态更新频率；

根据所述MIMU导航数据和辅助导航数据的时间戳信息，对所述MIMU导航数据和辅助导航数据进行对齐；

根据所述位置信息和姿态更新频率，确定变量节点，以及确定每个变量节点辅助导航数据对应的全局因子。

在其中一个实施例中，所述位置信息包括：位置、速度以及四元数方向；还包括：根据所述位置、速度以及四元数方向，确定卡尔曼滤波的过滤器状态；

根据所述MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量。

在其中一个实施例中，还包括：根据所述MIMU导航数据的步态特征，调节惯导参数；根据所述惯导参数进行零速检测，得到卡尔曼滤波的虚拟观测量。

在其中一个实施例中，还包括：根据所述全局优化结果和所述惯导解算结果对所述惯导参数进行优化。

在其中一个实施例中，还包括：所述辅助导航数据包括：磁强计数据、GPS数据、WiFi指纹数据以及蓝牙数据。

一种基于图优化框架的行人惯性导航装置，所述装置包括：

预处理模块，用于对获取的MIMU导航数据和辅助导航数据进行数据处理，确定变量节点，以及得到变量节点位置中MIMU导航数据对应的位置信息和辅助导航数据对应的全局因子；

惯导解算模块，用于根据所述位置信息通过卡尔曼滤波进行导航解算，并且根据所述MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量；根据所述虚拟观测量对卡尔曼滤波的导航解算进行误差修正，得到惯导解算结果；

全局优化模块，用于根据所述全局因子，以运动约束和场景约束为因子，构建因子图；根据所述因子图和所述惯导解算结果进行全局位姿优化，得到全局优化结果；

惯导优化模块，用于根据所述全局优化结果和所述惯导解算结果进行惯导参数优化。

上述基于图优化框架的行人惯性导航方法和装置，首先通过MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量，根据虚拟观测量对卡尔曼滤波的导航解算进行误差修正，得到惯导解算结果，然后根据全局因子，以运动约束和场景约束为因子，构建因子图，利用因子图和惯导解算结果进行全局位姿优化，得到全局优化结果，根据全局优化结果和惯导解算结果进行惯导参数优化。通过上述方式，结合全局优化的结果，对惯性导航参数优化。

附图说明

图1为一个实施例中基于图优化框架的行人惯性导航方法的流程示意图；

图2为一个实施例中卡尔曼滤波的过程的流程示意图；

图3为一个实施例中基于图优化框架的行人惯性导航的流程框图；

图4为一个实施例中全局位姿优化的因子图结构图；

图5为一个实施例中基于图优化框架的行人惯性导航装置的结构示意图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

在一个实施例中，如图1所示，提供了一种基于图优化框架的行人惯性导航方法，包括以下步骤：

步骤102，对获取的MIMU导航数据和辅助导航数据进行数据处理，确定变量节点，以及得到变量节点位置中MIMU导航数据对应的位置信息和辅助导航数据对应的全局因子。

步骤104，根据位置信息通过卡尔曼滤波进行导航解算，并且根据MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量。

步骤106，根据虚拟观测量对卡尔曼滤波的导航解算进行误差修正，得到惯导解算结果。

步骤108，根据全局因子，以运动约束和场景约束为因子，构建因子图。

步骤110，根据因子图和惯导解算结果进行全局位姿优化，得到全局优化结果。

步骤112，根据全局优化结果和惯导解算结果进行惯导参数优化。

上述基于图优化框架的行人惯性导航方法中，首先通过MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量，根据虚拟观测量对卡尔曼滤波的导航解算进行误差修正，得到惯导解算结果，然后根据全局因子，以运动约束和场景约束为因子，构建因子图，利用因子图和惯导解算结果进行全局位姿优化，得到全局优化结果，根据全局优化结果和惯导解算结果进行惯导参数优化。通过上述方式，结合全局优化的结果，对惯性导航参数优化。

在其中一个实施例中，基于加速度数据对获取的MIMU导航数据和辅助导航数据进行数据处理，得到位置信息和姿态更新频率；根据MIMU导航数据和辅助导航数据的时间戳信息，对MIMU导航数据和辅助导航数据进行对齐；根据位置信息和姿态更新频率，确定变量节点，以及确定每个变量节点辅助导航数据对应的全局因子。

本实施例中，分析导航信息源的数量和质量，并基于惯性数据完成步态分割和时间对齐。基于加速度数据进行行人运动的步态分割，确定行人位置和姿态更新的频率。在此基础上，根据信息源的时间戳，将多传感器数据与行人的步态对齐。

具体的，在不同的运动模式下，由于运动的剧烈程度和特点不同，加速度信号呈现出不同的变化趋势。将

式中，

对加速度矢量和

加速度滤波值的负峰值即脚刚刚离地。两个负峰值之间即为一步。

根据信息源的数量和质量进行数据的选择，并完成时间同步与对齐。具体以视觉和惯性对齐为例，行人在第 k 时刻的状态

式中

式中

值得说明的是，本实施例中仅以视觉导航为例进行说明，以其他导航数据进行对齐也可以以上述方式进行计算。

在其中一个实施例中，位置信息包括：位置、速度以及四元数方向，根据位置、速度以及四元数方向，确定卡尔曼滤波的过滤器状态；根据MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量。

本实施例中，在传统捷联惯导解算算法的基础上，结合足部运动特有的步态特性，进行零速状态的检测，并将零速状态作为卡尔曼滤波的虚拟观测，递推地对惯导解算结果进行误差补偿。

具体卡尔曼滤波的过程如图2所示，虚线框内是数据解算以及步态特征检测的过程，根据航向角和步态特征进行零速修正卡尔曼滤波，然后将卡尔曼滤波的结果作用于传统捷联惯导解算的数据进行误差修正。

在其中一个实施例中，根据MIMU导航数据的步态特征，调节惯导参数；根据惯导参数进行零速检测，得到卡尔曼滤波的虚拟观测量。

在其中一个实施例中，如图3所示，提供一种基于图优化框架的行人惯性导航的流程框图，根据全局优化结果和惯导解算结果对惯导参数进行优化。在图3中，在数据预处理阶段包括辅助信息的预处理和MIMU导航数据的预处理，然后通过MIMU导航数据进行零速卡尔曼滤波进行误差修正，得到惯导解算结果，对于辅助信息，提取得到全局因子，然后利用全局因子和惯导解算结果进行全局位姿优化，联合全局优化结果和惯导解算结果进行惯导解算参数优化，从而将参数优化结果用于参数调节，从而优化了惯性导航参数。

具体的，对行人的运动过程进行分割，确定变量节点，并以运动约束和场景约束为因子构建因子图，建立误差能量函数，再通过优化的方法求取误差能量函数的最小值，得到位姿最小二乘意义下的最优估计。

全局位姿优化的因子图结构如图4所示。因子图节点为行人六自由度的位姿（包含相对于初始点的位置和方向），且节点频率由步态更新频率决定。相邻节点之间的位姿约束由MIMU提供。视觉信息可以作为相邻节点之间的位姿约束信息，也可以通过图像特征提取提供全局约束。磁强计可以提供航向信息，在一定条件下对航向进行约束。WIFI指纹和GPS因子也提供全局约束。

最终，在参数优化时具体步骤如下：

优化后的位姿估计精度高于惯导解算的行人位姿，因此，将优化后的位姿结果作为观测量，则观测的误差为

式中，z为优化后的位姿估计， h为观测方程：

x为前一状态的位姿估计，u为惯性测量输入，p为惯导解算参数。将其他时刻的观测量也考虑进来，并且给每个误差设一个下标，那么整体的代价函数为

对这个最小二乘进行求解，就可以对解算的参数进行优化。

在其中一个实施例中，辅助导航数据包括：磁强计数据、GPS数据、WiFi指纹数据以及蓝牙数据。

应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

在一个实施例中，如图5所示，提供了一种基于图优化框架的行人惯性导航装置，包括：预处理模块502、惯导解算模块504、全局优化模块506和惯导优化模块508，其中：

预处理模块502，用于对获取的MIMU导航数据和辅助导航数据进行数据处理，确定变量节点，以及得到变量节点位置中MIMU导航数据对应的位置信息和辅助导航数据对应的全局因子；

惯导解算模块504，用于根据所述位置信息通过卡尔曼滤波进行导航解算，并且根据所述MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量；根据所述虚拟观测量对卡尔曼滤波的导航解算进行误差修正，得到惯导解算结果；

全局优化模块506，用于根据所述全局因子，以运动约束和场景约束为因子，构建因子图；根据所述因子图和所述惯导解算结果进行全局位姿优化，得到全局优化结果；

惯导优化模块508，用于根据所述全局优化结果和所述惯导解算结果进行惯导参数优化。

在其中一个实施例中，预处理模块502还用于基于加速度数据对获取的MIMU导航数据和辅助导航数据进行数据处理，得到位置信息和姿态更新频率；根据所述MIMU导航数据和辅助导航数据的时间戳信息，对所述MIMU导航数据和辅助导航数据进行对齐；根据所述位置信息和姿态更新频率，确定变量节点，以及确定每个变量节点辅助导航数据对应的全局因子。

在其中一个实施例中，位置信息包括：位置、速度以及四元数方向；惯导解算模块504还用于根据所述位置、速度以及四元数方向，确定卡尔曼滤波的过滤器状态；根据所述MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量。

在其中一个实施例中，惯导解算模块504还用于根据所述MIMU导航数据的步态特征，调节惯导参数；根据所述惯导参数进行零速检测，得到卡尔曼滤波的虚拟观测量。

在其中一个实施例中，惯导优化模块508还用于根据所述全局优化结果和所述惯导解算结果对所述惯导参数进行优化。

在其中一个实施例中，所述辅助导航数据包括：磁强计数据、GPS数据、WiFi指纹数据以及蓝牙数据。

关于基于图优化框架的行人惯性导航装置的具体限定可以参见上文中对于基于图优化框架的行人惯性导航方法的限定，在此不再赘述。上述基于图优化框架的行人惯性导航装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。