一种基于自适应零速区间调整的惯性行人定位方法与流程

本发明属于依托惯导系统的行人定位技术领域，具体涉及一种基于自适应零速区间调整的惯性行人定位方法。

背景技术：

目前，最常见的行人定位系统是全球定位系统(globalpositioningsystem，gps)，但是在高楼林立的街道和相对封闭的室内等环境中，行人无法正常接收gps信号，进而无法定位。而惯性导航系统(inertialnavigationsystem，ins，以下简称惯导)是一种不依赖于外部信息、也不向外辐射能量的自主式导航系统，适用于复杂多变的行人运动环境。正是由于微惯性传感器的快速发展，有体积小、重量轻、易集成、抗电磁干扰、可靠性高等优点，才使得将惯导系统应用于行人定位成为可能。惯导系统的基本工作原理是以牛顿力学定律为基础，通过实时测量载体的加速度和角速度，计算得载体的定位信息。但是，由于生产加工工艺的限制，惯导系统会不可避免的存在噪声，使得系统的定位误差随时间发散而不断增大，这是制约惯性行人定位系统定位精度的重要因素之一。

为了提高系统定位精度，可以采用高精度惯性元件或者先进的惯性行人定位系统误差抑制算法。但是，无限制的提高元件精度必然会导致制作成本的大幅度增加，考虑到行人定位系统的核心要求为低成本、高精度，采用先进的误差抑制算法才是最佳途径。通过采取先进的惯性行人定位系统定位误差抑制算法，自动抵消惯性器件的误差对系统精度的影响，这样就可以应用现有精度的惯性元件构成较高精度的惯性行人定位系统。

零速校正方法是一种惯性行人定位系统的误差自补偿方法，该方法需要将惯性器件与行人足部固联，实时检测行人足部的运动状态，当足部离地时，对足部的位置、速度、姿态信息(以下简称位姿信息)进行解算；当足部触地时，理论上认为足部与地面是相对静止的，并将这一段相对静止的时间称为“零速区间”，通过对零速区间内的位姿信息进行解算与误差补偿，可以抵消器件噪声对系统的影响，进而提高系统定位精度。但是，零速校正方法的有效性建立在零速区间检测的准确性上，只有系统能精确地检测出行的足部的零速区间，零速校正才能取得理想的效果。常见的零速检测方法是滑动窗口检测法，即对固定长度窗口内的数据进行检测从而判定足部是否处于零速状态。但是行人运动具有随意性，据参考相关文献及发明人不完全试验统计，当行人慢速行进时，每步时长可达2.7秒，其中零速时长可达1.15秒；当行人快速行进时，每步时长短达0.69秒，其中零速时长短达0.05秒。可见行人运动过程中零速区间的长度有很大差异，如果仍然利用固定窗长的滑动窗口来检测行人运动状态，这会导致零速检测不准确，即漏检或误检，从而导致零速校正算法的适用性降低，进而无法完全补偿定位误差，导致定位精度显著下降，无法满足行人定位的需求。

