CN104729501A - 基于旋转扇面激光的悬臂式掘进机位姿测量方法 - Google Patents

Abstract

本发明涉及一种用于煤矿掘进工作面的悬臂式掘进机位姿测量方法，充分利用基于旋转扇面激光测角的空间点三维坐标测量***，即将该***的扇面激光发射端安装在悬臂式掘进机机身上，扇面激光接收端（包含3个不共线的扇面激光接收器）固定安装在已经由悬臂式掘进机掘出的煤巷后方，扇面激光接收器相对于空间大地坐标系的坐标值由其它测量方式测得。由扇面激光发射端测得扇面激光接收器相对于发射端的坐标值，再通过相应的坐标转换，计算出悬臂式掘进机上固定三点（不共线）相对于空间大地坐标系的三维坐标值。若已知空间刚体上固定三点（不共线）相对于空间大地坐标系的三维坐标值，可计算出该空间刚体的位姿（位置和姿态）数据。

Description

基于旋转扇面激光的悬臂式掘进机位姿测量方法

技术领域

本发明属于工程机械设备位姿测量技术领域，特别涉及一种对煤矿用悬臂式掘进机的位置和姿态进行测量，进而对悬臂式掘进机掘进方向进行精确指向。

背景技术

传统的悬臂式掘进机位姿测量方法是人工目视法，悬臂式掘进机在煤巷中唯一的参考来自于点激光指向仪。在悬臂式掘进机后方安放1台点激光指向仪(指示巷道设计方向)，其在煤巷前部掘进断面形成一个光斑，悬臂式掘进机司机在作业中须以肉眼观察掘进断面上光斑的位置，时时注意巷道走向是否正确。该方法精度低，操作复杂，耗费时间，可靠性差，全凭操作司机的经验来保证巷道成型质量。

随着技术的发展，出现了大量的现代化位姿测量方式，如自动全站仪位姿测量***和惯性导航位姿测量***。对于自动全站仪位姿测量***，机器人在地下行进过程中可以自动跟踪标靶，对目标点进行实时连续检测。但全站仪使用激光束作为测量介质来对空间中某点进行定位，只能实现点对点的测量，抗遮挡能力差，煤矿掘进工作面设备杂多，且煤巷走向起伏不平，通视情况差，故使用全站仪的话其激光束很容易被遮挡。对于惯性导航***，定位误差会随时间而增大，不适用于长距离长时间的测量，需要定期的矫正。而煤矿掘进工作面一般位于地下几十至几百米处，无法使用GPS等定位技术来对惯性导航***进行校正。故绝大部分现代化的位姿测量方式不适用于悬臂式掘进机位姿测量。

发明内容

本发明的目的在于提出一种利用基于扇面激光测角的空间点三维坐标测量***进行悬臂式掘进机位姿测量的方法，可实现对悬臂式掘进机的位置和姿态进行测量，大大提高了对悬臂式掘进机位姿的测量精确性，为悬臂式掘进机控制提供精确数据，以提高巷道成型质量。

本发明的技术方案：利用基于扇面激光测角的空间点三维坐标测量***进行悬臂式掘进机位姿测量，包含以下步骤：

(1)将扇面激光发射端安装到悬臂式掘进机上，使其与悬臂式掘进机固连。扇面激光接收端安装到已成型的巷道中，即3个不共线的扇面激光接收器。

(2)由扇面激光发射端构建一坐标系，称为机身坐标系(O-XYZ)。扇面激光发射端由高精度滑台、高精度转台、扇面激光发射器、起始基准组成，设扇面激光发射器初始位置旋转轴为Z轴，高精度滑台移动方向为X轴。

(3)由激光接收端，即3个不共线的扇面激光接收器构建一坐标系，称为巷道坐标系(O′ -X′ Y′ Z′)。

(4)在悬臂式掘进机处于截割状态时，扇面激光发射器在高精度滑台和转台的作用下，在两个位置(初始位置和移动位置)发射出三束扇面激光交汇到一点。依次测得3个扇面激光接收器在机身坐标系(O-XYZ)的三维坐标值，即(x₁,y₁,z₁)、(x₂,y₂,z₂)、(x₃,y₃,z₃)。

(5)如图1所示，由3个扇面激光接收器的三维坐标值计算得巷道坐标系(O′ -X′ Y′ Z′)相对于机身坐标系(O-XYZ)的各变换参数。T₁为巷道坐标系(O′ -X′ Y′ Z′)的X′轴在机身坐标系(O-XYZ)下的方向向量，T₂、T₃分别为Y′、Z′轴的方向向量，x₀为巷道坐标系(O′ -X′ Y′ Z′)的原点在机身坐标系(O-XYZ)下的坐标。

从而获得平移矩阵D₁和旋转矩阵R₁。

