CN116929337A - 一种掘进机导航定位系统性能提升方法 - Google Patents

Abstract

本发明提供了一种掘进机导航定位系统性能提升方法，在巷道测量相机拍摄的图像中，按照顺时针的顺序找出四个光学靶标点T_A、T_B、T_C、T_D对应的四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧，随后将四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧连线构建成一个最小矩形，然后将最小矩形按照一定比例系数进行膨胀，构造辅助矩形区域框K_前帧；将辅助矩形区域框K_前帧的四个角点按照顺时针的顺序设定为O₁、O₂、O₃、O₄；在使用过程中，当掘进现场光学靶标点部分被遮挡时，采用该方法仍能够持续进行解算，解决了靶标点被遮挡造成测量中断问题，并提升了掘进机导航定位系统的抗干扰性能。

Description

一种掘进机导航定位系统性能提升方法

技术领域

本发明涉及掘进机导航定位领域，具体是一种掘进机导航定位系统性能提升方法。

背景技术

随着国家智慧矿山建设要求的提出，煤矿智能化建设也得到了广泛的关注和投入。掘进工作面作为煤矿井下最重要和最危险的工作区域，其中的核心设备掘进机的智能化是煤矿智能化建设的重中之重。掘进机导航定位系统是掘进机智能化的关键组成部分。目前掘进机的导航定位技术主要有基于惯性导航技术、激光雷达技术、视觉测量技术和组合导航技术等几种方案，其中“惯性导航+视觉测量”的组合导航方案是目前现场应用最广、效果最好的方案之一。

“惯性导航+视觉测量”的组合导航系统通常由三部分组成，包括悬挂在掘进机后端巷道顶板的视觉测量设备、安装在掘进机机身上的光学靶标、惯性导航装置。视觉测量设备采用相机观测掘进机机身上的光学靶标，对掘进机进行粗定位。随后粗定位信息发送给惯导装置，采用组合导航算法，提供掘进机更加精确的位置和姿态信息。通常情况下，光学靶标的形式是由固定在同一平面上、排列参数已知的一系列发光点组成。最常见的形式是4个发光点构成一个确定长宽尺寸的矩形靶标。

在实际使用过程中，光学靶标与惯导组成掘进机的位姿测量装置，并且固定在掘进机机身尾部，并且朝向视觉测量设备。视觉测量设备观测到靶标点后，采用视觉测量的PNP算法计算得到靶标中心位置。但由于掘进工作面设备繁多、情况复杂，经常会出现其他设备(如电缆、锚杆钻臂、风筒、一运机尾等)遮挡光学靶标点的情况，进而导致相机得到的靶标点图形与预置靶标图形不符，造成PNP算法无法正常解算，从而造成导航定位中断。

发明内容

本发明的目的在于提供一种掘进机导航定位系统性能提升方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

一种掘进机导航定位系统性能提升方法，包括如下步骤：

步骤1、在巷道测量相机拍摄的图像中，按照顺时针的顺序找出四个光学靶标点T_A、T_B、T_C、T_D对应的四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧，且四个光斑像点对应的像素点坐标分别为(u_{A_前帧},v_{A__前帧})、(u_{B_前帧},v_{B_前帧})、(u_{C_前帧},v_{C_前帧})、(u_{D_前帧},v_{D_前帧})；随后将四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧连线构建成一个最小矩形，然后将最小矩形按照一定比例系数进行膨胀，构造辅助矩形区域框K_前帧；将辅助矩形区域框K_前帧的四个角点按照顺时针的顺序设定为O₁、O₂、O₃、O₄，并且获取对应的像素点坐标分别为(u_O1,v_O1)、(u_O2,v_O2)、(u_O3,v_O3)、(u_O4,v_O4)；

A_前帧、B_前帧、C_前帧、D_前帧构成的最小矩形与O₁、O₂、O₃、O₄构成的辅助矩形区域框之间的相对位置关系为：A_前帧点距离O₁点欧式距离最小，B_前帧点距离O₂点欧式距离最小，C_前帧点距离O₃点欧式距离最小，D_前帧点距离O₄点欧式距离最小；

步骤2、在获取到下一帧图像后，根据步骤1中记录的辅助矩形区域框四个角点的像素点坐标(u_O1,v_O1)、(u_O2,v_O2)、(u_O3,v_O3)、(u_O4,v_O4)，在新图像中画出辅助矩形区域框K_后帧，并在新图像中的辅助矩形区域框K_后帧内搜索新的光斑像点；

步骤3、如果下一帧图像中的四个光学靶标点没有发生遮挡，则在新的辅助矩形区域框K_后帧内仍可找到四个光斑像点；根据光斑像点与辅助矩形区域框K_后帧中四个角点的欧式距离对应关系，将新图像中的四个光斑像点重新标记为A_后帧、B_后帧、C_后帧、D_后帧；

步骤4、如果下一帧图像中的四个光学靶标点中的某一个靶标点被遮挡，则在新的辅助矩形区域框K_后帧内将找到的三个光斑像点，根据步骤3中的方法，对检测到的三个光斑像点进行标记，标记结果为A_后帧、B_后帧、C_后帧或A_后帧、B_后帧、D_后帧或A_后帧、C_后帧、D_后帧或B_后帧、C_后帧、D_后帧四种情况；随后结合新图像中已知的三个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标；

