CN101819036A - 一种掘进机的空间位姿自动测量方法 - Google Patents

Abstract

本发明涉及一种掘进机的空间位姿自动测量方法，它是针对掘进机在煤矿井下狭小空间掘进作业的实际情况，在掘进机的后部设置工业摄像机、工控计算机、激光指向仪、图像采集卡，对掘进机煤岩断面的位姿进行测量、计算，确认机身最合理的位姿，然后指令进行采掘作业，使作业人员远离危险地段，减少突发事故，提高采掘的安全性，使掘进机处于最佳、最合理的作业位置和状态，最大覆盖角度为±60°，位移检测偏差＜10mm，偏转角检测精度为±12′，俯仰角检测精度为±6′，横滚角检测精度为±6′，以提高采掘质量和效率，此方法先进、合理、准确，是十分理想的采煤掘进机的采掘位姿测量方法，此方法也可在隧道采掘作业时使用。

Description

一种掘进机的空间位姿自动测量方法

技术领域

本发明涉及一种掘进机的空间位姿自动测量方法，属煤矿采掘设备应用、检测测量的技术领域。

背景技术

悬臂式掘进机，是大中型煤矿中最主要的采掘设备。

采煤掘进机是在一个狭小的巷道内往复采煤作业，空间有限、环境恶劣、工作面粉尘大、作业现场光线差，特别是在截割巷道底板时，操作员视线受到机身的遮挡，很难判断掘进机的截割位置和姿态，给掘进方向控制带来很大困难，致使掘进速度慢、生产效率低、巷道超采、欠采现象严重，采掘质量差，存在很大的安全隐患。

根据采煤掘进机在煤矿井下采煤作业的实际情况，如何使采煤掘进机在作业时处于最佳位置和空间位姿，并进行自动检测，是急需解决的问题。

公开号为CN201013380、名称为全自动掘进机的实用新型专利，提出采用航天导航定位仪测量掘进机位置，但没有测量掘进机身的姿态；公开号为CN101266134A、名称为悬臂掘进机头位姿的测量系统及其方法的发明专利，提出采用激光机动全站仪测量掘进机车体在大地坐标系的坐标，车体偏摆角传感器测量车体偏摆角，双轴倾角传感器测量车体俯仰角和横滚角，其俯仰角和横滚角均以水平面为基准，但只适用于水平掘进巷道，此外，该方法要求全站仪安装位置与掘进机身呈水平状，这进一步限制了该方法的应用范围。

发明内容

发明目的

本发明的目的就是针对背景技术的不足和工作状况，提出一种新的掘进机空间位姿的测量方法，采用激光指向仪、工控计算机、工业摄像机、图像采集卡等设备，对采掘巷道的煤岩断面与掘进机身、截割头的空间位姿进行测量，使掘进机处于最佳位置状态，以大幅度提高掘进机的采掘效率和安全性。

技术方案

本发明使用的测量仪器为激光指向仪、工业摄像机、工控计算机、图像采集卡，其规格如下：

激光指向仪：YBJ-600

工业摄像机：KBA112

工控计算机：PCM-9387F

图像采集卡：CC₁410

测量方法如下：

I、布置掘进机及测量仪器，使其处于准工作状态

①将掘进机身2置于煤岩断面3的前部，截割头8接触煤岩断面3；

②将激光指向仪4、工业摄像机5、朝向煤岩断面3、截割头8；

③工业摄像机5通过导线9联接图像采集卡7、工控计算机6；

II、激光指向仪光束与摄像机光轴方向相对位姿测量

①设定光斑接收装置

在掘进机身后部设置平面形光斑接收装置，平面上水平设置九个距离特征点，并将其排列为三行三列，行、列之间为等间距，九个特征点处于同一平面内，并平行、垂直左、右、上、下轴线；

②调整激光指向仪、工业摄像机与光斑接收装置之间的相对位置

调整激光指向仪、工业摄像机指向，使激光指向仪与其中一个特征点重合；

