CN111780666A - 一种矿用掘进机位姿监测***及监测方法 - Google Patents
一种矿用掘进机位姿监测***及监测方法 Download PDFInfo
- Publication number
- CN111780666A CN111780666A CN202010692933.7A CN202010692933A CN111780666A CN 111780666 A CN111780666 A CN 111780666A CN 202010692933 A CN202010692933 A CN 202010692933A CN 111780666 A CN111780666 A CN 111780666A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- camera
- lamp
- motor
- heading machine
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000011161 development Methods 0.000 title claims abstract description 42
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000012544 monitoring process Methods 0.000 title claims abstract description 39
- 238000005065 mining Methods 0.000 title claims abstract description 20
- 238000013519 translation Methods 0.000 claims abstract description 29
- 239000003086 colorant Substances 0.000 claims description 20
- 238000006243 chemical reaction Methods 0.000 claims description 18
- 239000011159 matrix material Substances 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 16
- 230000003287 optical effect Effects 0.000 claims description 12
- 238000000926 separation method Methods 0.000 claims description 11
- 238000004891 communication Methods 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 8
- 150000001875 compounds Chemical class 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 6
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 238000009825 accumulation Methods 0.000 claims description 3
- 230000005641 tunneling Effects 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 2
- 238000012545 processing Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 4
- 238000009412 basement excavation Methods 0.000 description 3
- 238000000605 extraction Methods 0.000 description 3
- 239000003245 coal Substances 0.000 description 2
- 230000000630 rising effect Effects 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000005260 corrosion Methods 0.000 description 1
- 230000007797 corrosion Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B11/00—Measuring arrangements characterised by the use of optical techniques
- G01B11/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
- G01C11/02—Picture taking arrangements specially adapted for photogrammetry or photographic surveying, e.g. controlling overlapping of pictures
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D3/00—Control of position or direction
- G05D3/10—Control of position or direction without using feedback
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明提供了一种矿用掘进机位姿监测***及监测方法，包括安装于掘进机的摄像机，摄像机在任意时刻都能拍摄三掘进机后方的三个灯，以此测量掘进机机身的位置与姿态参数；其中机身的位置参数是指掘进机机身沿世界坐标系Ow‑XwYwZw中三个坐标轴的平移量；姿态参数是指掘进机机身绕世界坐标系Ow‑XwYwZw中三个坐标轴的转动角度。利用安装于掘进机的摄像机4，以及掘进机后方的三个灯，测量掘进机机身的位置与姿态参数；监测过程中，可以同步获得掘进机机身的六个位姿参数。机身的位置参数是指掘进机机身沿世界坐标系Ow‑XwYwZw中三个坐标轴的平移量；姿态参数是指掘进机机身绕世界坐标系Ow‑XwYwZw中三个坐标轴的转动角度。
Description
技术领域
本发明涉及监测***，特别是一种矿用掘进机位姿监测***及监测方法。
背景技术
煤矿巷道掘进向少人(无人)值守的方向发展，同时煤矿掘进装备向智能化的方向发展，而掘进机在巷道内的位置与姿态直接影响巷道掘进的精准度，为此，必须准确监测掘进机的位姿参数。
发明内容
本发明的目的是提供一种矿用掘进机位姿监测***及监测方法，以便能及时获得矿用掘进机的位姿参数，从而提高巷道掘进的精准度。
本发明的目的是这样实现的，一种矿用掘进机位姿监测***，包括安装于掘进机的摄像机4，以及掘进机后方的三个灯，所述摄像机4在任意时刻都能拍摄三个灯，以此测量掘进机机身的位置与姿态参数；
其中机身的位置参数是指掘进机机身沿世界坐标系Ow-XwYwZw中三个坐标轴的平移量；姿态参数是指掘进机机身绕世界坐标系Ow-XwYwZw中三个坐标轴的转动角度。
还包括上电机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与上位机以串行方式通信。
一种矿用掘进机位姿监测***监测方法，包括安装于掘进机的下电机2、上电机3、摄像机4，以及掘进机后方的三个灯，以此测量掘进机机身的位置与姿态参数；机身的位置参数是指掘进机机身沿世界坐标系Ow-XwYwZw中三个坐标轴的平移量；姿态参数是指掘进机机身绕世界坐标系Ow-XwYwZw中三个坐标轴的转动角度；具体包括如下步骤：
摄像机4安装于上电机3上，上电机3安装于下电机2上，下电机2安装于掘进机1的机身上；摄像机4可绕上电机3和下电机2的轴转动，在掘进机后方安装有三个不同颜色的灯，蓝色灯5、红色灯6、绿色灯7，由摄像机4同时拍摄蓝色灯5、红色灯6、绿色灯7；
从摄像机4所拍摄的图片中分别提取蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标；
将蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标变换到掘进机坐标系Om-XmYmZm中；
上位机将三个灯在像素平面坐标系OU-UxUy中的坐标发送至微控制器STM32F103ZET6；
微控制器STM32F103ZET6根据三个灯在像素平面坐标系OU-UxUy中的坐标，计算三个灯所构成的三角形的形心在像素平面坐标系OU-UxUy中的坐标；
并且根据此形心在像素平面坐标系OU-UxUy中的坐标，驱动上电机3与下电机2的轴转动，从而带动摄像机4运动，使摄像机4的主光轴指向该形心附近；
微控制器STM32F103ZET6将上电机3与下电机2的转动角度发送至上位机；上位机计算掘进机的位姿参数，并显示结果。
所述的将蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标变换到掘进机坐标系Om-XmYmZm中包括如下：
摄像机4获取包括蓝色灯5、红色灯6、绿色灯7的图像，生成包括蓝色灯5、红色灯6、绿色灯7的彩色图片；然后，提取摄像机4所拍摄的一帧彩色图片，作为原图像；
所提取原图像为二维图像，其上的像素可以视为位于二维平面内的点，像素在原图像中的位置用不同的行、列表示；
在所提取的原图像中，逐行、逐列读取每一个像素的蓝、绿、红三种颜色的深度值；从中选取最大值，并且将其与其他两个值比较，如果该最大值与其他两个值之差均大于预设值，则认为该像素的颜色为最大值所对应的颜色；
将原图像中所有的蓝色、绿色、红色分别分离到不同的新的图片中，从而实现了将原图像中蓝、绿、红三种颜色分别分离在三个新图片中；其中，蓝色灯的图像必然存在于分离之后的包含蓝色的图片中；绿色灯的图像必然存在于分离之后包含绿色的图片中；红色灯的图像必然存在于分离之后的包含红色的图片中；
最后，在仅含有目标图像的图片中获得目标图像的外切矩形，通过目标图像的外切矩形求得目标图像的中心坐标，由此获得蓝色灯5、红色灯6、绿色灯7在像素坐标系OU-UxUy中的坐标。
所述的掘进机位姿参数的获取包括如下过程：
摄像机4拍摄蓝色灯5、红色灯6、绿色灯7，并且根据如前所述的坐标获得方法，获得蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标。由于像素平面坐标系OU-UxUy和图像平面坐标系o-xy在同一平面内，则像素平面坐标系OU-UxUy与图像平面坐标系o-xy之间的转化关系为
式中，Ux、Uy以及x、y分别为某一个灯在像素平面坐标系OU-UxUy和图像平面坐标系o-xy中的坐标；Ux0、Uy0为图像平面坐标系o-xy原点在像素平面坐标系OU-UxUy中的坐标；a为摄像机的像元尺寸。
图像平面坐标系o-xy与摄像机坐标系Oc-XcYcZc之间的转化关系为
在上电机3与下电机2上分别建立上电机3坐标系O1-X1Y1Z1和下电机2坐标系O2-X2Y2Z2；
下电机2坐标系O2-X2Y2Z2的原点位于下电机2的O2Y2轴和掘进机接触面的交点上，上电机3坐标系O1-X1Y1Z1的原点位于O2Y2的延长线与上电机3轴线的交点上；起始时刻，上电机3坐标系O1-X1Y1Z1、下电机2坐标系O2-X2Y2Z2的各坐标轴与世界坐标系Ow-XwYwZw各坐标轴平行且方向相同；
摄像机坐标系Oc-XcYcZc与上电机3坐标系O1-X1Y1Z1之间的转化关系为
式中，矩阵R1为摄像机坐标系Oc-XcYcZc绕X1坐标轴旋转的旋转矩阵；向量T1中的元素分别为点Oc到点O1沿O1X1、O1Y1、O1Z1平移的距离，根据两个坐标系之间的位置关系，有lyOcO1＝0，α为上电机3转动的角度，由编码器所产生的脉冲数计算得到；
同理，上电机3坐标系O1-X1Y1Z1与下电机2坐标系O2-X2Y2Z2的关系为
式中，矩阵R2为上电机3坐标系O1-X1Y1Z1系绕Y2坐标轴旋转的旋转矩阵；向量T2中的元素分别为点O1到点O2沿O2X2、O2Y2、O2Z2平移的距离，根据两个坐标系之间的位置关系，有lxO1O2＝0，lzO1O2＝0，β为下电机2转过的角度，由编码器所产生的脉冲数计算得到；
下电机2坐标系O2-X2Y2Z2与掘进机坐标系Om-XmYmZm只有平移的关系，其转换关系为
式中，向量T3中的元素分别为点O2到点Om沿OmXm、OmYm、OmZm平移的距离；
综合公式(1)(2)(3)(4)(5)可以得到三个灯的像在像素平面坐标系OU-UxUy与掘进机坐标系Om-XmYmZm的坐标转换关系为
同理，掘进机坐标系Om-XmYmZm与世界坐标系Ow-XwYwZw的关系为
式中，
ω、κ分别为掘进机的姿态参数，
R(ω)、R(κ)分别为关于
ω、κ的旋转矩阵，向量T4中的元素分别为点Om到点Ow沿OwXw、OwYw、OwZw平移的距离；
为了求解矩阵R4和向量T4，设(Xw,Yw,Zw)为蓝色灯、绿色灯、红色灯中的任意一个在世界坐标系Ow-XwYwZw中的坐标，而且设a点为其像，a点在掘进机坐标系Om-XmYmZm中的坐标为(Xm,Ym,Zm)，(Xf,Yf,Zf)为摄像机4的焦点在掘进机坐标系中的坐标；掘进机坐标系Om-XmYmZm的原点Om在世界坐标系Ow-XwYwZw中的坐标为(Xs,Ys,Zs)，根据各坐标系之间的关系，得
式中，λ为比例因子。
掘进机坐标系Om-XmYmZm与世界坐标系Ow-XwYwZw之间的转化需要经过旋转和平移，为此，引入掘进机辅助坐标系Om-Xm’Ym’Zm’，将掘进机坐标系Om-XmYmZm与世界坐标系Ow-XwYwZw的转化分为两个过程：掘进机坐标系Om-XmYmZm经过旋转得到掘进机辅助坐标系Om-Xm’Ym’Zm’，掘进机辅助坐标系Om-Xm’Ym’Zm’经过平移得到世界坐标系Ow-XwYwZw。掘进机坐标系Om-XmYmZm与掘进机辅助坐标系Om-Xm’Ym’Zm’之间的关系为
式中，(Xm’,Ym’,Zm’)为像点a在掘进机辅助坐标系Om-Xm’Ym’Zm’中的坐标；
在考虑摄像机4的内部参数带来误差的基础上，由(9)式得到方程
式中，x0、y0分别为摄像机4的主光轴和图像平面坐标系o-xy的交点到图像平面坐标系o-xy中两坐标轴的距离；
对公式(10)进行泰勒展开，使其线性化，得到相应的误差方程为
式中，x0、y0与(10)式中的Xm’、Ym’具有相同含义；ΔXs、ΔYs、ΔZs、
Δω、Δκ分别为掘进机位姿参数的修正值。
基于以上(8)式-(11)式，并且结合最小二乘间接平差原理，可以得到法方程解的表达式：
ΔX＝(ATA)-1ATl (12)
式中，
l＝{Xm1-x0 Ym1-y0 Xm2-x0 Ym2-y0 Xm3-x0 Ym3-y0}T，Xm1、Xm2、Xm1、Ym1、Ym2、Ym3分别为蓝色灯、绿色灯及红色灯的像在掘进机坐标系中的坐标；
通过n次迭代运算，向量l的元素逐渐趋于0，而掘进机的位姿参数逐渐趋于实际值，当向量ΔX中的角元素修正值足够小时，则停止迭代，就能够得到满足需求的掘进机的位姿参数
迭代算法要求必须给出掘进机位姿参数的初值，通过蓝色灯、绿色灯及红色灯在世界坐标系Ow-XwYwZw中的坐标确定初始值
Zs0＝m·f
式中，m为影像比例尺的分母，(Xwi,Ywi)表示三个灯在世界坐标系中的坐标。
一种矿用掘进机位姿监测***的监测方法，控制电路通过驱动上电机3、下电机2带动摄像机4运动；通过位置式PID算法控制；在PID算法控制中，PWM信号中高电平的周期值为：
式中，Kp、Ki、Kd分别为比例、积分、微分系数；ek为由三个灯所构成的三角形的形心在像素平面坐标系OU-UxUy中的坐标与图像平面坐标系o-xy原点在像素平面坐标系OU-UxUy中的坐标的差值，
为差值的累计，ek-1为上次差值；
通过位置式PID算法控制上电机3与下电机2的转动，可以消除ek，从而实现摄像机4追踪三个灯构成的三角形的形心；在上电机3与下电机2带动摄像机4追踪三个灯的过程中，STM32F103ZET6读取编码器的脉冲数，计算两个电机转过的角度，再将其转化为自定义通信协议的数据格式，发送至上位机。
本发明的优点是：利用安装于掘进机的下电机2、上电机3、摄像机4，以及掘进机后方的三个灯，测量掘进机机身的位置与姿态参数；监测过程中，可以同步获得掘进机机身的六个位姿参数。机身的位置参数是指掘进机机身沿世界坐标系Ow-XwYwZw中三个坐标轴的平移量；姿态参数是指掘进机机身绕世界坐标系Ow-XwYwZw中三个坐标轴的转动角度。
下面结合实施例及附图对本发明作进一步说明：
附图说明
图1是本发明实施例掘进机位姿监测***示意图；
图2是本发明实施例掘进机位姿监测流程图；
图3是图2流程中坐标提取流程图；
图4是本发明实施例掘进机位姿监测***控制电路图；
图5是下电机和上电机驱动正转/反转时序图；
图6是本发明实施例上位机和微控制器通信协议；
图7为上电机3坐标系O1-X1Y1Z1和下电机2坐标系O2-X2Y2Z2的示意图；
图8为监测***界面示意图；
图9为测量得到的位置参数数据曲线；
图10为测量得到的姿态参数数据曲线。
图中，1、掘进机；2、下电机；3、上电机；4、摄像机；5、蓝色灯；6、红色灯；7、绿色灯。
具体实施方式
如图1所示，一种矿用掘进机位姿监测***及监测方法，包括安装于掘进机的摄像机4，所述摄像机4在任意时刻都能拍摄三掘进机后方的三个个灯，以此测量掘进机机身的位置与姿态参数；其中机身的位置参数是指掘进机机身沿世界坐标系Ow-XwYwZw中三个坐标轴的平移量；姿态参数是指掘进机机身绕世界坐标系Ow-XwYwZw中三个坐标轴的转动角度。
如图1所示，还包括上电机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。
图1中，Ow-XwYwZw为世界坐标系；Oc-XcYcZc为摄像机坐标系；OU-UxUy为像素平面坐标系；o-xy为图像平面坐标系，图像平面坐标系与像素平面坐标系在同一平面内；Om-XmYmZm为掘进机坐标系；f为摄像机镜头的焦距；蓝色灯5、红色灯6、绿色灯7在世界坐标系Ow-XwYwZw中的坐标已知。
如图2所示，给出一种矿用掘进机位姿监测***监测方法流程，首先由摄像机4同时拍摄蓝色灯5、红色灯6、绿色灯7；
然后，从摄像机4所拍摄的图片中分别提取蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标；
随后，将蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标变换到掘进机坐标系Om-XmYmZm中；
同时，上位机将三个灯在像素平面坐标系OU-UxUy中的坐标发送至微控制器STM32F103ZET6；
微控制器根据三个灯在像素平面坐标系OU-UxUy中的坐标，计算三个灯所构成的三角形的形心在像素平面坐标系OU-UxUy中的坐标；
并且根据此形心在像素平面坐标系OU-UxUy中的坐标，驱动上电机3与下电机2的轴转动，从而带动摄像机4运动，使摄像机4的主光轴指向该形心附近；
之后，微控制器STM32F103ZET6将上电机3与下电机2的转动角度发送至上位机；上位机计算掘进机的位姿状态，并显示结果。
蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标获取流程如图3所示：
摄像机4通过USB总线与上位机连接，工作过程中，摄像机4获取包括蓝色灯5、红色灯6、绿色灯7的图像，生成包括蓝色灯5、红色灯6、绿色灯7的彩色图片；然后，提取摄像机4所拍摄的一帧彩色图片，作为原图像。
所提取原图像为二维图像，其上的像素可以视为位于二维平面内的点，像素在原图像中的位置用不同的行、列表示，例如，位于第i行、第j列的像素可以用Aij表示。
在所提取的原图像中，逐行、逐列读取每一个像素的蓝、绿、红三种颜色的深度值，例如，像素Aij的蓝、绿、红三种颜色的深度值为{bij,gij,rij}(bij、gij、rij分别对应蓝色、绿色、红色)。从bij、gij、rij中选取最大值，并且将其与其他两个值比较，如果该最大值与其他两个值之差均大于预设置值，则认为该像素的颜色为最大值所对应的颜色。例如，bij为像素Aij的三种颜色的深度值{bij,gij,rij}中的最大值，将其与其他两个值作比较：
式中，threshold为预先设定的值。
bij与gij、rij之差均大于预先设定的threshold值，则认为像素Aij的颜色为蓝色。否则，认为该像素的颜色为黑色，通过此方法，可以将原图像中所有蓝色的区域分离到一张新的图片中。同理，可以将原图像中所有的红色、绿色分别分离到不同的新的图片中。从而实现了将原图像中蓝、绿、红三种颜色分别分离在三个新图片中。其中，蓝色灯的图像必然存在于分离之后的包含蓝色的图片中；绿色灯的图像必然存在于分离之后包含绿色的图片中；红色灯的图像必然存在于分离之后的包含红色的图片中。
由实际工况可知，分离后的图片中除目标图像(蓝色灯的图像、绿色灯的图像、红色灯的图像)之外，存在两种干扰图像，一种是噪点，另一种是与目标图像颜色相近的非目标图像。对于这两种干扰图像，依次通过开运算和轮廓形状匹配算法予以消除。
开运算处理之前，对分离后的、分别包含蓝、绿、红三种颜色的三个新的图片进行二值化处理，使图片前景(分离图片中的蓝色区域、红色区域及绿色区域)为白色，背景为黑色。二值化处理可以更好地分辨分离后的图片中的前景和背景，有利于后续图像处理。
开运算进行时，首先利用腐蚀算法，将二值化图片中的前景(包括目标图像、非目标的图像与噪点)的轮廓由外向内收缩，从而将噪点以及面积较小的非目标图像由前景改为背景，由此去除前景中的噪点以及面积较小的非目标图像；面积较大的目标图像及非目标图像仍然保留在原位置(但是，其面积已经收缩)；随后，通过膨胀算法，放大此前被收缩的面积较大的目标图像与非目标图像，使其还原为原尺寸。经开运算处理，可以消除图像中的噪点及面积较小的非目标图像。
但是，在分离的图片中仍然可能存在面积较大的非目标图像，应将其消除。由实际工况可知，通常，面积较大的非目标图像的形状与目标图像的形状存在较大差异。为此，使用视觉技术中的轮廓匹配算法进行处理，根据轮廓形状匹配方法，分别获得分离图片中(经二值化处理之后)每个前景的轮廓(包括目标图像的轮廓及非目标图像的轮廓)与预先设定的目标轮廓的相似度，将相似度最大的前景图像作为目标图像，从而在分离后的图片中获得所需的目标图像。
最后，在仅含有目标图像的图片中获得目标图像的外切矩形，通过目标图像的外切矩形求得目标图像的中心坐标，由此获得目标在像素平面坐标系OU-UxUy中的坐标。
按照以上方法，逐次完成三个分离图片中目标坐标的提取，从而获得蓝色灯5、红色灯6、绿色灯7在像素坐标系OU-UxUy中的坐标。坐标提取流程图如图3所示。
由于掘进机工作过程中，其机身的位置与姿态实时变化，摄像机4的拍摄范围可能偏离蓝色灯5、红色灯6以及绿色灯7三个灯，为了保证蓝色灯5、红色灯6以及绿色灯7始终在摄像机4的拍摄范围内，通过上电机3、下电机2带动摄像机4运动，从而保证摄像机4的主光轴能够指向三个灯所构成的三角形的形心附近。
如图4所示，给出驱动上电机3及下电机2运动的控制电路图，包括：微控制器STM32F103ZET6、电机驱动器L298N、光电编码器MG513BP30，光电编码器MG513BP30分别与上电机3、下电机2一体式安装，上电机3与下电机2为永磁有刷直流减速电机；光电编码器MG513BP30转动一周输出的脉冲数为15000个，微控制器STM32F103ZET6与上位机以串行方式通信。
1)STM32F103ZET6的PA0、PA1、PA2、PA3分别与L298N的IN1、IN2、IN3、IN4相连，STM32F103ZET6的PA0、PA1、PA2、PA3控制引脚输出高电平或低电平，通过不同的高、低电平的组合，控制电机的转动方向以及启停(如表1所示)。
表1高、低电平组合逻辑表
| ENA | IN1 | IN2 | 运行状态 | |
| 0 | x | x | 停止 | |
| 1 | 1 | 0 | 正转 | |
| 1 | 0 | 1 | 反转 | |
| 1 | 1 | 1 | 停止 | |
| 1 | 0 | 0 | 停止 |
①ENA为使能端口，当其为低电平时，无论IN1、IN2的状态是否为高或者低电平，电机不转动。
②当ENA为高电平，且IN1为高电平，IN2为低电平时，电机正转。
③当ENA为高电平，且IN1为低电平，IN2为高电平时，电机反转。
④当ENA为高电平，且IN1为高电平，IN2也为高电平时，L298N的输出电压为零，电机停止转动。
⑤当ENA为高电平，且IN1为低电平，IN2也为低电平时，L298N的输出电压为零，电机停止转动。
2)STM32F103ZET6的PA8、PA11分别输出PWM信号，并且与L298N的ENA、ENB相连，由此控制电机的转速。
3)STM32F103ZET6的PF0、PF1、PF2、PF3为输入模式，与两个电机编码器中的A、B端口分别相连，用于接收编码器产生的脉冲数，由此获得上电机3及下电机2的转角。该编码器为AB相增量式编码器，AB相在工作时分别输出的信号如图5所示
由于AB两相的相位差为90°，本***通过比较AB两相的先后顺序判断电机的正反转。例如，当A相产生了一个上升沿信号，令STM32F103ZET6进入对应引脚的上升沿中断，在中断过程中，判断B相电平的高低状态，若B相为高电平，则可知电机反转，若B相为低电平，则可知电机正转，并且计算B相高电平及低电平的数量，以及相应的脉冲数，由此得到电机的转角。
为了更高效地实现在STM32F103ZET6与上位机之间接收和发送数据，需定义通信协议。上位机向STM32F103ZET6发送蓝色灯5、红色灯6、绿色灯7三个灯在像素平面坐标系OU-UxUy中的坐标，STM32F103ZET6向上位机发送上电机3和下电机2转过的角度数据。根据发送数据的特点，定义如图6所示的通信协议
该通信协议由12位ASCII码字符组成，每一位所代表的含义如下
1)第0位和第11位，分别为每组数据的起始位和停止位，初始化时，分别赋予‘S’和‘E’。当且仅当上位机或STM32F103ZET6接收到‘S’时，才会继续接收后续数据；当接收到‘E’时，则把接收到的整组数据传递至后续的处理函数中；
2)第1至4位，以及第6到9位，以四位为一个整体，存放需要发送的数据。由于所发送的数据不超过4位数，所以设置4位ASCII码字符存放数据；
3)第5位和第10位分别表示两个数据的正负，仅限定为‘+’和‘-’。
通过上述通信协议，将所发送的数据转化为12位ASCII码字符，当STM32F103ZET6或上位机接收到数据后，12位ASCII码字符数据转化为十进制数据，实现数据处理。
上电机3与下电机2的转动通过PID控制算法实现，本***使用位置式PID算法，PWM信号中高电平的周期值为
式中，Kp、Ki、Kd分别为比例、积分、微分系数；ek为由三个灯所构成的三角形的形心在像素平面坐标系OU-UxUy中的坐标与图像平面坐标系o-xy原点在像素平面坐标系OU-UxUy中的坐标的差值，
为差值的累计，ek-1为上次差值。
通过位置式PID算法控制上电机3与下电机2的转动，可以在一定程度上消除ek，从而实现摄像机4追踪三个灯。在上电机3与下电机2带动摄像机4追踪三个灯的过程中，STM32F103ZET6读取编码器的脉冲数，由此计算两个电机转过的角度，再将其转化为自定义通信协议的数据格式，发送至上位机。
掘进机位姿参数的获取包括如下过程：
摄像机4拍摄蓝色灯5、红色灯6、绿色灯7。并且根据如前所述的坐标获得方法，获得蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标。由于像素平面坐标系OU-UxUy和图像平面坐标系o-xy在同一平面内，则像素平面坐标系OU-UxUy与图像平面坐标系o-xy之间的转化关系为
式中，Ux、Uy以及x、y分别为某一个灯在像素平面坐标系OU-UxUy和图像平面坐标系o-xy中的坐标；Ux0、Uy0为图像平面坐标系o-xy原点在像素平面坐标系OU-UxUy中的坐标；a为摄像机的像元尺寸。
图像平面坐标系o-xy与摄像机坐标系Oc-XcYcZc之间的转化关系为
在上电机3与下电机2上分别建立上电机3坐标系O1-X1Y1Z1和下电机2坐标系O2-X2Y2Z2如图7所示。
下电机2坐标系O2-X2Y2Z2的原点位于下电机2的O2Y2轴和掘进机接触面交点上，上电机3坐标系O1-X1Y1Z1的原点位于O2Y2的延长线与上电机3轴线的交点上。起始时刻，上电机3坐标系O1-X1Y1Z1、下电机2坐标系O2-X2Y2Z2的各坐标轴与世界坐标系Ow-XwYwZw各坐标轴平行且方向相同。
摄像机坐标系Oc-XcYcZc与上电机3坐标系O1-X1Y1Z1之间的转化关系为
式中，矩阵R1为摄像机坐标系Oc-XcYcZc绕X1坐标轴旋转的旋转矩阵；向量T1中的元素分别为点Oc到点O1沿O1X1、O1Y1、O1Z1平移的距离，根据两个坐标系之间的位置关系，有lyOcO1＝0，α为上电机3转动的角度，由编码器所产生的脉冲数计算。
同理，上电机3坐标系O1-X1Y1Z1与下电机2坐标系O2-X2Y2Z2的关系为
式中，矩阵R2为上电机3坐标系O1-X1Y1Z1系绕Y2坐标轴旋转的旋转矩阵；向量T2中的元素分别为点O1到点O2沿O2X2、O2Y2、O2Z2平移的距离，根据两个坐标系之间的位置关系，有lxO1O2＝0，lzO1O2＝0，β为下电机2转过的角度，由编码器所产生的脉冲数计算得到。
下电机2坐标系O2-X2Y2Z2与掘进机坐标系Om-XmYmZm只有平移的关系，其转换关系为
式中，向量T3中的元素分别为点O2到点Om沿OmXm、OmYm、OmZm平移的距离。
综合公式(1)(2)(3)(4)(5)可以得到三个灯的像在像素平面坐标系OU-UxUy与掘进机坐标系Om-XmYmZm的坐标转换关系为
同理，掘进机坐标系Om-XmYmZm与世界坐标系Ow-XwYwZw的关系为
式中，
ω、κ分别为掘进机的姿态参数，
R(ω)、R(κ)分别为关于
ω、κ的旋转矩阵，向量T4中的元素分别为点Om到点Ow沿OwXw、OwYw、OwZw平移的距离。
为了求解矩阵R4和向量T4，将灯(三个灯中的任意一个)及其在摄像机4中的像，和摄像机镜头焦点连接，设(Xw,Yw,Zw)为蓝色、绿色、红色灯中的任意一个在世界坐标系Ow-XwYwZw中的坐标，a点为其像，a点在掘进机坐标系Om-XmYmZm中的坐标为(Xm,Ym,Zm)，(Xf,Yf,Zf)为摄像机4的焦点在掘进机坐标系中的坐标；掘进机坐标系Om-XmYmZm的原点Om在世界坐标系Ow-XwYwZw中的坐标为(Xs,Ys,Zs)，根据各坐标系之间的几何关系，得
式中，λ为比例因子。
掘进机坐标系Om-XmYmZm与世界坐标系Ow-XwYwZw之间的转化需要经过旋转和平移，为此，引入掘进机辅助坐标系Om-Xm’Ym’Zm’，将掘进机坐标系Om-XmYmZm与世界坐标系Ow-XwYwZw的转化分为两个过程：掘进机坐标系Om-XmYmZm经过旋转得到掘进机辅助坐标系Om-Xm’Ym’Zm’，掘进机辅助坐标系Om-Xm’Ym’Zm’经过平移得到世界坐标系Ow-XwYwZw。掘进机坐标系Om-XmYmZm与掘进机辅助坐标系Om-Xm’Ym’Zm’之间的关系为
式中，(Xm’,Ym’,Zm’)为像点a在掘进机辅助坐标系Om-Xm’Ym’Zm’中的坐标。
在考虑摄像机4的内部参数带来误差的基础上，由(9)式得到方程
式中，x0、y0为摄像机4的主光轴和图像平面坐标系o-xy的交点到图像平面坐标系o-xy中两坐标轴的距离。
对公式(10)进行泰勒展开，使其线性化，得到相应的误差方程为
式中，x0、y0与(10)式中Xm’、Ym’意义相同；ΔXs、ΔYs、ΔZs、
Δω、Δκ分别为掘进机位姿参数的修正值。
基于以上(8)式-(11)式，并且根据最小二乘间接平差原理，可以得到法方程解的表达式：
ΔX＝(ATA)-1ATl (12)
式中，
l＝{Xm1-x0 Ym1-y0 Xm2-x0 Ym2-y0 Xm3-x0 Ym3-y0}T，Xm1、Xm2、Xm1、Ym1、Ym2、Ym3分别为蓝色、绿色及红色灯的像在掘进机坐标系中的坐标。
通过n次迭代运算，向量l的元素逐渐趋于0，而掘进机的位姿参数逐渐趋于实际值，当向量ΔX中的角元素修正值足够小时，则停止迭代，就能够得到满足需求的掘进机的位姿参数
迭代算法要求必须给出掘进机位姿参数的初值，通过蓝色灯、绿色灯及红色灯在世界坐标系Ow-XwYwZw中的坐标确定初始值
Zs0＝m·f
式中，m为影像比例尺的分母，(Xwi,Ywi)表示三个灯在世界坐标系中的坐标。
本发明的监测结果见图9、图10所示。监测***的界面如图8所示，图中最左侧圆为红色灯6，中间圆为蓝色灯5，最右侧圆为绿色灯7。
Claims (9)
1.一种矿用掘进机位姿监测***，其特征是：包括安装于掘进机的摄像机4，以及掘进机后方的三个灯，所述摄像机4在任意时刻都能拍摄三个灯，以此测量掘进机机身的位置与姿态参数；
其中，机身的位置参数是指机身沿世界坐标系Ow-XwYwZw中三个坐标轴的平移量，姿态参数是指机身绕世界坐标系Ow-XwYwZw中三个坐标轴的转动角度。
2.根据权利要求1所述的一种矿用掘进机位姿监测***，其特征是：还包括上电机3和下电机2，所述摄像机4安装于上电机3上，上电机3安装于下电机2上，下电机2安装于掘进机1的机身上；摄像机4可绕上电机3和下电机2的轴转动，在掘进机后方安装有三个不同颜色的灯。
3.根据权利要求1所述的一种矿用掘进机位姿监测***，其特征是：三个不同颜色的灯分别为蓝色灯5、红色灯6、绿色灯7，蓝色灯5、红色灯6、绿色灯7构成的三角形分布，测量过程中，上电机3及下电机2的轴分别转动，带动摄像机4运动，使摄像机4的镜头的主光轴指向蓝色灯5、红色灯6、绿色灯7所构成的三角形的形心位置，以此保证摄像机4在任意时刻都能拍摄蓝色灯5、红色灯6、绿色灯7。
4.根据权利要求1所述的一种矿用掘进机位姿监测***，其特征是：摄像机4通过USB总线与上位机连接，工作过程中，控制电路通过驱动上电机3、下电机2带动摄像机4运动，从而保证摄像机4的主光轴能够指向三个灯所构成的三角形的形心附近；摄像机4获取包括蓝色灯5、红色灯6、绿色灯7的图像，生成包括蓝色灯5、红色灯6、绿色灯7的彩色图片；然后，提取摄像机4所拍摄的一帧彩色图片，作为原图像。
5.根据权利要求4所述的一种矿用掘进机位姿监测***，其特征是：控制电路包括：微控制器STM32F103ZET6、电机驱动器L298N、光电编码器MG513BP30，光电编码器MG513BP30分别与上电机3、下电机2一体式安装，上电机3与下电机2为永磁有刷直流减速电机；光电编码器MG513BP30转动一周输出的脉冲数为15000个，微控制器STM32F103ZET6与上位机以串行方式通信。
6.一种矿用掘进机位姿监测***的监测方法，其特征是：包括安装于掘进机的下电机2、上电机3、摄像机4，以及掘进机后方的三个灯，以此测量掘进机机身的位置与姿态参数；机身的位置参数是指机身沿世界坐标系Ow-XwYwZw中三个坐标轴的平移量；姿态参数是指机身绕世界坐标系Ow-XwYwZw中三个坐标轴的转动角度；具体包括如下步骤：
摄像机4安装于电机3上，上电机3安装于下电机2上，下电机2安装于掘进机1的机身上；摄像机4可绕上电机3和下电机2的轴转动，在掘进机后方安装有三个不同颜色的灯，蓝色灯5、红色灯6、绿色灯7，由摄像机4同时拍摄蓝色灯5、红色灯6、绿色灯7；
摄像机4所拍摄的图片中分别提取蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标；
将蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标变换到掘进机坐标系Om-XmYmZm中；
上位机将三个灯在像素平面坐标系OU-UxUy中的坐标发送至微控制器，微控制器采用STM32F103ZET6；
微控制器根据三个灯在像素平面坐标系OU-UxUy中的坐标，计算三个灯所构成的三角形的形心在像素平面坐标系OU-UxUy中的坐标；
并且根据此形心在像素平面坐标系OU-UxUy中的坐标，驱动上电机3与下电机2的轴转动，从而带动摄像机4运动，使摄像机4的主光轴指向该形心附近；
微控制器将上电机3与下电机2的转动角度发送至上位机；上位机计算掘进机的位姿状态，并显示结果。
7.根据权利要求6所述的一种矿用掘进机位姿监测***的监测方法，其特征是：所述的将蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标变换到掘进机坐标系Om-XmYmZm中包括如下：
摄像机4获取包括蓝色灯5、红色灯6、绿色灯7的图像，生成包括蓝色灯5、红色灯6、绿色灯7的彩色图片；然后，提取摄像机4所拍摄的一帧彩色图片，作为原图像；
所提取原图像为二维图像，其上的像素可以视为位于二维平面内的点，像素在原图像中的位置用不同的行、列表示；
在所提取的原图像中，逐行、逐列读取每一个像素的蓝、绿、红三种颜色的深度值；从中选取最大值，并且将其与其他两个值比较，如果该最大值与其他两个值之差均大于预设置值，则认为该像素的颜色为最大值所对应的颜色；
将原图像中所有的蓝色、绿色、红色分别分离到不同的新的图片中，从而实现了将原图像中蓝、绿、红三种颜色分别分离在三个新图片中；其中，蓝色灯的图像必然存在于分离之后的包含蓝色的图片中；绿色灯的图像必然存在于分离之后包含绿色的图片中；红色灯的图像必然存在于分离之后的包含红色的图片中；
最后，在仅含有目标图像的图片中获得目标图像的外切矩形，通过目标图像的外切矩形求得目标图像的中心坐标，由此获得蓝色灯5、红色灯6、绿色灯7在像素坐标系OU-UxUy中的坐标。
8.根据权利要求6所述的一种矿用掘进机位姿监测***监测方法，其特征是：所述的掘进机位姿参数的获取包括如下过程：
摄像机4拍摄蓝色灯5、红色灯6、绿色灯7，并且根据如前所述的坐标获得方法，获得蓝色灯5、红色灯6、绿色灯7在像素平面坐标系OU-UxUy中的坐标。由于像素平面坐标系OU-UxUy和图像平面坐标系o-xy在同一平面内，则像素平面坐标系OU-UxUy与图像平面坐标系o-xy之间的转化关系为
式中，Ux、Uy以及x、y分别为某一个灯在像素平面坐标系OU-UxUy和图像平面坐标系o-xy中的坐标；Ux0、Uy0为图像平面坐标系o-xy原点在像素平面坐标系OU-UxUy中的坐标；a为摄像机的像元尺寸；
图像平面坐标系o-xy与摄像机坐标系Oc-XcYcZc之间的转化关系为
在上电机3与下电机2上分别建立上电机3坐标系O1-X1Y1Z1和下电机2坐标系O2-X2Y2Z2；
下电机2坐标系O2-X2Y2Z2的原点位于下电机2的O2Y2轴和掘进机接触面交点上，上电机3坐标系O1-X1Y1Z1的原点位于O2Y2的延长线与上电机3轴线的交点上；起始时刻，上电机3坐标系O1-X1Y1Z1、下电机2坐标系O2-X2Y2Z2的各坐标轴与世界坐标系Ow-XwYwZw各坐标轴平行且方向相同；
摄像机坐标系Oc-XcYcZc与上电机3坐标系O1-X1Y1Z1之间的转化关系为
式中，矩阵R1为摄像机坐标系Oc-XcYcZc绕X1坐标轴旋转的旋转矩阵；向量T1中的元素分别为点Oc到点O1沿O1X1、O1Y1、O1Z1平移的距离，根据两个坐标系之间的位置关系，有lyOcO1＝0，α为上电机3转动的角度，由编码器所产生的脉冲数计算得到；
同理，上电机3坐标系O1-X1Y1Z1与下电机2坐标系O2-X2Y2Z2的关系为
式中，矩阵R2为上电机3坐标系O1-X1Y1Z1绕Y2坐标轴旋转的旋转矩阵；向量T2中的元素分别为点O1到点O2沿O2X2、O2Y2、O2Z2平移的距离，根据两个坐标系之间的位置关系，有lxO1O2＝0，lzO1O2＝0，β为下电机2转过的角度，由编码器所产生的脉冲数计算得到；
下电机2坐标系O2-X2Y2Z2与掘进机坐标系Om-XmYmZm只有平移的关系，其转换关系为
式中，向量T3中的元素分别为点O2到点Om沿OmXm、OmYm、OmZm平移的距离；
综合公式(1)(2)(3)(4)(5)可以得到三个灯的像在像素平面坐标系OU-UxUy与掘进机坐标系Om-XmYmZm的坐标转换关系为
同理，掘进机坐标系Om-XmYmZm与世界坐标系Ow-XwYwZw的关系为
式中，
ω、κ分别为掘进机的姿态参数，
R(ω)、R(κ)分别为关于
ω、κ的旋转矩阵，向量T4中的元素分别为点Om到点Ow沿OwXw、OwYw、OwZw平移的距离；
为了求解矩阵R4和向量T4，将三个灯中的任意一个及其在摄像机4中的像，和摄像机镜头焦点连接，设(Xw,Yw,Zw)为蓝色、绿色、红色灯中的任意一个在世界坐标系Ow-XwYwZw中的坐标，a点为其像，a点在掘进机坐标系Om-XmYmZm中的坐标为(Xm,Ym,Zm)，(Xf,Yf,Zf)为摄像机4的焦点在掘进机坐标系中的坐标；掘进机坐标系Om-XmYmZm的原点Om在世界坐标系Ow-XwYwZw中的坐标为(Xs,Ys,Zs)，则根据各坐标系之间的关系，得
式中，λ为比例因子。
掘进机坐标系Om-XmYmZm与世界坐标系Ow-XwYwZw之间的转化需要经过旋转和平移，为此，引入掘进机辅助坐标系Om-Xm’Ym’Zm’，将掘进机坐标系Om-XmYmZm与世界坐标系Ow-XwYwZw的转化分为两个过程：掘进机坐标系Om-XmYmZm经过旋转得到掘进机辅助坐标系Om-Xm’Ym’Zm’，掘进机辅助坐标系Om-Xm’Ym’Zm’经过平移得到世界坐标系Ow-XwYwZw。掘进机坐标系Om-XmYmZm与掘进机辅助坐标系Om-Xm’Ym’Zm’之间的关系为
式中，(Xm’,Ym’,Zm’)为像点a在掘进机辅助坐标系Om-Xm’Ym’Zm’中的坐标；
在考虑摄像机4的内部参数带来误差的基础上，由(9)式得到方程
式中，x0、y0为摄像机4的主光轴和图像平面坐标系o-xy的交点到图像平面坐标系o-xy中两坐标轴的距离；
对公式(10)进行泰勒展开，使其线性化，得到相应的误差方程为
式中，x0、y0与(10)式中Xm’、Ym’的意义相同；ΔXs、ΔYs、ΔZs、
Δω、Δκ分别为掘进机位姿参数的修正值。
基于以上(8)式-(11)式，并且根据最小二乘间接平差原理，可以得到包括三个灯的法方程解的表达式：
ΔX＝(ATA)-1ATl (12)
式中，
l＝{Xm1-x0 Ym1-y0 Xm2-x0 Ym2-y0 Xm3-x0 Ym3-y0}T，
Xm1、Xm2、Xm1、Ym1、Ym2、Ym3分别为蓝色灯、绿色灯及红色灯的像在掘进机坐标系中的坐标；
通过n次迭代运算，向量l的元素逐渐趋于0，而掘进机的位姿参数逐渐趋于实际值，当向量ΔX中的角元素修正值足够小时，则停止迭代，就能够得到满足需求的掘进机的位姿参数
迭代算法要求必须给出掘进机位姿参数的初值，通过蓝色、绿色及红色灯在世界坐标系Ow-XwYwZw中的坐标确定初始值
Zs0＝m·f
式中，m为影像比例尺的分母，(Xwi,Ywi)表示三个灯在世界坐标系中的坐标。
9.根据权利要求6所述的一种矿用掘进机位姿监测***监测方法，其特征是：控制电路通过驱动上电机3、下电机2带动摄像机4运动；通过位置式PID算法控制；在PID算法控制中，PWM信号中高电平的周期值为：
式中，Kp、Ki、Kd分别为比例、积分、微分系数；ek为由三个灯所构成的三角形的形心在像素平面坐标系OU-UxUy中的坐标与图像平面坐标系o-xy原点在像素平面坐标系OU-UxUy中的坐标的差值，
为差值的累计，ek-1为上次差值；
通过位置式PID算法控制上电机3与下电机2的转动，可以消除ek，从而实现摄像机4追踪三个灯；在上电机3与下电机2带动摄像机4追踪三个灯的过程中，STM32F103ZET6读取编码器的脉冲数，计算两个电机转过的角度，再将其转化为自定义通信协议的数据格式，发送至上位机。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010692933.7A CN111780666B (zh) | 2020-07-17 | 2020-07-17 | 一种矿用掘进机位姿监测***及监测方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010692933.7A CN111780666B (zh) | 2020-07-17 | 2020-07-17 | 一种矿用掘进机位姿监测***及监测方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111780666A true CN111780666A (zh) | 2020-10-16 |
| CN111780666B CN111780666B (zh) | 2023-11-07 |
Family
ID=72764250
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010692933.7A Active CN111780666B (zh) | 2020-07-17 | 2020-07-17 | 一种矿用掘进机位姿监测***及监测方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111780666B (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113701633A (zh) * | 2021-09-06 | 2021-11-26 | 安徽深核信息技术有限公司 | 一种掘进机位姿监测设备 |
Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006255826A (ja) * | 2005-03-16 | 2006-09-28 | Toguchi Seisakusho:Kk | 計測ヘッド及び工作機械 |
| CN101819036A (zh) * | 2009-11-25 | 2010-09-01 | 煤炭科学研究总院太原研究院 | 一种掘进机的空间位姿自动测量方法 |
| JP2010243478A (ja) * | 2009-03-31 | 2010-10-28 | Mitsubishi Electric Research Laboratories Inc | 環境内の３ｄ物体の３ｄ姿勢を推定する方法及び装置 |
| CN202066500U (zh) * | 2011-03-31 | 2011-12-07 | 天地（常州）自动化股份有限公司 | 一种悬臂式掘进机的位姿测量*** |
| CN102878976A (zh) * | 2012-09-26 | 2013-01-16 | 三一重型装备有限公司 | 掘进机位姿检测***和掘进机 |
| CN103363902A (zh) * | 2013-07-16 | 2013-10-23 | 清华大学 | 基于红外激光的粉尘环境中运动目标位姿检测装置及方法 |
| CN104764440A (zh) * | 2015-03-12 | 2015-07-08 | 大连理工大学 | 基于彩色图像的滚转物单目位姿测量方法 |
| CN105136134A (zh) * | 2015-08-10 | 2015-12-09 | 兖州煤业股份有限公司 | 一种掘进机位姿检测调整方法及*** |
| CN105203099A (zh) * | 2015-10-27 | 2015-12-30 | 中国矿业大学(北京) | 基于iGPS的掘进机单站位姿测量方法 |
| CN107316288A (zh) * | 2017-06-13 | 2017-11-03 | 西安科技大学 | 一种悬臂式掘进机截割头位姿视觉测量方法 |
| CN107741200A (zh) * | 2017-11-15 | 2018-02-27 | 西安科技大学 | 用于悬臂式掘进机的两平面激光对射位姿测量***与方法 |
| CN108007345A (zh) * | 2017-12-01 | 2018-05-08 | 南京工业大学 | 一种基于单目相机的挖掘机工作装置测量方法 |
| CN110700840A (zh) * | 2019-10-21 | 2020-01-17 | 北京易联创安科技发展有限公司 | 一种基于激光标靶定位的掘进机位姿测量装置及其测量方法 |
| CN110736446A (zh) * | 2019-10-28 | 2020-01-31 | 西安电子科技大学 | 一种悬臂式掘进机位姿识别***及方法 |
-
2020
- 2020-07-17 CN CN202010692933.7A patent/CN111780666B/zh active Active
Patent Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006255826A (ja) * | 2005-03-16 | 2006-09-28 | Toguchi Seisakusho:Kk | 計測ヘッド及び工作機械 |
| JP2010243478A (ja) * | 2009-03-31 | 2010-10-28 | Mitsubishi Electric Research Laboratories Inc | 環境内の３ｄ物体の３ｄ姿勢を推定する方法及び装置 |
| CN101819036A (zh) * | 2009-11-25 | 2010-09-01 | 煤炭科学研究总院太原研究院 | 一种掘进机的空间位姿自动测量方法 |
| CN202066500U (zh) * | 2011-03-31 | 2011-12-07 | 天地（常州）自动化股份有限公司 | 一种悬臂式掘进机的位姿测量*** |
| CN102878976A (zh) * | 2012-09-26 | 2013-01-16 | 三一重型装备有限公司 | 掘进机位姿检测***和掘进机 |
| CN103363902A (zh) * | 2013-07-16 | 2013-10-23 | 清华大学 | 基于红外激光的粉尘环境中运动目标位姿检测装置及方法 |
| CN104764440A (zh) * | 2015-03-12 | 2015-07-08 | 大连理工大学 | 基于彩色图像的滚转物单目位姿测量方法 |
| CN105136134A (zh) * | 2015-08-10 | 2015-12-09 | 兖州煤业股份有限公司 | 一种掘进机位姿检测调整方法及*** |
| CN105203099A (zh) * | 2015-10-27 | 2015-12-30 | 中国矿业大学(北京) | 基于iGPS的掘进机单站位姿测量方法 |
| CN107316288A (zh) * | 2017-06-13 | 2017-11-03 | 西安科技大学 | 一种悬臂式掘进机截割头位姿视觉测量方法 |
| CN107741200A (zh) * | 2017-11-15 | 2018-02-27 | 西安科技大学 | 用于悬臂式掘进机的两平面激光对射位姿测量***与方法 |
| CN108007345A (zh) * | 2017-12-01 | 2018-05-08 | 南京工业大学 | 一种基于单目相机的挖掘机工作装置测量方法 |
| CN110700840A (zh) * | 2019-10-21 | 2020-01-17 | 北京易联创安科技发展有限公司 | 一种基于激光标靶定位的掘进机位姿测量装置及其测量方法 |
| CN110736446A (zh) * | 2019-10-28 | 2020-01-31 | 西安电子科技大学 | 一种悬臂式掘进机位姿识别***及方法 |
Non-Patent Citations (4)
| Title |
|---|
| MARIA ANDRÉIA F. RODRIGUES等: "《A serious game to practice stretches and exercises for a correct and healthy posture》", 《ENTERTAINMENT COMPUTING》 * |
| RITWIK CHATTARAJ等: "《Vision-based human grasp reconstruction inspired by hand postural synergies》", 《COMPUTERS AND ELECTRICAL ENGINEERING》 * |
| 张旭辉等: "《位姿检测技术在煤矿井下工作面巡检机器人中的应用》", 《传感器与微***》 * |
| 杨文娟等: "《悬臂式掘进机截割头姿态视觉检测***》", 《煤炭学报》 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113701633A (zh) * | 2021-09-06 | 2021-11-26 | 安徽深核信息技术有限公司 | 一种掘进机位姿监测设备 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111780666B (zh) | 2023-11-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109544636B (zh) | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 | |
| Yang et al. | Hierarchical deep stereo matching on high-resolution images | |
| Chen et al. | Dsgn: Deep stereo geometry network for 3d object detection | |
| CN109784333B (zh) | 基于点云带权通道特征的三维目标检测方法及*** | |
| Kaneko et al. | Mask-SLAM: Robust feature-based monocular SLAM by masking using semantic segmentation | |
| Rebecq et al. | Evo: A geometric approach to event-based 6-dof parallel tracking and mapping in real time | |
| EP3343506A1 (en) | Method and device for joint segmentation and 3d reconstruction of a scene | |
| Gracias et al. | Underwater mosaicing and trajectory reconstruction using global alignment | |
| CN111780666B (zh) | 一种矿用掘进机位姿监测***及监测方法 | |
| JP2015501471A (ja) | 車載搭載型のコンピュータ・ベース視覚システムの校正方法 | |
| CN108154536A (zh) | 二维平面迭代的相机标定法 | |
| Lee et al. | Learning residual flow as dynamic motion from stereo videos | |
| CN104469170A (zh) | 双目摄像装置、图像处理方法及装置 | |
| Chaney et al. | Learning event-based height from plane and parallax | |
| CN113211439B (zh) | 一种基于bm匹配机械臂控制***和方法 | |
| Hu et al. | Dedgenet: Extrinsic calibration of camera and LiDAR with depth-discontinuous edges | |
| Li et al. | Depth-based 6dof object pose estimation using swin transformer | |
| CN111784753A (zh) | 自主水下机器人回收对接前景视场三维重建立体匹配方法 | |
| CN107730543A (zh) | 一种半稠密立体匹配的快速迭代计算方法 | |
| Shariati et al. | Towards privacy-preserving ego-motion estimation using an extremely low-resolution camera | |
| Gandhi et al. | Application of planar motion segmentation for scene text extraction | |
| Yang et al. | A feature extraction technique in stereo matching network | |
| CN111080530B (zh) | 一种基于光栅尺位置实现高精度坐标拼图的方法 | |
| Quan et al. | IMRL: An Improved Inertial-Aided KLT Feature Tracker | |
| Lin et al. | Learning to Estimate Object Poses without Real Image Annotations. |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |