[发明专利]基于图优化框架的行人惯性导航方法和装置

权利要求书

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

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

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

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

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

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

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

所述根据所述全局优化结果和所述惯导解算结果进行惯导参数优化，包括：

将全局优化结果作为观测量，则观测的误差为：

式中，z为全局优化结果，h为观测方程：

x为前一状态的位姿估计，u为MIMU导航数据输入，p为惯导解算结果，考虑其他时刻的观测量，并且给每个误差设一个下标，那么整体的代价函数为：

对代价函数进行最小二乘进行求解，得到对惯导参数进行优化的结果。

2.根据权利要求1所述的方法，其特征在于，所述对获取的MIMU导航数据和辅助导航数据进行数据处理，确定变量节点，以及得到变量节点位置中MIMU导航数据对应的位置信息和辅助导航数据对应的全局因子，包括：

基于加速度数据对获取的MIMU导航数据和辅助导航数据进行数据处理，得到位置信息和姿态更新频率；

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

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

3.根据权利要求1所述的方法，其特征在于，所述位置信息包括：位置、速度以及四元数方向；

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

根据所述位置、速度以及四元数方向，确定卡尔曼滤波的过滤器状态；

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

4.根据权利要求3所述的方法，其特征在于，根据所述MIMU导航数据的步态特征，进行零速检测，得到卡尔曼滤波的虚拟观测量，包括：

根据所述MIMU导航数据的步态特征，调节惯导参数；

根据所述惯导参数进行零速检测，得到卡尔曼滤波的虚拟观测量。

5.根据权利要求4所述的方法，其特征在于，根据所述全局优化结果和所述惯导解算结果进行惯导参数优化，包括：

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

6.根据权利要求1至5任一项所述的方法，其特征在于，所述辅助导航数据包括：磁强计数据、GPS数据、WiFi指纹数据以及蓝牙数据。

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

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

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

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

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

惯导优化模块还用于将全局优化结果作为观测量，则观测的误差为：

式中，z为全局优化结果，h为观测方程：

x为前一状态的位姿估计，u为MIMU导航数据输入，p为惯导解算结果，考虑其他时刻的观测量，并且给每个误差设一个下标，那么整体的代价函数为：

对代价函数进行最小二乘进行求解，得到对惯导参数进行优化的结果。