一种行人惯性导航的航向校正方法与流程

本发明属于惯性导航技术领域，尤其涉及一种行人惯性导航的航向校正方法。

背景技术：

随着社会的快速发展，导航技术无论在军用还是民用方面都得到了广泛的应用，在近些年也取得了长足的发展。在室外定位中，全球导航定位系统有着优秀的表现，然而，由于卫星信号穿透性较差，在高楼、地下建筑等室内场景中，卫星定位系统的表现不尽如人意。惯性导航技术作为一种不易受外界环境干扰，无需预先部署，无依赖，低成本的导航技术，在无gps情况下的定位中有着独特的优势。

惯性导航系统，是一种根据惯性传感器模块测量行人的运动参数并通过导航解算，解算出行人运动的位置信息的导航方法。惯性传感器模块主要由三轴加速度计、三轴陀螺仪组成，有些惯性传感器模块还会集成温度计、磁力计以及气压计。由于惯性导航系统单纯依靠惯性传感器的原始数据进行导航解算(无其他额外信息辅助)的特点，惯性传感器的质量往往成为左右惯性导航系统精度的主要因素。传统的惯性导航系统主要是对惯性传感器测得的加速度以及角加速度进行二次积分得到导航轨迹。然而，以积分的形式去推算导航轨迹，会不可避免地对噪声以及误差进行累积，这就会导致惯性导航系统在长距离、长时间的定位中出现严重的轨迹发散现象，难以符合长时间定位的要求。而基于行人航向推算系统(pdr)的惯性导航系统虽然可以有效减少积分所带来的航向累积误差，但在长距离的定位上仍然会存在航向的发散问题。

现有的解决航向发散问题的方法主要依赖于外界的信号辅助，比如磁场信号、毫米波雷达信号、激光信号、相机图像等等。然而，如毫米波雷达信号、激光信号及相机图像，对外界的设备要求比较高，造价昂贵且不易携带。磁场信号虽然容易获取，却非常容易受到外界信号的干扰，而室内磁场干扰源复杂多变，难以进行定量分析，所以现有的磁场融合算法都不能很好地证明算法的通用性与准确性。而不依赖于外界信号的航向纠正算法主要是利用行人的运动特性，典型的就是启发式漂移减少算法(hdr)。hdr算法利用行人行进过程中趋向于直线行走的特性，通过动态滑动窗口的方式，比较当前脚步的方向与过去几步中的脚步方向的差值，得出一个直线行走的判断。而后将当前脚步方向设置为过去几步脚步行进方向的均值，将此作为实际航向代入到惯导系统中求取接下来的航向。这种利用行人短期内运动特性的航向校正方法在一定程度上减小了航向误差，但由于这种方法只能利用短期内的惯导信息，未能充分利用过去时刻的惯导信息，因此在长时间上，它只能对航向信息微弱地进行补偿，不能完全满足惯性导航系统的要求。

技术实现要素：

针对现有技术中的上述不足，本发明提供的一种行人惯性导航的航向校正方法解决了现有的航向校正方法依赖外界信号、不能充分利用已有的惯导器件的信息、依赖短期行人运动信息而忽视长时间的运动信息的问题。

为了达到以上目的，本发明采用的技术方案为：

本方案提供一种行人惯性导航的航向校正方法，包括如下步骤：

s1、将行人惯性导航数据输入至航向推算系统中，得到推算航向；

s2、将单位加速度在载体坐标系的值作为观测量，将利用旋转矩阵得到的单位加速度的分量作为预测量；

s3、利用卡尔曼滤波算法对所述观测量和预测量进行融合计算，得到行人当前的航向信息，并存储至预设的航向信息存储池中；

s4、根据行人脚步落地的时刻，利用航向匹配算法从预设的航向信息存储池中寻找与所述行人当前的航向信息匹配的已有航向信息；

s5、判断是否寻找到匹配的航向信息，若是，则将所述匹配的航向信息作为输出结果，从而完成对行人惯性导航的航向校正，否则进入步骤s6；

s6、将航向推算系统中的推算航向作为最终的输出结果，并将该输出的航向信息输入至航向信息存储池中进行存储。

进一步地，所述步骤s2中预测量的表达式如下：

accpre＝[2×q(2)×q(4)-q(1)×q(3),2×q(3)×q(4)+q(1)×q(2),q(1)²-q(2)²-q(3)²+q(4)²]

