一种基于捷联惯导的掘进机组合定位系统及其定位方法

1.本发明涉及基于捷联惯导的掘进机组合定位系统及其定位方法领域，具体涉及一种基于捷联惯导的掘进机组合定位系统及其定位方法。


背景技术：

2.在井下煤炭采掘过程中，掘进机、采煤机以及液压支架是综合采掘的主要承担者，而这其中，掘进机承担着开凿巷道的重要作用。由于井下工况复杂，由地面崎岖、煤岩硬度等问题所造成的掘进机掘进方向性差的难题一直限制着掘进机工作效率。市面上诸多定位系统都对掘进机的定位有着独特的作用，但都有一些缺陷：gps系统虽然是最广泛使用的定位系统，但是由于井下的特殊情况根本无法使用；捷联惯导系统在掘进机、采煤机中都有广泛应用，但是本身精度却难以提升；uwb定位系统虽然定位精度更高，但是适用于环境友好的室内，在井下也有很多限制。因此，一种基于捷联惯导的掘进机组合定位系统及其定位方法，尝试将多种定位方法组合使用，并给出使用方法，提高了掘进机的定位进度，为智慧矿山的建立提供技术支持。


技术实现要素：

3.针对上述存在的技术不足，本发明的目的是提供一种基于捷联惯导的掘进机组合定位系统及其定位方法。
4.为解决上述技术问题，本发明采用如下技术方案：
5.本发明提供一种基于捷联惯导的掘进机组合定位系统，包括uwb定位系统、捷联惯导系统、编码器、激光定位系统、信号处理箱，所述激光定位系统用于检测掘进机是否处于正确的行进方向，所述uwb定位系统、捷联惯导系统、编码器用于检测掘进机机身的位置信号和运动信号，所述信号处理箱用于处理uwb定位系统、捷联惯导系统、编码器、激光定位系统传回的掘进机机身位置信息信号，并根据给定的掘进机组合定位位置算法得出掘进机实时位置信息。
6.优选地，所述的uwb定位系统包括布置在巷道内的矿用本安型的定位基站一、定位基站二、定位基站三、定位终端，所述定位基站一定位基站二、定位基站三分布于巷道的一侧，所述定位终端置于掘进机尾座上。
7.优选地，所述定位基站一、定位基站二、定位基站三之间，相邻的两个相隔两米进行布置。
8.优选地，所述捷联惯导系统包括集成的三轴光纤陀螺惯性导航单元，所述三轴光纤陀螺惯性导航单元置于掘进机座位底部。
9.优选地，所述激光定位系统包括激光器、激光传感器，所述激光传感器置于掘进机机身尾部，所述激光器置于掘进机机身后侧面与激光传感器对应。
10.优选地，所述信号处理箱置于掘进机控制台底部，所述的信号处理箱内置微处理器用以处理uwb定位系统、捷联惯导系统编码器以及激光定位系统传回的掘进机机身位置
信息。
11.本发明还提供一种使用上述装置的定位方法，具体包括以下步骤：
12.(a)启动掘进机；
13.(b)启动激光定位系统；
14.(c)判断掘进机是否处于正确的行进方向，若为是，则执行步骤(e)，若为否，则执行步骤(d)；
15.(d)进行掘进机主动位姿调整，调整后继续执行步骤(c)；
16.(e)启动掘进机组合定位系统；
17.(f)捷联惯导系统检测机身位置信号wjc，uwb系统检测机身位置信号wuc，编码器(2)检测机身位置信号wbc；
18.(g)位置信息wjc、wuc、wbc以50％、50％、50％的权重比输出，得出掘进机初始位置信息；
19.(h)掘进机工作；
20.(i)捷联惯导系统检测机身运动信号wjd，uwb系统检测机身运动信号wud，编码器(2)检测机身运动信号wbd；
21.(j)判断由uwb系统(5)测得的机身运动信号wud是否小于等于编码器(2)测得的机身运动信号wbd，若为是，则位置信息wjd、wud、wbd以50％、50％、50％的权重比输出，得到掘进机融合位置信息，若为否，则位置信息wjd、wud、wbd以75％、25％、25％的权重比输出，得到掘进机融合位置信息；
22.(k)得到的掘进机的融合位置信息进过卡尔曼滤波得到最终的掘进机实时位置信息。
23.有益效果
24.1、本发明将掘进机在掘进过程中由于路况或者工作面煤岩硬度不均等造成的行进方向突变的问题单独由激光定位方法进行实时测量，不再加入到定位算法中，缓解了算法压力；
25.2、本发明将捷联惯导定位、uwb定位以及编码器等定位方法进行融合，形成了一种组合定位方法，有效弥补了这些定位方法单独使用时的一些自身缺陷，提高了掘进机的定位精度。
附图说明
26.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
27.图1为本发明实例中基于捷联惯导掘进机组合定位系统安装位置示意图；
28.图2为本发明实例中种基于捷联惯导的掘进机组合定位系统及其定位方法工作流程图；
29.图3为本发明实例中种基于捷联惯导的掘进机组合定位系统及其定位方法的组合定位位置算法模型。
30.图中：1-1、掘进机截割头，1-2、掘进机截割臂，1-3、铲板部，1-4、掘进机回转台，1-5、掘进机控制台，1-6、控制台座位，1-7、掘进机行走部，1-8、掘进机运输部，2、编码器，3、信号处理箱，4、三轴光纤陀螺惯性导航单元，5、uwb定位系统，5-1、定位基站一，5-2、定位基站二、5-3、矿用本安型定位终端，5-4、定位基站三，6-1、激光传感器，6-2、激光器。
具体实施方式
31.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
32.实施例1：如图1-2所示，本实施例提供一种基于捷联惯导的掘进机组合定位系统，包括uwb定位系统、捷联惯导系统4、编码器2、激光定位系统、信号处理箱3，所述激光定位系统用于检测掘进机是否处于正确的行进方向，所述uwb定位系统、捷联惯导系统、编码器2用于检测掘进机机身的位置信号和运动信号，所述信号处理箱3用于处理uwb定位系统、捷联惯导系统、编码器2、激光定位系统传回的掘进机机身位置信息信号，并根据给定的掘进机组合定位位置算法得出掘进机实时位置信息。
33.本实例应用于掘进机上，所述掘进机包括掘进机截割头1-1、掘进机截割臂1-2、铲板部1-3、掘进机回转台1-4、掘进机控制台1-5、控制台座位1-6、掘进机行走部1-7、掘进机运输部1-8，所述掘进机为本领域技术人员所熟知的现有掘进机，上述各部件为本领域技术人员所熟知的现有部件，各部件之间的联系采用本领域技术人员所熟知的现有连接方式连接，在此不再具体描述。
34.所述的uwb定位系统包括布置在巷道内的矿用本安型的定位基站一5-1、定位基站二5-2、定位基站三5-4、定位终端5-3，所述定位基站一5-1定位基站二5-2、定位基站三5-4分布于巷道的一侧，所述定位终端5-3置于掘进机尾座上。所述定位基站一5-1定位基站二5-2、定位基站三5-4之间，相邻的两个相隔两米进行布置。所述捷联惯导系统包括集成的三轴光纤陀螺惯性导航单元4，所述三轴光纤陀螺惯性导航单元4置于掘进机座位1-6底部。所述激光定位系统包括激光器6-2、激光传感器6-1，所述激光传感器6-1置于掘进机机身尾部，所述激光器6-2置于掘进机机身后面的巷道内与激光传感器6-1对应。所述信号处理箱3置于掘进机控制台1-5底部，所述的信号处理箱3内置微处理器用以处理uwb定位系统5、捷联惯导系统、编码器2以及激光定位系统传回的掘进机机身位置信息。
35.本实例的各定位基站、定位终端、编码器2、三轴光纤陀螺惯性导航单元4、激光器6-2、激光传感器6-1、信号处理箱3选用本领域技术人员所熟知的现有产品或结构，其与掘进机之间采用本领域技术人员所熟知的连接关系，不再做具体描述。
36.三轴光纤陀螺惯性导航单元4包括三轴加速度计和三轴陀螺仪，用于输出三轴加速度信号(单位g,即重力加速度)和三轴角速度信号(单位rad/s)，根据捷联惯导更新算法解得车身姿态和位置信息；
37.uwb节点分为信标节点(巷道周围)和未知节点(车身)，实际为同一种传感元件，根据不同功能划分为两种，用于接收/发送超宽带无线信号，根据tw-tof法测量节点距离，然后测得车身移动距离；
38.编码器为电机内部自带，根据其脉冲频率和周期计算出转速，从而获得车身运动速度；
39.当激光传感器接受到激光信号时，向控制系统发送逻辑信号“是(1)”，从而控制系统得以继续运行，当激光传感器无法接受到激光信号时，向控制系统发送逻辑信号“否(0)”，判定此时掘进机姿态改变，调整姿态后继续进行后续动作。
40.本实施例还提供一种使用上述装置的定位方法，具体包括以下步骤：
41.(a)启动掘进机；
42.(b)启动激光定位系统；
43.(c)判断掘进机是否处于正确的行进方向，当激光传感器接受到激光信号时，向控制系统发送逻辑信号“是(1)”，从而控制系统得以继续运行，当激光传感器无法接受到激光信号时，向控制系统发送逻辑信号“否(0)”，判定此时掘进机姿态改变，调整姿态后继续进行后续动作，若为是，则执行步骤(e)，若为否，则执行步骤(d)；
44.(d)进行掘进机主动位姿调整，根据激光传感器的检测结果进行主动调整，调整动作需要人工操作，首先通过人眼观察激光发射器的激光在接收器的准确方位，若激光打在接收器的左侧，则将掘进机向左侧调整，反之则向右侧调整；若激光打在接收器上方，则将发射器向下调整，反之则向上方调整；调整后继续执行步骤(c)；
45.(e)启动本实施例的基于捷联惯导的掘进机组合定位系统；
46.(f)捷联惯导系统检测机身位置信号wjc，uwb系统检测机身位置信号wuc，编码器(2)检测机身位置信号wbc；
47.捷联惯导系统是根据本领域技术人员所熟知的方向余弦矩阵法来解算位置信息，对角速度、加速度计输出的角增量和比力进行误差补偿，得到掘进机姿态、速度更新式，与初始位置、速度、姿态进行矩阵乘法运算可得到更新后姿态、速度、位置，输出位置信号wjc＝[横滚角υ俯仰角θ航向角ψ速度vjcx速度vjcy速度vjcz位置pjcx位置pjcy位置pjcz]；
[0048]
uwb定位系统首先根据本领域技术人员所熟知的双边双程测距原理计算出信标节点与未知节点间的距离，然后根据三边定位原理解算出具体的车身位置信息，输出位置信号wuc＝[位置pucx位置pucy位置pucz]；
[0049]
编码器通过本领域技术人员所熟知的m/t法进行测速，通过检测编码器的脉冲个数和频率计算出转速，根据电机半径求出实际运动速度，输出位置信号wbc＝[速度vbcx速度vbcy速度vbcz]；
[0050]
(g)位置信息wjc、wuc、wbc以50％、50％、50％的权重比输出为初始姿态角ac、初始速度vc以及初始位置pc,得出掘进机初始位置信息，其中ac＝[横滚角υ俯仰角θ航向角ψ],pc＝50％*[pjcx pucx pjcy pucy pjcz pucz]；vc＝50％*[vjcx vbcx vjcy vbcy vjcz vbcz]；
[0051]
(h)掘进机工作；
[0052]
(i)掘进机正常运行时，捷联惯导系统检测机身运动信号wjd，uwb系统检测机身运动信号wud，编码器2检测机身运动信号wbd；检测方法和步骤(f)相同，得到由捷联惯导系统输出的位置信号wjd＝[υθψvjdx vjdy vjdz pjdx pjdy pjdz]，由uwb系统输出的位置信息wud＝[pudx pudy pudz]，由编码器输出位置信号wbd＝[vbdx vbdy vbdz]；
[0053]
(j)判断由uwb定位系统5测得的机身运动信号wud是否小于等于编码器2测得的机
身运动信号wbd，若为是，则位置信息wjd、wud、wbd以50％、50％、50％的权重比输出为更新姿态角ad、更新速度vd以及更新位置pd,得出掘进机更新位置信息，其中ad＝[υθψ],vd＝50％*[vjdx vbdx vjdy vbdy vjdz vbdz],pd＝50％*[pjdx pudx pjdy pudy pjdz pudz]；若为否，则位置信息wjd、wud、wbd以75％、25％、25％的权重比输出为更新姿态角ad、更新速度vd以及更新位置pd,得出掘进机更新位置信息，其中ad＝[υθψ],vd＝[75％*vjdx 25％*vbdx 75％*vjdy 25％*vbdy 75％*vjdz 25％*vbdz],pd＝[75％*pjdx 25％*pudx 75％*pjdy 25％*pudy 75％*pjdz 25％*pudz]；
[0054]
如附图3所示，通过捷联惯导系统解算位置信息，对角速度、加速度计输出的角增量和比力采用本领域技术人员所熟知的现有方法进行误差补偿，得到掘进机姿态、速度更新式，其中初始姿态与姿态更新式通过本领域技术人员所熟知的矩阵运算得到掘进机姿态；捷联惯导系统输出的速度更新式与编码器输出的速度更新式通过融合权重比进行加法运算得到掘进机融合速度；融合速度按照本领域技术人员所熟知的现有方法进行积分运算得到的位置信息与uwb定位系统得到的位置通过融合权重比进行加法运算得到掘进机融合位置；
[0055]
(k)融合的位置信息经过卡尔曼滤波得到最终的掘进机实时位置信息。
[0056]
本实施例的卡尔曼滤波方法为本领域技术人员所熟知的现有方法，在此不再作详细描述。
[0057]
本实施例利用激光定位系统确定掘进机行进方向，利用捷联惯导、uwb定位以及编码器等多元组合的定位方法，采用组合定位位置算法得出掘进机行进中的位置信息，有效提高了掘进机定位精度和智能化程度。
[0058]
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。