CN111649742A - 一种基于anfis辅助的高程估计方法 - Google Patents

Abstract

本发明公开了一种基于ANFIS辅助的高程估计方法，是以MEMS‑IMU传感器为载体，采用ANFIS对行人的运动模式进行识别，并结合楼梯的固定高度信息而提出。本发明提供的上述基于ANFIS辅助的高程估计方法，对环境具有鲁棒性，不受电磁干扰、气压变化等影响，也不依赖外部基础设施辅助；既可以利用模糊推理系统对规则的推理能力，又可以利用神经网络对参数的自学习能力，对上楼、下楼和和平地三种模式的检测具有健壮性；适合行人在多层建筑物内行走过程中，每个跨步高度不一样时的垂直位移估计。

Description

一种基于ANFIS辅助的高程估计方法

技术领域

本发明涉及室内定位技术领域，尤其涉及一种基于ANFIS辅助的高程估计方法。

背景技术

室内定位技术在消防领域的重要性日益凸显，消防员进入复杂的楼宇火灾现场救援时，消防室内定位系统能将作战人员所处火场位置提供给指挥中心，为被困人员提供最佳的救援路线。尤其，高程是消防室内定位中非常重要的要素，通过高程信息可估计出消防员在室内建筑救援抢险时所处的楼层，对保障一线消防员个人安危具有重要意义。

在室内环境的高程估计无法像室外一样采用卫星、激光等方式来获得。目前，通常采用WiFi指纹方法或者气压测高方法来获得室内的高程信息。WiFi指纹定位是通过在指纹库中存入楼层信息来进行楼层判定，然而，WiFi定位精度在3～5米，误差与楼层高相似，不利于楼层的判定，另外，WiFi信号受墙壁等遮挡严重，在具有天井的建筑物窗边，时常出现楼层误判的情况。气压测高方法则是依据地表高程与气压存在的物理关系，然而，气压易受温度、湿度、气流等影响。因此，对于火场等复杂环境中高程位置的确定，气压测高方法和WiFi指纹方法的实用性受限。

惯性定位是一种基于微机电惯性测量单元(Micro Electro Mechanical System-Inertial Measurement Unit,MEMS-IMU)传感器的室内自主定位方法，无需与周围环境发生声、光、电等的交互，不会发生个人信息与隐私的泄漏，易携带，非常适合消防单兵的定位。然而，由于MEMS-IMU传感器输出的加速度与角速度信息存在着固有漂移，以及惯性传感器的高度通道发散，因此，通常人们只关注平面位置的感知，而较少使用MEMS-IMU来获取高程信息。

发明内容

有鉴于此，本发明提供了一种基于ANFIS辅助的高程估计方法，用以估计消防员救灾时的垂直位移。

因此，本发明提供了一种基于ANFIS辅助的高程估计方法，包括如下步骤：

S1：采集MEMS-IMU传感器在第i个步态周期的输出数据，包括三维加速度和三维角速度，采用捷联惯性方法对采集的输出数据进行惯性解算，得到行人的姿态、速度和位置信息；其中，i为正整数；

S2：判断采集数据的统计值是否在阈值范围内；若是，则执行步骤S3、S5-S8；若否，则执行步骤S4-S8；

S3：确认采集数据所对应的时间段为第i个步态周期内的静止阶段；

S4：确认采集数据所对应的时间段为第i个步态周期内的运动阶段；

S5：在第i个步态周期内的静止阶段进行扩展卡尔曼滤波，对惯性解算得到的行人的姿态、速度和位置信息进行零速修正；

S6：在第i个步态周期内提取五个特征量，分别是垂直位移均值、垂直位移标准差、垂直位移差值、垂直位移最大值和俯仰角最大值；

S7：在第i个步态周期内的静止阶段，将提取的五个特征量输入训练好的ANFIS模型，将输出值与分类阈值进行比较，得到第i个步态周期内的运动模式识别结果；

S8：根据得到的运动模式识别结果对行人的垂直位置进行估计；