(6)用矿用激光全站仪测得3个扇面激光接收器相对于空间大地坐标系的三维坐标值，同样按照上述方法求得巷道坐标系(O′ -X′ Y′ Z′)与空间大地坐标系的平移矩阵D₂和旋转矩阵R₂，以及巷道坐标系原点在大地坐标系下的坐标(a，b，c)。

(7)将扇面激光发射端上固定三点，即机身坐标系(O-XYZ)上不共线固定3点变换至空间大地坐标系中，假设所取三点坐标分别为(100，0,0)、(0,100,0)、(0，0，100)，单位为mm。以(100,0,0)为例，变换公式如下：

(X,Y,Z,1)＝(100，0，0,1)·D₁·R₁·D₂·R₂

(X，Y，Z)即为机身坐标系中(100，0,0)在空间大地坐标系中的坐标值，依照上述公式分别求出(0,100,0)、(0，0，100)进行坐标系变换后的坐标值。

由于扇面激光发射端与悬臂式掘进机固联，故扇面激光发射端上固定三点就是悬臂式掘进机上固定三点。

(8)根据悬臂式掘进机上不共线固定三点相对于空间大地坐标系的三维坐标值，即可表示悬臂式掘进机在空间大地坐标系内的位置和姿态情况，由此实现对悬臂式掘进机的定位和姿态测量，为其进一步掘进提供精确数据。

本发明的有益效果：①基于旋转扇面激光测角的空间点三维坐标测量***是一种全自动化、高可靠性、高精度、低成本的三维坐标测量***，悬臂式掘进机在煤巷掘进时，将本***的扇面激光接收端(3个扇面激光接收器)固定在已掘巷道固定点处，将扇面激光发射端安装在悬臂式掘进机上，使其与悬臂式掘进机 固联，实现对悬臂式掘进机位姿状态的高精度、高自动化测量；②可达到1cm的定位精度和优于0.1°的姿态检测精度，完全满足对悬臂式掘进机的位姿测量需求，极大地提高了煤巷成型质量。

附图说明

图1为本发明实施例中悬臂式掘进机位姿测量过程示意图。

1：扇面激光发射器               2：高精度转台；

3：高精度滑台                   4：机身坐标系

5：巷道坐标系                   6：扇面激光接收器1

7：扇面激光接收器2              8：扇面激光接收器3

9：空间大地坐标系

具体实施方式

如图1所示，利用基于扇面激光测角的空间点三维坐标测量***进行悬臂式掘进机位姿测量的方法，位姿测量过程包含以下步骤：

(1)将扇面激光发射端安装到悬臂式掘进机上，使其与悬臂式掘进机固连。扇面激光接收端安装到已成型的巷道中，即3个不共线的扇面激光接收器。

(2)由扇面激光发射端构建一坐标系，称为机身坐标系(O-XYZ)。扇面激光发射端由高精度滑台、高精度转台、扇面激光发射器、起始基准组成，设扇面激光发射器初始位置旋转轴为Z轴，高精度滑台移动方向为X轴。

(3)由激光接收端，即3个不共线的扇面激光接收器构建一坐标系，称为巷道坐标系(O′ -X′ Y′ Z′)。

(4)在悬臂式掘进机处于截割状态时，扇面激光发射器在高精度滑台和转台的作用下，在两个位置(初始位置和移动位置)发射出三束扇面激光交汇到一点。依次测得3个扇面激光接收器在机身坐标系(O-XYZ)的三维坐标值，即扇面激光接收器1：(x₁,y₁,z₁)，扇面激光接收器2：(x₂,y₂,z₂)，扇面激光接收器3：(x₃,y₃,z₃)。

(5)如图1所示，由3个扇面激光接收器的三维坐标值计算得巷道坐标系(O′ -X′ Y′ Z′)相对于机身坐标系(O-XYZ)的各变换参数。T₁为巷道坐标系(O′ -X′ Y′ Z′)的X′轴在机身坐标系(O-XYZ)下的方向向量，T₂、T₃分别为Y′、Z′轴的方向向量，x₀为巷道坐标系(O′ -X′ Y′ Z′ )的原点在机身坐标系(O-XYZ)下的坐标。

因表达式过于复杂，故用t来代替部分表达式，设：

$t=\frac{(x_1-x_2)(x_3-x_1)+(y_1-y_2)(y_3-y_1)+(z_1-z_2)(z_3-z_1)}{(x_3-x_2)(x_3-x_1)+(y_3-y_2)(y_3-y_1)+(z_3-z_2)(z_3-z_1)}$

则坐标系变换需要的各参数表达式如下：

$x_0=(\frac{x_1-x_{3}t}{1-t},\frac{y_1-y_{3}t}{1-t},\frac{z_1-z_{3}t}{1-t})$

T₁＝T₂×T₃

T₂＝(x₃-x₁，y₃-y₁，z₃-z₁) 

$T_3=(x_2-\frac{x_1-x_{3}t}{1-t},y_2-\frac{y_1-y_{3}t}{1-t},z_2-\frac{z_1-z_{3}t}{1-t})$

