CN101832136B - 掘进机自动纠偏方法及系统 - Google Patents

Abstract

本发明涉及掘进机行进控制领域，具体是一种掘进机自动纠偏方法及系统。能针对掘进机前行方向相对于巷道中心线A发生的偏离实现自动、高准确度纠偏，为在不远的未来实现掘进机自动化提供基础，所述方法包括：测量步骤；计算步骤；保护性方位角纠偏步骤；中心偏距纠偏步骤；方位角纠偏步骤。所述系统包括：测量模块；计算模块；保护性方位角纠偏模块；中心偏距纠偏模块；方位角纠偏模块。本发明实现了掘进机自动纠偏，为掘进机自动化掘进提供了技术基础，使得掘进司机可以远离掘进工作面，从恶劣的工作环境中解放出来；能有效避免因掘进司机人为因素造成的巷道成型不规范问题，提高了巷道掘进效率，改善了巷道成形质量，有力于大断面煤巷的维护。

Description

掘进机自动纠偏方法及系统

技术领域

本发明涉及掘进机行进控制领域，具体是一种掘进机自动纠偏方法及系统。

背景技术

掘进机工作面环境恶劣、地质条件复杂，掘进机在进行断面截割以及行走过程中，会出现掘进机前行方向相对于巷道中心线A发生偏离的现象。所述偏离现象会导致掘进机姿态参数：方位角α(掘进机当前前行方向相对于巷道中心线A偏离的角度，如图2所示)、中心偏距a(掘进机机身中心点O₂相对于巷道中心线A的偏距，如图3所示)过大，超出掘进机正常截割、行进允许范围，导致掘进机在运行时无法按照规整的断面掘进，出现欠挖和超挖现象，增加后续施工强度与成本。而目前，针对掘进机前行方向相对于巷道中心线A发生的偏离主要采用手动方式纠偏，即由司机以肉眼观察为依据，操控掘进机来调整掘进机姿态，但由于司机个人体力、精力的限制及环境状况等因素的影响，司机极易出现误判，导致纠偏失败，影响掘进施工质量。因此，急需在掘进过程中实现自动、高准确度纠偏。

掘进机前行方向的理想情况：掘进机在前行方向上的中心线与巷道中心线A重合，即方位角α＝0、中心偏距a＝0，如图4所示，掘进机处于理想掘进状态，不需要纠偏。

而掘进机前行方向相对于巷道中心线A发生偏离会导致掘进机姿态参数：方位角α、中心偏距a有如下3种需要纠偏的情况：

1、仅存在方位角偏差，掘进机在前行方向上的中心线与巷道中心线A存在一定的夹角，但掘进机机身中心点在巷道中心线A上，即中心偏距a＝0、方位角α≠0，如图5所示；

2、仅存在中心偏距偏差，掘进机机身中心点不在巷道中心线A上，但掘进机在前行方向上的中心线与巷道中心线A平行，即中心偏距a≠0、方位角α＝0，如图6所示；

3、同时存在方位角偏差和中心偏距偏差，即中心偏距a≠0、方位角α≠0，如图7所示，为掘进机掘进过程中经常遇到的情况。

发明内容

本发明为了能针对掘进机前行方向相对于巷道中心线A发生的偏离实现自动、高准确度纠偏，为在不远的未来实现掘进机自动化提供基础，提供了一种掘进机自动纠偏方法及系统。

本发明是采用如下技术方案实现的：掘进机自动纠偏方法，所述掘进机上设置有三个与掘进机控制系统中机载可编程控制器PLC连接的测距传感器，其中两个测距传感器固定于掘进机同一侧，两个测距传感器的安装位置分别位于掘进机导向轮和驱动轮的轮轴正上方，且两个测距传感器的安装位置所在直线与掘进机在前行方向上的中心线平行，第三个测距传感器单独固定于掘进机另一侧，其安装位置以掘进机在前行方向上的中心线为对称轴与同侧固定的两测距传感器间的中心位置对称，三个测距传感器的测量方向与掘进机在前行方向上的中心线垂直；

