CN111553948A - 一种基于双示踪物的掘进机截割头定位***及方法 - Google Patents

Abstract

本发明公开了掘进机截割头的定位方法，其利用两个红外相机对截割头上的两个示踪物进行摄像，将获得的两个示踪物的图像进行图像处理进行轮廓提取处理；根据设定的特征值分割图像中最佳的一块或多块区域，将所述区域的边界曲线提取为方向链码，计算目标区域周长和面积，根据周长和面积计算出圆的最小半径和最大半径，采用最小半径和最大半径作为径向约束条件，选取合格的边界点，从而获得掘进机中的双示踪物的区域，根据双示踪物的二维信息获得其三维信息，并结合掘进机的自身参数获取截割头中心的三维坐标，从而实现对掘进机截割头的定位。

Description

一种基于双示踪物的掘进机截割头定位***及方法

技术领域

本发明属于立体视觉定位技术领域，涉及一种基于双示踪物的掘进机截割头定位***及方法。

背景技术

煤炭作为我国最主要的一次能源之一，是我国能源的基石。随着科学技术的进步，综采技术水平不断 提高。其安全可靠、经济高效的自动化采煤技术变得越来越重要。

近些年来国内外提出了多种解决方案，但是在掘进机截割头定位上还存在诸多问题。故本发明提出一 种面向掘进机的截割头定位的方法。

现今大多数关于截割头定位基本使用普通光学摄像机定位，例如：2014年5月，陕西陕煤黄陵矿业有 限公司完成了1.4m-2.2m煤层国产装备智能化无人综采技术研究与应用项目，其截割头定位方法使用普通 光学摄像机，未使用其他示踪物进行定位，其可视化、截割头三维信息精确化低，不能很好的适应未来数 字化、信息化发展。

在精确定位截割头方法上采用过单目单示踪物方法，但是在实际实验过程中截割头定位不准确，其截 割头三维坐标时常发生坐标点漂移，若应用在实际生产中可能会造成重大事故。

发明内容

本发明申请提供一种双示踪物定位方法，用于掘进机截割头的定位，从而解决现有技术中存在的截割 头定位不准确的问题。

本发明申请采用以下技术方案：

一种掘进机截割头的定位***，其特征在于：包括恒温球形双示踪物、左相机、右相机、以及地面服 务站；所述左相机和右相机均为红外相机；恒温球形双示踪物位于掘进机前端，左相机和右相机设置于掘 进机上且位于所述双示踪物后方，用于拍摄所述双示踪物的图像；所述地面服务站与左相机和右相机进行 无线通信，用于处理左相机和右相机采集的所述双示踪物的图像，从而获得掘进机截割头的准确位置。

一种掘进机截割头的定位方法，包括以下步骤：

1)利用左右红外相机获取图像，对所述图像进行二值化处理后得到二值化图像，对所述二值化图像进行 轮廓提取处理；

2)、所述轮廓提取处理包括：根据设定的特征值分割掘进机场景中最佳的一块或多块区域，并将所述一 块或多块区域的边界曲线提取为弗雷曼链码并归一化；计算目标区域周长Z和面积S；根据周长Z和面积 S计算得出圆的最小半径和最大半径：

R_max＝Z/2π；

3)、根据聚类思想和不共线三点确定圆心原理，采用R_min，R_max作为径向约束条件作预处理，利用最小 二乘法求取圆心；从所述链码中顺序选取不共线的三点，根据几何原理，不共线三点可唯一确定一个外接 圆，由此求得圆心坐标和半径。

4)、根据径向约束条件，选取合格的边界点；若不满足R_min＜R＜R_max，则顺次取不共线三点计算圆心 及半径R，直至满足约束条件的三点定为基准点；以三个基准点的前两个点为基础，继续从所述链码中选 取下一个点，并以新三点计算圆心坐标和半径R；若满足R_min＜R＜R_max，则保存该点，继续按顺序选区 下一点，不满足则从所述链码中删除该点；直到遍历所有链码，以去除无效或误差过大的边界点；

5)、使用这些合格的边界点，根据最小二乘法拟合获得圆心坐标；然后根据这些合格的边界点到圆心的 距离的方均根求得半径R。

6)、如果步骤2)中的上述根据设定的特征值分割掘进机场景中最佳的一块或多块区域为多块区域，则选 取半径R为最大的一个区域，作为场景中掘进机双示踪物的区域。

