一种基于航向角准确修正的惯性导航室内定位方法

摘要

本发明公开了一种基于航向角准确修正的惯性导航室内定位方法，本发明利用IMU器件，在传统HDE及PDR算法基础上，基于精细化的主方向设定、是否是直行以及是否沿建筑物主方向直行的区分、反馈系数的不同修正、峰峰值探测法、动态递推算法等，实现复杂室内环境对行人的准确定位。精细化定义16个主方向，采用三个相邻步间的航向变化来检测直线行走和转弯对是否是直行，以及是否沿建筑物主方向直行进行区分，并基于判决结果对反馈系数做相应修正，实现航向角准确估计。提升了航向角的解算精度。通过提出的峰峰值探测法，能在上下楼梯等复杂室内环境中，克服和加速度波形关于X轴不对称问题，降低了步频检测误差，对和加速度数据进行分析处理，避免了对航向角过度修正，准确解算出定位数据，满足对行人进行实时定位跟踪的需求。

说明书

技术领域

本发明涉及室内定位技术领域，尤其是基于惯性导航器件的行人室内导航定位方法。

背景技术

近年来，各种室内环境基于位置的服务受到了广泛关注，如何在室内环境实现高精度的行人定位导航已成为一个研究热点。当今城市各种建筑物内部结构错综复杂，GPS/北斗等全球卫星定位导航系统在室内环境一般缺乏有效信号覆盖，很难进行准确、有效的定位。基于WiFi、UWB、ZigBee、RFID等设备的室内定位技术一般都需要提前在建筑物内部署信号源等基础设施，难以在没有基础设施支撑的大量一般建筑物内实施实时精准定位。惯性导航定位技术是一种可以全天候、全时段、不停歇工作，且工作时不依赖事先部署基础设施，不易受到外界干扰的一种自主式定位导航技术。近年来随着微电机(MEMS)技术的发展，基于MEMS惯性测量单元(IMU)的体积越来越小，测量精度越来越高，性能愈来愈强，使得基于IMU的室内定位技术的研究及应用受到了更多的关注。基于IMU进行三维室内定位，需要实时采集行人行走过程中的三轴加速度、三轴角速度、高度数据，进而计算出步频、步长以及航向角，用于PDR定位。因此步频检测、步长估计和航向估计的精确度决定了IMU定位性能的优劣。

文献1：王鹏宇,孟之栋,邓志红.腰绑式行人导航系统基于缓存区的启发式航向补偿算法研究[J].导航定位与授时2020,7(03):150-156.。为避免航向角过度修正，动态改变反馈系数来调整航向补偿算法的修正强度。

文献2：A.R.Jiménez,F.Seco,F.Zampella,J.C.Prieto and J.Guevara,"Improved Heuristic Drift Elimination(iHDE)for pedestrian navigation incomplex buildings,"2011 International Conference on Indoor Positioning andIndoor Navigation,Guimaraes,2011,pp.1-8,doi:10.1109/IPIN.2011.6071923.基于脚踏式惯导模块和扩展卡尔曼滤波，改进的启发式漂移消除算法，用于复杂的建筑中行人导航。

目前步频检测采用的方法主要有峰值检测法，零点交叉法等，但这类方法在上下楼梯时会产生较大的步频检测误差。

文献3：申请号为CN202010819613.3，发明名称为一种基于改进式HDE的室内行人导航航向修正方法及系统。(1)基于启发式偏移消除算法为中心，设定8个主方向，主方向间的角度间隔Δ为45°。(2)使用航向角差值、角速率变化规律等进行运动类型分类，构建主方向航向偏移差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波(EKF)，得到航向的最优估计从而修正航向。

文献4：申请号为CN201810230263.X，发明名称为一种行人室内轨迹定位方法。(1)基于主方向进行所述航向角修正：走直线时，利用启发式随机漂移消除法进行航向角修正；走曲线(非直线)时，利用扩展卡尔曼滤波器进行偏移补偿，修正航向角。(2)在启发式随机漂移消除算法的基础上，设定“米”字形的8个主方向，采用三个相邻步间的航向变化来检测直线行走和转弯。

但在航向角的估计过程中由于陀螺仪自身不可避免会出现数据漂移，航向角会随着时间累计而产生不可逆的误差，且现代室内建筑主方向不仅仅是横平竖直，诸如环形、弧形等多种不规则转弯方向使得室内行人行走路径转向角度多样化，传统HDE算法的航向角估计精度已不能满足室内精确定位导航的需求。

发明内容

鉴于现有技术的以上情况，本发明的目的在于提出一种基于航向角准确修正的惯性导航室内定位方法，利用IMU器件，在传统HDE及PDR算法基础上，基于精细化的主方向设定、是否是直行以及是否沿建筑物主方向直行的区分、反馈系数的不同修正、峰峰值探测法、动态递推算法等，实现复杂室内环境对行人的准确定位。精细化定义主方向，采用三个相邻步间的航向变化来检测直线行走和转弯对是否是直行，以及是否沿建筑物主方向直行进行区分，并基于判决结果对反馈系数做相应修正，实现航向角准确估计，在上下楼梯等复杂室内环境中，克服和加速度波形关于X轴不对称问题，降低步频检测误差，对和加速度数据进行分析处理，避免对航向角过度修正，准确解算出定位数据，满足对行人进行实时定位跟踪的需求。