所述自动纠偏方法包括如下步骤：

测量步骤，触发三个测距传感器分别测量自身与巷道煤帮的距离，即掘进机侧帮固定测距传感器的安装位置在测距传感器的测量方向上距离巷道煤帮的距离；并标定固定于掘进机同侧的两个测距传感器中：导向轮轮轴上方测距传感器的测量结果表示为l₁、驱动轮轮轴上方测距传感器的测量结果表示为l₂，单独固定于掘进机另一侧的测距传感器的测量结果表示为l₃；

计算步骤，根据测量步骤的测量结果，经数学计算，获得掘进机当前前行方向相对于巷道中心线A偏离的方位角α、掘进机机身中心点O₂相对于巷道中心线A偏离的中心偏距a；

保护性方位角纠偏步骤，判断计算步骤获得的方位角α与掘进机方位角限定值θ_Bound的关系，如方位角α小于方位角限定值θ_Bound，直接进行中心偏距纠偏步骤；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量步骤、计算步骤确定的方位角α小于方位角限定值θ_Bound；

即：如l₂＞l₁，表明掘进机的当前前行方向相对于巷道中心线A偏向固定有两测距传感器的一侧偏离，则控制掘进机固定有两测距传感器一侧的驱动轮正转前进，另一侧驱动轮同速反转后退，直至方位角α小于方位角限定值θ_Bound；如l₂＜l₁，表明掘进机的当前前行方向相对于巷道中心线A偏向固定有第三测距传感器的一侧偏离，则控制掘进机固定有第三测距传感器一侧的驱动轮正转前进，另一侧驱动轮同速反转后退，直至方位角α小于方位角限定值θ_Bound；

中心偏距纠偏步骤，判断计算步骤获得的中心偏距a与掘进机中心偏距允许值L_ε的关系，如中心偏距a小于中心偏距允许值L_ε，直接进行方位角纠偏步骤；反之，判断掘进机方位角α是否为零、以及掘进机中心偏距a的偏置方向-即掘进机机身中心点相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反转后退实现纠偏，如方位角α为零，则掘进机处于巷道中心线A一侧的驱动轮反转速度小于掘进机偏离方向一侧的驱动轮反转速度；如方位角a非零，则掘进机两侧驱动轮同速反转后退，直至经测量步骤、计算步骤确定的中心偏距a小于中心偏距允许值L_ε；

方位角纠偏步骤，判断计算步骤获得的方位角α与掘进机方位角允许值θ_ε0的关系，如方位角α小于方位角允许值θ_ε0，则掘进机自动纠偏完成；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量步骤、计算步骤确定的方位角α小于方位角允许值θ_ε0；

其中，所述掘进机方位角限定值θ_Bound的设定应保证执行保护性方位角纠偏步骤后的掘进机在执行中心偏距纠偏时，能有效避免与巷道煤帮发生碰撞；所述中心偏距允许值L_ε、方位角允许值θ_ε0的设定应保证掘进机正常掘进的进行。

掘进机自动纠偏系统，所述掘进机上设置有三个与掘进机控制系统中机载可编程控制器PLC连接的测距传感器，其中两个测距传感器固定于掘进机同一侧，两个测距传感器的安装位置分别位于掘进机导向轮和驱动轮的轮轴正上方，且两个测距传感器的安装位置所在直线与掘进机在前行方向上的中心线平行，第三个测距传感器单独固定于掘进机另一侧，其安装位置以掘进机在前行方向上的中心线为对称轴与同侧固定的两测距传感器间的中心位置对称，三个测距传感器的测量方向与掘进机在前行方向上的中心线垂直；

所述自动纠偏系统包括如下模块：