返回步骤S1，进行第i＝i+1个步态周期的估计，直至定位结束。

在一种可能的实现方式中，在本发明提供的上述基于ANFIS辅助的高程估计方法中，所述ANFIS模型的训练过程，具体包括如下步骤：

SS1：采集MEMS-IMU传感器多个步态周期的输出数据，包括三维加速度和三维角速度，作为训练数据，采用捷联惯性方法对训练数据进行解算，得到行人的姿态、速度和位置信息；

SS2：以步态周期为单位，对训练数据进行标记，将平面步行状态标记为0，将步行上楼状态标记为+1，将步行下楼状态标记为-1；

SS3：判断训练数据的统计值是否在阈值范围内；若是，则执行步骤SS4、SS6-SS9；若否，则执行步骤SS5-SS9；

SS4：确认训练数据所对应的时间段为步态周期内的静止阶段；

SS5：确认训练数据所对应的时间段为步态周期内的运动阶段；

SS6：在每个步态周期内的静止阶段进行扩展卡尔曼滤波，对惯性解算得到的行人的姿态、速度和位置信息进行零速修正；

SS7：在每个步态周期内提取五个特征量，分别是垂直位移均值、垂直位移标准差、垂直位移差值、垂直位移最大值和俯仰角最大值；

SS8：将提取的五个特征量作为ANFIS模型的输入，并对五个特征量进行模糊化，每个特征量分别对应三个模糊集Z＝正常、B＝大、S＝小，输出采用线性模型的隶属函数，构建ANFIS模型；

SS9：利用标记好的训练数据对ANFIS模型进行训练，得到训练好的ANFIS模型进行保存。

在一种可能的实现方式中，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S1中，惯性解算得到的行人的姿态、速度和位置信息如下：

其中，n表示导航坐标系；b表示载体坐标系；Vⁿ表示导航坐标系下的三维速度；

表示导航坐标系下的三维速度的微分；Pⁿ表示导航坐标系下的三维位置，垂直方向上的分量用h表示；

表示导航坐标系下的三维位置的微分；

表示由b系到n系转换的姿态矩阵；

表示由b系到n系转换的姿态矩阵的微分；Ω^b表示由陀螺仪输出角速度构成的反对称矩阵；f^b表示载体坐标系下的比力；gⁿ表示地球重力场矢量。

在一种可能的实现方式中，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S2中，采集数据的统计值计算公式如下：

其中，a表示加速度；山表示角速度；W表示窗长；k表示采样序号，取值为从W开始的整数；l依次取从k-W+1到k的采样序号；g表示重力系数，为9.8；

表示加速度的方差；

表示角速度的方差；||·||表示L-2范数；

表示窗长在W以内的加速度均值。

在一种可能的实现方式中，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S5，在第i个步态周期内的静止阶段进行扩展卡尔曼滤波，对惯性解算得到的行人的姿态、速度和位置信息进行零速修正，具体包括：

定义扩展卡尔曼滤波的15维误差状态向量为：

其中，15维误差状态向量包含9个导航误差和6个传感器误差，分别为三维位置误差向量δPⁿ，三维速度误差向量δVⁿ，三维姿态误差向量δφⁿ，三维陀螺漂移ε^b及三维加速度偏置

系统扩展卡尔曼滤波模型为：

δx_m+1＝F_mδx_m+w_mm (6)

δz_m＝H_mδx_m+v_m (7)

其中，δx_m+1表示m+1时刻的系统状态，δx_m表示m时刻的系统状态，δz_m表示m时刻的误差观测量，F_m表示转移矩阵，H_m表示量测矩阵，w_m表示过程噪声，v_m表示量测噪声。

在一种可能的实现方式中，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S6中，提取的五个特征量分别为：

Δh_i＝h_i，r-h_i，0 (10)

h_i，max＝max_1≤j≤r{|h_i，j-h_i，0|} (11)

p_i，max＝max_1≤j≤r{p_i，j} (12)

