CN116448103A - 基于uwb测距辅助的行人足绑式惯性导航系统误差修正方法 - Google Patents

Abstract

本发明请求保护一种基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法。本发明包括以下步骤：S1：根据惯性测量单元输出数据，利用捷联惯导算法解算得到行人导航信息，并根据UWB模块测量双足间实时距离，通过IMU数据划分步态区间，并利用时间因子修正错误区间；S2：利用自适应随机森林算法对行人运动模式进行识别，在静止区间内利用零速修正算法修正导航系统误差；S3：针对平地行走、跑步模式进行足间距约束与抬脚高度约束，而针对上下楼运动模式进行楼梯高度约束，最终抑制惯导系统的误差漂移。本发明能够解决足绑式行人惯性导航系统中航向漂移的问题，减小行人导航定位的累积误差，提高定位精度。

Description

基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法

技术领域

本发明属于行人导航技术领域，特别涉及基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法。

背景技术

行人导航系统可以在陌生环境中为行人提供导航定位信息，广泛应用于室内定位、应急救援、单兵作战等领域。在室外环境中，主要依靠卫星定位系统实现行人导航系统，但是在高楼、森林等复杂环境中，卫星信号会严重衰退，导致定位精度无法满足导航需求。

惯性导航系统(Inertial Navigation System,INS)可以不依赖外界信息，利用惯性测量单元(Inertial Measurement Unit,IMU)自主地实现行人定位，成为行人导航系统的主要研究方向。但惯性传感器存在误差且会随时间迅速累积，导致INS长航时定位精度严重下降。

基于足绑式IMU的零速修正算法和多传感器信息融合算法是目前用于行人惯性导航系统误差修正的主要技术手段。零速修正算法(Zero Velocity Update,ZUPT)利用零速时速度误差修正导航误差，多传感器信息融合算法利用磁力计、蓝牙、WIFI等信息修正INS误差。但这些技术存在以下问题：1.零速修正算法仅在行人静止区间内修正导航误差。2.磁力计易受周围环境影响而产生大量磁干扰，导致磁航向误差过大。3.WIFI、蓝牙等定位技术需要通过信号发射设备进行信号广播，无法满足特殊环境需求且信号易受干扰。因此需要提出一种新的行人惯性导航系统误差修正算法，以提高长航时导航系统的精度和可靠性。

专利号为CN114608571A的申请公开了一种适用于运动平台场景的行人惯性导航方法，根据采集到的IMU数据解算行人的速度、姿态、位置信息，将速度信息与设定阈值进行，判断行人处于平面行走、自动扶梯或垂直电梯中，根据所处运动平台的类型采用不同的速度约束，实现对行人的导航解算信息修正，但该方法没有考虑行人通过上下楼梯改变高度位置的情况，且只利用零速修正算法无法有效抑制航向漂移误差。因此本发明利用足间距约束与抬脚高度约束提高水平面运动时的导航精度，并利用楼梯高度约束提高行人楼梯间运动的导航精度。

发明内容

本发明旨在解决以上现有技术的问题。提出了一种基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法。本发明的技术方案如下：

一种基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法，其包括以下步骤：

步骤一：将两个IMU(惯性测量单元)及UWB(超宽带)发射、接收模块分别安装在行人左右脚，根据安装于行人双足的IMU测量行人加速度与角速度信息，利用捷联惯导算法解算得到行人姿态、速度、位置信息，利用零速检测算法对行人步态区间进行划分，确定双足零速区间，并利用UWB测距模块实时测量行人足间距离；

步骤二：对采集到的IMU数据和UWB数据进行特征提取，利用自适应随机森林算法对行人运动模式进行分类识别；

步骤三：对处于静止区间的导航结果进行零速修正；对处于运动区间的导航结果按照运动模式进行分类，针对平地行走、跑步模式进行足间距约束与抬脚高度约束，针对上下楼运动模式进行楼梯高度约束。