测量模块，触发三个测距传感器分别测量自身与巷道煤帮的距离，即掘进机侧帮固定测距传感器的安装位置在测距传感器的测量方向上距离巷道煤帮的距离；并标定固定于掘进机同侧的两个测距传感器中：导向轮轮轴上方测距传感器的测量结果表示为l₁、驱动轮轮轴上方测距传感器的测量结果表示为l₂，单独固定于掘进机另一侧的测距传感器的测量结果表示为l₃；

计算模块，根据测量模块的测量结果，经数学计算，获得掘进机当前前行方向相对于巷道中心线A偏离的方位角α、掘进机机身中心点O₂相对于巷道中心线A偏离的中心偏距a；

保护性方位角纠偏模块，判断计算模块获得的方位角α与掘进机方位角限定值θ_Bound的关系，如方位角α小于方位角限定值θ_Bound，直接由中心偏距纠偏模块控制实现掘进机中心偏距纠偏；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量模块、计算模块确定的方位角α小于方位角限定值θ_Bound；

中心偏距纠偏模块，判断计算模块获得的中心偏距a与掘进机中心偏距允许值L_ε的关系，如中心偏距a小于中心偏距允许值L_ε，直接由方位角纠偏模块控制实现掘进机方位角纠偏；反之，判断掘进机方位角α是否为零、以及掘进机中心偏距a的偏置方向-即掘进机机身中心点相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反转后退实现纠偏，如方位角α为零，则掘进机处于巷道中心线A一侧的驱动轮反转速度小于掘进机偏离方向一侧的驱动轮反转速度；如方位角α非零，则掘进机两侧驱动轮同速反转后退，直至经测量模块、计算模块确定的中心偏距a小于中心偏距允许值L_ε；

方位角纠偏模块，判断计算模块获得的方位角α与掘进机方位角允许值θ_ε0的关系，如方位角α小于方位角允许值θ_ε0，则掘进机自动纠偏完成；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量模块、计算模块确定的方位角α小于方位角允许值θ_ε0；

其中，所述掘进机方位角限定值θ_Bound的设定应保证执行保护性方位角纠偏后的掘进机在执行中心偏距纠偏时，能有效避免与巷道煤帮发生碰撞；所述中心偏距允许值L_ε、方位角允许值θ_ε0的设定应保证掘进机正常掘进的进行。

本发明实现了掘进机自动纠偏，为掘进机自动化掘进提供了技术基础，使得掘进司机可以远离掘进工作面，从恶劣的工作环境中解放出来；能有效避免因掘进司机人为因素造成的巷道成型不规范问题，提高了巷道掘进效率，改善了巷道成形质量，有力于大断面煤巷的维护。

附图说明

图1为本发明所述系统的原理方框图；

图2为掘进机姿态参数-方位角α的示意图；

图3为掘进机姿态参数-中心偏距a的示意图；

图4为掘进机前行方向理想状态示意图；

图5为掘进机仅存在方位角偏差的示意图；

图6为掘进机仅存在中心偏距的示意图；

图7为掘进机同时存在方位角和中心偏距的示意图；

图8为图5的简化示意图；

图9为图6的简化示意图；

图10为图7的简化示意图；

图11为本发明所述方法的一具体流程图；

图12为本发明所述方法中保护性方位角纠偏步骤的一具体流程图；

图13为本发明所述方法中中心偏距纠偏步骤的一具体流程图；

图14为本发明所述方法中方位角纠偏步骤的一具体流程图；

具体实施方式

掘进机自动纠偏方法，所述掘进机上设置有三个与掘进机控制系统中机载可编程控制器PLC连接的测距传感器，其中两个测距传感器固定于掘进机同一侧，两个测距传感器的安装位置分别位于掘进机导向轮和驱动轮的轮轴正上方，且两个测距传感器的安装位置所在直线与掘进机在前行方向上的中心线平行，第三个测距传感器单独固定于掘进机另一侧，其安装位置以掘进机在前行方向上的中心线为对称轴与同侧固定的两测距传感器间的中心位置对称，三个测距传感器的测量方向与掘进机在前行方向上的中心线垂直；