公告号为cn104296750a的中国发明专利在2015年1月21日公开的《一种零速检测方法和装置以及行人导航方法和系统》，在传统滑动窗口检测法的基础上，结合聚类分析方法自适应的调整零速检测的阈值，进而检测出行人足部运动状态，在一定程度上提高了零速检测的可靠性。《导航与控制》2016年15卷第4期由孟祥宾等人撰写的《一种优化的零速检测行人导航算法》，该文章主要针对常用零速校正算法对步速变化、转弯等一些异常情况实用性低的问题，提出了用多种零速检测方法相结合的方式来检测行人足部运动状态的方法，提高了部分步态下惯性行人定位系统的精度。《transactionsonbiomedicalengineering》2010年57卷11期由skog等人撰写的《zero-velocitydetection-analgorithmevaluation》，该文章提出了三种不同的基于滑动窗口的零速检测方法，并指出滑动窗口的长度应由惯导系统的采样频率和行人足部的真实触地时间决定。《sensors》2016年16卷第10期由xiaochuntian等人撰写的《anovelzerovelocityintervaldetectionalgorithmforself-containedpedestriannavigationsystemwithinertialsensors》，该文章提出了一种基于平滑伪wigner-ville分布的自适应零速检测算法，该算法采用spwvd-rmfi方法提取行人步态频率，并通过建立阈值与步态频率之间的函数关系，实时调整零速检测阈值，在一定程度上提高了零速检测的精度。《ieeesensorsjournal》2017年17卷第7期由zhangrui等人撰写的《adaptivezerovelocityupdatebasedonvelocityclassificationforpedestriantracking》，该文章对行人步态与胸部运动特征的物理关系进行了分析，提出了一种胸部加速度辅助校准零速检测阈值的方案，实现了不同步速下零速检测阈值的自适应调节。哈尔滨工程大学2013年由刘峰丽撰写的《mimu/gps/磁力计单兵系统组合导航技术研究》，该文章提出了一种基于隐马尔科夫模型的零速校正技术，它利用陀螺仪输出数据进行零速检测，在一定程度上提高了快速运动时零速校正算法的适用性，但同时导致行人定位系统在慢速运动时定位精度下降。以上文献都在于自适应调整行人不同运动速度下的零速检测阈值，通过调整阈值来提高零速检测精度，或针对某一特定步态来改进零速检测算法，以此提高零速检测精度，综上，所有文献都没有提及零速检测窗口长度对任意步态下系统解算位姿信息精度及适用性的影响，也没有提出自适应调整零速检测窗口长度的方法。

技术实现要素：

本发明的目的在于提供提高定位精度，增强不同运动速度下系统解算行人定位信息的适用性的一种基于自适应零速区间调整的惯性行人定位方法。

本发明通过如下技术方案来实现：

一种基于自适应零速区间调整的惯性行人定位方法，包括以下步骤：

步骤一：将惯性器件固定在行人足部，打开电源并初始化系统；

步骤二：系统实时采集惯性器件的输出数据，包括加速度计输出的加速度信息和陀螺仪输出的角速度信息；

步骤三：利用步骤二中采集到的角速度信息，解算行人足部的姿态信息，得到其中b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵；

步骤四：根据步骤二中采集的加速度f和步骤三中解算得的转换矩阵计算行人足部位置信息和姿态信息：利用将加速度计输出的加速度信息投影到导航坐标系后，有其中，f^b表示载体坐标系下的加速度信息，fⁿ表示导航坐标系下的加速度信息；加速度信息经过一次积分求解出速度信息：νⁿ＝∫fⁿdt，再次积分求解出位置信息：pⁿ＝∫vⁿdt；其中ν表示速度信息，p表示位置信息，pⁿ表示导航坐标系下的位置信息，t表示时间；

步骤五：系统利用加加速度和角加速度幅值判别法实时检测行人的运动状态，将行人运动状态分为慢走、快走和跑步；

步骤六：根据步骤五中检测得的行人运动状态，判断零速检测窗口长度n；

步骤七：读取步骤二中采集的足部加速度f和足部角速度ω，根据步骤六中零速窗口长度n，利用基于滑动窗口的加速度和角速度方差检测法判断行人足部运动状态，并用zupt表示足部运动状态。当足部处于离地状态时，记作zupt＝0；当行人足部触地时，记作zupt＝1；

步骤八：根据步骤五中零速检测结果，如果zupt＝0，则将步骤四中解算的定位信息作为最终导航信息并输出；如果zupt＝1，则转至步骤九；

步骤九：根据步骤七中加速度和角速度信息，解算行人的速度和位置由于理论上行人足部触地时有ν＝0，所以其中δν为速度误差。

步骤十：以速度误差δν、位置误差δp和姿态误差δε为状态量，以速度误差为观测量，对零速区间内数据进行kalman滤波；

步骤十一：利用步骤十中解算出的δν，δp，δε补偿并将补偿后的结果作为最终导航输出，其中，分别为系统解算的ν，p，ε的值；

步骤十二：重复步骤二到步骤十一，直至惯性行人定位系统导航结束。

