CN114739397A - 矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法 - Google Patents

Abstract

本发明公开了矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，引入航向记忆与惯性置信参数参与卡尔曼滤波数据融合，航向记忆信息能够估计目标历史运动状态并参与计算惯性置信参数，而惯性置信参数则作为误差权重系数参与卡尔曼滤波数据融合，实现定位数据融合权重的自适应，本方法能够有效实现定位数据自适应数据融合，在减少定位误差，抑制定位数据漂移上取得良好效果。

Description

矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法

技术领域

本发明涉及一种卡尔曼滤波融合定位方法，具体涉及一种矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法。

背景技术

矿井环境作为地面空间的补充与延伸，实现矿井环境高精度定位对于实现矿井自动化、智能化、少人化具有重要意义。UWB射频技术具有对信道衰落不敏感、发射信号功率谱密度低、低截获能力、系统复杂度低、能提供数厘米的定位精度等优点，在理想条件下定位精度能够达到厘米级别。

但煤矿巷道环境异常复杂，巷道断面一般为2～15m不等但巷道长度往往达到400～600m，煤矿井下狭长有限空间内射频定位解算算法往往难以获得良好解算结果。同时井下巷道不同材质墙壁具有不同物理特性，对无线信号井下反射和衍射机理往往不同，对定位信号影响也不尽相同。并且考虑到煤矿井下高温、高粉尘、潮湿的特殊环境以及巷道内存在人体或设备遮挡造成的信号衰减与非视距干扰，往往对无线射频信号及射频定位技术具有不利影响。虽然UWB定位相较于其他射频定位技术具有诸多技术优势与抗干扰特性，仍无可避免在煤矿巷道环境下面临诸多干扰与挑战。通常考虑将UWB与无需与外界信息交互即可获得目标运动信息的IMU传感器结合提高矿井特殊环境下定位精度。

目前相关专利主要采用以下几种方式：第一，通过UWB技术自身单独定位，实现矿井环境定位与系统，如授权号为CN202021940409.9井下矿车定位技术，如授权号CN202011142017.2的DS-TWR井下定位技术，以及CN202022741002.X定位的手持仪通过对UWB信号放大实现高精度定位，国外如申请号US20050215269A1将到达时间与到达时间差定位方式融合实现定位；第二，融合多种传感器技术方式，实现定位数据融合提高定位精度，如授权号为CN201910806408.0通过激光与惯性传感器滑动窗口进行状态估计，并进行UWB坐标对齐，授权号CN201911342976.6的方法将UWB与Zigbee技术融合实现井下定位，以及授权号CN201610814605.3的WIFI-UWB混合井下人员精确定位系统及方法。而在UWB于IMU结合的方向上，现有方法大多使用IMU传感器数据结合卡尔曼滤波状态估计与UWB定位数据结合，以减少UWB在非视距或易产生多径效应情况下的误差。但大部分方法在自适应与泛用能力上表现不尽如人意，不能满足矿井巷道特殊环境高精度定位需要，也不能解决复杂环境下单一定位技术受限以及高效融合等问题。

发明内容

针对上述存在的技术不足，本发明的目的是提供矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，能够解决上述问题。

为解决上述技术问题，本发明采用如下技术方案：

本发明提供矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，包括以下步骤：

(1)在巷道内呈矩阵式安装4个定位基站，在巷道内确定一个坐标原点并根据此坐标原点以巷道走向为X轴、以巷道宽度方向为Y轴建立坐标系，然后以此坐标系确定每个定位基站的坐标，在目标物体上安装定位标签；

(2)设定航向计算采样间隔参数Δg，取两倍Δg长度获取定位标签的A(x_t，y_t)、B(x_t-1，y_t-1)、C(x_t-Δg，y_t-Δg)、D(x_t-Δg-1，y_t-Δg-1)四点定位坐标数据的UWB定位坐标与IMU增量解算坐标，以此坐标作为历史队列坐标，并计算向量

和向量

与X轴正方向夹角，将获得的夹角信息作为历史航向分量信息；计算公式如下：

其中arg(y/x)为向量的辐角主值，通过此公式能够将两定位点组成的向量在定位坐标系下从弧度度值转换为与X轴正方向组成的夹角，实现历史航线信息量化便于后续权重衡量；y与x分别为定位采样到的定位坐标系下定位目标。

