一种基于惯导的掘锚机多源信息融合定位定向系统

技术领域

本发明涉及矿用掘锚机技术领域，具体来说，涉及一种基于惯导的掘锚机多源信息融合定位定向系统。

背景技术

当前井工煤矿掘进作业中主要依靠激光准直系统推进，传统的激光准直系统依赖专业的测绘团队保障作业，存在定位定向基准建立难度大，并且持续推进指引能力差，巷道掘进准直度依赖驾驶员经验等诸多问题，从而导致了巷道作业人机不协调，推进作业慢，造成井下掘进、支护和采煤作业的严重失调。掘进作业的自动化、智能化是支护自动化的前提，更是制约煤矿产能提升的关键因素。此外，传统的煤矿掘进和支护作业工作面的工人工作量大，工作环境危险恶劣，工作面具有高温、高湿、高粉尘的特点，还面临着瓦斯爆炸等危险，由于掘进工作面自动化水平低，需要较多工人参与其中，致使矿工直接暴露于危险和恶劣的工作环境中。鉴于上述问题，亟需对井工矿掘进工作面进行自动化和智能化升级改造，服务于国家煤炭开采设备产业升级。掘进机作业要求精确的直线度和规则巷道截隔形状，这对安装在其上的定位定向设备提出了很高的定位定向精度要求，为了替代传统的激光准直系统，实现高效的掘进作业。

针对相关技术中的问题，目前尚未提出有效的解决方案。

发明内容

针对相关技术中的问题，本发明提出一种基于惯导的掘锚机多源信息融合定位定向系统，以克服现有相关技术所存在的上述技术问题。

为此，本发明采用的具体技术方案如下：

一种基于惯导的掘锚机多源信息融合定位定向系统，该基于惯导的掘锚机多源信息融合定位定向系统包括：惯性导航模块、惯导零速度修正模块、双里程计模块、毫米波雷达测距定位模块、信息融合模块及上位机数据管理软件；

其中，所述惯性导航模块，用于初始对准后，解算板分别采集光纤陀螺的角增量和加速度计的速度增量进行解算，并实时输出设备的位置、速度和姿态；

所述惯导零速度修正模块，用于判断设备是否处于停止状态，以进行零速滤波，并实时估算惯性器件的常值偏移，当满足滤波收敛后再进行反馈修正惯导；

所述双里程计模块，用于输出高精度的里程增量，采用惯导姿态信息进行航位推算；

所述毫米波雷达测距定位模块，用于利用毫米波雷达测距定位误差不随时间累积的特性修正惯导和里程计组合导航的定位误差，并求解出设备的水平二维位置；

所述信息融合模块，用于融合所述惯性导航模块、所述惯导零速度修正模块、所述双里程计模块及所述毫米波雷达测距定位模块测距定位的结果，确定设备的最优位置估计；

所述上位机数据管理软件，用于对下位机实时传输的位置、速度和姿态信息进行存储、管理和应用处理，并同步获取设备的行走轨迹和直线度；

所述惯性导航模块依次与所述惯导零速度修正模块及所述信息融合模块连接，所述双里程计模块依次与所述惯导零速度修正模块及所述信息融合模块连接，所述惯导零速度修正模块与所述信息融合模块连接，所述毫米波雷达测距定位模块通过所述信息融合模块与所述上位机数据管理软件连接。

进一步的，所述初始对准包括：初始粗对准和初始精对准。

进一步的，所述初始粗对准包括：

构建地球自转矢量方程和重力矢量方程；

构造出垂直于地球自转矢量和重力矢量的垂直矢量；

根据地球自转矢量与重力矢量及垂直矢量构成矩阵的可逆性，得出方向余弦矩阵的粗对准值；

其中，地球自转矢量方程为：

重力矢量方程为：

垂直矢量为：

方向余弦矩阵的粗对准值为：

式中：b为载体系；n为当地水平指北坐标系；

进一步的，所述初始精对准包括：

采用卡尔曼滤波器，在卡尔曼滤波收敛后的结果为粗对准姿态的失准角，将粗对准姿态的失准角补偿到粗对准的基础即可完成精对准；