本发明针对行人运动速度变化导致零速检测不准确的问题，在惯性行人定位系统解算行人位姿信息过程中，根据行人运动快慢自适应的调整零速检测窗口长度，并利用滑动窗口检测法实时检测行人足部的运动状态。在得到零速检测结果的基础上，通过零速校正算对行人的位姿信息进行补偿，提高定位精度，增强不同运动速度下系统解算行人定位信息的适用性。

本发明的有益效果在于：

本发明根据惯性行人定位系统位姿误差形式，建立了相应的误差传播模型。结合加加速度和角加速度幅值检测算法设计了自适应滑动窗口检测法，在系统解算过程中自适应地调整零速检测窗口长度，提高零速检测精度。再利用零速校正算法对零速区间内的行人位姿信息进行补偿，提高了不同运动速度下零速校正算法的适用性。零速校正后的定位信息作为最终导航解算输出信息。

增强了不同运动速度下零速校正算法的适用性，减小了因行人运动速度变化而导致的误差补偿不充分的问题；不需任何外界辅助信息就可提高定位精度；计算量小，满足实践中的实时性要求。

附图说明

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

图2为本发明中利用matlab仿真得到有无自适应零速区间调整算法的轨迹曲线。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明：

一种惯性行人定位系统定位误差抑制方法，方法流程如图1所示，包括以下步骤：

步骤一：将mimu器件水平固定在行人足部，其x、y、z轴分别指向行人的右、前、上方向。上电，运行。

导航初始时刻，需初始化系统：(1)初始化惯性定位系统初值：行人三轴位置信息(单位均为m)，三轴速度信息(单位均为ms)，以及三个姿态角信息φ0、θ0、ψ0((单位均为rad))，初始转换矩阵初始四元数q0；(2)初始化常值参量：当地重力海拔h(单位为m)，当地纬度λ(单位为rad)，运动状态检测窗口长度m，运动状态检测阈值γ′f、γ″f、γ′ω、γ″ω，零速检测阈值γ1、γ2，陀螺仪白噪声误差σω，采样时间t(单位为s)；(3)kalman滤波器参数初值：状态变量初值均方误差阵p0，系统噪声方差阵q，量测噪声方差阵r，量测阵h；将以上初始化信息装订至导航计算机中。

通常情况下，

姿态角φ0、θ0计算如下：

其中，φ0表示横滚角初始值，θ0表示俯仰角初始值，分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量。

初始转换矩阵计算如下：

其中，b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵。

初始四元数q0计算如下：

令则

其中，cij(i＝1,2,3，j＝1,2,3)表示中第i行第j列矩阵元素，[·]^t表示矩阵的转置；

当地重力加速度计算如下：

g＝9.780327×(1+0.0053024sin²λ-0.0000058sin²2λ)-(3.0877×10^-6-4×10^-9sin²λ)×h+7.2×10^-14×h²

其中，0为三行三列的零矩阵；

其余初值(例如h)需根据实际情况设定；

定位过程中，利用该初始信息进行更新，得到任意时刻行人的位置、速度和姿态信息；

步骤二：系统实时采集惯性器件的输出数据，包括加速度计输出的三轴加速度分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为m/s²)；陀螺仪输出的三轴角速度分别表示陀螺仪测量的角速度在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为rads)；

步骤三：利用步骤二中采集到的角速度信息，解算行人足部的姿态信息，得到其中b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵；具体过程如下：

在行人运动过程中，通过ω^b更新转换矩阵具体为：

四元数姿态矩阵更新：

设任意时刻载体系相对导航坐标系的转动四元数为：

q＝[q0q1q2q3]^t(1)

其中，q为四元数；q0为q的实数部分，q1、q2、q3为q的虚数部分。

四元数q的及时修正：

其中，分别表示q0、q1、q2、q3的变化率；

根据k时刻载体坐标系相对导航坐标系的转动四元数q0(k)、q1(k)、q2(k)、q3(k)，求取k时刻转动四元数的变化率为：

则k时刻载体的转动四元数具体为：

其中，i为单位阵，t为采样时间，式中ωx、ωy、ωz均省略了上角标b，当k＝1时，q(k-1)为步骤一中初始化系统时获得的载体初始四元数。

