CN114608571A - 一种适用于运动平台场景的行人惯性导航方法 - Google Patents

Abstract

本发明公开了一种适用于运动平台场景的行人惯性导航方法，包括步骤：(1)采集行人足绑式IMU数据，估计零速检测阈值；(2)预处理数据，进行速度、姿态、位置的实时解算；(3)建立步态检测模型，利用估计的零速检测阈值进行零速检测；(4)处于零速状态时，利用Kalman滤波器进行零速修正；否则不进行修正；(5)根据步骤(2)解算得到相关速度与设定阈值比较判断当前时刻是否存在运动平台以及运动平台的类型；当检测到某类运动平台的存在，对该类平台运动子状态分类，施加不同的速度约束，利用滤波方法对行人速度、姿态和位置进行修正；若未检测到运动平台，则重复步骤(2)‑(5)。本发明解决了运动平台场景中行人惯性导航定位不准的问题。

Description

一种适用于运动平台场景的行人惯性导航方法

技术领域

本发明属于行人惯性导航***技术领域，涉及一种适用于运动平台场景的行人惯性导航方法。

背景技术

室内行人定位服务是物联网((IoT))、大数据、人工智能(AI)等领域的核心支撑技术，随着这些技术的蓬勃发展，室内定位的重要性愈加凸显。基于随着MEMS技术的迅速发展和普及，使得基于MEMS惯性传感器的行人导航***得以实现，常见的行人惯性导航***是一种将惯性传感器固联于行人足部，该***在传统SINS算法的基础上，结合足部运动特有的步态特性，利用零速修正(Zero Velocity Updata,ZUPT)方法，可周期性地实现导航误差的校正。因此零速修正在惯导***中占据着至关重要的地位，它是进行约束惯导误差发散的有效手段。

目前的行人惯性导航方法仅针对静止地面场景开展相关研究，然而当行人在写字楼、商场等的高楼层建筑中，乘坐垂直电梯或自动扶梯等外部运动平台时，零速检测算法往往会误检为零速状态，从而构成错误的速度观测，***的速度和位置误差会迅速发散。因此在外部运动平台场景中，不进行速度误差约束或是错误的速度误差约束，无法满足高精度、可靠的行人定位导航需求。

发明内容

发明目的：针对以上缺点，本发明提供一种适用于运动平台场景的行人惯性导航方法，解决了运动平台场景中行人惯性导航定位不准的问题，减少行人导航定位的累积误差，提高导航定位精度。

技术方案：为解决上述问题，本发明一种适用于运动平台场景的行人惯性导航方法，包括以下步骤：

(1)采集行人足绑式IMU数据，通过初始静态数据估计零速检测阈值；

(2)对采集的足绑式IMU数据进行预处理并采用捷联惯性导航方法，进行速度、姿态、位置的实时解算；

(3)建立腿部及足部四节点的步态检测模型，利用估计的零速检测阈值进行零速检测；

(4)当零速检测结果处于零速状态，则利用Kalman滤波器进行零速修正，得到修正后的速度、姿态和位置，并对IMU器件进行误差校正；否则不进行修正；

(5)根据步骤(2)解算得到相关速度与设定阈值比较判断当前时刻是否存在运动平台以及运动平台的类型；当检测到某类运动平台的存在，同步进行该类平台运动子状态分类，根据不同子状态的运动特点施加不同的速度约束，利用滤波方法对行人速度、姿态和位置进行修正；若未检测到运动平台，则重复步骤(2)-(5)。

进一步的，步骤(5)中所述的运动平台检测与分类具体为：

若满足：

则表示k时刻行人处于自动扶梯；其中，V_k表示地理系下速度矢量，

表示天向速度矢量，T_escal,norm表示自动扶梯的速度阈值，T_escal,U表示自动扶梯的天向速度阈值；

若满足：

则表示k时刻行人处于垂直电梯，且电梯开始运行；其中，V_k表示地理系下速度矢量，

表示天向速度矢量，T_ele,norm表示垂直电梯的速度阈值，T_ele,U表示垂直电梯的天向速度阈值。

进一步的，当前时刻处于自动扶梯，则自动扶梯的全程运动状态分为五个子状态S11—S15，且五个子状态以固定顺序切换；其中，子状态S11表示行人处于扶梯入口，采用速度约束方法为：先进行侧向零速修正后再进行垂直方向零速修正；子状态S12表示行人位于靠近扶梯入口的拐弯位置，采用速度约束方法为：先进行侧向零速修正后再进行速度模值恒定修正；子状态S13表示行人位于扶梯上，采用速度约束方法为：先进行侧向零速修正后再进行前向、垂直方向恒速修正；子状态S14表示位于靠近扶梯出口的拐弯位置，采用速度约束方法为：先进行侧向零速修正后再进行速度模值恒定修正；子状态S11表示行人处于扶梯出口，采用速度约束方法为：先进行侧向零速修正后再进行垂直方向零速修正；

当前时刻处于垂直电梯，则垂直电梯的全程运动状态分为三个子状态S21—S23，且五个子状态以固定顺序切换；子状态S21表示垂直电梯加速向上运行，采用速度约束的方法是：进行水平零速修正；子状态S22表示垂直电梯恒速向上运行，采用速度约束的方法是：先进行水平零速修正后进行垂直方向恒速修正；子状态S23表示垂直电梯减速向上运行，采用速度约束的方法是：进行水平零速修正。

