一种基于侧向速度约束优化的行人导航定位方法与流程

本发明属于定位导航技术领域，具体涉及一种基于侧向速度约束优化的行人导航定位方法。

背景技术：

随着人们生活水平的提高，国内外掀起了研究行人导航定位技术的热潮。在开阔的室外环境下，行人可以借助gps进行定位导航，而在gps信号据止的室内环境下，一方面可以基于uwb、wifi、蓝牙等无线射频设备，成本较高且不够便捷；另一方面可基于惯性传感器进行航位推算或捷联解算，实现成本低但会形成累计误差，导致定位结果发散。

目前，针对基于惯性传感器的行人航位推算算法主要有步态检测、步长估计与航向推算三个方面，但传统的步长估计方法适用性较差，且存在航向累计误差，难以保证高精度的行人定位导航要求。

技术实现要素：

发明目的：为了克服现有技术中存在的不足，本发明提供一种基于侧向速度约束优化的行人导航定位方法，准确检测行人步态并估计步长，有效估计行人导航结果误差与传感器误差，抑制了惯性导航系统的误差发散。

技术方案：为实现上述目的，本发明采用的技术方案为：

一种基于侧向速度约束优化的行人导航定位方法，将惯性传感器水平固定放置在行人手掌上，定位方法包括如下步骤：

步骤1)采集并分析行人运动时的运动特性并进行姿态解算，其中姿态解算采用四元数方法解算；所述运动特性包括行人步行时的加速度、角速度及线速度特性；姿态结算得出的数据结果为传感器的姿态角即横滚角、俯仰角及航向角；

步骤2)建立行人运动步态检测模型，其中模型利用经过递推平滑方法处理加速度模值作为检测步态的条件量；其中，所述加速度模值为三轴加速度平方求和再开方，常用在惯性导航技术领域。

步骤3)利用步态检测所得到步态特征量作为神经网络的输入，将每步步长作为输出，构建基于bp神经网络的步长估计模型，其中训练样本中的步长信息由高精度的差分gps位置计算所得；

步骤4)建立基于侧向速度约束的滤波方法，其中利用卡尔曼滤波进行误差估计进而修正行人定位导航过程中的累积误差。

进一步的，所述步骤1)中：行人运动过程中人体存在与地面相对静止的区间，即零速区间，在此区间内行人理论运动速度为零；行人在行走或跑步状态下，不考虑侧向运动，即在垂直于行人运动方向的侧向方向上运动速度为0。

进一步的，所述步骤2)中建立行人运动步态检测模型的具体内容为：

首先对加速度模值进行递推平滑处理：

其中：fix、fiy、fiz为第i时刻加速度计采集得到的比力信息，fk为第k时刻加速度模值，n为滑动窗口大小；

采用检测加速度模值的谷值fk,valley与峰值fk,peak作为步态检测条件，模型具体如下：

1)检测加速度模值峰值：fk,peak>thpeak，状态标志位为1；

2)检测加速度模值谷值：fk,valley>thvalley，上一个峰值与此谷值时间差δtk,peak,valley满足δtk,peak,valley>thpeak,valley，状态标志位为2；

3)在2)的基础上，检测下一个加速度模值峰值，上一个谷值与此峰值时间差δtk,valley,peak满足δtk,valley,peak>thvalley,peak，状态标志位为3；

4)当状态标志位为3时，代表成功检测到一步，并将状态标志位为1；

其中，thpeak为检测峰值的阈值，thvalley为检测谷值的阈值，thpeak,valley为上一个峰值与此谷值时间差的阈值，thvalley,peak为上一个谷值与此峰值时间差的阈值；

在检测到峰值的基础上，继续寻找第一个小于静止状态下加速度模值的对应时刻，即零速时刻，令零速标志位为1。

进一步的，所述步骤3)中建立一种基于神经网络的步长估计方法模型为：

构建4输入1输出与12个隐藏节点的神经网络，4个输入为行人运动一步过程中的加速度模值峰值fk,peak、加速度模值谷值fk,valley、峰谷值之差fk,peak-fk,valley以及两相邻峰值之间的时间差δtk,peak,valley+δtk,valley,peak，1个输出为该步步长；

训练阶段：利用同步采集的加速度模值信息以及由差分gps位置求得的对应的步长信息对该bp神经网络进行训练，形成神经网络；

测试阶段：输入实时加速度模值峰值fk,peak、加速度模值谷值fk,valley、峰谷值之差以及两相邻峰值之间的时间差，利用训练好的神经网络输出实时的估计步长。

进一步的，所述步骤4)中建立基于侧向速度约束的滤波方法为：

构建9维的卡尔曼滤波器状态量，具体如下：

其中，为航向误差角，[δve；δvn]为东、北方向的速度误差；[δλ；δl]为水平位置误差；[εbz；εrz]为z轴陀螺仪的零偏与一阶马尔科夫过程随机误差；[▽rx；▽ry]为x轴与y轴加速度计一阶马尔科夫过程随机误差；x是状态量，t是采样周期；

状态方程为：其中a为状态转移矩阵，g为系统噪声矩阵，w为白噪声随机误差矢量，是状态量的一阶导数；

量测量为侧向速度，由航向角与东向北向速度得到，即其中，vce是人体侧向速度，ve是东向速度，δve是东向速度误差，是航向角，vn是北向速度，δvn是北向速度误差；

