CN111780748B - 基于双目视觉和捷联惯导的掘进机位姿纠偏方法、系统 - Google Patents

Abstract

本发明提供了一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端，通过掘进机相对巷道起始位置的位移和掘进机的方位角进行误差校正，以给出准确的姿态和位置测量结果，确保挖掘精度，包括以下步骤：通过在巷道内设置的激光指向仪，在第一激光靶和第二激光靶上形成第一光斑和第二光斑；通过在掘进机上设置的摄像头，拍摄第一光斑和第二光斑，获得第一光斑、第二光斑在摄像头坐标中的位置；将第一光斑和第二光斑在摄像头坐标中的位置投影到水平坐标系中，计算得到掘进机的方位角；获取掘进机相对巷道起始位置的位移；掘进机的捷联惯导系统根据计算得到的掘进机相对巷道起始位置的位移和掘进机的方位角进行误差校正。

Description

基于双目视觉和捷联惯导的掘进机位姿纠偏方法、系统

技术领域

本发明涉及掘进机位姿测量技术领域，具体涉及一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端。

背景技术

掘进机挖掘巷道时，要求挖掘出的巷道沿激光指向仪给出的方向，且上下左右边界相对激光指向仪光线的距离满足预定要求。

现有掘进机一般采用人工操作，挖掘一段距离之后检查挖掘误差，然后进行人工修正的方法。此类方法存以下缺点：

1、不能连续工作、效率低

2、挖掘精度不高

3、人员工作环境差和危险大

公开号为CN103234513B的中国发明专利《基于摄像头和航姿仪的掘进自动导向系统及导向方法》提出了在车上安装一个摄像头和一个激光靶，通过摄像头检测光斑偏离激光靶中心的水平距离，然后根据该信息和掘进机航姿信息调整掘进机位置和方向，始终让掘进机保持在激光指向仪轴线上，以确保挖掘精度的方法。该方法要求激光指向仪始终能够照射到车上的激光靶中心附近，但巷道一般存在较大的高度变化，很难保证激光指向仪始终能够照射到车上的激光靶，所以该方法很难在工程中实际应用。

发明内容

针对上述问题，本发明提供了一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端，其不受巷道高度变化的影响，能够在掘进机工作的过程中，通过掘进机相对巷道起始位置的位移和掘进机的方位角进行误差校正，以给出准确的姿态和位置测量结果，确保挖掘精度。

其技术方案是这样的：一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于，包括以下步骤：

步骤1：通过在巷道内设置的激光指向仪，在第一激光靶和第二激光靶上形成第一光斑和第二光斑；

步骤2：通过在掘进机上设置的摄像头，拍摄第一光斑和第二光斑，获得第一光斑、第二光斑在摄像头坐标中的位置；

步骤3：将第一光斑和第二光斑在摄像头坐标中的位置投影到水平坐标系中，计算得到掘进机的方位角；

步骤4：获取第一激光靶或第二激光靶到巷道起始位置的位移和对应的第一光斑或第二光斑相对摄像头的位移，得到掘进机相对巷道起始位置的位移；

步骤5：掘进机的捷联惯导系统根据计算得到的掘进机相对巷道起始位置的位移和掘进机的方位角进行误差校正。

进一步的，在步骤2中，在掘进机上设置第一摄像头和第二摄像头，第一摄像头测量第二光斑在第一摄像头归一化成像平面上的像点坐标(u₅,v₅)，第二摄像头测量第二光斑在第二摄像头归一化成像平面上的像点坐标(u₆,v₆)，第二光斑在第一摄像头的坐标中的位置(X,Y,Z)与第二光斑在第一摄像头归一化成像平面上的像点坐标(u₅,v₅)以及第二光斑在第二摄像头归一化成像平面上的像点坐标(u₆,v₆)之间的关系表示为：

其中，f_x、f_y为摄像头参数；d为第一摄像头和第二摄像头之间的距离；

第一光斑在第一摄像头的坐标中的位置(X₁,Y₁,Z₁)与第一光斑在第一摄像头归一化成像平面上的像点坐标(u₅₁,v₅₁)以及第一光斑在第二摄像头归一化成像平面上的像点坐标(u₆₁,v₆₁)之间的关系表示为：

其中，f_x、f_y为摄像头参数；d为第一摄像头和第二摄像头之间的距离。

进一步的，在步骤3中，结合第一光斑在摄像头坐标中的位置，将第一光斑摄像头坐标中的位置投影到水平坐标系h，表示为：