实现本发明目的的技术方案如下：

1、一种基于航向角准确修正的惯性导航室内定位方法，包括以下步骤：

A、初始位置与初始航向角的确定：在建筑物入口通过GPS进行初始位置确定，并分别进行两点位置坐标数据采集，获取初始位置以及初始航向角；

B、对采集的三轴和加速度进行去重力、低通滤波等数据预处理；

C、步频检测：采用峰峰值探测法，首先在三窗式滑动窗口中对和加速度进行波峰波谷检测，并分别存放在于队列中；然后分别依次获取一个波峰、波谷并用横向时间阈值和纵向峰峰值幅值进行有效步频检测；

D、步长估计：采用Weinberg等人提出的非线性的模型：

式中，k为跟行人身高、步频等参数有关的常数，通过训练来获得；A

E、航向角估计：使用的九轴惯性导航器件能够直接实时输出运载体坐标系下的三轴矢量角速度

F、航向角修正：室内航向标定为16个主方向，主方向间隔Δ＝22.5°；计算前一时刻航向角ψ

G、航向角的进一步修正；在所得实时修正后的航向角的基础上，通过对行走的路线进行直行和非直行两种路径类型判别，在直线行走下再区分沿着建筑主方向行走和非建筑主方向行走路径，然后分别使用不同的反馈系数i

H、位置推算：通过步频探测、步长估计得到每一步步长d，航向角估计值ψ'

采用本发明的方法，在传统HDE及PDR算法基础上，基于精细化的主方向设定、是否是直行以及是否沿建筑物主方向直行的区分、反馈系数的不同修正、峰峰值探测法、动态递推算法等，实现复杂室内环境对行人的准确定位。精细化定义主方向，采用三个相邻步间的航向变化来检测直线行走和转弯对是否是直行，以及是否沿建筑物主方向直行进行区分，并基于判决结果对反馈系数做相应修正，实现了航向角准确估计，在上下楼梯等复杂室内环境中，克服了和加速度波形关于X轴不对称问题，降低步频检测误差，对和加速度数据进行分析处理，避免对航向角过度修正，准确解算出定位数据，满足对行人进行实时定位跟踪的需求。

附图说明

图1本发明方法主要步骤的工作框图。

图2为初始位置和初始航向角确定示意图。

图3为基于三窗式滑动窗口的峰峰值探测图

图4为航向角准确修正算法示意图。

图5为主方向的设定图。

图6为直行和转弯时的航向角示意图。

图7为主方向的偏移角度示意图。

图8为行人航迹推算示意图。

图9便携式设备穿戴示意图。

图10多楼层行的3d轨迹示意图。

具体实施方式

下面结合附图对本发明方法作进一步地详述。

本发明一种基于航向角准确修正的惯性导航室内定位方法，包括以下步骤：

一种基于航向角准确修正的室内惯性导航定位方法，包括以下步骤：

A、初始位置与初始航向角的确定：如图2所示，在建筑物入口外通过GPS进行初始位置确定，获取初始位置(x

B、对采集的三轴和加速度进行去重力、低通滤波等数据预先处理，以便后续精准的步频探测。

C、步频检测：如图3所示，采用峰峰值探测法，首先在三窗式滑动窗口中进行波峰波谷检测并分别存放在于队列中，然后分别依次获取一个波峰、波谷并用横向时间阈值和纵向峰峰值幅值进行有效步频检测。

D、步长估计：采用Weinberg等人提出的非线性的模型：

式中，k为跟身高、步频等参数有关的常数，可通过训练来获得；A

E、航向角估计：本专利使用的惯性导航器件能够直接实时输出运载体坐标系下的三轴矢量角速度

F、航向角的修正：通过如图4所示航向角准确修正算法，有效地减小定位模块在室内长时间工作所产生的累计误差。首先建立坐标系，将室内航向标定为16个主方向，主方向间隔Δ＝22.5°；然后计算前一时刻航向角ψ

G、航向角的进一步修正：为了达到航向角准确修正的目的，在此基础上，首先对行走的路线进行了直行和转弯两种路径类型判别；然后区分直行和转弯路径以及在直线行走下区分沿着建筑主方向行走和非建筑主方向行走路径；最后使用不同的反馈系数i

MOD(n,m)＝n-m×INT(n/m) (3)

其中，INT(k)为取整函数，结果为不大于k值的最大整数，所以MOD函数的结果在区间[0,m]内。

偏移角

因此，可得反馈变量I

1)主方向的设定

如图5所示，为本文设定的16个主方向，主方向的角度间隔Δ＝22.5°。将主导方向细分为16个方向是为了应对长时间沿着非主方向行走或者持续小角度转弯行走的情况，以减缓HDE算法对角度的过度修正，增强算法适应各种不同室内道路环境的能力。

2)直行的判决