7)、根据步骤6)获得的双示踪物的区域后将双示踪物的二维信息转换成双示踪物的三维信息；根据左右 相机的成像公式获得掘进机上的第一示踪物的三维坐标和第二示踪物的三维坐标(第一示踪物和第二示踪 物为上述双示踪物)；

8)、根据上述第一示踪物的三维坐标和第二示踪物的三维坐标，结合掘进机的机臂直径、截割头轮廓形 状以及质心参数，获得截割头中心的三维坐标，从而实现对截割头的定位。

本发明申请通过利用双示踪物进行定位，通过采集截割头在工作中的红外图像，对红外图像进行二值化处 理，并进行轮廓提取处理，以获得截割头的轮廓，然后利用根据双示踪物在图像中的坐标，结合掘进机和 截割头的参数获取截割头中心三维坐标，从而对截割头进行准确定位。

附图说明

图1截割部定位***整体示意图；

图2双目视觉中的三种坐标系；

图3双目视觉中相机几何关系；

图4掘进机工作时截割头的中心点三维坐标；

图5双示踪物定位流程示意图。

具体实施方式

如图1所示，提供一种掘进机截割头定位***，包括：恒温球形双示踪物1、识别双示踪物的红外摄 像机2、以及进行运算处理的地面服务站3三大部分。恒温球形双示踪物位于掘进机前端；红外摄像机包 括左摄像机和右摄像机，布置于掘进机上，位于双示踪物后方，用于拍摄双示踪物的图像。地面服务站与 红外摄像机进行无线或有线地通讯，用于处理红外摄像机采集的双目标图像，从而获得截割头的准确位置。 下面详细介绍如何通过双示踪物的图像获取截割头的准确位置。

计算机视觉(Computer Vision)是使用计算机及图像采集设备对生物视觉的一种仿生模拟，根据包含一 张或多张图像的图像组解析示踪物体的三维信息。三维信息包括示踪物体的三维几何坐标，示踪物体之间 的遮挡、相交关系，示踪物体的三维运动信息等。双目视觉是一种利用数字图像恢复示踪物体三维几何信 息的计算机视觉算法。

双目立体视觉(Binocular Stereo Vision)是机器视觉的一种重要形式，它是基于视差原理并利用成像 设备从不同的位置获取被测物体的两幅图像，通过计算图像对应点间的位置偏差，来获取物体三维几何信 息的方法。使用双目立体视觉进行定位的算法简称双目定位。

如图2所示，讨论双目视觉原理涉及到三个坐标系：世界坐标系、相机坐标系和图像坐标系。世界坐 标系中的点坐标记为P(x_w,y_w,z_w)，在相机坐标系用(x_c,y_c,z_c)表示，图像坐标为相机所拍摄到的图像二 维坐标，一般分为两种：(u,v)是以像素为单位的图像坐标，(x,y)是以毫米为单位的图像坐标。建立以毫 米为单位的图像坐标是因为(u,v)坐标只表示了像素在数字图像中的行数和列数，并没有表示出该像素在 数字图像中的物理位置。

摄像机成像几何关系可由图1来表示。其中点O称为相机光心，x_c轴与y_c轴与图像的x轴与y轴平 行，z_c轴为摄像机的光心线，与图像平面垂直。由点O与轴x_c，y_c，z_c组成的直角坐标系即摄像机坐 标系。OO₁为摄像机焦距，记为f，其值是由摄像机本身决定的。光心线与图像平面的交点称为图像坐标 系的内极点O₁，其坐标习惯记为(u₀,v₀)，一般位于图像的中心。由摄像机拍摄的离散数字图像的像素坐 标用(u,v)表示，其坐标原点选在图像的左上角。一般情况下，摄像机成像模型为小孔成像模型，其成像 变换矩阵是一个透视变换矩阵。

根据摄像机成像原理，空间一点P(x_w,y_w,z_w)在图像中的像点P(u,v)，是点P与摄像机光心O的连 线与摄像机屏幕的交点。若已知空间一点在左、右摄像机中的像点分别为P₁(u₁,v₁)与P₂(u₂,v₂)，则原空 间点p位于空间射线O₁P₁与O₂P₂的延长线交点上，O₁、O₂分别为左右摄像机光心，对空间射线O₁P₁与 O₂P₂求交即可反求出P点的三维坐标如图3所示。因此，双目视觉可以看成是由图像坐标向三维世界坐 标影射的一个过程。

