CN114701544A - 一种煤矿井下单轨吊多源信息融合精确定位方法及系统 - Google Patents

Abstract

本发明适用于井下单轨吊定位技术领域，提供了一种煤矿井下单轨吊多源信息融合精确定位方法及系统，包括以下步骤：根据UWB系统车载标签和基站解算出运动车辆动态位置；根据捷联惯导系统输出的姿态矩阵对比力进行矩阵转换，解算出车辆行驶路程；根据地图构建系统构建的全场景地图，确定特征点的位置信息；根据单轨吊行驶位移模型和特征点精确位置信息，构建精确定位卡尔曼滤波模型，对单轨吊实时位置模型进行修正，得到精确位置信息。本发明通过对三种定位方式进行信息融合，能够实现对单轨吊在井下运行时的精确定位，同时融合三种定位方式的优点，在保证精确度的同时，也能拥有较高的稳定性和抗干扰性能，适用于井下环境中的单轨吊定位。

Description

一种煤矿井下单轨吊多源信息融合精确定位方法及系统

技术领域

本发明涉及井下单轨吊定位技术领域，具体是涉及一种煤矿井下单轨吊多源信息融合精确定位方法及系统。

背景技术

单轨吊是一种井下辅助运输设备，主要用于运输人员及物料，它的牵引力大，且沿固定在煤矿巷道顶部的钢轨运行，运行速度不受巷道地面的恶劣路况影响，因此井下应用广泛。随着国家煤炭开采智能化进程的推进，对单轨吊的精确定位是实现井下辅助运输系统智能调度的重要一环。

限于煤矿井下的特殊环境，GPS以及北斗导航此类定位系统无法在井下使用，目前，国内外针对井下定位技术探索了多种定位方式。一种是基于RFID的人员车辆定位技术，人员车辆携带标识卡与固定的读卡器进行信息交互，即可得到位置信息，但这种定位方式抗干扰能力差，定位精度较低；另一种是基于UWB的定位系统，通过固定基站与移动标签之间的信号传输时间，解算两者距离，但这种方式成本高且数据量大；还有一种基于捷联惯导的定位方法，通过测得的加速度及角速度信息解算出位置信息，但由于累积误差的存在，导致其无法进行长时间的高精度定位。

因此，如何对煤矿井下单轨吊进行精确定位，为实现井下辅助运输系统智能调度提供技术支持，是本领域亟需解决的重大问题。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种煤矿井下单轨吊多源信息融合精确定位方法及系统，以解决上述背景技术中存在的问题。

本发明是这样实现的，一种煤矿井下单轨吊多源信息融合精确定位方法，所述方法包括以下步骤：

根据UWB系统车载标签和固定在井下巷道内的基站，解算出运动车辆动态位置；

根据捷联惯导系统输出的姿态矩阵对比力进行矩阵转换，解算出车辆行驶路程；

根据地图构建系统构建的全场景地图，选取并确定特征点的位置信息；

当UWB系统正常工作时，分析由UWB移动节点坐标向量及UWB移动节点的速度构成的状态参数，根据运动过程中受到的随机扰动，建立适合井下单轨吊定位场景的状态模型和量测模型；

当捷联惯导系统正常工作时，根据初始姿态角建立初始姿态矩阵，构建陀螺仪和加速度计的误差补偿模型，对姿态矩阵进行更新并对速度进行坐标系转换，推导出单轨吊行驶位移模型；

当地图构建系统正常工作时，通过激光雷达和视觉摄像头构建井下巷道全场景地图，选取并确定特征点精确位置信息；

根据所述的单轨吊行驶位移模型以及特征点精确位置信息，构建精确定位卡尔曼滤波模型，对单轨吊实时位置模型进行修正，得到精确位置信息。

作为本发明进一步的方案：当UWB系统正常工作时，分析由UWB移动节点坐标向量及UWB移动节点的速度构成的状态参数，根据运动过程中受到的随机扰动，建立适合井下单轨吊定位场景的状态模型和量测模型的步骤，具体包括：