③确定光斑接收装置上的A、B两个特征点为激光指向仪、工业摄像机的基准点，A、B特征点处于同一水平面上；

④调整并确定工业摄像机、激光指向仪与A、B基准点的水平距离；

⑤调整并确定工业摄像机与光斑接收装置之间距离、工业摄像机与激光指向仪之间距离；

⑥采集光斑接收装置上两个特征点图像，并由工控计算机处理，计算激光指向仪光束与工业摄像机光轴之间相对位姿；

⑦识别图像中两光斑特征，将二者绕原点旋转，使连线与横轴平行，转过的角度即为工业摄像机的横滚角；

⑧基于前面所得数据，用旋转后的主特征点的横坐标，计算出工业摄像机光轴与激光指向仪光束之间的偏转角，由纵坐标算出工业摄像机光轴与激光指向仪光束之间的俯仰角；

⑨激光指向仪与工业摄像机为刚性固定联接，二者空间位姿只需检测一次；

III、工业摄像机对掘进机身的空间位姿测量

①特征点阵设置

掘进机身后部的光斑接收装置上的九个特征点，在同一水平面上，不共线，即机身位姿；

②将工业摄像机与激光指向仪悬挂于掘进机后部巷道壁顶上，并调整激光指向仪、工业摄像机指向，并固定，使掘进机身上的光斑接收装置的特征点均置于摄像机视场内，视场角为±60°，直线距离为10-12米；

③采集一组特征点阵图像，输入工控计算机，计算工业摄像机对特征点阵的空间位姿；

④提取图像中特征点区域并做标记，检测特征点位置坐标；

⑤根据特征点阵图像特征，自动完成特征点的图像匹配；

⑥在获得全部特征点及图像坐标数据后，用混合标定法对工业摄像机进行标定，计算出工业摄像机对特征点阵的空间位姿；

IV、将步骤II所得工业摄像机光轴与激光指向仪光束之间的空间位姿数据，和步骤III中所得工业摄像机与特征点阵相对位姿数据进行合并，计算出特征点阵，即：掘进机身相对激光指向仪光束，即：理论巷道轴线的空间位姿。

所述的掘进机的空间位姿测量设备的安装及布置为：由光斑接收装置特征点阵1、激光指向仪4、工业摄像机5、工控计算机6、图像采集卡7组成，激光指向仪4与工业摄像机5通过锚杆固定在掘进机身2后部10-12米的巷道壁顶上，光斑接收装置特征点阵1置于掘进机身2后部，光斑接收装置特征点阵1上设置九个特征点，激光指向仪、工业摄像机最大覆盖角为±60°，激光指向仪4、工业摄像机5、工控计算机6、图像采集卡7通过导线9联接。

所述的掘进机空间位姿测量相关计算如下：

1)计算工业摄像机5相对激光指向仪4的空间位姿；

如图2所示：

①在任意铅垂面π上水平设置两个特征点A和B；

②于距离铅垂面π为L处悬挂激光指向仪4和工业摄像机5，且使工业摄像机5垂直指向铅垂面π；

③在与铅垂面π平行的平面内移动激光指向仪4和工业摄像机5，使激光指向仪5发出光束光斑与特征点A重合；

用工业摄像机5采集两特征点图像，如图3、4所示，两特征点对应图像中心分别为a(x_A，y_A)、b(x_B，y_B)，则工业摄像机5横滚角为：

${\mathrm{\δ}}_1=\arctan \left(\frac{y_A-y_B}{x_A-x_B}\right)$ ①

将a(x_A，y_A)和b(x_B，y_B)围绕原点o旋转角度δ₁，旋转后的a点坐标为：

$\left(\begin{array}{c}{x_A}^{\′}\\ {y_A}^{\′}\end{array}\right)=\left(\begin{array}{cc}\cos {\mathrm{\δ}}_1& \sin {\mathrm{\δ}}_1\\ -\sin {\mathrm{\δ}}_1& \cos {\mathrm{\δ}}_1\end{array}\right)\·\left(\begin{array}{c}{x}_A\\ y_A\end{array}\right)$ ②