为了实现上述三维重构过程，必须解决如下两个问题：

(1)为了获取空间射线O₁P₁与O₂P₂的方程，首先须确定左、右摄像机各自的成像变换矩阵，求出成 像变换矩阵中的各元素，这一过程称为摄像机标定。

(2)像素点P₁(u₁,v₁)，与P₂(u₂,v₂)要对应于空间同一个点P(x_w,y_w,z_w)，这样才能保证空间射线 与O₁P₁与O₂P₂相交。像素P₁与P₂称为一对立体点对，已知二者之一，求另外一点的过程称为立体匹配。

基于上述讨论，双目视觉的基本原理可描述为，首先对左、右摄像机标定，确定各自的成像变换矩阵， 然后在左摄像机图像中指定一个像素点P₁(u₁,v₁)，在右摄像机图像中进行立体匹配，找出与之匹配的像 素点P₂(u₂,v₂)，最后对空间射线O₁P₁与O₂P₂求交，确定P₁与P₂共同对应的原空间点的三维坐标。

接下来，介绍摄像机的坐标标定。摄像机又名相机，摄像机标定即相机标定，下面介绍相机标定原理：

在双目视觉原理的三个坐标系分别为：世界坐标系中的点坐标记为p(x_w,y_w,z_w)，相机坐标系用p(x_c,y_c,z_c)，图像坐标系为(x,y)；三种坐标的关系如图2所示。

1、摄像机标定

摄像机标定是确定摄像机投影矩阵的一个数值过程。设内极点O₁在(u,v)坐标系中的坐标点(u₀,v₀)， 每一个像素在x轴与y轴方向上物理尺寸为dx、dy，则像素坐标与物理坐标(x,y)之间存在以下转换关 系：

x＝dx(u-u₀)

y＝dy(v-v₀)

将其写成矩阵形式：

由于摄像机可能是从任意角度和位置对世界坐标系中的场景生成图像，因此摄像机坐标系和世界坐标 系之间的关系可以用一个旋转矩阵R和一个平移向量t来描述。点P的坐标P(x_w,y_w,z_w)与P(x_c,y_c,z_c) 之间的变换关系可以写成：

其中R为3×3正交单位旋转矩阵，t为3×1平移向量，0＝(0,0,0)，M为4×4矩阵。

由图2可知，摄像机坐标系中任意一点P(x_c,y_c,z_c)在图像平面上的成像位置p(x,y)为光心O与P点 连线OP与图像平面交点，根据透视公式可得：

x＝fx_c/z_c

y＝fy_c/z_c

其中，f为摄像机焦距，z_c为点P在摄像机坐标系下的深度坐标。因此，物体的摄像机坐标与图像坐 标之间的透视投影关系可以表示为

把(1)(2)式代入(3)式，得到世界坐标系中的点P(x_w,y_w,z_w)在摄像机屏幕上对应的图像p(u,v)的计算 公式，为了不失一般性，可以引入扭曲参数γ，该参数用来表示像素坐标系两个坐标轴的扭曲：

其中f/dx、f/dy、γ、u₀、v₀只与摄像机内部结构有关，称这些参数为摄像机的内部参数，M¹称 为内参数矩阵，因于大多数标准相机而言γ参数可视为0，以下推导将其视为0考虑。M²取决于世界坐 标系的选取与摄像机放置方式，称为外参数矩阵。M＝M¹M²称为3×4成像变换矩阵，则：

摄像机的标定就是确定M矩阵中3×4＝12个分量的值，该过程可通过人机交互与最小二乘法完成，首 先选取一个标准测试物体(如标准柱面、标准倾斜平面)，选定合适的世界坐标系，并在标准测试物体上 标出若干测试点以p_i(i＝1,2,3,…,n)确定各测试点的三维坐标P(x_w,y_w,z_w)，通过人机交互方法确定各 测试点在图像中的像素坐标(u_i,v_i)，代入方程(5)，得到关于矩阵中的12个分量的N个方程，利用最 小二乘法求解得到矩阵中各分量，从而完成摄像机标定。

2、目标中心点像素坐标的确定

