CN112050732A - 一种悬臂式掘进机空间位姿自动检测方法及系统 - Google Patents

Abstract

本发明公开了一种悬臂式掘进机空间位姿自动检测方法及系统，首先前置防爆相机采集截割臂上的标靶图像，通过视觉测量的方法确定掘进机截割头相对于机体的位姿；计算掘进机机身相对于巷道的位姿；通过后置防爆相机采集巷道顶板上的两相互平行的激光线，并进行激光线图像的预处理、直线特征提取；通过视觉测量的方法确定掘进机机身相对于巷道的初始位姿；再结合全站仪对机身、激光指向仪相对巷道的位姿测量，完成后置防爆相机的外参标定，得到机身相对于巷道的实时位姿；另外将机身实时位姿以及截割头实时位姿在上位机进行实时的图形化界面显示。通过本发明的方法和系统利用视觉测量方法可实现掘进机机身以及截割头的空间位姿自动检测。

Description

一种悬臂式掘进机空间位姿自动检测方法及系统

技术领域

本发明属于矿用掘进机技术领域，具体涉及一种悬臂式掘进机空间位姿自动检测方法及系统。

背景技术

悬臂式掘进机被广泛地应用于煤矿巷道、地铁隧道以及铁路工程、洞穴的掘进施工中。但由于煤矿井下的复杂环境，掘进机在井下进行掘进工作时，司机仅依靠经验进行掘进机断面截割方向。导致巷道走向受到多种因素影响，仅依靠经验判断的方法难以把握。在巷道掘进时，仅靠司机把握巷道断面成形质量差，根据位移传感器测量油缸的伸缩量进行计算得到截割头的位姿的方式虽然一定程度上提高了截割头位姿检测的精度，但易受井下非结构化复杂环境的影响，易导致接触式传感器的接触不良而使得检测结果可靠性降低。而惯性导航对掘进机机身位姿的测量时，由于随时间累积误差的存在不适宜对机身位置的测量，仅能保证机身的横滚角、俯仰角以及航向角。另外利用超声激光等测量方式仅能获取掘进机相对巷道的位置，虽然与惯导结合能有效实现对于掘进机位姿的检测，但超声和激光传感器会因不规则的巷道而存在测量误差，进一步影响到巷道的整体走向。

现有一种悬臂式掘进机空间位姿自动检测系统及其测量方法专利，通过在巷道顶部安装矿用十字结构激光器，借助防爆相机采集激光条纹图像进行图像处理以及特征识别，实现对特征点位置和角度的检测，进而计算出机身的横滚角、偏航角、横向偏移距离、俯仰角和纵向偏移量参数。该方法存在以下缺陷：

(1)投射到台阶结构的激光线容易发生遮挡等情况，影响到位姿的解算

(2)实现截割略显复杂，所述台阶结构安装在掘进机机身上表面前端的方法在实际过程中实现起来较为困难，可能会对施工等造成阻挡。

发明内容

针对现有技术存在的缺点，本发明提供了一种悬臂式掘进机空间位姿自动检测方法及系统，解决现有的掘进机截割头及机身位姿测量方法的不足，以保证巷道掘进机效率及质量。

为了实现上述目的，本发明采用如下技术方案予以实现：

一种悬臂式掘进机空间位姿自动检测方法，用于实现悬臂式掘进机截割臂相对巷道的实时位姿检测，包括以下步骤：

步骤一，计算悬臂式掘进机截割臂相对于悬臂式掘进机机体的位姿；

步骤1.1，采集悬臂式掘进机截割臂的标靶图像；

步骤1.2，提取步骤1.1得到标靶图像的光斑，用区域划分法将所有光斑排序并采用最小二乘方法进行直线拟合，得到两两相交的四条直线将每两条直线的交点作为特征点；利用四点透视成像方法得到所有特征点在摄像机坐标系中的坐标；

步骤1.3，根据特征点在标靶坐标系中的坐标以及步骤1.2得到特征点在摄像机坐标系中坐标，采用对偶四元数误差模型计算截割臂相对于机体的水平摆角θ1和垂直摆角θ2；

