一种掘进机绝对空间位姿检测装置与方法与流程

本发明涉及一种掘进机绝对空间位姿检测装置与方法，属于自动化掘进技术。

背景技术：

我国是煤矿的开采与消费大国，与此同时我国对公路、铁路、隧道工程、水电工程等基础设施建设要求不断增强，这些工程中都面临对隧道或巷道的掘进，悬臂式掘进机是一种常见的采掘机械，广泛应用于巷道和隧道的挖掘。因掘进机工作环境差，危险性大，人工操作局限性很大，故掘进机的自动化作业是发展的必然趋势。我国在掘进机的自动化工作上起步较晚，虽然也取得了长足发展，形成了一部分半自动化掘进工作面，但是还不能适应我国煤炭安全高效生产的需要，存在掘进机定位精度低、巷道断面自动成形质量差、系统可靠性低、工人劳动强度大等问题。为了进一步提高掘进机的自动化水平，首先需要解决掘进机机体的精确定位问题，一些文献资料中提出了掘进机机体定位的方法，但是在井下的复杂环境中，很多定位方法都存在一定的局限性，或者定位精度达不到要求，无法为掘进机的自动控制提供条件。

技术实现要素：

发明目的：为了克服现有技术中存在的不足，本发明提供一种掘进机绝对空间位姿检测装置与方法，能够精确检测掘进机的六自由度位姿参数，且实时性好、抗干扰性强、可靠性高，能够为掘进机的自动控制提供条件。

技术方案：为实现上述目的，本发明采用的技术方案为：

一种掘进机绝对空间位姿检测装置，包括激光发射模块、图像获取模块、捷联惯导模块和数据处理与控制模块，激光发射模块固定于坐标已知的悬臂式掘进机主体的后方巷道中间位置，图像获取模块、捷联惯导模块和数据处理与控制模块固定在悬臂式掘进机主体上；激光发射模块用于产生激光图像，图像获取模块用于获取激光图像，捷联惯导模块用于提供导航信息，数据处理与控制模块根据激光图像与导航信息解算出悬臂式掘进机六自由度的位姿状态。

具体的，所述激光发射模块包括激光发射器安装架、点激光发射器和扇面激光发射器，点激光发射器和扇面激光发射器均为本安隔爆型激光发射器，点激光发射器和扇面激光发射器通过激光发射器安装架固定于坐标已知的悬臂式掘进机主体的后方巷道中间位置；所述扇面激光发射器布置在巷道横截面的中部位置，扇面激光发射器发射出的激光平行于巷道地面；点激光发射器的数量为三个，分布在扇面激光发射器的两侧，其中一侧安装两个，另一侧安装一个，位于同一侧的两个点激光发射器的连线垂直于扇面激光发射器，位于同一侧的两个点激光发射器中的一个与另一侧的一个点激光发射器的高度相同，三个点激光发射器发射出的激光均平行于巷道中轴线。

具体的，所述数据处理与控制模块包括嵌入式微处理器和plc控制器，嵌入式微处理器和plc控制器分别集成安装在隔爆箱内，通过隔振器固定在悬臂式掘进机主体上；所述捷联惯导模块安装在隔爆箱内，通过隔振器固定在悬臂式掘进机主体上；所述嵌入式微处理器对导航信息进行实时解算，得到悬臂式掘进机的六自由度位姿参数，同时对激光图像进行包括图像处理、特征提取、坐标运算、位姿求解在内的运算，得到悬臂式掘进机前进方向位置之外的五自由度位姿状态。

所述嵌入式微处理器分别与人机界面和plc控制器进行通信，一方面可以将位姿信息等传送给人机界面进行远程监控，另一方面可以将位置信息传送给plc控制器，便于plc控制器根据位姿信息进行相应的调整与控制。

具体的，所述图像获取模块安装在的数量为两个，分别安装在两个图像获取模块安装架上，通过隔振器前后交错、高度相同地固定在悬臂式掘进机主体的两侧；所述图像获取模块包括防尘箱、半透明板、摄像机和led灯，摄像机为本安隔爆型摄像机，led灯为隔爆型led灯，半透明板作为防尘箱朝向后方的一个侧板，该侧板垂直于巷道中轴线；在半透明板内侧四个角点的固定位置处标记或加工参考点，通过参考点为激光成像位置提供参考；摄像机安装在防尘箱内部，摄像机的镜头轴线与半透明板的中心轴线重合，确保摄像机能够拍摄到所有参考点，led灯沿摄像机的镜头轴线周向均匀布置。

具体的，所述图像获取模块的防尘箱可以设计为侧面可打开的结构，一边拆装维修。

具体的，所述激光发射模块发射出的激光为红光，所述图像获取模块中半透明板为绿色亚克力板，绿色与红色对比最为明显。

一种掘进机绝对空间位姿检测方法，是一种由捷联惯导位姿检测方法和视觉位姿检测方法复合而成的方法，具体为：使用捷联惯导位姿检测方法实时获取悬臂式掘进机主体的位姿信息，每间隔t时长使用视觉位姿检测方法获取一次悬臂式掘进机主体的位姿信息，然后使用最优估计方法对两种位姿检测方法得到的位姿信息进行异步融合，得到精准的位姿信息，同时更新捷联惯导位姿检测方法的初值；当视觉位姿检测方法获取的激光图像清晰度低于设定阈值时，调整激光图像发生源的位置。

具体的，所述最优估计方法为卡尔曼滤波方法。

