CN110146079A - 一种基于主副imu和气压计的三维行人导航方法 - Google Patents

Abstract

本发明提出一种基于主副IMU和气压计的三维行人导航方法，其步骤为：将主IMU安装于行人的足面，将副IMU和气压计分别安装于行人的胸前；主IMU测量的数据经惯性导航解算得到姿态、速度和位置，同时利用SVM决策函数判断行人所处的运动状态；副IMU测量的数据经方位解算得到姿态信息，气压计测量的数据经高度解算得到高度；根据行人运动状态，静止时，计算惯性导航的速度、角速度、姿态和高度误差，并通过扩展卡尔曼滤波器对误差进行估计和修正；运动时，通过主IMU获得行人的姿态、速度和位置，对运动轨迹实时更新。本发明根据副IMU和气压计得到的数据作为主IMU的参考数据进行误差修正，实现对行人的准确定位与导航跟踪。

Description

一种基于主副IMU和气压计的三维行人导航方法

技术领域

本发明涉及行人导航技术领域，特别是指一种基于主副IMU和气压计的三维行人导航方法。

背景技术

随着科学技术和信息化的快速发展，信息技术逐渐应用到人们的日常生活中，为人们的日常活动提供了更多的便利。在出行过程中，人们对于位置的需求以及路径的导航规划要求越来越高，甚至在每个时刻都想知道“我在哪儿？”、“从哪儿来到哪儿去？”，这就需要借助一些软件或者设备来确定。以前，大多采用指南针、电子罗盘等设备来确定自己的位置和方位，而现在主要依靠全球定位系统(Global Positioning System，GPS)或北斗卫星导航系统(BeiDou Navigation Satellite System，BDS)等无线导航定位系统来解决位置信息问题，以满足人们的位置需求。另外，计算机网络技术和智能控制技术的发展，极大地推动了物联网产业的兴起。以物联网及智能传感器为代表的新兴产业，获得了国家的政策扶持和大力推动。惯性器件与惯性导航定位技术作为物联网的重要组成部分，将在没有GPS或BDS等无线信号环境下的行人导航定位过程中发挥不可替代的作用。

行人导航(Pedestrian Navigation，PN)，是一种基于位置服务(Location BasedService，LBS)的技术，通过某种方法确定行人的位置。采用惯性导航技术的行人导航系统(Pedestrian Navigation System,PNS)，将IMU固定于行人足部、胸部或肩部等身上不同部位，通过实时测量行走过程中的加速度、角速度等数据参数，进行行人位置和方向的导航解算，从而对行人位置和方位进行定位和跟踪。将该系统应用到大型商场超市、图书馆、地下停车场、船舱、井下、隧道、密林等无线信号覆盖不到的盲区，能够实现各种不同区域下的导航定位。惯性导航技术在特定环境下进行导航定位具有抗干扰性强、实时性强和屏蔽性强等特点，因此该研究具有重要的社会意义和经济意义。

由于室内环境下没有GPS和BDS等无线导航信号，不能准确定位自己的位置。为了解决这个问题，采用不依赖于外部信号的MEMS惯性传感器作为自主导航元件，进行位置跟踪和导航服务。但是，由于MEMS惯性传感器存在零点漂移误差、运动状态误差和导航解算误差，导致位置和导航信息产生很大的偏差。

发明内容

针对现有的行人导航系统存在运动状态误差和导航解算误差，导致位置和导航信息产生较大误差的技术问题，本发明提出了一种基于主副IMU和气压计的三维行人导航系统及方法，利用胸前的副IMU和气压计测量得到的加速度、磁感应强度和气压数据，经方位解算和高度解算得到方位信息和高度信息，再将方位信息和高度信息作为足面的主IMU的参考数据进行误差修正，实现行人的准确导航和跟踪。

本发明的技术方案是这样实现的：

一种基于主副IMU和气压计的三维行人导航方法，其步骤如下：

S1、将主IMU安装于行人的足面，将副IMU和气压计分别安装于行人的胸前；

