一种有步态约束的行人自主定位误差修正方法与流程

本发明属于导航定位技术领域，特别是涉及一种有步态约束的行人自主定位误差修正方法。

背景技术：

随着互联网数据业务的快速增长，人们对导航与定位的需求日益增大，尤其在复杂的室内环境中，如大型商场、图书馆等常需要确定移动终端的位置、姿态等信息。目前常用的室内定位技术主要有超宽带无线定位技术(ultrawideband，uwb)、无线局域网定位技术(wifi)、超声波定位技术、蓝牙定位技术(bluetooth)等。其中，uwb定位技术精度较高，可达到厘米级，但该技术成本较高，且使用该定位技术时需要预先设置大量的节点，增加了导航系统的复杂度；随着当前无线网络的普及，wifi定位技术成本较低，且在定位前不需要预先设置节点，但是该技术定位精度较差，一般只能达到米级，只能应用在对定位精度要求不高的场所；而其他的室内定位技术在使用时均需要一些外界设施辅助，这无疑增加了导航系统的复杂度。然而基于mems的惯性导航系统，不需要任何外部设施的辅助，仅依靠自身的惯性器件(加速度计和陀螺仪)就可以实现行人的自主定位，并且其成本低、体积小、重量轻、自主性强，相对比其他的定位技术具有明显的优势。但该系统的惯性器件存在随时间累积的漂移误差，如果不及时校正系统的输出，会导致航向发散的现象，使定位的精度越来越低，甚至失去定位的功能。本发明基于此类问题提出了一种有步态约束的行人自主定位误差修正方法，有效地解决了行人自主定位过程中航向发散的问题，提高了行人自主定位的精度。

与同领域的相关申请专利进行对比，本发明的创造性及优点较为明显。比如，申请号为：201510486767.4，专利名称是《一种基于双imu的双模式室内个人导航系统及方法》的专利，其采用的是利用固定在肩部的imu测量单元得到的航向信息对足部imu解算的导航信息进行误差预估，该方法虽然定位精度较高，但双imu模式实施的难度较大，不利于在行人导航领域的推广。再比如，申请号为：201710022551.1，专利名称是《一种基于自适应卡尔曼滤波的行人航向最优融合方法》的专利，该方法通过捷联解算得到的航向角与磁航向角的最优融合，从而提高组合航向角的精度，但由于在室内建筑中磁场不稳定，地磁传感器发挥的效果并不理想，该方法在室内定位领域的实用性不强。

技术实现要素：

本发明的目的主要在于：面向室内行人自主定位，在ins+ekf+zupt框架的基础上，基于室内建筑结构规则化的事实，将行人在室内行走时的航向的变化约束为0°，90°或180°，通过训练行人常见的步行行为数据得到步态序列模板，利用模板下的航向约束实时的修正航向信息，从而解决行人自主定位过程中惯性器件随时间累积的漂移误差导致航向发散的问题，进一步提高行人自主定位的精度。

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

步骤1：采集imu数据，利用行人运动的约束条件进行静止步态检测；

步骤2：推算行人的航向信息并进行步行行为的判定，不同的步行行为匹配已训练好步态序列模板，根据模板下的航向约束实时地修正航向误差；

步骤3：利用智能滤波器融合系统误差进行误差估计，输出行人最终的定位信息。

进一步的，通过将imu惯性测量模块固定在行人的脚部，采集步骤1中所述的imu数据，获取行人运动时的加速度和角速度信息，然后通过行人运动的约束条件进行静止步态检测，利用行人脚接触地面的零速特征进行导航系统的更新操作，从而实时地修正导航系统的速度、位置和水平姿态误差。其中静止步态检测采用的判断方法如下：

c1：竖轴加速度峰值时间点区间法。行人脚接触地面的瞬间，竖直方向存在加速度的最大值。通过检测该加速度的峰值时间点k，并假设该时间点后的某一时间区间δt为零速区间。

式中：ap(k)为k时刻加速度的最大峰值，ay(k)为竖直方向的加速度，tay为加速度阈值；c1(k+δt)表示检测到加速度峰值时间点后所设定的行人处于静止步态的时间区间。

c2：横轴角速度阈值法。横向的陀螺仪角速率的变化可以测量出行人行走时抬脚、摆腿等动作，通过横向陀螺的输出取绝对值作为判断条件，判断方法如下：

式中：|wy(k)|表示k时刻横向陀螺仪输出的绝对值，tw为角速率阈值，当横向陀螺仪输出角速率的绝对值低于阈值时，行人处于静止步态。

以上两个条件同时成立，即c1、c2满足逻辑“与”运算时，行人处于静止步态。

进一步的，所述步骤2中，采用捷联解算和毕卡三阶更新四元数的方法来推算航向角。首先，四元数初始姿态角的表达式如下：

式中，ψ，θ，γ分别表示偏航角，俯仰角和横滚角，且在初始时刻ψ，θ，γ的值均为0。

然后，利用毕卡三阶算法更新四元数，方法如下：

式中，q(t)为t时刻的四元数，为3个轴角度平方和，i为单位矩阵，为对机体坐标系相对地理坐标系的旋转角速度的斜对称矩阵的积分。

最后，利用四元数表示姿态转换矩阵，两者的关系如下：

根据姿态转换矩阵推算航向角，方法如下：

式中，tij(n)代表的是姿态矩阵的第i行和第j列元素。

通常行人的转弯行为包括2-3步，为了判断准确，本发明通过对行人当前5步的航向变化来判断行人的步态类型，并利用状态号s来定义行人每一步航向变化的标准，s的定义方法如下所示：