根据图3，需要将目标在左右相机中所成像的中心点p₁(u₁,v₁)与p₂(u₂,v₂)构成立体点对，才能最终 映射到原始的目标点。在掘进机工作时，与岩层或煤层持续摩擦而产生大量热量的截割部温度远高于周围 物体，且成像面积也是红外相机可拍摄到的红外热源中最大的。因此我们采用阈值分割和面积筛选的方法 可以方便的得到掘进机截割部在红外相机中所成的像。

阈值分割的基本原理是：通过设定特征阈值，把像素分为若干类。常用的特征包括直接来自原始图像 的灰度或色彩特征，以及由原始灰度或者彩色值变换得到的特征。因红外相机拍摄的图像直接以灰度图像 呈现，所以可以根据灰度范围来分割原始图像以得到对应实景中掘进机截割部的图像部分。

设原始图像为I(x,y)，将I(x,y)先腐蚀再膨胀，可以得到消除较小噪点的图像I₀(x,y)。按照一定 准则在I₀(x,y)中找到特征值T，将图像分割为两个部分，分割后的图像为：

取b₀＝0，b₁＝255，直接将阈值分割出的图像二值化。因掘进机截割部在工作时温度远高于环境温 度以及其他可能出现在红外相机图像在的热源，因此特征值T的选取较为简单，根据实验情况，一般选取 230～255间的特征值可以很好的分割出图像中包含对应实景中掘进机截割部的一块或多块儿区域。

在确定图像中包含对应实景中掘进机截割部的一块或多块区域后，将各区域其边界曲线提取为Freeman方向链码(弗雷曼链码)并归一化。即在示踪物边界曲线上选取一点作为起点，记录起点的坐标， 然后按照顺时针的方向，以8连通的方式从编码为1的方向(45°)开始搜索下一个边界像素点。找到之 后记录方向编码，然后从找到的像素点开始重复以上步骤，得出一组链码。

设任一区域的归一化Freeman方向链码为A(a₁,a₂...,a_n)，对应的像素坐标序列为B(b₁,b₂,...,b_n)， b_i(u_i,v_i)为链码a_i对应的像素坐标。

因链码的编码方式为使用0～7八个数字表示某像素和8邻域内各个像素的连线方向，其中偶数码为水 平或垂直方向链码，码长为1，奇数码为对角线方向链码，码长为

所以目标区域周长Z可以表示为：

其中n为链码序列中链码总数，n_e为链码序列中偶数码个数。目标区域面积S可以 表示为边界链码对x轴的积分：

其中v_i＝v_i-1+a_i2，v₀是初始点的纵坐标，a_i0和 a_i2分别是链码第i环的长度在k＝0(水平方向)和k＝2(垂直方向)的分量。由于掘进机截割部对应的 图像区域的边界链码为封闭链码，v₀可任意选取。

根据聚类思想并结合不共线三点确定圆心原理，采用

R_max＝Z/(2π)作为径向约束 预处理的最小二乘法求取圆心。

从边界链码中顺序选取不共线的三点A(u_a,v_a)、B(u_b,v_b)、C(u_c,v_c)，根据几何原理，不共线三点 可唯一确定一个外接圆，设圆心为(u₀,v₀)则有：

(u_a-u₀)²+(v_a-v₀)²＝(u_b-u₀)²+(v_b-v₀)²

(u_a-u₀)²+(v_a-v₀)²＝(u_c-u₀)²+(v_c-v₀)² (7)

化简得：

(u_a-u_b)u₀+(v_a-v_b)v₀＝(u_a ²-u_b ²)-(v_b ²-v_a ²)²/2

(u_a-u_c)u₀+(v_c-v_a)v₀＝(u_a ²-u_c ²)-(v_c ²-v_a ²)²/2 (8)

根据克莱姆法则求解得：

根据u₀、v₀可进一步求出半径R。如果求出的半径R不满足R_min＜R＜R_max，再顺次取不共线三点计 算圆心及半径R，直到满足R_min＜R＜R_max。将满足约束条件的三点定为基准三点，以基准三点的前两个 点为基础，继续从链码中选取下一个点，并以新三点计算圆心坐标和半径R，若半径R满足R_min＜R＜R_max， 保存该点，继续按顺序选区下一点，不满足则从链码中删除该点。循环执行此操作，直到遍历所有链码， 以去除无效或误差过大的边界点。