所述自动纠偏方法包括如下步骤：(参见图11)

测量步骤，触发三个测距传感器分别测量自身与巷道煤帮的距离，即掘进机侧帮固定测距传感器的安装位置在测距传感器的测量方向上距离巷道煤帮的距离；并标定固定于掘进机同侧的两个测距传感器中：导向轮轮轴上方测距传感器的测量结果表示为l₁、驱动轮轮轴上方测距传感器的测量结果表示为l₂，单独固定于掘进机另一侧的测距传感器的测量结果表示为l₃；

计算步骤，根据测量步骤的测量结果，经数学计算，获得掘进机当前前行方向相对于巷道中心线A偏离的方位角α、掘进机机身中心点O₂相对于巷道中心线A偏离的中心偏距a；

保护性方位角纠偏步骤，判断计算步骤获得的方位角α与掘进机方位角限定值θ_Bound的关系，如方位角α小于方位角限定值θ_Bound，直接进行中心偏距纠偏步骤；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量步骤、计算步骤确定的方位角α小于方位角限定值θ_Bound；(参见图12)

即：如l₂＞l₁，表明掘进机的当前前行方向相对于巷道中心线A偏向固定有两测距传感器的一侧偏离，则控制掘进机固定有两测距传感器一侧的驱动轮正转前进，另一侧驱动轮同速反转后退，直至方位角α小于方位角限定值θ_Bound；如l₂＜l₁，表明掘进机的当前前行方向相对于巷道中心线A偏向固定有第三测距传感器的一侧偏离，则控制掘进机固定有第三测距传感器一侧的驱动轮正转前进，另一侧驱动轮同速反转后退，直至方位角α小于方位角限定值θ_Bound；

中心偏距纠偏步骤，判断计算步骤获得的中心偏距a与掘进机中心偏距允许值L_ε的关系，如中心偏距a小于中心偏距允许值L_ε，直接进行方位角纠偏步骤；反之，判断掘进机方位角α是否为零、以及掘进机中心偏距a的偏置方向-即掘进机机身中心点相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反转后退实现纠偏，如方位角α为零，则掘进机处于巷道中心线A一侧的驱动轮反转速度小于掘进机偏离方向一侧的驱动轮反转速度；如方位角α非零，则掘进机两侧驱动轮同速反转后退，直至经测量步骤、计算步骤确定的中心偏距a小于中心偏距允许值L_ε；(参见图13，图中L_ε1与中心偏距允许值L_ε对应)

方位角纠偏步骤，判断计算步骤获得的方位角α与掘进机方位角允许值θ_ε0的关系，如方位角α小于方位角允许值θ_ε0，则掘进机自动纠偏完成；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量步骤、计算步骤确定的方位角α小于方位角允许值θ_ε0；(参见图14)

其中，所述掘进机方位角限定值θ_Bound的设定应保证执行保护性方位角纠偏步骤后的掘进机在执行中心偏距纠偏时，能有效避免与巷道煤帮发生碰撞；所述中心偏距允许值L_ε、方位角允许值θ_ε0的设定应保证掘进机正常掘进的进行。

如图1所示，掘进机自动纠偏系统，所述掘进机上设置有三个与掘进机控制系统中机载可编程控制器PLC连接的测距传感器，其中两个测距传感器固定于掘进机同一侧，两个测距传感器的安装位置分别位于掘进机导向轮和驱动轮的轮轴正上方，且两个测距传感器的安装位置所在直线与掘进机在前行方向上的中心线平行，第三个测距传感器单独固定于掘进机另一侧，其安装位置以掘进机在前行方向上的中心线为对称轴与同侧固定的两测距传感器间的中心位置对称，三个测距传感器的测量方向与掘进机在前行方向上的中心线垂直；

所述自动纠偏系统包括如下模块：

