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

本发明专利技术公开了一种基于惯导的掘锚机多源信息融合定位定向系统，该系统包括惯性导航模块、惯导零速度修正模块、双里程计模块、毫米波雷达测距定位模块、信息融合模块及上位机数据管理软件解算板分别采集光纤陀螺的角增量和加速度计的速度增量进行解算，并实时输出设备的位置、速度和姿态；惯导零速度修正模块，用于判断设备是否处于停止状态，以进行零速滤波，并实时估算惯性器件的常值偏移。本发明专利技术基于毫米波雷达、惯导、里程计等多传感器信息融合进行定位导航，定位精度高，可靠性高，并益于惯导系统的自主寻北能力，整体自主能力高，极大的提高了掘锚机的连续作业时长，可有效提升掘进效率，从而改善掘进、支护和综采作业的不协调关系。关系。关系。

【技术实现步骤摘要】
一种基于惯导的掘锚机多源信息融合定位定向系统


[0001]本专利技术涉及矿用掘锚机
，具体来说，涉及一种基于惯导的掘锚机多源信息融合定位定向系统。

技术介绍

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

技术实现思路

[0004]针对相关技术中的问题，本专利技术提出一种基于惯导的掘锚机多源信息融合定位定向系统，以克服现有相关技术所存在的上述技术问题。
[0005]为此，本专利技术采用的具体技术方案如下：
[0006]一种基于惯导的掘锚机多源信息融合定位定向系统，该基于惯导的掘锚机多源信息融合定位定向系统包括：惯性导航模块、惯导零速度修正模块、双里程计模块、毫米波雷达测距定位模块、信息融合模块及上位机数据管理软件；
[0007]其中，所述惯性导航模块，用于初始对准后，解算板分别采集光纤陀螺的角增量和加速度计的速度增量进行解算，并实时输出设备的位置、速度和姿态；
[0008]所述惯导零速度修正模块，用于判断设备是否处于停止状态，以进行零速滤波，并实时估算惯性器件的常值偏移，当满足滤波收敛后再进行反馈修正惯导；
[0009]所述双里程计模块，用于输出高精度的里程增量，采用惯导姿态信息进行航位推算；
[0010]所述毫米波雷达测距定位模块，用于利用毫米波雷达测距定位误差不随时间累积的特性修正惯导和里程计组合导航的定位误差，并求解出设备的水平二维位置；
[0011]所述信息融合模块，用于融合所述惯性导航模块、所述惯导零速度修正模块、所述双里程计模块及所述毫米波雷达测距定位模块测距定位的结果，确定设备的最优位置估计；
[0012]所述上位机数据管理软件，用于对下位机实时传输的位置、速度和姿态信息进行存储、管理和应用处理，并同步获取设备的行走轨迹和直线度；
[0013]所述惯性导航模块依次与所述惯导零速度修正模块及所述信息融合模块连接，所述双里程计模块依次与所述惯导零速度修正模块及所述信息融合模块连接，所述惯导零速度修正模块与所述信息融合模块连接，所述毫米波雷达测距定位模块通过所述信息融合模块与所述上位机数据管理软件连接。
[0014]进一步的，所述初始对准包括：初始粗对准和初始精对准。
[0015]进一步的，所述初始粗对准包括：
[0016]构建地球自转矢量方程和重力矢量方程；
[0017]构造出垂直于地球自转矢量和重力矢量的垂直矢量；
[0018]根据地球自转矢量与重力矢量及垂直矢量构成矩阵的可逆性，得出方向余弦矩阵的粗对准值；
[0019]其中，地球自转矢量方程为：
[0020][0021]重力矢量方程为：
[0022][0023]垂直矢量为：
[0024][0025]方向余弦矩阵的粗对准值为：
[0026][0027]式中：b为载体系；n为当地水平指北坐标系；为方向余弦矩阵；w
ie
为地球自转角速度矢量；g为重力加速度矢量。
[0028]进一步的，所述初始精对准包括：
[0029]采用卡尔曼滤波器，在卡尔曼滤波收敛后的结果为粗对准姿态的失准角，将粗对准姿态的失准角补偿到粗对准的基础即可完成精对准；
[0030]其中，卡尔曼滤波器中采用的对准随机系统模型为：
[0031][0032]X＝[φ
E
φ
N
φ
U
δv
E
δv
N
ε
N
ε
U
]T
[0033][0034][0035][0036][0037][0038]式中：X表示卡尔曼滤波中系统模型的状态向量；表示系统模型状态向量的导数；F矩阵表示系统的状态转移矩阵；G表示系统噪声分配矩阵；W表示系统噪声向量；H表示量测矩阵；V表示量测噪声。
[0039]进一步的，所述解算板分别采集光纤陀螺的角增量和加速度计的速度增量进行解算，并实时输出设备的位置、速度和姿态包括：
[0040]采用捷联算法，在初始精对准的基础上，采用光纤陀螺输出的角增量构造等效旋转矢量进行姿态更新，完成姿态更新后，采用加速度计的速度增量输出进行更新速度和位置，以完成惯导实时更新。
[0041]进一步的，所述判断设备是否处于停止状态，以进行零速滤波，并实时估算惯性器件的常值偏移，当满足滤波收敛后再进行反馈修正惯导包括：
[0042]构造三维速度量测信息；
[0043]建立惯导的速度误差量测方程；
[0044]进行卡尔曼滤波；
[0045]修正惯导漂移；
[0046]其中，三维速度量测信息为：
[0047][0048]速度误差量测方程为：
[0049][0050]滤波更新方程为：
[0051][0052]式中：k下标的均为当前时刻的量，k
‑
1下标的均为上一个时刻的系统的量，φ
k,k
‑1为k
‑
1到k时刻的状态转移矩阵，W和V分别为系统过程和量测噪声；数学平台失准角为φ
x
、φ
y
、φ
z
，东北天向速度误差为δV
E
、δV
N
、δV
U
，位置误差为δλ、δL、δh，速度计随机常值误差为陀螺仪的随机常值误差为εx、εy、εz，K
k
表示滤波增益；Z
k
表示观测系向量；H
k
表示观测矩阵；P
k
表示当前估计的均方误差阵。
[0053]进一步的，所述输出高精度的里程增量，采用惯导姿态信息进行航位推算包括：
[0054]采用在惯导状态矢量基础上扩充里程计误差的方式建立局部滤波器的状态方程；
[0055]滤波器的观测方程采用惯导姿态阵将里程计速度转换到导航系，构建以速度差为观测量的量测方程；
[0056]子滤波器再进行航位推算滤波；本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于惯导的掘锚机多源信息融合定位定向系统，其特征在于，该基于惯导的掘锚机多源信息融合定位定向系统包括：惯性导航模块、惯导零速度修正模块、双里程计模块、毫米波雷达测距定位模块、信息融合模块及上位机数据管理软件；其中，所述惯性导航模块，用于初始对准后，解算板分别采集光纤陀螺的角增量和加速度计的速度增量进行解算，并实时输出设备的位置、速度和姿态；所述惯导零速度修正模块，用于判断设备是否处于停止状态，以进行零速滤波，并实时估算惯性器件的常值偏移，当满足滤波收敛后再进行反馈修正惯导；所述双里程计模块，用于输出高精度的里程增量，采用惯导姿态信息进行航位推算；所述毫米波雷达测距定位模块，用于利用毫米波雷达测距定位误差不随时间累积的特性修正惯导和里程计组合导航的定位误差，并求解出设备的水平二维位置；所述信息融合模块，用于融合所述惯性导航模块、所述惯导零速度修正模块、所述双里程计模块及所述毫米波雷达测距定位模块测距定位的结果，确定设备的最优位置估计；所述上位机数据管理软件，用于对下位机实时传输的位置、速度和姿态信息进行存储、管理和应用处理，并同步获取设备的行走轨迹和直线度；所述惯性导航模块依次与所述惯导零速度修正模块及所述信息融合模块连接，所述双里程计模块依次与所述惯导零速度修正模块及所述信息融合模块连接，所述惯导零速度修正模块与所述信息融合模块连接，所述毫米波雷达测距定位模块通过所述信息融合模块与所述上位机数据管理软件连接。2.根据权利要求1所述的一种基于惯导的掘锚机多源信息融合定位定向系统，其特征在于，所述初始对准包括：初始粗对准和初始精对准。3.根据权利要求2所述的一种基于惯导的掘锚机多源信息融合定位定向系统，其特征在于，所述初始粗对准包括：构建地球自转矢量方程和重力矢量方程；构造出垂直于地球自转矢量和重力矢量的垂直矢量；根据地球自转矢量与重力矢量及垂直矢量构成矩阵的可逆性，得出方向余弦矩阵的粗对准值；其中，地球自转矢量方程为：重力矢量方程为：垂直矢量为：方向余弦矩阵的粗对准值为：式中：b为载体系；n为当地水平指北坐标系；为方向余弦矩阵；w
ie
为地球自转角速度矢量；g为重力加速度矢量。
4.根据权利要求3所述的一种基于惯导的掘锚机多源信息融合定位定向系统，其特征在于，所述初始精对准包括：采用卡尔曼滤波器，在卡尔曼滤波收敛后的结果为粗对准姿态的失准角，将粗对准姿态的失准角补偿到粗对准的基础即可完成精对准；其中，卡尔曼滤波器中采用的对准随机系统模型为：X＝[φ
E
φ
N
φ
U
δv
E
δv
N
ε
N
ε
U
]
TTTTT
式中：X表示卡尔曼滤波中系统模型的状态向量；表示系统模型状态向量的导数；F矩阵表示系统的状态转移矩阵；G表示系统噪声分配矩阵；W表示系统噪声向量；H表示量测矩阵；V表示量测噪声。5.根据权利要求1所述的一种基于惯导的掘锚机多源信息融合定位定向系统，其特征在于，所述解算板分别采集光纤陀螺的角增量和加速度计的速度增量进行解算，并实时输出设备的位置、速度和姿态包括：
采用捷联算法，在初始精对准的基础上，采用光纤陀螺输出的角增量构造等效旋转矢量进行姿态更新，完成姿态更新后，采用加速度计的速度增量输出进行更新速度和位置，以完成惯导实时更新。6.根据权利要求1所述的一种基于惯导的掘锚机多源信息融合定位定向系统，其特征在于，所述判断设备是否处于停止状态，以进行零速滤波，并实时估算惯性器件的常值偏移，当满足滤波收敛后再进行反馈修正惯导包括：构造三维速度量测信息；建立惯导的速度误差量测方程；进行卡尔曼滤波；修正惯导漂移；其中，三维速度量测信息为：速度误差量测方程为：滤波更新方程为：式中：k下标的均为当前时刻的量，k
‑
1下标的均为上一个时刻的系...

【专利技术属性】
技术研发人员：杨功流，张江源，张志东，蔡庆中，
申请(专利权)人：江苏崇创嘉航智能科技有限公司，
类型：发明
国别省市：