CN111189443B - 一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法 - Google Patents

Abstract

本发明公开了一种在线校准步长、运动偏差角修正和自适应能量管理的行人导航方法，属于行人导航领域。该方法首先在初始化阶段基于滤波进行位置姿态更新，然后在此基础上对步长模型进行标定并基于滤波进行行人运动方向修正；最后在初始化阶段结束后进行步长加方向的位置推算。在此过程中提出一套自适应的能量管理测略：增加陀螺休眠机制，满足特定条件下进行唤醒；在初始化阶段选择高采样率，进入正式的位置推算阶段进行降采样处理。通过使用本方法进行行人导航位置推算，避免了离线校准，并且对行人运动方向进行修正，同时使用自适应能量管理策略，有效降低了能耗。

Description

一种在线校准步长、修正运动偏差角和自适应能量管理的行 人导航方法

技术领域

本发明属于行人导航技术领域，具体是一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法。

背景技术

近几十年来，随着微电子机械(MEMS)技术的发展，MEMS惯性传感器被广泛应用于行人导航。由三轴陀螺仪和三轴加速度计构成的MEMS惯性传感器通常附着在人体的某一部位上，用于运动监测或生物医学等目的。现有的基于惯性器件技术的行人导航代表性方法主要可分为两类。一类是基于捷联惯导的力学编排计算方法。由于受陀螺仪和加速度计的固有漂移与零偏影响，捷联惯性系统的导航误差迅速增大，不可避免地导致结果发散。为了解决这一问题，人们对零速更新(ZUPT)方法进行了大量的研究，将零速视为行走姿态阶段的伪测量引入其中。ZUPT被证明能够有效地补偿惯性传感器漂移并重置系统部分累积误差。为了保持良好的航向精度和抑制陀螺仪的垂向漂移，磁力计越来越普遍地与惯性传感器在大规模低成本装置上集成，被称为MARG传感器，包含磁强计、角速率和重力传感器阵列。为了最佳地融合MARG传感器数据，发展了各种线性或非线性卡尔曼滤波器(KF)如传统KF、扩展KF、无味KF和粒子KF，用以估计一个人的位置和方向。另一类方法是基于航位推算的行人导航方法。它递推地在每个检测步骤中计算行人步长和行走方向。然而，现有的航位推算法在使用前需要用足够的个人离线数据集校准步长参数。这一点是它作为产品便捷使用的主要障碍。此外，尽管有很多学者在汽车导航领域已经讨论并修正了由于安装的传感器框架和定义的车身框架不一致而导致的各种类型的失调角，然而目前还没有文献对步行导航中与人的步态相关的航向偏差角补偿问题进行研究。值得指出的是，这种航向偏差角因人和步态而异。能量管理是可穿戴设备的另一个关键问题。对于行人导航应用而言，与加速度计或磁强计相比，陀螺仪消耗的能量要更为显著。例如，根据STMicroelectronics公司的官方用户手册，其典型产品MEMS陀螺仪LSGD20和加速度计LIS3DH，前者工作电流约6.1毫安，而后者仅约为11为微安。

发明内容

针对上述问题，本发明提出一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法。该发明主要解决三个问题：第一，针对不同使用者的不同运动行为涉及步长模型进行在线标定；第二，对行人导航中与步态相关的运动方向偏差进行修正；第三，自适应能量管理策略：在推算过程中基于加速度信息进行运动行为智能检测与分类，根据运动行为自适应调整系统采样率；并引入陀螺休眠机制，在检测到明显运动行为变化后进行唤醒，达到降低能耗的目的。

本发明的技术方案为：一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法，具体包括以下步骤：

步骤1：采用MARG传感器获取行人位置、速度、姿态数据；

步骤2：行人开始运动后进入“捷联惯导推算”阶段，基于惯性推算方法进行行人位置姿态更新；

步骤3：进行行人步态检测，以每一步为单位进行分割；

步骤4：进行行人步长在线标定；

步骤5：进行行人航向偏差角修正；

步骤6：检测总位移是否达预设长度，如否，则转至步骤2；如是，MARG传感器中陀螺仪进行休眠，转至步骤7；

步骤7：进入“步态航位推算”阶段位置推算。

进一步的，所述步骤2的具体方法为：

在每一时刻接收到MARG元件数据后，进行位置速度姿态更新，具体如下：

进行k时刻的位置

速度

姿态

推算：

上标n表示导航坐标系n系，下标k表示第k时刻，

表示从b系到n系的姿态旋转矩阵，f表示比力，I₃表示3维的单位阵，Δt表示采样时间间隔，g表示重力加速度，v表示速度矢量，ω表示角速度，加速度计与陀螺仪的测量误差将导致位置、速度、姿态误差逐渐积累，通过构建滤波器进行实时误差估计与补偿x^I：

其中上标T表示矩阵转置，

表示失准角，δp表示位置误差，δv表示速度误差，δω表示角速率偏差，δa表示加速度偏差；

建立滤波状态方程：

其中，w表示过程噪声，服从高斯分布w～N(0,Q)，转移矩阵

为

进行一步预测：

P表示位置矢量，建立量测更新方程：

