一种基于惯导与里程计组合的综采工作面液压支架自动调直方法与流程

本发明属于煤矿开采技术领域，具体涉及一种基于惯导与里程计组合的综采工作面液压支架自动调直方法。

技术背景

随着我国经济的快速发展，能源的需求量逐年提升，近些年提倡清洁能源发展，但是一次性能源仍然占据主要地位，我国的能源结构是多煤、贫油、少气，以煤为主的能源结构在长时间内难以改变，因此，煤炭工业能否健康稳定发展是国家能源安全与经济可持续发展的关键问题之一。煤炭产业的健康稳定对当前经济持续发展具有重要的意义，保障煤矿开采的安全稳定，合理提高开采效率，大力发展煤炭行业自动化与智能化，从而实现煤矿井下的无人化或少人化。综采工作面“三机”即采煤机、刮板输送机和液压支架的协同作业是实现煤炭资源安全高效开采的关键技术。

根据《煤矿安全规程》的规定，综合机械化设备进行采煤时，保证“三直一平两畅通”，三直包括工作面煤壁、刮板输送机和液压支架必须保持直线，一平指顶板平，现在有些也要求底板要平，两畅通是指工作面两端要畅通。随着煤炭行业自动化和智能化深度融合，综采工作面的“两化”水平不断提高，在条件较好的矿区，工作面采煤机采用自动割煤技术进行自动控制，液压支架跟随采煤机实现位置自动控制。采煤机连续开采，液压支架每次推进都会产生少量误差，连续截割过程会不断累积，使得工作面的液压支架推移不齐，综采工作面的直线度无法保证，需要人工调整液压支架来保证采煤过程正常运行，制约了综采工作面的自动化开采。因此，工作面连续推进过程中刮板输送机直线度精确检测问题已成为综采工作面智能化开采的技术瓶颈。

在这种工程背景下，为了能够适应井下工作面复杂的采矿环境，实现采煤机自动截割和液压支架跟机自动化，对综采工作面的直线度，特别是刮板输送机的直线度检测方法提出了更高的要求。因此，设计一种方法步骤简单、设计合理的煤矿综采工作面直线度测量方法。

技术实现要素：

针对上述现有技术中的不足，本发明提供了一种基于惯导与里程计组合的综采工作面液压支架自动调直方法，步骤简单、设计合理且准确度高，可以实现对煤矿综采工作面的直线度准确测量，适应于采煤环境恶劣、空间有限、工作面粉尘大的煤矿综采工作面。

本发明采用的技术方案：

一种基于惯导与里程计组合的综采工作面液压支架自动调直方法，该方法以高精度的光纤惯导与里程计为测量系统，运用卡尔曼滤波方法对惯导与里程计信息进行融合，获得采煤机的位姿与位置信息，然后根据采煤机齿轨轮的运行轨迹，推算出刮板输送机的直线度，确定每个液压支架的推移量，从而实现液压支架自动调直；具体的，包括如下步骤：

s1、完成光纤惯导系统、里程计的安装，并记录光纤惯导系统与采煤机两齿轨轮的距离与高度以及齿轨轮行走里程，其中，光纤惯导系统安装在采煤机固定位置，里程计安装在采煤机齿轨轮处，与齿轨轮同步运行；

s2、解算惯导与里程计数据；

s201、构建光纤惯导系统的导航坐标系obxnynzn和采煤机坐标系obxbybzb，其中，导航坐标系obxnynzn的原点为采煤机重心，xn指向东向，yn北向和zn天向；采煤机坐标系obxbybzb中，xb沿采煤机横轴向右，yb沿采煤机纵轴向前，zb沿采煤机竖直向上；设采煤机的航向角俯仰角θ，横滚角γ；

s202、采集光纤惯导数据，得到采煤机的运行角速度与加速度信息，通过四元数法解算惯导姿态、速度和位置信息；

s203、采集里程计数据，里程计输出的位置增量在采煤机坐标系下投影为：

