CN111946340A - 基于视觉导航的煤矿悬臂式掘进机截割控制方法及系统 - Google Patents

Abstract

本发明公开了基于视觉导航的煤矿悬臂式掘进机截割控制方法及系统，首先获取掘进机运行状态信息、掘进机在巷道空间中的姿态角；根据掘进机相对于巷道的位姿以及截割头相对于掘进机机身的位姿，求解截割头相对于巷道断面的位姿；规划截割头的导引轨迹，并求解截割头所需摆动的关节角度回转角和升降角；计算截割头实际测量所得回转角和升降角并与所需摆动的关节角度回转角和升降角比较计算截割头误差，采用模糊PID控制方法，利用DSP控制器输出控制命令通过电液比例阀驱动升降油缸及回转油缸控制截割臂摆动进行位姿偏差修正。本发明将视觉导航控制技术用于悬臂式掘进机的自动截割控制中，利用视觉测量方法实现对截割头位置的检测和精确定位。

Description

基于视觉导航的煤矿悬臂式掘进机截割控制方法及系统

技术领域

本发明属于矿用掘进机技术领域，具体涉及一种基于视觉导航的煤矿悬臂式掘进机截割控制方法及系统。

背景技术

巷道掘进工作中环境复杂、司机视野差，通过掘进机司机手动操作进行煤巷掘进作业无法准确控制掘进机的运行位姿，导致巷道掘进过程中出现巷道成形质量差、掘进效率低。因此，实现掘进机的信息化、自动化和智能化对减轻工人劳动强度、提高开采效率具有重要意义。目前针对悬臂式掘进机所建立的控制系统中，大多利用位移传感器的间接式位移传感器及角度编码器来反馈掘进机截割头位姿信息，但利用位移传感器直接式测量方式对安装精度有较高的要求，且在振动环境下，现有方法的测量稳定性和传感器寿命较低。

发明内容

为解决现有技术中存在的不足，本发明提供了一种基于视觉导航的煤矿悬臂式掘进机截割控制方法及系统，解决现有的悬臂式掘进机工作过程中受人为和环境因素的影响难以保证巷道掘进效率和质量的问题。

为了解决上述技术问题，本发明采用如下技术方案予以实现：

一种基于视觉导航的煤矿悬臂式掘进机截割控制方法，包括以下步骤：

步骤1：获取掘进机运行状态信息、掘进机的红外标靶图像及掘进机在巷道空间中的姿态角：

步骤2：根据步骤1获得的掘进机运行状态信息、掘进机与前侧煤壁的距离、姿态角和掘进机机身参数，通过式(1)得到掘进机相对于巷道的位姿

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

根据掘进机相对于巷道的位姿

以及截割头相对于掘进机机身的位姿

得到掘进机的截割头相对于巷道断面的位姿

如下式(2)所示：

式中，a₁₁、a₁₂、a₁₃、a₂₁、a₂₂、a₂₃、a₃₁、a₃₂、a₃₃为截割头相对巷道的旋转矩阵参数，a₁₄、a₂₄、a₃₄为截割头相对巷道的平移矩阵参数；

步骤3：根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、巷道走向以及截割工艺规划掘进机截割头的导引轨迹，按照S型轨迹进行截割，并将导引轨迹在上位机上进行图形化界面显示；根据所规划导引轨迹，依据截割轨迹上关键点位置坐标，结合截割头与机身的位姿关系进行求解，得到截割头所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂；

步骤4：将步骤2所得到的截割头相对于巷道断面的位姿，计算截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂，反馈到DSP控制器中，与步骤3中规划导引轨迹比较并计算截割头误差信息，求解得到截割头所需调整的关节角度，并通过控制器反复调整，使得截割头位姿偏差Δ趋近于0；

步骤4.1：依据实时计算所得掘进机的截割头相对于巷道断面的位姿

矩阵，采用反变换法对

矩阵逆运动学求解，得到掘进机截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂；

步骤4.2：据步骤3规划的截割头导引轨迹及步骤4.1得到截割头实际摆动的关节角度，计算得到截割头位姿的偏差Δ：

Δ_x＝θ₁-θ1′，Δ_y＝θ₂-θ₂′ (10)

式中，Δ_x为水平方向偏差，Δ_y为垂直方向偏差，θ₁为截割头实际测量所得回转角、θ₂为截割头实际测量所得升降角；θ′₁为规划轨迹截割头所需摆动的关节角度回转角、θ′₂为规划轨迹截割头所需摆动的关节角度升降角；

步骤4.3：采用模糊PID控制方法，利用DSP控制器输出控制命令通过电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制：采用模糊PID控制方法建立悬臂式掘进机视觉导航截割控制模型，利用步骤4.2得到的偏差Δ_x、Δ_y作为DSP控制器的输入，经比例放大器及电液比例阀控制液压缸伸缩，使掘进机截割头到达期望位置；通过DSP控制器的反复调节，使得截割头位姿偏差Δ最终趋近于零；

步骤5：将经过位姿偏差修正后的实际回转角和升降角数据保存在数据库中，在巷道断面中用图形的方式显示截割头截割轨迹，实时监视系统工作时的运动状态。

本发明还包括如下技术特征：

具体的，所述步骤1中，使用安装在掘进机机身两侧超声波传感器获得掘进机与前侧煤壁的距离；利用捷联惯导传感器获得掘进机在巷道空间中的姿态角，姿态角包括掘进机机身的横滚角、掘进机机身的俯仰角和掘进机机身的偏航角。

具体的，所述步骤3中截割头所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂的计算步骤如下：

设截齿在截割头坐标系下的坐标向量⁴p＝(X_h，Y_h，Z_h，1)^T，截齿在截割头坐标系下的坐标向量⁴p由截割头截齿位置求得，规划的轨迹中取一个点，设其在掘进机机身下的坐标向量为°p，根据下式(3)求得°p为(X_o，Y_o，Z_o,1)^T：