进一步的，所述步骤一中利用捷联惯导算法解算得到行人姿态、速度、位置信息，该算法为惯性导航领域的现有技术，参考论文《基于实测轨迹的高精度捷联惯导模拟器》，基本的捷联惯导微分方程可以表示为：

其中，为载体系到导航系的方向余弦矩阵；/>为陀螺仪输出构成的反对称矩阵；f^b为加速度计输出；gⁿ为当地重力加速度；vⁿ和pⁿ分别为速度和位置信息。

进一步的，所述步骤一中，利用零速检测算法对行人步态区间进行划分，确定双足零速区间，并利用UWB测距模块实时测量行人足间距离，具体为：

(1)根据陀螺仪和加速度计的输出数据，利用基于广义似然比的零速区间检测法划分行人步态区间，基于广义似然比的零速区间检测法模型为：

其中，和/>为加速度计和陀螺仪的噪声方差，/>和/>为加速度计输出和陀螺仪输出，k为采样的加速度和角速度数据序号，n为检测器输出序号，W为窗口大小，/>为窗口内加速度均值，g为当地重力加速度，T(z_n)为检测器输出，γ为阈值；当检测器输出小于阈值时，判断该时刻为静止区间；

(2)将运动区间和静止区间分别定义为和/>并记录各区间内的数据量，m表示该运动阶段内包含的数据数量，s表示该静止阶段内包含的数据数量；

(3)将各区间内的数据数量与时间因子ΔT进行比较，若各区间的数据数量均大于时间因子，则表示区间划分正确，否则表明区间划分存在错误；

(4)对违反时间因子限制的区间进行状态转换，将误判为静止状态的区间转化为运动区间，将误判为运动状态的区间转化为静止区间，完成区间划分修正。

进一步的，所述步骤二利用自适应随机森林算法对行人运动模式进行分类识别，模式类型主要分为：静止、平地行走、跑步、上楼、下楼、跨步上楼、跨步下楼七种；具体操作为：

1)设共有N个待分类样本T＝{(x₁,y₁)…(x_N,y_N)}，样本特征包括x轴加速度最小值及均值、x轴加速度四分位距、y轴加速度偏度、z轴加速度最大值及偏度、x轴角速度均值、y轴角速度标准差、z轴角速度最大值及均值、UWB足间距数据，共11个特征，对应静止、平地行走、跑步、上楼、下楼、跨步上楼、跨步下楼七种类型Y＝{L₁,L₂,…L₇}，其中x_i∈X,y_i∈Y,i＝1,2,…N；

2)利用bootstrap原理对训练样本进行采样得到训练样本子集，并由此构建特征子空间，通过不断重复操作，得到包含M个决策树的随机森林；

3)根据决策树对每个类别的分类能力计算决策树对该种运动模式的分类权重；对于L₁类型，将y_i＝L₁的样本设置为正样本y_i＝1，y_i≠L₁的样本设置为负样本y_i＝-1；初始化训练样本权重：

4)对于每一棵决策树h_n(x)，n为决策树序号，计算其对类型L₁的分类错误率

其中，sign()为符号函数，其输出值为1或-1。

将分类错误率与错误率要求进行比较，当/>时计算该决策树对当前类别的权重/>

同时更新样本权重

并再次计算分类错误率以及权重/>直到/>结束计算。

5)针对每一类运动模式重复上述权重计算过程，得到各决策树对各类运动模式的投票权重，由决策树按其权重决定最终分类结果：

进一步的，所述步骤三中对处于零速区间的行人惯性导航系统进行零速修正，对于处于运动区间的导航结果按照运动模式进行分类，针对平地行走、跑步模式进行足间距约束与抬脚高度约束；具体足间约束方程为：

ρ₁＝[I_3×3-I_3×3]