式中，αθ为采煤机坐标系相对于里程计坐标系的航向、俯仰安装角，i为输出轴到齿轨轮传动比，n为里程计旋转一周产生的脉冲数目，n(t)为单位时间t内产生的脉冲数目，行走轮的分度圆半径为r；

只考虑标度因数误差δkd、安装误差角δαθ，则里程计实际输出的采煤机坐标系下分量可简化为：

式中，wd为随即噪声干扰；

s3、应用卡尔曼滤波算法实现光纤惯导数据与里程计数据的融合处理，获得采煤机的位置与姿态；

s301、建立位置观测模式下的系统模型，利用单位时间内惯导和航位推算位置增量之差作为量测，将航位推算误差扩充到状态中，该组合导航状态变量为18维：

式中，δvⁿ为速度误差，为姿态误差，δp为位置误差，ε^b为陀螺仪零漂，为加速度计零漂，δkd为里程计标度因数误差，δαθ，为俯仰、航向安装误差角；

kalman滤波器系统方程为：

式中，f(t)为系统矩阵，w(t)为系统噪声，g(t)为系统噪声转移矩阵；

其中，里程计刻度因数误差、安装误差角均可视为随即常数，对应的系统矩阵为：

式中，f1为惯导系统误差方程对应的转移矩阵；

惯导位置更新需要计算每个周期导航坐标系下的位置增量，位置增量的计算公式为：

式中，vi^n′和分别为ti时刻和ti-1时刻纯惯导解算得到的采煤机坐标系速度，tm为更新周期；

由里程计输出的航位推算位置更新算法公式为：

式中为ti-1时刻惯导系统解算的姿态矩阵；

由于惯导位置解算使用微分计算，随着时间的增加误差累积增大，因此每累积1s，导航坐标系下惯导位置增量和航位推算位置增量之差作为量测值：

式中，k为1s时间内位置更新次数；

在采煤机运行过程中惯导解算的真实位置和里程计航位推算的位置相同，根据公式进一步整理，即可得到观测矩阵h(t)；

s302、根据上述的状态方程和量测方程，得到标准的卡尔曼滤波过程，把k-1时刻的导航参数作为系统输入，解算得到k时刻下的转换矩阵速度与位置校正后的导航参数作为组合导航的输出，得出采煤机的姿态与位置信息；其中时间传播方程为：

f(k|k-1)＝f(k，k-1)p(k-1)×f^t(k，k-1)+q(k-1)

量测更新方程为：

k(k)＝p(k|k-1)h^t(k)[h(k)×p(k|k-1)h^t(k)+r(k)]^-1

p(k)＝[i-k(k)h(k)]p(k|k-1)

卡尔曼滤波更新后对相关导航参数进行修正，再将作为输入进行下一次滤波；

对于确定的滤波方程，选取合适的初始值x0和均方误差初值p0分别进行滤波回路与增益回路的时间更新与量测更新，利用递推的算法，随着滤波次数的增加，逐步提高系统的误差补偿精度；

s4、根据融合后的姿态与位置信息，计算采煤机的运行轨迹；

s401、光纤惯性系统安装在采煤机机面的任一位置o点，行走轮的中心位置向采煤机机面垂直投影点分别为m点和n点，定位点o点到m、n两点的距离分别为lm和ln，采煤机定位点o点到刮板输送机齿轨的垂直距离为lh，在导航坐标系下，采煤机o点的坐标为po(xo，yo，zo)，其姿态角分别为(θ，γ)，此时，获取采煤机前后两齿轨轮的运动轨迹，即可获得刮板输送机的完整直线度；

s402、为获取采煤机前后两齿轨轮的运行轨迹，需要对惯导与里程计组合的定位点进行空间坐标系转换；

s4021、根据空间转换公式：