步骤5、如果下一帧图像中的四个光学靶标点中的某两个靶标点被遮挡，则在新的辅助矩形区域框K_后帧内将找到的两个光斑像点，根据步骤3中的方法，对检测到的两个光斑像点进行标记，标记结果为A_后帧、B_后帧或A_后帧、C_后帧或A_后帧、D_后帧或B_后帧、C_后帧或B_后帧、D_后帧或C_后帧、D_后帧六种情况；随后结合新图像中已知的两个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标；

步骤6、根据步骤3、步骤4、步骤5中的方法，无论新图像中的四个光学靶标点是否被遮挡，采用上述步骤均可将四个光斑像点A_后帧、B_后帧、C_后帧、D_后帧在图像中补齐；随后使用P4P算法解算出靶标中心位置，并根据步骤1和步骤2中的方法构造辅助矩形区域框K′_前帧传递给下一帧图像。

作为本发明进一步的方案：在步骤1中，四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧连线构建成的最小矩形按照1.2-1.5倍的比例系数进行膨胀。

作为本发明进一步的方案：在步骤3中，将新图像中的四个光斑像点重新标记为A_后帧、B_后帧、C_后帧、D_后帧的标记方法包括如下步骤：

步骤3.1、已知四个光斑像点的像素点坐标和辅助矩形区域框K_后帧的四个角点的像素点坐标，分别求出四个光斑像点距离辅助矩形区域框K_后帧的四个角点的欧式距离；

步骤3.2、距离O_{1_前帧}点欧式距离最小的光斑像点标记为A_后帧点；距离O_{2_前帧}点欧式距离最小的光斑像点标记为B_后帧点；距离O_{3_前帧}点欧式距离最小的光斑像点标记为C_后帧点；距离O_{4_前帧}点欧式距离最小的光斑像点标记为D_后帧点。

作为本发明进一步的方案：在步骤4中，结合新图像中已知的三个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标的方法包括如下步骤：

步骤4.1、已知光学靶标上四个靶标点构成宽为2a、高为2b的矩形靶面，并与其成像的光斑像点A_后帧、B_后帧、C_后帧、D_后帧一一对应；在光学靶标的平面坐标系中，四个靶标点的坐标分别为T_A(-a、0、b)、T_B(a、0、b)、T_C(a、0、-b)、T_D(-a、0、-b)；利用与光学靶标固定在一起的惯导系统提供的航向和姿态信息，将靶面坐标系通过姿态矩阵变换生成与相机成像平面平行的投影靶面；投影靶面中四个靶标点的投影为T'_A、T'_B、T'_C、T'_D，它们的坐标表示为：

其中：

α为惯导给出的俯仰角、β为惯导给出的横滚角、γ为惯导给出的航向角与掘进巷道的航向角之差；

步骤4.2、如果被遮挡点为D_后帧点时，则已知像素点坐标A_后帧(u_{A_后帧},v_{A__后帧})、B_后帧(u_{B_后帧},v_{B_后帧})、C_后帧(u_{C_后帧},v_{C_后帧})，同时已知T'_A、T'_B、T'_C、T'_D四个投影点的坐标，则可得到各个投影点之间的距离L_{TA'_TB'}＝L_{TC'_TD'}＝q,L_{TA'_TD'}＝L_{TB'_TC'}＝p，L_{TA'_TC'}＝L_{TB'_TD'}＝m，假设被遮挡的D_后帧点的像素点坐标为D_后帧(u_{D_后帧},v_{D__后帧}),通过比例关系则有：

由以上三式求得像素点坐标D_后帧(u_{D_后帧},v_{D__后帧})；

步骤4.3、如果被遮挡点为C_后帧点时，在步骤4.1的基础上，被遮挡的C_后帧点像素点坐标C_后帧(u_{C_后帧},v_{C_后帧})通过下式求得：

步骤4.4、如果被遮挡点为B_后帧点时，在步骤4.1的基础上，被遮挡的B_后帧点像素点坐标B_后帧(u_{B_后帧},v_{B_后帧})通过下式求得：

步骤4.5、如果被遮挡点为A_后帧点时，在步骤4.1的基础上，被遮挡的A_后帧点像素点坐标A_后帧(u_{A_后帧},v_{A__后帧})通过下式求得：

作为本发明进一步的方案：在步骤5中，结合新图像中已知的两个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标的方法包括如下步骤：

步骤5.1、如果被遮挡点为C_后帧、D_后帧点时，按照步骤4.1中的方法得到光学靶标上四个靶标点通过姿态矩阵变换计算得到的投影靶面中四个靶标投影点的坐标T'_A、T'_B、T'_C、T'_D，以及各个投影点之间的距离L_{TA'_TB'}＝L_{TC'_TD'}＝q,L_{TA'_TD'}＝L_{TB'_TC'}＝p，L_{TA'_TC'}＝L_{TB'_TD'}＝m；假设被遮挡的C_后帧点的像素点坐标为C_后帧(u_{C_后帧},v_{C_后帧})、D_后帧点的像素点坐标为D_后帧(u_{D_后帧},v_{D__后帧})，通过比例关系则有：

由以上两式可以得到C_后帧点的像素点坐标有两个解(u'_{C_后帧},v'_{C_后帧})、(u'_C'_{_后帧},v'_C'_{_后帧})，再计算这两解对应的点距O₃(u_O3,v_O3)点的欧式距离，欧氏距离最小的点为C_后帧(u_{C_后帧},v_{C_后帧})；

同理可得：