其中，和/>分别表示k时刻左脚和右脚三维位置，/>表示k时刻解算出的左右脚间的距离，l表示由UWB测距模块测量的左右脚之间距离信息；利用带约束的最小二乘法求得满足足间距约束的位置信息：

进一步的，所述步骤三的高度约束方程为：

其中，表示k时刻通过捷联惯导算法解算出的左右脚之间的高度差，h表示最大抬脚高度；

将足间距约束后得到的导航状态约束到符合抬脚高度约束的子空间中，得到k时刻的双足导航状态信息：

其中，表示导航状态向量，D表示双足高度系数矩阵、/>表示误差协方差的逆矩阵，i∈{L,R}分别表示右脚和左脚，/>分别表示三维位置、速度、姿态信息；

判断双足位置信息是否满足抬脚高度约束，当解算的双足位置不满足抬脚高度约束时，使用不等式约束卡尔曼滤波得到最终导航状态

进一步的，对上下楼运动模式进行楼梯高度约束，高度约束条件根据楼梯高度与检测到的迈步情况决定：

h_k＝n_step×h_stair

得到高度信息误差值为：

δp_k,z＝h_k-p_k,z

其中，δp_k,z为高度误差，p_k,z捷联惯导解算得到的高度值，h_k为当前时刻的高度约束条件。

进一步的，所述步骤三将不同运动模式中的误差作为卡尔曼滤波器的量测值修正行人导航系统误差；卡尔曼滤波器的状态方程和量测方程为：

式中，为9维误差向量，δp_k、δv_k、/>分别表示k时刻的位置误差、速度误差、姿态误差；Z_k为测量误差；Γ_k/k-1为系统噪声驱动矩阵；W_k-1和V_k分别为系统噪声向量和量测噪声向量；F_k和H_k分别为状态转移矩阵和量测矩阵；

根据行人的运动模式类型C_k，卡尔曼滤波器量测矩阵H_k以及观测误差δ_k可以表示为：

式中，v_static为静止时足部三维速度理论值，即v_static＝[0 0 0]，static、walk、run、stairs分别代表行人处于静止、行走、跑步、上下楼状态。

本发明的优点及有益效果如下：

本发明相对于现有技术，首先利用自适应随机森林算法对决策树投票权重进行自适应修改，实现更精确的运动模式分类，将行人运动模式分为静止、平地行走、跑步、上楼、下楼、跨步上楼、跨步下楼七种类型，然后根据运动类型选取不同的误差修正算法进行误差修正。在行走和跑步运动模式下，利用UWB测距模块实时测量的足间距离信息构建双足位置修正模型，并利用左右脚间存在的抬脚高度限制进行抬脚高度约束，有效弥补了零速修正算法无法抑制航向漂移误差的问题，并针对上下楼情况，根据楼梯高度信息以及迈步方式构建高度约束，抑制高度漂移，实现行人室内三维捷联惯导系统长航时精确定位。

附图说明

图1是本发明提供优选实施例的方法实现流程图。

图2为本发明双足解算位置修正模型示意图；

图3为本发明的简要原理图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

参照图1所示，本发明提供了一种基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法，包括以下步骤：

步骤一：将两个IMU及UWB发射、接收模块分别安装在行人左右脚。根据安装于行人双足的IMU测量行人加速度与角速度信息，利用捷联惯导算法解算得到行人姿态、速度、位置信息，利用零速检测算法对行人步态区间进行划分，确定双足零速区间，并利用UWB测距模块实时测量行人足间距离；

步骤二：对采集到的IMU数据和UWB数据进行特征提取，利用自适应随机森林算法对行人运动模式进行分类识别，模式类型主要分为：静止、平地行走、跑步、上楼、下楼、跨步上楼、跨步下楼七种；

步骤三：对处于静止区间的导航结果进行零速修正；对处于运动区间的导航结果按照运动模式进行分类，针对平地行走、跑步模式进行足间距约束与抬脚高度约束，针对上下楼运动模式进行楼梯高度约束，由此提高惯性导航系统精度。

