CN112066980A - 一种基于人体四节点运动约束的行人导航定位方法 - Google Patents

Abstract

本发明公开了一种基于人体四节点运动约束的行人导航定位方法，该方法首先根据采集的行人的IMU数据估计零速检测阈值，再根据预处理后的IMU数据解算行人的姿态、速度、位置信息，建立行人运动步态检测模型，利用估计的零速状态阈值进行零速检测，若处于零速状态，则进行零速修正，得到修正后的位置、速度，而后根据基于双足不等式约束的滤波方法对行人航向进行修正。本发明解决了变速/剧烈/快速运动下零速状态缺失时，导航***累积误差难以抑制的问题，具有实时性好，运算量小的优势，提高了零速检测精度。

Description

一种基于人体四节点运动约束的行人导航定位方法

技术领域

本发明涉及一种行人导航定位方法，尤其涉及一种基于人体四节点运动约束的行人导航定位方法。

背景技术

近年来，随着室内定位技术的发展，适用于走路等稳态运动下的室内导航算法已经相对比较成熟。但是在消防救援、井下勘探、单兵作战等任务中，特种行业人员往往需要进行跑步、跨越等剧烈运动，这就对传统的导航方法提出了挑战。同时，在以上行业中要求导航设备便携、可穿戴，能够在复杂的室内环境进行精确定位。传统的基于UWB、WIFI、蓝牙等无线射频设备的室内定位技术，成本高且不便携，无法满足以上导航需求。惯性***体积小、成本低可用于穿戴式行人导航，但是由于器件存在累积误差，在剧烈运动情况下会导致定位结果发散。

目前，足绑式惯性导航***往往采用单节点导航模式，其核心算法主要是零速检测和零速修正。但是在快速运动下，单节点的惯性导航***存在零速检测率低，零速时刻短甚至缺失的问题，难以保证高精度的行人定位导航要求。

发明内容

发明目的：本发明提出一种可以在剧烈运动下定位准确的行人导航定位方法。

技术方案：本发明所述的基于人体四节点运动约束的行人导航定位方法，包括：

(1)采集行人足部IMU数据，利用运动特征概率统计的方法，估计零速检测阈值；

(2)对所述足部IMU数据进行预处理，进行姿态、速度、位置解算；

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

(4)若处于零速状态，则进行零速修正，得到修正后的位置、速度，否则不修正；

(5)计算双脚的距离，当双足距离大于最大步长时，利用基于双足不等式约束的滤波方法对行人航向进行修正，得到修正后的位置、速度和姿态；否则重复步骤(2)-(5)。

进一步地，所述步骤(1)包括：以各足部安装的惯性传感器的中心为坐标原点O，前向为Y轴，X轴垂直于Y轴指向右侧，Z轴指向上，采集足部两节点惯性传感器输出的数据，根据数据对行人运动特征进行频域统计，取特征值频率最高处为零速状态阈值，其中，所述四节点惯性传感器安装于行人的双足以及双腿上。

进一步地，所述检测阈值包括加速度模值f_norm的阈值THf_norm，加速度标准差σ_fnorm的阈值THσ_fnorm，X轴角速度w_x的阈值THw_x，计算公式如下：

THf_norm＝argmax(P(f_norm))

THσ_fnorm＝argmax(P(σ_fnorm))

THw_x＝argmax(P(w_x))。

本发明通过分析人体运动特征，根据足部零速周期性以及腿部俯仰角周期性显著的特点，从而得到针对不测试者、不同运动速度下的零速阈值，利用动态多阈值进行零速检测，提高了后续零速检测的准确率。

所述步骤(2)包括：

(21)从所述IMU数据中扣除惯性传感器的零偏；

(22)计算初始的姿态角，包括俯仰角θ和滚转角γ：

其中，g_x,g_y,g_z分别为x轴、y轴和z轴的重力加速度，g为当地重力加速度；

(23)实时解算行人的姿态、速度、位置，其中，

当前时刻行人的姿态利用四元数法进行解算，公式如下：

当前时刻行人的速度的解算公式如下：

当前时刻行人的位置的解算公式如下：

其中，Λ是四元数矩阵，ψ是偏航角，θ是俯仰角，γ是滚转角，v是载体的运动速度，

是机体系到导航系的转换矩阵，

代表比力，

为地球自转角速度在导航坐标系下的投影，