量测方程为：z＝vce-vreal＝hx+v，其中vreal为理论上行人的侧向速度，即为0；h为量测阵，v为量测噪声，z是量测量。有益效果：本发明提供的一种基于侧向速度约束优化的行人导航定位方法，与现有技术相比，具有以下优势：本发明首先分析行人的运动特性，研究步态检测方法判断行人步态与零速区间。基于此，若判断出当前状态处于零速区间，则将速度置零，并提出一种基于神经网络的步长估计方法进行步长估计，提高了步长估计的精度；反之，则进行速度推算，并提出一种基于侧向速度约束的滤波方法，减少行人导航定位的累积误差，提高行人导航定位的精度。

本发明适用于行人导航，适用于行人多种运动状态，利用消费级的惯性传感器也可以较理想地解决定位精度下降的问题，具有极高的工程应用和商业价值。

附图说明

图1为本发明定位算法流程图。

图2为本发明与传统方法对比图。

具体实施方式

下面结合附图和实施例对本发明作更进一步的说明。

如图1所示，本发明一种基于侧向速度约束优化的行人导航定位方法包括如下步骤：

步骤1，采集并分析行人运动时的运动特性，行人运动过程中人体存在与地面相对静止的区间，即零速区间，在此区间内行人理论运动速度为零。另外，行人在行走或跑步状态下，一般不考虑侧向运动，即在垂直于行人运动方向的侧向方向上运动速度为0。将加速度计与陀螺仪输出进行预处理，即扣除加速度计与陀螺仪的零偏，并利用四元数方法进行姿态解算，可参考：刘建业.导航系统理论与应用[m].西北工业大学出版社,2010。

步骤2，建立行人运动步态检测模型，首先对加速度模值进行递推平滑处理：

其中：fix、fiy、fiz为第i时刻加速度计采集得到的比力信息，fk为第k时刻加速度模值，n为滑动窗口大小。

根据加速度模值变化趋势可知，行人运动周期性明显，本发明采用检测加速度模值的谷值fk,valley与峰值fk,peak作为步态检测条件，模型具体如下：

1)检测加速度模值峰值：fk,peak>thpeak，状态标志位为1；

2)检测加速度模值谷值：fk,valley>thvalley，上一个峰值与此谷值时间差δtk,peak,valley满足δtk,peak,valley>thpeak,valley，状态标志位为2；

3)在2)的基础上，检测下一个加速度模值峰值，上一个谷值与此峰值时间差δtk,valley,peak满足δtk,valley,peak>thvalley,peak，状态标志位为3；

4)当状态标志位为3时，代表成功检测到一步，并将状态标志位为1；

其中，thpeak为检测峰值的阈值，thvalley为检测谷值的阈值，thpeak,valley为上一个峰值与此谷值时间差的阈值，thvalley,peak为上一个谷值与此峰值时间差的阈值；

在检测到峰值的基础上，继续寻找第一个小于静止状态下加速度模值的对应时刻，即零速时刻，令零速标志位为1。

步骤3，判断步态是否处于零速时刻，若处于零速时刻则将速度置零，并建立一种基于神经网络的步长估计方法，具体为4输入1输出与12个隐藏节点的神经网络，4个输入为行人运动一步过程中的加速度模值峰值、加速度模值谷值、峰谷值之差以及两相邻峰值之间的时间差，1个输出为该步步长；

训练阶段：利用同步采集的加速度模值信息以及由差分gps位置求得的对应的步长信息对该bp神经网络进行训练，形成神经网络；

测试阶段：输入实时加速度模值峰值、加速度模值谷值、峰谷值之差以及两相邻峰值之间的时间差，利用训练好的神经网络输出实时的估计步长。

在准确估计步长的基础上，则进行行人位置推算。

步骤4，若处于非零速时刻则将进行速度递推，并建立基于侧向速度约束的滤波方法，利用卡尔曼滤波进行误差估计进而修正行人定位导航过程中的累积误差。构建9维的卡尔曼滤波器状态量，具体如下：

其中，为航向误差角，[δve；δvn]为东、北方向的速度误差；[δλ；δl]为水平位置误差；[εbz；εrz]为z轴陀螺仪的零偏与一阶马尔科夫过程随机误差；为x轴与y轴加速度计一阶马尔科夫过程随机误差；x是状态量，t是采样周期；

状态方程为：其中a为状态转移矩阵，g为系统噪声矩阵，w为白噪声随机误差矢量，是状态量的一阶导数；

量测量为侧向速度，由航向角与东向北向速度得到，即其中，vce是人体侧向速度，ve是东向速度，δve是东向速度误差，是航向角，vn是北向速度，δvn是北向速度误差；

量测方程为：z＝vce-vreal＝hx+v，其中vreal为理论上行人的侧向速度，即为0；h为量测阵，v为量测噪声；z是量测量。

实施例

如图2所示，实验场景为实验楼内的走廊，路线为长度为54米的直线，利用传统的行人航位推算方法解算出来的结果如图虚线所示，终点误差为2.8米；利用本发明即一种基于侧向速度约束优化的行人导航定位方法解算出来的结果如图实线所示，终点误差为0.4米，且航向误差大大减小，提升了定位精度。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。