进一步的，所述侧向零速修正、水平零速修正、垂直零速修正方法均是零速修正方法的低维度表征，具体的公式为：

Z_zupt,lateral＝δV_lateral＝V^E-0＝H_lateralX+v_lateral。

Z_zupt,horiz＝δV_horiz＝[V^E V^N]^T-[0 0]^T＝H_horizX+v_horiz

Z_zupt,vert＝δV_vert＝V^U-0＝H_vertX+v_vert

其中，Z_zupt,lateral表示侧向零速修正的观测量、Z_{zupt_horiz}表示水平零速修正的观测量、Z_zupt,vert表示垂直零速修正的观测量；δV_lateral表示侧向零速修正的IMU速度误差量、δV_horiz表示水平零速修正的IMU速度误差量、δV_vert表示垂直速修正的IMU速度误差量；V^E表示东向的速度矢量、V^N表示北向的速度矢量、V^U表示天向的速度矢量；H_lateral＝[0_1×3 1 0_1×14]，H_horiz＝[0_2×3 I_2×2 0_2×13]，H_vert＝[0_1×5 1 0_1×12]；v_lateral表示侧向零速修正的观测噪声矩阵、v_horiz表示侧向零速修正的观测噪声矩阵、v_vert表示垂直零速修正的观测噪声矩阵；

速度模值恒定修正方法公式如下所示：

Z_{cons_norm}＝δV_k,norm＝V_k,norm-V_ob,norm＝f(V_k)-V_ob,norm+n_n

其中，T₁₁和T₁₂表示进入子状态S₁₁和S₁₂的时间；V_ob,norm表示运动子状态S₁₁过程中解算得到的速度向量模的均值，V_k,norm表示k时刻速度向量的模值，

表示k时刻东向的速度矢量、

表示k时刻北向的速度矢量、

表示k时刻天向的速度矢量；f( )表示求模函数,Z_{cons_norm}表示速度模值恒定修正的观测量，δV_k,norm表示k时刻中足绑式IMU的速度模值误差,n_n为观测噪声矩阵；

前向、垂直方向速度矢量恒定修正方法观测方程公式如下所示：

其中，Z_{cons_vect}表示前向、垂直方向速度矢量恒定修正的速度误差观测量；δV_k,vect表示k时刻东向、北向速度矢量与k-1时刻之差，n_v为观测噪声矩阵；H_{cons_vect}＝[0_2×3 I_2×20_2×13]。

进一步的，所述的子状态的分类算法公式为：

其中，Count_k,fp表示前向搜索标签，N表示滑动窗口长度，sgn( )表示符号函数，A^U表示加速度天向比力输出，g为当地重力加速度数值；

当运动平台为扶梯时，

当Count_k,fp＝1，表示在k时刻，子状态切换至S₁₁；

当0.9N≤Count_k,fp≤N，表示在k时刻，子状态由S₁₁切换至S₁₂；

当-0.2N≤Count_k,fp≤0.2N，表示在k时刻，子状态由S₁₂切换至S₁₃；

当-N≤Count_k,fp≤-0.9N，表示在k时刻，子状态由S₁₃切换至S₁₄；

当-0.2N≤Count_k,fp≤0.2N，表示在k时刻，子状态由S₁₄切换至S₁₅；

当运动平台为垂直电梯时，

当0.9N≤Count_k,fp≤N，表示在k时刻，行人处于垂直电梯开始运动，子状态切换至S₂₁；

当-0.2N≤Count_k,fp≤0.2N，表示在k时刻，子状态由S₂₁切换至S₂₂；

当-N≤Count_k,fp≤-0.9N，表示在k时刻，子状态由S₂₂切换至S₂₃。

进一步的，步骤(1)中估计零速检测阈值具体为：以足绑式惯性传感器的中心为坐标原点O，右向为X轴，Y轴垂直于X轴指向前向，Z轴指向上；采集行人足绑式IMU数据，根据初始静态区间中加速度和角速度的数据特性，获得零速检测阈值；所述检测阈值包括加速度模值的最小阈值T_A,min、加速度模值的最大阈值T_A,max、加速度标准差的阈值

角速度模值的阈值T_W、角速度速度标准差的阈值

计算公式如下：

其中，A_staic为静态区间内的加速度模的均值、σ_{A_static}为静态区间内的加速度模的标准差、W_staic为静态区间内的角速度模的均值、σ_{W_static}为静态区间内的角速度模的标准差；c_A,min表示为加速度模值最小常值系数、c_A,max表示为加速度模值最大常值系数，

表示为加速度标准差、

表示为角速度的模值、c_W表示为标准差的常值系数。

进一步的，步骤(2)具体包括以下步骤：

(2.1)从IMU静态数据中计算并扣除加速度计零偏ε_b和陀螺仪的零偏

(2.2)利用静态下加速度计的比力输出计算初始的姿态角，主要包括俯仰角θ和滚转角γ：

其中，g_x为静态下足绑式IMU的X轴的比力输出；g_y为静态下足绑式IMU的Y轴的比力输出；g_z为静态下足绑式IMU的Z轴的比力输出；

(2.3)实时解算行人的姿态、速度、位置

足绑式IMU的姿态更新算法公式如下：