使用保留的边界链码表中对应的边界点坐标根据最小二乘法拟合圆心，设取出的边界点集合为 (u_i,v_i)(i＝1,2,...,n)，设与该组数据点的距离的平方和为最小的圆的方程为：

(u-u₀)²+(v-v₀)²＝R (10)

设a₁＝∑u_i，b₁＝∑v_i，a₂＝∑u_i ²，b₂＝∑v_i ²，a₃＝∑u_i ³，b₃＝∑v_i ³，c₁₁＝∑u_i*v_i，c₁₂＝∑u_i*v_i ²，c₂₁＝∑u_i ²*v_i，并设f(u₀,v₀,R)＝∑((u_i-u₀)²+(v_i-v₀)²-R²)²，令

即：

-4∑((u_i-u₀)²+(v_i-v₀)²-R²)(u_i-u₀)＝0

-4∑((u_i-u₀)²+(v_i-v₀)²-R²)(v_i-v₀)＝0

-4∑((u_i-u₀)²+(v_i-v₀)²-R²)R＝0 (11)

将a₁，b₁，a₂，b₂，a₃，b₃，c₁₁，c₁₂，c₂₁，带入(11)式并整理得：

a₁(u₀ ²+v₀ ²)-2a₂u₀-2c₁₁v₀+a₃+c₁₂-a₁R²＝0

b₁(u₀ ²+v₀ ²)-2c₁₁u₀-2b₂v₀+c₂₁+b₃-b₁R²＝0

n(u₀ ²+v₀ ²)-2a₁u₀-2b₁v₀+a₂+b₂-nR²＝0 (12)

解得：

因工作中的掘进机截割部在红外相机拍摄范围内是面积最大的红外热源，因此通过对比每张图像中所 有目标区域的半径R，取半径R最大的一个区域便可确定图像中对应实景中掘进机截割部的区域。

3、目标点空间坐标的确定

由成像变换公式(5)可知，已知空间点的二维图像坐标(u,v)，则式(5)成为关于x_w、y_w、z_w以及z_c的 一次代数方程组，对于两个摄像机，可以得到关于x_w、y_w、z_w以及z_c的两个极线方程，消去z_c后可得极 线交点，即为空间点P(x_w,y_w,z_w)。设左右两摄像机的成像公式分别为：

和

在两个成像变换公式中分别消去z_c1和z_c2，可以得到方程组：

设：

则

P＝(D^TD)^-1D^TH (15)

由(15)式可得P点的空间三维坐标x_w、y_w、z_w。

4、基于双示踪物的掘进机截割部定位

如图3所示，根据上文所述双目定位方法，可求出示踪物tp₁、tp₂的三维坐标obj₁(x_obj1,y_obj1,z_obj1)和 obj₂(x_obj2,，_obj2，z_obj2)。

为双示踪物构成的与掘进臂平行的直线距离掘进臂中心直线的距离，

为过tp₂做与掘进臂中心相交的垂直直线的交点md₂到截个部中心的距离。则向量

其中O＝(0,0,0)，其单位向量

当

由勾股定理可得

由几何关系可得

由于θ会影响sin(θ-π/2)的值，为将其拓展到π＞θ＞0范围内，令

则在π＞θ＞0范围内有：。

由(16)式即可得到掘进机截割头的精确三维位置。

至此我们根据示踪物的定位信息精确地对掘进机截割部进行识别定位。

下面，具体描述一下基于双示踪物的掘进机截割头定位的方法步骤：

1、利用左右红外相机获取图像，对所述图像进行二值化处理后得到二值化图像，对所述二值化图像进行 轮廓提取处理；

2、所述轮廓提取处理包括：根据设定的特征值分割掘进机场景中最佳的一块或多块区域，并将所述 一块或多块区域的边界曲线提取为Freeman方向链码并归一化；计算目标区域周长Z和面积S；根据周长 Z和面积S计算得出圆的最小半径和最大半径：

R_max＝Z/2π

3、根据聚类思想和不共线三点确定圆心原理，采用R_min，R_max作为径向约束条件作预处理，利用最 小二乘法求取圆心；从Freeman方向链码中顺序选取不共线的三点，根据几何原理，不共线三点可唯一确 定一个外接圆，由此求得圆心坐标和半径；