其中，所述标靶坐标系是指以标靶图像的中心为原点，X轴平行于标靶上下边指向右，Y平行于标靶左右边指向下，Z轴垂直于标靶图像平面；

步骤1.4，根据θ1和θ2确定截割臂相对于悬臂式掘进机机体的位姿

其中，d为悬臂式掘进机油缸的伸缩距离，b1为悬臂式掘进机抬升关节与伸缩关节之间的高度差；a1为悬臂式掘进机回转台中心与抬升关节之间水平距离，a2为悬臂式掘进机抬升关节与伸缩关节之间的距离，a3为悬臂式掘进机伸缩关节与截割臂之间的水平距离；

步骤二，确定截割臂相对巷道的实时位姿

其中，

为采用视觉测量方法确定的悬臂式掘进机机体相对于巷道的位姿。

本发明还提供一种悬臂式掘进机空间位姿自动检测系统，该系统用于实现悬臂式掘进机截割臂相对巷道的实时位姿检测，包括：

通信模块，包括相机，用于采集悬臂式掘进机截割臂上的标靶图像以及悬臂式掘进机运行状态信息，所述相机设置在悬臂式掘进机机体上部两侧；所述悬臂式掘进机运行状态信息包括悬臂式掘进机油缸的伸缩距离，悬臂式掘进机抬升关节与伸缩关节之间的高度差，悬臂式掘进机回转台中心与抬升关节之间水平距离，悬臂式掘进机抬升关节与伸缩关节之间的距离，悬臂式掘进机伸缩关节与截割臂之间的水平距离以及悬臂式机体距离前煤壁的距离；

视觉测量模块，用于测量悬臂式掘进机机体相对于巷道的位姿，以多点红外LED和两条平行激光束为特征进行视觉测量，包括红外LED标靶和两个激光指向仪，红外标靶设置在截割臂上部，用于提取相机采集的标靶图像的光斑；两个激光指向仪设置在巷道内上部，用于沿着巷道顶板发出两条相互平行的红色激光束；所述相机还用于采集激光指向仪的激光束图像；

处理器，存储有计算机程序，用于按照权利要求1中步骤一的内容计算悬臂式掘进机截割臂相对于悬臂式掘进机机体的位姿以及按照权利要求1中步骤二的内容计算截割臂相对于巷道的实时位姿。

进一步，该系统还包括上位机，用于将悬臂式掘进机截割臂以及机体相对巷道的的位姿进行实时图形化界面显示。

进一步，该系统同于还用于实现悬臂式掘进机机体相对于巷道的位姿检测，机体相对于巷道的位姿

的计算如下：

式中，

为相机相对于激光指向仪的位姿；

为激光指向仪在巷道中的位姿，通过全站仪测量得到；

为相机相对于机体的外参矩阵，通过相机外参标定得到；

其中，

的计算步骤如下：

分别提取相机采集的两个激光束图像，对激光束图像中的激光光斑信息分别进行特征提取得到两个光斑区域并采用最小二乘法进行直线拟合，分别获得两个直线方程；同时通过两个光斑区域分别获得两个光斑中心点；通过两个光斑中心点的连线结合两个直线方程建立悬臂式掘进机位姿解算模型，确定相机相对于激光指向仪的位姿

更进一步，确定相机相对于激光指向仪的位姿

具体计算如下：

其中，α1、β1、γ1分别为相机坐标系在激光指向仪坐标系下绕x轴、y轴、z轴旋转的角度，X1、Y1、Z1分别为相机坐标系在激光指向仪坐标系下x轴、y轴、z轴三个方向的分量；所述激光指向仪坐标系的原点在两个激光指向仪连线的中心位置；所述相机坐标系的原点在相机质心位置，激光指向仪坐标系与相机坐标系的各个坐标轴同向。

与现有技术相比，本发明的有益效果是：

(1)本发明通过视觉测量的方法计算出掘进机机身和截割头在巷道中的实时位姿，并实现了在计算机上的可视化显示，可实时对掘进机在空间中位姿进行自动检测，解决了在煤矿复杂环境下的巷道断面成形质量差以及机身位姿测量困难的问题。掘进机司机可在机载计算机上实时观察到掘进机机身以及截割头的空间位姿，能有效保证巷道的截割质量，同时提高了巷道掘进效率以及工作质量。

