一种悬臂式掘进机位姿识别系统及方法与流程

本发明属于采掘设备应用和机器视觉应用技术领域，涉及一种悬臂式掘进机位姿识别系统及方法。

背景技术：

悬臂式掘进机是一种采掘设备，被广泛应用于煤矿井下巷道施工。

煤矿井下悬臂式掘进机控制前行方向的传统方法是人工目视截割断面的激光光斑来指导操作，此方法下操作人员劳动强度大、危险度高，并且人为因素的干扰导致超挖、欠挖的现象时常发生，降低了企业生产效率。因此，煤矿井下悬臂式掘进机自动化、智能化改造势在必行。要实现采掘装备的自动化，就必须有一种可以对悬臂式掘进机位姿进行实时识别的系统，用于实时监测悬臂式掘进机相对于巷道设计轴线的位置和姿态，识别悬臂式掘进机的偏转角、俯仰角、横滚角，竖直偏距和水平偏距，这个系统是悬臂式掘进机自动控制前行方向的基础。目前已有多种技术应用于井下悬臂式掘进机位姿实时检测系统及方法，比如多传感器融合技术、机器视觉技术、惯性导航技术等。其中，基于机器视觉技术的悬臂式掘进机位姿检测方法，采用了防爆摄像机来获取固定在巷道上的激光发射器发射的激光照射在固定在悬臂式掘进机机身上的激光标靶上的图像信息，通过机器视觉技术来处理防爆摄像机获取到的图像，解算当前悬臂式掘进机的空间位姿参数，以代替人工目视的方法来检测悬臂式掘进机的空间位姿，此方法具有操作简便、精度高等特点。

目前多种基于机器视觉技术的悬臂式掘进机位姿检测方法中采用激光标靶的方案都必须考虑脱靶情况的发生，在不发生脱靶的情况下增大悬臂式掘进机的偏转角、俯仰角、横滚角，竖直偏距和水平偏距的识别极限范围的方法是增大激光标靶的面积，但是井下巷道内空间有限，不能无限增大激光标靶的面积，限制了悬臂式掘进机在不脱靶前提下的可测工作空间。例如申请公布号为cn106052645a，名称为“一种悬臂式掘进机空间位姿实时检测系统及方法”的专利申请，提供了一种掘进机位姿检测系统及方法，该悬臂式掘进机空间位姿实时检测系统包括两台十字激光发射器、工控计算机、两台网络摄像机、两个激光标靶；两台十字激光发射器固定安装在巷道后方，两个激光标靶为磨砂面半透明pc板，固定安装在掘进机机身水平面板的左前方和右后方，分别用于接收两台十字激光发射器发射的十字激光投影，两台网络摄像机分别安装在两个激光标靶后方，用于分别采集两个激光标靶上的十字激光成像，工控计算机提取两台网络摄像机采集的图像，进行处理和特征参数提取，最终得到掘进机在巷道坐标系下的空间位姿参数。

该悬臂式掘进机空间位姿实时检测方法为：使用两台固定在巷道上的十字激光器向对应的激光标靶进行投影，然后通过两个固定在掘进机机身的网络摄像机实时采集掘进机工作时两个激光标靶上的成像，随后使用rentinex图像增强算法、图像畸变校正和直线检测的图像处理方法对采集到的视频信号进行处理，最终利用建立好的位姿解算模型得到掘进机行进过程中的偏航角、俯仰角、横滚角以及机身上固定一点在巷道截面上的偏移量。

该发明存在的不足之处在于，首先激光指向装置采用两台十字光发射器，要求两台十字激光发射器发射的激光光束必须相互平行并且和巷道设计轴线方向一致，但是实际操作过程中，两台十字激光发射器的安装误差会导致发射的两个激光光束产生平行度误差，在远距离掘进的情况下，十字激光发射器和激光标靶之间的距离很长，此时由于安装误差导致的激光光束的平行度误差在工控计算机建立的位姿解算模型中造成的影响会被放大，最终导致远距离掘进时工控计算机解算得出的掘进机行进过程中的偏航角、俯仰角、横滚角以及机身上固定一点在巷道截面上的偏移量和实际情况偏差较大，系统识别精度降低。其次激光标靶采用两个磨砂面半透明pc板，固定安装在掘进机机身水平面板的左前方和右后方，这种平面式激光标靶只能以一个面来接收激光投影，当掘进机的偏航角、俯仰角、横滚角以及机身上固定一点在巷道截面上的偏移量较大时，很容易导致脱靶现象发生，也就是说，采用平面式激光标靶会导致掘进机的可测工作空间太小，可测工作空间即不脱靶时掘进机的偏航角、俯仰角、横滚角以及机身上固定一点在巷道截面上的偏移量所有可能值的集合，因此需要频繁调整掘进机以使得激光标靶重新接收到激光投影。