(3)对步骤(2)获取的历史航向分量信息，用以下公式获取均值HMA_t；

式中t为采样次数，i表示当前求和公式求和区间起始次数，本方法以t-Δg为求和过程起点，

为运用航向计算公式对记忆队列定位坐标两两计算航向均值；

(4)以航向计算采样间隔参数Δg对应获取时刻t,获取下一个Δg长度对应的时刻t的历史坐标信息A(x_t，y_t)，以及t+1时刻标签的UWB定位坐标L1(x₁，y₁)、IMU增量解算坐标L2(x₂，y₂)，并形成向量关系

与

计算

与

与X轴正方向夹角数值，并按照以下公式计算上述夹角数值与步骤(3)获得的HMA_t的差值θ₁与θ₂；

式中：

分别为上一时刻融合位置与新一时刻两种测量位置所形成向量计算的航向角度值；

(5)根据步骤(4)获得的θ₁与θ₂，按照以下公式计算权重系数T₁、T₂、C₁、C₂；

式中：σ为比重调解参数，用于成倍扩大或缩小比重；μ为航向控制系数，用于处理航向为零或航向角度极小等过度影响控制权重比例的情况；

(6)利用步骤(5)获得的权重系数C₁、C₂；按照以下公式控制UWB与IMU误差协方差矩阵参与卡尔曼滤波进行印象融合权重，获得融合坐标信息；

式中：X_t+1为t+1时刻后验估计信息，Z_t+1为UWB定位观测位置量；Q为IMU增量解算误差协方差矩阵；R为UWB三边定位误差协方差矩阵；

为t+1时刻的先验估计协方差；A为状态转移矩阵，为n×n阶方阵；P_t为t时刻后验估计协方差；A^T为状态转移矩阵的转置；K_t+1为t+1时刻的卡尔曼滤波增益；H^T为量测矩阵的转置；H为量测矩阵；

为t+1时刻后验估计位置数据；I为单位矩阵；

(7)将步骤(6)获得融合坐标信息更新步骤(2)中的历史队列坐标。

将记忆队列更新，将距离此次测量时间最远的融合位置弹出，并将最新融合位置压入队列末端，以此完成历史定位队列数据更新。

优选地，所述步骤(2)中获得UWB定位坐标时采用TWR测距方法，并采用三边定位获得坐标解算，使用最小二乘算法进行解算优化。如公式：

并求取S(x,y)S为0时x，y最小值作为最终解算定位坐标数据。

优选地，利用TWR测距方法获得UWB定位坐标时，以c为电磁波传播速度即光速，ct值为测得的各锚节点与标签之间的距离，(x_i,y_i)为第i个锚节点的坐标位置信息；按照以下公式计算出标签坐标信息(x,y)：

优选地，所述步骤(2)中获得IMU增量解算坐标时，采用以下公式对UWB定位坐标进行转化后获得：

C_t＝C_t-1·(2I_3×3+ω_tΔt)·(2I_3×3-ω_tΔt)^-1

式中：

表示陀螺仪角速率矩阵，C_t-1表示t-1时刻的旋转矩阵，Δt为传感器采样间隔，ω_t表示IMU测量得到的角速度旋转矩阵。I为三阶单位矩阵。

优选地，所述ω_t具有xyz三轴分量，按照以下公式表达；

其中ω_t(x)、ω_t(y)、ω_t(z)分别为传感器坐标系下x、y、z轴传感器数据分量；

优选地，所述ω_t三轴分量上的加速度采用如下公式计算：

式中：

为传感器坐标系下的加速度矩阵，g为重力加速度，计算获得定位坐标系下各轴定位加速度分量a_t；C_t为计算得出的t时刻的旋转矩阵。

本发明的有益效果在于：

本发明基于矿井巷道内狭长物理特征与目标历史运动状态，能够充分挖掘目标运动趋势，而且设计了一种权重估计算法并参与卡尔曼滤波数据融合，能够充分，实现UWB与IMU定位数据自适应融合，能够满足矿井巷道特殊环境高精度定位需要。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明的历史运动状态特征提取示意图。