(2)本发明采用视觉测量的方法实现了机身在巷道空间以及截割头的位姿自动检测，采用非接触测量的方法提高了掘进机的空间位姿检测精度。

附图说明

图1为位姿测量系统组成示意图。

图2为基于两平行激光束的掘进机机身位姿解算。

图3为三个棱镜放置位置示意图。

图4为掘进机程序运行初始位置图。

图5为掘进过程中的程序运行界面。

图6为对光斑进行排序后的标靶图像。

图7为本发明实施例1中四个特征点摄像机坐标系中的坐标计算示意。

具体实施方式

本发明的两激光指向仪在巷道顶板上沿巷道水平方向放置，并且两激光指向仪平行且指向仪前端放于同一平面内；标靶一般是指方形板，并在方形板的四个边上均匀设置有16个红外光源，所述的光斑即为光源在图像中形成的点。

本发明中的摄像机坐标系是指以摄像机的光心为原点X轴沿水平方向，Y轴沿竖直方向，Z轴垂直Y、X轴指向标靶方向；激光指向仪坐标系是指以两激光指向仪中心为原点Y轴沿水平方向，X轴沿两激光指向仪中心的连线，Z轴垂直Y、X轴指向巷道顶板；标靶坐标系是指以标靶中心为原点，X轴平行于标靶上下边指向右，Y平行于标靶左右边指向下，Z轴垂直于标靶平面。

此外，在本发明的计算过程中，提出测量系统全局坐标系的概念，具体包括：坐标系OXYZ为巷道坐标系，O₀X₀Y₀Z₀坐标系为截割臂的基坐标系，其坐标原点在悬臂式掘进机器人的质心处；1坐标系O₁X₁Y₁Z₁的原点在截割臂的回转台轴线上与2坐标系在同一高度与基坐标系重合；2坐标系O₂X₂Y₂Z₂的Z轴在截割臂抬升关节的轴线上；3坐标系O₃X₃Y₃Z₃建立在伸缩油缸的前关节处，Z轴在伸缩油缸活塞杆的轴线上；4坐标系O₄X₄Y₄Z₄建立在截割头最小截割半径的中心，Z轴在截割头锥台的轴线上；O_c1X_c1Y_c1Z_c1为摄像机坐标系；O_wX_wY_wZ_w为标靶坐标系，其原点在矩形标靶的中心，Z轴与坐标系4的Z轴同向。坐标系O_dX_dY_dZ_d为激光定向坐标系，坐标系O_c2X_c2Y_c2Z_c2为相机坐标系。

另外，本发明的方法中提到的掘进机位姿解算模型，为已有的方法，即是掘进机的门型三线位姿解算方法，具体是：根据单目相机针孔投影模型和空间几何投影约束条件，由P4P解算模型求解得到相机坐标系下的特征点空间三维坐标，设标靶坐标系下截割臂上标靶的LED特征点坐标为P^*，相机坐标系下标靶的LED特征点坐标为P，相机坐标系与标靶坐标系之间的转换关系可表示为：P＝RP^*+T，其中R、T分别为标靶坐标系到相机坐标系的旋转和平移矩阵，

本发明的构思在于，掘进机机身测量方法利用单目视觉测量技术，建立空间几何投影约束条件建立位姿解算模型，利用工业相机拍摄两激光指向仪，将采集到的图像通过传输到机载计算机中；截割头测量方法同样利用单目视觉测量技术，提取采集的标靶图像中的光斑，采用区域分割法进行排序、基于最小二乘法的直线拟合，得到两两相交的四条直线并将每两条直线的交点作为特征点，根据四点透视成像的方法确定所有特征点在掘进机坐标系中的坐标，再结合对偶四元数误差模型对截割臂水平和垂直摆角进行解算。

本发明的区域分割法指的是图像分割法中基于区域的分割方法，将具有相似性质的像素集合起来构成区域，为图像分割常用的方法。对光斑进行排序指的是按照光斑图像的灰度值分别进行排序，进一步，在本发明中，基于区域的分割方法，利用坐标系中第一、二、三、四象限，将所获取截割臂标靶图像分为四部分；对于各部分点按照光斑图像的灰度值分别进行排序，然后将所得点按照顶部、底部、左侧、右侧进行集合，进一步进行直线拟合。