式中，假设θ1＝10°，θ2＝40°，θ3＝70°，且行人左转时方向为负，如行人某个左转弯行为可根据状态号s表示为{-2，-3，-1}。步行行为确定好后，通过大量训练步行行为数据得到步态序列模板，不同的步行行为匹配相应的模板序列，利用模板下的航向约束实时地修正航向误差。假设有k个步态与模板匹配，修正的方法如下：

此时应注意：由于行人转弯90°时，转弯180°的模板也可匹配，也就是说转弯90°的一些序列包含在转弯180°的序列中，因此，校正的过程中可以用90度取代180度，或以正度代替负度。且当行人的步行行为出现异常时，如有人在原地转圈摆动，该方法并不适用，此时通过步长的约束进一步判断行人当前的步态类型，方法如下：

式中，λ用来限制行人步长太小的异常状况；k是行人当前步的步长，是行人步行过程中从第一步到最后一步的数据集。

进一步的，所述步骤3中，利用智能滤波器融合系统误差进行误差估计，误差状态量x为：

x＝[δψk，δvk，δwk，δpk]^t

其中，δψk为3个姿态角误差，δvk＝[δvx，δvy，δvz]为3轴速度误差，δwk＝[δwx，δwy，δwz]为三轴角速度误差，δpk＝[δl，δλ，δh]为位置误差。

将行人静止步态的零速误差和航向误差一同作为系统的观测值，观测方程如下：

得到的量测阵h为：

最终，得到行人的定位信息。

与现有技术相比，本发明的有益效果是：

(1)本发明自主性强，且其体积小、重量轻、具有可穿戴的特点，使操作更加便捷。

(2)本发明通过对行人步态的约束，解决了航向误差可观测性差，zupt无法修正航向的问题，抑制了航向发散的现象。

(3)本发明将将行人静止步态的零速误差和航向误差一同作为系统的观测值，进一步提高了导航系统的定位精度。

附图说明

图1为导航系统的整体框图；

图2为行人航向约束为0°时的校正效果图；

图3为步态序列模板；

图4为航向修正的方法图。

具体实施方式

本发明提供的一种有步态约束的行人导航误差修正算法具体步骤如下：

如图1所示，首先利用惯性测量单元(1-1)测量出人体运动时的加速度和角速度信息，然后通过捷联解算(1-2)得到行人未经修正的速度、位置和姿态信息；利用静止步态检测(1-3)中行人脚接触地面速度为零的特征作为一种触发条件，进行惯性导航系统的更新操作，实时地修正导航系统的速度、位置和水平姿态误差；然后采用毕卡三阶更新四元数的方法进行航向角的推算(1-4)，并通过行人当前五步的航向变化判定行人的步行行为类型(1-5)，(直线，左前方转弯，右前方转弯，左后方转弯，右后方转弯)；通过训练行人常见的步行行为得到典型的步态序列模板，不同的步态序列匹配相应的模板类型，利用模板下的航向约束实时地修正航向误差(1-6)；最终将行人静止步态的零速误差和航向误差一同作为系统的观测值，利用智能滤波器融合系统的误差(1-7)，输出经过校正后的行人的位置、速度和姿态信息(1-8)，从而提高导航系统的精度。

如图2所示，为行人沿直线行走时的航迹校正效果图。假设行人当前在建筑物内沿直线行走，由于脚部的轻微摆动和惯性器件随时间累积的漂移误差，会导致航向发散的现象，使行人的航迹与真实的轨迹存在偏离，如图2-1所示，此时通过行人的航向变化为0°的约束，航迹可以被校正为一条直线，校正的结果如图2-2所示。其他步行行为与此相同，具有一些随机误差的转弯路径均可被校正为理想的转弯曲线。

如图3所示，为通过训练行人常见的五种步行行为数据得到的步态序列模板，通常行人的转弯行为包括2-3步，为了判断准确，本发明通过对行人当前5步的航向变化来判断行人的步行行为，其中“x”表示可以用2或3替换。将行人当前k步的步态序列与模板序列相匹配，利用模板下的航向约束实时地修正航向漂移误差，从而提高导航系统的定位精度。当然，为了使判断更加准确，还可以通过更多次的训练行人不同步行行为数据继续补充步态序列模板。

如图4所示，为本发明所采用的基于步态序列检测的误差修正方法，通过行人当前的航向信息计算出步态序列状态s(4-1)，并利用(4-2)检测行人当前的步行行为是否出现异常，如有人在原地转圈摆动时，所提出的航向漂移消除方法并不适用，此时可利用步长的约束(4-3)进一步判断行人当前的步行行为。行人不同的步行行为匹配相应的模板序列，利用模板下的航向约束实时地修正航向误差，此时应注意，行人转弯90°时，转弯180°的模板也是匹配的，也就是说转弯90°的一些序列包含在转弯180°的序列中(4-4)，因此，校正的过程中用90°度取代180°度，或以正度代替负度。

此外，当行人出现异常的步行行为导致步态类型判断有误或某些转弯行为丢失时，可以通过继续训练步行行为数据来扩展要匹配的步态序列模板，也可以通过更小的角度间隔分割阈值来增加目标模板的多样性，以提高判断结果的准确性。

由上述步骤，便可完成一种有步态约束的行人自主定位误差修正的设计发明。

本发明提供的这种有步态约束的行人自主定位误差修正方法，不仅有效地抑制了惯性器件随时间累积的漂移误差而导致航向发散的现象，提高了导航系统的定位精度，还利用其自主性强、体积小、重量轻的可穿戴特点，提升了用户使用的便捷度。以上仅是本发明的具体应用范例，对本发明的保护范围不构成任何限制。凡采用等同变换或者等效替换而形成的技术方案，均落在本发明的权利保护范围之内。