矿用悬臂式掘进机可视化辅助导引方法和系统与流程

本发明属于矿用掘进机技术领域，具体涉及一种矿用悬臂式掘进机可视化辅助导引方法和系统。

背景技术：

悬臂式掘进机被广泛地应用于煤矿巷道、地铁隧道以及铁路工程、洞穴的掘进施工中，但是，悬臂式掘进机截割煤岩时，由于司机所处位置的原因，司机只能看到一侧的截割，另一侧往往需要有一位副司机专门看着并用灯光指示司机截割，同时，由于截割产尘和喷雾洒水等原因，导致巷道形成质量不好，当巷道断面为半圆拱形或三心拱形时，司机更是难以把握。目前巷道的掘进工艺主要有两种，一是靠司机的经验把握，另一种就是根据位移传感器测量油缸的伸缩量进行计算得到截割头的位姿。第一种方式不可靠，容易出现超挖欠挖；第二种方式相较于第一种有了很大的提升，但是，由于井下环境非常复杂和振动的影响较大，易导致接触式传感器接触不良，使得检测结果可靠性大大降低。

技术实现要素：

针对现有技术存在的缺点，本发明的提供了一种矿用悬臂式掘进机可视化辅助导引方法和系统，解决现有的悬臂式掘进机工作过程中受人为和环境因素的影响难以保证巷道掘进效率和质量的问题。

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

矿用悬臂式掘进机可视化辅助导引方法，该方法中的掘进机悬臂上设置有标靶，标靶的边上设置有多个光源点，该方法包括以下步骤：

步骤1：采集掘进机运行状态信息以及掘进机悬臂上的标靶图像，所述的掘进机运行状态信息包括掘进机机身的横滚角、俯仰角、偏航角、掘进机机体在巷道中的位置以及掘进机机身与两侧煤壁的距离、掘进机机身距离前煤壁的距离；

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

步骤2.1：提取步骤1采集的标靶图像中的光斑，对光斑进行排序、直线拟合，将每两条直线的交点作为特征点；

步骤2.2：利用四点透视成像方法确定所有特征点在摄像机坐标系中的坐标；

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

步骤2.4：根据步骤2.3确定的θ1和θ2，通过式(1)计算截割头相对于掘进机机体的位姿

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

步骤3：根据步骤1获得的掘进机运行状态信息，通过式(2)得到掘进机相对于巷道的位姿

式中，γ为掘进机机身的横滚角，β为掘进机机身的俯仰角，α为掘进机机身的偏航角，d1为机身到侧方煤壁的距离，l为掘进机到前煤壁的距离；d3为掘进机机身宽度，b2为抬升关节与地面之间的距离；

步骤4：根据步骤2得到的截割头相对于机体位姿数据和步骤3得到的掘进机机身相对于巷道断面的位姿数据，通过式(3)可得到截割头相对于巷道断面的位姿

步骤5：根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、走向以及截割工艺确定掘进机截割头的导引轨迹，并将导引轨迹在上位机上进行图形化界面显示；

步骤6：将步骤4得到截割头相对于巷道断面的位姿数据与上位机进行数据交互，将截割头在上位机上图形化界面显示，并根据步骤5确定的导引轨迹进行截割头移动过程中的轨迹导引。

具体的，所述的步骤2.3中截割头水平摆角θ1和截割头垂直摆角θ2的计算过程为：

步骤2.3.1：利用对偶四元数误差模型构建矩阵a，式(4)所示，

式中，q(pi^w)表示第i个特征点在标靶坐标系下坐标的正交矩阵；w(pi^c)表示第i个特征点在摄像机坐标系下坐标的正交矩阵；表示第i个特征点在标靶坐标系中的坐标，表示第i个特征点在摄像机坐标系下的坐标，i＝1,2,...,n，n表示特征点的个数；

当矩阵a的特征值最大时，模型的误差最小，求取此时矩阵a的特征向量r，其中，表示特征向量r里的四个元素；

步骤2.3.2：根据步骤2.3.1得到的特征向量r，通过式(5)确定摄像机坐标系与标靶坐标系之间的旋转矩阵r，

步骤2.3.3：根据步骤2.3.2获得的旋转矩阵r，通过式(6)中相等矩阵对应元素相等原理计算得到θ1和θ2，