本发明所述最小二乘法的直线拟合，其中最小二乘法是在工作中做图像处理最常用的一种拟合算法，同时也是在机器学习中线性回归的一种重要算法。

本发明的步骤一中通过视觉测量的方法确定截割头相对于机体的位姿。具体实现包括：

步骤1.1：提取采集的标靶图像中的光斑；

步骤1.2：采用区域分割法对光斑进行排序、基于最小二乘法的直线拟合，得到四条两两相交的直线，并将每两条直线的交点作为特征点；利用四点透视成像方法得到所有特征点在摄像机坐标系中的坐标；

步骤1.3，根据特征点在标靶坐标系中的坐标以及步骤1.2得到特征点在摄像机坐标系中坐标，再结合对偶四元数误差模型，根据特征点在标靶坐标系中坐标，计算截割头的水平摆角θ1和截割头垂直摆角θ2；

步骤1.4：根据求得的θ1和θ2，通过式(1)计算截割头相对掘进机机体的位姿

其中，θ1为截割头水平摆角，θ2为截割头垂直摆角，d为油缸的伸缩距离，b1为掘进机抬升关节与伸缩关节之间的高度差；a1为掘进机回转台中心与抬升关节之间水平距离，a2为掘进机抬升关节与伸缩关节之间的距离，a3为掘进机伸缩关节与截割头之间的水平距离。

本发明的步骤二中确定截割臂相对巷道的实时位姿

具体包括：

步骤2.1：通过视觉测量的方法确定相机相对于激光指向仪的位姿

步骤2.1.1：提取采集的激光线图像，进行畸变校正和图像预处理。对图像中的激光光斑信息进行特征提取，借助工业CCD相机获取激光光斑区域，结合高斯曲面拟合方法准确获取激光光斑中心。

步骤2.1.2：激光束直线特征提取，利用Hough变换将图像中检测到图像变换到参数空间，进行直线粗定位，再结合最小二乘法实现更精确的直线拟合，获得直线方程；

步骤2.1.3：借助步骤2.1.2提取到的两激光线方程，结合两光斑中心点连线所在直线，依据这已知的三条直线，建立基于两平行激光束的掘进机位姿解算模型。依据摄像机成像原理，确定相机相对于激光指向仪的位姿

步骤2.2：本发明

获取，通过全站仪测量直接得到。

步骤2.3：

的计算，通过标定获取，本发明的相机，包括两个(后置和前置)，分别对后置相机和前置相机进行外参标定，其中后置相机在标定过程中采用全站仪进行辅助标定；这一步骤为针对实际情况所采用的标定方法，目的为确定相机和机身位置之间的相对关系。具体包括：

步骤2.3.1：利用全站仪测量获取机身相对于巷道的初始位姿

步骤2.3.2：根据已经获得的相机相对于激光指向仪的位姿关系

和已经获得的激光指向仪在巷道中的位姿

和步骤2.3.1获得的机身相对于巷道的初始位姿

通过式(2)计算得到后置相机相对于机身的外参矩阵

完成后置相机外参的标定。

步骤2.4：

的获取：

步骤2.4.1：由相机内参标定方法求得标靶坐标系到摄像机坐标系的转换矩阵

根据在悬臂式掘进机上的相对安装位置确定标靶坐标系到截割头坐标系的转换矩阵

步骤2.4.2：确定掘进机截割头坐标系到掘进机基坐标系的转换矩阵

利用倾角传感器获取截割臂垂直方向的角度信息，利用重锤将掘进机的截割头摆动到掘进机的中线位置。即为截割臂水平摆角θ₁＝0°，截割臂垂直摆角θ₂＝-90°带入即可确定该位置下的

步骤2.4.3：通过式(3)计算得到掘进机基座标系到相机坐标系的转换矩阵

使用摄像机采集标靶的图片并进行处理和计算，根据上述摄像机外参标定的方法对摄像机进行外参标定，为保证计算的经确定，设定摄像机采集图片间隔为90ms，并连续对15张图片进行处理，得到标靶相对于相机的位姿，并求平均值最终得到摄像机的外参。