技术实现要素：

本发明的目的在于克服上述现有技术存在的缺陷，提出了一种悬臂式掘进机位姿识别系统及方法，旨在降低远距离掘进时位姿识别的误差，扩大悬臂式掘进机的可测工作空间。

为实现上述目的，本发明采取的技术方案为：

一种悬臂式掘进机位姿识别系统，包括激光指向装置、激光标靶、防爆摄像机和工控计算机，其中：所述激光指向装置固定在巷道顶部，用于向悬臂式掘进机掘进方向发射激光；所述激光标靶，用于接收激光指向装置发射的激光光束；所述防爆摄像机，用于采集激光标靶表面图像；所述工控计算机，用于获取悬臂式掘进机的空间位姿参数；

所述激光指向装置采用激光指向仪，其所发射的激光为单束点状激光；

所述激光标靶采用六块长方形透光板材拼接成的中空的长方体结构，固定在悬臂式掘进机机身前向轴线的正上方，且该激光标靶的上下表面与悬臂式掘进机的顶部平面平行，前后侧面与悬臂式掘进机的尾部平面平行，左右侧面与悬臂式掘进机的侧部平面平行；

所述防爆摄像机的数量为两台，相对安装在悬臂式掘进机机身上方，每台防爆摄像机的拍摄范围各包含激光标靶的三个表面，且一台防爆摄像机拍摄范围所包含的激光标靶的三个表面与另一台防爆摄像机拍摄范围所包含的激光标靶的三个表面互不重叠；

所述悬臂式掘进机的机身上固定有本安型倾角传感器，且该本安型倾角传感器的测量轴与悬臂式掘进机的尾部平面共面，同时与悬臂式掘进机的顶部平面平行，用于实时采集自身的测量轴相对于水平面的倾角数据；

所述工控计算机固定在悬臂式掘进机机身内部，其所获取的悬臂式掘进机的空间位姿参数，是通过接收两台防爆摄像机所采集的激光标靶表面图像和本安型倾角传感器所采集自身的测量轴相对于水平面的倾角数据，对悬臂式掘进机的位姿逆解程序进行解算实现的。

上述一种悬臂式掘进机位姿识别系统，所述激光标靶，采用有机玻璃、亚克力材料或其它透光材料。

上述一种悬臂式掘进机位姿识别系统，所述两台防爆摄像机安装点的连线与激光标靶的体对角线共线，其中一台防爆摄像机安装位置高于激光标靶的上表面，另外一台防爆摄像机安装位置低于激光标靶的下表面。

一种悬臂式掘进机位姿识别方法，包括如下步骤：

(1)设定基本参数：

设定激光标靶形心相对于悬臂式掘进机形心在悬臂式掘进机前向轴线方向上的安装距离为xm2c，在水平轴线方向上的安装距离为ym2c，在向上轴线方向上的安装距离为zm2c；设定激光标靶的前侧面为f，后侧面为b，并对激光标靶的六个表面分别进行标注；

(2)建立三个参考坐标系：

ⅰ.建立以巷道宽度中心且距离地面的高度为(mz+cz/2)的点ob为原点、以巷道设计掘进方向为xb轴正向指向、以竖直向上方向为zb轴正向指向、以通过右手规则确定的yb轴正向指向的大地坐标系ob-xbybzb，其中mz为悬臂式掘进机机体的高度，cz为激光标靶的高度；

ⅱ.建立以悬臂式掘进机的几何形心om为原点、以悬臂式掘进机前向轴线为xm轴正向指向、以悬臂式掘进机向上轴线为zm轴正向指向、以右手规则确定的ym轴正向指向的掘进机坐标系om-xmymzm；

ⅲ.建立以激光标靶形心oc为原点、以xm轴正向指向为xc轴正向指向、以zm轴正向指向为zc轴正向指向、以zm轴正向指向为zc轴正向指向的标靶坐标系oc-xcyczc；

(3)对悬臂式掘进机的初始位置进行调整：

调整悬臂式掘进机的初始位置，使当激光指向仪发射的单束点状激光的光束方向平行于巷道设计掘进方向时，激光从激光标靶后侧面的中心射入，穿过激光标靶几何形心后，从激光标靶前侧面的中心射出，并在激光标靶后侧面的中心和前侧面的中心各留下一个光点；

(4)两台防爆摄像机采集标定图像并上传：