式中，r为步骤2.3.2计算得到的旋转矩阵，r^*为θ1＝0°、θ2＝-90°时，根据步骤2.3.2计算的特征点在摄像机坐标系下的坐标所对应的旋转矩阵r。

具体的，所述的步骤1中，利用捷联惯导传感器获取悬臂式掘进机机体的横滚角、俯仰角和偏航角以及掘进机机体在巷道中的位置信息；利用超声波传感器测量掘进机机身到两侧煤壁的距离和掘进机机身到前煤壁的距离。

本发明还公开了矿用悬臂式掘进机可视化辅助导引系统，该系统中所述的掘进机悬臂上设置有标靶，标靶的边上设置有多个光源点，该系统包括：

通信模块，采集掘进机运行状态信息以及掘进机悬臂上的标靶图像，所述的掘进机运行状态信息包括掘进机机身的横滚角、俯仰角、偏航角以及掘进机机身与两侧煤壁的距离、掘进机机身距离前煤壁的距离；

视觉测量模块，用于测量截割头相对于掘进机机体的位姿，具体包括：

提取通信模块采集的标靶图像中的光斑，对光斑进行排序、直线拟合，将每两条直线的交点作为特征点；

利用四点透视成像原理确定所有特征点在摄像机坐标系中的坐标；

利用对偶四元数的误差模型，根据特征点在标靶坐标系中的坐标以及特征点在摄像机坐标系中的坐标，计算截割头的水平摆角θ1和截割头垂直摆角θ2；

根据θ1和θ2，通过式(1)计算截割头相对于掘进机机体的位姿

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

掘进机位姿检测模块，用于掘进机机身相对于巷道的位姿数据，根据通信模块获得的掘进机运行状态信息，利用公式(2)得到掘进机机身相对于巷道的位姿

式中，γ为掘进机机体的横滚角，β为掘进机机体的俯仰角，α为掘进机机体的偏航角，d1为机身到侧方煤壁的距离，l为掘进机到前煤壁的距离；d3为掘进机机身宽度，b2为抬升关节与地面之间的距离；

数据处理模块，用于确定截割头相对于巷道断面的位姿数据，根据视觉测量模块得到的截割头相对于机体位姿数据和掘进机位姿检测模块得到的掘进机机身相对于巷道的位姿数据，通过式(3)得到截割头相对于巷道断面的位姿数据

轨迹显示模块，用于显示掘进机截割头的导引轨迹，根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、走向以及截割工艺确定掘进机截割头的导引轨迹，并将导引轨迹在上位机上进行图形化界面显示；

轨迹导引模块，用于截割头移动过程中的轨迹导引，将数据处理模块得到的截割头相对于巷道断面的位姿数据与上位机进行数据交互，将截割头在上位机上图形化界面显示，并根据轨迹显示模块确定的导引轨迹进行截割头移动过程中的轨迹导引。

具体的，所述的视觉测量模块中截割头的水平摆角θ1和截割头垂直摆角θ2的确定过程为：

利用对偶四元数误差模型构建矩阵a，如式(4)所示，

式中，q(pi^w)表示第i个特征点在标靶坐标系下坐标的正交矩阵；w(pi^c)表示第i个特征点在摄像机坐标系下坐标的正交矩阵；表示第i个特征点在标靶坐标系中的坐标，表示第i个特征点在摄像机坐标系下的坐标，i＝1,2,...,n，n表示特征点的个数；

当矩阵a的特征值最大时，模型的误差最小，求取此时矩阵a的特征向量r，其中，表示特征向量r里的四个元素；

根据得到的特征向量r，通过式(5)确定摄像机坐标系与标靶坐标系之间的旋转矩阵r，

根据获得的旋转矩阵r，通过式(6)中相等矩阵对应元素相等原理计算得到θ1和θ2，

式中，r为旋转矩阵，r^*为θ1＝0°、θ2＝-90°时特征点在摄像机坐标系下的坐标所对应的旋转矩阵r。

具体的，所述的通信模块中，利用捷联惯导传感器获取悬臂式掘进机机体的横滚角、俯仰角和偏航角以及掘进机机体在巷道中的位置信息；利用超声波测距传感器和激光测距传感器测量机身到两侧煤壁的距离和掘进机到前煤壁的距离。

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