根据式(3)得：

Z₀＝-X_hsinθ₂′-Z_hcosθ₂′+-b₁sinθ₂′-(a₂+a₃+d)cosθ₂′ (4)

Y₀＝X_hsinθ₁′cosθ₂′-Y_hcosθ₁′-Z_hsinθ₁′sinθ₂′+b₁sinθ₁′cosθ₂′-(a₂+a₃+d)sinθ₁′sinθ₂′+a₁sinθ₁′ (5)

根据式(4)求解得到截割头所需摆动的关节角度回转角θ′₁，带入式(5)求得截割头所需摆动的关节角度升降角θ′₂；其中，X_h、Y_h、Z_h分别为截齿在截割头坐标系下三维坐标，X_o、Y_o、Z_o为规划轨迹中一点在机身坐标系下的三维坐标值，其中a1为掘进机机身标系O₀X₀Y₀Z₀到掘进机回转台坐标系O₁X₁Y₁Z₁x方向距离，a2为回转台关节坐标系O₁X₁Y₁Z₁到截割臂抬升关节坐标系O₂X₂Y₂Z₂x方向距离，a3为截割臂抬升关节坐标系O₂X₂Y₂Z₂到截割头伸缩关节坐标系O₃X₃Y₃Z₃x方向距离，a4为截割头伸缩关节坐标系到截割头坐标系O₄X₄Y₄Z₄x方向距离，b1为掘进机机身标系O₀X₀Y₀Z₀到掘进机截割头坐标系O₄X₄Y₄Z₄ y方向距离，b2为掘进机机身高度，d为截割头伸缩量。

具体的，所述步骤4.1中，对于截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂的求解如下：

截割头相对于巷道断面的位姿

矩阵中：

a₁₄＝