步骤2.4.4：计算得到机身相对于巷道的位姿

式中，相机相对于激光指向仪的位姿关系

由步骤3获得，激光指向仪在巷道中的位姿

由步骤2.2中全站仪测量得到，后置相机相对于机身的外参矩阵

通过步骤2.3中标定获得。

步骤2.4.5：

的获取。

实施例1：

本实施例提供一种悬臂式掘进机空间位姿自动检测方法，该方法以多点红外LED和两平行激光束为特征进行视觉测量，包括如下步骤：

步骤1：通过视觉测量的方法确定截割头相对于机体的位姿；

1.1采集悬臂式掘进机截割臂的标靶图像；

1.2提取步骤1.1采集的标靶图像中的光斑，借助区域分割法对光斑进行排序、最小二乘法进行直线拟合，得到两两相交的四条直线并将每两条直线的交点作为特征点；利用四点透视成像方法得到所有特征点在摄像机坐标系中的坐标；

如图6为对光斑进行排序后的图像，对图6中的光斑进行直线拟合，可以得到两两相交的四条直线，如图7所示，相机采集到标靶的图像，经步骤1.2所得特征点，确定标靶四个顶点，即四个共面的特征点P₁～P₄。进一步得到像面坐标系中的点为C₁～C₄，经图像处理得到像素坐标，利用P4P四点透视成像方法计算得到P₁～P₄在摄像机像素坐标系下的坐标

步骤1.3，根据特征点在标靶坐标系中的坐标以及步骤1.2得到特征点在摄像机坐标系中坐标，再结合对偶四元数误差模型，根据特征点在标靶坐标系中坐标，计算截割头的水平摆角θ1和截割头垂直摆角θ2；

根据特征点在标靶坐标系中坐标及步骤1.2所得特征点在像素坐标系中坐标，借助对偶四元数误差模型得到截割头的水平摆角θ₁和截割头垂直摆角θ₂；通过本申请的仿真实验获得掘进机在该时刻截割头水平摆角-84.12°和截割头垂直摆角为-113.634°。

1.4根据求得的θ1和θ2，计算截割头相对掘进机机体的位姿

其中，θ1为截割头水平摆角，θ2为截割头垂直摆角，d为油缸的伸缩距离，b1为掘进机抬升关节与伸缩关节之间的高度差；a1为掘进机回转台中心与抬升关节之间水平距离，a2为掘进机抬升关节与伸缩关节之间的距离，a3为掘进机伸缩关节与截割头之间的水平距离；

步骤二，确定截割臂相对巷道的实时位姿

其中，

为采用视觉测量方法确定的悬臂式掘进机机体相对于巷道的位姿。

为采用视觉测量方法确定的悬臂式掘进机机体相对于巷道的位姿具体包括：

2.1通过视觉测量的方法确定相机相对于激光指向仪的位姿

2.1.1：提取采集的激光线图像，进行畸变校正和图像预处理。对图像中的激光光斑信息进行特征提取，借助工业CCD相机获取激光光斑区域，结合高斯曲面拟合方法准确获取激光光斑中心。

步骤2.1.2：激光束直线特征提取，利用Hough变换将图像中检测到图像变换到参数空间，进行直线粗定位，再结合最小二乘法实现更精确的直线拟合，获得直线方程；

步骤2.1.3：借助步骤2.1.2提取到的两激光线方程，结合两光斑中心点连线所在直线，依据这已知的三条直线，建立基于两平行激光束的掘进机位姿解算模型。依据三线透视方法，确定相机相对于激光指向仪的位姿

具体包括：建立基于两平行激光束的掘进机位姿解算模型，其中两点为激光指向仪的光斑中心点，为图1中P₁、P₂点。三线分别指激光指向仪的两条激光线L₁、L₃两条线和量光斑中心连线的L₂直线，其中L₁、L₃两条线平行，L₂垂直于L₁、L₃。l₁、l₂、l₃为L₁、L₂、L₃在图像上的投影，q₁、q₂为P₁、P₂在图像上的投影。根据三线透视问题的闭式解计算得到相机坐标系O_cX_cY_cZ_c相对于激光定向坐标系O_dX_dY_dZ_d之间的坐标变换的唯一解，即旋转矩阵R和平移向量T。即得到相机相对于激光指向仪的位姿