(1)本发明的通过视觉测量的方法计算出的掘进机机身和截割头空间实时位置，并实现图形化的界面实时显示，可实时了解截割头运动的真实移动轨迹，解决了煤矿井下掘进机工作环境粉尘大、噪音大，操作人员难以掌握掘进断面具体状况。掘进机司机可以通过界面显示以及构建的截割路线的提示，完成可视化的截割轨迹引导，即保证截割质量的要求，同时也避免巷道超挖和欠挖得发生，同时大大提高了巷道掘进的工作效率及工作质量。

(2)本发明采用视觉测量的方法测量截割头相对于掘进机机体的位姿，并结合捷联惯性导航技术进行组合定位确定截割头在巷道中的位姿，提高了位姿检测精度。

附图说明

图1为四点透视成像方法原理图。

图2为掘进机各个坐标系示意图。

图3为本发明的视觉测量流程图。

图4为本发明的系统框图。

图5为仿真实验中掘进机及导引轨迹的可视化界面显示图。

图6为仿真实验中掘进机可视化截割过程可视化界面显示图。

具体实施方式

本发明的标靶一般是指方形板，并在方形板的四个边上均匀设置有16个红外光源，所述的光斑即为光源在图像中形成的点。

本发明中的摄像机坐标系是指以摄像机的光心为原点x轴沿水平方向，y轴沿竖直方向，z轴垂直y、x轴指向标靶方向。标靶坐标系是指以标靶中心为原点，x轴平行于标靶上下边指向右，y平行于标靶左右边指向下，z轴垂直于标靶平面。

本发明的矿用悬臂式掘进机可视化辅助导引方法，该方法中的掘进机悬臂上设置有标靶，标靶的边上设置有多个光源点，该方法包括以下步骤：

步骤1：采集掘进机运行状态信息以及掘进机悬臂上的标靶图像，具体，使用安装在掘进机机身上的摄像机采集安装在掘进机悬臂上的标靶图像，使用捷联惯导传感器获得掘进机在工作过程中掘进机机身的位姿，即横滚角、俯仰角、偏航角，以及掘进机机体在巷道中的位置信息，使用安装在机身两侧超声波传感器获得掘进机与两侧煤壁的距离、使用安装在掘进机机身前侧的激光测距传感器测量机身距离前煤壁的距离。利用捷联惯导获得的位置信息确定掘进机的地理位置，以确保掘进机在工作时的方向，防止巷道打偏；再利用横滚角、俯仰角、偏航角以及超声波等传感器确定掘进机相对于巷道的位置信息，进行精确定位。

步骤2：确定截割头相对于掘进机机体的位姿数据；

步骤2.1：对采集的标靶图像进行降噪处理，优选的使用高斯滤波进行降噪，提取降噪后图像上每个光斑的中心点，利用最小二乘法对标靶图像上的每个边上的光斑点进行拟合，将每两条直线的交点作为特征点；

步骤2.2：利用四点透视成像方法(即p4p)确定所有特征点在摄像机坐标系中的坐标，具体包括：将步骤2.1得到的特征点的坐标为像素坐标，将像素坐标转换到图像坐标系下，如图1所示为共面p4p原理图，像素坐标系与图像坐标系转换关系如下：

其中，(x^p,y^p)表示图像坐标系下的坐标，(u0,v0)为图像坐标系的原点在像素坐标系下的坐标，(u,v)表示像素坐标系下的坐标；

在不考虑成像畸变的情况下，图像坐标系与摄像机坐标系之间的关系：

其中，f为摄像机的有效焦距，(x^c,y^c,z^c)表示摄像机坐标系下的坐标；根据以上的转换关系便可以将像素坐标系下的特征点的坐标转换得到在摄像机坐标系下的坐标；

如图1所示，四个共面的特征点p1～p4为正方形标靶的四个顶点，其在像面坐标系中形成的点为c1～c4，c1～c4的坐标可以通过处理图像得到像素坐标，在利用像素坐标系与像面坐标系的关系(公式(7))求得像面坐标系下坐标再利用p4p方法计算p1～p4在摄像机坐标系中的坐标

步骤2.3：根据特征点在标靶坐标系中的坐标以及步骤2.2得到的特征点在摄像机坐标系中的坐标，计算截割头的水平摆角θ1和截割头垂直摆角θ2，即截割头的空间姿态角；本发明利用对偶四元数的误差模型来确定，进一步的，还可使用基于三维视觉的平行透视模型位姿测量方法，优选对偶四元数模型方法，具体利用对偶四元数的误差模型的计算过程包括：