在悬臂式掘进机开始工作前，每次在激光标靶的一个表面上固定棋盘格标定板，并通过拍摄范围包括该固定棋盘格标定板表面的防爆摄像机对该表面进行拍摄，共拍摄六次，然后将六幅标定图像上传至工控机计算机；

(5)两台防爆摄像机采集激光标靶表面图像并上传：

在悬臂式掘进机工作过程中，移除棋盘格标定板，两台防爆摄像机对激光指向仪发射的激光光束穿过激光标靶六个表面中两个表面时激光标靶的六个表面进行实时拍摄，并将所拍摄的激光标靶表面图像连续上传至工控计算机；

(6)本安型倾角传感器采集自身的测量轴相对于水平面的倾角数据并上传：

在悬臂式掘进机工作过程中，本安型倾角传感器实时采集自身的测量轴相对于水平面的倾角数据sensor_h，并将其上传至工控计算机；

(7)工控计算机对悬臂式掘进机的位姿逆解程序进行解算：

(7a)工控计算机对激光标靶入射表面上光点所在表面的标签和出射表面上光点所在表面的标签进行识别，并计算激光标靶入射表面上的光点到入射表面四条边的实际距离，以及激光标靶出射表面上的光点到出射表面四条边的实际距离，得到实际距离值集合q；

(7b)工控计算机通过实际距离值集合q和激光标靶入射表面上光点所在表面的标签和出射表面上光点所在表面的标签，确定入射表面上光点和出射表面上光点在标靶坐标系oc-xcyczc下的坐标^ce1和^ce2，并建立^ce1和^ce2所在直线k在标靶坐标系oc-xcyczc下直线方程；

(7c)工控计算机建立关于直线k的直线方程、前侧面f在标靶坐标系oc-xcyczc下的平面方程和后侧面b在标靶坐标系oc-xcyczc下的平面方程的方程组，并求解该方程组，得到直线k与激光标靶前侧面f所在平面的交点在标靶坐标系oc-xcyczc下的坐标^cef，以及与后侧面b所在平面的交点在标靶坐标系oc-xcyczc下的坐标^ceb；

(7d)工控计算机通过^cef与^ceb的差所形成的激光光路矢量v，以及本安型倾角传感器实时采集自身的测量轴相对于水平面的倾角数据sensor_h，计算悬臂式掘进机的俯仰角finv、偏转角pinv、横滚角hinv，以及悬臂式掘进机的几何形心在水平方向的偏移距离yinv和在竖直方向上的偏移距离zinv，其中v＝^cef-^ceb。

本发明与现有技术相比，具有如下优点：

1.本发明中工控计算机接收的图像信号输入来源于防爆摄像机采集激光指向仪发射的单束点状激光穿过激光标靶之后留下的两个激光光点所在表面的图像，这两个激光光点是由同一个激光指向装置发射的，远距离掘进时位姿逆解程序的理论计算仍然成立，而现有技术中工控计算机接收的图像信号输入来源于防爆摄像机采集两台十字激光发射器分别在两个磨砂面半透明pc板上的十字激光投影图像，这两个十字激光投影是由不同的激光指向装置发射的，远距离掘进时，两台十字激光发射器的安装误差会导致位姿解算模型的两个激光光路相互平行的前提不成立，理论计算会产生误差，与现有技术相比，本发明有效降低了远距离掘进时位姿识别的误差。

2.本发明中的激光标靶采用中空透光的长方体结构，其六个面均可以用来接收激光指向仪的激光投影，而现有技术中的激光标靶采用两个磨砂面半透明pc板，只有两个面可以用来接收激光投影，与现有技术相比，本发明可接收激光投影面积的增加可以直接增大可以测量的悬臂式掘进机空间位姿参数集合，有效扩大了悬臂式掘进机的可测工作空间。

附图说明

图1为本发明识别系统的结构示意图；

图2为本发明识别方法的实现流程图；

图3为本发明识别方法的参考坐标系模型图；

图4为本发明位姿逆解程序实现流程图；

图5为本发明位姿解算原理按运动坐标系旋转过程示意图；

图6为本发明位姿解算原理按固定坐标系旋转过程示意图；

图7为因旋转导致的偏差示意图。

具体实施方式

以下结合附图和具体实施例，对本发明作进一步详细描述。

实施例1、本实施例所采用的防爆摄像机的数量为两台。

参照图1、一种悬臂式掘进机位姿识别系统，包括激光指向装置1、激光标靶2、两台防爆摄像机3、本安型倾角传感器4和工控计算机5，其中：

激光指向装置1固定在巷道顶部，用于向悬臂式掘进机掘进方向发射激光；

激光指向装置1采用激光指向仪，其所发射的激光为单束点状激光；