S2、主IMU通过其自身携带的加速度计和陀螺仪测量加速度和角速度数据，经惯性导航姿态解算得到姿态、位置和速度；副IMU通过其自身的加速度计和磁力计测量加速度和磁感应强度，经方位解算得到姿态信息；气压计测量的气压数据经高度解算得到高度信息；

S3、将行人的状态分为静止和运动两个状态；将步骤S2中主IMU得到的行人的加速度和角速度输入SVM决策方程，判断行人处于哪个状态；当行人处于静止状态时执行步骤S4，否则执行步骤S5；

S4、当行人处于静止状态时：初始化行人的角速度I、速度I和加速度I；主IMU测量的加速度II、角速度II和磁感应强度I经惯性导航解算得到行人的姿态、速度II和位置；副IMU测量的角速度III和磁感应强度II经方位解算得到行人的姿态信息；气压计采集的气压数据经高度解算得到行人的高度信息；将姿态与姿态信息作对比得到姿态角误差，将加速度II与加速度I作对比得到加速度误差，将角速度II与角速度I作对比得到角速度误差，将速度II与速度I作对比得到速度误差，将位置与高度信息作对比得到位置误差；再将姿态角误差、角速度误差、加速度误差、速度误差和位置误差输入扩展卡尔曼滤波器得到行人的航向误差、角速度误差、速度误差和高度误差；再将航向误差、角速度误差、速度误差和高度误差输入惯性导航解算中对行人的姿态、速度和位置进行实时反馈及修正，输出行人的姿态、速度和位置；

S5、当行人处于运动状态时：通过足面的主IMU测量的加速度和角速度经惯性导航解算获得行人的姿态、速度和位置，再对姿态、速度和位置进行坐标变换获得行人的运动轨迹；根据主IMU实时测量的数据对行人的姿态、速度和位置进行更新，进而对行人的运动轨迹进行更新。

优选地，所述步骤S3中的行人状态的判别方法为：

S31、选取一组已知运动状态的数据集作为样本训练集，样本训练集记为：

T＝{(x_i,y_i)}＝{(x₁,y₁),(x₂,y₂),…,(x_n,y_n)}，

其中，i＝1,2,…,n，x_i∈R^N，R^N为有理数集合，x_i为第i组主IMU提取的加速度数据和角速度数据组建的向量组，y_i∈{+1,-1}为第i组行人的运动状态，+1表示静止状态，-1表示运动状态；

S32、通过SVM分类算法对样本训练集进行训练，在n维数据空间中找到一个最优的超平面wx+b＝0，满足y_i(wx_i+b)≥1，使几何间隔最大，相应的分类决策函数为：

f(x)＝sign(w^Tx+b)，

其中，sign()为符号函数，f(x)等于y为行人的运动状态，b为截距，w为法向量。

优选地，所述步骤S4中行人处于静止状态时，采用的扩展卡尔曼滤波器的状态方程为：

X_k＝F_k,k-1X_k-1+W_k，

其中， 为导航坐标系下的姿态角误差，姿态角包括俯仰角、横滚角和航向角，δω_k为载体坐标系下的x、y和z三个方向的角速度误差，δr_k为导航坐标系下的E向、N向和U向三个方向的位置误差，δv_k为导航坐标系下的E向、N向和U向三个方向的速度误差，δa_k为载体坐标系下的E向、N向和U向三个方向的加速度误差， 为载体坐标系到导航坐标系的姿态转移矩阵，Δt为采样时间，为加速度的反对称矩阵，矩阵 为导航坐标系下的E向、N向和U向的加速度，I为单位矩阵，W_k为状态白噪声矩阵，W_k的均值为零、协方差矩阵为Q_k。

优选地，所述扩展卡尔曼滤波器的观测方程为：

Z_k＝H_kX_k+V_k，