cosγcosβ(b₃cosθ₁cosθ₂-(a₃+a₄+d)cosθ₁sinθ₂+a₂cosθ₁+a₁)+(cosγsinβsinα-sinγcosαb3sinθ1cosθ2-a3+a4+dsinθ1sinθ2+a2sinθ1+(cosγsinβcosα+sinγsinα)(-b3sinθ2-a3+a4+dcosθ2+b2)+Sx(6)

a₂₄＝

sinγcosβ(b₁cosθ₁cosθ₂-(a₃+a₄+d)cosθ₁sinθ₂+a₂cosθ₁+a₁)+(sinγsinβsinα+cosγcosαb1sinθ1cosθ2-a3+a4+dsinθ1sinθ2+a2sinθ1+sinγsinβcosα-cosγsinα-b1sinθ2-a3+a4+dcosθ2+b2+Sy (7)

a₃₄＝-sinβ(-cosθ₁sinθ₂a₄+cosθ₁cosθ₂b₁-cosθ₁sinθ₂(a₃+d)+cosθ₁a₂+a₁)+cosβsinα(-sinθ₁sinθ₂a₄+sinθ₁cosθ₂b₁-sinθ₁sinθ₂(a₃+d)+sinθ₁a₂)+cosαcosβ(-cosθ₂a₄-sinθ₂b₁-cosθ₂(a₃+d)+b₂)+S_z (8)

联立(6)(7)(8)式可求得截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂；其中a1为掘进机机身标系O₀X₀Y₀Z₀到掘进机回转台坐标系O₁X₁Y₁Z₁ x方向距离，a2为回转台关节坐标系O₁X₁Y₁Z₁到截割臂抬升关节坐标系O₂X₂Y₂Z₂x方向距离，a3为截割臂抬升关节坐标系O₂X₂Y₂Z₂到截割头伸缩关节坐标系O₃X₃Y₃Z₃x方向距离，a4为截割头伸缩关节坐标系到截割头坐标系O₄X₄Y₄Z₄x方向距离，b1为掘进机机身标系O₀X₀Y₀Z₀到掘进机截割头坐标系O₄X₄Y₄Z₄ y方向距离，b2为掘进机机身高度，d为截割头伸缩量，S_x、S_y、S_z分别为式(1)中，掘进机相对于巷道的位姿矩阵

中的平移矩阵；

其中S_x＝-Lcosβcosγ，S_y＝-(D₁+D₃/2)cosαcosγ，S_z＝Lsinβ+b2。

具体的，所述步骤4.3中，具体的，DSP控制器的工作过程：

采用闭环控制，利用步骤2所得截割头相对于巷道断面的位姿信息，实现视觉反馈，视觉反馈采用以下传递函数模型：

式中：G(s)是传递函数，Kp为模糊PID控制器的比例环节增益；Kv为比例放大器的增益；Kt为电液比例阀的流量增益，m3/s·A；K_μ为视觉测量模块的增益；Ah为液压缸有效工作面积；s为系统的阶数；ω_v为换向阀相频宽，rad/S；δ_v为比例换向阀的阻尼比；ω_h为液压缸固有频率，rad/S；ζ_h为液压阻尼比。

具体的，所述步骤4.3中，采用模糊PID控制方法具体步骤如下：

模糊PID控制是以误差e和误差变化率ec为输入对象的，将Kp参数作为输出量；通过模糊控制规则表针对不同时间内的误差e和误差变化率ec进行对应的PID参数自动调节，从而满足相应的需求，使得控制系统具有良好的动静性能；

Kp参数的计算如下所示：

K_p＝K_p1+{e,ec} (12)

其中K_p1为PID控制的基本参数，{e,ec}为各个对应的模糊控制所调节参数的输出量；将位置模糊PID控制作为最外环，根据视觉测量系统所反馈的位置信号实时调节控制参数，DSP将位置模糊PID控制的输出电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制。

一种基于视觉导航的煤矿悬臂式掘进机截割控制系统，包括：

数据采集模块，用于获取掘进机运行状态信息、掘进机的红外标靶图像及掘进机在巷道空间中的姿态角；所述数据采集模块包括安装在掘进机机身两侧超声波传感器，用于获得掘进机与前侧煤壁的距离；所述数据采集模块还包括捷联惯导传感器，用于获得掘进机在巷道空间中的姿态角，姿态角包括掘进机机身的横滚角、掘进机机身的俯仰角和掘进机机身的偏航角；

截割头的位姿解算模块，用于根据掘进机相对于巷道的位姿

以及截割头相对于掘进机机身的位姿

求解掘进机的截割头相对于巷道断面的位姿

截割头轨迹规划模块，用于根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、巷道走向以及截割工艺规划掘进机截割头的导引轨迹，并结合截割头与机身的位姿关系进行求解，得到截割头所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂；

截割头位姿修正模块，用于计算掘进机截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂，并与截割头所规划导引轨迹所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂进行比较计算截割头误差信息，求解得到截割头所需调整的关节角度，采用模糊PID控制方法，利用DSP控制器输出控制命令通过电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制，从而对截割头进行位姿偏差修正；

数据存储及监控模块，用于将经过位姿偏差修正后的实际回转角和升降角数据保存在数据库中，在巷道断面中用图形的方式显示截割头截割轨迹，实时监视系统工作时的运动状态。

与现有技术相比，本发明的有益效果是：

(1)本发明将视觉导航控制技术用于悬臂式掘进机的自动截割控制中，利用视觉测量方法实现对截割头位置的检测和精确定位，并建立掘进机运动学模型实现掘进机的自动截割控制。解决直接式测量方法的测量不稳定性和传感器寿命较低的问题，同时通过掘进机的自动截割保证截割质量，有效避免了巷道的超挖和欠挖的产生，大大提高了巷道掘进的工作效率和工作质量。

(2)本发明采用视觉导航控制的方法，根据视觉测量得到的信息作为视觉导航的信息，采用PID控制器调整掘进机关节驱动器实现掘进机自动截割，提高了控制精度。

附图说明

图1为本发明掘进机各个坐标系示意图；

图2为本发明的系统框图；

图3为本发明的视觉导航控制结构图；

图4为截割控制系统传递函数模型图；

图5为仿真实验中的实时轨迹的可视化界面显示图；

图6为规划轨迹示意图。

以下结合附图和实施例对本发明的具体内容作进一步详细解释说明。

具体实施方式

本发明中的视觉导航技术是基于位置的伺服控制，据建立的目标与摄像机之间的数学模型将图像信息转换成世界坐标系中的位置信息，并传给机器人关节控制器控制机器人运动。

本发明中的摄像机坐标系是指以摄像机的光心为原点X轴沿水平方向，Y轴沿竖直方向，Z轴垂直Y、X轴指向标靶方向。标靶坐标系是指以标靶中心为原点，X轴平行于标靶上下边指向右，Y平行于标靶左右边指向下，Z轴垂直于标靶平面。

本发明方法中的视觉导航控制技术，采用位置反馈型机器人视觉控制技术建立悬臂式掘进机视觉导航截割控制系统，该系统包括：悬臂式掘进机、比例放大器、电液比例阀、液压缸、防爆计算机、红外LED标靶、防爆工业相机、DSP控制器和各种传感器，截割臂上有红外标靶，捷联惯导传感器、超声波传感器安装于掘进机机身，其中防爆工业相机通过以太网口将采集到图片传输到计算机，捷联惯导、超声波传感器、电液比例阀与DSP控制器连接，电液比例阀安装于液压缸回路，DSP控制器通过RS485与防爆计算机之间进行数据交互。图2为本发明系统框图，系统截割臂上有红外标靶，捷联惯导传感器、超声波传感器安装于掘进机机身，其中防爆工业相机通过以太网口将采集到图片传输到计算机，捷联惯导、超声波传感器、电液比例阀与DSP控制器连接，电液比例阀安装于液压缸回路，DSP控制器通过RS485与防爆计算机之间进行数据交互，DSP中的运动控制模块控制掘进机液压回路上的电液比例阀动作，从而控制掘进机截割臂的液压缸动作。

图1为本发明掘进机各个坐标系示意图，坐标系O₀X₀Y₀Z₀、O₁X₁Y₁Z₁、O₂X₂Y₂Z₂、O₃X₃Y₃Z₃、O₄X₄Y₄Z₄分别为悬臂式掘进机基坐标系、回转关节坐标系、抬升关节坐标系、伸缩关节坐标系及截割头坐标系，坐标系O_cX_cY_cZ_c、O_wX_wY_wZ_w分别为摄像机坐标系及标靶坐标系，坐标系O_hX_hY_hZ_h为巷道坐标系，D3为掘进机机身宽度，b2为截割臂的抬升关节与地面之间距离。

本发明提供一种基于视觉导航的煤矿悬臂式掘进机截割控制方法，该方法包括以下步骤：

步骤1：获取掘进机运行状态信息、掘进机的红外标靶图像及掘进机在巷道空间中的姿态角：

采集掘进机截割臂上的标靶图像，具体的，在截割臂上安装有红外标靶，利用安装在掘进机上的防爆工业相机采用软触发的形式实时采集红外标靶图像；使用安装在掘进机机身两侧超声波传感器获得掘进机与前侧煤壁的距离；利用捷联惯导传感器获得掘进机在巷道空间中的姿态角，姿态角包括掘进机机身的横滚角、掘进机机身的俯仰角和掘进机机身的偏航角；

步骤2：根据步骤1获得的掘进机运行状态信息、掘进机与前侧煤壁的距离、姿态角和掘进机机身参数，通过式(1)得到掘进机相对于巷道的位姿

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

根据掘进机相对于巷道的位姿

以及截割头相对于掘进机机身的位姿

(截割头相对于掘进机机身的位姿

的求解方法见文献[1]杨文娟,马宏伟,张旭辉.悬臂式掘进机截割头姿态视觉检测系统[J].煤炭学报,2018,43(S2):581-590.)

得到掘进机的截割头相对于巷道断面的位姿

如下式(2)所示：

式中，a₁₁、a₁₂、a₁₃、a₂₁、a₂₂、a₂₃、a₃₁、a₃₂、a₃₃为截割头相对巷道的旋转矩阵参数，a₁₄、a₂₄、a₃₄为截割头相对巷道的平移矩阵参数；截割头相对于掘进机机身的位姿

是通过截割头视觉测量系统得到的；

上式中，

a₁₁＝cosγcosβcosθ₁cosθ₂+(cosγsinβsinα-sinγcosα)sinθ₁cosθ₂-(cosγsinβcosα+sinγsinα)sinθ₂

a₁₂＝cosγcosβsinθ₁-(cosγsinβsinα-sinγcosα)cosθ₁

a₁₃＝-cosγcosβcosθ₁sinθ₂-(cosγsinβsinα-sinγcosα)sinθ₁sinθ₂-(cosγsinβcosα+sinγsinα)cosθ₂

a₁₄＝cosγcosβ(b₁cosθ₁cosθ₂-(a₃+a₄+d)cosθ₁sinθ₂+a₂cosθ₁+a₁)+(cosγsinβsinα-sinγcosαb1sinθ1cosθ2-a3+a4+dsinθ1sinθ2+a2sinθ1+(cosγsinβcosα+sinγsinα)(-b1sinθ2-a3+a4+dcosθ2+b2)+Sx (6)

a₂₁＝sinγcosβcosθ₁cosθ₂+(sinγsinβsinα+cosγcosα)sinθ₁cosθ₂-(sinγsinβcosα-cosγsinα)sinθ₂

a₂₂＝sinγcosβsinθ₁-(sinγsinβsinα+cosγcosα)cosθ₁

a₂₃＝-sinγcosβcosθ₁sinθ₂-(sinγsinβsinα+cosγcosα)sinθ₁sinθ₂-(sinγsinβcosα-cosγsinα)cosθ₂

a₂₄＝sinγcosβ(b₁cosθ₁cosθ₂-(a₃+a₄+d)cosθ₁sinθ₂+a₂cosθ₁+a₁)+(sinγsinβsinα+cosγcosαb1sinθ1cosθ2-a3+a4+dsinθ1sinθ2+a2sinθ1+sinγsinβcosα-cosγsinα-b1sinθ2-a3+a4+dcosθ2+b2+Sy (7)

a₃₁＝-sinβcosθ₁cosθ₂+cosβsinαsinθ₁cosθ₂-cosαcosβsinθ₂

a₃₂＝-sinβsinθ₁-cosβsinαcosθ₁

a₃₃＝sinβcosθ₁sinθ₂-cosβsinαsinθ₁sinθ₂-cosαcosβcosθ₂

a₃₄＝-sinβ(-cosθ₁sinθ₂a₄+cosθ₁cosθ₂b₁-cosθ₁sinθ₂(a₃+d)+cosθ₁a₂+a₁)+cosβsinα(-sinθ₁sinθ₂a₄+sinθ₁cosθ₂b₁-sinθ₁sinθ₂(a₃+d)+sinθ₁a₂)+cosαcosβ(-cosθ₂a₄-sinθ₂b₁-cosθ₂(a₃+d)+b₂)+S_z (8)

其中S_x＝-Lcosβcosγ，S_y＝-(D₁+D₃/2)cosαcosγ，S_z＝Lsinβ+b2

步骤3：根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、巷道走向以及截割工艺规划掘进机截割头的导引轨迹，按照S型轨迹进行截割，并将导引轨迹在上位机上进行图形化界面显示；根据所规划导引轨迹，依据截割轨迹上关键点位置坐标，结合截割头与机身的位姿关系进行求解，得到截割头所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂；计算方式如下：

设截齿在截割头坐标系下的坐标向量⁴p＝(X_h，Y_h，Z_h，1)^T，⁴p由截割头截齿位置求得以B点(图6)为例，可根据下式(3)求得B点相对于掘进机机体坐标系的坐标向量^op为(X_o，Y_o，Z_o，1)^T

根据式3得：

Z₀＝-X_hsinθ₂′-Z_hcosθ₂′+-b₁sinθ₂′-(a₂+a₃+d)cosθ₂′ (4)

Y₀＝X_hsinθ₁′cosθ₂′-Y_hcosθ₁′-Z_hsinθ₁′sinθ₂′+b₁sinθ₁′cosθ₂′-(a₂+a₃+dsinθ1′sinθ2′+a1sinθ1′ (5)

根据式(4)求解得到回转角θ₁′，带入式(5)求得升降角θ₂′；其中，X_h、Y_h、Z_h分别为截齿在截割头坐标系下三维坐标，X_o、Y_o、Z_o为规划轨迹中一点在机身坐标系下的三维坐标值，其中a1为掘进机机身标系O₀X₀Y₀Z₀到掘进机回转台坐标系O₁X₁Y₁Z₁ x方向距离，a2为回转台关节坐标系O₁X₁Y₁Z₁到截割臂抬升关节坐标系O₂X₂Y₂Z₂x方向距离，a3为截割臂抬升关节坐标系O₂X₂Y₂Z₂到截割头伸缩关节坐标系O₃X₃Y₃Z₃x方向距离，a4为截割头伸缩关节坐标系到截割头坐标系O₄X₄Y₄Z₄x方向距离，b1为掘进机机身标系O₀X₀Y₀Z₀到掘进机截割头坐标系O₄X₄Y₄Z₄y方向距离，b2为掘进机机身高度，d为截割头伸缩量；

步骤4：将步骤2所得到的截割头相对于巷道断面的位姿，计算截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂，反馈到DSP控制器中，与步骤3中规划导引轨迹比较并计算截割头误差信息，求解得到截割头所需调整的关节角度，并通过控制器反复调整，使得截割头位姿偏差Δ趋近于0；

步骤4.1：依据实时计算所得掘进机的截割头相对于巷道断面的位姿

矩阵，采用反变换法对

矩阵逆运动学求解，得到掘进机截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂；

具体的，对于截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂的求解如下：

截割头相对于巷道断面的位姿

矩阵中：

a₁₄＝cosγcosβ(b₁cosθ₁cosθ₂-(a₃+a₄+d)cosθ₁sinθ₂+a₂cosθ₁+a₁)+(cosγsinβsinα-sinγcosαb1sinθ1cosθ2-a3+a4+dsinθ1sinθ2+a2sinθ1+(cosγsinβcosα+sinγsinα)(-b1sinθ2-a3+a4+dcosθ2+b2)+Sx (6)

a₂₄＝sinγcosβ(b₁cosθ₁cosθ₂-(a₃+a₄+d)cosθ₁sinθ₂+a₂cosθ₁+a₁)+(sinγsinβsinα+cosγcosαb1sinθ1cosθ2-a3+a4+dsinθ1sinθ2+a2sinθ1+sinγsinβcosα-cosγsinα-b1sinθ2-a3+a4+dcosθ2+b2+Sy (7)

a₃₄＝-sinβ(-cosθ₁sinθ₂a₄+cosθ₁cosθ₂b₁-cosθ₁sinθ₂(a₃+d)+cosθ₁a₂+a₁)+cosβsinα(-sinθ₁sinθ₂a₄+sinθ₁cosθ₂b₁-sinθ₁sinθ₂(a₃+d)+sinθ₁a₂)+cosαcosβ(-cosθ₂a₄-sinθ₂b₁-cosθ₂(a₃+d)+b₂)+S_z (8)

联立(6)(7)(8)式可求得截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂；其中a1为掘进机机身标系O₀X₀Y₀Z₀到掘进机回转台坐标系O₁X₁Y₁Z₁ x方向距离，a2为回转台关节坐标系O₁X₁Y₁Z₁到截割臂抬升关节坐标系O₂X₂Y₂Z₂x方向距离，a3为截割臂抬升关节坐标系O₂X₂Y₂Z₂到截割头伸缩关节坐标系O₃X₃Y₃Z₃x方向距离，a4为截割头伸缩关节坐标系到截割头坐标系O₄X₄Y₄Z₄x方向距离，b1为掘进机机身标系O₀X₀Y₀Z₀到掘进机截割头坐标系O₄X₄Y₄Z₄ y方向距离，b2为掘进机机身高度，d为截割头伸缩量，S_x、S_y、S_z分别为式(1)中，掘进机相对于巷道的位姿矩阵

中的平移矩阵；

其中S_x＝-Lcosβcosγ，S_y＝-(D₁+D₃/2)cosαcosγ，S_z＝Lsinβ+b2；

步骤4.2：据步骤3规划的截割头导引轨迹及步骤4.1得到截割头实际摆动的关节角度，计算得到截割头位姿的偏差Δ：

Δ_x＝θ₁-θ₁’，Δ_y＝θ₂-θ₂’ (10)

式中，Δ_x为水平方向偏差，Δ_y为垂直方向偏差，θ₁为截割头实际测量所得回转角、θ₂为截割头实际测量所得升降角；θ′₁为规划轨迹截割头所需摆动的关节角度回转角、θ′₂为规划轨迹截割头所需摆动的关节角度升降角；

步骤4.3：采用模糊PID控制方法，利用DSP控制器输出控制命令通过电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制：采用模糊PID控制方法建立悬臂式掘进机视觉导航截割控制模型，利用步骤4.2得到的偏差Δ_x、Δ_y作为DSP控制器的输入，经比例放大器及电液比例阀控制液压缸伸缩，使掘进机截割头到达期望位置；通过DSP控制器的反复调节，使得截割头位姿偏差Δ最终趋近于零；

具体的，DSP控制器的工作过程如下所示：

采用闭环控制，利用步骤2所得截割头相对于巷道断面的位姿信息，实现视觉反馈，视觉反馈采用以下传递函数模型：

式中：G(s)是传递函数，Kp为模糊PID控制器的比例环节增益；Kv为比例放大器的增益；Kt为电液比例阀的流量增益，m3/s·A；K_u为视觉测量模块的增益；Ah为液压缸有效工作面积；s为系统的阶数；ω_v为换向阀相频宽，rad/S；δ_v为比例换向阀的阻尼比；ω_h为液压缸固有频率，rad/S；ζ_h为液压阻尼比；

图4为截割控制系统传递函数模型图；通过分析比例放大器、电液比例阀、液压缸等各环节特性可以得到视觉导航的掘进机自动截割控制系统传递函数数学模型，在截割头的视觉导航部分中，可将视觉测量部分等效为一个比例环节，在理想情况下，位姿反馈系数为1。

具体的，采用的模糊PID控制方法具体步骤如下：

其中模糊PID控制是以误差e和误差变化率ec为输入对象的，将Kp参数作为输出量；通过模糊控制规则表针对不同时间内的误差e和误差变化率ec进行对应的PID参数自动调节，从而满足相应的需求，使得控制系统具有良好的动静性能。

其中Kp参数的计算如下所示：

K_p＝K_p1+{e,ec} (12)

其中K_p1为PID控制的基本参数，{e,ec}为各个对应的模糊控制所调节参数的输出量。将位置模糊PID控制作为最外环，根据视觉测量系统所反馈的位置信号实时调节控制参数，DSP将位置模糊PID控制的输出电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制。

图3为本发明的视觉导航控制图，进行截割头截割轨迹规划，利用固定在机体上的工业相机连续拍摄固定在截割臂上标靶图像并计算得到截割头位姿，在对掘进机进行运动学建模基础之上，计算得到截割头在巷道中位姿，并将得到截割头位置信息反馈到DSP控制器中，再根据截割头位姿调整策略利用关节控制器及伺服放大器对掘进机液压缸进行控制。

步骤5：将经过位姿偏差修正后的实际回转角和升降角数据保存在数据库中，在巷道断面中用图形的方式显示截割头截割轨迹，实时监视系统工作时的运动状态。

图5为仿真实验中的实时轨迹的可视化界面显示图；可根据已设定的巷道参数自动生成规划的截割轨迹，掘进机司机根据规划轨迹进行可视化截割，其截割轨迹以绿色圆形图案显示，若截割过程中，截割头位置超出规划区域，则显示相应的超挖报警信号灯开始闪烁并进行数据记录。

本发明还提供一种基于视觉导航的煤矿悬臂式掘进机截割控制系统，包括：

数据采集模块，用于获取掘进机运行状态信息、掘进机的红外标靶图像及掘进机在巷道空间中的姿态角；所述数据采集模块包括安装在掘进机机身两侧超声波传感器，用于获得掘进机与前侧煤壁的距离；所述数据采集模块还包括捷联惯导传感器，用于获得掘进机在巷道空间中的姿态角，姿态角包括掘进机机身的横滚角、掘进机机身的俯仰角和掘进机机身的偏航角；

截割头的位姿解算模块，用于根据掘进机相对于巷道的位姿

以及截割头相对于掘进机机身的位姿

求解掘进机的截割头相对于巷道断面的位姿

截割头轨迹规划模块，用于根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、巷道走向以及截割工艺规划掘进机截割头的导引轨迹，并结合截割头与机身的位姿关系进行求解，得到截割头所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂；

截割头位姿修正模块，用于计算掘进机截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂，并与截割头所规划导引轨迹所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂进行比较计算截割头误差信息，求解得到截割头所需调整的关节角度，采用模糊PID控制方法，利用DSP控制器输出控制命令通过电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制，从而对截割头进行位姿偏差修正；

数据存储及监控模块，用于将经过位姿偏差修正后的实际回转角和升降角数据保存在数据库中，在巷道断面中用图形的方式显示截割头截割轨迹，实时监视系统工作时的运动状态。

仿真实验：

本仿真实验所述的掘进机悬臂式设有正方形的红外LED标靶，并设有DSP控制器，液压缸可通过DSP控制的关节控制器以及伺服放大器进行调整，其具体视觉导航控制过程包括：

步骤1：根据视觉导航控制的需要，对掘进机截割臂上的标靶图像以及掘进机运行状态信息进行采集。利用摄像机采用软触发的形式实时采集红外标靶的图像，编写程序实现工业摄像机的软触发，为保证实时性，设定视觉测量的周期不超过100ms将采集到的图像通过千兆网GigE接口传输到机载计算机中；使用捷联惯导传感器获得掘进机在工作过程中掘进机机身的位姿、使用安装在机身两侧超声波传感器获得掘进机与前侧煤壁的距离；将获得的数据通过串口通信传输到机载计算机中，将各传感器的数据处理与计算并存入数据库。在本仿真实验中，掘进机截割头在轨迹引导的初始阶段，各传感器实时采集的截割头的横滚角γ为0.02°，俯仰角β为0.08°，偏航角α为0.2°。

步骤2：通过截割头视觉测量系统，解算出截割头相对于巷道断面的位姿；

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

利用对偶四元数误差模型，依据提取到的标靶图像中特征点在标靶坐标系中坐标，结合四点透视成像方法确定的标靶特征点在摄像机坐标系中的坐标，计算截割头的水平摆角θ1和截割头垂直摆角θ2，通过式(1)计算截割头相对于掘进机机体的位姿

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

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

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

步骤4：将步骤2所得到的巷道空间下的截割头位姿反馈到DSP控制器中，与规划的信息比较得到截割头误差信息，据此采用反变换法对掘进机逆运动学求解得到截割头所需摆动的关节角度；

采用模糊PID控制方法，利用DSP控制器输出控制命令通过电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制；

步骤5：将解算得到的位姿数据保存在数据库中，在巷道断面中用图形的方式显示截割头截割轨迹，监视系统工作时的运动状态；

按实际巷道等比例设定模拟巷道的尺寸为1000mm×1400mm，在断面坐标系下设以A(50，-77，191.5)为起点，根据轨迹规划示意图中规划轨迹进行自动截割，其中点B点坐标为(50，-1223，191.5)、C点坐标为(50，-1243，411.6)和D点坐标为(50，-56，411.6)。为方便计算设定掘进机位于巷道中间位置，并且当截割头水平且位于掘进机中间位置时设定截割头回转角及升降角分别为0°和-90°，根据截割头的视觉测量模块可计算得到截割头位于A点、B点、C点和D点时，截割头的(规划)回转角及升降角分别为(-34°，-69°)、(28°，-69°)、(28°，-82°)以及(-34°，-82°)，测得掘进机初始位姿的掘进机机身位姿横滚角、俯仰角及偏航角分别为0.02°、0.08°、0.2°。使掘进机的截割部空载运行，根据规划的截割轨迹不断截割断面。

表1截割头水平角测量

表2截割头升降角测量

通过本仿真实验可得知，截割头运动过程中回转角以及升降角的角度误差在0.8°以内，截割头中心点位置误差在18mm以内，实验结果误差满足巷道施工标准的误差允许范围。

实验结果表明在利用视觉导航的掘进机控制系统中，掘进机截割头可以按照规划的截割轨迹进行自动截割，并且截割轨迹满足巷道掘进的精度要求，对实现悬臂式掘进机自动化及智能化具有重要意义。

本发明将视觉传感器应用于悬臂式掘进机控制系统中，提出了悬臂式掘进机视觉导航截割控制系统。根据悬臂式掘进机的运动学，利用视觉反馈结果融合各传感器得到有效信息，采用PID控制方法建立悬臂式掘进机视觉导航截割控制模型。工作人员在规划好截割头在巷道断面的截割轨迹后，系统根据各传感器的测量结果以及误差信息控制掘进机截割头按照规划的轨迹进行自动截割，保存并显示截割头的截割轨迹。

需要说明的是本发明并不局限以上具体实现方式中，凡在本申请技术方案基础上做的等同变换均落入本发明的保护范围。

Claims (7)

1.一种基于视觉导航的煤矿悬臂式掘进机截割控制方法，其特征在于，包括以下步骤：

步骤1：获取掘进机运行状态信息、掘进机的红外标靶图像及掘进机在巷道空间中的姿态角：

步骤2：根据步骤1获得的掘进机运行状态信息、掘进机与前侧煤壁的距离、姿态角和掘进机机身参数，通过式(1)得到掘进机相对于巷道的位姿

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

根据掘进机相对于巷道的位姿

以及截割头相对于掘进机机身的位姿

得到掘进机的截割头相对于巷道断面的位姿

如下式(2)所示：

式中，a₁₁、a₁₂、a₁₃、a₂₁、a₂₂、a₂₃、a₃₁、a₃₂、a₃₃为截割头相对巷道的旋转矩阵参数，a₁₄、a₂₄、a₃₄为截割头相对巷道的平移矩阵参数；

步骤3：根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、巷道走向以及截割工艺规划掘进机截割头的导引轨迹，按照S型轨迹进行截割，并将导引轨迹在上位机上进行图形化界面显示；根据所规划导引轨迹，依据截割轨迹上关键点位置坐标，结合截割头与机身的位姿关系进行求解，得到截割头所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂；

步骤4：将步骤2所得到的截割头相对于巷道断面的位姿，计算截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂，反馈到DSP控制器中，与步骤3中规划导引轨迹比较并计算截割头误差信息，求解得到截割头所需调整的关节角度，并通过控制器反复调整，使得截割头位姿偏差Δ趋近于0；

步骤4.1：依据实时计算所得掘进机的截割头相对于巷道断面的位姿

矩阵，采用反变换法对

矩阵逆运动学求解，得到掘进机截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂；

步骤4.2：据步骤3规划的截割头导引轨迹及步骤4.1得到截割头实际摆动的关节角度，计算得到截割头位姿的偏差Δ：

Δ_x＝θ₁-θ₁′，Δ_y＝θ₂-θ₂′ (10)

式中，Δ_x为水平方向偏差，Δ_y为垂直方向偏差，θ₁为截割头实际测量所得回转角、θ₂为截割头实际测量所得升降角；θ′₁为规划轨迹截割头所需摆动的关节角度回转角、θ′₂为规划轨迹截割头所需摆动的关节角度升降角；

步骤4.3：采用模糊PID控制方法，利用DSP控制器输出控制命令通过电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制：采用模糊PID控制方法建立悬臂式掘进机视觉导航截割控制模型，利用步骤4.2得到的偏差Δ_x、Δ_y作为DSP控制器的输入，经比例放大器及电液比例阀控制液压缸伸缩，使掘进机截割头到达期望位置；通过DSP控制器的反复调节，使得截割头位姿偏差Δ最终趋近于零；

步骤5：将经过位姿偏差修正后的实际回转角和升降角数据保存在数据库中，在巷道断面中用图形的方式显示截割头截割轨迹，实时监视系统工作时的运动状态。

2.如权利要求1所述的基于视觉导航的煤矿悬臂式掘进机截割控制方法，其特征在于，所述步骤1中，使用安装在掘进机机身两侧超声波传感器获得掘进机与前侧煤壁的距离；利用捷联惯导传感器获得掘进机在巷道空间中的姿态角，姿态角包括掘进机机身的横滚角、掘进机机身的俯仰角和掘进机机身的偏航角。

3.如权利要求1所述的基于视觉导航的煤矿悬臂式掘进机截割控制方法，其特征在于，所述步骤3中截割头所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂的计算步骤如下：

设截齿在截割头坐标系下的坐标向量⁴p＝(X_h，Y_h，Z_h，1)^T，截齿在截割头坐标系下的坐标向量⁴p由截割头截齿位置求得，规划的轨迹中取一个点，设其在掘进机机身下的坐标向量为°p，根据下式(3)求得°p为(X_o，Y_o，Z_o，1)^T：

根据式(3)得：

Z₀＝-X_hsinθ₂′-Z_hcosθ₂′+-b₁sinθ₂′-(a₂+a₃+d)cosθ₂′ (4)

Y₀＝X_hsinθ₁′cosθ₂′-Y_hcosθ₁′-Z_hsinθ₁′sinθ₂′+b₁sinθ₁′cosθ₂′-(a₂+a₃+d)sinθ₁′sinθ₂′+a₁sinθ₁′ (5)

根据式(4)求解得到截割头所需摆动的关节角度回转角θ′₁，带入式(5)求得截割头所需摆动的关节角度升降角θ′₂；其中，X_h、Y_h、Z_h分别为截齿在截割头坐标系下三维坐标，X_o、Y_o、Z_o为规划轨迹中一点在机身坐标系下的三维坐标值，其中a1为掘进机机身标系O₀X₀Y₀Z₀到掘进机回转台坐标系O₁X₁Y₁Z₁ x方向距离，a2为回转台关节坐标系O₁X₁Y₁Z₁到截割臂抬升关节坐标系O₂X₂Y₂Z₂ x方向距离，a3为截割臂抬升关节坐标系O₂X₂Y₂Z₂到截割头伸缩关节坐标系O₃X₃Y₃Z₃ x方向距离，a4为截割头伸缩关节坐标系到截割头坐标系O₄X₄Y₄Z₄ x方向距离，b1为掘进机机身标系O₀X₀Y₀Z₀到掘进机截割头坐标系O₄X₄Y₄Z₄ y方向距离，b2为掘进机机身高度，d为截割头伸缩量。

4.如权利要求1所述的基于视觉导航的煤矿悬臂式掘进机截割控制方法，其特征在于，所述步骤4.1中，对于截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂的求解如下：

截割头相对于巷道断面的位姿

矩阵中：

a₁₄＝cosγcosβ(b₃cosθ₁cosθ₂-(a₃+a₄+d)cosθ₁sinθ₂+a₂cosθ₁+a₁)+(cosγsinβsinα-sinγcosαb3sinθ1cosθ2-a3+a4+dsinθ1sinθ2+a2sinθ1+(cosγsinβcosα+sinγsinα)(-b3sinθ2-a3+a4+dcosθ2+b2)+Sx (6)

a₂₄＝sinγcosβ(b₁cosθ₁cosθ₂-(a₃+a₄+d)cosθ₁sinθ₂+a₂cosθ₁+a₁)+(sinγsinβsinα+cosγcosαb1sinθ1cosθ2-a3+a4+dsinθ1sinθ2+a2sinθ1+sinγsinβcosα-cosγsinα-b1sinθ2-a3+a4+dcosθ2+b2+Sy (7)

a₃₄＝-sinβ(-cosθ₁sinθ₂a₄+cosθ₁cosθ₂b₁-cosθ₁sinθ₂(a₃+d)+cosθ₁a₂+a₁)+cosβsinα(-sinθ₁sinθ₂a₄+sinθ₁cosθ₂b₁-sinθ₁sinθ₂(a₃+d)+sinθ₁a₂)+cosαcosβ(-cosθ₂a₄-sinθ₂b₁-cosθ₂(a₃+d)+b₂)+S_z (8)

联立(6)(7)(8)式可求得截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂；其中a1为掘进机机身标系O₀X₀Y₀Z₀到掘进机回转台坐标系O₁X₁Y₁Z₁ x方向距离，a2为回转台关节坐标系O₁X₁Y₁Z₁到截割臂抬升关节坐标系O₂X₂Y₂Z₂ x方向距离，a3为截割臂抬升关节坐标系O₂X₂Y₂Z₂到截割头伸缩关节坐标系O₃X₃Y₃Z₃ x方向距离，a4为截割头伸缩关节坐标系到截割头坐标系O₄X₄Y₄Z₄ x方向距离，b1为掘进机机身标系O₀X₀Y₀Z₀到掘进机截割头坐标系O₄X₄Y₄Z₄ y方向距离，b2为掘进机机身高度，d为截割头伸缩量，S_x、S_y、S_z分别为式(1)中，掘进机相对于巷道的位姿矩阵

中的平移矩阵；

其中S_x＝-L cosβcosγ，S_y＝-(D₁+D₃/2)cosαcosγ，S_z＝L sinβ+b2。

5.如权利要求1所述的基于视觉导航的煤矿悬臂式掘进机截割控制方法，其特征在于，所述步骤4.3中，具体的，DSP控制器的工作过程：

采用闭环控制，利用步骤2所得截割头相对于巷道断面的位姿信息，实现视觉反馈，视觉反馈采用以下传递函数模型：

式中：G(s)是传递函数，Kp为模糊PID控制器的比例环节增益；Kv为比例放大器的增益；Kt为电液比例阀的流量增益，m3/s·A；K_μ为视觉测量模块的增益；Ah为液压缸有效工作面积；s为系统的阶数；ω_v为换向阀相频宽，rad/S；δ_v为比例换向阀的阻尼比；ω_h为液压缸固有频率，rad/S；ζ_h为液压阻尼比。

6.如权利要求1所述的基于视觉导航的煤矿悬臂式掘进机截割控制方法，其特征在于，所述步骤4.3中，采用模糊PID控制方法具体步骤如下：

模糊PID控制是以误差e和误差变化率ec为输入对象的，将Kp参数作为输出量；通过模糊控制规则表针对不同时间内的误差e和误差变化率ec进行对应的PID参数自动调节，从而满足相应的需求，使得控制系统具有良好的动静性能；

Kp参数的计算如下所示：

K_p＝K_p1+{e，ec} (12)

其中K_p1为PID控制的基本参数，{e，ec}为各个对应的模糊控制所调节参数的输出量；将位置模糊PID控制作为最外环，根据视觉测量系统所反馈的位置信号实时调节控制参数，DSP将位置模糊PID控制的输出电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制。

7.一种基于视觉导航的煤矿悬臂式掘进机截割控制系统，其特征在于，包括：

数据采集模块，用于获取掘进机运行状态信息、掘进机的红外标靶图像及掘进机在巷道空间中的姿态角；所述数据采集模块包括安装在掘进机机身两侧超声波传感器，用于获得掘进机与前侧煤壁的距离；所述数据采集模块还包括捷联惯导传感器，用于获得掘进机在巷道空间中的姿态角，姿态角包括掘进机机身的横滚角、掘进机机身的俯仰角和掘进机机身的偏航角；

截割头的位姿解算模块，用于根据掘进机相对于巷道的位姿

以及截割头相对于掘进机机身的位姿

求解掘进机的截割头相对于巷道断面的位姿

截割头轨迹规划模块，用于根据掘进机工作面巷道断面类型、截割头直径、巷道尺寸、巷道走向以及截割工艺规划掘进机截割头的导引轨迹，并结合截割头与机身的位姿关系进行求解，得到截割头所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂；

截割头位姿修正模块，用于计算掘进机截割头实际测量所得回转角θ₁和截割头实际测量所得升降角θ₂，并与截割头所规划导引轨迹所需摆动的关节角度回转角θ′₁、截割头所需摆动的关节角度升降角θ′₂进行比较计算截割头误差信息，求解得到截割头所需调整的关节角度，采用模糊PID控制方法，利用DSP控制器输出控制命令通过电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制，从而对截割头进行位姿偏差修正；

数据存储及监控模块，用于将经过位姿偏差修正后的实际回转角和升降角数据保存在数据库中，在巷道断面中用图形的方式显示截割头截割轨迹，实时监视系统工作时的运动状态。

Images