图2是本发明的权重计算示意图。

图3是本发明流程示意图。

附图标记及符号说明：

11、基站；12、真实坐标；13、历史定位坐标；14、航向向量；15、UWB定位坐标；16、融合坐标；17、IMU解算坐标；18、真实运动航向向量；21、上一时刻真实坐标；22、上一时刻估计位置；23、新时刻UWB定位坐标；24、新时刻融合坐标；25、IMU解算坐标；26、真实航向向量。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实例提供一种矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，包括以下步骤：

(1)在巷道内呈矩阵式安装4个定位基站，在巷道内确定一个坐标原点并根据此坐标原点以巷道走向为X轴、以巷道宽度方向为Y轴建立坐标系，然后以此坐标系确定每个定位基站的坐标，在目标物体上安装定位标签；

(2)设定航向计算采样间隔参数Δg，取两倍Δg长度获取定位标签的A(x_t，y_t)、B(x_t-1，y_t-1)、C(x_t-Δg，y_t-Δg)、D(x_t-Δg-1，y_t-Δg-1)四点定位坐标数据，以此坐标作为历史队列坐标，并计算向量

和向量

与X轴正方向夹角，将获得的夹角信息作为历史航向分量信息；计算公式如下：

其中arg(y/x)为向量的辐角主值，通过此公式能够将两定位点组成的向量在定位坐标系下从弧度度值转换为与X轴正方向组成的夹角，实现历史航线信息量化便于后续权重衡量；y与x分别为定位采样到的定位坐标系下定位目标；

(3)对步骤(2)获取的历史航向分量信息，用以下公式获取均值HMA_t；

式中t为采样次数，i表示当前求和公式求和区间起始次数，本方法以t-Δg为求和过程起点，

为运用航向计算公式对记忆队列定位坐标两两计算航向均值；

(4)以航向计算采样间隔参数Δg对应获取时刻t,获取下一个Δg长度对应的时刻t的历史坐标信息A(x_t，y_t)，以及t+1时刻标签的UWB定位坐标L1(x₁，y₁)、IMU增量解算坐标L2(x₂，y₂)，并形成向量关系

与

计算

与

与X轴正方向夹角数值，并按照以下公式计算上述夹角数值与步骤(3)获得的HMA_t的差值θ₁与θ₂；

式中：

分别为上一时刻融合位置与新一时刻两种测量位置所形成向量计算的航向角度值；

(5)根据步骤(4)获得的θ₁与θ₂，按照以下公式计算权重系数T1、T2、C1、C2；

式中：σ为比重调解参数，用于成倍扩大或缩小比重；μ为航向控制系数，用于处理航向为零或航向角度极小等过度影响控制权重比例的情况；

(6)利用步骤(5)获得的权重系数C₁、C₂；按照以下公式控制UWB与IMU误差协方差矩阵参与卡尔曼滤波进行印象融合权重，获得融合坐标信息；

式中：X_t+1为t+1时刻后验估计信息，Z_t+1为UWB定位观测位置量；Q为IMU增量解算误差协方差矩阵；R为UWB三边定位误差协方差矩阵；

为t+1时刻的先验估计协方差；A为状态转移矩阵，为n×n阶方阵；P_t为t时刻后验估计协方差；A^T为状态转移矩阵的转置；K_t+1为t+1时刻的卡尔曼滤波增益；H^T为量测矩阵的转置；H为量测矩阵；

为t+1时刻后验估计位置数据；I为单位矩阵；

(7)将步骤(6)获得融合坐标信息更新步骤(2)中的历史队列坐标。将记忆队列更新，将距离此次测量时间最远的融合位置弹出，并将最新融合位置压入队列末端，以此完成历史定位队列数据更新。

所述步骤(2)中获得UWB定位坐标时采用TWR测距方法，当测得标签与各基站之间的距离信息后，采用三边定位算法进行目标位置的解算。假设在二维平面上，则基站与标签的关系为

其中c为电磁波传播速度即光速，ct值为测得的距离锚与标签之间的距离，(x_i,y_i)为第i个锚节点的坐标位置信息。根据测量得到的距离与锚节点已知坐标信息，即可反解出标签坐标信息(x,y)。