则工业摄像机5轴线相对激光指向仪4光束的偏航角：

${\mathrm{\θ}}_1=\arctan \left(\frac{{x_A}^{\′}}{f}\right)$ ③

工业摄像机5轴线相对激光指向仪4光束的俯仰角：

④

式中，H为工业摄像机5与激光指向仪4安装距离，L为工业摄像机5与铅垂面π之间距离，f为工业摄像机5的镜头焦距。

2)计算工业摄像机5相对特征点阵1的空间位姿：

①提取特征点阵图像中所有特征点区域并作标记，检测每一特征点区域中心位置坐标；

②根据特征点阵图像特征，完成特征点及其图像匹配；

③在获得特征点及其图像坐标数据的基础上，利用混合标定法对工业摄像机5进行标定，实时计算工业摄像机5相对特征点阵1的空间位姿；

计算过程如下：

设各特征点空间坐标为P_i(X_2，i，Y_2，i，Z_2，i)，对应图像中各光斑中心坐标为p_i(x_i，y_i)，其中i＝1，2，…，9，按照摄像机成像模型和标定理论，令

A_18×8·M_8×1＝B_18×1    ⑤

式中，A_18×8为18行8列且参数已知的矩阵，B_18×1为18行1列且参数已知的矩阵，M_8×1为8行1列的矩阵，包含8个未知参数m_j，其中j＝1，2，…，8，具体为：

$A_{18\×8}=\left(\begin{array}{cccccccc}f\·X_{\mathrm{2,1}},& f\·Y_{\mathrm{2,1}},& 0,& 0,& x_1\·X_{\mathrm{2,1}},& x_1\·Y_{\mathrm{2,1}},& f,& 0\\ 0,& 0,& f\·X_{\mathrm{2,1}},& f\·Y_{\mathrm{2,1}},& y_1\·X_{\mathrm{2,1}},& y_1\·Y_{\mathrm{2,1}},& 0,& f\\ & & & .& & & & \\ & & & .& & & & \\ & & & .& & & & \\ f\·X_{\mathrm{2,9}},& f\·Y_{\mathrm{2,9}},& 0,& 0,& x_9\·X_{\mathrm{2,9}},& x_9\·Y_{\mathrm{2,9}},& f,& 0\\ 0,& 0,& f\·X_{\mathrm{2,9}},& f\·Y_{\mathrm{2,9}},& y_9\·X_{\mathrm{2,9}},& y_9\·Y_{\mathrm{2,9}},& 0,& f\end{array}\right)$

$B_{18\×1}=\left(\begin{array}{c}{x}_1\\ y_1\\ .\\ .\\ .\\ x_9\\ y_9\end{array}\right),$ $M_{8\×1}=\left(\begin{array}{c}{m}_1\\ .\\ .\\ .\\ m_8\end{array}\right)$

对上式作矩阵变换，得

M_8×1＝(A_18×8′·A_18×8)^-1·A_18×8′·B_18×1    ⑥

式中，A_18×8′表示矩阵A_18×8的转置，(A_18×8′·A_18×8)^-1表示矩阵A_18×8′·A_18×8的逆矩阵。

则

掘进机身2与工业摄像机5在摄像机光轴Y₁方向上距离

$Z^{\′}=f+\frac{1}{\sqrt{{m_1}^2+{m_3}^2+{m_5}^2}}$ ⑦

掘进机身2在工业摄像机5横轴X₁方向偏移距离

X′＝m₇·(f-Z′)    ⑧

掘进机身2在工业摄像机5纵轴Z₁方向偏移距离

Y′＝m₈·(f-Z′)    ⑨

掘进机身2轴线矢量

在摄像机坐标系中偏航角

θ₂＝-arcsin(m₅·(f-Z′))    ⑩

掘进机身2轴线矢量

在摄像机坐标系中横滚角

${\mathrm{\δ}}_2=-\arcsin \left(\frac{m_3\·(f-Z^{\′})}{\cos {\mathrm{\θ}}_2}\right)$