激光标靶2，采用六块长方形透光板材拼接成的中空的长方体结构，固定在悬臂式掘进机6机身前向轴线的正上方，且该激光标靶的上下表面与悬臂式掘进机6的顶部平面平行，前后侧面与悬臂式掘进机6的尾部平面平行，左右侧面与悬臂式掘进机6的侧部平面平行，用于接收所述激光指向装置1发射的激光光束；

激光标靶2，采用有机玻璃或者亚克力作为透光板材的材料，以保证激光指向装置1发射的激光光束在穿过激光标靶2的入射表面并留下第一个激光光点之后，可以在出射表面上留下第二个激光光点，激光标靶2形状为中空的长方体结构，当激光光束穿过入射表面时，由于激光标靶2的表面材料和空气的折射率不同，激光光束会发生折射现象，所以激光标靶2的每一个面使用的透光板材的厚度要尽可能小，折射所产生的误差可以忽略不计。

在掘进机正常工作过程中，悬臂式掘进机的位姿会发生变化，由于激光标靶2固定安装在悬臂式掘进机机身上，激光标靶2相对于激光激光指向装置1发射的激光光束的位姿也会发生变化，激光光束可能从激光标靶2的六个表面中的任何一个表面射入，从任何一个表面射出，正常工作过程中，激光标靶上永远留有两个激光光点。当掘进机机身位姿偏差过大时，可能会造成脱靶现象，此时激光指向装置1发射的激光光束与激光标靶2无交点，此时悬臂式掘进机应该停止工作并调整机身位姿，使激光标靶2可以重新接收到激光指向装置1发射的激光光束。

激光指向装置1和激光标靶2结合起来使用，既可以获取到足够的图像信息来进行位姿逆解程序解算，又避免现有技术中使用两台十字激光发射器导致的远距离掘进时位姿识别误差较大的技术问题，并且长方体结构的激光标靶的六个面均可以接收激光指向仪发射的激光光束，扩大了悬臂式掘进机的可测工作空间。

两台防爆摄像机3，相对安装在悬臂式掘进机机身上方，每台防爆摄像机的拍摄范围各包含激光标靶的三个表面，且一台防爆摄像机拍摄范围所包含的激光标靶的三个表面与另一台防爆摄像机拍摄范围所包含的激光标靶的三个表面互不重叠，用于采集激光标靶表面图像；

参照图1，两台防爆摄像机3的两个安装点连线和激光标靶的体对角线共线，其中一台防爆摄像机安装于悬臂式掘进机机身上表面左前方，且其位置高于激光标靶的上表面，拍摄范围包括激光标靶的上面、左侧面和前侧面，另外一台防爆摄像机安装于悬臂式掘进机机身上表面右后方，且其安装位置低于激光标靶的下表面，拍摄范围包括激光标靶的下面、右侧面和后侧面。

本安型倾角传感器4与悬臂式掘进机机身固定，该本安型倾角传感器的测量轴与悬臂式掘进机的尾部平面共面，且与悬臂式掘进机的顶部平面平行，用于实时采集自身的测量轴相对于水平面的倾角数据；

工控计算机5固定在悬臂式掘进机机身内部，用于通过接收两台防爆摄像机所采集的激光标靶表面图像和本安型倾角传感器所采集自身的测量轴相对于水平面的倾角数据，对悬臂式掘进机的位姿逆解程序进行解算，以获取悬臂式掘进机的空间位姿参数。

两台防爆摄像机3和本安型倾角传感器4通过有线的方式连接至工控计算机来传输数据。

参照图2、一种悬臂式掘进机位姿识别方法，包括如下步骤：

步骤1)设定基本参数：

设定激光标靶形心相对于悬臂式掘进机形心在悬臂式掘进机前向轴线方向上的安装距离为xm2c，在水平轴线方向上的安装距离为ym2c，在向上轴线方向上的安装距离为zm2c；设定激光标靶的前侧面为f，后侧面为b，并对激光标靶的六个表面分别进行标注；

标注激光标靶前侧面的标签为1、激光标靶后侧面的标签为2、激光标靶左侧面的标签为3、激光标靶右侧面的标签为4、激光标靶上面的标签为5、激光标靶下面的标签为6；

步骤2)建立如图3所示的三个参考坐标系：

ⅰ.建立以巷道宽度中心且距离地面的高度为(mz+cz/2)的点ob为原点、以巷道设计掘进方向为xb轴正向指向、以竖直向上方向为zb轴正向指向、以通过右手规则确定的yb轴正向指向的大地坐标系ob-xbybzb，其中mz为悬臂式掘进机机体的高度，cz为激光标靶的高度；