其中H表示观测涉及矩阵，ε表示观测噪声，服从高斯分布ε～N(0,R)；

基于零速检测结果及磁力计解算的偏航角，确定量测矢量

与量测设计矩阵

其中，ψ_mag,k表示借助磁力计计算的航向角，ψ_gyro,k表示借助陀螺仪计算的航向角，D表示偏航误差角与失准角间的转移关系，D＝[-sinψtanγ-cosψtanγ1]，γ表示姿态俯仰角；

进行量测更新，得到最终状态估计量及其方差：

其中，K表示滤波器增益矩阵；

将误差量返回公式(1)中进行修正，由此完成了一个周期的位置更新。

进一步的，所述步骤7的具体步骤为：

步骤7-1：基于加速度数据进行步态检测与分割；

步骤7-2：对行为模式进行检测，如检测到该模式已标定，则执行7-3，否则执行步骤2；

步骤7-3：利用标定完成的步长公式计算每一步的步长；

步骤7-4：借助磁力计计算每一步的偏航角；

步骤7-5：对每一步的航向进行偏差修正，得到实际运动方向；

步骤7-6：利用每一步的步长与修正后的方向，进行位置推算。

本发明公开了一种在线校准步长、运动偏差角修正和自适应能量管理的行人导航方法。该方法在初始化阶段基于惯性推算进行位置姿态更新，在此基础上对步长模型进行标定；基于滤波的行人运动方向修正；在初始化阶段结束后进行步长加方向方法的位置推算；并提出一套自适应的能量管理测略：增加陀螺休眠机制，只有满足条件时才进行唤醒；在初始化阶段选择高采样率，进入正式的位置推算阶段则进行降采样处理。通过使用本方法进行行人导航位置推算，避免了离线校准，通过对与步态相关的运动方向进行修正，提高导航推算精度，同时使用自适应能量管理策略，有效节省系统能耗。

附图说明

图1是工作模式流程图。

图2是行人步态方向偏差角示意图。

图3是工作模式切换图。

具体实施方式

下面结合附图，对本发明中的在线校准步长、运动偏差角修正和自适应能量管理的行人导航方法进行详细说明：

为描述方便，对下文出现的符号定义如下：

表1角标定义

表2符号定义

步骤1：安装好MARG传感器后，静止数秒，用于传感器数据统计特征分析及直流分量确定。

步骤2：开始运动后，基于零速检测辅助的力学编排惯性推算位置姿态，此阶段记为“惯导推算”阶段，具体步骤如下：

在每一时刻接收到MARG元件数据后，进行位置速度姿态更新，具体如下：

进行k时刻的位置

速度

姿态

推算：

加速度计与陀螺仪的测量误差将导致位置、速度、姿态误差逐渐积累，通过构建滤波器进行实时误差估计与补偿：

建立滤波状态方程：

其中，转移矩阵为

进行一步预测：

建立量测更新方程：

基于零速检测结果及磁力计解算的偏航角，确定量测矢量与量测设计矩阵：

其中，D表示偏航误差角与失准角间的转移关系，取D＝[0 0 1]表示进行强修正。

进行量测更新，得到最终状态估计量及其方差：

将误差量返回公式(1)中进行修正，由此完成了一个周期的位置更新；

步骤3：在“捷联惯导推算”阶段过程中进行步态检测与每一步的分割，确定分割节点：对加速度数据进行FIR滤波，移除直流分量；自适应设置低、中、高门限值，对每一步进行划分；

步骤4：计算每一步步频f_s与FIR滤波后每一步加速度均值

利用每一步的分割节点与“捷联惯导推算”阶段计算的位移长度，对步长公式参数进行标定；针对不同运动模式给出相关步长公式如下：

其中，d_W,i为第i步走路步长，d_R,i为第i步跑步步长，K_j为待估参数，j＝0,1,2,3,4，利用最小二乘法进行求解；

步骤5：利用每一步的分割节点与“捷联惯导推算”阶段的位置、姿态信息进行航向偏差角修正，步态相关的偏差角如图2所示，其具体步骤如下：

对航向偏差角一阶导数作如下假设关系：

取滤波状态矢量

状态模型可写为：

状态转移矩阵为

建立观测模型：

其中，观测设计矩阵

观测矢量

θ_i为第i步的实际运动方向，根据每一步的运动位移方向、速度方向，其计算公式可选：

其中，Δp_x,i,Δp_y,i分别为捷联惯导解算的第i步x、y方向位移，

分别为第i步x、y方向速度的均值；ψ_i为捷联惯导解算的第i步落地时的偏航角；修正方法与步骤2中修正方法完全相同；

根据步长节点划分，每一步结束进行一次修正，在初始化结束阶段获得最终方向偏差估计值δθ；

步骤6：检测总位移是否达预设长度(实验中取50米)，如否，则表明仍未完成步长标定与方向修正，转至步骤2继续进行标定；如是，则表明标定完成，“捷联惯导推算”阶段结束，工作模式切换至“步态航位推算”阶段，转至步骤7；

步骤7：完成步长校准与方向修正后，初始化阶段结束，进入“步态航位推算”阶段，其具体步骤如下：

