[发明专利]矿用悬臂式掘进机可视化辅助导引方法和系统

权利要求书

1.矿用悬臂式掘进机可视化辅助导引方法，该方法中的掘进机悬臂上设置有标靶，标靶的边上设置有多个光源点，其特征在于，该方法包括以下步骤：

步骤1：利用捷联惯导传感器获取悬臂式掘进机机体的横滚角、俯仰角和偏航角以及掘进机机体在巷道中的位置信息；利用超声波传感器测量掘进机机身到两侧煤壁的距离和掘进机机身到前煤壁的距离；

步骤2：通过视觉测量的方法确定掘进机截割头相对于机体的位姿；

步骤2.1：提取步骤1采集的标靶图像中的光斑，对光斑进行排序、直线拟合，将每两条直线的交点作为特征点；

步骤2.2：利用四点透视成像方法确定所有特征点在摄像机坐标系中的坐标；

步骤2.3：利用对偶四元数的误差模型，根据特征点在标靶坐标系中的坐标以及步骤2.2得到的特征点在摄像机坐标系中的坐标，计算截割头的水平摆角θ1和截割头垂直摆角θ2；

步骤2.3.1：利用对偶四元数误差模型构建矩阵A，式(4)所示，

式中，Q(P_i^W)表示第i个特征点在标靶坐标系下坐标的正交矩阵；W(P_i^C)表示第i个特征点在摄像机坐标系下坐标的正交矩阵；表示第i个特征点在标靶坐标系中的坐标，表示第i个特征点在摄像机坐标系下的坐标，N表示特征点的个数；

当矩阵A的特征值最大时，模型的误差最小，求取此时矩阵A的特征向量r，其中，表示特征向量r里的四个元素；

步骤2.3.2：根据步骤2.3.1得到的特征向量r，通过式(5)确定摄像机坐标系与标靶坐标系之间的旋转矩阵R，

步骤2.3.3：根据步骤2.3.2获得的旋转矩阵R，通过式(6)中相等矩阵对应元素相等原理计算得到θ1和θ2，

式中，R为步骤2.3.2计算得到的旋转矩阵，R^*为θ1＝0°、θ2＝-90°时，根据步骤2.3.2计算的特征点在摄像机坐标系下的坐标所对应的旋转矩阵R；

步骤2.4：根据步骤2.3确定的θ1和θ2，通过式(1)计算截割头相对于掘进机机体的位姿

其中，θ1为截割头水平摆角，θ2为截割头垂直摆角，d为油缸的伸缩距离，b1为掘进机抬升关节与伸缩关节之间的高度差；a1为掘进机回转台中心与抬升关节之间水平距离，a2为掘进机抬升关节与伸缩关节之间的距离，a3为掘进机伸缩关节与截割头之间的水平距离；

步骤3：根据步骤1获得的掘进机运行状态信息，通过式(2)得到掘进机相对于巷道的位姿

式中，γ为掘进机机身的横滚角，β为掘进机机身的俯仰角，α为掘进机机身的偏航角，D₁为机身到侧方煤壁的距离，L为掘进机到前煤壁的距离；D₃为掘进机机身宽度，b2为抬升关节与地面之间的距离；

步骤4：根据步骤2得到的截割头相对于机体位姿数据和步骤3得到的掘进机机身相对于巷道断面的位姿数据，通过式(3)可得到截割头相对于巷道断面的位姿

步骤5：根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、走向以及截割工艺确定掘进机截割头的导引轨迹，并将导引轨迹在上位机上进行图形化界面显示；

步骤6：将步骤4得到截割头相对于巷道断面的位姿数据与上位机进行数据交互，将截割头在上位机上图形化界面显示，并根据步骤5确定的导引轨迹进行截割头移动过程中的轨迹导引。

2.矿用悬臂式掘进机可视化辅助导引系统，该系统中所述的掘进机悬臂上设置有标靶，标靶的边上设置有多个光源点，其特征在于，该系统包括：

通信模块，利用捷联惯导传感器获取悬臂式掘进机机体的横滚角、俯仰角和偏航角以及掘进机机体在巷道中的位置信息；利用超声波测距传感器和激光测距传感器测量机身到两侧煤壁的距离和掘进机到前煤壁的距离；

视觉测量模块，用于测量截割头相对于掘进机机体的位姿，具体包括：

提取通信模块采集的标靶图像中的光斑，对光斑进行排序、直线拟合，将每两条直线的交点作为特征点；

利用四点透视成像原理确定所有特征点在摄像机坐标系中的坐标；

利用对偶四元数的误差模型，根据特征点在标靶坐标系中的坐标以及特征点在摄像机坐标系中的坐标，计算截割头的水平摆角θ1和截割头垂直摆角θ2，具体过程为：

利用对偶四元数误差模型构建矩阵A，如式(4)所示，

式中，Q(P_i^W)表示第i个特征点在标靶坐标系下坐标的正交矩阵；W(P_i^C)表示第i个特征点在摄像机坐标系下坐标的正交矩阵；表示第i个特征点在标靶坐标系中的坐标，表示第i个特征点在摄像机坐标系下的坐标，N表示特征点的个数；

当矩阵A的特征值最大时，模型的误差最小，求取此时矩阵A的特征向量r，其中，表示特征向量r里的四个元素；

根据得到的特征向量r，通过式(5)确定摄像机坐标系与标靶坐标系之间的旋转矩阵R，

根据获得的旋转矩阵R，通过式(6)中相等矩阵对应元素相等原理计算得到θ1和θ2，

式中，R为旋转矩阵，R^*为θ1＝0°、θ2＝-90°时特征点在摄像机坐标系下的坐标所对应的旋转矩阵R；

根据θ1和θ2，通过式(1)计算截割头相对于掘进机机体的位姿

其中，θ1为截割头水平摆角，θ2为截割头垂直摆角，d为油缸的伸缩距离，b1为掘进机抬升关节与伸缩关节之间的高度差；a1为掘进机回转台中心与抬升关节之间水平距离，a2为掘进机抬升关节与伸缩关节之间的距离，a3为掘进机伸缩关节与截割头之间的水平距离；

掘进机位姿检测模块，用于掘进机机身相对于巷道的位姿数据，根据通信模块获得的掘进机运行状态信息，利用公式(2)得到掘进机机身相对于巷道的位姿

式中，γ为掘进机机体的横滚角，β为掘进机机体的俯仰角，α为掘进机机体的偏航角，D₁为机身到侧方煤壁的距离，L为掘进机到前煤壁的距离；D₃为掘进机机身宽度，b2为抬升关节与地面之间的距离；

数据处理模块，用于确定截割头相对于巷道断面的位姿数据，根据视觉测量模块得到的截割头相对于机体位姿数据和掘进机位姿检测模块得到的掘进机机身相对于巷道的位姿数据，通过式(3)得到截割头相对于巷道断面的位姿数据

轨迹显示模块，用于显示掘进机截割头的导引轨迹，根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、走向以及截割工艺确定掘进机截割头的导引轨迹，并将导引轨迹在上位机上进行图形化界面显示；

轨迹导引模块，用于截割头移动过程中的轨迹导引，将数据处理模块得到的截割头相对于巷道断面的位姿数据与上位机进行数据交互，将截割头在上位机上图形化界面显示，并根据轨迹显示模块确定的导引轨迹进行截割头移动过程中的轨迹导引。