其中， 为主IMU测量得到的航向角与副IMU测量得到的航向角之差，Δω_k为载体坐标系下的x、y和z三个方向的角速度误差，Δv_k为导航坐标系下的E向、N向和U向三个方向的速度误差，Δr_k为载体坐标系下的高度误差，V_k为观测白噪声矩阵，V_k服从均值为零、协方差矩阵为R_k的高斯分布。

一种基于主副IMU和气压计的三维行人导航系统，包括方位高度参考系统和行人足部导航系统，所述方位高度参考系统包括固定于行人胸前的副IMU和气压计；所述行人足部导航系统包括固定于行人足面的主IMU和控制器；所述主IMU、副IMU和气压计均与控制器相连接；所述主IMU包括加速度计I、陀螺仪I和磁力计I，所述副IMU包括加速度计II、陀螺仪II和磁力计II，加速度计I、陀螺仪I、磁力计I、加速度计II、陀螺仪II、磁力计II和气压计均与控制器相连接。

本发明产生的有益效果如下：

1.利用足面的主IMU获得行人的运动状态和航向信息，利用胸前的副IMU和气压计得到行人的方位信息和高度信息，以此作为参考数据对航向信息进行误差修正，实现三维行人导航定位。

2.通过SVM决策方程和零速检测方法判断行人的运动状态。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的主IMU、副IMU和气压计的位置示意图。

图2为本发明的行人导航系统示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，一种基于主副IMU和气压计的三维行人导航系统，包括方位高度参考系统和行人足部导航系统，所述方位高度参考系统包括固定于行人胸前的副IMU和气压计；所述行人足部导航系统包括固定于行人足面的主IMU和控制器；所述主IMU、副IMU和气压计分别与控制器相连接；所述主IMU包括加速度计I、陀螺仪I和磁力计I，所述副IMU包括加速度计II、陀螺仪II和磁力计II，加速度计I、陀螺仪I、磁力计I、加速度计II、陀螺仪II、磁力计II和气压计均与控制器相连接。

如图2所示，一种基于主副IMU和气压计的三维行人导航方法，其步骤如下：

S1、将主IMU安装于行人的足面，将副IMU和气压计分别安装于行人的胸前。

S2、主IMU通过其自身携带的加速度计和陀螺仪测量得到的数据经惯性导航姿态解算得到姿态、位置和速度；副IMU通过其自身的加速度计和磁强计测量得到加速度和磁感应强度经方位解算得到姿态信息，气压计测量的气压数据经高度解算得到高度信息。

S3、将行人的状态分为静止和运动两个状态；将步骤S2中主IMU得到的行人的加速度和角速度输入SVM决策方程，判断行人处于哪个状态。当行人处于静止状态时执行步骤S4，否则执行步骤S5。

所述SVM决策方程的训练过程为：

S31、选取一组已知运动状态的数据集作为样本训练集，样本训练集记为：

T＝{(x_i,y_i)}＝{(x₁,y1),(x₂,y2),…,(x_n,yn)}，

其中，i＝1,2,…,n，x_i∈R^N，R^N为有理数集合，x_i为第i组主IMU提取的加速度数据和角速度数据组建的向量组，y_i∈{+1,-1}，为第i组行人的运动状态，+1表示静止状态，-1表示运动状态。

S32、通过SVM分类算法对样本训练集的学习和训练，在n维数据空间中找到一个最优的超平面wx+b＝0，满足y_i(wx_i+b)≥1，使几何间隔最大，相应的分类决策函数为：

f(x)＝sign(w^Tx+b)，

其中，sign()为符号函数，f(x)等于y为行人的运动状态，b为截距，w为法向量。

S4、当行人处于静止状态时：初始化行人的角速度I、速度I和加速度I，角速度I和速度I的初值为零；主IMU测量的加速度II、角速度II和磁感应强度I经惯性导航解算得到行人的姿态、速度II和位置；副IMU测量的角速度III和磁感应强度II经方位解算得到行人的姿态信息；气压计采集的气压数据经高度解算得到行人的高度信息；将姿态与姿态信息作对比得到姿态角误差，将加速度II与加速度I作对比得到加速度误差，将角速度II与角速度I作对比得到角速度误差，将速度II与速度I作对比得到速度误差，将位置与高度信息作对比得到位置误差；再将姿态角误差、角速度误差、加速度误差、速度误差和位置误差输入扩展卡尔曼滤波器得到行人的航向误差、角速度误差、速度误差和高度误差；再将航向误差、角速度误差、速度误差和高度误差输入惯性导航解算中对行人的姿态、速度和位置进行实时反馈及修正，输出行人的姿态、速度和位置。