构建单轨吊UWB定位状态模型：将单轨吊UWB定位系统划分为若干个由4个UWB定位基站和1个UWB移动节点组成的定位网络最小单元，每一个最小单元中，定位基站坐标向量记为x_a∈R²,a＝1,2,3,4，UWB移动节点坐标向量及UWB移动节点的速度作为状态参数，即X_d(k)＝[p_x(k) p_y(k) v_x(k) v_y(k)]^T，运动过程中受到随机扰动为u_d(k)＝[u_x(k) u_y(k)]^T，且u_x～N(0,δ_x ²)，u_y～N(0,δ_y ²)，将单轨吊的运动模型表示为：

其状态方程为X_d(k)＝ΦX_d(k-1)+τu_d(k-1)，其中：状态转移矩阵

噪声驱动矩阵

△T为UWB数据采样间隔，

为采样时刻k处的系统噪声，q为均值为零，方差为

的白噪声，噪声协方差矩阵为Q＝τqτ^T；

构建单轨吊UWB定位量测模型：通过单轨吊UWB定位系统定位所需的测量值为UWB移动节点与每一个UWB参考节点之间的距离信息，则在k时刻，使用r(k)表示移动节点在采样时刻k处的真实位置，用y(k)表示移动节点在采样时刻k处的观测值，则有y(k)＝r(k)+v(k)，其中v(k)～N(0,δ_R)²代表距离测量噪声，k时刻的量测方程为Y(k)＝[d₁ ²(k)…d_n ²(k)]^T+V(k)，其中

即

通过泰勒级数展开将非线性量测方程转化为线性量测方程，即Y(k)＝H(k)X_d(k)+V(k)，其中：

作为本发明进一步的方案：当捷联惯导系统正常工作时，根据初始姿态角建立初始姿态矩阵，构建陀螺仪和加速度计的误差补偿模型，对姿态矩阵进行更新并对速度进行坐标系转换，推导出单轨吊行驶位移模型的步骤，具体包括：

生成姿态阵更新式：

其中

地球自转速率ω_ie＝7.2921151467×10^-5rad/s，惯导位置速率

R_M＝R_e(1-2f_e+3f_e sin²L),

R_N＝R_e(1+f_esin²L),

分别是惯导系统在东向、北向和高度方向速度向量；h为惯导系统所在高度,L为惯导系统所在地球上纬度，R_e为参考地球模型的椭圆长轴半径，f_e为参考地球模型的扁率；

生成速度更新式：

生成位置更新式：

P＝[L λ h]^T

当

νⁿ,gⁿ,M_pV时，

生成圆锥误差补偿式：

其中，陀螺在时间段[t_m-1,t_m]内(T＝t_m-t_m-1)进行了两次等间隔采样，角增量分别为△θ_m1,△θ_m2；

生成旋转误差补偿式：

其中△θ_m为陀螺采样角增量，△ν_m为加速度计采样比力增量；

生成划桨误差补偿式：

作为本发明进一步的方案：当地图构建系统正常工作时，通过激光雷达和视觉摄像头构建井下巷道全场景地图，选取并确定特征点精确位置信息的步骤包括：通过激光雷达和视觉摄像头构建的井下巷道模型为：M＝{P_sta1,P_sta2,P_sta3…P_staN}，其中P_staN为特征点位置信息，P_staN＝[L_staN λ_staN h_staN]^T。

本发明的另一目的在于提供一种煤矿井下单轨吊多源信息融合精确定位系统，所述系统包括：

UWB定位模块，用于根据UWB系统车载标签和固定在井下巷道内的基站，解算出运动车辆动态位置；

捷联惯导定位模块，用于根据捷联惯导系统输出的姿态矩阵对比力进行矩阵转换，解算出车辆行驶路程；

地图构建模块，用于根据地图构建系统构建的全场景地图，选取并确定特征点的位置信息；