由以上两式可得D_后帧点的像素点坐标有两个解(u'_{D_后帧},v'_{D_后帧})、(u″_{D_后帧},v″_{D_后帧})，计算这两点距O₄(u_O4,v_O4)点的欧式距离，欧氏距离最小的点为D_后帧(u_{D_后帧},v_{D__后帧})；

步骤5.2、如果被遮挡点为B_后帧、D_后帧点时，采用5.1步骤中的方法，可以得到：

由以上两式可得B_后帧点的像素点坐标有两个解(u'_{B_后帧},v'_{B_后帧})、(u″_{B_后帧},v″_{B_后帧})，计算这两点距O₂(u_O2,v_O2)点的欧式距离，欧氏距离最小的点为B_后帧(u_{B_后帧},v_{B_后帧})；

同理可得：

由以上两式可得D_后帧点的像素点坐标有两个解(u'_{D_后帧},v'_{D_后帧})、(u″_{D_后帧},v″_{D_后帧})，计算这两点距O₄(u_O4,v_O4)点的欧式距离，欧氏距离最小的点为D_后帧(u_{D_后帧},v_{D_后帧})；

步骤5.3、如果被遮挡点为B_后帧、C_后帧点时，采用5.1步骤中的方法，可以得到：

由以上两式可得B_后帧点的像素点坐标有两个解(u'_{B_后帧},v'_{B_后帧})、(u'_B'_{_后帧},v'_B'_{_后帧})，计算这两点距O₂(u_O2,v_O2)点的欧式距离，欧氏距离最小的点为B_后帧(u_{B_后帧},v_{B_后帧})；

同理可得：

由以上两式可得C_后帧点的像素点坐标有两个解(u'_{C_后帧},v'_{C_后帧})、(u'_C'_{_后帧},v'_C'_{_后帧})，计算这两点距O₃(u_O3,v_O3)点的欧式距离，欧氏距离最小的点为C_后帧(u_{C_后帧},v_{C_后帧})；

步骤5.4、如果被遮挡点为A_后帧、D_后帧点时，采用5.1步骤中的方法，可以得到：

以上两式可得A_后帧点的像素点坐标有两个解(u'_{A_后帧},v'_{A_后帧})、(u'_A'_{_后帧},v'_A'_{_后帧})，计算这两点距O₁(u_O1,v_O1)点的欧式距离，欧氏距离最小的点为A_后帧(u_{A_后帧},v_{A_后帧})；

同理可得：

由以上两式可得D_后帧点的像素点坐标有两个解(u'_{D_后帧},v'_{D_后帧})、(u″_D__后帧,v″_{D_后帧})，计算这两点距O₄(u_O4,v_O4)点的欧式距离，欧氏距离最小的点为D_后帧(u_{D_后帧},v_{D_后帧})；

步骤5.5、如果被遮挡点为A_后帧、C_后帧点时，采用5.1步骤中的方法，可以得到：

以上两式可得A_后帧点的像素点坐标有两个解(u'_{A_后帧},v'_{A_后帧})、(u″_{A_后帧},v″_{A_后帧})，计算这两点距O₁(u_O1,v_O1)点的欧式距离，欧氏距离最小的点为A_后帧(u_{A_后帧},v_{A_后帧})；

同理可得：

由以上两式可得C_后帧点的像素点坐标有两个解(u'_{C_后帧},v'_{C_后帧})、(u″_C__后帧,v″_C__后帧)，计算这两点距O₃(u_O3,v_O3)点的欧式距离，欧氏距离最小的点为C_后帧(u_{C_后帧},v_{C_后帧})；

步骤5.6、如果被遮挡点为A_后帧、B_后帧点时，采用5.1步骤中的方法，可以得到：