其中，h_i，0表示第i个步态周期开始时处于静止状态的高度，h_i，j表示第i个步态周期运动阶段第j个采样的高度，p_i，j表示第i个步态周期运动阶段第j个采样的俯仰角，h_i，r表示第i个步态周期运动阶段最后一个采样的高度，r为第i个步态周期运动阶段的总采样数目。

在一种可能的实现方式中，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S7中，得到的第i个步态周期内的运动模式识别结果如下：

其中，y_i表示第i个步态周期中ANFIS模型的输出值，σ_th表示分类阈值，C_i表示在第i个步态周期的分类结果。

在一种可能的实现方式中，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S8，根据得到的运动模式识别结果对行人的垂直位置进行估计，具体包括：

根据得到的第i个步态周期内的运动模式识别结果，对行人的垂直位置进行如下估计：

其中，K表示每个台阶的高度；

表示第i步的垂直估计位置；

表示第i-1步的垂直估计位置；INT(x)为取整函数，取距离x最近的整数；sgn(x)为符号函数，当x＞0时，取值为1，当x＝0时，取值为0，当x＜0时，取值为-1。

本发明提供的上述基于ANFIS辅助的高程估计方法，是以鞋嵌式MEMS-IMU传感器作为载体，采用ANFIS对行人的运动模式进行识别，并结合楼梯的固定高度信息而提出的一种高程估计方法。基于ANFIS的运动模式识别，既对输入具有弹性，又具有自学习参数的能力，对垂直位移的估计具有重要作用。本发明提供的上述基于ANFIS辅助的高程估计方法，对环境具有鲁棒性，不受电磁干扰、气压变化等影响，也不依赖外部基础设施辅助，尤其适用于多层建筑火灾场景中消防员上下楼梯或同一层水平行走时的垂直定位，对于救灾与灭火具有重要意义；既可以利用模糊推理系统对规则的推理能力，又可以利用神经网络对参数的自学习能力，对上楼、下楼和和平地三种模式的检测具有健壮性；本发明适合行人在多层建筑物内行走过程中，每个跨步高度不一样时的垂直位移估计；并且，算法复杂度低，易于工程实现。

附图说明

图1为本发明提供的一种基于ANFIS辅助的高程估计方法的流程图；

图2为本发明提供的一种基于ANFIS辅助的高程估计方法中ANFIS模型训练过程的流程图；

图3为本发明实施例1所描述的实验场景中的实际测试结果图。

具体实施方式

下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整的描述，显然，所描述的实施方式仅仅是作为例示，并非用于限制本发明。

本发明提供的一种基于ANFIS辅助的高程估计方法，如图1所示，包括如下步骤：

S1：采集MEMS-IMU传感器在第i个步态周期的输出数据，包括三维加速度和三维角速度，采用捷联惯性方法对采集的输出数据进行惯性解算，得到行人的姿态、速度和位置信息；其中，i为正整数；

具体地，可以在行人的鞋跟位置安装MEMS-IMU传感器；行人行走时的步态周期由静止阶段和运动阶段构成；

S2：判断采集数据的统计值是否在阈值范围内；若是，则执行步骤S3、S5-S8；若否，则执行步骤S4-S8；

S3：确认采集数据所对应的时间段为第i个步态周期内的静止阶段；

S4：确认采集数据所对应的时间段为第i个步态周期内的运动阶段；

S5：在第i个步态周期内的静止阶段进行扩展卡尔曼滤波(Extended KalmanFilter,EKF)，对惯性解算得到的行人的姿态、速度和位置信息进行零速修正；

S6：在第i个步态周期内提取五个特征量，分别是垂直位移均值、垂直位移标准差、垂直位移差值、垂直位移最大值和俯仰角最大值；

S7：在第i个步态周期内的静止阶段，将提取的五个特征量输入训练好的ANFIS模型，将输出值与分类阈值进行比较，得到第i个步态周期内的运动模式识别结果；

S8：根据得到的运动模式识别结果对行人的垂直位置进行估计；

返回步骤S1，进行第i＝i+1个步态周期的估计，直至定位结束。