测量模块，触发三个测距传感器分别测量自身与巷道煤帮的距离，即掘进机侧帮固定测距传感器的安装位置在测距传感器的测量方向上距离巷道煤帮的距离；并标定固定于掘进机同侧的两个测距传感器中：导向轮轮轴上方测距传感器的测量结果表示为l₁、驱动轮轮轴上方测距传感器的测量结果表示为l₂，单独固定于掘进机另一侧的测距传感器的测量结果表示为l₃；

计算模块，根据测量模块的测量结果，经数学计算，获得掘进机当前前行方向相对于巷道中心线A偏离的方位角α、掘进机机身中心点O₂相对于巷道中心线A偏离的中心偏距a；

保护性方位角纠偏模块，判断计算模块获得的方位角α与掘进机方位角限定值θ_Bound的关系，如方位角α小于方位角限定值θ_Bound，直接由中心偏距纠偏模块控制实现掘进机中心偏距纠偏；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量模块、计算模块确定的方位角α小于方位角限定值θ_Bound；

中心偏距纠偏模块，判断计算模块获得的中心偏距a与掘进机中心偏距允许值L_ε的关系，如中心偏距a小于中心偏距允许值L_ε，直接由方位角纠偏模块控制实现掘进机方位角纠偏；反之，判断掘进机方位角α是否为零、以及掘进机中心偏距a的偏置方向-即掘进机机身中心点相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反转后退实现纠偏，如方位角α为零，则掘进机处于巷道中心线A一侧的驱动轮反转速度小于掘进机偏离方向一侧的驱动轮反转速度；如方位角α非零，则掘进机两侧驱动轮同速反转后退，直至经测量模块、计算模块确定的中心偏距a小于中心偏距允许值L_ε；

方位角纠偏模块，判断计算模块获得的方位角α与掘进机方位角允许值θ_ε0的关系，如方位角α小于方位角允许值θ_ε0，则掘进机自动纠偏完成；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量模块、计算模块确定的方位角α小于方位角允许值θ_ε0；

其中，所述掘进机方位角限定值θ_Bound的设定应保证执行保护性方位角纠偏后的掘进机在执行中心偏距纠偏时，能有效避免与巷道煤帮发生碰撞；所述中心偏距允许值L_ε、方位角允许值θ_ε0的设定应保证掘进机正常掘进的进行。

具体实施时，所述测距传感器采用现有超声波传感测距传感器；

且本发明所述方法中的计算步骤可以按照如下计算式计算得到方位角α和中心偏距a：

$\mathrm{\α}=\arctan \frac{l_2-l_1}{D}---(2-1)$

$a=\frac{L}{2}-(l_3+\frac{d}{2})\cos \mathrm{\α}---(2-2)$

或者也可以按照如下计算式计算得到方位角α和中心偏距a：

$\cos \mathrm{\α}=\frac{D}{\sqrt{D^2+{(l_1-l_2)}^2}}---(2-3)$

$a=\frac{L}{2}-(l_3+\frac{d}{2})\frac{D}{\sqrt{D^2+{(l_1-l_2)}^2}}---(2-4)$

式中：D-掘进机同侧导向轮和驱动轮之间的距离；d-两驱动轮之间的距离；L-巷道的宽度；

如图10所示，结合上述计算式可知，当l₂＝l₁＝l₃时，a＝0且α＝0，表明掘进机中心轴线与巷道中心线A重合；当l₂＝l₁＞l₃时，a＞0，α＝0，表明掘进机中心轴线与巷道中心线A并行但沿掘进机前行方向向右偏；当l₂＝l₁＜l₃时，a＜0，α＝0，表明掘进机中心轴线与巷道中心线A并行但沿掘进机前行方向向左偏；当l₂≠l₁≠l₃时，a≠0且α≠0，表明掘进机中心轴线与巷道中心线A不并行且掘进机机身中心不在巷道中心线A上。

按照本发明中掘进机上三个测距传感器的布置特点，本发明所述方法可以对掘进机姿态参数：方位角α、中心偏距a按照下述情况具体实施纠偏：