其中，水平坐标系h原点与第一摄像头坐标的原点O₅重合，水平坐标系h与捷联惯导坐标系b之间仅存在俯仰角θ和横滚角γ，水平坐标系h与捷联惯导坐标系b的方位角一致，

为第一摄像头相对掘进机的安装矩阵，

为捷联惯导坐标系b到水平坐标系h的转换矩阵，表示为：

结合第二光斑在摄像头坐标中的位置，将第二光斑摄像头坐标中的位置投影到水平坐标系h，表示为

掘进机的方位角通过如下公式表示：

φ_m＝θ₄+φ-0.5π

其中，掘进机的方位角φ_m为掘进机纵轴线在水平面的投影Y_h与地理北向的夹角，φ为激光指向仪方位角；θ₄＝π-θ₁-θ₂-θ₃；将第一光斑、第二光斑摄像头坐标中的位置投影到水平坐标系h后，第一光斑在水平坐标系h中的投影为点P₁₀，第二光斑在水平坐标系h中的投影为点P₈，过点P₈和点P₁₀的直线与水平坐标系的Xh轴的交点为点A，水平面内过点P₁₀沿地理北向的直线与Yh轴的交点为点B，θ₁为Xh轴与线段P₈O₅的夹角，θ₂为线段P₈O₅与线段P₁₀O₅的夹角，θ₃为线段P₈P₁₀与线段P₁₀O₅的夹角，θ₄为线段P₈A与水平坐标系的Xh轴的夹角，θ₁＝atan(y_8h/x_8h)；θ₂＝atan(v_10h/u_10h)-θ₁；L为第一激光靶与第二激光靶之间的距离，记为线段P₈P₁₀的长度，线段O₅P₈的长度表示为

根据正弦定理，

得

由四边形AP₈BO₅内角和为2π的约束得(π-φ_m)+0.5π+θ₄+φ＝2π，故得φ_m＝θ₄+φ-0.5π。

进一步的，在步骤4中，先获取第一激光靶、第二激光靶与巷道起始位置之间的距离信息；根据第一激光靶或第二激光靶到巷道起始位置的距离，获取第一激光靶或第二激光靶到巷道起始位置的位移；根据第一光斑或第二光斑在摄像头坐标中的位置对应获取第一光斑或第二光斑相对摄像头的位移，然后计算得到掘进机相对巷道起始位置的位移。

进一步的，在步骤4中，通过设置在巷道起始位置的激光测距仪，获取第一激光靶、第二激光靶以及巷道起始位置之间的距离信息。

进一步的，在步骤4中，第二激光靶到巷道起始位置的位移通过如下公式计算：

其中，L₂为第二激光靶到巷道起始位置的距离，φ为激光指向仪的方位信息；

第二光斑相对第一摄像头的位移在掘进机的导航坐标系n中坐标通过如下公式表示：

其中，

为第一摄像头相对掘进机的安装矩阵，

为捷联惯导姿态矩阵；

掘进机相对巷道起始位置的位移通过如下公式表示：

其中，

为掘进机相对巷道起始位置的位移。

基于双目视觉和捷联惯导的掘进机位姿纠偏系统，其特征在于，包括：

第一激光靶，设置于巷道内；

第二激光靶，设置于巷道内且位于所述第一激光靶的后侧；

激光指向仪,设置于巷道的起始位置且能够在所述第一激光靶、第二激光靶上分别形成第一光斑、第二光斑；

激光测距仪，设置于巷道的起始位置，用于测量第一激光靶、第二激光靶与巷道起始位置之间的距离信息；

摄像头，包括安装于掘进机上的第一摄像头和第二摄像头，用于拍摄拍摄第一光斑、第二光斑；

通讯单元，用于所述激光测距仪、摄像头、数据处理单元、捷联惯导系统之间的通讯；

数据处理单元，所述数据处理单元能够根据掘进机在行进过程中摄像头拍摄到的第一光斑、第二光斑，获得第一光斑、第二光斑在摄像头坐标中的位置，进而获取第一光斑或第二光斑相对摄像头的位移；所述数据处理单元根据所述激光测距仪测量的对应的第一激光靶或第二激光靶到巷道起始位置的距离，获取第一激光靶或第二激光靶到巷道起始位置的位移；然后根据第一激光靶或第二激光靶到巷道起始位置的位移及对应的第一光斑或第二光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移；所述数据处理单元能够将第一光斑和第二光斑在摄像头坐标中的位置投影到水平坐标系中，结合第一激光靶和第二激光靶之间的距离，计算得到掘进机的方位角；

