基于视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端与流程

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

背景技术：

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

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

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

2、挖掘精度不高

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

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

技术实现要素：

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

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

步骤1：在巷道内设置激光靶和激光指向仪，激光指向仪在激光靶上形成光斑；

步骤2：获取激光靶到巷道起始位置的位移；

步骤3：在掘进机上设置摄像头，通过摄像头拍摄光斑，获取光斑相对摄像头的位移；

步骤4：根据激光靶到巷道起始位置的位移和光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移；

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

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

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

进一步的，在步骤3中，在掘进机上设置第一摄像头和第二摄像头，第一摄像头测量光斑在第一摄像头归一化成像平面上的像点坐标(u5,v5)，第二摄像头测量光斑在第二摄像头归一化成像平面上的像点坐标(u6,v6)，光斑在第一摄像头的坐标中的位置(x,y,z)与光斑在第一摄像头归一化成像平面上的像点坐标(u5,v5)以及光斑在第二摄像头归一化成像平面上的像点坐标(u6,v6)之间的关系表示为：

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

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

其中，为第一摄像头5相对掘进机的安装矩阵，为捷联惯导姿态矩阵。

进一步，在步骤4中，掘进机相对巷道起始位置的位移通过如下公式表示：

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

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

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

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

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

摄像头，安装于掘进机上，所述摄像头能够拍摄光斑；

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

数据处理单元，所述数据处理单元能够根据掘进机在行进过程中摄像头拍摄到的光斑，获取光斑相对摄像头的位移；所述数据处理单元根据所述测距仪测量的激光靶到巷道起始位置的距离，获取激光靶到巷道起始位置的位移；并根据激光靶到巷道起始位置的位移和光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移；

捷联惯导系统，根据所述数据处理单元计算得到的掘进机相对巷道起始位置的位移进行误差校正。

进一步的，所述测距仪采用激光测距仪。

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

进一步的，所述激光靶安装在掘进机后方巷道顶面或侧面上。

进一步的，所述摄像头包括第一摄像头和第二摄像头，光斑在第一摄像头的坐标中的位置通过光斑在第一摄像头和第二摄像头的归一化成像平面上的像点坐标以及第一摄像头和第二摄像头之间的距离得到。

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

激光指向仪，用于获取激光靶到巷道起始位置的距离；

摄像头，用于获取激光靶上形成的光斑；

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

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

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

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

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

附图说明

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

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

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

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

具体实施方式

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

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

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

掘进机的捷联惯导系统，根据内部陀螺仪和加速度计测得的加速度和角速度信息不断计算掘进机的姿态矩阵和相对巷道起始位置的位移信息，捷联惯导给出的姿态矩阵和位移信息的误差随时间不断累积，需要进行修正，为此，见图1、4，本实施例提供了一种基于视觉和捷联惯导的掘进机位姿纠偏方法，其至少包括以下步骤：

步骤1：在巷道内设置激光靶和激光指向仪，激光指向仪在激光靶上形成光斑；

步骤2：获取激光靶到巷道起始位置的位移；

步骤3：在掘进机上设置摄像头，通过摄像头拍摄光斑，获取光斑相对摄像头的位移；

步骤4：根据激光靶到巷道起始位置的位移和光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移；

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

具体在本实施例中，在步骤2中，激光靶到巷道起始位置的位移通过如下公式计算：

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

具体在本实施例中，在步骤3中，在掘进机上设置第一摄像头和第二摄像头，第一摄像头测量光斑在第一摄像头归一化成像平面上的像点坐标(u5,v5)，第二摄像头测量光斑在第二摄像头归一化成像平面上的像点坐标(u6,v6)，光斑在第一摄像头归一化成像平面上的像点坐标(u5,v5)以及光斑在第二摄像头归一化成像平面上的像点坐标(u6,v6)，与光斑在第一摄像头的坐标中的位置(x,y,z)与之间的关系表示为：

其中，fx、fy为摄像头参数；d为第一摄像头和第二摄像头之间的距离，第一摄像头和第二摄像头的输出已经经过误差校正；

第一摄像头和第二摄像头的输出(u5,v5)、(u6,v6)与光斑相对第一摄像头5位移(x,y,z)的关系为

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

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

其中，为第一摄像头5相对掘进机的安装矩阵，为捷联惯导姿态矩阵；

具体在本实施例中，在步骤4中，掘进机相对巷道起始位置的位移通过如下公式表示：

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

通过本实施例中的方法得到的掘进机相对巷道起始位置的位移提供给捷联惯导系统用于修正捷联惯导的误差，从而保证捷联惯导能够给出精度更高的姿态信息和位移信息，保证摄像探头或激光指向仪的光线暂时受遮挡时掘进机依靠捷联惯导给出的姿态和位置信息仍能够正常工作。

见图2、3，在本发明的实施例中，还提供了一种能够实现上述的基于视觉和捷联惯导的掘进机位姿纠偏的方法的基于视觉和捷联惯导的掘进机位姿纠偏系统，包括：

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

激光指向仪2,设置于巷道的起始位置且能够在激光靶上形成光斑，激光指向仪2发射的光线平行于巷道的设定方向设置；

测距仪3，设置于巷道的起始位置，用于测量激光靶1到巷道起始位置的距离，在本实施例中，测距仪采用激光测距仪，测距仪3的测量方向平行于激光指向仪2发射的光线的方向；

摄像头，安装于掘进机4上，摄像头能够拍摄光斑，摄像头包括第一摄像头51和第二摄像头52，光斑在第一摄像头51的坐标中的位置通过光斑在第一摄像头和第二摄像头的归一化成像平面上的像点坐标以及第一摄像头51和第二摄像头52之间的距离得到；

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

数据处理单元7，数据处理单元7能够根据掘进机在行进过程中摄像头拍摄到的光斑，获取光斑相对摄像头的位移；数据处理单元7根据测距仪3测量的激光靶1到巷道起始位置的距离，获取激光靶1到巷道起始位置的位移；并根据激光靶1到巷道起始位置的位移和光斑相对摄像头的位移计算得到掘进机相对巷道起始位置的位移。

捷联惯导系统8，根据数据处理单元7计算得到的掘进机相对巷道起始位置的位移进行误差校正。

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

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

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

摄像头，用于获取激光靶上形成的光斑；

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

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

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

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

存储器可以是，但不限于，随机存取存储器(randomaccessmemory，简称：ram)，只读存储器(readonlymemory，简称：rom)，可编程只读存储器(programmableread-onlymemory，简称：prom)，可擦除只读存储器(erasableprogrammableread-onlymemory，简称：eprom)，电可擦除只读存储器(electricerasableprogrammableread-onlymemory，简称：eeprom)等。其中，存储器用于存储程序，处理器在接收到执行指令后，执行程序。

处理器可以是一种集成电路芯片，具有信号的处理能力。上述的处理器可以是通用处理器，包括中央处理器(centralprocessingunit，简称：cpu)、网络处理器(networkprocessor，简称：np)等。该处理器还可以是其他通用处理器、数字信号处理器(digitalsignalprocessor，dsp)、专用集成电路(applicationspecificintegratedcircuit，asic)、现场可编程门阵列(field-programmablegatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

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

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

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