利用得到的q(k)中元素q0(k)、q1(k)、q2(k)、q3(k)，更新捷联矩阵

其中，(5)式中的qi(i＝1,2,3,4)为(4)式中qi(k)(i＝1,2,3,4)，(5)式中省略了下角标k。

更新载体姿态信息，具体为：

步骤四：根据步骤二中采集的加速度f和步骤三中解算得的转换矩阵计算行人足部位置信息和姿态信息：利用将加速度计输出的加速度信息投影到导航坐标系后，有其中，f^b表示载体坐标系下的加速度信息，fⁿ表示导航坐标系下的加速度信息；加速度信息经过一次积分求解出速度信息：νⁿ＝∫fⁿdt，再次积分求解出位置信息：pⁿ＝∫vⁿdt；其中ν表示速度信息，p表示位置信息，pⁿ表示导航坐标系下的位置信息，t表示时间；具体过程如下：

将加速度计沿载体坐标系测量的比力信息，通过捷联矩阵进行投影转换：

利用下列微分方程求解载体运动速度：

其中，分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的分量；表示vx、vy、vz的变化率，即载体沿导航系oxn轴、oyn轴、ozn轴的运动加速度；g为当地重力加速度。

根据k时刻的载体三轴速度vx(k)、vy(k)和vz(k)，求取k时刻载体速度变化率为：

在k时刻载体速度和位置分别为：

其中，分别表示载体位置在导航系oxn轴、oyn轴、ozn轴上的分量，当k＝1时，vx(1)、vy(1)、vz(1)为步骤一中初始化系统时获得的载体初始速度，为步骤一中初始化系统时获得的载体初始位置。

至此，根据(6)、(10)、(11)式，得到了行人的姿态、速度、位置。

步骤五：在工程上，加加速度又名“急动度”，它可以用来描述运动过程中力的变化快慢；在行人运动过程中，足部角加速度会因运动速度的不同而具有明显的差异性，故行人定位系统可以利用加加速度和角加速度幅值检测法实时检测行人的运动状态，将行人运动状态分为慢走(包含静止)、快走和跑步；具体过程如下：

k时刻，行人足部加加速度幅值为角加速度幅值为其中，||·||表示向量的2范数。

记m为行人运动状态检测窗口长度，对前m时刻的数据进行分析，若满足且则可认为行人处于跑步状态；若且则可认为行人处于快走状态；否则，认为行人处于慢走状态。其中，γ′f、γ″f、γ′ω、γ′ω为运动状态检测阈值，当k＜m时，属于行人运动初始时刻，此时行人足部一般的静止的，将其归类为慢走状态；

步骤六：根据步骤五中检测得的行人运动状态，判断零速检测窗口长度n；

若行人处于跑步状态，则零速检测窗口长度选取为n1；若行人处于快走状态，则零速检测窗口长度选取为n2；若行人处于慢走状态，则零速检测窗口长度选取为n3；

步骤七：读取步骤二中采集的足部加速度f和足部角速度ω，根据步骤六中零速窗口长度n，利用基于滑动窗口的加速度和角速度方差检测法判断行人足部运动状态，并用zupt表示足部运动状态。当足部处于离地状态时，记作zupt＝0；当行人足部触地时，记作zupt＝1，理论上认为此时足部与地面相对静止，即足部运动速度为0，故将这一段相对静止的时间称为“零速区间”。具体过程如下：

若k时刻，满足且则认为足部处于触地状态，记作zupt(k)＝1，理论上认为此时足部与地面相对静止，即足部运动速度为0，并将这一段相对静止的时间称为“零速区间”；反之，则认为足部处于离地状态，记作zupt(m)＝0。其中，γ1、γ2为零速检测阈值，σω为陀螺仪白噪声误差；

步骤八：根据步骤五中零速检测结果，如果zupt＝0，则将步骤四中解算的定位信息作为最终导航信息，并输出；如果zupt＝1，则转至步骤九；

步骤九：根据k-1时刻行人足部位姿信息，解算k时刻行人的速度和位置具体过程如下：

理论上行人足部触地时，位置保持不变，k时刻载体位置为：

速度为：

其中，分别为ν，p的计算值。