其中，Q表示四元数矩阵，t_k-1表示k-1时刻，t_k表示k时刻，即k-1时刻的下一采样时刻；ψ表示航向角、θ表示俯仰角、γ表示滚转角，

表示姿态；

足绑式IMU的速度更新算法公式如下：

其中，V_k为k时刻地理系下的运动速度矢量，V_k-1为k-1时刻地理系下的运动速度矢量；

表示机体系到导航系的坐标转换矩阵，

代表比力，gⁿ表示当地重力加速度，Δt表示采样时间间隔；

足绑式IMU的位置更新算法公式如下：

其中，L表示代表经度，λ表示纬度，h表示高度；R_m表示地球子午面内曲率半径，R_n表示地球卯酉面内曲率半径。

进一步的，步骤(3)中腿部及足部四节点的步态检测模型为：

ZVD(k)＝S₁(k)&S₂(k)&S₃(k)&S₄(k)

其中，

代表第k至k+N时刻内的加速度的均值、σ_A,k:k+n代表第k至k+N时刻内的加速度的标准差、

代表第k至k+N时刻内的角速度的均值、σ_W,k:k+N代表第k至k+N时刻内的角速度的标准差；T_A,min为加速度的最小阈值、T_A,max为加速度的最大阈值、

为加速度标准差的阈值、T_W为角速度的阈值、

为角速度标准差的阈值；当S₁(k)、S₂(k)、S₃(k)、S₄(k)同时为1时同时为1时，ZVD(k)＝1，表示当前为零速状态。

进一步的，步骤(4)具体包括以下步骤：

(4.1)当检测足部处于零速状态时，利用卡尔曼滤波计算进行误差估计，构建18维的卡尔曼滤波器状态量X：

其中，

分别表示东向、北向、天向的平台误差角；δV＝[δV_EδV_N δV_U]^T分别为东向、北向、天向的速度误差；δP＝[δL δλ δh]^T分别表示经度、纬度、高度的误差；

分别表示陀螺X轴、Y轴、Z轴的零偏误差；

分别表示陀螺X轴、Y轴和Z轴的一阶马尔可夫漂移误差；

分别表示加速度计X轴、Y轴和Z轴的一阶马尔可夫漂移误差；

(4.2)建立状态方程：

其中，A表示状态转移矩阵，G表示***噪声转移矩阵，W表示***噪声；

(4.3)建立量测方程：

Z_zupt＝δV＝V_INS-V_ZUPT＝HX+v

其中，Z_zupt表示观测量，即为IMU速度误差量δV，V_INS为捷联惯导解算得到的IMU速度矢量，V_ZUPT的理论值为[0 0 0]^T，H是观测矩阵，v为观测噪声矩阵，表示速度观测不确定性；

(4.4)导航误差修正和IMU器件误差修正：

根据步骤(4.1)、(4.2)、(4.3)构建Kalman滤波器，求解得到最优估计状态量

从步骤(2)捷联惯导解算结果中相应扣除前九维状态量

得到修正后的姿态、速度和位置，在IMU数据中扣除后九维状态量

对IMU的零偏误差和一阶马尔可夫漂移误差进行修正。

进一步的，步骤(5)中利用滤波方法对行人速度、姿态和位置进行修正具体为；

其中，A表示状态转移矩阵，Q表示状态转移协方差矩阵，

表示k时刻的先验估计协方差，P_k-1表示表示k-1时刻的后验估计协方差，K_k表示卡尔曼滤波增益，J表示雅可比矩阵，R表示测量噪声协方差，Z表示观测量，

表示由***方程得到k时刻的状态先验估计，X_k由Kalman滤波器求解得到的最优估计，

表示k时刻的后验估计协方差，I表示单位矩阵；

求解最优估计误差状态量X_k后，对位置、速度、姿态和IMU器件误差进行再次修正。

有益效果：本发明相对于现有技术，其显著优点是：通过分析不同运动平台的不同状态下的运动特点，针对性的施加不同的速度约束，并结合滤波的方法对行人的速度、姿态和位置进行修正，从而有效弥补了在运动平台场景的行人惯性可靠导航问题，有效抑制了惯性导航***的误差发散。

附图说明

图1所示为本发明的流程图；

图2所示为本发明中自动扶梯子状态分类图；

图3所示为本发明中垂直电梯子状态分类图；

图4所示为本实施例方法自动扶梯定位结果与传统方法结果对比图；

图5所示为本实施例方法垂直电梯定位结果与传统方法结果对比图。

具体实施方式

下面结合附图对本发明的技术方案进一步说明。

如图1所示，一种适用于运动平台场景的行人惯性导航方法，包括以下步骤：

步骤一：采集行人足绑式IMU数据，利用足绑式IMU输出的1000个静态数据，得到零速检测阈值。

具体的，以足绑式惯性传感器的中心为坐标原点O，右向为X轴，Y轴垂直于X轴指向前向，Z轴指向上，三轴满足右手螺旋定则。采集足绑式惯性传感器输出的1000个静态数据，根据初始静态区间中加速度和角速度的数据特性，获得零速检测的加速度和角速度阈值。

所述检测阈值包括加速度模值的最小阈值T_A,min、加速度模值的最大阈值T_A,max、加速度标准差的阈值

角速度模值的阈值T_W、角速度速度标准差的阈值

计算公式如下：