本发明提供的上述基于ANFIS辅助的高程估计方法，是以鞋嵌式MEMS-IMU传感器作为载体，采用自适应神经网络模糊推理系统 (Adaptive Network-based FuzzyInference System,ANFIS)对行人的运动模式进行识别，并结合楼梯的固定高度信息而提出的一种高程估计方法。基于ANFIS的运动模式识别，既对输入具有弹性，又具有自学习参数的能力，对垂直位移的估计具有重要作用。本发明提供的上述基于ANFIS辅助的高程估计方法，对环境具有鲁棒性，不受电磁干扰、气压变化等影响，也不依赖外部基础设施辅助，尤其适用于多层建筑火灾场景中消防员上下楼梯或同一层水平行走时的垂直定位，对于救灾与灭火具有重要意义；既可以利用模糊推理系统对规则的推理能力，又可以利用神经网络对参数的自学习能力，对上楼、下楼和和平地三种模式的检测具有健壮性；本发明适合行人在多层建筑物内行走过程中，每个跨步高度不一样时的垂直位移估计；并且，算法复杂度低，易于工程实现。

在具体实施时，在本发明提供的上述基于ANFIS辅助的高程估计方法中，ANFIS模型的训练过程，如图2所示，具体包括如下步骤：

SS1：采集MEMS-IMU传感器多个步态周期的输出数据，包括三维加速度和三维角速度，作为训练数据，采用捷联惯性方法对训练数据进行解算，得到行人的姿态、速度和位置信息；

SS2：以步态周期为单位，对训练数据进行标记，将平面步行状态标记为0，将步行上楼状态标记为+1，将步行下楼状态标记为-1；

SS3：判断训练数据的统计值是否在阈值范围内；若是，则执行步骤SS4、SS6-SS9；若否，则执行步骤SS5-SS9；

SS4：确认训练数据所对应的时间段为步态周期内的静止阶段；

SS5：确认训练数据所对应的时间段为步态周期内的运动阶段；

SS6：在每个步态周期内的静止阶段进行扩展卡尔曼滤波，对惯性解算得到的行人的姿态、速度和位置信息进行零速修正；

SS7：在每个步态周期内提取五个特征量，分别是垂直位移均值、垂直位移标准差、垂直位移差值、垂直位移最大值和俯仰角最大值；

SS8：将提取的五个特征量作为ANFIS模型的输入，并对五个特征量进行模糊化，每个特征量分别对应三个模糊集Z＝正常、B＝大、S＝小，输出采用线性模型的隶属函数(Membership Function,MF)，构建ANFIS模型；具体地，模糊集的MF曲线可以采用三角形形状；

SS9：利用标记好的训练数据对ANFIS模型进行训练，得到训练好的ANFIS模型进行保存。

在具体实施时，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S1中，惯性解算得到的行人的姿态、速度和位置信息如下：

其中，n表示导航坐标系；b表示载体坐标系；Vⁿ表示导航坐标系下的三维速度；

表示导航坐标系下的三维速度的微分；Pⁿ表示导航坐标系下的三维位置，垂直方向上的分量用h表示；

表示导航坐标系下的三维位置的微分；

表示由b系到n系转换的姿态矩阵；

表示由b系到n系转换的姿态矩阵的微分；Ω^b表示由陀螺仪输出角速度构成的反对称矩阵；f^b表示载体坐标系下的比力；gⁿ表示地球重力场矢量。利用上述公式(1)、(2)、(3)解算出的行人速度、位置和姿态对环境具有鲁棒性。

在具体实施时，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S2中，采集数据的统计值计算公式如下：

其中，a表示加速度；ω表示角速度；W表示窗长；k表示采样序号，取值为从W开始的整数；l依次取从k-W+1到k的采样序号；g表示重力系数，为9.8；

表示加速度的方差；

表示角速度的方差；||·||表示L-2范数；

表示窗长在W以内的加速度均值。利用上述公式(4)计算出统计值，通过判断统计值是否在阈值范围内可以有效检测出行人步态周期中的静止阶段和运动阶段。