为导航系相对于地球系下的角速度在导航坐标系下的投影，v_E、v_N、v_U分别是东北天三个方向的速度矢量，L代表经度，λ代表纬度，h表示运动高度，R_m表示子午面内曲率半径，R_n表示卯酉面内曲率半径。

进一步地，所述步骤(3)包括：采集腿部及足部四节点数据，采用检测足部加速度模值、足部加速度标准差、足部X轴角速度、互异腿部滚转角变化作为步态检测条件，结合估计的零速状态阈值，建立腿部及足部四节点的步态检测模型；根据所述腿部及足部四节点的步态检测模型进行零速检测。进一步地，所述腿部及足部四节点的步态检测模型为：

其中，k代表第k时刻，f_k,norm、σ_k,fnorm、w_k,x分别代表第k时刻加速度模值和加速度标准差和X轴的角速度，Δφ＝φ_k-φ_k-1，φ_k是第k时刻腿部滚转角，THf_norm、THσ_fnorm、THw_x分别为加速度模值f_norm、加速度标准差σ_fnorm、X轴角速度w_x的阈值，S₁、S₂、S₃、S₄为标签，当同时满足当S₁＝1&S₂＝1&S₃＝1&S₄＝1则当前为零速状态。

进一步地，加速度模值、加速度标准差计算公式如下，

加速度模值f_k,norm：

加速度标准差σ_k,fnorm：

其中，

为第k时刻X、Y、Z三轴比力，

是第k时刻比力的均值。

进一步地，所述步骤(4)包括：当检测到零速状态时，根据步骤(41)-(44)对行人定位导航过程中的累积误差进行修正，得到修正后的位置、速度，否则不进行修正；

(41)当检测到零速状态时，利用卡尔曼滤波计算进行误差估计，构建9维的卡尔曼滤波器状态量X：

X＝[Φ_E Φ_N Φ_U δv_E δv_N δv_U δL δλ δh]^T

其中，Φ_E,Φ_N,Φ_U是东北天方向的平台误差角，δv_E,δv_N,δv_U为东北天方向的速度误差，δL，δλ，δh分别为经度，纬度和高度误差；

(42)建立状态方程：

其中，A为状态转移矩阵，G为***噪声矩阵，

是机体系到导航系的转换矩阵，W为白噪声随机误差矢量，W＝[w_gx w_gy w_gz]^T，w_gx，w_gy，w_gz分别为陀螺X轴、Y轴和Z轴的白噪声，

是状态量X的一阶导数；

(43)建立量测方程：

Z_zupt＝v_ins-v_zupt＝HX+V

其中，v_ins为捷联惯导解算的速度矢量，v_zupt理论值为[0 0 0]^T，H是观测矩阵，H＝I_3x3，V为观测噪声矩阵，是零速状态下的速度误差；

(44)根据步骤(42)、(43)求解得到的速度、位置、姿态的误差，即得到状态量X，从步骤(2)得到的位置、速度中去除状态量X，得到修正后的位置、速度。

进一步地，所述步骤(5)包括：计算每一次采样下双足的距离，判断双足距离是否大于最大步长，若是则将最大步长作为当前双足距离，利用伪距离建立量测方程，对姿态、速度、位置误差进行二次修正，否则跳过此步骤。

进一步地，所述利用伪距离建立量测方程包括步骤：

根据步骤(41)的构建方法，建立双足的18维状态模型：

X＝[X^l X^r]

其中，左脚惯性***状变量为

右脚惯性***状态变量为

δp，δv，

分别为三轴的位置、速度、姿态误差，上标l，r分别代表左右脚；

建立双足不等式约束方程：

其中，δρ＝ρ-ρ_max为双脚距离误差，ρ为当前双脚距离，ρ_max为最大步长，

为估计量，L_s为距离约束模型为：

如果存在k时刻状态方程满足

则认为此时的双脚距离为最大步长，将

线性化后得到一个重组后的滤波模型约束模型如下：

其中,

为新的状态量，κ、

分别表示为：

其中，x,y为步骤(4)中得到的修正后的位置；

最优解集合满足以下方程：

其中，Z为观测量，P为误差协方差矩阵，表示如下：

对上式进行求解得到更新后的状态量

以及误差协方差矩阵

表示如下：

其中，

μ＝Pκ^T(κPκ^T)^-1κ

得到重组滤波模型下的误差量

后，对位置、速度、姿态进行二次修正。

本发明通过所述双足不等式约束对行人航向进行修正，有效弥补了快速运动下零速区间短的问题，有效估计行人导航结果误差与传感器误差，抑制了惯性导航***的误差发散。