获取导航坐标系下所述交点m、n在t时刻的位置坐标pm(t)、pn(t)，其中，组合定位点在t时刻的位置为po(t)；lq为采煤机两齿轨轮的中心距，lm为所述光纤惯导测量模块到所述交点m的距离；

在t时刻下b、d点的坐标转换公式为：

根据bb0＝dd0＝lh，可以把bd两点坐标转换成b0，d0两点坐标，即为采煤机两齿轨轮坐标，因此，t时刻的位置坐标的位置坐标为：

其中，lh为所述光纤惯导测量模块的安装点与所对应刮板输送机水平面上投影点的垂线长度；

s5、解算采煤机运行轨迹，推算刮板输送机的直线度；

s501、工作面中的采煤机在其往复割煤过程中，采煤机机身位置的长度横跨多节刮板输送机，包含了多节齿轨，需要对采煤机机身轨迹进行分析，因此，需要对bo、d0进行轨迹融合；

s502、在采煤机从左向右运行过程当中，bo点在y方向的坐标最大值为bn，取bobn的轨迹线为lb，d0点的行走轨迹中，取出其中在y轴上坐标最大值为dn，得到图中的bndn段，当d的第x个点大于bn点时，lbd就是采煤机的机身横跨的齿轨曲线，其中

将bo的轨迹线为lb与d0的轨迹线ld融合，就可以得到完整的刮板输送机的齿轨轨迹l＝lb+lbd，通过以上方法即可得到完整的刮板输送机直线度；

s6、液压支架推移量的计算和调直，基于刮板输送机的直线度，计算出每个液压支架的偏移量，通过计算机控制液压支架进行调直，保证在300m工作面的直线度检测误差≤10cm；

s601、根据得到刮板输送机机身的形态曲线，对其进行调直，解算出每节刮板输送机中部槽的位置及所需调整的位移，即约束点处的坐标信息；刮板输送机直线度控制过程可以分为目标曲线为直线和目标曲线为斜线两类，以目标曲线是直线为例，采煤机起始位置为x0，对应的液压支架编号为in，支架约束点的位置i可以通过下式得到：

i＝int[(xn-x0)/l]

其中，int为取整函数，xn为采煤机位置；x0为采煤机起始点位置到第一个通过完整刮板的距离，如果采煤机起始位置为液压支架之间的边界点，则x0＝0；l为每个刮板机的宽度；第n节刮板输送机齿轨的坐标位置

根据刮板输送机各节齿轨的理论调整位移公式计算出各节刮板的实际调整位移，对综采工作面的直线度进行调整；式中，δy为理论调整位置，yi为理想曲线位置。

本实施例中，通过获取采煤机两个齿轨轮的位置推算出刮板输送机的完整直线度，直线度检测误差小于等于10cm。

本实施例中，根据获取的刮板输送机的直线度，推算出每一个液压支架的推移量，上位机控制系统根据液压支架推移量进行调直控制。

本发明与现有技术相比具有的优点：

1)现有的设计中有采用惯导系统在综采工作面中进行测量直线度的方法，但是纯惯导系统存在漂移现象，测量结果不能达到综采工作面的测量与控制水平。因此，本设计将惯导与里程计应用闭环卡尔曼滤波的方法，提高了采煤机的定位精度，稳定性较好。

2)现有的采煤机定位技术，不能完整的体现出刮板输送机的直线度，本设计应用空间坐标系转换模型，对采煤机两个齿轨轮的轨迹融合，获得了完整的刮板输送机直线度。

3)现有的综采工作面直线度检测技术中，只对采煤机的运行轨迹进行定位，没有确定到每个液压支架需要调整的位移。本设计通过刮板输送机直线度确定液压支架的位置，并提供液压支架调整位移，提高了自动化调直水平。

附图说明

图1为本发明实施例一种基于惯导与里程计组合的综采工作面液压支架自动调直方法的流程图

图2为本发明实施例中光纤惯导系统与里程计的安装方式示意图。

图3为本发明实施例中的组合惯导框图。