其中，A_staic为静态区间内的加速度模的均值、σ_{A_static}为静态区间内的加速度模的标准差、W_staic为静态区间内的角速度模的均值、σ_{W_static}为静态区间内的角速度模的标准差；c_A,min表示为加速度模值最小常值系数、c_A,max表示为加速度模值最大常值系数，

表示为加速度标准差、

表示为角速度的模值、c_W表示为标准差的常值系数。

步骤二：在初始时静止时刻，进行初始姿态求解和器件的零偏校准，然后利用捷联惯性导航方法进行实时解算。

(1)从IMU静态数据中计算并扣除加速度计零偏ε_b和陀螺仪的零偏

(2)利用静态下加速度计的比力输出计算初始的姿态角，主要包括俯仰角θ和滚转角γ：

其中，g_x为静态下足绑式IMU的X轴的比力输出；g_y为静态下足绑式IMU的Y轴的比力输出；g_z为静态下足绑式IMU的Z轴的比力输出；

(3)实时解算行人的姿态、速度、位置

足绑式IMU的姿态更新算法公式如下：

其中，Q表示四元数矩阵，t_k-1表示k-1时刻，t_k表示k时刻，即k-1时刻的下一采样时刻，Δθ表示采样时间内陀螺仪三轴的角度增量，ΔΘ表示由Δθ构成的反对称矩阵，ψ表示航向角、θ表示俯仰角、γ表示滚转角，

表示姿态；

足绑式IMU的速度更新算法公式如下：

其中，V_k为k时刻地理系下的运动速度矢量，V_k-1为k-1时刻地理系下的运动速度矢量；

表示机体系到导航系的坐标转换矩阵，

代表比力，gⁿ表示当地重力加速度，Δt表示采样时间间隔；

足绑式IMU的位置更新算法公式如下：

其中，L表示代表经度，λ表示纬度，h表示高度；R_m表示地球子午面内曲率半径，R_n表示地球卯酉面内曲率半径；V^E表示东向的速度矢量、V^N表示北向的速度矢量、V^U表示天向的速度矢量。

步骤三：建立步态检测模型，以加速度模值、足部加速度标准差、角速度模值、角速度标准差作为检测条件，结合估计的零速检测阈值，进行零速检测。将足部运动状态分为摆动相和支撑相，当检测处于零速状态时，则足部处于支撑相，否则为摆动相。

具体的，腿部及足部四节点的步态检测模型为：

ZVD(k)＝S₁(k)&S₂(k)&S₃(k)&S₄(k)

其中，

代表第k至k+N时刻内的加速度的均值、σ_A,k:k+n代表第k至k+N时刻内的加速度的标准差、

代表第k至k+N时刻内的角速度的均值、σ_W,k:k+N代表第k至k+N时刻内的角速度的标准差；T_A,min为加速度的最小阈值、T_A,max为加速度的最大阈值、

为加速度标准差的阈值、T_W为角速度的阈值、

为角速度标准差的阈值；当标签S₁(k)、S₂(k)、S₃(k)、S₄(k)同时为1时，ZVD(k)＝1，表示当前为零速状态。

步骤四：捷联惯导***解算过程中，当检测到足部为零速状态时，利用卡尔曼滤波计算进行误差估计，得到修正后的位置、速度，否则不进行修正；

(4.1)当检测足部处于零速状态时，利用卡尔曼滤波计算进行误差估计，构建18维的卡尔曼滤波器状态量X：

其中，

分别表示东向、北向、天向的平台误差角；δV＝[δV_EδV_N δV_U]^T分别为东向、北向、天向的速度误差；δP＝[δL δλ δh]^T分别表示经度、纬度、高度的误差；

分别表示陀螺X轴、Y轴、Z轴的零偏误差；

分别表示陀螺X轴、Y轴和Z轴的一阶马尔可夫漂移误差；

分别表示加速度计X轴、Y轴和Z轴的一阶马尔可夫漂移误差；

(4.2)建立状态方程：

其中，A表示状态转移矩阵，G表示***噪声转移矩阵，W表示***噪声；

为状态量X的一阶导数；W＝[w_gx w_gy w_gz]^T，w_gx为陀螺X轴的白噪声，w_gy为陀螺Y轴的白噪声，w_gz为陀螺Z轴的白噪声；

(4.3)建立量测方程：

Z_zupt＝δV＝V_INS-V_ZUPT＝HX+v

其中，Z_zupt表示观测量，即为IMU速度误差量δV，V_INS为捷联惯导解算得到的IMU速度矢量，V_ZUPT的理论值为[0 0 0]^T，H是观测矩阵，v为观测噪声矩阵，表示速度观测不确定性；

(4.4)导航误差修正和IMU器件误差修正：

根据步骤(4.1)、(4.2)、(4.3)构建Kalman滤波器，求解得到最优估计状态量

即姿态、速度、位置的估计误差；从步骤二捷联惯导解算结果中相应扣除前九维状态量

得到修正后的姿态、速度和位置，在IMU数据中扣除后九维状态量

对IMU的零偏误差和一阶马尔可夫漂移误差进行修正。

步骤五：根据步骤二解算得到相关速度与设定阈值比较判断当前时刻是否存在运动平台以及运动平台的类型；当检测到某类运动平台的存在，同步进行该类平台运动子状态分类，根据不同子状态的运动特点施加不同的速度约束，利用滤波方法对行人速度、姿态和位置进行修正；若未检测到运动平台，则重复步骤二至步骤。具体如下：