其中，卡尔曼滤波器中采用的对准随机系统模型为：

X＝[φ

/>

式中：X表示卡尔曼滤波中系统模型的状态向量；

进一步的，所述解算板分别采集光纤陀螺的角增量和加速度计的速度增量进行解算，并实时输出设备的位置、速度和姿态包括：

采用捷联算法，在初始精对准的基础上，采用光纤陀螺输出的角增量构造等效旋转矢量进行姿态更新，完成姿态更新后，采用加速度计的速度增量输出进行更新速度和位置，以完成惯导实时更新。

进一步的，所述判断设备是否处于停止状态，以进行零速滤波，并实时估算惯性器件的常值偏移，当满足滤波收敛后再进行反馈修正惯导包括：

构造三维速度量测信息；

建立惯导的速度误差量测方程；

进行卡尔曼滤波；

修正惯导漂移；

其中，三维速度量测信息为：

速度误差量测方程为：

/>

滤波更新方程为：

式中：k下标的均为当前时刻的量，k-1下标的均为上一个时刻的系统的量，φ

进一步的，所述输出高精度的里程增量，采用惯导姿态信息进行航位推算包括：

采用在惯导状态矢量基础上扩充里程计误差的方式建立局部滤波器的状态方程；

滤波器的观测方程采用惯导姿态阵将里程计速度转换到导航系，构建以速度差为观测量的量测方程；

子滤波器再进行航位推算滤波；

其中，里程计误差包括安装误差、标度因子误差以及杆臂误差。

进一步的，所述利用毫米波雷达测距定位误差不随时间累积的特性修正惯导和里程计组合导航的定位误差，并求解出设备的水平二维位置包括：

对预先布置的控制点进行探测；

通过点云处理算法用以区分不同的控制点的信息；

得到每个控制点的高精度距离R1和R2；

在巷道掘进工作面的水平基线上利用双圆交汇原理进行水平位置求解；

其中，R1和R2的公式为：

式中：R1和R2为掘锚机到每个控制点的高精度距离。

进一步的，所述融合所述惯性导航模块、所述惯导零速度修正模块、所述双里程计模块及所述毫米波雷达测距定位模块测距定位的结果，确定设备的最优位置估计包括：

构造组合导航系统，采用卡尔曼滤波器进行信息融合，得到高精度、高输出率的掘锚机位置、姿态信息；

其中，采用卡尔曼滤波器进行信息融合包括：子滤波和主滤波；

子滤波包括建立各子滤波器状态方程和量测方程的离散差分方程，并进行时间更新与量测更新；

主滤波包括信息分配与全局融合。

进一步的，所述各子滤波器状态方程和量测方程的离散差分方程为：

式中：系统噪声方差强度阵为Q

各局部滤波器根据各自的状态方程进行相应的时间更新方程为：

式中，

当子传感器有量测值时，各局部滤波器根据量测值进行状态的修正，即进行量测更新，方程为：

式中：

信息分配公式为：

式中：P

在各局部滤波器计算出各自的局部估计之后，由主滤波器把各局部滤波器的公共状态

全局融合公式为：

式中：

本发明的有益效果为：

1、本发明基于毫米波雷达、惯导、里程计等多传感器信息融合进行定位导航，定位精度高，可靠性高，并益于惯导系统的自主寻北能力，整体自主能力高，极大的提高了掘锚机的连续作业时长，可有效提升掘进效率，从而改善掘进、支护和综采作业的不协调关系。

2、通过高精度毫米波雷达测距可获得掘锚机与时间累积无关的水平位置信息，修正了惯导和里程计组合导航随时间累计的误差，能实现高输出频率、多掘锚机运动参数的位姿信息，提高了系统定位可靠性。

3、通过设置惯导零速度修正模块，当掘进机速度为零时，零速修正滤波器可实时估计惯性传感器的零偏误差并反馈校正，以提高掘锚机的定位精度。

4、通过设置信息融合模块，使得可以输出高频率并且丰富的掘锚机姿态、速度和位置信息，非常有利于掘锚机后续控制、支护系统的设计和应用。

5、通过设置双里程计模块，使得在高精度的里程计信息进行航位推算导航，在保正掘锚机自主导航能力的前提下，显著提升了纯惯性导航的精度。

6、通过设置上位机数据管理软件，使得可以通过接收下位机实时传输过来的掘锚机姿态、速度和位置数据，实时计算掘锚机的掘进轨迹和巷道的直线度。

附图说明

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

图1是根据本发明实施例的一种基于惯导的掘锚机多源信息融合定位定向系统的原理框图；

图2是根据本发明实施例的一种基于惯导的掘锚机多源信息融合定位定向系统中毫米波雷达水平工作面测距定位示意图；

图3是根据本发明实施例的一种基于惯导的掘锚机多源信息融合定位定向系统中信息融合滤波器的结构图。

图中：

1、惯性导航模块；2、惯导零速度修正模块；3、双里程计模块；4、毫米波雷达测距定位模块；5、信息融合模块；6、上位机数据管理软件。

具体实施方式

为进一步说明各实施例，本发明提供有附图，这些附图为本发明揭露内容的一部分，其主要用以说明实施例，并可配合说明书的相关描述来解释实施例的运作原理，配合参考这些内容，本领域普通技术人员应能理解其他可能的实施方式以及本发明的优点。

根据本发明的实施例，提供了一种基于惯导的掘锚机多源信息融合定位定向系统。

现结合附图和具体实施方式对本发明进一步说明，如图1-3所示，根据本发明实施例的基于惯导的掘锚机多源信息融合定位定向系统，该基于惯导的掘锚机多源信息融合定位定向系统包括：惯性导航模块1、惯导零速度修正模块2、双里程计模块3、毫米波雷达测距定位模块4、信息融合模块5及上位机数据管理软件6；

其中，所述惯性导航模块1，用于初始对准后，解算板分别采集光纤陀螺的角增量和加速度计的速度增量进行解算，并实时输出设备(掘锚机)的位置、速度和姿态；

所述惯导零速度修正模块2，用于判断设备(掘锚机)是否处于停止状态，以进行零速滤波，并实时估算惯性器件的常值偏移，当满足滤波收敛后再进行反馈修正惯导；

具体的，利用掘进机两侧履带从动轮里程计的输出信息可以准确判断掘进机是否处于静止状态，如果是则执行零速修正程序，并且满足收敛时间以后则用于校正惯导系统，否则等待下一个零速修正机会。

所述双里程计模块3，用于输出高精度的里程增量，采用惯导姿态信息进行航位推算；

具体的，采用惯导的姿态信息进行航位推算，由于掘锚机来回走动作业，为了提高掘锚机里程测量精度，两个里程计安装在掘锚机的两侧履带从动轮上，可根据两侧履带从动轮的转动转换成对应里程增量。

所述毫米波雷达测距定位模块4，用于利用毫米波雷达测距定位误差不随时间累积的特性修正惯导和里程计组合导航的定位误差，并求解出设备(掘锚机)的水平二维位置；

具体的，控制点靶标布置在掘锚机后方，可被毫米波雷达探测到，实时的检测掘锚机距离控制点的距离，由于控制点的位置可事先人为的测量，可视为已知，根据毫米波雷达实时探测的高精度距离，利用两个毫米波雷达可以求解出掘锚机的水平二维位置。

所述信息融合模块5，用于融合所述惯性导航模块1、所述惯导零速度修正模块2、所述双里程计模块3及所述毫米波雷达测距定位模块4测距定位的结果，确定设备(掘锚机)的最优位置估计；

具体的，信息融合模块主要由电路接口、FPGA和DSP处理器构成。其主要作用是融合惯性导航、零速修正模块、里程计航位推算和毫米波雷达测距定位的结果，给出掘锚机可靠的高精度的最优位置估计。由于惯导和里程计组合导航属于航位推算导航，导航误差会不断累计，导致掘锚机定位误差过大而引起掘进估计非直线误差加大和巷道截割不规则。

所述上位机数据管理软件6，用于对下位机实时传输的位置、速度和姿态信息进行存储、管理和应用处理，并同步获取设备(掘锚机)的行走轨迹和直线度；

具体的，通过接收下位机实时传输过来的掘锚机姿态、速度和位置数据，实时计算掘锚机的掘进轨迹和巷道的直线度。

所述惯性导航模块1依次与所述惯导零速度修正模块2及所述信息融合模块5连接，所述双里程计模块3依次与所述惯导零速度修正模块2及所述信息融合模块5连接，所述惯导零速度修正模块2与所述信息融合模块5连接，所述毫米波雷达测距定位模块4通过所述信息融合模块5与所述上位机数据管理软件6连接。

在一个实施例中，所述初始对准包括：初始粗对准和初始精对准。

在一个实施例中，所述初始粗对准包括：

构建地球自转矢量方程和重力矢量方程；

构造出垂直于地球自转矢量和重力矢量的垂直矢量；

根据地球自转矢量与重力矢量及垂直矢量构成矩阵的可逆性，得出方向余弦矩阵的粗对准值；

其中，地球自转矢量方程为：

重力矢量方程为：

垂直矢量为：

方向余弦矩阵的粗对准值为：

式中：b为载体系；n为当地水平指北坐标系；

在一个实施例中，所述初始精对准包括：

采用卡尔曼滤波器，在卡尔曼滤波收敛后的结果为粗对准姿态的失准角，将粗对准姿态的失准角补偿到粗对准的基础即可完成精对准；

其中，卡尔曼滤波器中采用的对准随机系统模型为：

X＝[φ

式中：X表示卡尔曼滤波中系统模型的状态向量；

在一个实施例中，所述解算板分别采集光纤陀螺的角增量和加速度计的速度增量进行解算，并实时输出设备(掘锚机)的位置、速度和姿态包括：

采用捷联算法，在初始精对准的基础上，采用光纤陀螺输出的角增量构造等效旋转矢量进行姿态更新，完成姿态更新后，采用加速度计的速度增量输出进行更新速度和位置，以完成惯导实时更新。

在一个实施例中，所述判断设备(掘锚机)是否处于停止状态，以进行零速滤波，并实时估算惯性器件的常值偏移，当满足滤波收敛后再进行反馈修正惯导包括：

构造三维速度量测信息；

建立惯导的速度误差量测方程；

进行卡尔曼滤波；

修正惯导漂移；

其中，三维速度量测信息为：

速度误差量测方程为：

滤波更新方程为：

式中：k下标的均为当前时刻的量，k-1下标的均为上一个时刻的系统的量，φ

具体的，掘锚机在井下进行巷道作业时，处于走走停停的运动状态，考虑到其多为履带式，可认为掘锚机作业时不发生打滑和侧滑现象，是理想的零速状态，此时，可知采煤机东北天三个方向的实时速度均为0，从而可以构造三维速度量测信息，建立惯导的速度误差量测方程，进行卡尔曼滤波，修正惯导的漂移，提高其姿态和导航精度。

在一个实施例中，所述输出高精度的里程增量，采用惯导姿态信息进行航位推算包括：

采用在惯导状态矢量基础上扩充里程计误差的方式建立局部滤波器的状态方程；

滤波器的观测方程采用惯导姿态阵将里程计速度转换到导航系，构建以速度差为观测量的量测方程；

子滤波器再进行航位推算滤波；

其中，里程计误差包括安装误差、标度因子误差以及杆臂误差。

在一个实施例中，所述利用毫米波雷达测距定位误差不随时间累积的特性修正惯导和里程计组合导航的定位误差，并求解出设备(掘锚机)的水平二维位置包括：

对预先布置的控制点进行探测；

通过点云处理算法用以区分不同的控制点的信息；

得到每个控制点的高精度距离R1和R2；

在巷道掘进工作面的水平基线上利用双圆交汇原理进行水平位置求解；

其中，R1和R2的公式为：

式中：R1和R2为掘锚机到每个控制点的高精度距离。

具体的，如图2所示，在掘进机掘进的后方区域巷道内，通过人为布置2个控制点靶标，用于完成毫米波雷达对掘锚机的测距定位。控制点的位置是精确已知的，安装在掘锚机的毫米波雷达对预先布置的控制点进行探测，通过点云处理算法用以区分不同的控制点的信息，并且得到每个控制点的高精度距离R1，R2，从而在巷道掘进工作面的水平基线上利用双圆交汇原理进行水平位置求解。

在一个实施例中，所述融合所述惯性导航模块1、所述惯导零速度修正模块2、所述双里程计模块3及所述毫米波雷达测距定位模块4测距定位的结果，确定设备(掘锚机)的最优位置估计包括：

构造组合导航系统，采用卡尔曼滤波器进行信息融合，得到高精度、高输出率的掘锚机位置、姿态信息；

其中，采用卡尔曼滤波器进行信息融合包括：子滤波和主滤波；

子滤波包括建立各子滤波器状态方程和量测方程的离散差分方程，并进行时间更新与量测更新；

主滤波包括信息分配与全局融合。

具体的，信息融合算法是根据毫米波雷达测距定位方法获取掘锚机的位置信息，定位的精度取决于毫米波雷达的测距精度和水平控制点之间的距离，在保正毫米波雷达可以探测到控制点的前提下，控制点间的测量基线越长，定位精度越高。基于毫米波雷达测距定位的方法具有测距精度高、定位误差不累积(和时间无关)的优点，但是，这种方法定位信息输出频率较低，先比较而言，惯导里程计定位信息更新频率低，但是误差随时间累积发散，二者存在天然的互补关系，因此，可构造组合导航系统，采用卡尔曼滤波器进行信息融合，得到高精度，高输出率的掘锚机位置、姿态信息。

在一个实施例中，所述各子滤波器状态方程和量测方程的离散差分方程为：

式中：系统噪声方差强度阵为Q

各局部滤波器根据各自的状态方程进行相应的时间更新方程为：

式中，

当子传感器有量测值时，各局部滤波器根据量测值进行状态的修正，即进行量测更新，方程为：

式中：

信息分配公式为：

式中：P

在各局部滤波器计算出各自的局部估计之后，由主滤波器把各局部滤波器的公共状态

全局融合公式为：

式中：

具体的，如图3所示，毫米波雷达测距定位方法可以获取掘锚机的位置信息，定位的精度取决于毫米波雷达的测距精度和水平控制点之间的距离，在保正毫米波雷达可以探测到控制点的前提下，控制点间的测量基线越长，定位精度越高。基于毫米波雷达测距定位的方法具有测距精度高、定位误差不累积(和时间无关)的优点，但是，这种方法定位信息输出频率较低。先比较而言，惯导里程计定位信息更新频率低，但是误差随时间累积发散，二者存在天然的互补关系，因此，可构造组合导航系统，采用卡尔曼滤波器进行信息融合，得到高精度，高输出率的掘锚机位置、姿态信息。

为了方便理解本发明的上述技术方案，以下就本发明在实际过程中的工作原理或者操作方式进行详细说明。

综上所述，借助于本发明的上述技术方案，本发明基于毫米波雷达、惯导、里程计等多传感器信息融合进行定位导航，定位精度高，可靠性高，并益于惯导系统的自主寻北能力，整体自主能力高，极大的提高了掘锚机的连续作业时长，可有效提升掘进效率，从而改善掘进、支护和综采作业的不协调关系。通过高精度毫米波雷达测距可获得掘锚机与时间累积无关的水平位置信息，修正了惯导和里程计组合导航随时间累计的误差，能实现高输出频率、多掘锚机运动参数的位姿信息，提高了系统定位可靠性。通过设置零速修正模块，当掘进机速度为零时，零速修正滤波器可实时估计惯性传感器的零偏误差并反馈校正，以提高掘锚机的定位精度。通过设置信息融合模块，使得可以输出高频率并且丰富的掘锚机姿态、速度和位置信息，非常有利于掘锚机后续控制、支护系统的设计和应用。通过设置双里程计模块，使得在高精度的里程计信息进行航位推算导航，在保正掘锚机自主导航能力的前提下，显著提升了纯惯性导航的精度。通过设置上位机数据管理软件，使得可以通过接收下位机实时传输过来的掘锚机姿态、速度和位置数据，实时计算掘锚机的掘进轨迹和巷道的直线度。

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