如图5、8所示，如掘进机仅存在方位角偏差，则掘进机上三个测距传感器的测量结果会满足如下数学模型：

$\left\{\begin{array}{c}{l}_3=(l_1+l_2)/2\\ l_1\≠l_2\\ \mathrm{Flag}=\mathrm{sign}(l_1-l_2)\end{array}\right.---(2-29)$

其中，Flag为标志位。

1)如果Flag＞0，则说明掘进机沿掘进机前行方向向左偏离巷道中心线A，此时，控制掘进机左、右驱动轮向相反的方向行进，并有V_{L_ahead}＝V_{R_back}(左侧驱动轮正转向前，右侧驱动轮反转后退。其中V_{L_ahead}表示左侧驱动轮正转向前速度，V_{R_back}表示右侧驱动轮反转后退速度)，保持两个驱动轮运行，直到l₁＝l₂，掘进机方位角偏差消除。

2)如果Flag＜0，则说明掘进机沿掘进机前行方向向右偏离巷道中心线A，此时，控制掘进机左、右驱动轮向相反的方向行进，并有V_{L_back}＝V_{R_ahead}(左侧驱动轮反转后退，右侧驱动轮正转向前。其中V_{L_back}表示左侧驱动轮反转后退速度，V_{R_ahead}表示右侧驱动轮正转向前速度)，保持两个驱动轮运行，直到l₁＝l₂。

即如果掘进机仅存在方位角偏差，只需控制掘进机左右驱动轮以相同速度相相反方向运行，在原地旋转消除方位角偏差。

如图6、9所示，如掘进机仅存在中心偏距偏差，则掘进机上三个测距传感器的测量结果会满足如下数学模型：

$\left\{\begin{array}{c}{l}_3\≠(l_1+l_2)/2\\ l_1=l_2\\ \mathrm{Flag}=\mathrm{sign}(l_3-l_1)\end{array}\right.---(2-30)$

其中Flag为标志位。

1)如果Flag＞0，则说明掘进机沿掘进方向向右偏离巷道中心线A，此时，通过纠偏控制器控制掘进机左、右驱动轮同时反转后退，并有V_{R_back}＝V_{L_back}/2，直到l₃＝(l₁+l₂)/2，此时掘进机位置状态变为仅存在方位角偏差，按照前述方位角偏差纠偏方法消除方位角偏差即可。

2)如果Flag＜0，则说明掘进机沿掘进方向向左偏离巷道中心线A，此时，通过纠偏控制器控制掘进机左、右驱动轮同时反转后退，并有V_{L_back}＝V_{R_back}/2，直到l₃＝(l₁+l₂)/2，此时掘进机位置状态变为仅存在方位角偏差，按照前述方位角偏差纠偏方法消除方位角偏差即可。

即如果掘进机仅存在中心偏距，控制掘进机左右驱动轮行走速度不相等，产生方位角偏差，逐渐减小中心偏距，待中心偏距消除后，再纠正方位角偏差。

如图7、10所示，如掘进机同时存在方位角偏差和中心偏距偏差，则掘进机上三个测距传感器的测量结果会满足如下数学模型：

$\left\{\begin{array}{c}{l}_3\≠(l_1+l_2)/2\\ l_1\≠l_2\\ \mathrm{Flag}=\mathrm{sign}(l_1-l_2)\end{array}\right.---(2-31)$

其中，Flag为标志位。

1)如果Flag＞0，则说明掘进机沿掘进方向向左偏离巷道中心线A，此时，通过纠偏控制器控制掘进机左、右驱动轮同时反转后退，并有V_{R_back}＝V_{L_back}，直到l₃＝(l₁+l₂)/2，此时掘进机位置状态变为仅存在方位角偏差，按照前述方位角偏差纠偏方法消除方位角偏差即可。

2)如果Flag＜0，则说明掘进机沿掘进方向向右偏离巷道中心线A，此时，通过纠偏控制器控制掘进机左、右驱动轮同时反转后退，并有V_{L_back}＝V_{R_back}，直到l₃＝(l₁+l₂)/2，此时掘进机位置状态变为仅存在方位角偏差，按照前述方位角偏差纠偏方法消除方位角偏差即可。