单独的捷联惯导位姿检测方法的特点是：求解简单、实时性好、不需要外部参考，属于自主定位导航，但是由于其解算方法决定了单独的捷联惯导位姿检测方法存在累计误差。单独的视觉位姿检测方法的特点是：需要外部参考和图像处理，因此计算量大、实时性不好，但其精度高，不存在累计误差。本发明将两种方法进行复合，能够进行优势互补；由于悬臂式掘进机前进方向的位置信息对悬臂式掘进机的自动控制作用不大，在一定时间内，可以忽略该自由度的累计误差；当悬臂式掘进机前进一段长度的巷道后，激光成像变得模糊，因此需要将激光发射模块整体向前移动一定距离，并对系统进行重新标定和初始化。

有益效果：本发明提供的掘进机绝对空间位姿检测装置与方法，与现有技术相比，采用了捷联惯导位姿检测方法和视觉位姿检测方法相复合的方法对掘进机六自由度的位姿参数进行检测，精度高、可靠性高、实时性好，将内部定位方式与外部定位方式进行融合，发挥了两种定位方式的优势，适用于井下巷道中的恶劣环境，能够为掘进机的自动化作业提供条件。

附图说明

图1为本发明掘进机绝对空间位姿检测装置安装示意图；

图2为悬臂式掘进机主体的安装示意图；

图3为本发明的系统框图；

图4为图像获取模块示意图；

图5为悬臂式掘进机主体在没有位姿偏差时半透明板上激光成像图；

图中：1、悬臂式掘进机主体，2、捷联惯导模块，3、数据处理与控制模块，4、隔振器，5、图像获取模块安装架，6、图像获取模块，6-1、防尘箱，6-2、半透明板，6-3、摄像机，6-4、led灯，6-2-1、参考点，6-2-2、点激光成像，6-2-3、扇面激光成像，7、激光发射模块，7-1、点激光发射器，7-2、扇面激光发射器，7-3、激光发射器安装架。

具体实施方式

下面结合附图对本发明作更进一步的说明。

如图1～图4所示为一种掘进机绝对空间位姿检测装置，包括激光发射模块7、图像获取模块6、捷联惯导模块2和数据处理与控制模块3，激光发射模块7固定于坐标已知的悬臂式掘进机主体1的后方巷道中间位置，图像获取模块6、捷联惯导模块2和数据处理与控制模块3固定在悬臂式掘进机主体1上；激光发射模块7用于产生激光图像，图像获取模块6用于获取激光图像，捷联惯导模块2用于提供导航信息，数据处理与控制模块3根据激光图像与导航信息解算出悬臂式掘进机六自由度的位姿状态。

所述激光发射模块7包括激光发射器安装架7-3、点激光发射器7-1和扇面激光发射器7-2，点激光发射器7-1和扇面激光发射器7-2均为本安隔爆型激光发射器，点激光发射器7-1和扇面激光发射器7-2通过激光发射器安装架7-3固定于坐标已知的悬臂式掘进机主体1的后方巷道中间位置；所述扇面激光发射器7-2布置在巷道横截面的中部位置，扇面激光发射器7-2发射出的激光平行于巷道地面；点激光发射器7-1的数量为三个，分布在扇面激光发射器7-2的两侧，其中一侧安装两个，另一侧安装一个，位于同一侧的两个点激光发射器7-1的连线垂直于扇面激光发射器7-2，位于同一侧的两个点激光发射器7-1中的一个与另一侧的一个点激光发射器7-1的高度相同，三个点激光发射器7-1发射出的激光均平行于巷道中轴线。

所述数据处理与控制模块3包括嵌入式微处理器和plc控制器，嵌入式微处理器和plc控制器分别集成安装在隔爆箱内，通过隔振器4固定在悬臂式掘进机主体1上；所述捷联惯导模块2安装在隔爆箱内，通过隔振器4固定在悬臂式掘进机主体1上；所述嵌入式微处理器对导航信息进行实时解算，得到悬臂式掘进机的六自由度位姿参数，同时对激光图像进行包括图像处理、特征提取、坐标运算、位姿求解在内的运算，得到悬臂式掘进机前进方向位置之外的五自由度位姿状态。

所述图像获取模块6安装在的数量为两个，分别安装在两个图像获取模块安装架5上，通过隔振器4前后交错、高度相同地固定在悬臂式掘进机主体1的两侧；所述图像获取模块6包括防尘箱6-1、半透明板6-2、摄像机6-3和led灯6-4，摄像机6-3为本安隔爆型摄像机，led灯6-4为隔爆型led灯，半透明板6-2作为防尘箱6-1朝向后方的一个侧板，该侧板垂直于巷道中轴线；在半透明板6-2内侧四个角点的固定位置处标记或加工参考点6-2-1，通过参考点6-2-1为激光成像位置提供参考；摄像机6-3安装在防尘箱6-1内部，摄像机6-3的镜头轴线与半透明板6-2的中心轴线重合，确保摄像机6-3能够拍摄到所有参考点6-2-1，led灯6-4沿摄像机6-3的镜头轴线周向均匀布置。

一种掘进机绝对空间位姿检测方法，是一种由捷联惯导位姿检测方法和视觉位姿检测方法复合而成的方法，具体为：使用捷联惯导位姿检测方法实时获取悬臂式掘进机主体的位姿信息，每间隔t时长使用视觉位姿检测方法获取一次悬臂式掘进机主体的位姿信息，然后使用卡尔曼滤波方法等最优估计方法对两种位姿检测方法得到的位姿信息进行异步融合，得到精准的位姿信息，同时更新捷联惯导位姿检测方法的初值；当视觉位姿检测方法获取的激光图像清晰度低于设定阈值时，调整激光图像发生源的位置。

如图5所示，当悬臂式掘进机主体1没有位姿偏差时，在半透明板6-2上，扇面激光成像6-2-3位于半透明板6-2的上下中间位置，点激光成像6-2-2位于半透明板6-2的左右中间位置，其中，位于同一侧的两个点激光发射器7-1在半透明板6-2上的成像位置关于扇面激光成像6-2-3对称。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。