惯性导航模块佩戴于腰部，航向角曲线在直行和转弯时有着明显的特征。图6是算法测试时行走的两段转弯和三段直行的航向角曲线图，其中星号点为直行每一步的航向角，圆圈点为转弯每一步的航向角。

对行走中的航向角进行时域特征提取并分类。假定连续三步的航向角数值分别为ψ

第一类为T

第二类为T

第三类为T

第四类为T

当S

3)主方向直行的判决

对于直行前提下还需要判断是否是沿着建筑物间隔90°的4个主方向之一直行，先选择4个建筑物主方向，其角度间隔Δ＝90°。在图5中设定的16个主方向中，一方面22.5°的倍数中虽包含了4个建筑物主方向，但还有12个非建筑物主方向，需要加以区分；另一方面正常行走过程中航向值波动在正负10°左右，在建筑主方向间隔Δ＝22.5°上进行主方向直行判别，极容易发生误判。

可对连续的三步航向角数值求出其均值后模360°得到一个角度值μ，再把μ映射到第一象限中去，最后选择与象限轴夹角最小的角度σ＝min(μ,90°-μ)，如图7所示。筑物主方向直行判决公式如式所示。

当S

H、位置推算：通过步频探测、步长估计得到每一步步长d，航向角估计值ψ'

其中V

其中，ρ

还需要说明的是，为提高求算的精确度，本发明从步骤B至步骤F的数据解算过程是在一个队列式动态递推算法中进行，该算法能够实现数据近实时地被处理、分析。算法过程描述如下：

(1)给定一个固定长度为N的队列Q，分为三个窗口，用于存放和加速度数据；

(2)先填充N/3冗余数据进队首，为后续滤波做准备；

(3)队列中N个数据填满后做低通滤波、去重力等数据预处理；

(4)进行峰峰值探测，获取N/3～2N/3区间探测的步频、步长和航向角等数据；

(5)队首N/3个数据出队列，队尾进N/3个数据后，重复第3～4步操作；

在步骤G中，动态递推算法使用的队列长度N是3倍的惯导数据采集频率。

实施例

实验场地：西南交通大学9号教学楼的负一层至四层。

实验设备：基于本方法设计的定位设备，穿戴方式如图9所示。定位设备硬件中惯性导航器件的数据采样速率设置为200Hz，通信波特率为460800Hz，工作电压为3.3V。定位设备软件程序中，步频检测的横向时间阈值预设为10个采样点，纵向峰峰值阈值预设为2.3；步长估计模型参数k值采用离线测试获取，预设行走k值为0.4332，奔跑k值为0.4792；航向角准确修正算法中主方向间隔预设Δ为22.5°，相邻两航向角差值的门限阈值θ和β预设为15°和10°。判断主方向阈值γ预设为10°。

实验内容：定位设备穿戴好后，如图2所示，首先正面对着教学楼1楼入口站立，上电开启设备并进行初始方位和初始航向角的确定；然后进入1楼，下楼梯进入地下室，先在地下室绕墙柱环绕行走一圈，接着爬楼梯到地上四层每一层依次行走一圈，最后下楼梯返回出发点，全程行走大约1123米。在行走过程中穿戴设备实时按照图1所示的流程进行步频检测、步长估计、航向估计、航向角准确修正以及高度数据采集，在得到每一步的步长、航向角及高度数据后，利用PDR算法进行位置推算。基于位置推算的结果可画出整个行程的行走轨迹，并计算、统计定位误差。

实验目的：模拟复杂室内行走场景，在地下室绕柱行走、爬4层楼梯及途中转弯都是沿着非主方向行走，以检验本发明方法对复杂室内场景的适应性。

实验结果：由定位设备采集数据推算的行走过程三维坐标数据绘制的行走轨迹如图10所示，与教学楼楼道结构及地下室结构准确吻合。实验测得起始点和终点的首尾水平误差仅为0.77米，与总航程的相对误差为0.07％，三维相对定位误差为0.17％，与现有方法相比取得了更好的定位性能。

与现有室内定位技术相比，本发明具有明显的优点：

一、与基于WiFi、UWB、ZigBee、蓝牙的室内定位技术相比，本发明无需在待定位场所事先部署基站、锚节点等基础设施，适合在任意建筑物室内及隧道、矿井等环节中对人员的定位。

二、与现有室内惯性导航定位方法相比，本发明提出的设计峰峰值探测法，能在上下楼梯等复杂室内环境中，解决和加速度波形关于X轴不对称问题，降低了步频检测误差。

三、本发明在航向角解算过程中，将主方向精细化为16个，能适应建筑物内转角、转弯的各种情况，最大程度兼容室内建筑风格，保证了航向角解算的准确性和可靠性。

四、本发明在航向角解算过程中，对是否是直行，以及是否沿建筑物主方向直行进行了区分，设计了相应的判决条件，并基于判决结果对反馈系数做相应修正，提升了航向角的解算精度。

本发明在数据解算过程中，采用动态递推算法，对和加速度数据进行分析处理，准确解算出定位数据，满足对行人进行实时定位跟踪的需求。

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