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

说明书

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

技术领域

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

背景技术

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

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

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为第一摄像头和第二摄像头之间的距离；