面向煤矿复杂环境的UWB与惯导融合的定位抗干扰方法与流程

本发明涉及煤矿井下人员与设备安全防护，尤其涉及面向煤矿复杂环境的uwb与惯导融合的定位抗干扰方法。

背景技术：

1、在煤矿井下掘进工作面作业过程中，由于巷道空间狭窄、机电设备密集、电磁环境复杂，同时伴随高浓度粉尘、强烈机械振动以及掘进机履带频繁打滑的恶劣工况，导致对作业人员与掘进机相对位置的实时精准感知面临严重干扰。当前煤矿智能化建设对人员接近防护与设备协同控制提出了高精度、高可靠的定位需求，尤其在掘进机截割与移动期间，若无法准确获取人员与掘进机之间的动态相对位置关系，极易因视觉盲区或误判距离而引发挤压碰撞安全事故。现有技术中，基于超宽带信号的定位方法因功耗低、测距精度高而被广泛应用于井下人员定位，但超宽带信号在煤矿掘进工作面易受煤尘散射吸收、多径效应以及金属支护结构反射影响，导致信号功率衰减、首径检测偏差和测距跳变；单纯依靠惯性导航进行掘进机位姿推算虽不受粉尘和电磁干扰，但积分误差随时间累积，且在掘进机剧烈振动与打滑状态下航向漂移显著。因此亟需一种面向煤矿复杂环境的uwb与惯导融合的定位抗干扰方法，以提升复杂工况下人员与掘进机实时位置的解算精度和动态安全预警能力。

技术实现思路

1、为了克服现有技术难以在掘进机工作过程中感知周围人员位置的缺点，本发明提供了面向煤矿复杂环境的uwb与惯导融合的定位抗干扰方法。

2、本发明的技术实施方案为：面向煤矿复杂环境的uwb与惯导融合的定位抗干扰方法，包括以下步骤：

3、s1：采集人员的uwb信号数据，并获取掘进机在煤矿掘进工作面作业过程中的掘进机的uwb信号数据、粉尘检测数据、惯性导航数据和旋转编码器数据，同时获取人员与掘进机之间的uwb测距数据；

4、s2：基于所述惯性导航数据构建掘进机的惯性导航可信数据序列；

5、s3：对所述掘进机的uwb信号数据进行处理，结合所述粉尘检测数据构建掘进机的uwb可信数据序列；

6、s4：根据所述惯性导航可信数据序列和所述掘进机的uwb可信数据序列，求解掘进机实时位置和掘进机位置协方差矩阵；

7、s5：根据所述旋转编码器数据和所述掘进机实时位置计算打滑状态因子，将所述掘进机作为移动基站，根据所述打滑状态因子、掘进机实时位置和掘进机位置协方差矩阵计算移动基站可信度；

8、s6：基于所述移动基站可信度、人员的uwb信号数据和uwb测距数据，解算人员实时位置；

9、s7：根据所述人员实时位置和所述掘进机实时位置计算动态安全风险。

10、优选地，所述采集人员的uwb信号数据，并获取掘进机在煤矿掘进工作面作业过程中的掘进机的uwb信号数据、粉尘检测数据、惯性导航数据和旋转编码器数据，同时获取人员与掘进机之间的uwb测距数据，包括：获取uwb基站位置，采集每个uwb基站的人员的uwb信号数据、掘进机的uwb信号数据和掘进机的移动uwb信号数据，通过部署于环境中传感器获取掘进机在煤矿掘进工作面作业过程中的粉尘检测数据、惯性导航数据和旋转编码器数据，根据掘进机的uwb基站获取人员与掘进机之间的uwb测距数据。

11、优选地，所述基于所述惯性导航数据构建掘进机的惯性导航可信数据序列，包括：所述惯性导航数据包括角速度、校正间隔时长和振动强度，获取惯性导航实验标定的旋转累积阈值和惯导累积误差基准时间，根据所述角速度计算角速度模长积分，基于所述振动强度通过高斯滤波获取高频振动强度和振动强度阈值，计算预设时间窗口内惯性导航数据的可信度，将惯性导航数据的可信度与惯性导航数据关联构建惯性导航可信数据序列。

12、优选地，所述计算预设时间窗口内惯性导航数据的可信度，包括：惯性导航可信度计算公式：

13、；

14、式中，为惯性导航可信度，为校正间隔时长，为惯导累积误差基准时间，为角速度模长积分，为旋转累积阈值，为高频振动强度，为振动强度阈值。

15、优选地，所述对所述掘进机的uwb信号数据进行处理，结合所述粉尘检测数据构建掘进机的uwb可信数据序列，包括：所述uwb信号数据包括首径功率和基站总功率，基于粉尘检测数据获取uwb基站的粉尘浓度，根据uwb基站位置构建uwb基站的初始拓扑图，筛选初始拓扑图中首径功率大于预设功率阈值的uwb基站作为参与定位的uwb基站，构建定位拓扑图，分析获取定位拓扑图中连通分量个数，通过粉尘误差实验标定粉尘误差线性因数和粉尘误差二次因数，基于定位拓扑图通过几何矩阵运算获得水平几何精度因子、最佳水平几何精度因子和水平几何精度衰减参考值，计算预设时间窗口内uwb信号数据的可信度，将uwb信号数据的可信度与惯性导航数据关联构建uwb可信数据序列。