4、根据径向约束条件，选取合格的边界点；若不满足R_min＜R＜R_max，则顺次取不共线三点计算圆 心及半径R，直至满足约束条件的三点定为基准点；以三个基准点的前两个点为基础，继续从Freeman方 向链码中选取下一个点，并以新三点计算圆心坐标和半径R；若满足R_min＜R＜R_max，则保存该点，继续 按顺序选区下一点，不满足则从Freeman方向链码中删除该点；直到遍历所有链码，以去除无效或误差过 大的边界点；

5、使用这些合格的边界点，根据最小二乘法拟合获得圆心坐标；然后根据这些合格的边界点到圆心 的距离的方均根求得半径R；

6、如果步骤2)中的上述根据设定的特征值分割掘进机场景中最佳的一块或多块区域为多块区域，则 选取半径R为最大的一个区域，作为场景中掘进机双示踪物的区域。

7、根据步骤6)获得的双示踪物的区域后将双示踪物的二维信息转换成双示踪物的三维信息；根据左 右相机的成像公式获得掘进机上的第一示踪物的三维坐标和第二示踪物的三维坐标；

8、根据上述第一示踪物的三维坐标和第二示踪物的三维坐标，结合掘进机的机臂直径、截割头轮廓 形状以及质心参数，获得截割头中心的三维坐标，从而实现对截割头的定位。

上述方法步骤的流程示意图如图5所示。

需要注意的是，公布实施例的目的在于帮助进一步理解本发明，但是本领域的技术人员可以理解：在 不脱离本发明及所附权利要求的精神和范围内，各种替换和修改都是可能的。因此，本发明不应局限于实 施例所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。

Claims (2)

1.一种掘进机截割头的定位***，其特征在于：包括恒温球形双示踪物、左相机、右相机、以及地面服务站；所述左相机和右相机均为红外相机；恒温球形双示踪物位于掘进机前端，左相机和右相机设置于掘进机上且位于所述双示踪物后方，用于拍摄所述双示踪物的图像；所述地面服务站与左相机和右相机进行无线通信，用于处理左相机和右相机采集的所述双示踪物的图像，从而获得掘进机截割头的准确位置。

2.一种掘进机截割头的定位方法，其利用权利要求1所述的定位***对截割头进行定位，其特征在于：包括以下步骤：

1)利用左右红外相机获取图像，对所述图像进行二值化处理后得到二值化图像，对所述二值化图像进行轮廓提取处理；

2)、所述轮廓提取处理包括：根据设定的特征值分割掘进机场景中最佳的一块或多块区域，并将所述一块或多块区域的边界曲线提取为弗雷曼链码并归一化；计算目标区域周长Z和面积S；根据周长Z和面积S计算得出圆的最小半径和最大半径：

R_max＝Z/2π；

3)、根据聚类思想和不共线三点确定圆心原理，采用R_min，R_max作为径向约束条件作预处理，利用最小二乘法求取圆心；从所述链码中顺序选取不共线的三点，根据几何原理，不共线三点可唯一确定一个外接圆，由此求得圆心坐标和半径。

4)、根据径向约束条件，选取合格的边界点；若不满足R_min＜R＜R_max，则顺次取不共线三点计算圆心及半径R，直至满足约束条件的三点定为基准点；以三个基准点的前两个点为基础，继续从所述链码中选取下一个点，并以新三点计算圆心坐标和半径R；若满足R_min＜R＜R_max，则保存该点，继续按顺序选区下一点，不满足则从所述链码中删除该点；直到遍历所有链码，以去除无效或误差过大的边界点；

5)、使用这些合格的边界点，根据最小二乘法拟合获得圆心坐标；然后根据这些合格的边界点到圆心的距离的方均根求得半径R。

6)、如果步骤2)中的上述根据设定的特征值分割掘进机场景中最佳的一块或多块区域为多块区域，则选取半径R为最大的一个区域，作为场景中掘进机双示踪物的区域。

7)、根据步骤6)获得的双示踪物的区域后将双示踪物的二维信息转换成双示踪物的三维信息；根据左右相机的成像公式获得掘进机上的第一示踪物的三维坐标和第二示踪物的三维坐标(第一示踪物和第二示踪物为上述双示踪物)；

8)、根据上述第一示踪物的三维坐标和第二示踪物的三维坐标，结合掘进机的机臂直径、截割头轮廓形状以及质心参数，获得截割头中心的三维坐标，从而实现对截割头的定位。

Images