一种悬臂式掘进机空间位姿实时检测系统及方法
【专利摘要】本发明提出一种悬臂式掘进机空间位姿实时检测系统及方法，包括两台十字激光发射器、工控计算机、两台网络摄像机、两个激光标靶。使用机器视觉技术作为位姿参数的采集方式，通过建立掘进机位姿解算数学模型，得到掘进机行进过程中的偏航角、俯仰角、横滚角以及机身上固定一点在巷道截面上的偏移量，即掘进机的空间位姿，可用于掘进机自动行进中的纠偏控制。实验结果表明该系统所检测角度的精度在0.5°范围内，位移的精度优于20mm，且每帧图像的处理与显示速度约为5s，满足巷道施工过程中自动、精确、实时定位的要求。
【专利说明】
一种悬臂式掘进机空间位姿实时检测系统及方法
技术领域
[0001] 本发明涉及一种矿用悬臂式掘进机的空间位姿实时检测系统及方法，属采掘设备 应用和机器视觉应用领域。
【背景技术】
[0002] 悬臂式掘进机是一种常见的采掘机械，被广泛应用于煤矿巷道和隧道的挖掘中。
[0003] 当前，掘进机的掘进过程主要依靠激光标靶导向以及人工目测实现，易受到现场 光线和人为等因素干扰，从而造成对巷道的超挖、欠挖现象，导致企业采煤效率低下甚至引 发安全问题。因此，实现掘进机掘进过程的自动化和智能化就显得尤为迫切，而要解决掘进 机的自动精确掘进就必须完成对其机身空间位姿参数的自动实时检测。基于立体计算机视 觉的掘进机机身位姿检测技术，其技术实质就是利用机器视觉技术代替人眼来检测和分析 空间刚体的位姿，采用该技术方案具有硬件成本低、操作简便、测量精度高等特点。
[0004] 已有发明专利如公开号为CN101819036A和公开号CN102878976A，均采用立体计算 机视觉的原理测量掘进机的机身位姿参数，但由于此类发明专利建立模型时，采用点激光 源作为指向单元，故均存在激光指向单元发射的点光源易受遮挡，从而造成测量系统准确 性差、不稳定，甚至导致无法检测出掘进机位姿参数。所以，此类发明具有局限性，不能进一 步推广。
【发明内容】
[0005] 本发明专利针对【背景技术】的不足，提出一种悬臂式掘进机空间位姿实时检测系统 及方法，可实时准确地检测出掘进机机身的空间位姿参数，且具有抗干扰性强、稳定性高等 特点。
[0006] 为了实现上述目的，本发明是通过以下技术方案实现的：
[0007] 本发明专利是一种悬臂式掘进机空间位姿实时检测系统及方法，可实时获取掘进 机的位姿参数，包括偏航角a、俯仰角0、横滚角y、以及机身中心点D(d X，dy，dz)的坐标，由 于在实际操作中车前距L(即dy)对于掘进机的纠偏用处不大，故所述检测系统及方法只计 算了掘进机机身上固定一点得dx与dz的值。
[0008] 进一步的，所述系统包括两台十字激光发射器、一台工控计算机、两台网络摄像 机、两个激光标靶。
[0009] 进一步的，所述两台十字激光发射器为矿用防爆十字激光发射器，铅直悬挂固定 于巷道顶板，分别向所述两个激光标靶发射十字激光信号。
[0010] 进一步的，所述两个激光标靶分别固定在掘进机机身水平面的左前方与右后方， 用于接收所述十字激光发射器发射的十字激光的投影。
[0011] 进一步的，所述两台网络摄像机为矿用防爆网络摄像机，分别安装于所述两个激 光标靶的正后方，用于采集掘进机运行时刻在所述激光标靶的十字激光成像。
[0012] 进一步的，所述工控计算机可以实时处理大量图形化数据，通过与所述两台网络 摄像机连接，分别提取所述网络摄像机采集的左右激光标靶上的成像并进行图像处理和特 征参数提取。
[0013] 进一步的，经所述工控计算机对所述网络摄像机采集图像进行处理并提取特征参 数之后，从所述激光标靶靶面上可解算出激光图像的光学特征点的坐标，包括所述十字激 光发射器发射出的十字激光和所述激光标靶靶面交点的坐标，所述十字激光发射器发射出 的十字激光在所述激光标靶靶面投影的中心点坐标。
[0014] 进一步的，根据巷道坐标系和机身坐标系双坐标系模型和上述解算出的特征点的 坐标，进行位姿解算，最终得到掘进机在巷道坐标系下的空间位姿参数，即偏航角a、俯仰角 0、横滚角y、左右偏距dx与上下偏距dz的值。
[0015] 与已有技术相比，本发明有益效果是：
[0016] 本发明将机器视觉技术应用于悬臂式掘进机机身位姿检测系统，将硬件软件化， 具有成本低、操作简便、测量精度高等特点。此外，本发明还采用双十字激光发射器，有效解 决了点光源易受遮挡造成的测量系统不稳定、测量结果不精确的问题，具有抗干扰性强、成 本可控等特点，具备一定市场推广价值。
【附图说明】
[0017] 附图1为本发明掘进机与测量设备相互位置图；
[0018] 附图2为本发明巷道坐标系与掘进机机身坐标系模型图；
[0019] 附图3为本发明掘进机空间位姿检测流程图；
[0020] 附图4为本发明双坐标系机身位姿数学模型向量与角度转换示意图；
[0021 ]附图5为本发明激光标靶成像特征点解算示意图；
[0022] 附图6为本发明掘进机空间位姿解算示意图；
[0023] 附图7为本发明机身坐标系与巷道坐标系坐标转换示意图。
【具体实施方式】
[0024] 为了能够更清楚地理解本发明的上述目的、特征、优点，下面结合附图和具体实施 方式对本发明进一步详细阐述。
[0025]设备安装及模型的建立：
[0026]参考附图1所示，本发明专利是一种悬臂式掘进机空间位姿实时检测系统及方法， 包括两台十字激光发射器1、2,两个激光标靶4、6,两台网络摄像机5、7和一台工控计算机。 [0027]所述两台十字激光发射器1、2分别铅直悬挂固定于巷道顶板，安装时，保证两束水 平激光重叠并平行于掘进机预设行进方向，所述两台十字激光发射器1、2发射的十字激光 平面分别与所述激光标靶平面4、6垂直，所述两台激光发射器中心与巷道中轴线距离相等， 所述两台激光发射器间距为1;所述两个激光标靶4、6分别固定在掘进机机身水平面的左前 方与右后方，安装时保证所述两个激光标靶平行于掘进机机身的铅直面且两束十字激光分 别在所述两个激光标靶上的初始成像位于中心位置;所述两台网络摄像机5、7安装于标靶 的正后方，并调整相互之间距离，以保证成像的完整性。
[0028]参考附图2和附图4所示，建立巷道坐标系和机身坐标系双坐标系机身位姿数学模 型，所述模型要求巷道断面坐标系为〇hXhYhZh，初始位置时刻所述掘进机机身中线与巷道中 线重合，所述巷道坐标系原点Oh位于巷道断面的几何中心，Xh轴指向巷道右侧，Yh轴与巷道 中轴线平行并指向掘进机前进方向，Zh轴垂直与上两轴构成右手关系并取向上为正方向。 [0029]所述机身坐标系为ObXbYbZb建立在掘进机机身上，原点0 b位于机身中轴线上，在初 始位置时Xb、Yb、Zb三轴分别与Xh、Yh、Zh三轴平行。
[0030] 所述模型可简化为巷道坐标系下长方体刚体位姿的确定，即机身坐标系的Xb、Yb、 Zb在巷道坐标系下的向量(郊、济、苏）以及刚体上任意一点在该坐标系下的坐标。
[0031] 其中，机身坐标系下的机身三个平面的法向量与H y的转换关系为：
[0033] 进而可得，
[0035]掘进机机身位姿检测整体流程：
[0036]参考附图3所示，首先使用所述工控计算机对所述两台网络摄像机中提取的所述 两个激光标靶上的成像进行取样，可定时对采集到的视频流取样一帧图像。
[0037]进一步的，使用所述工控计算机处理图像并提取特征参数，先用Retinex图像增强 算法对图像进行自适应亮度调节，由于采集到的图像产生了较大的桶行畸变，故需对图像 进行畸变校正，再通过对色彩的处理过滤掉两个靶面图像中不需要的激光线，使用直线边 缘检测的方法并根据标靶安装的实际尺寸，解算出机身坐标系下所述十字激光发射器发射 出的十字激光分别与所述激光成像标靶靶面交点的坐标及所述十字激光发射器发射出的 十字激光中心点在所述激光靶面成像的坐标。
[0038]所述解算过程参考附图4,以所述激光标靶^:为例，分别在极坐标下计算出直线 li、l2与四个参考点(Pi-P4)连线的交点A、B、E、F、M在图像坐标系下的坐标，再按式（1)转换 到机身坐标系下：
[0040] 其中，（x，y)为五点在图像坐标系中的坐标，h为Pi、P4间的实际距离，w为间的 实际距离，（X，Y，Z)为5点在机身坐标系中的坐标。
[0041] 进一步的，根据上述两个激光标靶yi、丫：中10个点的坐标，解算出巷道截面坐标 系下掘进机的偏航角、俯仰角、横滚角以及机身上固定一点在巷道截面上的偏移量，即掘进 机的空间位姿(a，0, y，dx，dz)。
[0042] 机身相对巷道坐标系下的偏航角a、俯仰角0与横滚角y解算过程参考附图6,将所 述两台十字激光发射器产生的重叠水平面视为面a，左十字激光竖直面为扮，右十字激光竖 直面为此，右十字激光(a、fe)在靶面 yi上投影的两条相交直线为AB、EF并交于点M，左十字 激光(a、fo)在靶面y 2上投影的两条相交直线为CD、GH并交于点N。因面y :与面y 2平行，故 水平激光面a在所述激光标靶靶面上形成的两条直线AB与CD相互平行，通过上一步的图像 处理与所述激光标靶的实际尺寸与安装位置可以得到71与^2上十个点的具体坐标，从而 得到平面a在坐标系ObXbYbZb下的法向量与平面方程。
[0043] 特殊情况下，向量屈|平行于向量^",此时掘进机的俯仰角0为零，横滚角Y为直 线CD与轴Xb间的夹角，偏航角a为g与初始位置时的她處间的夹角；若向量g不平行于 向量C,则涵x应即为平面01的法向量瓦T,兩、.范分别于巷道截面坐标系Xh、Zh的方向 相同，Yh的方向即为gxg (￥)，易求得，线段CD延长线上的一点N'满足]即右 十字激光的中心轴线，从而将平面扮右移到平面此的位置，模拟了理想状态下的十字激光投 影效果，为点坐标的精确解算提供条件。
[0044] 参考附图7,上述所得的瓦瓦|为机身坐标系0bXbYbZb下的机身方向向量， 须再进行坐标转换，得到巷道坐标系OhXhYhZh下机身的方向向量。具体解算过程如下：对矩 阵(Nx，Ny，Nz )%矩阵变换，与转换矩阵R相乘转换为单位矩阵，即可得到0hXhYhZ h坐标系下 三个轴的单位向量。
[0045] N ? R = E (4)
[0046] 向量在OhXhYhZh坐标系下为单位矩阵E，对单位矩阵做相同变换得到 的矩阵，即为OhXhYhZh坐标系下的机身位姿。
[0047] B = E ? R (5)
[0047] SPB = N-S 可得到
[0049] 再由（2)式即可得到机身相对巷道坐标系下的偏航角a、俯仰角0与横滚角Y。
[0050] 机身相对巷道坐标系下的点坐标解算过程如下：
[0051]进一步的，为确定空间内一刚体的位姿，只知道机体坐标系三个轴的方向角并不 能精确定位，还需得到刚体上任意一点〇1在空间内的具体坐标，即（〇1\，〇^，〇12)。在掘进机 定位系统中，掘进机在巷道截面上左右、上下的偏移距离比沿巷道中心轴走过的距离更为 有效的帮助矿井工作人员发现掘进机的行走偏差并及时纠偏。且由于所述检测系统采用视 觉定位的方法，无法确切知晓掘进机与巷道坐标系坐标原点间的轴向距离〇 ly，故所述检测 系统针对点坐标的解算只计算了（〇lx，〇lz)，即在巷道截面上Ch点相对于断面中心点的偏移 距离。
[0052] 参考附图6,直线MN'为右十字激光中心轴线，平行于巷道截面坐标系的Yh轴。所述 检测系统选择〇b点为机身定位点，求点0b在面ZhOhXh上相对巷道中轴线的偏移，可先简化为 求点0b在平行于ZhOhXh-平面上的投影相对MN'投影点的偏移，即在直线丽'上找一点0〇,满 足5^)赫=〇6
[0053] 点0b在坐标平面ZhOhXh上投影点的坐标(dx，dz)计算方法如下，
[0056]本发明中涉及的未说明部分与现有技术相同或采用现有技术加以实现。
【主权项】
1. 一种悬臂式掘进机空间位姿实时检测系统及方法，其特征在于，包括:两台十字激光 发射器、一台工控计算机、两台网络摄像机、两个激光标靶，其中， 所述两台十字激光发射器为矿用防爆十字激光发射器，分别铅直悬挂固定于巷道顶 板，发射十字激光束； 所述两个激光标靶为磨砂面半透明PC板，固定在掘进机机身水平面板的左前方与右后 方，分别用于接收所述十字激光发射器发射的十字激光的投影； 所述两台网络摄像机为矿用防爆网络摄像机，分别安装于所述两个激光标靶的正后 方，采集掘进机运行时刻在所述激光成像标靶的十字激光成像； 所述工控计算机通过与所述两台网络摄像机连接，分别提取所述网络摄像机采集的左 右激光标靶上的图像，并进行处理和特征参数提取，最终得到掘进机在巷道坐标系下的空 间位姿参数。2. 根据权利要求1所述的一种悬臂式掘进机空间位姿实时检测系统及方法，其特征在 于，所述两台十字激光器的安装需保证两束水平激光重叠并平行于掘进机预设行进方向， 所述两台十字激光发射器发射的十字激光平面分别与所述激光标靶平面垂直，所述两台激 光发射器中心与巷道中轴线距离相等，所述两台激光发射器间距为1。3. 根据权利要求1所述的一种悬臂式掘进机空间位姿实时检测系统及方法，其特征在 于，所述两个激光标靶安装时需保证平行于掘进机机身的尾部平面且两束十字激光分别在 所述两个激光标靶上的初始成像位于中心位置。4. 根据权利要求1所述的一种悬臂式掘进机空间位姿实时检测系统及方法，其特征在 于，所述两台网络摄像机安装时需保证分别位于所述激光标靶的正后方，并调整相互之间 距离保证成像的完整性。5. 根据权利要求1所述的一种悬臂式掘进机空间位姿实时检测系统及方法，其特征在 于，所述工控计算机能够即时处理各类数据，安装在掘进机内部，与掘进机刚性连接，通过 网络连接线分别与所述两台网络摄像机连接，获取所述两个激光标靶的成像，并进行图像 处理和特征参数提取，最终计算出掘进机在巷道坐标系下的空间位姿参数。6. 根据权利要求1至5所述的一种悬臂式掘进机空间位姿实时检测系统及方法，其特征 在于，所述悬臂式掘进机空间位姿实时检测系统检测方法为:使用两台固定在巷道上的所 述十字激光器向对应的所述激光标靶进行投影，然后通过两个固定在掘进机机身上的所述 网络摄像机实时采集掘进机工作时所述激光标靶上的成像，随后使用Retinex图像增强算 法、图像畸变校正和直线检测等图像处理方法对采集到的视频信号进行处理，最终利用建 立好的位姿解算模型得到掘进机行进过程中的偏航角、俯仰角、横滚角以及机身上固定一 点在巷道截面上的偏移量。
【文档编号】G01C11/00GK106052645SQ201610141544
【公开日】2016年10月26日
【申请日】2016年3月11日 公开号201610141544.9, CN 106052645 A, CN 106052645A, CN 201610141544, CN-A-106052645, CN106052645 A, CN106052645A, CN201610141544, CN201610141544.9
【发明人】董海波, 刘停, 杜雨馨, 周玲玲, 童敏明, 梁良
【申请人】中国矿业大学