S41、所述惯性导航解算的输入为主IMU测量的角速度II和加速度II，经过坐标变换输出为姿态、速度和位置。

S42、所述方位解算的输入为副IMU测量的角速度III和磁感应强度II，经过坐标变换输出为姿态信息。

S43、所述高度解算的输入为气压计测量的气压值，利用如下公式输出为高度信息：

h(k)＝44330*(1-(p(k)/p₀)^1/5.255)，

其中，p₀为标准气压值，p₀＝101.325kPa；p(k)为k时刻气压计测量的气压值，h(k)为k时刻的高度。

所述扩展卡尔曼滤波器的状态方程为：

X_k＝F_k,k-1X_k-1+W_k，

其中， 为导航坐标系下的姿态角误差，姿态角包括俯仰角、横滚角和航向角，δω_k为载体坐标系下的x、y和z三个方向的角速度误差，δr_k为导航坐标系下的E向、N向和U向三个方向的位置误差，δv_k为导航坐标系下的E向、N向和U向三个方向的速度误差，δa_k为载体坐标系下的E向、N向和U向三个方向的加速度误差， 为载体坐标系到导航坐标系的姿态转移矩阵，Δt为采样时间，为加速度的反对称矩阵， 为导航坐标系下的E向、N向和U向的加速度，I为单位矩阵，W_k为状态白噪声矩阵，W_k的均值为零、协方差矩阵为Q_k。

所述扩展卡尔曼滤波器的观测方程为：

Z_k＝H_kX_k+V_k，

其中， 为主IMU测量得到的航向与副IMU测量得到的航向之差，Δω_k为载体坐标系下的x、y和z三个方向的角速度误差，Δv_k为导航坐标系下的E向、N向和U向三个方向的速度误差，Δr_k为载体坐标系下的高度误差，V_k为观测白噪声矩阵，V_k服从均值为零、协方差矩阵为R_k的高斯分布。

S5、当行人处于运动状态时：通过足面的主IMU测量的加速度和角速度经惯性导航解算获得行人的姿态、速度和位置，再对姿态、速度和位置进行坐标变换获得行人的运动轨迹；根据主IMU实时测量的数据对行人的姿态、速度和位置进行更新，进而对行人的运动轨迹进行更新。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于主副IMU和气压计的三维行人导航方法，其特征在于，其步骤如下：

S1、将主IMU安装于行人的足面，将副IMU和气压计分别安装于行人的胸前；

S2、主IMU通过其自身携带的加速度计和陀螺仪测量加速度和角速度数据，经惯性导航姿态解算得到姿态、位置和速度；副IMU通过其自身的加速度计和磁力计测量加速度和磁感应强度，经方位解算得到姿态信息；气压计测量的气压数据经高度解算得到高度信息；

S3、将行人的状态分为静止和运动两个状态；将步骤S2中主IMU得到的行人的加速度和角速度输入SVM决策方程，判断行人处于哪个状态；当行人处于静止状态时执行步骤S4，否则执行步骤S5；

S4、当行人处于静止状态时：初始化行人的角速度I、加速度I和速度I；主IMU测量的加速度II、角速度II和磁感应强度I经惯性导航解算得到行人的姿态、速度II和位置；副IMU测量的角速度III和磁感应强度II经方位解算得到行人的姿态信息；气压计采集的气压数据经高度解算得到行人的高度信息；将姿态与姿态信息作对比得到姿态角误差，将加速度II与加速度I作对比得到加速度误差，将角速度II与角速度I作对比得到角速度误差，将速度II与速度I作对比得到速度误差，将位置与高度信息作对比得到位置误差；再将姿态角误差、角速度误差、加速度误差、速度误差和位置误差输入扩展卡尔曼滤波器得到行人的航向误差、角速度误差、速度误差和高度误差；再将航向误差、角速度误差、速度误差和高度误差输入惯性导航解算中对行人的姿态、速度和位置进行实时反馈及修正，输出行人的姿态、速度和位置；

S5、当行人处于运动状态时：通过足面的主IMU测量的加速度和角速度经惯性导航解算获得行人的姿态、速度和位置，再对姿态、速度和位置进行坐标变换获得行人的运动轨迹；根据主IMU实时测量的数据对行人的姿态、速度和位置进行更新，进而对行人的运动轨迹进行更新。

2.根据权利要求1所述的基于主副IMU和气压计的三维行人导航方法，其特征在于，所述步骤S3中的行人状态的判别方法为：

S31、选取一组已知运动状态的数据集作为样本训练集，样本训练集记为：

T＝{(x_i,y_i)}＝{(x₁,y₁),(x₂,y₂),…,(x_n,y_n)}，

其中，i＝1,2,…,n，x_i∈R^N，R^N为有理数集合，x_i为第i组主IMU提取的加速度数据和角速度数据组建的向量组，y_i∈{+1,-1}为第i组行人的运动状态，+1表示静止状态，-1表示运动状态；

S32、通过SVM分类算法对样本训练集进行训练，在n维数据空间中找到一个最优的超平面wx+b＝0，满足y_i(wx_i+b)≥1，使几何间隔最大，相应的分类决策函数为：

f(x)＝sign(w^Tx+b)，

其中，sign()为符号函数，f(x)等于y为行人的运动状态，b为截距，w为法向量。

3.根据权利要求1所述的基于主副IMU和气压计的三维行人导航方法，其特征在于，所述步骤S4中行人处于静止状态时，采用的扩展卡尔曼滤波器的状态方程为：

X_k＝F_k,k-1X_k-1+W_k，

其中， 为导航坐标系下的姿态角误差，姿态角包括俯仰角、横滚角和航向角，δω_k为载体坐标系下的x、y和z三个方向的角速度误差，δr_k为导航坐标系下的E向、N向和U向三个方向的位置误差，δv_k为导航坐标系下的E向、N向和U向三个方向的速度误差，δa_k为载体坐标系下的E向、N向和U向三个方向的加速度误差， 为载体坐标系到导航坐标系的姿态转移矩阵，Δt为采样时间，为加速度的反对称矩阵，矩阵 为导航坐标系下的E向、N向和U向的加速度，I为单位矩阵，W_k为状态白噪声矩阵，W_k的均值为零、协方差矩阵为Q_k。

4.根据权利要求3所述的基于主副IMU和气压计的三维行人导航方法，其特征在于，所述扩展卡尔曼滤波器的观测方程为：

Z_k＝H_kX_k+V_k，

其中， 为主IMU测量得到的航向角与副IMU测量得到的航向角之差，Δω_k为载体坐标系下的x、y和z三个方向的角速度误差，Δv_k为导航坐标系下的E向、N向和U向三个方向的速度误差，Δr_k为载体坐标系下的高度误差，V_k为观测白噪声矩阵，V_k服从均值为零、协方差矩阵为R_k的高斯分布。

5.一种基于主副IMU和气压计的三维行人导航系统，包括方位高度参考系统和行人足部导航系统，其特征在于，所述方位高度参考系统包括固定于行人胸前的副IMU和气压计；所述行人足部导航系统包括固定于行人足面的主IMU和控制器；所述主IMU、副IMU和气压计均与控制器相连接；所述主IMU包括加速度计I、陀螺仪I和磁力计I，所述副IMU包括加速度计II、陀螺仪II和磁力计II，加速度计I、陀螺仪I、磁力计I、加速度计II、陀螺仪II、磁力计II和气压计均与控制器相连接。