掘进机身2轴线矢量在摄像机坐标系中俯仰角

3)求解掘进机身2相对激光指向仪4光束的空间位姿：

工业摄像机5光轴与激光指向仪4所发出光束即理论巷道轴线之间空间位姿数据，和工业摄像机5与特征点阵1所在平面即掘进机身2相对位姿数据通过空间坐标变换进行合并，即可计算出掘进机身2相对理论巷道轴线的空间位姿，其中，掘进机身2空间位置由下式计算：

$\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)=R_{3\×3}\·\left(\begin{array}{c}{X}^{\′}\\ Y^{\′}\\ Z^{\′}\end{array}\right)+\left(\begin{array}{c}0\\ H\\ 0\end{array}\right)$

式中，R_3×3为3行3列且参数已知的矩阵，具体为：

掘进机身2空间姿态由下式计算：

掘进机身2轴线矢量

在摄像机坐标系中偏航角：

θ＝θ₁+θ₂

掘进机身2轴线矢量

在摄像机坐标系中横滚角：

δ＝δ₁+δ₂

掘进机身2轴线矢量

在摄像机坐标系中俯仰角：

式中：

OXYZ：世界坐标系，用于表示理论巷道；

X：水平且垂直于理论巷道轴向；

Y：平行于理论巷道轴向；

Z：与X轴、Y轴正交的方向

O：激光指向仪4激光出口

O₁X₁Y₁Z₁：工业摄像机5坐标系

X₁：平行于工业摄像机5成像元件水平扫描方向

Y₁：工业摄像机5光轴方向

Z₁：平行于工业摄像机5成像元件竖直扫描方向

O₁：工业摄像机5光心

O₂X₂Y₂Z₂：特征点阵1坐标系，同掘进机身2坐标系；

X₂：平行于掘进机身2左右向轴线方向

Y₂：平行于掘进机身2前后向轴线方向，同特征点阵1所在平面法线方向

Z₂：平行于掘进机身2上下向轴线方向

O₂：特征点阵1中心

oxy：图像坐标系

x：工业摄像机5成像元件水平扫描方向

y：工业摄像机5成像元件竖直扫描方向

O₂X₃Y₃Z₃：O₁X₁Y₁Z₁平移，使其坐标系原点O₁与坐标系O₂X₂Y₂Z₂原点O₂重合后所形成的坐标系

X₃：平行于工业摄像机5成像元件水平扫描方向

Y₃：工业摄像机5光轴方向

Z₃：平行于工业摄像机5成像元件竖直扫描方向

掘进机身2前后轴向矢量

掘进机身2在世界坐标系OXYZ中的俯仰角

θ：掘进机身2在世界坐标系OXYZ中的偏转角

δ：掘进机身2在世界坐标系OXYZ中的横滚角

掘进机身2在工业摄像机5坐标系O₁X₁Y₁Z₁中的俯仰角

θ₂：掘进机身2在工业摄像机5坐标系O₁X₁Y₁Z₁中的偏转角

δ₂：掘进机身2在工业摄像机5坐标系O₁X₁Y₁Z₁中的横滚角

工业摄像机5在世界坐标系OXYZ中的俯仰角

θ₁：工业摄像机5在世界坐标系OXYZ中的偏转角

δ₁：工业摄像机5在世界坐标系OXYZ中的横滚角

H：工业摄像机5与激光指向仪4的垂直安装距离

L：工业摄像机5与铅垂面π之间的距离

有益效果

本发明与背景技术相比具有明显的先进性，它是根据掘进机在煤矿井下狭小空间掘进作业的实际情况，在掘进机的后部设置工业摄像机、工控计算机、激光指向仪、图像采集卡，对掘进机相对煤岩断面的位姿进行实时自动测量、计算，确认机身最合理的位姿，使作业人员远离危险地段、减少事故，提高采掘的安全性，使掘进机处于最佳、最合理的空间位姿进行作业，以提高采掘质量和效率，最大覆盖角度为±60°，位移检测偏差＜10mm，偏转角检测精度为±12′，俯仰角检测精度为±6′，横滚角检测精度为±6′，此方法先进、合理、准确，是十分理想的掘进机的采掘位姿测量方法，此方法也可在隧道采掘作业中使用。