捷联惯导系统，用于根据计算得到的掘进机相对巷道起始位置的位移和掘进机的方位角进行误差校正。

进一步的，所述激光指向仪发射的光线平行于巷道的设定方向设置，所述激光测距仪的测量方向平行于激光指向仪发射的光线的方向。

进一步的，所述第一激光靶、第二激光靶分别对应安装在掘进机后方巷道顶面或侧面上。

一种终端，其特征在于，包括：

第一激光靶，设置于巷道内；

第二激光靶，设置于巷道内且位于所述第一激光靶的后侧；

激光指向仪,设置于巷道的起始位置且能够在所述第一激光靶、第二激光靶上分别形成第一光斑、第二光斑；

激光测距仪，用于测量第一激光靶、第二激光靶到巷道起始位置的距离；

摄像头，包括第一摄像头和第二摄像头，用于获取第一激光靶、第二激光靶上形成的第一光斑、第二光斑；

捷联惯导系统，用于对掘进机进行导航；

存储器，存储有数据处理程序；

处理器，用于执行所述数据处理程序，以实现如上述的基于双目视觉和捷联惯导的掘进机位姿纠偏方法。

本发明所能带来的有益效果：

本发明将两个激光靶悬挂在掘进机后面的巷道上，且确保激光指向仪始终能够照射到激光靶，不受巷道高度变化的影响，采用摄像头视觉测量的方法测量光斑在摄像头坐标中的位置，然后将光斑在摄像头坐标中的位置投影到水平坐标系中，计算得到掘进机的方位角；同时结合捷联惯性技术，通过激光靶到巷道起始位置的位移和对应的光斑相对摄像头的位移，得到掘进机相对巷道起始位置的位移，从而实时给出掘进机相对巷道起始位置的位移和掘进机的方位角，作为掘进机自动掘进的基准，在掘进机工作的过程中，校正捷联惯导的误差，以给出准确的姿态和位置测量结果，确保挖掘精度，也大大提高了巷道掘进的工作效率及工作质量，具有显著的经济效益和较高的工程应用价值，对实现高精度的自动掘进控制具有重要意义。

附图说明

图1为本发明的基于双目视觉和捷联惯导的掘进机位姿纠偏方法的流程图；

图2为本发明的方法中摄像头的归一化成像平面的示意图；

图3为本发明的方法中掘进机方位角计算原理示意图；

图4为本发明的基于双目视觉和捷联惯导的掘进机位姿纠偏系统的示意图；

图5为本发明的基于双目视觉和捷联惯导的掘进机位姿纠偏系统的组成框图。

具体实施方式

下面结合附图和实施例对本申请进行说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本申请，而非对本申请的限定。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互任意组合。另外还需要说明的是，为了便于描述，附图中仅示出了与本申请相关的部分而非全部结构。

应当理解，本公开的方法实施方式中记载的各个步骤可以按照不同的顺序执行，和/或并行执行。此外，方法实施方式可以包括附加的步骤和/或省略执行示出的步骤。本公开的范围在此方面不受限制。

需要注意，本公开中提及的“第一”、“第二”等概念仅用于对不同的装置、模块或单元进行区分，并非用于限定这些装置、模块或单元所执行的功能的顺序或者相互依存关系。

掘进机的捷联惯导系统，根据内部陀螺仪和加速度计测得的加速度和角速度信息不断计算掘进机的姿态矩阵

和相对巷道起始位置的位移信息，捷联惯导给出的姿态矩阵和位移信息的误差随时间不断累积，需要进行修正，为此，见图1、2、3，本实施例提供了一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法，其至少包括以下步骤：

步骤1：通过在巷道内设置的激光指向仪，在第一激光靶和第二激光靶上形成第一光斑和第二光斑；

步骤2：通过在掘进机上设置的摄像头，拍摄第一光斑和第二光斑，获得第一光斑、第二光斑在摄像头坐标中的位置；

步骤3：将第一光斑和第二光斑在摄像头坐标中的位置投影到水平坐标系中，计算得到掘进机的方位角；

步骤4：获取第一激光靶或第二激光靶到巷道起始位置的位移和对应的第一光斑或第二光斑相对摄像头的位移，得到掘进机相对巷道起始位置的位移；

步骤5：掘进机的捷联惯导系统根据计算得到的掘进机相对巷道起始位置的位移和掘进机的方位角进行误差校正。