UWB定位状态模型和量测模型建立模块，当UWB系统正常工作时，分析由UWB移动节点坐标向量及UWB移动节点的速度构成的状态参数，根据运动过程中受到的随机扰动，建立适合井下单轨吊定位场景的状态模型和量测模型；

捷联惯导定位位移模型建立模块，当捷联惯导系统正常工作时，根据初始姿态角建立初始姿态矩阵，构建陀螺仪和加速度计的误差补偿模型，对姿态矩阵进行更新并对速度进行坐标系转换，推导出单轨吊行驶位移模型；

地图构建系统特征点位置建立模块，当地图构建系统正常工作时，通过激光雷达和视觉摄像头构建井下巷道全场景地图，选取并确定特征点精确位置信息；以及

卡尔曼滤波模型建立模块以及位置修正模块，用于根据所述的单轨吊行驶位移模型以及特征点精确位置信息，构建精确定位卡尔曼滤波模型，对单轨吊实时位置模型进行修正，得到精确位置信息。

与现有技术相比，本发明的有益效果是：

本发明根据UWB定位系统的车载标签和移动节点之间的交互信息以及单轨吊运行的实际约束，建立运动状态模型和量测模型；然后根据捷联惯导系统的陀螺仪和加速度计的误差补偿模型，建立行驶过程中的位移模型；最后再根据地图构建系统所确定的特征点的位置信息，建立精确定位的卡尔曼滤波模型，经过修正后得到单轨吊的精确位置信息。该方法及系统通过对三种定位方式进行信息融合，能够实现对单轨吊在井下运行时的精确定位，同时融合三种定位方式的优点，在保证精确度的同时，也能拥有较高的稳定性和抗干扰性能，非常适用于井下环境中的单轨吊定位。

附图说明

图1为煤矿井下单轨吊多源信息融合精确定位技术的原理框图。

图2为一种煤矿井下单轨吊多源信息融合精确定位方法的系统流程图。

图3为一种煤矿井下单轨吊多源信息融合精确定位方法的流程图。

图4为一种煤矿井下单轨吊多源信息融合精确定位系统的结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清晰，以下结合附图及具体实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

以下结合具体实施例对本发明的具体实现进行详细描述。

需要说明的是，本发明涉及井下单轨吊定位技术领域，由于煤矿井下复杂多变的地质环境以及深处地下的采矿环境，GPS以及北斗等传统定位方式无法使用，但对煤矿井下人员车辆进行精确定位是不可或缺的，目前常用的RFID人员车辆定位虽然技术比较成熟，但是定位范围小且定位精度低，不适用于单轨吊这样运行路程和时间较长的定位设备，本发明实施例旨在解决上述问题。

如图1和图2所示，UWB定位系统通过计算基站与标签之间电磁波信号的飞行时间从而计算二者之间的距离，再通过三边定位原理便可得到移动标签较为精确的位置信息，但是由于井下非视距以及信号失真等影响，UWB定位常常会出现位置丢失的现象；捷联惯导定位是一种不依赖于外接条件的定位方式，其通过陀螺仪以及加速度输出的比力和加速度信息，通过误差补偿以及矩阵变换后能计算出该惯导系统在一段时间内较为精确的位置值，但是由于累积误差的存在，捷联惯导系统常常需要进行位置修正；三维激光雷达以及视距相机能够构建出单轨吊运行巷道的整体模型，通过选取确定的特征点，便可将特征点的位置信息上传至整个定位系统作为位置修正参考点。为此，本发明提供了一种利用UWB定位系统、捷联惯导系统以及地图构建系统进行煤矿井下单轨吊多源信息融合精确定位的方法及系统，该方法及系统通过对三种定位方式进行信息融合，能够实现对单轨吊在井下运行时的精确定位，同时融合三种定位方式的优点，在保证精确度的同时，也能拥有较高的稳定性和抗干扰性能。

如图3所示，本发明实施例提供了一种煤矿井下单轨吊多源信息融合精确定位方法，所述方法包括以下步骤：