之后采用最小二乘算法作为UWB定位解算算法，最终获取UWB坐标解算位置(x₂,y₂)。

如公式:

并求取S为0时x，y最小值作为最终解算定位坐标数据。

IMU坐标系与定位坐标系之间通过坐标系转换矩阵完成坐标系转化；转换矩阵计算如公式如下所示：

C_t＝C_t-1·(2I_3×3+ω_tΔt)·(2I_3×3-ω_tΔt)^-1

式中：

表示陀螺仪角速率矩阵，C_t-1表示t-1时刻的旋转矩阵，Δt为传感器采样间隔，ω_t表示IMU测量得到的角速度旋转矩阵。I为三阶单位矩阵。

三轴陀螺仪旋转矩阵ω_t具有xyz三轴分量，按如下形式表示：

其中ω_t(x)、ω_t(y)、ω_t(z)分别为传感器坐标系下x、y、z轴传感器数据分量；

由陀螺仪角速率矩阵ω_t(x)、ω_t(y)、ω_t(z)与历史状态矩阵信息C_t-1结合，计算t时刻状态矩阵

估计状态矩阵

即可视为C_t，将固定安装于待测井下设备的加速度计测得的传感器各加速度分量转化至定位坐标系中获得定位坐标系加速度；定位坐标系中加速度计算公式如公式：

优选地，所述ω_t三轴分量上的加速度采用如下公式计算：

式中：

为传感器坐标系下的加速度矩阵，g为重力加速度，计算获得定位坐标系下各轴定位加速度分量a_t；C_t为计算得出的t时刻的旋转矩阵。

在获得各加速度分量信息后，通过两次积分获得目标预测位置，首次积分加速度后获得速度增量，二次积分通过对速度积分获得位移增量，与t-1时刻位置信息相加后获得t时刻目标预测位置，速度与位置按照如下公式计算：

其中，v_t-1为t-1时刻速度信息，Δt为采样时间间隔，公式中p_t-1为t-1时刻位置信息，

与

分别为t时刻速度估计结果与位置估计结果；在惯性模块长时间单独运行情况下，传感器误差随时间不断积累最终导致严重的位置漂移，单一MEMS IMU难以满足长时间定位解算需求，本实施例将IMU解算定位与UWB定位通过卡尔曼滤波融合结合以提高矿井巷道复杂定位环境下整体精度，抑制定位误差。

图1是本实施例的历史运动状态特征提取示意图；如图1所示，包含基站11、真实坐标12、历史定位坐标13、航向向量14、UWB定位坐标15、融合坐标16、IMU解算坐标17、真实运动航向向量18。

如图1所示，本实例采用历史航向信息计算方法将历史定位信息量化，以便描述煤矿巷道下历史物体运动状态并加以充分利用，用于下一时刻多种定位方式权重测算与数据融合；具体包括：

首先，设定航向计算采样间隔参数Δg，取两倍Δg长度定位坐标队列作为历史记忆信息；如图3所示的流程示意图为例，假设取Δg为2则记忆队列中共有A(x_t，y_t)、B(x_t-1，y_t-1)、C(x_t-Δg，y_t-Δg)、D(x_t-Δg-1，y_t-Δg-1)四点定位坐标数据，分别计算向量

和向量

与X轴正方向夹角，并将此夹角定义为目标物体运动航向，航向通过计算向量

辐角主值与π的差值获得目标向量反正切弧值，后将弧值转化为角度形成航向数值，其值域为0°至180°，其中

和向量

与X轴夹角即为历史航向分量，具体利用以下公式计算：

其中

为向量

于定位坐标系下航向值，arg(y/x)为向量的辐角主值。通过上述公式能够将两历史定位点组成的向量在定位坐标系下从弧度度值转换为与X轴正方向组成的夹角，实现历史航线信息量化便于以Δg间隔分别获取记忆队列两点间航向信息后，对其取均值HMA_t为t时刻历史向量航向均值。

运用如下公式将所有航向分量取均值作为最终目标估计运动航向，以避免记忆队列所形成的单个航向向量误差较大航向偏差或定位异常点造成的航向干扰：

通过上式从航向向量转化为与X轴正方向夹角的计算，作为历史航向与下一时刻位置估计评价的量化指标，以此作为度量调整下一时刻各位置估计分量所占融合算法的比重，以达到自适应增加误差较小位置权重提高定位精度的方法。