(1)运动平台检测与分类：

步骤二解算得到的地理系下速度矢量V_k和天向速度矢量

的模值进行运动平台检测与分类：

自动扶梯检测公式如下：

其中，T_escal,norm表示自动扶梯的速度阈值，T_escal,U表示自动扶梯的天向速度阈值；

垂直电梯检测公式如下：

则表示k时刻行人处于垂直电梯，且电梯开始运行；T_ele,norm表示垂直电梯的速度阈值，T_ele,U表示垂直电梯的天向速度阈值。

(2)检测到某类运动平台的存在，同步进行该类平台运动子状态分类，子状态分类模型为：

(2.1)以上行为例，根据自动扶梯运行过程中梯级速度矢量方向不同，将自动扶梯的全程运动状态分为五个子状态S11—S15，且五个子状态以固定顺序(S11-S12-S13-S14-S15)切换，如下图2所示：

子状态S11，表示行人处于自动扶梯入口；电梯仅有前向速度，且速度矢量恒定；

子状态S12，表示行人位于靠近自动扶梯入口的拐弯位置；电梯前向速度减小，垂直方向速度增大，且速度模值恒定，值与子状态S11一致；

子状态S13，表示行人位于自动扶梯上随自动扶梯向上运动；电梯有前向速度和垂直方向速度，速度矢量恒定，且速度模值与子状态S11一致；

子状态S14，表明行人位于靠近自动扶梯出口的拐弯位置；电梯前向速度增大，垂直方向速度减小，且速度模值与子状态S11一致；

子状态S15中，表示行人处于自动扶梯出口，电梯仅有前向速度，且速度矢量恒定，且速度模值与子状态S11一致。

(2.2)根据垂直电梯运行过程速度矢量模值变化规律不同，将垂直电梯的全程运动状态分为三个子状态S21—S23，且三个子状态以固定顺序(S21-S22-S23)切换如下图3所示。同时，在垂直电梯运行过程中，仅有垂直方向速度，水平方向速度为0；

子状态S21，表示垂直电梯加速向上运行，垂直方向速度从0逐渐增大至定值；

子状态S22，表示垂直电梯恒速向上运行，垂直方向速度恒定；

子状态S23，表示垂直电梯减速向上运行，垂直方向速度减小至0。

(3)由上述分析可知，通过比较滑动窗口内垂直方向绝对加速的正负，取前向搜索标签Count_k,fp判断运动子状态切换条件是否成立。针对不同平台对具体的子状态分类算法：

其中，Count_k,fp表示前向搜索标签，N表示滑动窗口长度，sgn( )表示符号函数，参数为正数则返回1，参数为0则返回0，参数为负数则返回；A^U表示加速度天向比力输出，

g为当地重力加速度数值。

当运动平台为自动扶梯时，以自动扶梯上行为例：

当Count_k,fp＝1，表示在k时刻，子状态切换至S₁₁；

当0.9N≤Count_k,fp≤N，表示在k时刻，子状态由S₁₁切换至S₁₂；

当-0.2N≤Count_k,fp≤0.2N，表示在k时刻，子状态由S₁₂切换至S₁₃；

当-N≤Count_k,fp≤-0.9N，表示在k时刻，子状态由S₁₃切换至S₁₄；

当-0.2N≤Count_k,fp≤0.2N，表示在k时刻，子状态由S₁₄切换至S₁₅；

当运动平台为垂直电梯时，以垂直电梯上升为例：

当0.9N≤Count_k,fp≤N，表示在k时刻，行人处于垂直电梯开始运动，子状态切换至S₂₁；

当-0.2N≤Count_k,fp≤0.2N，表示在k时刻，子状态由S₂₁切换至S₂₂；

当-N≤Count_k,fp≤-0.9N，表示在k时刻，子状态由S₂₂切换至S₂₃。

综上，即可判断出垂直电梯和自动扶梯的各个运动子状态的时间区间。

(4)基于子状态分类设置相对应的速度约束：

根据自动扶梯和垂直电梯运动过程中不同子状态的运动特性，设定不同的速度约束方法，如下表所示：

其中，所述侧向零速修正、水平零速修正、垂直零速修正方法均是步骤四中零速修正方法的低维度表征，具体的公式为：

Z_zupt,lateral＝δV_lateral＝V^E-0＝H_lateralX+v_lateral。

Z_zupt,horiz＝δV_horiz＝[V^E V^N]^T-[0 0]^T＝H_horizX+v_horiz

Z_zupt,vert＝δV_vert＝V^U-0＝H_vertX+v_vert

其中，Z_zupt,lateral表示侧向零速修正的观测量、Z_{zupt_horiz}表示水平零速修正的观测量、Z_zupt,vert表示垂直零速修正的观测量；δV_lateral表示侧向零速修正的IMU速度误差量、δV_horiz表示水平零速修正的IMU速度误差量、δV_vert表示垂直速修正的IMU速度误差量；V^E表示东向的速度矢量、V^N表示北向的速度矢量、V^U表示天向的速度矢量；H_lateral＝[0_1×3 1 0_1×14]，H_horiz＝[0_2×3 I_2×2 0_2×13]，H_vert＝[0_1×5 1 0_1×12]；v_lateral表示侧向零速修正的观测噪声矩阵、v_horiz表示侧向零速修正的观测噪声矩阵、v_vert表示垂直零速修正的观测噪声矩阵。