S100，根据UWB系统车载标签和固定在井下巷道内的基站，解算出运动车辆动态位置。

该步骤首先通过双边双程测距原理测得车载标签与井下巷道内多个基站的距离，将车载标签与基站都视为固定节点，那么一旦UWB节点工作，每个节点生成一个独立的时间戳，节点A在Ta1发出请求信号帧，节点B在其Tb1接收该信号。经过一个时间Treply1后在Tb2处发送响应信号帧，节点A在Ta2处接收到响应信号。经过一个时间Treply2后在Ta3处发送结束信号帧，节点B在Tb2处接收到该结束信号帧，测距过程结束。最终得到四个时间差数据Treply1、Treply2、Tround1、Tround12，在节点A与节点B之间UWB信号的飞行时间为：

由上式可得到节点A与节点B之间的距离，其中c为光速。再根据三边定位原理，假设巷道内三个固定基站的坐标分别为：BS1(x1，y1)、BS2(x2，y2)、BS3(x3，y3)，三个定位基站与定位标签MS(x，y)的距离分别为R1＝v*t1、R2＝v*t2、R3＝v*t3，则根据圆周公式可建立方程组，即：

就可以计算出运动车辆的实时位置。

S200，根据捷联惯导系统输出的姿态矩阵对比力进行矩阵转换，解算出车辆行驶路程。

该步骤首先进行载体坐标系(b系)与导航坐标系(n系统)之间的转换，n系可经过三次单轴旋转变换到b系，旋转顺序如下所示：

则三次基本旋转所对应的坐标变换矩阵为：

将上述三个转换矩阵相乘，即可得到n系到b系的姿态转换矩阵，如下式所示：

再根据捷联惯导比力方程，进行积分后可得单轨吊位移为：

其中，

为ti时刻至ti+1时刻n系中的速度投影，

为单轨吊的初始位置信息。

S300，根据地图构建系统构建的全场景地图，选取并确定特征点的位置信息。

需要说明的是，在单轨吊进行运输活动前，首先进行多次模型运行，通过安装在单轨吊车身上激光雷达和视觉摄像头构建完整精确的井下巷道模型，通过对比整个井下模型的样貌，选取合适的特征点。

S400，当UWB系统正常工作时，分析由UWB移动节点坐标向量及UWB移动节点的速度构成的状态参数，根据运动过程中受到的随机扰动，建立适合井下单轨吊定位场景的状态模型和量测模型。

S400步骤具体包括：

构建单轨吊UWB定位状态模型：将单轨吊UWB定位系统划分为若干个由4个UWB定位基站和1个UWB移动节点组成的定位网络最小单元，每一个最小单元中，定位基站坐标向量记为x_a∈R²,a＝1,2,3,4，UWB移动节点坐标向量及UWB移动节点的速度作为状态参数，即X_d(k)＝[p_x(k) p_y(k) v_x(k) v_y(k)]^T，运动过程中受到随机扰动为u_d(k)＝[u_x(k) u_y(k)]^T，且u_x～N(0,δ_x ²)，u_y～N(0,δ_y ²)，假设UWB标签固定在单轨吊车身上并未被异物遮盖，单轨吊沿轨道保持匀速行进，单轨吊的运动模型为可表示为：

其状态方程为X_d(k)＝ΦX_d(k-1)+τu_d(k-1)，其中：状态转移矩阵

噪声驱动矩阵

△T为UWB数据采样间隔，

为采样时刻k处的系统噪声，q为均值为零，方差为

的白噪声，噪声协方差矩阵为Q＝τqτ^T；

构建单轨吊UWB定位量测模型：在单轨吊UWB定位系统中，定位所需的测量值为UWB移动节点与每一个UWB参考节点之间的距离信息，则在k时刻，使用r(k)表示移动节点在采样时刻k处的真实位置，用y(k)表示移动节点在采样时刻k处的观测值，则有y(k)＝r(k)+v(k)，其中v(k)～N(0,δ_R)²代表距离测量噪声，δ_R由UWB系统的测量精度决定，则系统k时刻的量测方程可以表示为：Y(k)＝[d₁ ²(k)…d_n ²(k)]^T+V(k)，其中