步骤2.2：分别对后置相机和前置相机进行外参标定，其中后置相机在标定过程中采用全站仪进行辅助标定；

步骤2.2.1：利用全站仪测量获取激光指向仪在巷道中的位姿

步骤2.2.2：利用全站仪测量获取机身相对于巷道的初始位姿

具体的，用全站仪进行激光指向仪相对于巷道、机身相对于巷道的位姿解算，需要在激光指向仪、机身上设置三个棱镜放置点(如图3，标五角星的位置为三个棱镜放置点，位于坐标原点处的棱镜距离其他两个棱镜的水平距离为1m)，借助三个棱镜放置点的坐标计算得到其绝对位姿，具体的步骤如下所示：

用全站仪分别测量掘进机机身上固定的三个棱镜的坐标点分别记为(x1，y1，z1)，(x2，y2，z2)，(x3，y3，z3)。由下式(4)可依次求出机身相对巷道的航向角α、俯仰角β以及横滚角γ，即得到旋转矩阵R：

另外机身相对巷道的平移矩阵T直接可由掘进机中心所测得的棱镜坐标表示。

其中，使用摄像机采集标靶的图片并进行处理和计算，根据上述摄像机外参标定的方法对摄像机进行外参标定，为保证计算的经确定，设定摄像机采集图片间隔为90ms，并连续对15张图片进行处理，得到标靶相对于相机的位姿，并求平均值最终得到摄像机的外参。

实施例2：

本实施例提供一种悬臂式掘进机空间位姿自动检测系统，该系统中所述巷道顶板上设有两条相互平行的红色激光线，该系统包括：

通信模块：用于采集激光指向仪的图像、掘进机截割臂上标靶图像以及掘进机运行状态信息，掘进机运行状态信息包括机身距离前煤壁的距离。

视觉测量模块：用于测量相机相对于激光指向仪(定位模型)以及相机相对于标靶的位姿，具体包括：

机身视觉测量：提取通信模块采集的激光束图像，采用滤镜滤除杂光后对图像进行去噪、增强和畸变矫正，利用Hough直线检测算法+最小二乘法提取激光束直线特征；

建立掘进机机身位姿解算模型，根据直线方程得到防爆相机在定位模型坐标系下的转换关系

其中，α1、β1、γ1分别为相机坐标系在定位模型(激光指向仪)坐标系下绕x轴、y轴、z轴旋转的角度，X1、Y1、Z1分别为相机坐标系在定位模型(激光指向仪)坐标系下的x、y、z三个方向的位置。

截割头视觉测量：提取采集的标靶图像中的光斑，借助区域分割法对光斑进行排序、最小二乘法进行直线拟合，得到两两相交的四条直线，利用摄像机成像理论确定所有特征点在摄像机坐标系中的坐标；

处理器，或者说是解算模块，包括：

机身位姿解算模块：用于确定机身相对于巷道的位姿数据，具体过程包括：

利用全站仪测量激光指向仪在巷道中的位姿

机身相对于巷道的初始位姿

以及通过视觉测量的方法确定相机相对于激光指向仪的转换矩阵

计算得到后置相机相对于机身的外参矩阵

再结合相机相对于激光指向仪的转转换关系

以及激光指向仪相对于巷道的转换关系

计算得到机身相对于巷道的位姿

得到掘进机机身坐标系与巷道坐标系之间的转换关系之后，可以计算得到任意时刻机身在巷道的位置。

截割头位姿解算模块：用于确定截割头相对于巷道断面的位姿数据，借助截割头视觉测量系统，通过下式得到截割头相对于巷道断面的位姿数据

其由

为截割头相对于掘进机机体的位姿，

为掘进机相对于巷道的位姿，掘进机相对于巷道的位姿可由掘进机机身视觉测量模块获得。

还包括位姿显示模块：用于显示掘进机在巷道中位姿及截割头的截割轨迹，将机身位姿测量模块所得的掘进机在巷道中的位姿数据、截割头位姿解算模块所得到的截割头相对巷道断面的位姿数据与上位机进行数据交互，在上位机图像化界面上进行显示，以实现掘进机的空间位姿自动检测。

