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

说明书

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

技术领域

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

背景技术

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

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

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

2、挖掘精度不高

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

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

发明内容

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

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

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

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

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

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

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

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

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

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