具体在本实施例的步骤2中，在掘进机上设置第一摄像头和第二摄像头，见图2，第一摄像头测量第二光斑在第一摄像头归一化成像平面上的像点坐标(u₅,v₅)，第二摄像头测量第二光斑在第二摄像头归一化成像平面上的像点坐标(u₆,v₆)，第二光斑在第一摄像头的坐标中的位置(X,Y,Z)与第二光斑在第一摄像头归一化成像平面上的像点坐标(u₅,v₅)以及第二光斑在第二摄像头归一化成像平面上的像点坐标(u₆,v₆)之间的关系表示为：

其中，f_x、f_y为摄像头参数；d为第一摄像头和第二摄像头之间的距离；

第一光斑在第一摄像头的坐标中的位置(X₁,Y₁,Z₁)与第一光斑在第一摄像头归一化成像平面上的像点坐标(u₅₁,v₅₁)以及第一光斑在第二摄像头归一化成像平面上的像点坐标(u₆₁,v₆₁)之间的关系表示为：

其中，f_x、f_y为摄像头参数；d为第一摄像头和第二摄像头之间的距离。

具体在本实施例的步骤3中，结合第一光斑在摄像头坐标中的位置，将第一光斑摄像头坐标中的位置投影到水平坐标系h，表示为：

其中，见图3，水平坐标系h原点与第一摄像头坐标的原点O₅重合，水平坐标系h与捷联惯导坐标系b之间仅存在俯仰角θ和横滚角γ，水平坐标系h与捷联惯导坐标系b的方位角一致，

为第一摄像头相对掘进机的安装矩阵，

为捷联惯导坐标系b到水平坐标系h的转换矩阵，表示为：

结合第二光斑在摄像头坐标中的位置，将第二光斑摄像头坐标中的位置投影到水平坐标系h，表示为

掘进机的方位角通过如下公式表示：

φ_m＝θ₄+φ-0.5π

其中，掘进机的方位角φ_m为掘进机纵轴线在水平面的投影Y_h与地理北向的夹角，φ为激光指向仪方位角；θ₄＝π-θ₁-θ₂-θ₃；将第一光斑、第二光斑摄像头坐标中的位置投影到水平坐标系h后，第一光斑在水平坐标系h中的投影为点P₁₀，第二光斑在水平坐标系h中的投影为点P₈，过点P₈和点P₁₀的直线与水平坐标系的Xh轴的交点为点A，水平面内过点P₁₀沿地理北向的直线与Yh轴的交点为点B，θ₁为Xh轴与线段P₈O₅的夹角，θ₂为线段P₈O₅与线段P₁₀O₅的夹角，θ₃为线段P₈P₁₀与线段P₁₀O₅的夹角，θ₄为线段P₈A与水平坐标系的Xh轴的夹角，θ₁＝atan(y_8h/x_8h)；θ₂＝atan(v_10h/u_10h)-θ₁；L为第一激光靶与第二激光靶之间的距离，记为线段P₈P₁₀的长度，线段O₅P₈的长度表示为

根据正弦定理，

得

由四边形AP₈BO₅内角和为2π的约束得(π-φ_m)+0.5π+θ₄+φ＝2π，故得φ_m＝θ₄+φ-0.5π。

在步骤4中，先获取第一激光靶、第二激光靶与巷道起始位置之间的距离信息；根据第一激光靶或第二激光靶到巷道起始位置的距离，获取第一激光靶或第二激光靶到巷道起始位置的位移；根据第一光斑或第二光斑在摄像头坐标中的位置对应获取第一光斑或第二光斑相对摄像头的位移，然后计算得到掘进机相对巷道起始位置的位移。

具体在本实施例中，通过设置在巷道起始位置的激光测距仪，获取第一激光靶、第二激光靶以及巷道起始位置之间的距离信息；

第二激光靶到巷道起始位置的位移通过如下公式计算：

其中，L₂为第二激光靶到巷道起始位置的距离，φ为激光指向仪的方位信息；

第二光斑相对第一摄像头的位移在掘进机的导航坐标系n中坐标通过如下公式表示：

其中，

为第一摄像头相对掘进机的安装矩阵，

为捷联惯导姿态矩阵；

掘进机相对巷道起始位置的位移通过如下公式表示：

其中，

为掘进机相对巷道起始位置的位移。

通过本实施例中的方法得到的掘进机的方位角φ_m和掘进机相对巷道起始位置的位移

提供给捷联惯导系统用于修正捷联惯导的误差，从而保证捷联惯导能够给出精度更高的姿态信息和位移信息，保证摄像探头或激光指向仪的光线暂时受遮挡时掘进机依靠捷联惯导给出的姿态和位置信息仍能够正常工作。