在具体实施时，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S5，在第i个步态周期内的静止阶段进行扩展卡尔曼滤波，对惯性解算得到的行人的姿态、速度和位置信息进行零速修正，具体可以通过以下方式实现：

定义扩展卡尔曼滤波的15维误差状态向量为：

其中，15维误差状态向量包含9个导航误差和6个传感器误差，分别为三维位置误差向量8Pⁿ，三维速度误差向量8Vⁿ，三维姿态误差向量δφⁿ，三维陀螺漂移ε^b及三维加速度偏置

系统扩展卡尔曼滤波模型为：

δx_m+1＝F_mδx_m+w_m (6)

δz_m＝H_mδx_m+v_m (7)

其中，δx_m+1表示m+1时刻的系统状态，δx_m表示m时刻的系统状态，δz_m表示m时刻的误差观测量，F_m表示转移矩阵，H_m表示量测矩阵，w_m表示过程噪声，v_m表示量测噪声。利用行人步行时每个步态周期的静止阶段速度为零作为约束，可以有效减少速度、位置及姿态误差。

在具体实施时，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S6中，提取的五个特征量分别为：

Δh_i＝h_i，r-h_i，0 (10)

h_i，max＝max_1≤j≤r{|h_i，j-h_i，0|} (11)

p_i，max＝max_1≤j≤r{p_i，j} (12)

其中，h_i，0表示第i个步态周期开始时处于静止状态的高度；h_i，j表示第i个步态周期运动阶段第j个采样的高度；p_i，j表示第i个步态周期运动阶段第j个采样的俯仰角，可以通过当前时刻姿态矩阵的运算得到；h_i，r表示第i个步态周期运动阶段最后一个采样的高度，r为第i个步态周期运动阶段的总采样数目。利用上述公式(8)、(9)、(10)、(11)、(12)得到的五个特征量，对行人运动模式识别的准确性与稳健性起着重要作用。

在具体实施时，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S7中，得到的第i个步态周期内的运动模式识别结果如下：

其中，y_i表示第i个步态周期中ANFIS模型的输出值，σ_th表示分类阈值，C_i表示在第i个步态周期的分类结果。利用上述公式(13)可以将ANFIS模型的输出值转换成三种运动模式，为高程估计奠定基础。

在具体实施时，在本发明提供的上述基于ANFIS辅助的高程估计方法中，步骤S8，根据得到的运动模式识别结果对行人的垂直位置进行估计，具体可以通过以下方式实现：

根据得到的第i个步态周期内的运动模式识别结果，对行人的垂直位置进行如下估计：

其中，K表示每个台阶的高度；

表示第i步的垂直估计位置；

表示第i-1步的垂直估计位置；INT(x)为取整函数，取距离x最近的整数；sgn(x)为符号函数，当x>0时，取值为1，当x＝0时，取值为0，当x<0时，取值为-1。利用上述公式(14)估计得到的行人高程不受惯性垂直通道发散的影响。

下面通过一个具体的实施例对本发明提供的上述基于ANFIS辅助的高程估计方法的具体实施进行详细说明。

实施例1：

实验场景为：测试者从一处工厂的二楼步行到一楼后，在室外行走了大约500秒后回到室内，并沿相同的楼梯从一楼回到二楼的起点位置。整个测试时间持续了约640秒，工厂一楼和二楼之间的楼高为5.4米，楼梯台阶数为36个，单个楼梯台阶高0.15米，室外行走时高程保持不变，上楼或下楼期间跨步高度不相同。如图3所示，长虚线是通过对激光测距仪(徕卡X310)测出的数据进行插值得到的，由于激光测距仪具有毫米级的精度，因此，长虚线可以被用来当作垂直方向上的真实参考轨迹；点虚线表示采用型号为BMP180的气压计进行高程估计的结果，由于室外温度造成的影响，气压计的输出波动大，当测试者返回起始点时，估计结果达到了2米的误差；实线代表采用本发明提供的上述基于ANFIS辅助的高程估计方法进行高程估计的结果，该结果与长虚线(真实参考轨迹)接近，不受环境中如气压等因素的影响，最大误差仅为0.3米。该实验验证了本发明提供的上述基于ANFIS辅助的高程估计方法具有稳健性与较高的准确性。