附图说明

图1为测量仪器与掘进机相互位置图

图2为工业摄像机、激光指向仪与基准面、基准点位置图

图3为测量平面示意图

图4为空间位姿计算原理图

图中所示，附图标记清单如下：

1、光斑接收装置特征点阵，2、掘进机身，3、煤岩断面，4、激光指向仪，5、工业摄像机，6、工控计算机，7、图像采集卡，8、截割头，9、导线。

具体实施方式

以下结合附图对本发明做进一步说明：

图1所示，为测量仪器、掘进机、煤岩断面位置图，各部位置、覆盖角度、方向、高度要合理、正确，各仪器导线联接要牢固。

图2所示，为工业摄像机、激光指向仪与基准面、基准点位置图，图中可知：π平面为基准面，A、B点为基准点。

图3所示，为测量平面示意图，测量平面均在X、Y、O坐标轴上。

图4所示，为空间位姿计算原理图，图中可知：由激光指向仪光束确定所要求的巷道指向，并在其上建立世界坐标系OXYZ，坐标原点位于光束起点，光束指向为Y轴，与Y轴水平正交的方向为X轴，垂直正交的方向为Z轴，摄像机与激光指向仪刚性连接，O₁X₁Y₁Z₁为摄像机坐标系，坐标系原点位于成像平面中心，摄像机光轴为Y₁轴，以成像元件排列方向为X₁轴及Z₁轴，O₂X₂Y₂Z₂为特征点阵确定的坐标系，以点阵中心即掘进机身轴线与特征点阵平面交点为坐标系原点，机身前后轴线方向为Y_O轴，机身左右轴线方向为X₂轴，以与Y₂轴和X₂轴正交的方向为Z₂，因特征点阵与掘进机刚性连接，所以可视为掘进机身坐标系，掘进机轴线向量与O₂Y₂重合，O₂X₃Y₃Z₃为将摄像机坐标系O₁X₁Y₁Z₁经平移至坐标系原点O₁与掘进机坐标系原点O₂重合后形成坐标系。

掘进机在摄像机坐标系下的完整空间位姿包括掘进机身质心，在摄像机坐标系中的位置坐标X′、Y′、Z′和掘进机轴线向量

相对于摄像机坐标系三个坐标轴的偏转角如航向角θ₂，即方位角、俯仰角

横滚角δ₂等六个参数。

Claims (3)

1.一种掘进机的空间位姿自动测量方法，其特征在于：使用的测量仪器为激光指向仪、工业摄像机、工控计算机、图像采集卡，其规格如下：

激光指向仪：YBJ-600

工业摄像机：KBA112

工控计算机：PCM-9387F

图像采集卡：CC₁410

测量方法如下：

I、布置掘进机及测量仪器，使其处于准工作状态

①将掘进机身(2)置于煤岩断面(3)的前部，截割头(8)接触煤岩断面3；

②将激光指向仪(4)、工业摄像机(5)、朝向煤岩断面(3)、截割头(8)；

③工业摄像机(5)通过导线(9)联接图像采集卡(7)、工控计算机(6)；

II、激光指向仪光束与摄像机光轴方向相对位姿测量

①设定光斑接收装置

在掘进机身后部设置平面形光斑接收装置，平面上水平设置九个距离特征点，并将其排列为三行三列，行、列之间为等间距，九个特征点处于同一平面内，并平行、垂直左、右、上、下轴线；

②调整激光指向仪、工业摄像机与光斑接收装置之间的相对位置

调整激光指向仪、工业摄像机指向，使激光指向仪与其中一个特征点重合；

③确定光斑接收装置上的A、B两个特征点为激光指向仪、工业摄像机的基准点，A、B特征点处于同一水平面上；

④调整并确定工业摄像机、激光指向仪与A、B基准点的水平距离；