本发明通过两个摄像头检测两个光斑指示的光线方向，对捷联惯导进行误差修正提高整个系统的精度，为掘进机提供精确的方位信息；本发明不要求激光指向仪持续照射到第二激光靶，偶尔受到遮挡整个系统仍能正常工作；本发明给出的掘进机的方位角和掘进机相对巷道起始位置的位移在整个工作过程中能够始终保持精度，不受工作时间和掘进距离影响。

在本实施例中计算掘进机相对巷道起始位置的位移时，是通过第二激光靶和第二光斑来实现的，在本发明的其他实施例中，也可以通过第一激光靶和第一光斑来实现计算掘进机相对巷道起始位置的位移，对应方法同上述实施例，在此不再赘述。

见图4、5，在本发明的实施例中，还提供了基于双目视觉和捷联惯导的掘进机位姿纠偏系统，包括：

第一激光靶1，设置于巷道内，第一激光靶1可以安装在掘进机后方巷道顶面或侧面上，在本实施例中，第一激光靶1安装在掘进机后方巷道顶面上；

第二激光靶2，设置于巷道内且位于第一激光靶的后侧，第二激光靶2可以安装在掘进机后方巷道顶面或侧面上，在本实施例中，第二激光靶2安装在掘进机后方巷道顶面上；

激光指向仪3,设置于巷道的起始位置且能够在第一激光靶1、第二激光靶2上分别形成第一光斑4、第二光斑5，激光指向仪3射的光线平行于巷道的设定方向设置；

激光测距仪6，设置于巷道的起始位置，用于测量第一激光靶4、第二激光靶5与巷道起始位置之间的距离信息，激光测距仪6的测量方向平行于激光指向仪发射的光线的方向；

摄像头，包括安装于掘进机上的第一摄像头71和第二摄像头72，用于拍摄拍摄第一光斑4、第二光斑5；

通讯单元8，用于激光测距仪3、摄像头、数据处理单元9、捷联惯导系统10之间的通讯；

数据处理单元9，数据处理单元9能够根据掘进机在行进过程中摄像头拍摄到的第一光斑、第二光斑，获得第一光斑、第二光斑在摄像头坐标中的位置，进而获取第一光斑或第二光斑相对摄像头的位移；数据处理单元9根据激光测距仪测量的对应的第一激光靶或第二激光靶到巷道起始位置的距离，获取第一激光靶或第二激光靶到巷道起始位置的位移；然后根据第一激光靶或第二激光靶到巷道起始位置的位移及对应的第一光斑或第二光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移；数据处理单元9能够将第一光斑和第二光斑在摄像头坐标中的位置投影到水平坐标系中，结合第一激光靶和第二激光靶之间的距离，计算得到掘进机的方位角；

捷联惯导系统10，用于根据计算得到的掘进机相对巷道起始位置的位移和掘进机的方位角进行误差校正。

在摄像头或激光指向仪光线偶尔受遮挡时，数据处理单元将捷联惯导系统给出的位姿信息提供给用，确保整个系统连续工作。

在本实施例的基于双目视觉和捷联惯导的掘进机位姿纠偏系统中，使用激光作为传播介质，进行无接触的测量定位，克服传统的测量定位方式的劳动强度大、作业时间长和受工作环境影响大等局限性，为空间测量定位技术开辟更宽更特殊的应用环境和领域；捷联惯导系统实时给出截割头位置和姿态，可作为掘进机自动掘进的基准，给出的截割头姿态和位置在整个工作过程中能够始终保持精度，不受工作时间和掘进距离的影响；将激光靶悬挂在掘进机后面的巷道上，容易保证激光指向仪始终能够照射到激光靶，系统的适应性更好，悬挂激光靶时，只要保证激光指向仪和激光测距仪能够照射到靶面即可，便于操作，同时，也不要求激光指向仪持续照射到激光靶，偶尔收到遮挡整个系统仍能正常工作；

系统采用摄像头视觉测量的方式测量光斑在摄像头坐标中的位置，然后将光斑在摄像头坐标中的位置投影到水平坐标系中，计算得到掘进机的方位角；同时结合捷联惯性技术，通过激光靶到巷道起始位置的位移和对应的光斑相对摄像头的位移，得到掘进机相对巷道起始位置的位移，从而实时给出掘进机相对巷道起始位置的位移和掘进机的方位角，作为掘进机自动掘进的基准，在掘进机工作的过程中，校正捷联惯导的误差，以给出准确的姿态和位置测量结果，确保挖掘精度，也大大提高了巷道掘进的工作效率及工作质量，具有显著的经济效益和较高的工程应用价值，对实现高精度的自动掘进控制具有重要意义。