图2是本实施例融合权重计算示意图，用于构建各定位方式权重系数，其中21为上一时刻真实坐标，22为上一时刻估计位置，23为新时刻UWB定位坐标，24为新时刻融合坐标，25为IMU解算坐标，26为真实航向向量。

当获得历史航向信息后，利用其作为下一时刻采集位置信息的衡量基准，计算该各定位方式融合权重系数，参与卡尔曼滤波数据融合；在取得下一时刻各位置估计信息后，分别计算下一时刻两种定位估计分量权重；首先计算t时刻融合定位数据，如图2所示三角号A(x_t，y_t)与t+1时刻虚线圆L1(x₁，y₁)与虚线圆L2(x₂，y₂)两种估计位置所形成的向量关系

与

并计算两向量航向信息，即上述向量与定位坐标X轴正方向夹角数值；然后分别计算其与历史航向信息HMA_t的差值θ₁与θ₂，作为误差衡量与自适应的初始衡量参数；具体计算公式为：

运用公式

将航向角度非线性化并扩大角度误差数值比重，获得中间权重系数；公式将比值非线性化，用于扩大两种航向角度差值对于权重的影响；最后计算权重数C₁与C₂并获得参与自适应卡尔曼滤波融合的权重系数；其中σ为比重调解参数，为使两种航向角差值相同时卡尔曼误差取得原始设定效果，本实施例将σ取2值，即当T₁与T₂具有相同数值时C₁与C₂值取1，最终融合过程卡尔曼滤波误差系数为初始误差协方差矩阵。

卡尔曼滤波预测方程如公式

所示，

为t+1时刻先验估计位置，X_t为t时刻后验估计位置，其中A为状态转移矩阵，B为控制矩阵，μ_t为控制量，ω为误差参数。

本实施例的UWB融合IMU定位中，估计量

由IMU信息与解算坐标计算，如公式

所示，通过分别计算在定位坐标系XY轴下t时刻各速度分量

与采样时间间隔成绩作为位移增量，与前一时刻坐标相加后得到t+1时刻估计坐标，其各轴速度分量由经坐标转换后加速度分量与历史速度分量计算后获得；

后续卡尔曼滤波公式如下文所示：

UWB三边解算坐标作为观测变量Z_t+1参与后验位置估计计算；权重数C₁与C₂通过参与控制UWB与IMU误差协方差矩阵达到参与卡尔曼滤波进而印象融合权重的功能。

其中X_t+1为t+1时刻后验估计信息，Z_t+1为UWB定位观测位置量；Q为IMU增量解算误差协方差矩阵；R为UWB三边定位误差协方差矩阵；C₁为计算出的惯性导航滤波权重系数；C₁Q为动态调整权重后的增量误差系数；C₂为UWB定位权重系数，与R相乘后以此参与卡尔曼滤波数据融合。

本实例方法的详细过程如图3所示，首先获取新一时刻UWB与INS传感器数据，然后通过三边定位与增量定位算法解算增量坐标；若历史数据满足本专利方法需要则计算历史航向均值，并计算两新解算位置与上一时刻坐标所组成向量信息，并计算权重系数；将权重系数应用于卡尔曼滤波自适应融合后输出最终融合坐标，更新历史定位坐标序列。

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

Claims (6)

1.矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，其特征在于，包括以下步骤：

(1)在巷道内呈矩阵式安装4个定位基站，在巷道内确定一个坐标原点并根据此坐标原点以巷道走向为X轴、以巷道宽度方向为Y轴建立坐标系，然后以此坐标系确定每个定位基站的坐标，在目标物体上安装定位标签；

(2)设定航向计算采样间隔参数Δg，取两倍Δg长度获取定位标签的A(x_t，y_t)、B(x_t-1，y_t-1)、C(x_t-Δg，y_t-Δg)、D(x_t-Δg-1，y_t-Δg-1)四点定位坐标数据的UWB定位坐标与IMU增量解算坐标，以此坐标作为历史队列坐标，并计算向量

和向量

与X轴正方向夹角，将获得的夹角信息作为历史航向分量信息；航向计算公式如下：

