[发明专利]一种矿用掘进机位姿监测系统及监测方法

说明书

本发明提供了一种矿用掘进机位姿监测系统及监测方法，包括安装于掘进机的摄像机，摄像机在任意时刻都能拍摄三掘进机后方的三个灯，以此测量掘进机机身的位置与姿态参数；其中机身的位置参数是指掘进机机身沿世界坐标系O_w‑X_wY_wZ_w中三个坐标轴的平移量；姿态参数是指掘进机机身绕世界坐标系O_w‑X_wY_wZ_w中三个坐标轴的转动角度。利用安装于掘进机的摄像机4，以及掘进机后方的三个灯，测量掘进机机身的位置与姿态参数；监测过程中，可以同步获得掘进机机身的六个位姿参数。机身的位置参数是指掘进机机身沿世界坐标系O_w‑X_wY_wZ_w中三个坐标轴的平移量；姿态参数是指掘进机机身绕世界坐标系O_w‑X_wY_wZ_w中三个坐标轴的转动角度。

技术领域

本发明涉及监测系统，特别是一种矿用掘进机位姿监测系统及监测方法。

背景技术

煤矿巷道掘进向少人(无人)值守的方向发展，同时煤矿掘进装备向智能化的方向发展，而掘进机在巷道内的位置与姿态直接影响巷道掘进的精准度，为此，必须准确监测掘进机的位姿参数。

发明内容

本发明的目的是提供一种矿用掘进机位姿监测系统及监测方法，以便能及时获得矿用掘进机的位姿参数，从而提高巷道掘进的精准度。

本发明的目的是这样实现的，一种矿用掘进机位姿监测系统，包括安装于掘进机的摄像机4，以及掘进机后方的三个灯，所述摄像机4在任意时刻都能拍摄三个灯，以此测量掘进机机身的位置与姿态参数；

其中机身的位置参数是指掘进机机身沿世界坐标系O_w-X_wY_wZ_w中三个坐标轴的平移量；姿态参数是指掘进机机身绕世界坐标系O_w-X_wY_wZ_w中三个坐标轴的转动角度。

还包括上电机3和下电机2，所述摄像机4安装于上电机3上，上电机3安装于下电机2上，下电机2安装于掘进机1的机身上；摄像机4可绕上电机3和下电机2的轴转动，在掘进机后方安装有三个不同颜色的灯。

三个不同颜色的灯分别为蓝色灯5、红色灯6、绿色灯7，蓝色灯5、红色灯6、绿色灯7构成三角形分布，测量过程中，上电机3及下电机2的轴分别转动，带动摄像机4运动，使摄像机4的镜头的主光轴指向蓝色灯5、红色灯6、绿色灯7所构成的三角形的形心位置，以此保证摄像机4在任意时刻都能拍摄到蓝色灯5、红色灯6、绿色灯7。

摄像机4通过USB总线与上位机连接，工作过程中，控制电路通过驱动上电机3、下电机2带动摄像机4运动，从而保证摄像机4的主光轴能够指向三个灯所构成的三角形的形心附近；摄像机4获取包括蓝色灯5、红色灯6、绿色灯7的图像，生成包括蓝色灯5、红色灯6、绿色灯7的彩色图片；然后，提取摄像机4所拍摄的一帧彩色图片，作为原图像。

控制电路包括：微控制器STM32F103ZET6、电机驱动器L298N、光电编码器MG513BP30；光电编码器MG513BP30分别与上电机3、下电机2一体式安装，上电机3与下电机2为永磁有刷直流减速电机；光电编码器MG513BP30转动一周输出的脉冲数为15000个，微控制器STM32F103ZET6与上位机以串行方式通信。