在本发明的实施例中，还提供了一种终端，包括：

第一激光靶，设置于巷道内；

第二激光靶，设置于巷道内且位于第一激光靶的后侧；

激光指向仪,设置于巷道的起始位置且能够在第一激光靶、第二激光靶上分别形成第一光斑、第二光斑；

激光测距仪，用于测量第一激光靶、第二激光靶到巷道起始位置的距离；

摄像头，包括第一摄像头和第二摄像头，用于获取第一激光靶、第二激光靶上形成的第一光斑、第二光斑；

捷联惯导系统，用于对掘进机进行导航；

存储器，存储有数据处理程序；

处理器，用于执行数据处理程序，以实现如上述的基于双目视觉和捷联惯导的掘进机位姿纠偏方法。

该终端可包括，但不仅限于处理器、存储器。本领域技术人员可以理解，本实施例仅仅是终端的举例，并不构成对终端的限定，可以包括比本实施例更多或更少的部件，或者组合某些部件，或者不同的部件，例如还可以包括输入输出设备、网络接入设备等。

存储器可以是，但不限于，随机存取存储器(Random Access Memory，简称：RAM)，只读存储器(Read Only Memory，简称：ROM)，可编程只读存储器(Programmable Read-OnlyMemory，简称：PROM)，可擦除只读存储器(Erasable Programmable Read-OnlyMemory，简称：EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory，简称：EEPROM)等。其中，存储器用于存储程序，处理器在接收到执行指令后，执行程序。