其中arg(y/x)为向量的辐角主值，通过此公式能够将两定位点组成的向量在定位坐标系下从弧度度值转换为与X轴正方向组成的夹角，实现历史航线信息量化便于后续权重衡量；y与x分别为定位采样到的定位坐标系下定位目标；

(3)对步骤(2)获取的历史航向分量信息，用以下公式获取均值HMA_t；

式中t为采样次数，i表示当前求和公式求和区间起始次数，本方法以t-Δg为求和过程起点，

为运用航向计算公式对记忆队列定位坐标两两计算航向均值；

(4)以航向计算采样间隔参数Δg对应获取时刻t,获取下一个Δg长度对应的时刻t的历史坐标信息A(x_t，y_t)，以及t+1时刻标签的UWB定位坐标L1(x₁，y₁)、IMU增量解算坐标L2(x₂，y₂)，并形成向量关系

与

计算

与

与X轴正方向夹角数值，并按照以下公式计算上述夹角数值与步骤(3)获得的HMA_t的差值θ₁与θ₂；

式中：

分别为上一时刻融合位置与新一时刻两种测量位置所形成向量计算的航向角度值；

(5)根据步骤(4)获得的θ₁与θ₂，按照以下公式计算权重系数T₁、T₂、C₁、C₂；

式中：σ为比重调解参数，用于成倍扩大或缩小比重；μ为航向控制系数，用于处理航向为零或航向角度极小等过度影响控制权重比例的情况；

(6)利用步骤(5)获得的权重系数C₁、C₂；按照以下公式控制UWB与IMU误差协方差矩阵参与卡尔曼滤波进行印象融合权重，获得融合坐标信息；

式中：X_t+1为t+1时刻后验估计信息，Z_t+1为UWB定位观测位置量；Q为IMU增量解算误差协方差矩阵；R为UWB三边定位误差协方差矩阵；

为t+1时刻的先验估计协方差；A为状态转移矩阵，为n×n阶方阵；P_t为t时刻后验估计协方差；A^T为状态转移矩阵的转置；K_t+1为t+1时刻的卡尔曼滤波增益；H^T为量测矩阵的转置；H为量测矩阵；

为t+1时刻后验估计位置数据；I为单位矩阵；

(7)将步骤(6)获得融合坐标信息更新步骤(2)中的历史队列坐标。将记忆队列更新，将距离此次测量时间最远的融合位置弹出，并将最新融合位置压入队列末端，以此完成历史定位队列数据更新。

2.如权利要求1所述的矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，其特征在于，所述步骤(2)中获得UWB定位坐标时采用TWR测距方法，并采用三边定位获得坐标解算，使用最小二乘算法进行解算优化。如公式:

并求取S(x,y)S为0时x，y最小值作为最终解算定位坐标数据。

3.如权利要求2所述的矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，其特征在于，利用TWR测距方法获得UWB定位坐标时，以c为电磁波传播速度即光速，ct值为测得的各锚节点与标签之间的距离，(x_i,y_i)为第i个锚节点的坐标位置信息；按照以下公式计算出标签坐标信息(x,y)；

4.如权利要求1所述的矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，其特征在于，所述步骤(2)中获得IMU增量解算坐标时，采用以下公式对UWB定位坐标进行转化后获得；

C_t＝C_t-1·(2I_3×3+ω_tΔt)·(2I_3×3-ω_tΔt)^-1

式中：

表示陀螺仪角速率矩阵，C_t-1表示t-1时刻的旋转矩阵，Δt为传感器采样间隔，ω_t表示IMU测量得到的角速度旋转矩阵。I为三阶单位矩阵。

5.如权利要求4所述的矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，其特征在于，所述ω_t具有xyz三轴分量，按照以下公式表达；

其中ω_t(x)、ω_t(y)、ω_t(z)分别为传感器坐标系下x、y、z轴传感器数据分量；

6.如权利要求5所述的矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法，其特征在于，所述ω_t三轴分量上的加速度采用如下公式计算：

式中：

为传感器坐标系下的加速度矩阵，g为重力加速度，计算获得定位坐标系下各轴定位加速度分量a_t；C_t为计算得出的t时刻的旋转矩阵。

Images