⑤调整并确定工业摄像机与光斑接收装置之间距离、工业摄像机与激光指向仪之间距离；

⑥采集光斑接收装置上两个特征点图像，并由工控计算机处理，计算激光指向仪光束与工业摄像机光轴之间相对位姿；

⑦识别图像中两光斑特征，将二者绕原点旋转，使连线与横轴平行，转过的角度即为工业摄像机的横滚角；

⑧基于前面所得数据，用旋转后的主特征点的横坐标，计算出工业摄像机光轴与激光指向仪光束之间的偏转角，由纵坐标算出工业摄像机光轴与激光指向仪光束之间的俯仰角；

⑨激光指向仪与工业摄像机为刚性固定联接，二者空间位姿只需检测一次；

III、工业摄像机对掘进机身的空间位姿测量

①特征点阵设置

掘进机身后部的光斑接收装置上的九个特征点，在同一水平面上，不共线，即机身位姿；

②将工业摄像机与激光指向仪悬挂于掘进机后部巷道壁顶上，并调整激光指向仪、工业摄像机指向，并固定，使掘进机身上的光斑接收装置的特征点均置于摄像机视场内，视场角为±60°，直线距离为10-12米；

③采集一组特征点阵图像，输入工控计算机，计算工业摄像机对特征点阵的空间位姿；

④提取图像中特征点区域并做标记，检测特征点位置坐标；

⑤根据特征点阵图像特征，自动完成特征点的图像匹配；

⑥在获得全部特征点及图像坐标数据后，用混合标定法对工业摄像机进行标定，计算出工业摄像机对特征点阵的空间位姿；

IV、将步骤II所得工业摄像机光轴与激光指向仪光束之间的空间位姿数据，和步骤III中所得工业摄像机与特征点阵相对位姿数据进行合并，计算出特征点阵，即：掘进机身相对激光指向仪光束，即：理论巷道轴线的空间位姿。

2.根据权利要求1所述的一种掘进机的空间位姿自动测量方法，其特征在于：所述的掘进机的空间位姿测量设备的安装及布置为：由光斑接收装置特征点阵(1)、激光指向仪(4)、工业摄像机(5)、工控计算机(6)、图像采集卡(7)组成，激光指向仪(4)与工业摄像机(5)通过锚杆固定在掘进机身(2)后部10-12米的巷道壁顶上，光斑接收装置特征点阵(1)置于掘进机身(2)后部，光斑接收装置特征点阵(1)上设置九个特征点，激光指向仪、工业摄像机最大覆盖角为±60°，激光指向仪(4)、工业摄像机(5)、工控计算机(6)、图像采集卡(7)通过导线(9)联接。

3.根据权利要求1所述的一种掘进机的空间位姿自动测量方法，其特征在于：所述的掘进机空间位姿测量相关计算如下：

2)计算工业摄像机(5)相对激光指向仪(4)的空间位姿；

如图2所示：

①在任意铅垂面π上水平设置两个特征点A和B；

②于距离铅垂面π为L处悬挂激光指向仪(4)和工业摄像机(5)，且使工业摄像机(5)垂直指向铅垂面π；

③在与铅垂面π平行的平面内移动激光指向仪(4)和工业摄像机(5)，使激光指向仪(5)发出光束光斑与特征点A重合；

用工业摄像机(5)采集两特征点图像，如图3、4所示，两特征点对应图像中心分别为a(x_A，y_A)、b(x_B，y_B)，则工业摄像机(5)横滚角为：

${\mathrm{\δ}}_1=\arctan \left(\frac{y_A-y_B}{x_A-x_B}\right)$ ①

将a(x_A，y_A)和b(x_B，y_B)围绕原点o旋转角度δ₁，旋转后的a点坐标为：

$\left(\begin{array}{c}{x_A}^{\′}\\ {y_A}^{\′}\end{array}\right)=\left(\begin{array}{cc}\cos {\mathrm{\δ}}_1& \sin {\mathrm{\δ}}_1\\ -\sin {\mathrm{\δ}}_1& \cos {\mathrm{\δ}}_1\end{array}\right)\·\left(\begin{array}{c}{x}_A\\ y_A\end{array}\right)$ ②