处理器可以是一种集成电路芯片，具有信号的处理能力。上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，简称：CPU)、网络处理器(NetworkProcessor，简称：NP)等。该处理器还可以是其他通用处理器、数字信号处理器(DigitalSignal

Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的数据处理程序，程序设计语言包括面向对象的程序设计语言—诸如Java、C++等，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。数据处理程序可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中，远程计算设备可以通过任意种类的网络，包括局域网(LAN)或广域网(WAN)，连接到用户计算设备，或者，可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (8)

1.一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于，包括以下步骤：

步骤1：通过在巷道内设置的激光指向仪，在第一激光靶和第二激光靶上形成第一光斑和第二光斑；

步骤2：通过在掘进机上设置的摄像头，拍摄第一光斑和第二光斑，获得第一光斑、第二光斑在摄像头坐标中的位置；

步骤3：将第一光斑和第二光斑在摄像头坐标中的位置投影到水平坐标系中，计算得到掘进机的方位角；

步骤4：获取第一激光靶或第二激光靶到巷道起始位置的位移和对应的第一光斑或第二光斑相对摄像头的位移，得到掘进机相对巷道起始位置的位移；

步骤5：掘进机的捷联惯导系统根据计算得到的掘进机相对巷道起始位置的位移和掘进机的方位角进行误差校正；

在步骤2中，在掘进机上设置第一摄像头和第二摄像头，第一摄像头测量第二光斑在第一摄像头归一化成像平面上的像点坐标(u₅,v₅)，第二摄像头测量第二光斑在第二摄像头归一化成像平面上的像点坐标(u₆,v₆)，第二光斑在第一摄像头的坐标中的位置(X,Y,Z)与第二光斑在第一摄像头归一化成像平面上的像点坐标(u₅,v₅)以及第二光斑在第二摄像头归一化成像平面上的像点坐标(u₆,v₆)之间的关系表示为：

其中，f_x、f_y为摄像头参数；d为第一摄像头和第二摄像头之间的距离；

第一光斑在第一摄像头的坐标中的位置(X₁,Y₁,Z₁)与第一光斑在第一摄像头归一化成像平面上的像点坐标(u₅₁,v₅₁)以及第一光斑在第二摄像头归一化成像平面上的像点坐标(u₆₁,v₆₁)之间的关系表示为：

其中，f_x、f_y为摄像头参数；d为第一摄像头和第二摄像头之间的距离；

在步骤3中，结合第一光斑在摄像头坐标中的位置，将第一光斑摄像头坐标中的位置投影到水平坐标系h，表示为：

其中，水平坐标系h原点与第一摄像头坐标的原点O₅重合，水平坐标系h与捷联惯导坐标系b之间仅存在俯仰角θ和横滚角γ，水平坐标系h与捷联惯导坐标系b的方位角一致，

为第一摄像头相对掘进机的安装矩阵，

为捷联惯导坐标系b到水平坐标系h的转换矩阵，表示为：

结合第二光斑在摄像头坐标中的位置，将第二光斑摄像头坐标中的位置投影到水平坐标系h，表示为

掘进机的方位角通过如下公式表示：

φ_m＝θ₄+φ-0.5π

其中，掘进机的方位角φ_m为掘进机纵轴线在水平面的投影Y_h与地理北向的夹角，φ为激光指向仪方位角；θ₄＝π-θ₁-θ₂-θ₃；将第一光斑、第二光斑摄像头坐标中的位置投影到水平坐标系h后，第一光斑在水平坐标系h中的投影为点P₁₀，第二光斑在水平坐标系h 中的投影为点P₈，过点P₈和点P₁₀的直线与水平坐标系的Xh轴的交点为点A，水平面内过点P₁₀沿地理北向的直线与Yh轴的交点为点B，θ₁为Xh轴与线段P₈O₅的夹角，θ₂为线段P₈O₅与线段P₁₀O₅的夹角，θ₃为线段P₈P₁₀与线段P₁₀O₅的夹角，θ₄为线段P₈A与水平坐标系的Xh轴的夹角，θ₁＝atan(y_8h/x_8h)；θ₂＝atan(v_10h/u_10h)-θ₁；L为第一激光靶与第二激光靶之间的距离，记为线段P₈P₁₀的长度，线段O₅P₈的长度表示为

根据正弦定理，

得

由四边形AP₈BO₅内角和为2π的约束得(π-φ_m)+0.5π+θ₄+φ＝2π，故得φ_m＝θ₄+φ-0.5π。

2.根据权利要求1所述的一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于：在步骤4中，先获取第一激光靶、第二激光靶与巷道起始位置之间的距离信息；根据第一激光靶或第二激光靶到巷道起始位置的距离，获取第一激光靶或第二激光靶到巷道起始位置的位移；根据第一光斑或第二光斑在摄像头坐标中的位置对应获取第一光斑或第二光斑相对摄像头的位移，然后计算得到掘进机相对巷道起始位置的位移。

3.根据权利要求2所述的一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于：在步骤4中，通过设置在巷道起始位置的激光测距仪，获取第一激光靶、第二激光靶以及巷道起始位置之间的距离信息。

4.根据权利要求3所述的一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于：在步骤4中，第二激光靶到巷道起始位置的位移通过如下公式计算：

其中，L₂为第二激光靶到巷道起始位置的距离，φ为激光指向仪的方位信息。

5.根据权利要求4所述的一种基于双目视觉和捷联惯导的掘进机位姿纠偏方法，其特征在于：在步骤4中，第二光斑相对第一摄像头的位移在掘进机的导航坐标系n中坐标通过如下公式表示：

其中，

为第一摄像头相对掘进机的安装矩阵，

为捷联惯导姿态矩阵；

掘进机相对巷道起始位置的位移通过如下公式表示：

其中，

为掘进机相对巷道起始位置的位移。

6.基于双目视觉和捷联惯导的掘进机位姿纠偏系统，其特征在于，包括：

第一激光靶，设置于巷道内；

第二激光靶，设置于巷道内且位于所述第一激光靶的后侧；

激光指向仪,设置于巷道的起始位置且能够在所述第一激光靶、第二激光靶上分别形成第一光斑、第二光斑；

激光测距仪，设置于巷道的起始位置，用于测量第一激光靶、第二激光靶与巷道起始位置之间的距离信息；

摄像头，包括安装于掘进机上的第一摄像头和第二摄像头，用于拍摄第一光斑、第二光斑；

通讯单元，用于所述激光测距仪、摄像头、数据处理单元、捷联惯导系统之间的通讯；

数据处理单元，所述数据处理单元能够根据掘进机在行进过程中摄像头拍摄到的第一光斑、第二光斑，获得第一光斑、第二光斑在摄像头坐标中的位置，进而获取第一光斑或第二光斑相对摄像头的位移；所述数据处理单元根据所述激光测距仪测量的对应的第一激光靶或第二激光靶到巷道起始位置的距离，获取第一激光靶或第二激光靶到巷道起始位置的位移；然后根据第一激光靶或第二激光靶到巷道起始位置的位移及对应的第一光斑或第二光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移；所述数据处理单元能够将第一光斑和第二光斑在摄像头坐标中的位置投影到水平坐标系中，结合第一激光靶和第二激光靶之间的距离，计算得到掘进机的方位角；

第一摄像头测量第二光斑在第一摄像头归一化成像平面上的像点坐标(u₅,v₅)，第二摄像头测量第二光斑在第二摄像头归一化成像平面上的像点坐标(u₆,v₆)，第二光斑在第一摄像头的坐标中的位置(X,Y,Z)与第二光斑在第一摄像头归一化成像平面上的像点坐标(u₅,v₅)以及第二光斑在第二摄像头归一化成像平面上的像点坐标(u₆,v₆)之间的关系表示为：

其中，f_x、f_y为摄像头参数；d为第一摄像头和第二摄像头之间的距离；

第一光斑在第一摄像头的坐标中的位置(X₁,Y₁,Z₁)与第一光斑在第一摄像头归一化成像平面上的像点坐标(u₅₁,v₅₁)以及第一光斑在第二摄像头归一化成像平面上的像点坐标(u₆₁,v₆₁)之间的关系表示为：

其中，f_x、f_y为摄像头参数；d为第一摄像头和第二摄像头之间的距离；

结合第一光斑在摄像头坐标中的位置，将第一光斑摄像头坐标中的位置投影到水平坐标系h，表示为：

其中，水平坐标系h原点与第一摄像头坐标的原点O₅重合，水平坐标系h与捷联惯导坐标系b之间仅存在俯仰角θ和横滚角γ，水平坐标系h与捷联惯导坐标系b的方位角一致，

为第一摄像头相对掘进机的安装矩阵，

为捷联惯导坐标系b到水平坐标系h的转换矩阵，表示为：

结合第二光斑在摄像头坐标中的位置，将第二光斑摄像头坐标中的位置投影到水平坐标系h，表示为

掘进机的方位角通过如下公式表示：

φ_m＝θ₄+φ-0.5π

其中，掘进机的方位角φ_m为掘进机纵轴线在水平面的投影Y_h与地理北向的夹角，φ为激光指向仪方位角；θ₄＝π-θ₁-θ₂-θ₃；将第一光斑、第二光斑摄像头坐标中的位置投影到水平坐标系h后，第一光斑在水平坐标系h中的投影为点P₁₀，第二光斑在水平坐标系h中的投影为点P₈，过点P₈和点P₁₀的直线与水平坐标系的Xh轴的交点为点A，水平面内过点P₁₀沿地理北向的直线与Yh轴的交点为点B，θ₁为Xh轴与线段P₈O₅的夹角，θ₂为线段P₈O₅与线段P₁₀O₅的夹角，θ₃为线段P₈P₁₀与线段P₁₀O₅的夹角，θ₄为线段P₈A与水平坐标系的Xh轴的夹角，θ₁＝atan(y_8h/x_8h)；θ₂＝atan(v_10h/u_10h)-θ₁；L为第一激光靶与第二激光靶之间的距离，记为线段P₈P₁₀的长度，线段O₅P₈的长度表示为

根据正弦定理，

得

由四边形AP₈BO₅内角和为2π的约束得(π-φ_m)+0.5π+θ₄+φ＝2π，故得φ_m＝θ₄+φ-0.5π；

捷联惯导系统，用于根据计算得到的掘进机相对巷道起始位置的位移和掘进机的方位角进行误差校正。

7.根据权利要求6所述的基于双目视觉和捷联惯导的掘进机位姿纠偏系统，其特征在于：所述激光指向仪发射的光线平行于巷道的设定方向设置，所述激光测距仪的测量方向平行于激光指向仪发射的光线的方向；所述第一激光靶、第二激光靶分别对应安装在掘进机后方巷道顶面或侧面上。

8.一种终端，其特征在于，包括：

第一激光靶，设置于巷道内；

第二激光靶，设置于巷道内且位于所述第一激光靶的后侧；

激光指向仪,设置于巷道的起始位置且能够在所述第一激光靶、第二激光靶上分别形成第一光斑、第二光斑；

激光测距仪，用于测量第一激光靶、第二激光靶到巷道起始位置的距离；

摄像头，包括第一摄像头和第二摄像头，用于获取第一激光靶、第二激光靶上形成的第一光斑、第二光斑；

捷联惯导系统，用于对掘进机进行导航；

存储器，存储有数据处理程序；

处理器，用于执行所述数据处理程序，以实现如权利要求1所述的基于双目视觉和捷联惯导的掘进机位姿纠偏方法。

Images