其中，accpre表示利用旋转矩阵得到的单位加速度，q(1)，q(2)，q(3)和q(4)均表示以四元数表示的旋转矩阵的四个分量。

再进一步地，所述步骤s3中行人当前的航向信息的表达式如下：

其中，表示状态向量的初始推算值，kk表示卡尔曼增益，accnorm表示单位加速度的观测量，accpre表示利用旋转矩阵得到的单位加速度。

再进一步地，所述步骤s4包括如下步骤：

s401、根据行人脚步落地的时刻，将所述行人的初步航向作为行人当前脚步周期内的航向信息；

s402、根据所述航向信息存储池中的已有航向信息，利用阈值筛选法筛选出与当前脚步周期的航向信息匹配的已有航向信息；

s403、查询筛选出的已有航向信息中最早时刻的航向信息点，将该时刻的航向信息作为参考的航向信息，并与当前航向信息进行融合处理，得到融合后的航向信息；

s404、将融合后的航向信息作为实际航向信息的观测量，将当前时刻的航向信息作为预测量，并利用卡尔曼算法对当前时间的航向信息进行补偿，得到当前时刻航向信息；

s405、将所述当前时刻的航向信息输入至航向信息存储池中进行存储，从而得到与所述行人当前航向匹配的已有航向信息。

再进一步地，所述步骤s402中的筛选的条件为：

|heading(t)-heading(i)|＜180±3

|heading(t)-heading(i)|＜±3

其中，heading(t)表示当前时刻的航向信息，heading(i)表示航向信息存储池中的已有航向。

再进一步地，所述步骤s403中融合后的航向信息heading_act的表达式如下：

headingob＝a×heading(t)+(1-a)×heading(j)

a＝init×e^-t/τ

其中，heading(j)表示筛选出的前期航向信息，heading(t)表示当前时刻的航向信息，headingob表示融合后的航向信息，a表示前期的权重值，init表示初始权重系数，τ表示控制衰减程度的系数，e^-t/τ表示权重的衰减系数。

再进一步地，所述步骤s404中当前时刻航向信息的表达式如下：

其中，表示预测航向，headingob表示融合后的航向信息，kk表示当前系统的卡尔曼增益。

本发明的有益效果：

本发明提供了一种行人惯性导航的航向校正方法。相较于利用多传感器融合的航向校正方法，本方法只使用惯导信息(加速度信号与陀螺仪信号)实现了较为准确的航向校正，节省了硬件成本，简化了算法复杂度。相较于现有的基于纯惯性导航系统的航向校正方法，本方法深入挖掘惯导元器件的特性，充分利用了行进过程中已有的信息，而不是只局限在某一个特定时间段内的局部信息，并且利用时间优先的原则对筛选出的匹配信号进行再一次选取，使得本方法有更好的鲁棒性、更精确的航向校正效果。

附图说明

图1为本发明的方法流程图。

图2为基于惯性导航系统的航向校正方法的总体框图。

图3为本实施例中的航向推算系统的具体框图。

图4为本实施例中的航向匹配算法的具体实现框图。

图5为本实施例中的卡尔曼滤波器的具体框图。

图6为本方法与传统基于纯惯导系统的航向校正方法的比较示意图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

实施例

如图1-图2所示，本发明公开了一种行人惯性导航的航向校正方法，其实现方法如下：

s1、将行人惯性导航数据输入至航向推算系统中，得到推算航向；

s2、将单位加速度在载体坐标系的值作为观测量，将利用旋转矩阵得到的单位加速度的分量作为预测量，所述预测量的表达式如下：

accpre＝[2×q(2)×q(4)-q(1)×q(3),2×q(3)×q(4)+q(1)×q(2),q(1)²-q(2)²-q(3)²+q(4)²]

其中，accpre表示利用旋转矩阵得到的单位加速度，q(1)，q(2)，q(3)和q(4)均表示以四元数表示的旋转矩阵的四个分量；

s3、利用卡尔曼滤波算法对所述观测量和预测量进行融合计算，得到行人当前的航向信息，所述行人当前的航向信息的表达式如下：

其中，表示状态向量的初始推算值，kk表示卡尔曼增益，accnorm表示单位加速度的观测量，accpre表示利用旋转矩阵得到的单位加速度。