ⅱ.建立以悬臂式掘进机的几何形心om为原点、以悬臂式掘进机前向轴线为xm轴正向指向、以悬臂式掘进机向上轴线为zm轴正向指向、以右手规则确定的ym轴正向指向的掘进机坐标系om-xmymzm；

ⅲ.建立以激光标靶形心oc为原点、以xm轴正向指向为xc轴正向指向、以zm轴正向指向为zc轴正向指向、以zm轴正向指向为zc轴正向指向的标靶坐标系oc-xcyczc；

步骤3)对悬臂式掘进机的初始位置进行调整：

调整悬臂式掘进机的初始位置，使当激光指向仪发射的单束点状激光的光束方向平行于巷道设计掘进方向时，激光从激光标靶后侧面的中心射入，穿过激光标靶几何形心后，从激光标靶前侧面的中心射出，并在激光标靶后侧面的中心和前侧面的中心各留下一个光点；

本发明使用的一种悬臂式掘进机位姿识别方法，具体来说，可以实时识别悬臂式掘进机参考坐标系相对于激光指向仪发射的激光光束所在直线的偏转角、俯仰角、横滚角、竖直偏距和水平偏距。在初始位置时，悬臂式掘进机的理想位置就是使激光指向仪发射的激光穿过激光标靶的后面的中心，留下第一个光点，再穿过激光标靶几何形心，从激光标靶的前面的中心射出。调整悬臂式掘进机的初始位置有利于悬臂式掘进机以一个正确的姿态开始工作。

步骤4)两台防爆摄像机采集标定图像并上传：

在悬臂式掘进机开始工作前，每次在激光标靶的一个表面上固定棋盘格标定板，并通过拍摄范围包括该固定棋盘格标定板表面的防爆摄像机对该表面进行拍摄，共拍摄六次，然后将六幅标定图像上传至工控机计算机；

因为每台防爆摄像机的拍摄范围只包含了激光标靶的三个表面，所以防爆摄像机5需要拍摄激光标靶的上面、左侧面和前侧面中只有一个面固定棋盘格标定板时的标定图像，拍摄三次，防爆摄像机3需要拍摄激光标靶的下面、右侧面和后侧面中只有一个面固定棋盘格标定板时的标定图像，拍摄三次，最终得到六幅标定图像；

步骤5)两台防爆摄像机采集激光标靶表面图像并上传：

在悬臂式掘进机工作过程中，移除棋盘格标定板，两台防爆摄像机对激光指向仪发射的激光光束穿过激光标靶六个表面中两个表面时激光标靶的六个表面进行实时拍摄，并将所拍摄的激光标靶表面图像连续上传至工控计算机；

步骤6)本安型倾角传感器采集自身的测量轴相对于水平面的倾角数据并上传：

在悬臂式掘进机工作过程中，本安型倾角传感器实时采集自身的测量轴相对于水平面的倾角数据sensor_h，并将其上传至工控计算机；

步骤7)工控计算机对悬臂式掘进机的位姿逆解程序进行解算，实现步骤如图4所示：

(7a)工控计算机对激光标靶入射表面上光点所在表面的标签和出射表面上光点所在表面的标签进行识别，并计算激光标靶入射表面上的光点到入射表面四条边的实际距离，以及激光标靶出射表面上的光点到出射表面四条边的实际距离，得到实际距离值集合q；

(7a1)工控计算机通过两台防爆摄像机上传的六幅标定图像中各截取的一张标定图片，生成六份标定文件；

(7a1)工控计算机采用图像增强算法对两台防爆摄像机连续上传的激光标靶表面图像进行亮度调节，得到亮度增强后连续的激光标靶表面图像；

(7a2)工控计算机采用特征匹配方法，并通过六个激光标靶单一表面特征模板和一个激光光点特征模板，分别对亮度增强后连续的激光标靶表面图像进行特征匹配，得到激光标靶的六个表面对应匹配区域以及两个激光光点对应匹配区域；

(7a3)工控计算机采用直线检测方法，对激光标靶六个表面对应的匹配区域中的直线进行检测，得到激光标靶每个表面四条边对应的直线；

(7a4)工控计算机采用点线测量方法，对两个激光光点对应匹配区域中心点到激光标靶每个表面的四条边所在的直线的像素距离进行测量，得到两个激光光点对应匹配区域中心点到激光标靶每个表面的四条边所在的直线的像素距离集合；

(7a5)工控计算机通过所测量的两个激光光点对应匹配区域中心点到激光标靶每个表面的四条边所在的直线的像素距离集合，对两个激光光点对应的表面标签进行识别，并根据识别结果确定激光标靶的入射表面和出射表面；