即如果掘进机同时存在方位角偏差和中心偏距偏差，通过掘进机先消除中心偏距，再消除方位角偏差。

总结后，本发明所述方法的纠偏策略可以概括为：

(1)先纠掘进机中心偏距，再纠掘进机方位角偏差。

(2)只有中心偏距时，控制左右驱动轮同向不同速运行进行掘进机中心偏距纠正，再控制左右驱动轮同速反向运行纠正掘进机方位角偏差。

(3)同时存在中心偏距和方位角偏差时，控制左右驱动轮同向同速运行先纠正掘进机中心偏距，再控制左右驱动轮同速反向运行纠掘进机方位角偏差。

Claims (2)

1.一种掘进机自动纠偏方法，其特征在于：所述掘进机上设置有三个与掘进机控制系统中机载可编程控制器PLC连接的测距传感器，其中两个测距传感器固定于掘进机同一侧，两个测距传感器的安装位置分别位于掘进机导向轮和驱动轮的轮轴正上方，且两个测距传感器的安装位置所在直线与掘进机在前行方向上的中心线平行，第三个测距传感器单独固定于掘进机另一侧，其安装位置以掘进机在前行方向上的中心线为对称轴与同侧固定的两测距传感器间的中心位置对称，三个测距传感器的测量方向与掘进机在前行方向上的中心线垂直；

所述自动纠偏方法包括如下步骤：

测量步骤，触发三个测距传感器分别测量自身与巷道煤帮的距离，即掘进机侧帮固定测距传感器的安装位置在测距传感器的测量方向上距离巷道煤帮的距离；并标定固定于掘进机同侧的两个测距传感器中：导向轮轮轴上方测距传感器的测量结果表示为l₁、驱动轮轮轴上方测距传感器的测量结果表示为l₂，单独固定于掘进机另一侧的测距传感器的测量结果表示为l₃；

计算步骤，根据测量步骤的测量结果，经数学计算，获得掘进机当前前行方向相对于巷道中心线A偏离的方位角α、掘进机机身中心点O₂相对于巷道中心线A偏离的中心偏距α；

保护性方位角纠偏步骤，判断计算步骤获得的方位角α与掘进机方位角限定值θ_Bound的关系，如方位角α小于方位角限定值θ_Bound，直接进行中心偏距纠偏步骤；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮相反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量步骤、计算步骤确定的方位角α小于方位角限定值θ_Bound；

即：如l₂＞l₁，表明掘进机的当前前行方向相对于巷道中心线A偏向固定有两测距传感器的一侧偏离，则控制掘进机固定有两测距传感器一侧的驱动轮正转前进，另一侧驱动轮同速反转后退，直至方位角α小于方位角限定值θ_Bound；如l₂＜l₁，表明掘进机的当前前行方向相对于巷道中心线A偏向固定有第三测距传感器的一侧偏离，则控制掘进机固定有第三测距传感器一侧的驱动轮正转前进，另一侧驱动轮同速反转后退，直至方位角α小于方位角限定值θ_Bound；

中心偏距纠偏步骤，判断计算步骤获得的中心偏距a与掘进机中心偏距允许值L_ε的关系，如中心偏距a小于中心偏距允许值L_ε，直接进行方位角纠偏步骤；反之，判断掘进机方位角α是否为零、以及掘进机中心偏距a的偏置方向-即掘进机机身中心点相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反转后退实现纠偏，如方位角α为零，则掘进机处于巷道中心线A一侧的驱动轮反转速度小于掘进机偏离方向一侧的驱动轮反转速度；如方位角α非零，则掘进机两侧驱动轮同速反转后退，直至经测量步骤、计算步骤确定的中心偏距a小于中心偏距允许值L_ε；