图4为本发明实施例中的采煤机空间状态图。

图5为本发明实施例中的采煤机运行图。

图6为本发明实施例中的刮板输送机形态曲线示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

如图1所示，本发明实施例提供了一种基于惯导与里程计组合的综采工作面液压支架自动调直方法，包括如下步骤：

s1、完成光纤惯导系统、里程计的安装，并记录光纤惯导系统与采煤机两齿轨轮的距离与高度以及齿轨轮行走里程，其中，如图2所示，光纤惯导系统安装在采煤机固定位置，里程计安装在采煤机齿轨轮处，与齿轨轮同步运行；

s2、解算惯导与里程计数据；

s201、构建光纤惯导系统的导航坐标系obxnynzn和采煤机坐标系obxbybzb，其中，导航坐标系obxnynzb的原点为采煤机重心，xn指向东向，yn北向和zn天向；采煤机坐标系obxbybzb中，xb沿采煤机横轴向右，yb沿采煤机纵轴向前，zb沿采煤机竖直向上；设采煤机的航向角俯仰角θ，横滚角γ；

s202、采集光纤惯导数据，得到采煤机的运行角速度与加速度信息，通过四元数法解算惯导姿态、速度和位置信息；

s203、采集里程计数据，里程计输出的位置增量在采煤机坐标系下投影为：

式中，αθ为采煤机坐标系相对于里程计坐标系的航向、俯仰安装角，i为输出轴到齿轨轮传动比，n为里程计旋转一周产生的脉冲数目，n(t)为单位时间t内产生的脉冲数目，行走轮的分度圆半径为r；

只考虑标度因数误差δkd、安装误差角δαθ，则里程计实际输出的采煤机坐标系下分量可简化为：

式中，wd为随即噪声干扰；

s3、应用卡尔曼滤波算法实现光纤惯导数据与里程计数据的融合处理，获得采煤机的位置与姿态；

s301、建立位置观测模式下的系统模型，利用单位时间内惯导和航位推算位置增量之差作为量测，将航位推算误差扩充到状态中，该组合导航状态变量为18维：

式中，δvⁿ为速度误差，为姿态误差，δp为位置误差，ε^b为陀螺仪零漂，为加速度计零漂，δkd为里程计标度因数误差，δαθ，为俯仰、航向安装误差角；

kalman滤波器系统方程为：

式中，f(t)为系统矩阵，w(t)为系统噪声，g(t)为系统噪声转移矩阵；

其中，里程计刻度因数误差、安装误差角均可视为随即常数，对应的系统矩阵为：

式中，f1为惯导系统误差方程对应的转移矩阵；

惯导位置更新需要计算每个周期导航坐标系下的位置增量，位置增量的计算公式为：

式中，vi^n′和分别为ti时刻和ti-1时刻纯惯导解算得到的采煤机坐标系速度，tm为更新周期。

由里程计输出的航位推算位置更新算法公式为：

式中为ti-1时刻惯导系统解算的姿态矩阵；

由于惯导位置解算使用微分计算，随着时间的增加误差累积增大，因此每累积1s，导航坐标系下惯导位置增量和航位推算位置增量之差作为量测值：

式中，k为1s时间内位置更新次数；

在采煤机运行过程中惯导解算的真实位置和里程计航位推算的位置相同，根据公式进一步整理，即可得到观测矩阵h(t)；

s302、根据上述的状态方程和量测方程，得到标准的卡尔曼滤波过程，把k-1时刻的导航参数作为系统输入，解算得到k时刻下的转换矩阵速度与位置工作原理如3图所示，校正后的导航参数作为组合导航的输出，得出采煤机的姿态与位置信息；其中时间传播方程为：

f(k|k-1)＝f(k，k-1)p(k-1)×f^t(k，k-1)+q(k-1)

量测更新方程为：

k(k)＝p(k|k-1)h^t(k)[h(k)×p(k|k-1)h^t(k)+r(k)]^-1