以上两式可得A_后帧点的像素点坐标有两个解(u'_{A_后帧},v'_{A_后帧})、(u″_A__后帧,v″_A__后帧，计算这两点距O₁(u_O1,v_O1)点的欧式距离，欧氏距离最小的点为A_后帧(u_{A_后帧},v_{A_后帧})；

同理可得：

由以上两式可得B_后帧点的像素点坐标有两个解(u'_{B_后帧},v'_{B_后帧})、(u″_{B_后帧},v″_{B_后帧})，计算这两点距O₂(u_O2,v_O2)点的欧式距离，欧氏距离最小的点为B_后帧(u_{B_后帧},v_{B_后帧})。

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

本发明采用上述方法后，对于采用视觉测量、四点光学靶标和惯导的掘进机导航定位系统，首先对靶标点在成像平面中的光斑像点进行编号，并在起外围构造辅助矩形区域框，并将该矩形框传递给下一帧图像；在下一帧图像中的矩形框范围内查找光学靶标点对应的光斑像点，并根据与矩形框角点的欧式距离对光斑像点进行编号；如果矩形框范围内存在四个光学靶标点对应的光斑像点，则采用PNP算法解算靶标中心点位置坐标；如果光学靶标点中部分点受遮挡导致矩形框范围内光斑像点数量为三个或两个时，则通过图像中已知的光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的点的像素点坐标，并将光斑像点补全至四个，再采用PNP算法解算靶标中心点位置坐标。在使用过程中，当掘进现场光学靶标点部分被遮挡时，采用该方法仍能够持续进行解算，解决了靶标点被遮挡造成测量中断问题，并提升了掘进机导航定位系统的抗干扰性能。

附图说明

图1为掘进机导航定位系统示意图。

图2为掘进机导航定位系统性能提升方法的流程图。

图3为构造辅助矩形区域框方法的示意图。

图4为四个光斑像点构成的最小矩形顶点与辅助矩形区域框顶点相对位置关系的示意图。

图5为由三个可观测光斑像点条件下补齐成完整四个光斑像点平面的示意图。

图6为光学靶标通过惯导提供的姿态数据变换生成与相机成像平面平行的投影靶面示意图。

图7为由两个可观测光斑像点条件下补齐成完整四个光斑像点平面的示意图。

具体实施方式

下面结合具体实施方式对本专利的技术方案作进一步详细地说明。

请参阅图1-7，掘进机导航定位系统的基本工作原理如图2所示，视觉测量设备(内含测量相机)安装在巷道顶板上，并朝向掘进机。通过人工调整，确保测量相机的测量光轴与巷道掘进方向一致，且成像平面与巷道掘进方向平行。光学靶面和惯导系统集成在一起并安装在掘进机机尾，光学靶标朝向测量相机。测量相机通过检测光学靶面上的4个发光靶标点，并采用PNP算法求解靶面中心点坐标，将该坐标传递给惯导装置，随后采用组合导航算法求解掘进机的定位信息和姿态信息。

掘进机导航定位系统性能提升方法的工作流程图如图2所示，具体的，一种掘进机导航定位系统性能提升方法，包括如下步骤：

步骤1、在巷道测量相机拍摄的图像中，按照顺时针的顺序找出四个光学靶标点T_A、T_B、T_C、T_D对应的四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧，如图3所示。且四个光斑像点对应的像素点坐标分别为(u_{A_前帧},v_{A__前帧})、(u_{B_前帧},v_{B_前帧})、(u_{C_前帧},v_{C_前帧})、(u_{D_前帧},v_{D_前帧})；随后将四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧连线构建成一个最小矩形，然后将最小矩形按照1.2-1.5倍的比例系数进行膨胀，比例系数进行膨胀，构造辅助矩形区域框K_前帧；将辅助矩形区域框K_前帧的四个角点按照顺时针的顺序设定为O₁、O₂、O₃、O₄，并且获取对应的像素点坐标分别为(u_O1,v_O1)、(u_O2,v_O2)、(u_O3,v_O3)、(u_O4,v_O4)；构成的辅助矩形区域框K_前帧之间的相对位置关系如图4所示。A_前帧、B_前帧、C_前帧、D_前帧构成的最小矩形与O₁、O₂、O₃、O₄构成的辅助矩形区域框之间的相对位置关系为：A_前帧点距离O₁点欧式距离最小，B_前帧点距离O₂点欧式距离最小，C_前帧点距离O₃点欧式距离最小，D_前帧点距离O₄点欧式距离最小。

步骤2、在获取到下一帧图像后，根据步骤1中记录的辅助矩形区域框四个角点的像素点坐标(u_O1,v_O1)、(u_O2,v_O2)、(u_O3,v_O3)、(u_O4,v_O4)，在新图像中画出辅助矩形区域框K_后帧，并在新图像中的辅助矩形区域框K_后帧内搜索新的光斑像点；

步骤3、如果下一帧图像中的四个光学靶标点没有发生遮挡，则在新的辅助矩形区域框K_后帧内仍可找到四个光斑像点；根据光斑像点与辅助矩形区域框K_后帧中四个角点的欧式距离对应关系，将新图像中的四个光斑像点重新标记为A_后帧、B_后帧、C_后帧、D_后帧；

步骤4、如果下一帧图像中的四个光学靶标点中的某一个靶标点被遮挡，则在新的辅助矩形区域框K_后帧内将找到的三个光斑像点，根据步骤3中的方法，对检测到的三个光斑像点进行标记，标记结果为A_后帧、B_后帧、C_后帧或A_后帧、B_后帧、D_后帧或A_后帧、C_后帧、D_后帧或B_后帧、C_后帧、D_后帧四种情况；随后结合新图像中已知的三个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标；

步骤5、如果下一帧图像中的四个光学靶标点中的某两个靶标点被遮挡，则在新的辅助矩形区域框K_后帧内将找到的两个光斑像点，根据步骤3中的方法，对检测到的两个光斑像点进行标记，标记结果为A_后帧、B_后帧或A_后帧、C_后帧或A_后帧、D_后帧或B_后帧、C_后帧或B_后帧、D_后帧或C_后帧、D_后帧六种情况；随后结合新图像中已知的两个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标；

步骤6、根据步骤3、步骤4、步骤5中的方法，无论新图像中的四个光学靶标点是否被遮挡，采用上述步骤均可将四个光斑像点A_后帧、B_后帧、C_后帧、D_后帧在图像中补齐；随后使用P4P算法解算出靶标中心位置，并根据步骤1和步骤2中的方法构造辅助矩形区域框'

K_前帧传递给下一帧图像。

进一步的，在步骤3中，将新图像中的四个光斑像点重新标记为A_后帧、B_后帧、C_后帧、D_后帧的标记方法包括如下步骤：

步骤3.1、已知四个光斑像点的像素点坐标和辅助矩形区域框K_后帧的四个角点的像素点坐标，分别求出四个光斑像点距离辅助矩形区域框K_后帧的四个角点的欧式距离；

步骤3.2、距离O_{1_前帧}点欧式距离最小的光斑像点标记为A_后帧点；距离O_{2_前帧}点欧式距离最小的光斑像点标记为B_后帧点；距离O_{3_前帧}点欧式距离最小的光斑像点标记为C_后帧点；距离O_{4_前帧}点欧式距离最小的光斑像点标记为D_后帧点。

在本实施例中，以D_后帧点被遮挡为例对步骤4进行说明。当获取到的图像中仅有三个光斑像点时，先通过三个点分别与辅助矩形区域框K后帧的4个顶点O₁、O₂、O₃、O₄的距离关系，确定并标记三个光斑像点分别为A_后帧、B_后帧、C_后帧，如图5所示。具体的，在步骤4中，结合新图像中已知的三个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标的方法包括如下步骤：

步骤4.1、已知光学靶标上四个靶标点构成宽为2a、高为2b的矩形靶面，并与其成像的光斑像点A_后帧、B_后帧、C_后帧、D_后帧一一对应；在光学靶标的平面坐标系中，四个靶标点的坐标分别为T_A(-a、0、b)、T_B(a、0、b)、T_C(a、0、-b)、T_D(-a、0、-b)；利用与光学靶标固定在一起的惯导系统提供的航向和姿态信息，将靶面坐标系通过姿态矩阵变换生成与相机成像平面平行的投影靶面，如图6所示。投影靶面中四个靶标点的投影为T'_A、T'_B、T'_C、T'_D，它们的坐标表示为：

其中：

α为惯导给出的俯仰角、β为惯导给出的横滚角、γ为惯导给出的航向角与掘进巷道的航向角之差；

步骤4.2、如果被遮挡点为D_后帧点时，则已知像素点坐标A_后帧(u_{A_后帧},v_{A__后帧})、B_后帧(u_{B_后帧},v_{B_后帧})、C_后帧(u_{C_后帧},v_{C_后帧})，同时已知T'_A、T'_B、T'_C、T'_D四个投影点的坐标，则可得到各个投影点之间的距离L_{TA'_TB'}＝L_{TC'_TD'}＝q,L_{TA'_TD'}＝L_{TB'_TC'}＝p，L_{TA'_TC'}＝L_{TB'_TD'}＝m，假设被遮挡的D_后帧点的像素点坐标为D_后帧(u_{D_后帧},v_{D__后帧}),通过比例关系则有：

由以上三式求得像素点坐标D_后帧(u_{D_后帧},v_{D__后帧})，并将求解得到的D_后帧点补全到图像中，如图5所示。

同理当A_后帧、B_后帧、C_后帧点分别被遮挡住时，也可采用同样的方法求解得到其像素点坐标。

步骤4.3、如果被遮挡点为C_后帧点时，在步骤4.1的基础上，被遮挡的C_后帧点像素点坐标C_后帧(u_{C_后帧},v_{C_后帧})通过下式求得：

步骤4.4、如果被遮挡点为B_后帧点时，在步骤4.1的基础上，被遮挡的B_后帧点像素点坐标B_后帧(u_{B_后帧},v_{B_后帧})通过下式求得：

步骤4.5、如果被遮挡点为A_后帧点时，在步骤4.1的基础上，被遮挡的A_后帧点像素点坐标A_后帧(u_{A_后帧},v_{A__后帧})通过下式求得：

在本实施中，以C_后帧、、D_后帧两点被遮挡为例，如图7所示，获取到的图像中仅有两个光斑像点时，先通过2个点分别与辅助矩形区域框4个顶点O₁、O₂、O₃、O₄的距离关系，确定并标记两个光斑像点分别为A_后帧、B_后帧。具体的，在步骤5中，结合新图像中已知的两个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标的方法包括如下步骤：

步骤5.1、如果被遮挡点为C_后帧、D_后帧点时，按照步骤4.1中的方法得到光学靶标上四个靶标点通过姿态矩阵变换计算得到的投影靶面中四个靶标投影点的坐标T'_A、T'_B、T'_C、T'_D，以及各个投影点之间的距离L_{TA'_TB'}＝L_{TC'_TD'}＝q,L_{TA'_TD'}＝L_{TB'_TC'}＝p，L_{TA'_TC'}＝L_{TB'_TD'}＝m；假设被遮挡的C_后帧点的像素点坐标为C_后帧(u_{C_后帧},v_{C_后帧})、D_后帧点的像素点坐标为D_后帧(u_{D_后帧},v_{D__后帧})，通过比例关系则有：

/>

由以上两式可以得到C_后帧点的像素点坐标有两个解(u'_{C_后帧},v'_{C_后帧})、(u″_{C_后帧},v″_{C_后帧})，再计算这两解对应的点距O₃(u_O3,v_O3)点的欧式距离，欧氏距离最小的点为C_后帧(u_{C_后帧},v_{C_后帧})；

同理可得：

由以上两式可得D_后帧点的像素点坐标有两个解(u'_{D_后帧},v'_{D_后帧})、(u″_{D_后帧},v″_{D_后帧})，计算这两点距O₄(u_O4,v_O4)点的欧式距离，欧氏距离最小的点为D_后帧(u_{D_后帧},v_{D__后帧})，如图7所示。

步骤5.2、如果被遮挡点为B_后帧、D_后帧点时，采用5.1步骤中的方法，可以得到：

由以上两式可得B_后帧点的像素点坐标有两个解(u'_{B_后帧},v'_{B_后帧})、(u″_{B_后帧},v″_{B_后帧})，计算这两点距O₂(u_O2,v_O2)点的欧式距离，欧氏距离最小的点为B_后帧(u_{B_后帧},v_{B_后帧})；

同理可得：

由以上两式可得D_后帧点的像素点坐标有两个解(u'_{D_后帧},v'_{D_后帧})、(u″_{D_后帧},v″_{D_后帧})，计算这两点距O₄(u_O4,v_O4)点的欧式距离，欧氏距离最小的点为D_后帧(u_{D_后帧},v_{D_后帧})；

步骤5.3、如果被遮挡点为B_后帧、C_后帧点时，采用5.1步骤中的方法，可以得到：

/>

由以上两式可得B_后帧点的像素点坐标有两个解(u'_{B_后帧},v'_{B_后帧})、(u″_{B_后帧},v″_{B_后帧})，计算这两点距O₂(u_O2,v_O2)点的欧式距离，欧氏距离最小的点为B_后帧(u_{B_后帧},v_{B_后帧})；

同理可得：

由以上两式可得C_后帧点的像素点坐标有两个解(u'_{C_后帧},v'_{C_后帧})、(u″_{C_后帧},v″_{C_后帧})，计算这两点距O₃(u_O3,v_O3)点的欧式距离，欧氏距离最小的点为C_后帧(u_{C_后帧},v_{C_后帧})；

步骤5.4、如果被遮挡点为A_后帧、D_后帧点时，采用5.1步骤中的方法，可以得到：

以上两式可得A_后帧点的像素点坐标有两个解(u'_{A_后帧},v'_{A_后帧})、(u″_{A_后帧},v″_{A_后帧})，计算这两点距O₁(u_O1,v_O1)点的欧式距离，欧氏距离最小的点为A_后帧(u_{A_后帧},v_{A_后帧})；

同理可得：

由以上两式可得D_后帧点的像素点坐标有两个解(u'_{D_后帧},v'_{D_后帧})、(u″_{D_后帧},v″_{D_后帧})，计算这两点距O₄(u_O4,v_O4)点的欧式距离，欧氏距离最小的点为D_后帧(u_{D_后帧},v_{D_后帧})；

步骤5.5、如果被遮挡点为A_后帧、C_后帧点时，采用5.1步骤中的方法，可以得到：

/>

以上两式可得A_后帧点的像素点坐标有两个解(u'_{A_后帧},v'_{A_后帧})、(u″_{A_后帧},v″_{A_后帧})，计算这两点距O₁(u_O1,v_O1)点的欧式距离，欧氏距离最小的点为A_后帧(u_{A_后帧},v_{A_后帧})；

同理可得：

由以上两式可得C_后帧点的像素点坐标有两个解(u'_{C_后帧},v'_{C_后帧})、(u″_{C_后帧},v″_{C_后帧})，计算这两点距O₃(u_O3,v_O3)点的欧式距离，欧氏距离最小的点为C_后帧(u_{C_后帧},v_{C_后帧})；

步骤5.6、如果被遮挡点为A_后帧、B_后帧点时，采用5.1步骤中的方法，可以得到：

以上两式可得A_后帧点的像素点坐标有两个解(u'_{A_后帧},v'_{A_后帧})、(u″_{A_后帧},v″_{A_后帧}，计算这两点距O₁(u_O1,v_O1)点的欧式距离，欧氏距离最小的点为A_后帧(u_{A_后帧},v_{A_后帧})；

同理可得：

由以上两式可得B_后帧点的像素点坐标有两个解(u'_{B_后帧},v'_{B_后帧})、(u″_{B_后帧},v″_{B_后帧})，计算这两点距O₂(u_O2,v_O2)点的欧式距离，欧氏距离最小的点为B_后帧(u_{B_后帧},v_{B_后帧})。

上面对本专利的较佳实施方式作了详细说明，但是本专利并不限于上述实施方式，在本领域的普通技术人员所具备的知识范围内，还可以在不脱离本专利宗旨的前提下做出各种变化。

Claims (5)

1.一种掘进机导航定位系统性能提升方法，其特征在于，包括如下步骤：

步骤1、在巷道测量相机拍摄的图像中，按照顺时针的顺序找出四个光学靶标点T_A、T_B、T_C、T_D对应的四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧，且四个光斑像点对应的像素点坐标分别为(u_{A_前帧},v_{A__前帧})、(u_{B_前帧},v_{B_前帧})、(u_{C_前帧},v_{C_前帧})、(u_{D_前帧},v_{D_前帧})；随后将四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧连线构建成一个最小矩形，然后将最小矩形按照一定比例系数进行膨胀，构造辅助矩形区域框K_前帧；将辅助矩形区域框K_前帧的四个角点按照顺时针的顺序设定为O₁、O₂、O₃、O₄，并且获取对应的像素点坐标分别为(u_O1,v_O1)、(u_O2,v_O2)、(u_O3,v_O3)、(u_O4,v_O4)；

A_前帧、B_前帧、C_前帧、D_前帧构成的最小矩形与O₁、O₂、O₃、O₄构成的辅助矩形区域框之间的相对位置关系为：A_前帧点距离O₁点欧式距离最小，B_前帧点距离O₂点欧式距离最小，C_前帧点距离O₃点欧式距离最小，D_前帧点距离O₄点欧式距离最小；

步骤2、在获取到下一帧图像后，根据步骤1中记录的辅助矩形区域框四个角点的像素点坐标(u_O1,v_O1)、(u_O2,v_O2)、(u_O3,v_O3)、(u_O4,v_O4)，在新图像中画出辅助矩形区域框K_后帧，并在新图像中的辅助矩形区域框K_后帧内搜索新的光斑像点；

步骤3、如果下一帧图像中的四个光学靶标点没有发生遮挡，则在新的辅助矩形区域框K_后帧内仍可找到四个光斑像点；根据光斑像点与辅助矩形区域框K_后帧中四个角点的欧式距离对应关系，将新图像中的四个光斑像点重新标记为A_后帧、B_后帧、C_后帧、D_后帧；

步骤4、如果下一帧图像中的四个光学靶标点中的某一个靶标点被遮挡，则在新的辅助矩形区域框K_后帧内将找到的三个光斑像点，根据步骤3中的方法，对检测到的三个光斑像点进行标记，标记结果为A_后帧、B_后帧、C_后帧或A_后帧、B_后帧、D_后帧或A_后帧、C_后帧、D_后帧或B_后帧、C_后帧、D_后帧四种情况；随后结合新图像中已知的三个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标；

步骤5、如果下一帧图像中的四个光学靶标点中的某两个靶标点被遮挡，则在新的辅助矩形区域框K_后帧内将找到的两个光斑像点，根据步骤3中的方法，对检测到的两个光斑像点进行标记，标记结果为A_后帧、B_后帧或A_后帧、C_后帧或A_后帧、D_后帧或B_后帧、C_后帧或B_后帧、D_后帧或C_后帧、D_后帧六种情况；随后结合新图像中已知的两个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标；

步骤6、根据步骤3、步骤4、步骤5中的方法，无论新图像中的四个光学靶标点是否被遮挡，采用上述步骤均可将四个光斑像点A_后帧、B_后帧、C_后帧、D_后帧在图像中补齐；随后使用P4P算法解算出靶标中心位置，并根据步骤1和步骤2中的方法构造辅助矩形区域框K'_前帧传递给下一帧图像。

2.根据权利要求1所述的一种掘进机导航定位系统性能提升方法，其特征在于，在步骤1中，四个光斑像点A_前帧、B_前帧、C_前帧、D_前帧连线构建成的最小矩形按照1.2-1.5倍的比例系数进行膨胀。

3.根据权利要求1所述的一种掘进机导航定位系统性能提升方法，其特征在于，在步骤3中，将新图像中的四个光斑像点重新标记为A_后帧、B_后帧、C_后帧、D_后帧的标记方法包括如下步骤：

步骤3.1、已知四个光斑像点的像素点坐标和辅助矩形区域框K_后帧的四个角点的像素点坐标，分别求出四个光斑像点距离辅助矩形区域框K_后帧的四个角点的欧式距离；

步骤3.2、距离O_{1_前帧}点欧式距离最小的光斑像点标记为A_后帧点；距离O_{2_前帧}点欧式距离最小的光斑像点标记为B_后帧点；距离O_{3_前帧}点欧式距离最小的光斑像点标记为C_后帧点；距离O_{4_前帧}点欧式距离最小的光斑像点标记为D_后帧点。

4.根据权利要求1所述的一种掘进机导航定位系统性能提升方法，其特征在于，在步骤4中，结合新图像中已知的三个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标的方法包括如下步骤：

步骤4.1、已知光学靶标上四个靶标点构成宽为2a、高为2b的矩形靶面，并与其成像的光斑像点A_后帧、B_后帧、C_后帧、D_后帧一一对应；在光学靶标的平面坐标系中，四个靶标点的坐标分别为T_A(-a、0、b)、T_B(a、0、b)、T_C(a、0、-b)、T_D(-a、0、-b)；利用与光学靶标固定在一起的惯导系统提供的航向和姿态信息，将靶面坐标系通过姿态矩阵变换生成与相机成像平面平行的投影靶面；投影靶面中四个靶标点的投影为T'_A、T'_B、T'_C、T'_D，它们的坐标表示为：

其中：

α为惯导给出的俯仰角、β为惯导给出的横滚角、γ为惯导给出的航向角与掘进巷道的航向角之差；

步骤4.2、如果被遮挡点为D_后帧点时，则已知像素点坐标A_后帧(u_{A_后帧},v_{A__后帧})、B_后帧(u_{B_后帧},v_{B_后帧})、C_后帧(u_{C_后帧},v_{C_后帧})，同时已知T'_A、T'_B、T'_C、T'_D四个投影点的坐标，则可得到各个投影点之间的距离L_{TA'_TB'}＝L_{TC'_TD'}＝q,L_{TA'_TD'}＝L_{TB'_TC'}＝p，L_{TA'_TC'}＝L_{TB'_TD'}＝m，假设被遮挡的D_后帧点的像素点坐标为D_后帧(u_{D_后帧},v_{D__后帧}),通过比例关系则有：

由以上三式求得像素点坐标D_后帧(u_{D_后帧},v_{D__后帧})；

步骤4.3、如果被遮挡点为C_后帧点时，在步骤4.1的基础上，被遮挡的C_后帧点像素点坐标C_后帧(u_{C_后帧},v_{C_后帧})通过下式求得：

步骤4.4、如果被遮挡点为B_后帧点时，在步骤4.1的基础上，被遮挡的B_后帧点像素点坐标B_后帧(u_{B_后帧},v_{B_后帧})通过下式求得：

步骤4.5、如果被遮挡点为A_后帧点时，在步骤4.1的基础上，被遮挡的A_后帧点像素点坐标A_后帧(u_{A_后帧},v_{A__后帧})通过下式求得：

5.根据权利要求4所述的一种掘进机导航定位系统性能提升方法，其特征在于，在步骤5中，结合新图像中已知的两个光斑像点的像素点坐标、惯导姿态参数与靶标光斑几何参数，求解被遮挡的光斑像点的像素点坐标的方法包括如下步骤：

步骤5.1、如果被遮挡点为C_后帧、D_后帧点时，按照步骤4.1中的方法得到光学靶标上四个靶标点通过姿态矩阵变换计算得到的投影靶面中四个靶标投影点的坐标T'_A、T'_B、T'_C、T'_D，以及各个投影点之间的距离L_{TA'_TB'}＝L_{TC'_TD'}＝q,L_{TA'_TD'}＝L_{TB'_TC'}＝p，L_{TA'_TC'}＝L_{TB'_TD'}＝m；假设被遮挡的C_后帧点的像素点坐标为C_后帧(u_{C_后帧},v_{C_后帧})、D_后帧点的像素点坐标为D_后帧(u_{D_后帧},v_{D__后帧})，通过比例关系则有：

由以上两式可以得到C_后帧点的像素点坐标有两个解(u'_{C_后帧},v'_{C_后帧})、(u″_{C_后帧},v″_{c_后帧})，再计算这两解对应的点距O₃(u_O3,v_O3)点的欧式距离，欧氏距离最小的点为C_后帧(u_{C_后帧},v_{C_后帧})；

同理可得：

由以上两式可得D_后帧点的像素点坐标有两个解(u'_{D_后帧},v'_{D_后帧})、(u″_{D_后帧},v″_{D_后帧})，计算这两点距O₄(u_O4,v_O4)点的欧式距离，欧氏距离最小的点为D_后帧(u_{D_后帧},v_{D__后帧})；

步骤5.2、如果被遮挡点为B_后帧、D_后帧点时，采用5.1步骤中的方法，可以得到：

由以上两式可得B_后帧点的像素点坐标有两个解(u'_{B_后帧},v'_{B_后帧})、(u″_{B_后帧},v″_{B_后帧})，计算这两点距O₂(u_O2,v_O2)点的欧式距离，欧氏距离最小的点为B_后帧(u_{B_后帧},v_{B_后帧})；

同理可得：

由以上两式可得D_后帧点的像素点坐标有两个解(u'_{D_后帧},v'_{D_后帧})、(u″_{D_后帧},v″_{D_后帧})，计算这两点距O₄(u_O4,v_O4)点的欧式距离，欧氏距离最小的点为D_后帧(u_{D_后帧},v_{D_后帧})；

步骤5.3、如果被遮挡点为B_后帧、C_后帧点时，采用5.1步骤中的方法，可以得到：

由以上两式可得B_后帧点的像素点坐标有两个解(u'_{B_后帧},v'_{B_后帧})、(u″_{B_后帧},v″_{B_后帧})，计算这两点距O₂(u_O2,v_O2)点的欧式距离，欧氏距离最小的点为B_后帧(u_{B_后帧},v_{B_后帧})；

同理可得：

由以上两式可得C_后帧点的像素点坐标有两个解(u'_{C_后帧},v'_{C_后帧})、(u″_{C_后帧},v″_{C_后帧})，计算这两点距O₃(u_O3,v_O3)点的欧式距离，欧氏距离最小的点为C_后帧(u_{C_后帧},v_{C_后帧})；

步骤5.4、如果被遮挡点为A_后帧、D_后帧点时，采用5.1步骤中的方法，可以得到：

以上两式可得A_后帧点的像素点坐标有两个解(u'_{A_后帧},v'_{A_后帧})、(u″_{A_后帧},v″_{A_后帧})，计算这两点距O₁(u_O1,v_O1)点的欧式距离，欧氏距离最小的点为A_后帧(u_{A_后帧},v_{A_后帧})；

同理可得：

由以上两式可得D_后帧点的像素点坐标有两个解(u'_{D_后帧},v'_{D_后帧})、(u″_{D_后帧},v″_{D_后帧})，计算这两点距O₄(u_O4,v_O4)点的欧式距离，欧氏距离最小的点为D_后帧(u_{D_后帧},v_{D_后帧})；

步骤5.5、如果被遮挡点为A_后帧、C_后帧点时，采用5.1步骤中的方法，可以得到：

以上两式可得A_后帧点的像素点坐标有两个解(u'_{A_后帧},v'_{A_后帧})、(u″_{A_后帧},v″_{A_后帧})，计算这两点距O₁(u_O1,v_O1)点的欧式距离，欧氏距离最小的点为A_后帧(u_{A_后帧},v_{A_后帧})；

同理可得：

由以上两式可得C_后帧点的像素点坐标有两个解(u'_{C_后帧},v'_{C_后帧})、(u″_{C_后帧},v″_{C_后帧})，计算这两点距O₃(u_O3,v_O3)点的欧式距离，欧氏距离最小的点为C_后帧(u_{C_后帧},v_{C_后帧})；

步骤5.6、如果被遮挡点为A_后帧、B_后帧点时，采用5.1步骤中的方法，可以得到：

以上两式可得A_后帧点的像素点坐标有两个解(u'_{A_后帧},v'_{A_后帧})、(u″_{A_后帧},v″_{A_后帧}，计算这两点距O₁(u_O1,v_O1)点的欧式距离，欧氏距离最小的点为A_后帧(u_{A_后帧},v_{A_后帧})；

同理可得：

由以上两式可得B_后帧点的像素点坐标有两个解(u'_{B_后帧},v'_{B_后帧})、(u″_B__后帧,v″_{B_后帧})，计算这两点距O₂(u_O2,v_O2)点的欧式距离，欧氏距离最小的点为B_后帧(u_{B_后帧},v_{B_后帧})。