则工业摄像机(5)轴线相对激光指向仪(4)光束的偏航角：

${\mathrm{\θ}}_1=\arctan \left(\frac{{x_A}^{\′}}{f}\right)$ ③

工业摄像机(5)轴线相对激光指向仪(4)光束的俯仰角：

式中，H为工业摄像机(5)与激光指向仪(4)安装距离，L为工业摄像机(5)与铅垂面π之间距离，f为工业摄像机(5)的镜头焦距。

2)计算工业摄像机(5)相对特征点阵(1)的空间位姿：

①提取特征点阵图像中所有特征点区域并作标记，检测每一特征点区域中心位置坐标；

②根据特征点阵图像特征，完成特征点及其图像匹配；

③在获得特征点及其图像坐标数据的基础上，利用混合标定法对工业摄像机(5)进行标定，实时计算工业摄像机(5)相对特征点阵(1)的空间位姿；

计算过程如下：

设各特征点空间坐标为P_i(X_2，i，y_2，i，Z_2，i)，对应图像中各光斑中心坐标为p_i(x_i，y_i)，其中i＝1，2，…，9，按照摄像机成像模型和标定理论，令

A_18×8·M_8×1＝B_18×1                    ⑤

式中，A_18×8为18行8列且参数已知的矩阵，B_18×1为18行1列且参数已知的矩阵，M_8×1为8行1列的矩阵，包含8个未知参数m_j，其中j＝1，2，…，8，具体为：

$A_{18\×8}=\left(\begin{array}{cccccccc}f\·X_{\mathrm{2,1}},& f\·Y_{\mathrm{2,1}}& 0,& 0,& x_1\·X_{\mathrm{2,1}},& x_1\·Y_{\mathrm{2,1}},& f,& 0\\ 0,& 0,& f\·X_{\mathrm{2,1}}\text{,}& f\·Y_{\mathrm{2,1}}& y_1\·X_{\mathrm{2,1}},& y_1\·Y_{\mathrm{2,1}},& 0,& f\\ & & & \·& & & & \\ & & & \·& & & & \\ & & & \·& & & & \\ f\·X_{\mathrm{2,9}},& f\·Y_{\mathrm{2,9}},& 0,& 0,& x_9\·X_{\mathrm{2,9}},& x_9\·Y_{\mathrm{2,9}},& f,& 0\\ 0,& 0,& f\·X_{\mathrm{2,9}},& f{\·Y}_{\mathrm{2,9}}& y_9\·X_{\mathrm{2,9}},& y_9\·Y_{\mathrm{2,9}},& 0,& f\end{array}\right)$

$B_{18\×1}=\left(\begin{array}{c}{x}_1\\ y_1\\ \·\\ \·\\ \·\\ x_9\\ y_9\end{array}\right),$ $M_{8\×1}=\left(\begin{array}{c}{m}_1\\ \·\\ \·\\ \·\\ m_8\end{array}\right)$

对上式作矩阵变换，得

M_8×1＝(A_18×8′·A_18×8)^-1·A_18×8′·B_18×1           ⑥

式中，A_18×8′表示矩阵A_18×8的转置，(A_18×8′·A_18×8)^-1表示矩阵A_18×8′·A_18×8的逆矩阵。

则

掘进机身(2)与工业摄像机(5)在摄像机光轴Y₁方向上距离

$Z^{\′}=f+\frac{1}{\sqrt{{m_1}^2+{m_3}^2+{m_5}^2}}$ ⑦

掘进机身(2)在工业摄像机(5)横轴X₁方向偏移距离

X′＝m₇·(f-Z′)                ⑧

掘进机身(2)在工业摄像机(5)纵轴Z₁方向偏移距离

Y′＝m₈·(f-Z′)                ⑨

掘进机身(2)轴线矢量

在摄像机坐标系中偏航角