速度模值恒定修正方法公式如下所示：

Z_{cons_norm}＝δV_k,norm＝V_k,norm-V_ob,norm＝f(V_k)-V_ob,norm+n_n

其中，T₁₁和T₁₂表示进入子状态S₁₁和S₁₂的时间；V_ob,norm表示运动子状态S₁₁过程中解算得到的速度向量模的均值，V_k,norm表示k时刻速度向量的模值，

表示k时刻东向的速度矢量、

表示k时刻北向的速度矢量、

表示k时刻天向的速度矢量；f( )表示求模函数,Z_{cons_norm}表示速度模值恒定修正的观测量，δV_k,norm表示k时刻中足绑式IMU的速度模值误差,n_n为观测噪声矩阵；

将上式非线性方程作泰勒级数展开并保留线性项后，获得线性观误差量测方程：

Z_{cons_n}＝δV_k,norm＝JX+n_n

其中J可表示非线性函数相对于***状态向量X的雅可比矩阵，公式为

前向、垂直方向速度矢量恒定修正方法观测方程公式如下所示：

其中，Z_{cons_vect}表示前向、垂直方向速度矢量恒定修正的速度误差观测量；δV_k,vect表示k时刻东向、北向速度矢量与k-1时刻之差，n_v为观测噪声矩阵；H_{cons_vect}＝[0_2×3 I_2×20_2×13]。

根据上述***方程和量测方程应用应用扩展卡尔曼滤波方法对行人速度、姿态和位置进行修正具体为；

其中，A表示状态转移矩阵，Q表示状态转移协方差矩阵，

表示k时刻的先验估计协方差，P_k-1表示表示k-1时刻的后验估计协方差，K_k表示卡尔曼滤波增益，J表示雅可比矩阵，R表示测量噪声协方差，Z表示观测量，

表示由***方程得到k时刻的状态先验估计，X_k由Kalman滤波器求解得到的最优估计，

表示k时刻的后验估计协方差，I表示单位矩阵；

求解最优估计误差状态量X_k后，对位置、速度、姿态和IMU器件误差进行再次修正。

本实施例的实验场景选用教学实验楼，分别为自动扶梯和垂直电梯实验，路线总长度分别为41.5m和63.2m。

如图4所示，自动扶梯实验中，测试者以1.5m/s的步行前进，行进过程中遇到自动扶梯不做停顿，正常乘坐。在乘坐电梯过程中，仅在行走时使用零速修正算法导航解算结果如图虚线所示，惯导误差迅速发散，起终点误差大于20米；利用本发明即一种适用于运动平台场景的行人惯性导航方法解算结果如图实线所示，起终点位置误差为0.53米，水平和高度定位误差都大大减小，有效提升了定位精度。

如图5所示，和垂直电梯实验中，在总高度为10层建筑物内，测试者实验开始时静止在1楼电梯内，随后在随机楼层进行8次停靠，最终停靠在2楼处。不使用动平台速度误差约导航方法解算出来的结果如图虚线所示，惯导误差迅速发散，起终点误差大于100米；利用本发明即一种适用于运动平台场景的行人惯性导航方法解算出来的结果如图实线所示，起终点位置误差为1.7米，高度定位误差都大大减小，有效提升了定位精度。

Claims (10)

1.一种适用于运动平台场景的行人惯性导航方法，其特征在于，包括以下步骤：

(1)采集行人足绑式IMU数据，通过初始静态数据估计零速检测阈值；

(2)对采集的足绑式IMU数据进行预处理并采用捷联惯性导航方法，进行速度、姿态、位置的实时解算；

(3)建立腿部及足部四节点的步态检测模型，利用估计的零速检测阈值进行零速检测；

(4)当零速检测结果处于零速状态，则利用Kalman滤波器进行零速修正，得到修正后的速度、姿态和位置，并对IMU器件进行误差校正；否则不进行修正；

(5)根据步骤(2)解算得到相关速度与设定阈值比较判断当前时刻是否存在运动平台以及运动平台的类型；当检测到某类运动平台的存在，同步进行该类平台运动子状态分类，根据不同子状态的运动特点施加不同的速度约束，利用滤波方法对行人速度、姿态和位置进行修正；若未检测到运动平台，则重复步骤(2)-(5)。

2.根据权利要求1所述的适用于运动平台场景的行人惯性导航方法，其特征在于，步骤(5)中所述的运动平台检测与分类具体为：

若满足：

则表示k时刻行人处于自动扶梯；其中，V_k表示地理系下速度矢量，

表示天向速度矢量，T_escal,norm表示自动扶梯的速度阈值，T_escal,U表示自动扶梯的天向速度阈值；

若满足：

则表示k时刻行人处于垂直电梯，且电梯开始运行；其中，V_k表示地理系下速度矢量，

表示天向速度矢量，T_ele,norm表示垂直电梯的速度阈值，T_ele,U表示垂直电梯的天向速度阈值。

3.根据权利要求2所述的适用于运动平台场景的行人惯性导航方法，其特征在于，