本发明提供的上述基于ANFIS辅助的高程估计方法，是以鞋嵌式MEMS-IMU传感器作为载体，采用ANFIS对行人的运动模式进行识别，并结合楼梯的固定高度信息而提出的一种高程估计方法。基于ANFIS的运动模式识别，既对输入具有弹性，又具有自学习参数的能力，对垂直位移的估计具有重要作用。本发明提供的上述基于ANFIS辅助的高程估计方法，对环境具有鲁棒性，不受电磁干扰、气压变化等影响，也不依赖外部基础设施辅助，尤其适用于多层建筑火灾场景中消防员上下楼梯或同一层水平行走时的垂直定位，对于救灾与灭火具有重要意义；既可以利用模糊推理系统对规则的推理能力，又可以利用神经网络对参数的自学习能力，对上楼、下楼和和平地三种模式的检测具有健壮性；本发明适合行人在多层建筑物内行走过程中，每个跨步高度不一样时的垂直位移估计；并且，算法复杂度低，易于工程实现。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (8)

1.一种基于ANFIS辅助的高程估计方法，其特征在于，包括如下步骤：

S1：采集MEMS-IMU传感器在第i个步态周期的输出数据，包括三维加速度和三维角速度，采用捷联惯性方法对采集的输出数据进行惯性解算，得到行人的姿态、速度和位置信息；其中，i为正整数；

S2：判断采集数据的统计值是否在阈值范围内；若是，则执行步骤S3、S5-S8；若否，则执行步骤S4-S8；

S3：确认采集数据所对应的时间段为第i个步态周期内的静止阶段；

S4：确认采集数据所对应的时间段为第i个步态周期内的运动阶段；

S5：在第i个步态周期内的静止阶段进行扩展卡尔曼滤波，对惯性解算得到的行人的姿态、速度和位置信息进行零速修正；

S6：在第i个步态周期内提取五个特征量，分别是垂直位移均值、垂直位移标准差、垂直位移差值、垂直位移最大值和俯仰角最大值；

S7：在第i个步态周期内的静止阶段，将提取的五个特征量输入训练好的ANFIS模型，将输出值与分类阈值进行比较，得到第i个步态周期内的运动模式识别结果；

S8：根据得到的运动模式识别结果对行人的垂直位置进行估计；

返回步骤S1，进行第i＝i+1个步态周期的估计，直至定位结束。

2.如权利要求1所述的基于ANFIS辅助的高程估计方法，其特征在于，所述ANFIS模型的训练过程，具体包括如下步骤：

SS1：采集MEMS-IMU传感器多个步态周期的输出数据，包括三维加速度和三维角速度，作为训练数据，采用捷联惯性方法对训练数据进行解算，得到行人的姿态、速度和位置信息；

SS2：以步态周期为单位，对训练数据进行标记，将平面步行状态标记为0，将步行上楼状态标记为+1，将步行下楼状态标记为-1；

SS3：判断训练数据的统计值是否在阈值范围内；若是，则执行步骤SS4、SS6-SS9；若否，则执行步骤SS5-SS9；

SS4：确认训练数据所对应的时间段为步态周期内的静止阶段；

SS5：确认训练数据所对应的时间段为步态周期内的运动阶段；

SS6：在每个步态周期内的静止阶段进行扩展卡尔曼滤波，对惯性解算得到的行人的姿态、速度和位置信息进行零速修正；

SS7：在每个步态周期内提取五个特征量，分别是垂直位移均值、垂直位移标准差、垂直位移差值、垂直位移最大值和俯仰角最大值；

SS8：将提取的五个特征量作为ANFIS模型的输入，并对五个特征量进行模糊化，每个特征量分别对应三个模糊集Z＝正常、B＝大、S＝小，输出采用线性模型的隶属函数，构建ANFIS模型；