即：

通过泰勒级数展开可将非线性量测方程转化为线性量测方程，即Y(k)＝H(k)X_d(k)+V(k)，

其中：

S500，当捷联惯导系统正常工作时，根据初始姿态角建立初始姿态矩阵，构建陀螺仪和加速度计的误差补偿模型，对姿态矩阵进行更新并对速度进行坐标系转换，推导出单轨吊行驶位移模型。

S500的步骤具体包括：

生成姿态阵更新式：

其中

地球自转速率ω_ie＝7.2921151467×10^-5rad/s，惯导位置速率

R_M＝R_e(1-2f_e+3f_e sin²L),

R_N＝R_e(1+f_esin²L),

分别是惯导系统在东向、北向和高度方向速度向量；h为惯导系统所在高度,L为惯导系统所在地球上纬度，R_e为参考地球模型的椭圆长轴半径，f_e为参考地球模型的扁率；

生成速度更新式：

生成位置更新式：

P＝[L λ h]^T

当

νⁿ,gⁿ,M_pV时，

生成圆锥误差补偿式：

其中，陀螺在时间段[t_m-1,t_m]内(T＝t_m-t_m-1)进行了两次等间隔采样，角增量分别为△θ_m1,△θ_m2；

生成旋转误差补偿式：

其中△θ_m为陀螺采样角增量，△ν_m为加速度计采样比力增量；

生成划桨误差补偿式：

S600，当地图构建系统正常工作时，通过激光雷达和视觉摄像头构建井下巷道全场景地图，选取并确定特征点精确位置信息。

S600步骤具体包括：通过激光雷达和视觉摄像头构建的井下巷道模型为：M＝{P_sta1,P_sta2,P_sta3…P_staN}，其中P_staN为特征点位置信息，P_staN＝[L_staN λ_staN h_staN]^T。

S700，根据所述的单轨吊行驶位移模型以及特征点精确位置信息，构建精确定位卡尔曼滤波模型，对单轨吊实时位置模型进行修正，得到精确位置信息。

需要说明的是，井下单轨吊位置修正采用Kalman滤波的方式，系统状态量的选取和UWB/INS组合定位相同，滤波观测量为：

式中：Z_p(t)是滤波位置观测量；r_p(t)是特征点位置值。则Kalman的量测方程为：

Z′_k＝Z_p(t)＝H′_kx+ν_p

式中：H′_k位置修正滤波的观测矩阵；ν_p为位置修正滤波的观测噪声，满足高斯白噪声特性。

如图4所示，本发明实施例还提供了一种煤矿井下单轨吊多源信息融合精确定位系统，所述系统包括：

UWB定位模块100，用于根据UWB系统车载标签和固定在井下巷道内的基站，解算出运动车辆动态位置；

捷联惯导定位模块200，用于根据捷联惯导系统输出的姿态矩阵对比力进行矩阵转换，解算出车辆行驶路程；

地图构建模块300，用于根据地图构建系统构建的全场景地图，选取并确定特征点的位置信息；

UWB定位状态模型和量测模型建立模块400，当UWB系统正常工作时，分析由UWB移动节点坐标向量及UWB移动节点的速度构成的状态参数，根据运动过程中受到的随机扰动，建立适合井下单轨吊定位场景的状态模型和量测模型；

捷联惯导定位位移模型建立模块500，当捷联惯导系统正常工作时，根据初始姿态角建立初始姿态矩阵，构建陀螺仪和加速度计的误差补偿模型，对姿态矩阵进行更新并对速度进行坐标系转换，推导出单轨吊行驶位移模型；

地图构建系统特征点位置建立模块600，当地图构建系统正常工作时，通过激光雷达和视觉摄像头构建井下巷道全场景地图，选取并确定特征点精确位置信息；