16、优选地，所述计算预设时间窗口内uwb信号数据的可信度，包括：

17、uwb可信度计算公式：

18、;

19、式中，为uwb可信度，为参与定位的uwb基站数量，为第个uwb基站的首径功率，为第个uwb基站的总功率，为极小正数，为第个uwb基站的粉尘浓度，为粉尘误差线性因数，为粉尘误差二次因数，为最大允许粉尘误差，为几何拓扑因子；

20、；

21、式中，为几何拓扑因子，为水平几何精度因子，为最佳水平几何精度因子，为水平几何精度衰减参考值，为定位拓扑图中连通分量个数。

22、优选地，所述根据所述惯性导航可信数据序列和所述掘进机的uwb可信数据序列，求解掘进机实时位置和掘进机位置协方差矩阵，包括：基于ds证据理论，根据所述惯性导航可信数据序列和所述掘进机的uwb可信数据序列进行求解，获得掘进机实时位置和掘进机位置协方差矩阵。

23、优选地，所述根据所述旋转编码器数据和所述掘进机实时位置计算打滑状态因子，将所述掘进机作为移动基站，根据所述打滑状态因子、掘进机实时位置和掘进机位置协方差矩阵计算移动基站可信度，包括：获取掘进机的机身长度和行走轮半径，根据所述旋转编码器数据和所述行走轮半径计算获得履带理论位移增量，根据所述掘进机实际位置计算实际位移增量，基于所述履带理论位移增量和实际位移增量计算获得打滑状态因子，基于所述打滑状态因子、掘进机位置协方差矩阵和惯性导航可信度计算移动基站可信度；

24、移动基站可信度公式：

25、；

26、式中，为移动基站可信度，为uwb可信度，为惯性导航可信度，为掘进机位置协方差矩阵，为表示求矩阵的迹，为位置协方差参考值，为打滑状态因子。

27、优选地，所述基于所述移动基站可信度、人员的uwb信号数据和uwb测距数据，解算人员实时位置，包括：根据所述人员的uwb信号数据获取人员与uwb基站的测距值、人员与uwb基站的测距噪声方差和基础测距噪声方差，基于所述uwb测距数据获取人员与移动基站的测距值，以最小化目标函数值为目标求解人员实时位置；

28、；

29、式中，为目标函数值，为参与定位的uwb基站数量，为第个uwb基站的测距噪声方差，为人员实时位置，为掘进机实时位置，为第个uwb基站的位置，为人员与第个uwb基站的测距值，为移动基站可信度，为基础测距噪声方差，为欧几里得距离，为人员与移动基站的测距值。

30、优选地，所述根据所述人员实时位置和所述掘进机实时位置计算动态安全风险，包括：基于所述打滑状态因子、掘进机位置协方差矩阵、人员定位误差的平方和掘进机机身长度计算获得综合定位误差，基于人员实时位置和掘进机实时位置计算人员与掘进机相对距离，基于人员实时位置和掘进机实时位置获取人员轨迹和掘进机轨迹，根据人员轨迹与掘进机轨迹的夹角计算人员意图因子，根据综合定位误差和人员与掘进机相对距离计算动态安全风险；

31、综合定位误差公式：

32、；

33、式中，为综合定位误差，为掘进机位置协方差矩阵，为表示求矩阵的迹，为人员定位误差的平方，为打滑状态因子，为掘进机机身长度；

34、动态安全风险公式：

35、

36、式中，为动态安全风险，为基准距离，为人员与掘进机相对距离，为综合定位误差，为最大定位误差，为人员意图因子。

37、有益效果为：

38、本发明通过引入包含角速度模长积分与校正间隔时长参数的惯性导航可信度计算公式，利用角速度累积阈值和高斯滤波提取的高频振动强度进行联合约束，使得当掘进机处于剧烈截割振动或长时间未进行外部校正时，惯性导航可信度能够自适应衰减，在后续融合过程中削弱由振动引起的角速度噪声和由时间累积引起的航向发散对位置推算的干扰。

39、本发明基于粉尘误差实验标定的线性系数与二次系数，将粉尘浓度作为uwb可信度计算的修正因子，并结合水平几何精度因子及定位拓扑图中的连通分量个数，实现了对uwb非视距传输及节点孤立风险的量化评估，在粉尘浓度陡增或基站布局受遮挡导致几何构型恶化时，uwb可信度能够实时反映当前信号的可用性，降低了因信号穿透粉尘团簇导致的测距正偏误差对掘进机定位精度的污染。

40、本发明将掘进机进一步复用为辅助人员定位的移动基站，针对掘进机履带在湿滑或破碎底板上易发生空转打滑的工况，通过旋转编码器理论位移增量与实际位移增量的差值比对，构建了打滑状态因子，结合掘进机位置协方差矩阵的迹以及前述惯性导航和uwb可信度，定义了移动基站可信度，当掘进机发生打滑导致自身位置估计协方差增大或位移增量失真时，人员定位算法能够自动切断或降低对打滑状态下掘进机这一移动基站的依赖，防止掘进机因打滑产生的瞬时定位偏差映射为人员位置的解算误差，保障了人员轨迹在掘进机动态扰动下的平滑性与连续性。