步骤2.3.1：利用对偶四元数误差模型确定摄像机坐标系与标靶坐标系之间的旋转矩阵r；

①对偶四元数由两部分构成：其中ε为对偶运算符，且对偶运算符满足ε²＝0，ε≠0；

对偶四元数和纯四元数都具有一个相似的表示：

对偶向量表示坐标系绕其旋转和平移的空间曲线，u为单位向量，表明旋转和平移方向；为旋转和平移的对偶角；r和s分别为纯四元数的实部和对偶部，其数学表达式为：r＝[r0r1r2r3]^t，s＝[s0s1s2s3]^t；

由对偶四元数的性质可知：r^ts＝s^tr＝0，r^tr＝1。

两坐标系间的旋转矩阵和平移量r和t可以表示为

其中，q(r)、w(r)为正交矩阵

②基于误差模型的最优解

由坐标系之间的转换可得其中为特征点在在摄像机坐标系中的理论坐标，为特征点在标靶坐标系中的坐标，t为标靶坐标系到摄像机坐标的平移量。

在实际计算中，由于提取中心点误差、计算误差等导致特征点在摄像机坐标系中的理论坐标与实际坐标之间存在误差

通过式和建立误差模型：

式中，n为特征点个数；

由式以及式(9)和式(10)可得：

其中，

式中，q(pi^w)表示第i个特征点在标靶坐标系下坐标的正交矩阵；w(pi^c)表示第i个特征点在摄像机坐标系下坐标的正交矩阵；表示第i个特征点在标靶坐标系中的坐标，为第i个特征点在摄像机坐标系下的坐标，i＝1,2,...,n，n表示特征点的个数；

利用对偶四元数的约束条件构造拉格朗函数：

l(r，s，λ1，λ2)＝r^tg1r+s^tg2r+g3+4ns^ts+λ1(r^tr-1)+λ2s^t

其中，λ1和λ2为拉格朗日乘数。

分别对r、s求偏导并计算可以求出λ2＝0，且

把式(15)代入式(14)中，得指标函数

f(r,s)＝(g3-λ)/n(15)

设定λ表示矩阵a的特征值，要使得指标函数f(r,s)最小，即误差最小，则λ最大；求取此时矩阵a的特征向量r，即对偶四元数的实部，

步骤2.3.2：根据确定的特征向量r，通过式(5)即可摄像机坐标系与标靶坐标系之间的旋转矩阵r，

步骤2.3.3：根据步骤2.3.2获得的旋转矩阵r，根据相等矩阵对应元素相等原理，通过式(6)中计算得到θ1和θ2，

式中，r为步骤2.3.2计算得到的旋转矩阵，r^*为θ1＝0°、θ2＝-90°时根据步骤2.3.2计算的特征点在摄像机坐标系下的坐标所对应的旋转矩阵r。一般通过给定θ1和θ2的方法确定r^*，本发明通过设定θ1＝0°、θ2＝-90°即掘进机悬臂水平且位于机体中间的位置，确定了该条件下特征点在摄像机坐标系下的坐标再根据步骤步骤2.3.2计算出当前位置时摄像机坐标系与标靶坐标系之间的旋转矩阵r，设此时的r为r^*。

步骤2.4：对悬臂式掘进机进行运动学分析，如图2所示建立悬臂式掘进机坐标系，基坐标系o0x0y0z0的原点与截割臂抬升关节的旋转轴线在同一高度，x0轴在掘进机的中轴线上，z0轴在回转台的旋转轴线上；回转关节坐标系o1x1y1z1的原点与基坐标系原点重合；抬升关节坐标系o2x2y2z2的原点在截割臂抬升关节的中心，z2轴在截割臂抬升关节的轴线上；伸缩关节坐标系o3x3y3z3的原点在伸缩油缸前关节的中心，y3轴在关节的轴线上，z3轴在油缸活塞杆的轴线上；截割头坐标系o4x4y4z4的原点在截割头最小截割半径的中心，z4轴在截割头锥台的轴线上；摄像机坐标系ocxcyczc以摄像机的光心为原点；标靶坐标系owxwywzw以正方形标靶中心为原点，zw轴垂直于红外标靶；巷道断面坐标系oxxxyxzx以巷道断面的左下角点为原点，zx坐标轴垂直地面，yx坐标轴平行与巷道地面水向左。根据建立的坐标系得到截割头到掘进机机体的变化矩阵，即式(1)；再根据步骤2.3确定的θ1和θ2，计算截割头相对于掘进机机体的位姿数据

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