卡尔曼滤波模型建立模块以及位置修正模块700，用于根据所述的单轨吊行驶位移模型以及特征点精确位置信息，构建精确定位卡尔曼滤波模型，对单轨吊实时位置模型进行修正，得到精确位置信息。

本发明提供的煤矿井下单轨吊多源信息融合精确定位系统，为煤矿井下单轨吊精确定位领域提供了一种实际可行的新的定位方法，该方法具有抗干扰能力强、定位距离远、定位精确高以及可靠性高等显著优势，对煤矿井下单轨吊的定位具有很好的适用性。本发明通过融合UWB定位、捷联惯导定位以及地图构建技术，利用捷联惯导短时间内精确位置数据补偿UWB定位系统可能出现的数据丢失现象，利用地图构建技术生成的特征点的位置信息修正捷联惯导定位中的累积误差，让本定位系统在保证可靠性的同时，极大地提高了定位精度，为后续实现井下辅助运输系统智能调度提供了有效的技术支持。

以上仅对本发明的较佳实施例进行了详细叙述，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

应该理解的是，虽然本发明各实施例的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，各实施例中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

本领域技术人员在考虑说明书及实施例处的公开后，将容易想到本公开的其它实施方案。本申请旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的真正范围和精神由权利要求指出。

Claims (5)

1.一种煤矿井下单轨吊多源信息融合精确定位方法，其特征在于，所述方法包括以下步骤：

根据UWB系统车载标签和固定在井下巷道内的基站，解算出运动车辆动态位置；

根据捷联惯导系统输出的姿态矩阵对比力进行矩阵转换，解算出车辆行驶路程；

根据地图构建系统构建的全场景地图，选取并确定特征点的位置信息；

当UWB系统正常工作时，分析由UWB移动节点坐标向量及UWB移动节点的速度构成的状态参数，根据运动过程中受到的随机扰动，建立适合井下单轨吊定位场景的状态模型和量测模型；

当捷联惯导系统正常工作时，根据初始姿态角建立初始姿态矩阵，构建陀螺仪和加速度计的误差补偿模型，对姿态矩阵进行更新并对速度进行坐标系转换，推导出单轨吊行驶位移模型；

当地图构建系统正常工作时，通过激光雷达和视觉摄像头构建井下巷道全场景地图，选取并确定特征点精确位置信息；

根据所述的单轨吊行驶位移模型以及特征点精确位置信息，构建精确定位卡尔曼滤波模型，对单轨吊实时位置模型进行修正，得到精确位置信息。

2.根据权利要求1所述一种煤矿井下单轨吊多源信息融合精确定位方法，其特征在于，当UWB系统正常工作时，分析由UWB移动节点坐标向量及UWB移动节点的速度构成的状态参数，根据运动过程中受到的随机扰动，建立适合井下单轨吊定位场景的状态模型和量测模型的步骤，具体包括：

构建单轨吊UWB定位状态模型：将单轨吊UWB定位系统划分为若干个由4个UWB定位基站和1个UWB移动节点组成的定位网络最小单元，每一个最小单元中，定位基站坐标向量记为x_a∈R²,a＝1,2,3,4，UWB移动节点坐标向量及UWB移动节点的速度作为状态参数，即X_d(k)＝[p_x(k) p_y(k) v_x(k) v_y(k)]^T，运动过程中受到随机扰动为u_d(k)＝[u_x(k) u_y(k)]^T，且u_x～N(0,δ_x ²)，u_y～N(0,δ_y ²)，将单轨吊的运动模型表示为：

其状态方程为X_d(k)＝ΦX_d(k-1)+τu_d(k-1)，其中：状态转移矩阵

噪声驱动矩阵

△T为UWB数据采样间隔，

为采样时刻k处的系统噪声，q为均值为零，方差为

的白噪声，噪声协方差矩阵为Q＝τqτ^T；