进一步的，所述步骤一中零速检测模型为：

(1)根据陀螺仪和加速度计的输出数据，利用基于广义似然比的零速区间检测法划分行人步态区间，基于广义似然比的零速区间检测法模型为：

其中，和/>为加速度计和陀螺仪的噪声方差，/>和/>为加速度计输出和陀螺仪输出，k为采样的加速度和角速度数据序号，n为检测器输出序号，W为窗口大小，/>为窗口内加速度均值，g为当地重力加速度，T(z_n)为检测器输出，γ为阈值；当检测器输出小于阈值时，判断该时刻为静止区间；

(2)将运动区间和静止区间分别定义为和/>并记录各区间内的数据量，m表示该运动阶段内包含的数据数量，s表示该静止阶段内包含的数据数量；

(3)将各区间内的数据数量与时间因子ΔT进行比较，若各区间的数据数量均大于时间因子，则表示区间划分正确，否则表明区间划分存在错误；

(4)对违反时间因子限制的区间进行状态转换，将误判为静止状态的区间转化为运动区间，将误判为运动状态的区间转化为静止区间，完成区间划分修正。

进一步的，利用自适应随机森林算法对行人运动模式进行分类识别，模式类型主要分为：静止、平地行走、跑步、上楼、下楼、跨步上楼、跨步下楼七种；具体操作为：

1)设共有N个待分类样本T＝{(x₁,y₁)…(x_N,y_N)}，样本特征包括x轴加速度最小值及均值、x轴加速度四分位距、y轴加速度偏度、z轴加速度最大值及偏度、x轴角速度均值、y轴角速度标准差、z轴角速度最大值及均值、UWB足间距数据，共11个特征，对应静止、平地行走、跑步、上楼、下楼、跨步上楼、跨步下楼七种类型Y＝{L₁,L₂,…L₇}，其中x_i∈X,y_i∈Y,i＝1,2,…N；

2)利用bootstrap原理对训练样本进行采样得到训练样本子集，并由此构建特征子空间，通过不断重复操作，得到包含M个决策树的随机森林；

3)根据决策树对每个类别的分类能力计算决策树对该种运动模式的分类权重；对于L₁类型，将y_i＝L₁的样本设置为正样本y_i＝1，y_i≠L₁的样本设置为负样本y_i＝-1；初始化训练样本权重：

4)对于每一棵决策树h_n(x)，n为决策树序号，计算其对类型L₁的分类错误率

其中，sign()为符号函数，其输出值为1或-1。

将分类错误率与错误率要求进行比较，当/>时计算该决策树对当前类别的权重/>

同时更新样本权重

并再次计算分类错误率以及权重/>直到/>结束计算。

5)针对每一类运动模式重复上述权重计算过程，得到各决策树对各类运动模式的投票权重，由决策树按其权重决定最终分类结果：

进一步的，对处于零速区间的行人惯性导航系统进行零速修正，对于处于运动区间的导航结果按照运动模式进行分类，针对平地行走、跑步模式进行足间距约束与抬脚高度约束；具体足间约束方程为：

ρ₁＝[I_3×3-I_3×3]

其中，和/>分别表示k时刻左脚和右脚三维位置，/>表示k时刻解算出的左右脚间的距离，l表示由UWB测距模块测量的左右脚之间距离信息。

得到满足足间距约束的左右脚位置信息：

进一步的，所述步骤三中，高度约束方程为：

其中，表示k时刻通过捷联惯导算法解算出的左右脚之间的高度差，h表示最大抬脚高度。

将足间距约束后得到的导航状态约束到符合抬脚高度约束的子空间中，得到k时刻的双足导航状态信息：

其中，表示导航状态向量，D表示双足高度间系数矩阵、/>表示误差协方差的逆矩阵，i∈{L,R}分别表示右脚和左脚，/>分别表示三维位置、速度、姿态信息；