平移矩阵D₁：

$\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ \frac{x_{3}t-x_1}{1-t}& \frac{y_{3}t-y_1}{1-t}& \frac{z_{3}t-z_1}{1-t}& 1\end{array}\right]$

旋转矩阵R₁：

$\left[\begin{array}{cccc}\·& \·& \·& 0\\ T_1& T_2& T_3& 0\\ \·& \·& \·& 0\\ 0& 0& 0& 1\end{array}\right]$

(6)用矿用激光全站仪测得3个扇面激光接收器相对于空间大地坐标系的三维坐标值，同样按照上述方法求得巷道坐标系(O′ -X′ Y′ Z′)与空间大地坐标系的平移矩阵D₂和旋转矩阵R₂，以及巷道坐标系原点在大地坐标系下的坐标(a，b，c)。

(7)将扇面激光发射端上固定三点，即机身坐标系(O-XYZ)上不共线固定3点变换至空间大地坐标系中，假设所取三点坐标分别为(100，0,0)、(0,100,0)、(0，0，100)，单位为mm。以(100,0,0)为例，变换公式如下：

(X,Y,Z,1)＝(100，0，0,1)·D₁·R₁·D₂·R₂

(X，Y，Z)即为机身坐标系中(100，0,0)在空间大地坐标系中的坐标值，依照上述公式分别求出(0,100,0)、(0，0，100)进行坐标系变换后的坐标值。

由于扇面激光发射端与悬臂式掘进机固联，故扇面激光发射端上固定三点就是悬臂式掘进机上固定三点。

(8)根据悬臂式掘进机上不共线固定三点相对于空间大地坐标系的三维坐标值，即可表示悬臂式掘进机在空间大地坐标系内的位置和姿态情况，由此实现对悬臂式掘进机的定位和姿态测量，为其进一步掘进提供精确数据。

Claims (1)

1.一种用于煤矿掘进工作面的悬臂式掘进机位姿测量方法，其特征在于位姿测量过程包含以下步骤：

(1)将扇面激光发射端安装到悬臂式掘进机上，使其与悬臂式掘进机固连。扇面激光接收端安装到已成型的巷道中，即3个不共线的扇面激光接收器。

(2)由扇面激光发射端构建一坐标系，称为机身坐标系(O-XYZ)。扇面激光发射端由高精度滑台、高精度转台、扇面激光发射器、起始基准组成，设扇面激光发射器初始位置旋转轴为Z轴，高精度滑台移动方向为X轴。

(3)由激光接收端，即3个不共线的扇面激光接收器构建一坐标系，称为巷道坐标系(O′-X′Y′Z′)。

(4)在悬臂式掘进机处于截割状态时，扇面激光发射器在高精度滑台和转台的作用下，在两个位置(初始位置和移动位置)发射出三束扇面激光交汇到一点。依次测得3个扇面激光接收器在机身坐标系(O-XYZ)的三维坐标值，即(x₁,y₁,z₁)、(x₂,y₂,z₂)、(x₃,y₃,z₃)。

(5)如图1所示，由3个扇面激光接收器的三维坐标值计算得巷道坐标系(O′-X′Y′Z′)相对于机身坐标系(O-XYZ)的各变换参数。T₁为巷道坐标系(O′-X′Y′Z′)的X′轴在机身坐标系(O-XYZ)下的方向向量，T₂、T₃分别为Y′、Z′轴的方向向量，x₀为巷道坐标系(O′-X′Y′Z′)的原点在机身坐标系(O-XYZ)下的坐标。

从而获得平移矩阵D₁和旋转矩阵R₁。

(6)用矿用激光全站仪测得3个扇面激光接收器相对于空间大地坐标系的三维坐标值，同样按照上述方法求得巷道坐标系(O′-X′Y′Z′)与空间大地坐标系的平移矩阵D₂和旋转矩阵R₂，以及巷道坐标系原点在大地坐标系下的坐标(a，b，c)。

(7)将扇面激光发射端上固定三点，即机身坐标系(O-XYZ)上不共线固定3点变换至空间大地坐标系中，假设所取三点坐标分别为(100，0,0)、(0,100,0)、(0，0，100)，单位为mm。以(100,0,0)为例，变换公式如下：

(X,Y,Z,1)＝(100，0，0,1)·D₁·R₁·D₂·R₂

(X，Y，Z)即为机身坐标系中(100，0,0)在空间大地坐标系中的坐标值，依照上述公式分别求出(0,100,0)、(0，0，100)进行坐标系变换后的坐标值。

由于扇面激光发射端与悬臂式掘进机固联，故扇面激光发射端上固定三点就是悬臂式掘进机上固定三点。

(8)根据悬臂式掘进机上不共线固定三点相对于空间大地坐标系的三维坐标值，即可表示悬臂式掘进机在空间大地坐标系内的位置和姿态情况，由此实现对悬臂式掘进机的定位和姿态测量，为其进一步掘进提供精确数据。