构建单轨吊UWB定位量测模型：通过单轨吊UWB定位系统定位所需的测量值为UWB移动节点与每一个UWB参考节点之间的距离信息，则在k时刻，使用r(k)表示移动节点在采样时刻k处的真实位置，用y(k)表示移动节点在采样时刻k处的观测值，则有y(k)＝r(k)+v(k)，其中v(k)～N(0,δ_R)²代表距离测量噪声，k时刻的量测方程为Y(k)＝[d₁ ²(k)…d_n ²(k)]^T+V(k)，其中

即

通过泰勒级数展开将非线性量测方程转化为线性量测方程，即Y(k)＝H(k)X_d(k)+V(k)，其中：

3.根据权利要求2所述一种煤矿井下单轨吊多源信息融合精确定位方法，其特征在于，当捷联惯导系统正常工作时，根据初始姿态角建立初始姿态矩阵，构建陀螺仪和加速度计的误差补偿模型，对姿态矩阵进行更新并对速度进行坐标系转换，推导出单轨吊行驶位移模型的步骤，具体包括：

生成姿态阵更新式：

其中

地球自转速率ω_ie＝7.2921151467×10^-5rad/s，惯导位置速率

R_M＝R_e(1-2f_e+3f_esin²L),R_N＝R_e(1+f_esin²L),

分别是惯导系统在东向、北向和高度方向速度向量；h为惯导系统所在高度,L为惯导系统所在地球上纬度，R_e为参考地球模型的椭圆长轴半径，f_e为参考地球模型的扁率；

生成速度更新式：

生成位置更新式：

P＝[L λ h]^T

当

时，

生成圆锥误差补偿式：

其中，陀螺在时间段[t_m-1,t_m]内(T＝t_m-t_m-1)进行了两次等间隔采样，角增量分别为△θ_m1,△θ_m2；

生成旋转误差补偿式：

其中△θ_m为陀螺采样角增量，△ν_m为加速度计采样比力增量；

生成划桨误差补偿式：

4.根据权利要求3所述一种煤矿井下单轨吊多源信息融合精确定位方法，其特征在于，当地图构建系统正常工作时，通过激光雷达和视觉摄像头构建井下巷道全场景地图，选取并确定特征点精确位置信息的步骤包括：通过激光雷达和视觉摄像头构建的井下巷道模型为：M＝{P_sta1,P_sta2,P_sta3…P_staN}，其中P_staN为特征点位置信息，P_staN＝[L_staN λ_staN h_staN]^T。

5.一种煤矿井下单轨吊多源信息融合精确定位系统，其特征在于，所述系统包括：

UWB定位模块，用于根据UWB系统车载标签和固定在井下巷道内的基站，解算出运动车辆动态位置；

捷联惯导定位模块，用于根据捷联惯导系统输出的姿态矩阵对比力进行矩阵转换，解算出车辆行驶路程；

地图构建模块，用于根据地图构建系统构建的全场景地图，选取并确定特征点的位置信息；

UWB定位状态模型和量测模型建立模块，当UWB系统正常工作时，分析由UWB移动节点坐标向量及UWB移动节点的速度构成的状态参数，根据运动过程中受到的随机扰动，建立适合井下单轨吊定位场景的状态模型和量测模型；

捷联惯导定位位移模型建立模块，当捷联惯导系统正常工作时，根据初始姿态角建立初始姿态矩阵，构建陀螺仪和加速度计的误差补偿模型，对姿态矩阵进行更新并对速度进行坐标系转换，推导出单轨吊行驶位移模型；

地图构建系统特征点位置建立模块，当地图构建系统正常工作时，通过激光雷达和视觉摄像头构建井下巷道全场景地图，选取并确定特征点精确位置信息；以及

卡尔曼滤波模型建立模块以及位置修正模块，用于根据所述的单轨吊行驶位移模型以及特征点精确位置信息，构建精确定位卡尔曼滤波模型，对单轨吊实时位置模型进行修正，得到精确位置信息。

Images