p(k)＝[i-k(k)h(k)]p(k|k-1)

卡尔曼滤波更新后对相关导航参数进行修正，再将作为输入进行下一次滤波；

对于确定的滤波方程，选取合适的初始值x0和均方误差初值p0分别进行滤波回路与增益回路的时间更新与量测更新，利用递推的算法，随着滤波次数的增加，逐步提高系统的误差补偿精度；

s4、根据融合后的姿态与位置信息，计算采煤机的运行轨迹；

s401、如图4所示，光纤惯性系统安装在采煤机机面的任一位置o点，行走轮的中心位置向采煤机机面垂直投影点分别为m点和n点，定位点o点到m、n两点的距离分别为lm和ln，采煤机定位点o点到刮板输送机齿轨的垂直距离为lh，在导航坐标系下，采煤机o点的坐标为po(xo，yo，zo)，其姿态角分别为(θ，γ)，此时，获取采煤机前后两齿轨轮的运动轨迹，即可获得刮板输送机的完整直线度；

s402、为获取采煤机前后两齿轨轮的运行轨迹，需要对惯导与里程计组合的定位点进行空间坐标系转换；

s4021、根据空间转换公式：

获取导航坐标系下所述交点m、n在t时刻的位置坐标pm(t)、pn(t)，其中，组合定位点在t时刻的位置为po(t)；lq为采煤机两齿轨轮的中心距，lm为所述光纤惯导测量模块到所述交点m的距离；

在t时刻下b、d点的坐标转换公式为：

根据bb0＝dd0＝lh，可以把bd两点坐标转换成b0，d0两点坐标，即为采煤机两齿轨轮坐标，因此，t时刻的位置坐标的位置坐标为：

其中，lh为所述光纤惯导测量模块的安装点与所对应刮板输送机水平面上投影点的垂线长度；

s5、解算采煤机运行轨迹，推算刮板输送机的直线度；

s501、工作面中的采煤机在其往复割煤过程中，采煤机机身位置的长度横跨多节刮板输送机，包含了多节齿轨，需要对采煤机机身轨迹进行分析，因此，需要对bo、d0进行轨迹融合，如图5所示；

s502、在采煤机从左向右运行过程当中，bo点在y方向的坐标最大值为bn，取bobn的轨迹线为lb，d0点的行走轨迹中，取出其中在y轴上坐标最大值为dn，得到图中的bndn段，当d的第x个点大于bn点时，lbd就是采煤机的机身横跨的齿轨曲线，其中

将bo的轨迹线为lb与d0的轨迹线ld融合，就可以得到完整的刮板输送机的齿轨轨迹l＝lb+lbd，通过以上方法即可得到完整的刮板输送机直线度；

s6、液压支架推移量的计算和调直，基于刮板输送机的直线度，计算出每个液压支架的偏移量，通过计算机控制液压支架进行调直，保证在300m工作面的直线度检测误差≤10cm；

s601、根据得到刮板输送机机身的形态曲线，对其进行调直，解算出每节刮板输送机中部槽的位置及所需调整的位移，即约束点处的坐标信息；如图6所示，刮板输送机直线度控制过程可以分为目标曲线为直线和目标曲线为斜线两类，以目标曲线是直线为例，采煤机起始位置为x0，对应的液压支架编号为in，支架约束点的位置i可以通过下式得到：

i＝int[(xn-x0)/l]

其中，int为取整函数，xn为采煤机位置；x0为采煤机起始点位置到第一个通过完整刮板的距离，如果采煤机起始位置为液压支架之间的边界点，则x0＝0；l为每个刮板机的宽度；第n节刮板输送机齿轨的坐标位置

根据刮板输送机各节齿轨的理论调整位移公式计算出各节刮板的实际调整位移，对综采工作面的直线度进行调整；式中，δy为理论调整位置，yi为理想曲线位置。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。