[发明专利]基于视觉和捷联惯导的掘进机位姿纠偏方法、系统及终端

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

激光靶，设置于巷道内；

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

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

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

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

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

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

所述基于视觉和捷联惯导的掘进机位姿纠偏系统采用权利要求1所述的方法进行掘进机位姿纠偏。

3.根据权利要求2所述的基于视觉和捷联惯导的掘进机位姿纠偏系统，其特征在于：所述测距仪采用激光测距仪。

4.根据权利要求3所述的基于视觉和捷联惯导的掘进机位姿纠偏系统，其特征在于：所述激光指向仪发射的光线平行于巷道的设定方向设置，测距仪的测量方向平行于激光指向仪发射的光线的方向。

5.根据权利要求2所述的基于视觉和捷联惯导的掘进机位姿纠偏系统，其特征在于：所述激光靶安装在掘进机后方巷道顶面或侧面上。

6.根据权利要求2所述的基于视觉和捷联惯导的掘进机位姿纠偏系统，其特征在于：所述摄像头包括第一摄像头和第二摄像头，光斑在第一摄像头的坐标中的位置通过光斑在第一摄像头和第二摄像头的归一化成像平面上的像点坐标以及第一摄像头和第二摄像头之间的距离得到。

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

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

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

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

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