当前时刻处于自动扶梯，则自动扶梯的全程运动状态分为五个子状态S11—S15，且五个子状态以固定顺序切换；其中，子状态S11表示行人处于扶梯入口，采用速度约束方法为：先进行侧向零速修正后再进行垂直方向零速修正；子状态S12表示行人位于靠近扶梯入口的拐弯位置，采用速度约束方法为：先进行侧向零速修正后再进行速度模值恒定修正；子状态S13表示行人位于扶梯上，采用速度约束方法为：先进行侧向零速修正后再进行前向、垂直方向恒速修正；子状态S14表示位于靠近扶梯出口的拐弯位置，采用速度约束方法为：先进行侧向零速修正后再进行速度模值恒定修正；子状态S11表示行人处于扶梯出口，采用速度约束方法为：先进行侧向零速修正后再进行垂直方向零速修正；

当前时刻处于垂直电梯，则垂直电梯的全程运动状态分为三个子状态S21—S23，且三个子状态以固定顺序切换；子状态S21表示垂直电梯加速向上运行，采用速度约束的方法是：进行水平零速修正；子状态S22表示垂直电梯恒速向上运行，采用速度约束的方法是：先进行水平零速修正后进行垂直方向恒速修正；子状态S23表示垂直电梯减速向上运行，采用速度约束的方法是：进行水平零速修正。

4.根据权利要求3所述的适用于运动平台场景的行人惯性导航方法，其特征在于，所述侧向零速修正、水平零速修正、垂直零速修正方法均是零速修正方法的低维度表征，具体的公式为：

Z_zupt,lateral＝δV_lateral＝V^E-0＝H_lateralX+v_lateral

Z_zupt,horiz＝δV_horiz＝[V^E V^N]^T-[0 0]^T＝H_horizX+v_horiz

Z_zupt,vert＝δV_vert＝V^U-0＝H_vertX+v_vert

其中，Z_zupt,lateral表示侧向零速修正的观测量、Z_{zupt_horiz}表示水平零速修正的观测量、Z_zupt,vert表示垂直零速修正的观测量；δV_lateral表示侧向零速修正的IMU速度误差量、δV_horiz表示水平零速修正的IMU速度误差量、δV_vert表示垂直速修正的IMU速度误差量；V^E表示东向的速度矢量、V^N表示北向的速度矢量、V^U表示天向的速度矢量；H_lateral＝[0_1×3 1 0_1×14]，H_horiz＝[0_2×3 I_2×2 0_2×13]，H_vert＝[0_1×5 1 0_1×12]；v_lateral表示侧向零速修正的观测噪声矩阵、v_horiz表示侧向零速修正的观测噪声矩阵、v_vert表示垂直零速修正的观测噪声矩阵；

速度模值恒定修正方法公式如下所示：

Z_{cons_norm}＝δV_k,norm＝V_k,norm-V_ob,norm＝f(V_k)-V_ob,norm+n_n

其中，T₁₁和T₁₂表示进入子状态S₁₁和S₁₂的时间；V_ob,norm表示运动子状态S₁₁过程中解算得到的速度向量模的均值，V_k,norm表示k时刻速度向量的模值，

表示k时刻东向的速度矢量、

表示k时刻北向的速度矢量、

表示k时刻天向的速度矢量；f()表示求模函数,Z_{cons_norm}表示速度模值恒定修正的观测量，δV_k,norm表示k时刻中足绑式IMU的速度模值误差,n_n为观测噪声矩阵；

前向、垂直方向速度矢量恒定修正方法观测方程公式如下所示：

其中，Z_{cons_vect}表示前向、垂直方向速度矢量恒定修正的速度误差观测量；δV_k,vect表示k时刻东向、北向速度矢量与k-1时刻之差，n_v为观测噪声矩阵；H_{cons_vect}＝[0_2×3 I_2×2 0_2×13]。

5.根据权利要求3所述的适用于运动平台场景的行人惯性导航方法，其特征在于，所述的子状态的分类算法公式为：

其中，Count_k,fp表示前向搜索标签，N表示滑动窗口长度，sgn()表示符号函数，A^U表示加速度天向比力输出，g为当地重力加速度数值；

当运动平台为扶梯时，

当Count_k,fp＝1，表示在k时刻，子状态切换至S₁₁；

当0.9N≤Count_k,fp≤N，表示在k时刻，子状态由S₁₁切换至S₁₂；

当-0.2N≤Count_k,fp≤0.2N，表示在k时刻，子状态由S₁₂切换至S₁₃；

当-N≤Count_k,fp≤-0.9N，表示在k时刻，子状态由S₁₃切换至S₁₄；

当-0.2N≤Count_k,fp≤0.2N，表示在k时刻，子状态由S₁₄切换至S₁₅；

当运动平台为垂直电梯时，

当0.9N≤Count_k,fp≤N，表示在k时刻，行人处于垂直电梯开始运动，子状态切换至S₂₁；

当-0.2N≤Count_k,fp≤0.2N，表示在k时刻，子状态由S₂₁切换至S₂₂；

当-N≤Count_k,fp≤-0.9N，表示在k时刻，子状态由S₂₂切换至S₂₃。

6.根据权利要求1所述的适用于运动平台场景的行人惯性导航方法，其特征在于，步骤(1)中估计零速检测阈值具体为：以足绑式惯性传感器的中心为坐标原点O，右向为X轴，Y轴垂直于X轴指向前向，Z轴指向上；采集行人足绑式IMU数据，根据初始静态区间中加速度和角速度的数据特性，获得零速检测阈值；所述检测阈值包括加速度模值的最小阈值T_A,min、加速度模值的最大阈值T_A,max、加速度标准差的阈值