步骤3：根据步骤1获得的掘进机运行状态信息，通过式(2)得到掘进机机身相对于巷道的位姿数据

式中，γ为掘进机机身的横滚角，β为掘进机机身的俯仰角，α为掘进机机身的偏航角，d1为机身到侧方煤壁的距离，l为掘进机到前煤壁的距离；d3为掘进机机身宽度，b2为抬升关节与地面之间的距离；

步骤4：融合惯性导航与视觉测量技术进行组合定位，即根据步骤2得到的截割头相对于机体位姿数据和步骤3得到的掘进机机身相对于巷道断面的位姿数据，通过式(3)可得到截割头相对于巷道断面的位姿数据

步骤5：根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、走向以及截割工艺(即截割路径组合方式)确定掘进机截割头的导引轨迹，并将导引轨迹在上位机上进行图形化界面显示；

步骤6：将步骤4得到截割头相对于巷道断面的位姿数据与上位机进行数据交互，将截割头在上位机上图形化界面显示，并根据步骤5确定的导引轨迹进行截割头移动过程中的轨迹导引。可用于获取超挖欠挖及进尺查询和超挖欠挖报警等功能。

本发明还公开了矿用悬臂式掘进机可视化辅助导引系统，该系统包括：

通信模块，用于采集掘进机运行状态信息以及掘进机悬臂上的标靶图像，其中，掘进机运行状态信息包括掘进机机身的横滚角、俯仰角、偏航角以及掘进机机身与两侧煤壁的距离、掘进机机身距离前煤壁的距离；

视觉测量模块，用于测量截割头相对于掘进机机体的位姿，具体包括：

提取通信模块采集的标靶图像中的光斑，对光斑进行排序、直线拟合，将每两条直线的交点作为特征点；

利用四点透视成像原理确定所有特征点在摄像机坐标系中的坐标，具体包括：利用像素坐标系与像面坐标系的关系(公式(7))求得像面坐标系下坐标再利用p4p方法计算p1～p4在摄像机坐标系中的坐标

利用对偶四元数的误差模型，根据特征点在标靶坐标系中的坐标以及特征点在摄像机坐标系中的坐标，计算截割头的水平摆角θ1和截割头垂直摆角θ2，具体包括：

利用对偶四元数误差模型构建矩阵a，式(4)所示，

式中，q(pi^w)表示第i个特征点在标靶坐标系下坐标的正交矩阵；w(pi^c)表示第i个特征点在摄像机坐标系下坐标的正交矩阵；表示第i个特征点在标靶坐标系中的坐标，表示第i个特征点在摄像机坐标系下的坐标，i＝1,2,...,n，n表示特征点的个数；

当矩阵a的特征值最大时，模型的误差最小，求取此时矩阵a的特征向量r，其中，表示特征向量r里的四个元素；

然后根据得到的特征向量r，通过式(5)确定摄像机坐标系与标靶坐标系之间的旋转矩阵r，

最后根据获得的旋转矩阵r，通过式(6)中相等矩阵对应元素相等原理计算得到θ1和θ2，

式中，r为步骤2.3.2计算得到的旋转矩阵，r^*为θ1＝0°、θ2＝-90°时，根据步骤2.3.2计算的特征点在摄像机坐标系下的坐标所对应的旋转矩阵r。

根据θ1和θ2，通过式(1)计算截割头相对于掘进机机体的位姿数据

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

掘进机位姿检测模块，用于掘进机机身相对于巷道的位姿数据，根据通信模块获得的掘进机运行状态信息，利用公式(2)得到掘进机机身相对于巷道的位姿数据

式中，γ为掘进机机体的横滚角，β为掘进机机体的俯仰角，α为掘进机机体的偏航角，d1为机身到左侧煤壁的距离，l为掘进机到前煤壁的距离；d3为掘进机机身宽度，b2为抬升关节与地面之间的距离；

数据处理模块，用于确定截割头相对于巷道断面的位姿数据，根据视觉测量模块得到的截割头相对于机体位姿数据和掘进机位姿检测模块得到的掘进机机身相对于巷道的位姿数据，通过式(3)得到截割头相对于巷道断面的位姿数据