步骤7-1：基于加速度数据进行步态检测与每一步分割；

步骤7-2：对行为模式进行检测，如检测到该模式已标定，则执行7-3，否则转至步骤2；

步骤7-3：利用标定完成的步长公式计算每一步的步长d；

步骤7-4：借助磁力计计算每一步的偏航角ψ；

步骤7-5：对每一步的航向进行偏差修正，得到实际运动方向θ_i＝ψ_i+δθ；

步骤7-6：利用每一步的步长与方向，进行位置推算：

整个行人定位过程中引入自适应的能量管理机制，降低了能耗；

使用两种工作模式：惯性推算能耗较高，只有在步长初始化与运动偏差角估计即“捷联惯导推算”阶段采用此方式，一旦初始化完成则切换至“步态航位推算”模式；

引入陀螺休眠机制：陀螺的能耗远高于加速度计、磁力计，为节省能量，陀螺为休眠状态，只有在满足特定情况时才进行唤醒：在“捷联惯导推算”阶段进行位置推算、步长标定与方向偏差角估计时进行陀螺仪唤醒；利用加速度序列检测到静止状态、或工作模式处于“步态航位推算”阶段则进行休眠。

采样率调整机制：在“捷联惯导推算”阶段进行惯性推算时需要保证高采样率，一旦转至“步态航位推算”模式，采样率降低，节省了能量。

本发明属于行人导航技术领域，能够针对不同用户的不同运动行为模式实现在线的步长标定，避免离线标定的繁复操作；对步态相关的运动航向误差角进行估计，减小位置推算误差；具有自适应的能量管理机制，最大程度降低能耗，有利于行人导航算法在小型化智能穿戴设备上应用。

Claims (2)

1.一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法，具体包括以下步骤：

步骤1：采用MARG传感器获取行人位置、速度、姿态数据；

步骤2：行人开始运动后进入“捷联惯导推算”阶段，基于惯性推算方法进行行人位置姿态更新；

在每一时刻接收到MARG元件数据后，进行位置速度姿态更新，具体如下：

进行k时刻的位置

速度

姿态

推算：

上标n表示导航坐标系n系，下标k表示第k时刻，

表示从b系到n系的姿态旋转矩阵，f表示比力，I₃表示3维的单位阵，Δt表示采样时间间隔，g表示重力加速度，v表示速度矢量，ω表示角速度，加速度计与陀螺仪的测量误差将导致位置、速度、姿态误差逐渐积累，通过构建滤波器进行实时误差估计与补偿x^I：

其中上标T表示矩阵转置，

表示失准角，δp表示位置误差，δv表示速度误差，δω表示角速率偏差，δa表示加速度偏差；

建立滤波状态方程：

其中，w表示过程噪声，服从高斯分布w～N(0,Q)，转移矩阵

为

进行一步预测：

P表示位置矢量，建立量测更新方程：

其中H表示观测涉及矩阵，ε表示观测噪声，服从高斯分布ε～N(0,R)；

基于零速检测结果及磁力计解算的偏航角，确定量测矢量

与量测设计矩阵

其中，ψ_mag,k表示借助磁力计计算的航向角，ψ_gyro,k表示借助陀螺仪计算的航向角，D表示偏航误差角与失准角间的转移关系，D＝[-sinψtanγ -cosψtanγ 1]，γ表示姿态俯仰角；

进行量测更新，得到最终状态估计量及其方差：

其中，K表示滤波器增益矩阵；

将误差量返回公式(1)中进行修正，由此完成了一个周期的位置更新；

步骤3：进行行人步态检测，以每一步为单位进行分割；

步骤4：进行行人步长在线标定；

步骤5：进行行人航向偏差角修正；

对航向偏差角一阶导数作如下假设关系：

取滤波状态矢量

状态模型可写为：

状态转移矩阵为

建立观测模型：

其中，观测设计矩阵

观测矢量

θ_i为第i步的实际运动方向，根据每一步的运动位移方向、速度方向，其计算公式可选：

其中，Δp_x,i,Δp_y,i分别为捷联惯导解算的第i步x、y方向位移，

分别为第i步x、y方向速度的均值；ψ_i为捷联惯导解算的第i步落地时的偏航角；修正方法与步骤2中修正方法完全相同；

根据步长节点划分，每一步结束进行一次修正，在初始化结束阶段获得最终方向偏差估计值δθ；

步骤6：检测总位移是否达预设长度，如否，则转至步骤2；如是，MARG传感器中陀螺仪进行休眠，转至步骤7；

步骤7：进入“步态航位推算”阶段位置推算。

2.如权利要求1所述的一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法，其特征在于所述步骤7的具体步骤为：

步骤7-1：基于加速度数据进行步态检测与分割；

步骤7-2：对行为模式进行检测，如检测到该模式已标定，则执行7-3，否则执行步骤2；

步骤7-3：利用标定完成的步长公式计算每一步的步长；

步骤7-4：借助磁力计计算每一步的偏航角；

步骤7-5：对每一步的航向进行偏差修正，得到实际运动方向；

步骤7-6：利用每一步的步长与修正后的方向，进行位置推算。

Images