判断双足位置信息是否满足抬脚高度约束，当解算的双足位置不满足抬脚高度约束时，使用不等式约束卡尔曼滤波得到最终导航状态

进一步的，对上下楼运动模式进行楼梯高度约束，高度约束条件根据楼梯高度与检测到的迈步情况决定：

h_k＝n_step×h_stair

进一步的，得到高度信息误差值为：

δp_k,z＝h_k-p_k,z

其中，δp_k,z为高度误差，p_k,z捷联惯导解算得到的高度值，h_k为当前时刻的高度约束条件。

进一步的，将不同运动模式中的误差作为卡尔曼滤波器的量测值修正行人导航系统误差。卡尔曼滤波器的状态方程和量测方程为：

式中，为9维误差向量，δp_k、δv_k、/>分别表示k时刻的位置误差、速度误差、姿态误差；Z_k为测量误差；Γ_k/k-1为系统噪声驱动矩阵；W_k-1和V_k分别为系统噪声向量和量测噪声向量；F_k和H_k分别为状态转移矩阵和量测矩阵；

根据行人的运动模式类型C_k，卡尔曼滤波器量测矩阵H_k以及观测误差δ_k可以表示为：

上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。

其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (7)

1.一种基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法，其特征在于，包括以下步骤：

步骤一：将两个IMU惯性测量单元及UWB超宽带发射、接收模块分别安装在行人左右脚，根据安装于行人双足的IMU测量行人加速度与角速度信息，利用捷联惯导算法解算得到行人姿态、速度、位置信息，利用零速检测算法对行人步态区间进行划分，确定双足零速区间，并利用UWB测距模块实时测量行人足间距离；

步骤二：对采集到的IMU数据和UWB数据进行特征提取，利用自适应随机森林算法对行人运动模式进行分类识别；

步骤三：对处于静止区间的导航结果进行零速修正；对处于运动区间的导航结果按照运动模式进行分类，针对平地行走、跑步模式进行足间距约束与抬脚高度约束，针对上下楼运动模式进行楼梯高度约束。

2.根据权利要求1所述的基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法，其特征在于，所述步骤一中，利用零速检测算法对行人步态区间进行划分，确定双足零速区间，并利用UWB测距模块实时测量行人足间距离，具体为：

(1)根据陀螺仪和加速度计的输出数据，利用基于广义似然比的零速区间检测法划分行人步态区间，基于广义似然比的零速区间检测法模型为：

其中，和/>为加速度计和陀螺仪的噪声方差，/>和/>为加速度计输出和陀螺仪输出，k为采样的加速度和角速度数据序号，n为检测器输出序号，W为窗口大小，/>为窗口内加速度均值，g为当地重力加速度，T(z_n)为检测器输出，γ为阈值；当检测器输出小于阈值时，判断该时刻为静止区间；

(2)将运动区间和静止区间分别定义为和/>并记录各区间内的数据量，m表示该运动阶段内包含的数据数量，s表示该静止阶段内包含的数据数量；

(3)将各区间内的数据数量与时间因子ΔT进行比较，若各区间的数据数量均大于时间因子，则表示区间划分正确，否则表明区间划分存在错误；

(4)对违反时间因子限制的区间进行状态转换，将误判为静止状态的区间转化为运动区间，将误判为运动状态的区间转化为静止区间，完成区间划分修正。

3.根据权利要求1所述的基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法，其特征在于，所述步骤二利用自适应随机森林算法对行人运动模式进行分类识别，模式类型主要分为：静止、平地行走、跑步、上楼、下楼、跨步上楼、跨步下楼七种，具体操作为：