SS9：利用标记好的训练数据对ANFIS模型进行训练，得到训练好的ANFIS模型进行保存。

3.如权利要求1所述的基于ANFIS辅助的高程估计方法，其特征在于，步骤S1中，惯性解算得到的行人的姿态、速度和位置信息如下：

其中，n表示导航坐标系；b表示载体坐标系；Vⁿ表示导航坐标系下的三维速度；

表示导航坐标系下的三维速度的微分；Pⁿ表示导航坐标系下的三维位置，垂直方向上的分量用h表示；

表示导航坐标系下的三维位置的微分；

表示由b系到n系转换的姿态矩阵；

表示由b系到n系转换的姿态矩阵的微分；Ω^b表示由陀螺仪输出角速度构成的反对称矩阵；f^b表示载体坐标系下的比力；gⁿ表示地球重力场矢量。

4.如权利要求3所述的基于ANFIS辅助的高程估计方法，其特征在于，步骤S2中，采集数据的统计值计算公式如下：

其中，a表示加速度；ω表示角速度；W表示窗长；k表示采样序号，取值为从W开始的整数；l依次取从k-W+1到k的采样序号；g表示重力系数，为9.8；

表示加速度的方差；

表示角速度的方差；||·||表示L-2范数；

表示窗长在W以内的加速度均值。

5.如权利要求4所述的基于ANFIS辅助的高程估计方法，其特征在于，步骤S5，在第i个步态周期内的静止阶段进行扩展卡尔曼滤波，对惯性解算得到的行人的姿态、速度和位置信息进行零速修正，具体包括：

定义扩展卡尔曼滤波的15维误差状态向量为：

其中，15维误差状态向量包含9个导航误差和6个传感器误差，分别为三维位置误差向量δPⁿ，三维速度误差向量δVⁿ，三维姿态误差向量δφⁿ，三维陀螺漂移ε^b及三维加速度偏置

系统扩展卡尔曼滤波模型为：

δx_m+1＝F_mδx_m+w_m (6)

δz_m＝H_mδx_m+v_m (7)

其中，δx_m+1表示m+1时刻的系统状态，δx_m表示m时刻的系统状态，δz_m表示m时刻的误差观测量，F_m表示转移矩阵，H_m表示量测矩阵，w_m表示过程噪声，v_m表示量测噪声。

6.如权利要求5所述的基于ANFIS辅助的高程估计方法，其特征在于，步骤S6中，提取的五个特征量分别为：

Δh_i＝h_i,r-h_i,0 (10)

h_i,max＝max_1≤j≤r{|h_i,j-h_i,0|} (11)

p_i,max＝max_1≤j≤r{p_i,j} (12)

其中，h_i,0表示第i个步态周期开始时处于静止状态的高度，g_i,j表示第i个步态周期运动阶段第j个采样的高度，p_i,j表示第i个步态周期运动阶段第j个采样的俯仰角，h_i,r表示第i个步态周期运动阶段最后一个采样的高度，r为第i个步态周期运动阶段的总采样数目。

7.如权利要求6所述的基于ANFIS辅助的高程估计方法，其特征在于，步骤S7中，得到的第i个步态周期内的运动模式识别结果如下：

其中，y_i表示第i个步态周期中ANFIS模型的输出值，σ_th表示分类阈值，C_i表示在第i个步态周期的分类结果。

8.如权利要求7所述的基于ANFIS辅助的高程估计方法，其特征在于，步骤S8，根据得到的运动模式识别结果对行人的垂直位置进行估计，具体包括：

根据得到的第i个步态周期内的运动模式识别结果，对行人的垂直位置进行如下估计：

其中，K表示每个台阶的高度；

表示第i步的垂直估计位置；

表示第i-1步的垂直估计位置；INT(x)为取整函数，取距离x最近的整数；sgn(x)为符号函数，当x>0时，取值为1，当x＝0时，取值为0，当x<0时，取值为-1。

Images