有益效果：本发明解决了剧烈/快速/变速运动下行人导航定位不准的问题，减少行人导航定位的累积误差，提高行人导航定位的精度。

附图说明

图1是本发明方法流程图；

图2是本实施例方法定位结果与传统方法结果对比图。

具体实施方式

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

如图1所示，一种基于人体四节点约束的行人导航定位方法包括如下步骤：

步骤1，采集并分析行人运动时的运动特性，行人一步运动周期中，处于零速状态区间概率最大。为此采集足部惯导传感器(IMU)输出的500个数据进行预处理，得到走路/跑步运动下的零速阈值。

步骤2，将加速度计与陀螺仪输出进行预处理，即扣除加速度计与陀螺仪的零偏，并利用四元数方法进行姿态、速度、位置解算。

采用加速度计和陀螺仪进行初始对准，其中，人为设定初始航向角，根据三轴重力计数据[g_x,g_y,g_z]求出俯仰角θ和滚转角γ，其中，g为重力常数：

其中，g_x,g_y,g_z分别为x轴、y轴和z轴的重力加速度，g为当地重力加速度；

再根据姿态、速度、位置微分方程求解当前时刻的位置、速度、姿态，具体公式如下：

姿态解算微分方程：

速度解算微分方程：

位置解算微分方程：

其中，Λ是四元数矩阵，ψ是偏航角，θ是俯仰角，γ是滚转角，v是载体的运动速度，

是机体系到导航系的转换矩阵，

代表比力，

为地球自转角速度在导航坐标系下的投影，

为导航系相对于地球系下的角速度在导航坐标系下的投影，实际上低成本的IMU无法敏感到地球自转，所以

w_en＝[0 0 0]^T，v_E、v_N、v_U分别是东、北、天三个方向的速度矢量，L代表经度，λ代表纬度，h表示运动高度，R_m表示子午面内曲率半径，R_n表示卯酉面内曲率半径。

步骤3，建立腿部及足部四节点的步态检测模型，首先分别对足部加速度模值、足部加速度标准差进行处理：

其中，

为第k时刻X、Y、Z三轴比力，

是第k时刻比力的均值，f_k,norm，σ_k,fnorm分别代表第k时刻加速度模值和加速度标准差。

然后，用加计算得到的加速度模值、加速度标准差、角速度模值进行零速状态检测：

Δφ＝φ_k-φ_k-1，φ_k是第k时刻腿部滚转角，THf_norm、THσ_fnorm、THw_x分别为加速度模值f_norm、加速度标准差σ_fnorm、X轴角速度w_x的阈值；S₁、S₂、S₃、S₄为标签。当同时满足当S₁＝1&S₂＝1&S₃＝1&S₄＝1则当前为零速状态。

步骤4，双足分别进行捷联惯导***INS解算(如图1所示，左脚解算记为INS1，右脚解算记为INS2)，当检测到足部为零速状态时，认为此时行人的运动速度为0，利用卡尔曼滤波计算进行误差估计，并修正行人定位导航过程中的累积误差，得到修正后的位置、速度，否则不进行修正；

41)构建9维的卡尔曼滤波器状态量，具体如下：

X＝[Φ_E Φ_N Φ_U δv_E δv_N δv_U δL δλ δh]^T

其中，Φ_E，Φ_N，Φ_U是东北天方向的平台误差角，δv_E，δv_N，δv_U为东北天方向的速度误差，δL，δλ，δh分别为经度，纬度和高度误差。

42)建立状态方程：

其中，A为状态转移矩阵，G为***噪声矩阵，

是机体系到导航系的转换矩阵，W为白噪声随机误差矢量，W＝[w_gx w_gy w_gz]^T，w_gx，w_gy，w_gz分别为陀螺X轴、Y轴和Z轴的白噪声，

是状态量的一阶导数；

43)当检测到零速点时，以速度为量测，建立量测方程：

Z_zupt＝v_ins-v_zupt＝HX+V

其中，v_ins为捷联惯导解算的速度，v_zupt理论为[0 0 0]^T，H是观测矩阵，H＝I_3x3，V为观测噪声矩阵，即零速状态下的速度误差；

44)根据步骤42)、43)求解得到的速度、位置、姿态的误差，即得到状态量X，从位置、速度中去除状态量X，得到修正后的位置、速度。

步骤5，如果双足距离大于最大的步长，则认为当前双足距离为最大步长，进行误差估计，否则跳过此步骤。