轨迹显示模块，用于显示掘进机截割头的导引轨迹，根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、走向以及截割工艺确定掘进机截割头的导引轨迹，并将导引轨迹在上位机上进行图形化界面显示；

轨迹导引模块，用于截割头移动过程中的轨迹导引，将数据处理模块得到的截割头相对于巷道断面的位姿数据与上位机进行数据交互，将截割头在上位机上图形化界面显示，并根据轨迹显示模块确定的导引轨迹进行截割头移动过程中的轨迹导引。

仿真实验：

本仿真实验中掘进机悬臂上的标靶为正方形标靶，标靶的四个边上均匀设置有16个红外光源灯，掘进机引导过程包括：

步骤1：根据可视化辅助截割的需要，对掘进机运行状态信息进行采集，使用捷联惯导传感器获得掘进机在工作过程中掘进机机身的位姿、使用安装在机身两侧超声波传感器获得掘进机与两侧煤壁的距离、使用安装在掘进机机身前侧的激光测距传感器测量机身距离前煤壁的距离、以及使用防爆摄像机安装在掘进机机身上的方形标靶进行图像采集；将获得的数据通过串口通信传输到机载计算机中，将各传感器的数据处理与计算并存入数据库。在本仿真实验中，掘进机截割头在轨迹引导的初始阶段，各传感器实时采集的截割头的横滚角γ为0.120°，俯仰角β为-0.069°，偏航角α为0.172°，掘进机机身距离煤矿右侧的距离为32cm。

步骤2：通过视觉测量的方法确定掘进机截割头相对于机体的位姿；具体如图2所示，步骤2.1：利用高斯滤波对提取的图像进行降噪处理，提取降噪后图像中的光斑中心点，对方形标靶上四个边的每个边上的光斑点进行排序、拟合最后确定计算特征点；

步骤2.2：利用四点透视成像方法确定四个特征点在摄像机坐标系中的坐标；

步骤2.3：利用对偶四元数这一数学工具，建立误差模型，建立截割头的位姿误差模型，利用拉格朗日乘子法求最小值的方法，确定相机坐标系与地面坐标系之间的旋转矩阵r；再通过式(6)所示的旋转矩阵r与θ1和θ2的关系，确定了θ1和θ2；

步骤2.4：利用式(1)计算截割头相对于掘进机机体的位姿04t，并存入数据库；

步骤3：根据步骤1中获得的掘进机运行状态信息，通过式(2)计算得到掘进机机身相对于巷道的位姿数据

步骤4：通过式(3)可得到截割头相对于巷道断面的位姿数据

步骤5：根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、走向以及截割工艺(即截割路径组合方式)确定掘进机截割头的导引轨迹，并将导引轨迹在上位机上进行图形化界面显示。如图5所示为掘进机可视化显示图，图中右上部分的弯折线表示导引轨迹，弯折曲线上的原点表示截割头的；图中左上角的视图表示掘进机在巷道中的视图，其中方框表示巷道。

步骤6：将步骤4得到截割头相对于巷道断面的位姿数据与上位机进行数据交互，将截割头在上位机上图形化界面显示，如图5所示的原点为表示截割头；并根据步骤5确定的导引轨迹进行截割头移动过程中的轨迹导引。如图6所示，图上的截割头覆盖部分即为截割头的行走路径。从图6可以看出，当掘进机行走到此位置时，各传感器实时采集的截割头的横滚角γ为0.008°，俯仰角β为-0.013°，偏航角α为0.132°，掘进机机身距离煤矿右侧的距离为21.1cm。

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

本发明通过视觉测量的方法计算出的截割头空间实时位置，并实现图形化的界面实时显示，掘进机司机可以通过界面显示以及构建的截割路线的提示，完成可视化的截割轨迹引导。即保证截割质量的要求，同时也避免巷道超挖和欠挖得发生。在巷道掘进过程中如果超挖和欠挖发生，可视化界面立即做出提示，掘进机司机可以根据情况做出补救措施。另外还可通过查询页面，可以对掘进距离及历史报警情况进行查看。

需要说明的是本发明并不局限于以上具体实方式中，凡在本申请技术方案基础上做的等同变换均落入本发明的保护范围。