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

权利要求书

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π。