(7a6)工控计算机选择激光标靶入射表面的标定文件和出射表面的标定文件，并从两个激光光点对应匹配区域中心点距离激光标靶每个表面的四条边所在的直线的像素距离集合中找出激光标靶入射表面上的光点到入射表面四条边的像素距离，以及激光标靶出射表面上的光点到出射表面四条边的像素距离，然后使用入射表面的标定文件对激光标靶入射表面上的光点到入射表面四条边的像素距离进行标定转换，得到激光标靶入射表面上的光点到入射表面四条边的实际距离，同时使用出射表面的标定文件对激光标靶出射表面上的光点到出射表面四条边的像素距离进行标定转换，得到激光标靶出射表面上的光点到出射表面四条边的实际距离，将这八个实际距离值组成实际距离值集合q。

(7b)工控计算机通过实际距离值集合q和激光标靶入射表面上光点所在表面的标签和出射表面上光点所在表面的标签，确定入射表面上光点和出射表面上光点在标靶坐标系oc-xcyczc下的坐标^ce1和^ce2，并建立^ce1和^ce2所在直线k在标靶坐标系oc-xcyczc下直线方程；

(7c)工控计算机建立关于直线k的直线方程、前侧面f在标靶坐标系oc-xcyczc下的平面方程和后侧面b在标靶坐标系oc-xcyczc下的平面方程的方程组，并求解该方程组，得到直线k与激光标靶前侧面f所在平面的交点在标靶坐标系oc-xcyczc下的坐标^cef，以及与后侧面b所在平面的交点在标靶坐标系oc-xcyczc下的坐标^ceb；

(7d)工控计算机通过^cef与^ceb的差所形成的激光光路矢量v，以及本安型倾角传感器实时采集自身的测量轴相对于水平面的倾角数据sensor_h，计算悬臂式掘进机的俯仰角finv、偏转角pinv、横滚角hinv，以及悬臂式掘进机的几何形心在水平方向的偏移距离yinv和在竖直方向上的偏移距离zinv，其中v＝^cef-^ceb。

(7d1)工控计算机通过激光光路矢量v计算悬臂式掘进机的偏转角pinv：

其中，vx表示激光光路矢量v在xc轴上的分量，vy表示激光光路矢量v在yc轴上的分量；

计算悬臂式掘进机的偏转角，只考虑平移之后的旋转过程，参照图5，对于按运动坐标系旋转的姿态描述方法，坐标系变换全过程总结如下：

掘进机坐标系om-xmymzm初始位姿和大地坐标系ob-xbybzb完全重合，掘进机坐标系先沿大地坐标系各轴平移得到om1-xm1ym1zm1，之后开始旋转，先绕zm1轴旋转pinv角度得到om2-xm2ym2zm2，如图5(a)所示，再绕ym1轴旋转finv角度得到om3-xm3ym3zm3，如图5(b)所示，再绕xm1轴旋转hinv角度得到om-xmymzm，如图5(c)所示。

图5中画出了按运动坐标系旋转过程中从om1-xm1ym1zm1到om-xmymzm坐标系各个轴的姿态变化，以及三个转动角度pinv、hinv、finv。从图5(b)中可以看出，xm2、xm3、zm2和zm3四个轴在同一个平面内，也就是说，激光光路xb在坐标系om3-xm3ym3zm3中的xm3om3ym3平面内的投影与xm3轴重合。从图5(c)中可以看出，最后一步旋转pinv角度的过程中，zm3和zm重合，那么xm3om3ym3平面和xmomym平面也仍然在同一平面上。所以偏转角pinv就是矢量v在掘进机坐标系om-xmymzm中xmomym平面内的投影(与xm3轴重合)与xm轴的夹角。

现在已知的是矢量v在标靶坐标系oc-xcyczc下的坐标，而标靶坐标系的三个坐标平面是和掘进机坐标系om-xmymzm中对应的三个坐标平面相互平行的，所以矢量v与掘进机坐标系中各个轴和平面的夹角和其与标靶坐标系中对应各个轴和平面的夹角相同，可以直接在标靶坐标系内使用矢量v与标靶坐标系中各个轴和平面的夹角进行计算。

偏转角pinv是矢量v在掘进机坐标系om-xmymzm中xmomym平面内的投影(与xm3轴重合)与xm轴的夹角，也等于矢量v在标靶坐标系oc-xcyczc中xcocyc平面内的投影与xc轴的夹角，得偏转角pinv计算公式如上所述。

(7d2)工控计算机通过激光光路矢量v和悬臂式掘进机的偏转角pinv计算悬臂式掘进机的俯仰角finv：