在步骤4)的基础上构建18维状态模型如下：

X＝[X^l X^r]

其中，左脚惯性***状变量为

右脚惯性***状态变量为

δp，δv，

分别为三轴的位置、速度、姿态误差，上标l，r分别代表左右脚。

由于跑步过程中双脚最大距离不超过最大步长，所以存在约束方程如下：

其中δρ＝ρ-ρ_max为双脚距离误差，ρ为当前双脚距离，ρ_max为最大步长，

为估计量，L_s为距离约束模型为：

如果存在k时刻状态方程满足

则认为此时的双脚距离为最大步长。将

线性化后得到一个重组后的滤波模型约束模型如下：

其中,

为新的状态量，κ,

分别表示为：

其中，x,y为步骤(4)中得到的修正后的位置。

最优解集合满足以下方程：

其中，Z为观测量，P为误差协方差矩阵，表示如下：

对上式进行求解得到更新后的状态量

以及误差协方差矩阵

表示如下：

其中

μ＝Pκ^T(κPκ^T)^-1κ

得到重组滤波模型下的误差量

后，对位置、速度、姿态进行二次修正。

如图2所示，实验场景为实验楼内的走廊，路线为长度为56.8米，宽度为47米的矩形，测试者以2.2m/s的跑步运动，利用传统的行人零速修正方法解算出来的结果如图虚线所示，起终点误差大于10米；利用本发明即一种基于人体四节点约束的行人导航定位方法解算出来的结果如图实线所示，起终点位置误差为1.7米，且航向误差大大减小，提升了定位精度。

Claims (10)

1.一种基于人体四节点约束的行人导航定位方法，其特征在于，包括步骤：

(1)采集行人足部IMU数据，利用运动特征概率统计的方法，估计零速检测阈值；

(2)对所述足部IMU数据进行预处理，进行姿态、速度、位置解算；

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

(4)若处于零速状态，则进行零速修正，得到修正后的位置、速度，否则不修正；

(5)计算双脚的距离，当双足距离大于最大步长时，利用基于双足不等式约束的滤波方法对行人航向进行修正，得到修正后的位置、速度和姿态；否则重复步骤(2)-(5)。

2.根据权利要求1所述的基于人体四节点约束的行人导航定位方法，其特征在于，所述步骤(1)包括：以各足部安装的惯性传感器的中心为坐标原点O，前向为Y轴，X轴垂直于Y轴指向右侧，Z轴指向上，采集足部两节点惯性传感器输出的数据，根据数据对行人运动特征进行频域统计，取特征值频率最高处为零速状态阈值，其中，所述四节点惯性传感器安装于行人的双足以及双腿上。

3.根据权利要求2所述的基于人体四节点约束的行人导航定位方法，其特征在于：所述检测阈值包括加速度模值f_norm的阈值THf_norm、加速度标准差σ_fnorm的阈值THσ_fnorm、X轴角速度w_x的阈值THw_x，计算公式如下：

THf_norm＝argmax(P(f_norm))

THσ_fnorm＝argmax(P(σ_fnorm))

THw_x＝argmax(P(w_x))。

4.根据权利要求1所述的基于人体四节点约束的行人导航定位方法，其特征在于，所述步骤(2)包括：

(21)从所述IMU数据中扣除惯性传感器的零偏；

(22)计算初始的姿态角，包括俯仰角θ和滚转角γ：

其中，g_x,g_y,g_z分别为x轴、y轴和z轴的重力加速度，g为当地重力加速度；

(23)实时解算行人的姿态、速度、位置，其中，

当前时刻行人的姿态利用四元数法进行解算，公式如下：

当前时刻行人的速度的解算公式如下：

当前时刻行人的位置的解算公式如下：

其中，Λ是四元数矩阵，ψ是偏航角，θ是俯仰角，γ是滚转角，v是载体的运动速度，

是机体系到导航系的转换矩阵，

代表比力，

为地球自转角速度在导航坐标系下的投影，

为导航系相对于地球系下的角速度在导航坐标系下的投影，v_E、v_N、v_U分别是东、北、天三个方向的速度矢量；L表示代表经度，λ表示纬度，h表示运动高度，R_m表示子午面内曲率半径，R_n表示卯酉面内曲率半径。