本实施例中，如图3所示，将惯导数据送入航向推算系统中，在航向推算系统中把加速度作为观测量，以单位加速度在载体坐标系中的值作为观测量，以旋转矩阵解算出的单位加速度的分量作为预测值，利用卡尔曼滤波进行融合，从而推算出行人当前初步的航向信息。

s4、如图4所示，根据行人脚步落地的时刻，利用航向匹配算法从预设的航向信息存储池中寻找与所述行人初步航向匹配的已有航向信息，其实现方法如下：

s401、根据行人脚步落地的时刻，将所述行人的初步航向作为行人当前脚步周期内的航向信息；

s402、根据所述航向信息存储池中的已有航向信息，利用阈值筛选法筛选出与当前脚步周期的航向信息匹配的已有航向信息；

本实施例中，将当前脚步周期的航向与航向信息存储池中的储存的过去航向信息相比较，利用阈值筛选的方法，筛选出符合要求的过去航向信息。如果当前时刻方向与前期某个时刻方向差小于某个阈值，就可以利用前期的方向去修正当前时刻的方向。在算法过程中，取相同方向和相反方向两种方向差(当前方向与选定方向差值为0或180度)，航向筛选的条件为：

|heading(t)-heading(i)|＜180±3

|heading(t)-heading(i)|＜±3

其中，heading(t)表示当前时刻的航向信息，heading(i)表示航向信息存储池中的已有航向；

s403、查询筛选出的已有航向信息中最早时刻的航向信息点，将该时刻的航向信息作为参考的航向信息，并与当前航向信息进行融合处理，得到融合后的航向信息；

本实施例中，对筛选出的过去航向信息进行查询，找出最早时刻的航向信息点，将该时刻的航向信息作为参考的航向与当前航向进行融合处理。选择最早时刻的航向，是因为根据陀螺仪信号的短期精准的特点，由于航向漂移，越是时刻靠前，航向信息就越准确地趋于实际航向值。融合处理后的航向公式为：

headingob＝a×heading(t)+(1-a)×heading(j)

a＝init×e^-t/τ

其中，heading(j)表示筛选出的前期航向信息，heading(t)表示当前时刻的航向信息，headingob表示融合后的航向信息，即进行权重取舍之后得到的行人的方向，用于卡尔曼滤波中作观测量，a表示前期的权重值，init表示初始权重系数，τ表示控制衰减程度的系数，e^-t/τ表示权重的衰减系数。

s404、将融合后的航向信息作为实际航向信息的观测量，将当前时刻的航向信息作为预测量，并利用卡尔曼算法对当前时间的航向信息进行补偿，得到当前时刻航向信息，当前时刻航向信息的表达式如下：

其中，表示预测航向，headingob表示融合后的航向信息，kk表示当前系统的卡尔曼增益。

本实施例中，如图5所示，利用卡尔曼滤波与航向匹配结果校正航向信息，将取得的航向信息当作实际的航向信息作为观测量，将当前时刻的航向作为预测量，利用卡尔曼滤波的方法对当前时刻的航向进行补偿，得到一个相对准确的当前时刻的航向信息输出；

s405、将所述当前时刻的航向信息输入至航向信息存储池中进行存储，从而得到与所述行人当前航向匹配的已有航向信息

s5、判断是否寻找到匹配的航向信息，若是，则将所述匹配的航向信息作为输出结果，从而完成对行人惯性导航系统的航向校正，否则进入步骤s6；

s6、将航向推算系统中的推算航向作为最终的输出结果，并将该输出的航向信息输入至航向信息存储池中进行存储。

如图6所示，实验中走了一个闭环轨迹，传统的零速更新方法的轨迹会随着时间的推移而逐渐发散，而本文所提出的方法轨迹更接近于一个完整的闭环，相较而言，在航向的校正方面有着更为优秀的表现。总体而言，本发明提供了一种行人惯性导航的航向校正方法。相较于利用多传感器融合的航向校正方法，本方法只使用惯导信息(加速度信号与陀螺仪信号)实现了较为准确的航向校正，节省了硬件成本，简化了算法复杂度。相较于现有的基于纯惯性导航系统的航向校正方法，本方法深入挖掘惯导元器件的特性，充分利用了行进过程中已有的信息，而不是只局限在某一个特定时间段内的局部信息，并且利用时间优先的原则对筛选出的匹配信号进行再一次选取，这使得本方法有更好的鲁棒性、更精确的航向校正效果。