角速度模值的阈值T_W、角速度速度标准差的阈值

计算公式如下：

其中，A_staic为静态区间内的加速度模的均值、σ_{A_static}为静态区间内的加速度模的标准差、W_staic为静态区间内的角速度模的均值、σ_{W_static}为静态区间内的角速度模的标准差；c_A,min表示为加速度模值最小常值系数、c_A,max表示为加速度模值最大常值系数，

表示为加速度标准差、

表示为角速度的模值、c_W表示为标准差的常值系数。

7.根据权利要求1所述的适用于运动平台场景的行人惯性导航方法，其特征在于，步骤(2)具体包括以下步骤：

(2.1)从IMU静态数据中计算并扣除加速度计零偏ε_b和陀螺仪的零偏▽_b；

(2.2)利用静态下加速度计的比力输出计算初始的姿态角，包括俯仰角θ和滚转角γ：

其中，g_x为静态下足绑式IMU的X轴的比力输出；g_y为静态下足绑式IMU的Y轴的比力输出；g_z为静态下足绑式IMU的Z轴的比力输出；

(2.3)实时解算行人的姿态、速度、位置

足绑式IMU的姿态更新算法公式如下：

其中，Q表示四元数矩阵，t_k-1表示k-1时刻，t_k表示k时刻，即k-1时刻的下一采样时刻；ψ表示航向角、θ表示俯仰角、γ表示滚转角，

表示姿态；

足绑式IMU的速度更新算法公式如下：

其中，V_k为k时刻地理系下的运动速度矢量，V_k-1为k-1时刻地理系下的运动速度矢量；

表示机体系到导航系的坐标转换矩阵，

代表比力，gⁿ表示当地重力加速度，Δt表示采样时间间隔；

足绑式IMU的位置更新算法公式如下：

其中，L表示代表经度，λ表示纬度，h表示高度；R_m表示地球子午面内曲率半径，R_n表示地球卯酉面内曲率半径。

8.根据权利要求1所述的适用于运动平台场景的行人惯性导航方法，其特征在于，步骤(3)中腿部及足部四节点的步态检测模型为：

ZVD(k)＝S₁(k)&S₂(k)&S₃(k)&S₄(k)

其中，

代表第k至k+N时刻内的加速度的均值、σ_A,k:k+n代表第k至k+N时刻内的加速度的标准差、

代表第k至k+N时刻内的角速度的均值、σ_W,k:k+N代表第k至k+N时刻内的角速度的标准差；T_A,min为加速度的最小阈值、T_A,max为加速度的最大阈值、

为加速度标准差的阈值、T_W为角速度的阈值、

为角速度标准差的阈值；当S₁(k)、S₂(k)、S₃(k)、S₄(k)同时为1时，ZVD(k)＝1，表示当前为零速状态。

9.根据权利要求1所述的适用于运动平台场景的行人惯性导航方法，其特征在于，步骤(4)具体包括以下步骤：

(4.1)当检测足部处于零速状态时，利用卡尔曼滤波计算进行误差估计，构建18维的卡尔曼滤波器状态量X：

其中，

分别表示东向、北向、天向的平台误差角；δV＝[δV_E δV_N δV_U]^T分别为东向、北向、天向的速度误差；δP＝[δL δλ δh]^T分别表示经度、纬度、高度的误差；

分别表示陀螺X轴、Y轴、Z轴的零偏误差；

分别表示陀螺X轴、Y轴和Z轴的一阶马尔可夫漂移误差；

分别表示加速度计X轴、Y轴和Z轴的一阶马尔可夫漂移误差；

(4.2)建立状态方程：

其中，A表示状态转移矩阵，G表示***噪声转移矩阵，W表示***噪声；

(4.3)建立量测方程：

Z_zupt＝δV＝V_INS-V_ZUPT＝HX+v

其中，Z_zupt表示观测量，即为IMU速度误差量δV，V_INS为捷联惯导解算得到的IMU速度矢量，V_ZUPT的理论值为[0 0 0]^T，H是观测矩阵，v为观测噪声矩阵，表示速度观测不确定性；

(4.4)导航误差修正和IMU器件误差修正：

根据步骤(4.1)、(4.2)、(4.3)构建Kalman滤波器，求解得到最优估计状态量

从步骤(2)捷联惯导解算结果中相应扣除前九维状态量

得到修正后的姿态、速度和位置，在IMU数据中扣除后九维状态量

对IMU的零偏误差和一阶马尔可夫漂移误差进行修正。

10.根据权利要求1所述的适用于运动平台场景的行人惯性导航方法，其特征在于，步骤(5)中利用滤波方法对行人速度、姿态和位置进行修正具体为；

其中，A表示状态转移矩阵，Q表示状态转移协方差矩阵，

表示k时刻的先验估计协方差，P_k-1表示表示k-1时刻的后验估计协方差，K_k表示卡尔曼滤波增益，J表示雅可比矩阵，R表示测量噪声协方差，Z表示观测量，

表示由***方程得到k时刻的状态先验估计，X_k由Kalman滤波器求解得到的最优估计，

表示k时刻的后验估计协方差，I表示单位矩阵；

求解最优估计误差状态量X_k后，对位置、速度、姿态和IMU器件误差进行再次修正。

Images