5.根据权利要求1所述的基于人体四节点约束的行人导航定位方法，其特征在于，所述步骤(3)包括：采集腿部及足部四节点数据，采用检测足部加速度模值、足部加速度标准差、足部X轴角速度、互异腿部滚转角变化作为步态检测条件，结合估计的零速状态阈值，建立腿部及足部四节点的步态检测模型；根据所述腿部及足部四节点的步态检测模型进行零速检测。

6.根据权利要求5所述的基于人体四节点约束的行人导航定位方法，其特征在于，所述腿部及足部四节点的步态检测模型为：

其中，k代表第k时刻，f_k,norm、σ_k,fnorm、w_k,x分别代表第k时刻加速度模值和加速度标准差和X轴的角速度，Δφ＝φ_k-φ_k-1，φ_k是第k时刻腿部滚转角，THf_norm、THσ_fnorm、THw_x分别为加速度模值f_norm、加速度标准差σ_fnorm、X轴角速度w_x的阈值，S₁、S₂、S₃、S₄为标签，当同时满足当S₁＝1&S₂＝1&S₃＝1&S₄＝1则当前为零速状态。

7.根据权利要求5或6所述的基于人体四节点约束的行人导航定位方法，其特征在于，所述加速度模值、加速度标准差计算公式如下，

加速度模值f_k,norm：

加速度标准差σ_k,fnorm：

其中，

为第k时刻X、Y、Z三轴比力，

是第k时刻比力的均值。

8.根据权利要求1所述的基于人体四节点约束的行人导航定位方法，其特征在于，所述步骤(4)包括：当检测到零速状态时，根据步骤(41)-(44)对行人定位导航过程中的累积误差进行修正，得到修正后的位置、速度，否则不进行修正；

(41)当检测到零速状态时，利用卡尔曼滤波计算进行误差估计，构建9维的卡尔曼滤波器状态量X：

X＝[Φ_E Φ_N Φ_U δv_E δv_N δv_U δL δλ δh]^T

其中，Φ_E,Φ_N,Φ_U是东北天方向的平台误差角，δv_E,δv_N,δv_U为东北天方向的速度误差，δL,δλ,δh分别为经度，纬度和高度误差；

(42)建立状态方程：

其中，A为状态转移矩阵，G为***噪声矩阵，

是机体系到导航系的转换矩阵，W＝[w_gx w_gy w_gz]^T，w_gx，w_gy，w_gz分别为陀螺X轴、Y轴和Z轴的白噪声，

是状态量X的一阶导数；

(43)建立量测方程：

Z_zupt＝v_ins-v_zupt＝HX+V

其中，v_ins为捷联惯导解算的速度矢量，v_zupt理论值为[0 0 0]^T，H是观测矩阵，H＝I_3x3，V为观测噪声矩阵，是零速状态下的速度误差；

(44)根据步骤(42)、(43)求解得到的速度、位置、姿态的误差，即得到状态量X，从步骤(2)得到的位置、速度中去除状态量X，得到修正后的位置、速度。

9.根据权利要求1所述的基于人体四节点约束的行人导航定位方法，其特征在于，所述步骤(5)包括：计算每一次采样下双足的距离，判断双足距离是否大于最大步长，若是则将最大步长作为当前双足距离，利用伪距离建立量测方程，对姿态、速度、位置误差进行二次修正，否则跳过此步骤。

10.根据权利要求1所述的基于人体四节点约束的行人导航定位方法，其特征在于，所述利用伪距离建立量测方程包括步骤：

根据步骤(41)的构建方法，建立双足的18维状态模型：

X＝[X^l X^r]

其中，左脚惯性***状变量为

右脚惯性***状态变量为

δp，δv，

分别为三轴的位置、速度、姿态误差，上标l，r分别代表左右脚；

建立双足不等式约束方程：

其中，δρ＝ρ-ρ_max为双脚距离误差，ρ为当前双脚距离，ρ_max为最大步长，

为估计量，L_s为距离约束模型为：

如果存在k时刻状态方程满足

则认为此时的双脚距离为最大步长，将

线性化后得到一个重组后的滤波模型约束模型如下：

其中

为新的状态量，κ、

分别表示为：

其中，x,y为步骤(4)中得到的修正后的位置；

最优解集合满足以下方程：

其中，Z为观测量，P为误差协方差矩阵，表示如下：

对上式进行求解得到更新后的状态量

以及误差协方差矩阵

表示如下：

其中，

μ＝Pκ^T(κPκ^T)^-1κ

得到重组滤波模型下的误差量

后，对位置、速度、姿态进行二次修正。

Images