仿真实验：

仿真实验平台由悬臂式掘进机、防爆计算机、防爆工业相机、红外LED标靶、两激光指向仪和放置在机身前侧的两超声测距传感器、惯性导航构成，视觉测量方法通过单目视觉测量技术，分别用前置、后置工业相机拍摄红外LED特征和两平行激光束，将采集到的图像传输到机载计算机中，分别对图像进行特征提取，根据摄像机成像原理确定所有特征点在掘进机坐标系中的坐标；使用捷联惯导传感器获得掘进机在工作过程中掘进机机身的位姿、使用安装在掘进机机身前侧的激光测距传感器测量机身距离前煤壁的距离；将采集到的掘进机运行状态信息传输至机载计算机，将各传感器的数据处理并计算，存入数据库。掘进机位姿测量过程包括：

在仿真实验中，掘进机在截割过程中采集到的掘进机空间位姿为：截割头的横滚角γ为1.019°，俯仰角β为0.750°，偏航角α为-16.164°，掘进机机身距离左侧煤壁32.1cm，掘进机距离前侧煤壁为785.961cm。如图4所示的原点为表示截割头；如图5所示，图上的截割头覆盖部分即为截割头的行走路径。从图5可以看出，当掘进机行走到此位置时，各传感器实时采集的截割头的俯仰角β为0.651°，偏航角α为9.547°，掘进机机身距离左侧煤壁47.3cm，距离煤壁前侧为806.992cm。

如图6所示，相机采集到标靶的图像，经步骤1.2所得特征点，确定标靶四个顶点，即四个共面特征点P₁～P₄。进一步得到像面坐标系中的点为C₁～C₄，经图像处理得到像素坐标，利用P4P计算方法得到P₁～P₄在像素坐标系下的坐标

根据特征点在标靶坐标系中坐标及步骤1.2所得特征点在像素坐标系中坐标，借助对偶四元数误差模型得到截割头的水平摆角θ₁和截割头垂直摆角θ₂。

设定掘进机的初始状态为：截割头水平摆角为0°，垂直摆角为-90°。在掘进机实际运行过程中，测量得到掘进机运行过程中某一时刻截割头抬升23.051°，向右转动-84.61°；通过本仿真实验获得掘进机在该时刻截割头水平摆角-84.12°和截割头垂直摆角为-113.634°，则截割头水平摆角和截割头垂直摆角分别变化为15.562°及23.634°，与实际测量的得到的截割头的抬升角度和向右转动角度很接近，说明本发明的方法和系统具有很高的精确度。

Claims (6)

1.一种悬臂式掘进机空间位姿自动检测方法，用于实现悬臂式掘进机截割臂相对巷道的实时位姿检测，其特征在于，包括以下步骤：

步骤一，计算悬臂式掘进机截割臂相对于悬臂式掘进机机体的位姿；

步骤1.1，采集悬臂式掘进机截割臂的标靶图像；

步骤1.2，提取步骤1.1得到标靶图像的光斑，用区域划分法将所有光斑排序并采用最小二乘法进行直线拟合，得到四条两两相交的直线，将每两条直线的交点作为特征点；

步骤1.3，根据特征点在标靶坐标系中的坐标，采用对偶四元数误差模型计算截割臂相对于机体的水平摆角θ1和垂直摆角θ2；

其中，所述标靶坐标系是指以标靶图像的中心为原点，X轴平行于标靶上下边指向右，Y平行于标靶左右边指向下，Z轴垂直于标靶图像平面；

步骤1.4，根据θ1和θ2确定截割臂相对于悬臂式掘进机机体的位姿

其中，d为悬臂式掘进机油缸的伸缩距离，b1为悬臂式掘进机抬升关节与伸缩关节之间的高度差；a1为悬臂式掘进机回转台中心与抬升关节之间水平距离，a2为悬臂式掘进机抬升关节与伸缩关节之间的距离，a3为悬臂式掘进机伸缩关节与截割臂之间的水平距离；

步骤二，确定截割臂相对巷道的实时位姿

其中，