其中，vz表示激光光路矢量v在zc轴上的分量；

图5(d)中同样标出了俯仰角finv是矢量v与其在xmomym平面内的投影的夹角，等于矢量v与其在xcocyc平面内的投影的夹角，现已求得偏转角pinv，则可得俯仰角finv计算公式如上所述。

(7d3)工控计算机选择掘进机坐标系om-xmymzm的ym轴负方向上单位矢量u作为特征矢量，并计算按照固定坐标系旋转过程中，特征矢量u绕zm轴旋转pinv角度，再绕ym轴旋转finv角度之后得到的u2矢量：

u2＝(txyz(0,0,0)rx(0)ry(finv)rz(pinv))·u1

其中，txyz表示坐标变换平移矩阵，rx、ry、rz表示坐标变换旋转矩阵；

要计算计算悬臂式掘进机的横滚角，需要倾角传感器参数sensor_h，首先要找到sensor_h在旋转过程中对应的角度,按固定坐标系旋转过程如图6所示。因为倾角传感器的安装轴线和掘进机的ym轴平行，选择掘进机ym轴负方向上单位矢量u作为特征矢量。

u矢量首先绕zm轴旋转pinv角度得到u1，再绕ym轴旋转finv角度得到u2，如图6(a)所示此时u2矢量的末端所处的与xm轴垂直的平面设为s面,使用坐标变换公式求u2的公式如上所述。

(7d4)工控计算机计算按照固定坐标系旋转过程中，u2矢量终点在u2矢量终点所在的与xm轴平行的平面上的旋转半径r，u2矢量终点到xmomym平面的投影高度h2，u2矢量绕xm轴旋转hinv角度得到的u3矢量的矢量终点到xmomym平面的投影高度h3：

h3＝||u3||sin(|sensor_h|)

其中，u2y表示u2矢量在ym轴上的分量，u2z表示u2矢量在zm轴上的分量；

最后一次旋转，u2绕xm轴旋转hinv角度得到u3，也是全部旋转过程之后矢量u的最终位置。从图6(b)中可以看出，倾角传感器参数sensor_h是u3与其在xm1om1ym1平面上投影的夹角，从图6中的几何关系可以看出旋转半径r、投影高度h2和h3的计算公式如上所述。

(7d5)工控计算机根据旋转半径r、投影高度h2和h3计算横滚角hinv：

从图6中的几何关系可以得到由旋转半径r、投影高度h2和h3来计算横滚角hinv的计算公式如上所述。

(7d6)工控计算机计算无偏距状态下，标靶坐标系原点oc在掘进机坐标系om-xmymzm下的坐标^moc在经过俯仰角finv、偏转角pinv、横滚角hinv旋转之后在大地坐标系ob-xbybzb下的坐标^boc：

^boc＝(txyz(0,0,0)rx(hinv)ry(finv)rz(pinv))·^moc

其中，标靶坐标系原点oc在掘进机坐标系om-xmymzm下的坐标^moc为[xm2c,ym2c,zm2c,1]^t；

参照图7，假设悬臂式掘进机在大地坐标系的yb方向和zb方向上两个偏差均为0的时候，如果悬臂式掘进机发生绕某个轴的转动，由于标靶坐标系相对于掘进机坐标系有一个安装距离，所以激光光路相对于标靶坐标系的位置也可能会发生变化，从而产生因旋转带来的偏距。根据俯仰角finv、偏转角pinv、横滚角hinv旋转使用坐标变换公式来求解无偏距状态下标靶坐标系原点oc旋转之后在大地坐标系ob-xbybzb下的坐标^boc的公式如上所述。

(7d7)工控计算机计算激光光路被激光标靶截取的激光段的中点指向标靶坐标系原点ob的矢量^cd在大地坐标系ob-xbybzb下的矢量^bd：

^cd＝ob-(^cef+^ceb)/2

^bd＝(txyz(0,0,0)rx(hinv)ry(finv)rz(pinv))·^cd

再计算实际情况下，即掘进机在yb方向和zb方向上的偏距不为0时，标靶坐标系原点oc到激光光路所在直线在yb方向和zb方向上的距离，也就是激光光路上任意一点到长方体坐标系原点ob的连线矢量在大地坐标系的ybobzb平面上的投影矢量的y值和z值，为简化计算，取激光光路上任意一点为被长方体截取的激光段的中点，使用坐标变换公式计算激光光路被激光标靶截取的激光段的中点指向标靶坐标系原点ob的矢量^cd在大地坐标系ob-xbybzb下的矢量^bd的公式如上所述。