θ₂＝-arcsin(m₅·(f-Z′))       ⑩

掘进机身(2)轴线矢量

在摄像机坐标系中横滚角

${\mathrm{\δ}}_2=-\arcsin \left(\frac{m_3\·(f-Z^{\′})}{\cos {\mathrm{\θ}}_2}\right)$

掘进机身(2)轴线矢量

在摄像机坐标系中俯仰角

3)求解掘进机身(2)相对激光指向仪(4)光束的空间位姿：

工业摄像机(5)光轴与激光指向仪(4)所发出光束即理论巷道轴线之间空间位姿数据，和工业摄像机(5)与特征点阵(1)所在平面即掘进机身(2)相对位姿数据通过空间坐标变换进行合并，即可计算出掘进机身(2)相对理论巷道轴线的空间位姿，其中，掘进机身(2)空间位置由下式计算：

$\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)=R_{3\×3}\·\left(\begin{array}{c}{X}^{\′}\\ Y^{\′}\\ Z^{\′}\end{array}\right)+\left(\begin{array}{c}0\\ H\\ 0\end{array}\right)$

式中，R_3×3为3行3列且参数已知的矩阵，具体为：

掘进机身(2)空间姿态由下式计算：

掘进机身(2)轴线矢量

在摄像机坐标系中偏航角：

θ＝θ₁+θ₂

掘进机身(2)轴线矢量

在摄像机坐标系中横滚角：

δ＝δ₁+δ₂

掘进机身(2)轴线矢量

在摄像机坐标系中俯仰角：

式中：

OXYZ：世界坐标系，用于表示理论巷道；

X：水平且垂直于理论巷道轴向；

Y：平行于理论巷道轴向；

Z：与X轴、Y轴正交的方向

O：激光指向仪(4)激光出口

O₁X₁Y₁Z₁：工业摄像机(5)坐标系

X₁：平行于工业摄像机(5)成像元件水平扫描方向

Y₁：工业摄像机(5)光轴方向

Z₁：平行于工业摄像机(5)成像元件竖直扫描方向

O₁：工业摄像机(5)光心

O₂X₂Y₂Z₂：特征点阵(1)坐标系，同掘进机身(2)坐标系；

X₂：平行于掘进机身(2)左右向轴线方向

Y₂：平行于掘进机身(2)前后向轴线方向，同特征点阵(1)所在平面法线方向

Z₂：平行于掘进机身(2)上下向轴线方向

O₂：特征点阵(1)中心

oxy：图像坐标系

x：工业摄像机(5)成像元件水平扫描方向

y：工业摄像机(5)成像元件竖直扫描方向

O₂X₃Y₃Z₃：O₁X₁Y₁Z₁平移，使其坐标系原点O₁与坐标系O₂X₂Y₂Z₂原点O₂重合后所形成的坐标系

X₃：平行于工业摄像机(5)成像元件水平扫描方向

Y₃：工业摄像机(5)光轴方向

Z₃：平行于工业摄像机(5)成像元件竖直扫描方向

掘进机身(2)前后轴向矢量

掘进机身(2)在世界坐标系OXYZ中的俯仰角

θ：掘进机身(2)在世界坐标系OXYZ中的偏转角

δ：掘进机身(2)在世界坐标系OXYZ中的横滚角

掘进机身(2)在工业摄像机(5)坐标系O₁X₁Y₁Z₁中的俯仰角

θ₂：掘进机身(2)在工业摄像机(5)坐标系O₁X₁Y₁Z₁中的偏转角

δ₂：掘进机身(2)在工业摄像机(5)坐标系O₁X₁Y₁Z₁中的横滚角

工业摄像机(5)在世界坐标系OXYZ中的俯仰角

θ₁：工业摄像机(5)在世界坐标系OXYZ中的偏转角

δ₁：工业摄像机(5)在世界坐标系OXYZ中的横滚角

H：工业摄像机(5)与激光指向仪(4)的垂直安装距离

L：工业摄像机(5)与铅垂面π之间的距离

Images