为采用视觉测量方法确定的悬臂式掘进机机体相对于巷道的位姿。

2.如权利要求1所述的悬臂式掘进机空间位姿自动检测方法，其特征在于，步骤1.2包括，提取步骤1.1得到标靶图像的光斑，用区域划分法将所有光斑排序并采用最小二乘法进行直线拟合，得到四条两两相交的直线，将每两条直线的交点作为特征点；利用四点透视成像方法得到所有特征点在摄像机坐标系中的坐标；

步骤1.3，根据特征点在标靶坐标系中的坐标以及步骤1.2得到特征点在摄像机坐标系中坐标，采用对偶四元数误差模型计算截割臂相对于机体的水平摆角θ1和垂直摆角θ2；

所述摄像机坐标系是指以摄像机的光心为原点，X轴沿水平方向，Y轴沿竖直方向，Z轴垂直Y、X轴指向标靶方向，所述摄相机设置在悬臂式掘进机机体上部两侧。

3.一种悬臂式掘进机空间位姿自动检测系统，该系统用于实现悬臂式掘进机截割臂相对巷道的实时位姿检测，其特征在于，包括：

通信模块，包括相机，用于采集悬臂式掘进机截割臂上的标靶图像以及悬臂式掘进机运行状态信息，所述相机设置在悬臂式掘进机机体上部两侧；所述悬臂式掘进机运行状态信息包括悬臂式掘进机油缸的伸缩距离，悬臂式掘进机抬升关节与伸缩关节之间的高度差，悬臂式掘进机回转台中心与抬升关节之间水平距离，悬臂式掘进机抬升关节与伸缩关节之间的距离，悬臂式掘进机伸缩关节与截割臂之间的水平距离以及悬臂式机体距离前煤壁的距离；

视觉测量模块，用于测量悬臂式掘进机机体相对于巷道的位姿，以多点红外LED和两条平行激光束为特征进行视觉测量，包括红外LED标靶和两个激光指向仪，红外标靶设置在截割臂上部，用于提取相机采集的标靶图像的光斑；两个激光指向仪设置在巷道内上部，用于沿着巷道顶板发出两条相互平行的红色激光束；所述相机还用于采集激光指向仪的激光束图像；

处理器，存储有计算机程序，用于按照权利要求1中步骤一的内容计算悬臂式掘进机截割臂相对于悬臂式掘进机机体的位姿以及按照权利要求1中步骤二的内容计算截割臂相对于巷道的实时位姿。

4.按照权利要求3所述悬臂式掘进机空间位姿自动检测系统，其特征在于，该系统还包括位姿显示模块，用于将悬臂式掘进机截割臂以及机体相对巷道的的位姿进行实时图形化界面显示。

5.按照权利要求3所述悬臂式掘进机空间位姿自动检测系统，其特征在于，该系统同于还用于实现悬臂式掘进机机体相对于巷道的位姿检测，机体相对于巷道的位姿

的计算如下：

式中，

为相机相对于激光指向仪的位姿；

为激光指向仪在巷道中的位姿，通过全站仪测量得到；

为相机相对于机体的外参矩阵，通过相机外参标定得到；

其中，

的计算步骤如下：

分别提取相机采集的两个激光束图像，对激光束图像中的激光光斑信息分别进行特征提取得到两个光斑区域并采用最小二乘法进行直线拟合，分别获得两个直线方程；同时通过两个光斑区域分别获得两个光斑中心点；通过两个光斑中心点的连线结合两个直线方程建立悬臂式掘进机位姿解算模型，确定相机相对于激光指向仪的位姿

6.按照权利要求5所述悬臂式掘进机空间位姿自动检测系统，其特征在于，确定相机相对于激光指向仪的位姿

具体计算如下：

其中，α1、β1、γ1分别为相机坐标系在激光指向仪坐标系下绕x轴、y轴、z轴旋转的角度，X1、Y1、Z1分别为相机坐标系在激光指向仪坐标系下x轴、y轴、z轴三个方向的分量；所述激光指向仪坐标系的原点在两个激光指向仪连线的中心位置；所述相机坐标系的原点在相机质心位置，激光指向仪坐标系与相机坐标系的各个坐标轴同向。

Images