由于理论上行人足部触地时有ν＝0，所以其中δν为速度误差。

步骤十：以速度误差δν、位置误差δp和姿态误差δε为状态量，以速度误差δν为观测量，对零速区间内数据进行kalman滤波，具体过程如下：

利用下列微分方程建立载体运动速度、位置和姿态的误差模型：

其中：δp为载体位置误差，且δν为载体速度误差，且δε为载体姿态角误差并且有δε＝[δφδθδψ]。分别为δp，δν，δε的变化率，ω^b为载体角速度，且[fn×]为由加速度信息构成的反对称阵，且03×1为三行一列的零向量。

根据k时刻的载体位置误差δp(k)，载体速度误差δν(k)，载体姿态角误差δε(k)，求取k时刻载体位置误差变化率、速度误差变化率、姿态角误差变化率为：

在k时刻载体位置误差δp(k)、速度误差δν(k)、姿态角误差δε(k)分别为：

其中，式(17)和式(18)中省去了上角标n。

由于理论上行人足部触地时，应有ν＝0，故此时系统解算出的速度信息ν为误差量，即ν＝δν，其中δν为速度误差。

根据上述误差模型，利用卡尔曼滤波器，对行人的位姿信息进行误差修正，具体算法如下：

kk＝pkh^t(hpkh^t+r)^-1(18)

其中，pk为k时刻的估计均方误差阵，fk,k-1为k-1时刻到k时刻的为状态转移矩阵，且i为单位阵，0为零矩阵，为k-1时刻的噪声驱动阵，kk为k时刻的滤波增益阵。

xk＝-kkzk(19)

更新pk：

其中，xk为k时刻的状态变量，当k＝1时，状态量x＝[δp(1)δν(1)δε(1)]为步骤一中初始化系统时获得的载体初始状态量x0，pknew为更新后的pk。

保存pk与xk，并将其作为下一次滤波的输入。

式(17)应当在整个定位过程中实时结算，而非只针对零速区间。

步骤十一：利用步骤十中解算出的δν，δp，δε补偿并将补偿后的结果作为最终导航输出，其中，分别为系统解算的ν，p，ε的值。具体过程如下：

利用步骤九中解算出的δν，δp，δε补偿步骤八中的更新四元数qk，并将补偿后的结果作为最终导航输出：

位置、速度信息补偿：

姿态角补偿：

令利用公式更新转换矩阵利用式(6)求出更新后的φ，θ，ψ；

其中，[·]^-1为矩阵的逆运算，为步骤三中的计算值。

更新四元数：

令则

至此，k时刻行人足部的位置、速度和姿态信息误差补偿完毕。将补偿后的位姿信息作为最终导航结果，并输出。

步骤十二：重复步骤二到步骤十一，直至惯性行人定位系统导航结束。

实施例：

对本发明的有益效果如下方式得以验证：

采用mti-710系列mimu构建惯性行人定位系统，进行惯性行人定位系统的变速行走试验。

行人预设路线为一条直线，行人变速(先快速再慢速)行进22.69秒，行进路程长度为48.4米；

系统初始化参数如下：

载体三轴位置信息：

载体三轴速度信息：

载体航向角信息：ψ0＝0rad；

当地重力海拔：h＝100m；

当地纬度：λ＝0.7988rad；

采样时间：t＝0.01s；

陀螺仪白噪声误差：σω＝0.00506；

运动状态检测窗口长度：m＝3；

运动状态检测阈值：γ′f＝15；

γ″f＝6.15、

γ′ω＝2；

γ′ω＝0.45，

零速检测阈值：γ1＝8500；

γ2＝0.3；

kalman滤波器初始参数：

初始状态量：

初始均方误差阵：

系统噪声方差阵：

量测噪声方差阵：

利用发明所述方法，得到行人变速行走时有、无自适应零速区间调整算法的定位轨迹比较曲线，其中实线、虚线分别表示有自适应零速区间调整算法的定位轨迹曲线，无自适应零速区间调整算法的定位轨迹曲线。结果表明本发明抑制行人变速运动时定位误差能力较好，可以满足实际需求。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。