方位角纠偏步骤，判断计算步骤获得的方位角α与掘进机方位角允许值θ_ε0的关系，如方位角α小于方位角允许值θ_ε0，则掘进机自动纠偏完成；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮相反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量步骤、计算步骤确定的方位角α小于方位角允许值θ_ε0；

其中，所述掘进机方位角限定值θ_Bound的设定应保证执行保护性方位角纠偏步骤后的掘进机在执行中心偏距纠偏时，能有效避免与巷道煤帮发生碰撞；所述中心偏距允许值L_ε、方位角允许值θ_ε0的设定应保证掘进机正常掘进的进行。

2.一种掘进机自动纠偏系统，其特征在于：所述掘进机上设置有三个与掘进机控制系统中机载可编程控制器PLC连接的测距传感器，其中两个测距传感器固定于掘进机同一侧，两个测距传感器的安装位置分别位于掘进机导向轮和驱动轮的轮轴正上方，且两个测距传感器的安装位置所在直线与掘进机在前行方向上的中心线平行，第三个测距传感器单独固定于掘进机另一侧，其安装位置以掘进机在前行方向上的中心线为对称轴与同侧固定的两测距传感器间的中心位置对称，三个测距传感器的测量方向与掘进机在前行方向上的中心线垂直；

所述自动纠偏系统包括如下模块：

测量模块，触发三个测距传感器分别测量自身与巷道煤帮的距离，即掘进机侧帮固定测距传感器的安装位置在测距传感器的测量方向上距离巷道煤帮的距离；并标定固定于掘进机同侧的两个测距传感器中：导向轮轮轴上方测距传感器的测量结果表示为l₁、驱动轮轮轴上方测距传感器的测量结果表示为l₂，单独固定于掘进机另一侧的测距传感器的测量结果表示为l₃；

计算模块，根据测量模块的测量结果，经数学计算，获得掘进机当前前行方向相对于巷道中心线A偏离的方位角α、掘进机机身中心点O₂相对于巷道中心线A偏离的中心偏距a；

保护性方位角纠偏模块，判断计算模块获得的方位角α与掘进机方位角限定值θ_Bound的关系，如方位角α小于方位角限定值θ_Bound，直接由中心偏距纠偏模块控制实现掘进机中心偏距纠偏；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮相反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量模块、计算模块确定的方位角α小于方位角限定值θ_Bound；

中心偏距纠偏模块，判断计算模块获得的中心偏距a与掘进机中心偏距允许值L_ε的关系，如中心偏距a小于中心偏距允许值L_ε，直接由方位角纠偏模块控制实现掘进机方位角纠偏；反之，判断掘进机方位角α是否为零、以及掘进机中心偏距a的偏置方向-即掘进机机身中心点相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮反转后退实现纠偏，如方位角α为零，则掘进机处于巷道中心线A一侧的驱动轮反转速度小于掘进机偏离方向一侧的驱动轮反转速度；如方位角α非零，则掘进机两侧驱动轮同速反转后退，直至经测量模块、计算模块确定的中心偏距a小于中心偏距允许值L_ε；

方位角纠偏模块，判断计算模块获得的方位角α与掘进机方位角允许值θ_ε0的关系，如方位角α小于方位角允许值θ_ε0，则掘进机自动纠偏完成；反之，判断掘进机方位角α的偏置方向-即掘进机当前前行方向相对于巷道中心线A的偏离方向，控制掘进机两侧驱动轮相反方向、同速运转实现纠偏，其中，掘进机上处于偏离方向一侧的驱动轮正转前进，另一侧驱动轮反转后退，直至经测量模块、计算模块确定的方位角α小于方位角允许值θ_ε0；

其中，所述掘进机方位角限定值θ_Bound的设定应保证执行保护性方位角纠偏后的掘进机在执行中心偏距纠偏时，能有效避免与巷道煤帮发生碰撞；所述中心偏距允许值L_ε、方位角允许值θ_ε0的设定应保证掘进机正常掘进的进行。

Images