1)设共有N个待分类样本T＝{(x₁,y₁)…(x_N,y_N)}，样本特征包括x轴加速度最小值及均值、x轴加速度四分位距、y轴加速度偏度、z轴加速度最大值及偏度、x轴角速度均值、y轴角速度标准差、z轴角速度最大值及均值、UWB足间距数据，共11个特征，对应静止、平地行走、跑步、上楼、下楼、跨步上楼、跨步下楼七种类型Y＝{L₁,L₂,…L₇}，其中x_i∈X,y_i∈Y,i＝1,2,…N；

2)利用bootstrap原理对训练样本进行采样得到训练样本子集，并由此构建特征子空间，通过不断重复操作，得到包含M个决策树的随机森林；

3)根据决策树对每个类别的分类能力计算决策树对该种运动模式的分类权重；对于L₁类型，将y_i＝L₁的样本设置为正样本y_i＝1，y_i≠L₁的样本设置为负样本y_i＝-1；初始化训练样本权重：

4)对于每一棵决策树h_n(x)，n为决策树序号，计算其对类型L₁的分类错误率

其中，sign()为符号函数，其输出值为1或-1。

将分类错误率与错误率要求进行比较，当/>时计算该决策树对当前类别的权重/>

同时更新样本权重

并再次计算分类错误率以及权重/>直到/>结束计算。

5)针对每一类运动模式重复上述权重计算过程，得到各决策树对各类运动模式的投票权重，由决策树按其权重决定最终分类结果：

4.根据权利要求1所述的基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法，其特征在于，所述步骤三中对处于零速区间的行人惯性导航系统进行零速修正，对于处于运动区间的导航结果按照运动模式进行分类，针对平地行走、跑步模式进行足间距约束与抬脚高度约束；具体足间约束方程为：

ρ₁＝[I_3×3 -I_3×3]

其中，和/>分别表示k时刻左脚和右脚三维位置，/>表示k时刻解算出的左右脚间的距离，l表示由UWB测距模块测量的左右脚之间距离信息；利用带约束的最小二乘法求得满足足间距约束的位置信息：

5.根据权利要求1所述的基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法，其特征在于，所述步骤三的抬脚高度约束方程为：

其中，表示k时刻通过捷联惯导算法解算出的左右脚之间的高度差，h表示最大抬脚高度；

将足间距约束后得到的导航状态约束到符合抬脚高度约束的子空间中，得到k时刻的双足导航状态信息：

其中，表示导航状态向量，D表示双足高度系数矩阵、/>表示误差协方差的逆矩阵，i∈{L,R}分别表示右脚和左脚，/>分别表示三维位置、速度、姿态信息；

判断双足位置信息是否满足抬脚高度约束，当解算的双足位置不满足抬脚高度约束时，使用不等式约束卡尔曼滤波得到最终导航状态

6.根据权利要求1所述的基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法，其特征在于，对上下楼运动模式进行楼梯高度约束，高度约束条件根据楼梯高度与检测到的迈步情况决定：

h_k＝n_step×h_stair

得到高度信息误差值为：

δp_k,z＝h_k-p_k,z

其中，δp_k,z为高度误差，p_k,z捷联惯导解算得到的高度值，h_k为当前时刻的高度约束条件。

7.根据权利要求1所述的基于UWB测距辅助的行人足绑式惯性导航系统误差修正方法，其特征在于，所述步骤三将不同运动模式中的误差作为卡尔曼滤波器的量测值修正行人导航系统误差；卡尔曼滤波器的状态方程和量测方程为：

式中，为9维误差向量，δp_k、δv_k、/>分别表示k时刻的位置误差、速度误差、姿态误差；Z_k为测量误差；Γ_k/k-1为系统噪声驱动矩阵；W_k-1和V_k分别为系统噪声向量和量测噪声向量；F_k和H_k分别为状态转移矩阵和量测矩阵；

根据行人的运动模式类型C_k，卡尔曼滤波器量测矩阵H_k以及观测误差δ_k可以表示为：

式中，v_static为静止时足部三维速度理论值，即v_static＝[000]，static、walk、run、stairs分别代表行人处于静止、行走、跑步、上下楼状态。