(7d8)工控计算机计算悬臂式掘进机在yb方向和zb方向上的移动偏差yinv和zinv：

yinv＝^bdy-^bocy

zinv＝^bdz-^bocz

其中，^bdy表示矢量^bd在yb轴上的分量，^bdz表示矢量^bd在zb轴上的分量，^bocy表示坐标^boc的y值，^bocz表示坐标^boc的z值。

现已计算得出实际情况下，即掘进机在yb方向和zb方向上的偏距不为0时，标靶坐标系原点oc到激光光路所在直线在yb方向和zb方向上的矢量^bd，以及无偏距状态下，即掘进机在yb方向和zb方向上的偏距均为0时，标靶坐标系原点oc旋转之后在大地坐标系ob-xbybzb下的坐标^boc，两个坐标之差在yb轴和zb轴上的分量就是悬臂式掘进机在在yb轴和zb轴上的偏差。

实施例2、本实施例所采用的防爆摄像机的数量为一台，其他结构与实施例1相同。

一台防爆摄像机，安装在激光标靶内部上侧面中心，安装基座为自带驱动的云台，云台可以控制防爆摄像机进行360度旋转，用于拍摄激光标靶除了上侧面的其他五个表面图像。

正常工作时，云台控制防爆摄像机以一定的速度旋转并采集五个表面图像，当激光标靶除了上侧面的其他五个表面中的两个表面上有两个光点的时候，在一个旋转周期内，通过防爆摄像机拍摄的两个有光点的表面的图像将会被上传至工控计算机，经过位姿逆解程序的图像处理部分得到实际距离值集合q，并进行之后的解算步骤，最终计算得出悬臂式掘进机的俯仰角finv、偏转角pinv、横滚角hinv，以及悬臂式掘进机的几何形心在水平方向的偏移距离yinv和在竖直方向上的偏移距离zinv。

本实施例防爆摄像机在采集图像的时候，由于云台旋转需要一定的时间，所采集到的两个光点的图像实际上并不是同一时刻的图像，而位姿逆解程序要求必须是同一时刻两个点的图像进行处理解算才能到准确的实时位姿参数。考虑实际的工作过程中，悬臂式掘进机机身并不是每时每刻都在变化，在掘进机对截割面进行截割时，只有截割臂在运动，机身必须是固定的，这也就意味着忽略机身振动时，这段时间内两个光点在激光标靶上的位置是不变的，所以使用一台防爆摄像机时，两个激光光点所在激光标靶表面图像的采集即使有一定的时间差，也不影响最终的位姿识别结果。

本实施例相比于实施例1，无法采集到激光标靶上侧面的图像，这意味着激光的接收表面直接减少一个，当激光光点落在上侧面的时候，位姿逆解程序无法进行解算，悬臂式掘进机的可测工作空间相比于实施例1会变小，但是仍然优于现有技术的两个平面式激光标靶的可测工作空间。本实施例的优点是，显著降低的系统的硬件成本，由实施例1的两台防爆摄像机变为1台防爆摄像机。

实施例3、本实施例所采用的防爆摄像机的数量为多台，其他结构与实施例1相同。

多台防爆摄像机，安装在悬臂式掘进机机身上，激光标靶的四周，数量和安装位置没有明确要求，但是总体必须满足一个条件，所有的防爆摄像机的拍摄范围的并集必须包括激光标靶的全部六个表面。所有防爆摄像机拍摄的图像中一定包含有两个有光点所在表面的图像，这些图像将会被上传至工控计算机，经过位姿逆解程序的图像处理部分得到实际距离值集合q，并进行之后的解算步骤，最终计算得出悬臂式掘进机的俯仰角finv、偏转角pinv、横滚角hinv，以及悬臂式掘进机的几何形心在水平方向的偏移距离yinv和在竖直方向上的偏移距离zinv。

本实施例相比于实施例2，防爆摄像机的成本增加，但是可以拍摄到激光标靶的全部六个表面，可测工作空间能够达到理论上的最大值。

本实施例相比于实施例1，防爆摄像机的数量有所增加，成本偏高，识别精度和可测工作空间相同。本实施例的应用场景是，对于多种不同型号的悬臂式掘进机，机身上方各种设备的数量、种类、体积位置都不同，而且悬臂式掘进机还有可能挂接各种外部设备，比如通风设备、二车、锚护设备等。这些不确定因素都有可能导致实施例1中对的安装要求无法满足，需要根据现场实际情况采用本实施例，在满足所有的防爆摄像机的拍摄范围的并集必须包括激光标靶的全部六个表面的条件下，选择适用的防爆摄像机数量和安装位置。