CN106014466A - 一种用于二柱式液压支架位姿检测与控制的方法 - Google Patents

Abstract

本发明涉及一种用于二柱式液压支架位姿检测与控制的方法，该方法借助由控制器、数据采集装置、第一加速度传感器、第二加速度传感器、行程传感器、激光测距仪、第一电磁比例阀和第二电磁比例阀组成的检控系统，利用加速度传感器、行程传感器、激光测距仪测量的数据传输给控制器，控制器根据最优动作规则表优选出最优动作方案，并采用PID控制规则对液压支架位姿进行实时全姿态控制，通过控制器控制电磁比例阀的开关和流量大小，以此调节实现液压支架升架过程的快速水平升、降架以及工作面顶板倾斜时的智能贴顶，提升了液压支架的升、移架速度及跟机速度；该系统同时具备位姿检测与控制系统，功能更多，性能更好，具有不可比拟的优势。

Description

一种用于二柱式液压支架位姿检测与控制的方法

技术领域

本发明涉及一种用于二柱式液压支架位姿检测与控制的方法，属于液压支架技术领域。

背景技术

在综采工作面为了防止煤层顶板冒落，保证工作人员的安全和生产的正常进行，必须对顶板进行支护，液压支架是煤矿综采工作面必备的支护设备。我国许多煤矿都建立了综采自动化生产面，并配备了综采装备远程监控功能。对液压支架进行实时可靠、形象直观的远程监控，不但可以及时掌握液压支架的准确运行状态，还可以预测液压支架支护质量，而且可以根据检查结果调整支架的运行参数，提高工作面支护稳定性。

液压支架典型的工作循环包括升柱、支撑、降柱三个阶段，在支架升柱阶段，我们希望液压支架能自动快速水平的上升，从而迅速接触顶板进入支撑阶段，减小支架移架期间的沉降；支撑阶段，我们希望液压支架能自动适应、贴合工作面倾斜顶板，从而保证支架与顶板接触良好，增强支架对顶板的控制力，防止支架出现低头、高射炮等支护失稳现象；降柱阶段我们希望支架可以快速水平的降柱，从而实现快速推溜、拉架，加快支架的跟机速度。目前针对液压支架的位姿检测与调整，国内众多学者展开了不同的研究。

例如，中国专利文献CN103899338A公开了一种基于空间坐标变换的液压支架工作姿态确定方法，该方法通过实时测量支架构件变化的倾角值，从而获得四连杆液压支架的工作姿态，并实时测量液压支架的底座、连杆和顶梁相对于参数坐标系的倾斜角度，利用上述三个角度值以及支架本身的几何尺寸，采用建立参考坐标系和相对坐标系的方法计算出支架当前各构件的位置、角度，以及驱动油缸的长度。

公开号为CN103968856A的中国专利公开了一种液压支架位姿的实时检测方法。该方法使用三轴加速度传感器和三轴陀螺仪实时测量支架顶梁的位置、倾角的变化量，通过积分的方式获得液压支架任意时刻的位姿。

中国专利文献为CN103899344B的中国专利公开了一种液压支架顶梁自适应调平方法，该方法通过传感器检测顶梁倾角利用盲动试探方式对支架顶梁状态进行调整，该专利从支架初撑阶段开始控制，具有一定的局限性，且该方案采用的盲动试探法不仅试探比较过程耗费大量的时间，且极易出现支架在调整过程中步距不合适而造成支架持续振荡位姿，降低了支架支护工作的稳定性，调节精度较差。

虽然目前针对液压支架位姿的检测方法有很多种，但是这些方法仅仅解决了如何获取液压支架位姿数据，且检测数据较多，受环境因素影响较大，检测精度较差；在控制方法上，现有的支架位姿控制方法往往采用盲动试探方法对支架的位姿进行控制，控制过程繁琐且精度低，支架位姿调整时易于出现振荡，无法快速、精准的实现液压支架在升柱、支撑、降柱阶段的位姿调节，因此，有必要设计一种高效率、精度调节液压支架位姿的控制方法。

发明内容

针对现有技术的不足，本发明提供一种用于二柱式液压支架位姿检测与控制的方法。

本发明的技术方案如下：

一种用于二柱式液压支架位姿检测与控制的方法，所述方法包括主要由控制器、数据采集装置、第一加速度传感器、第二加速度传感器、行程传感器、激光测距仪、第一电磁比例阀和第二电磁比例阀组成的检控系统，所述第一加速度传感器设置在液压支架的底座上，第二加速度传感器设置在液压支架的后连杆上，行程传感器设置在液压支架的平衡千斤顶内部，激光测距仪设置在液压支架的立柱上，第一电磁比例阀设置在液压支架平衡千斤顶液压控制回路上，第二电磁比例阀设置在液压支架立柱液压控制回路上，所述第一加速度传感器、第二加速度传感器、行程传感器、激光测距仪均与数据采集装置连接，数据采集装置与控制器连接，控制器还连接第一电磁比例阀和第二电磁比例阀，所述方法包括以下步骤，

(1)信号采集：将第一加速度传感器测量的底座加速度、第二加速度传感器测量的后连杆加速度、行程传感器测取的平衡千斤顶长度、激光测距仪测取的立柱长度传输给数据采集装置，数据采集装置对接收的信号进行滤波处理后得到加速度、长度原始数据，并将加速度、长度原始数据传输给控制器；

(2)数据处理：控制器根据接收到的加速度、长度原始数据，计算出液压支架底座的实时倾角、后连杆与底座的夹角以及液压支架实时行为位姿，并输出液压支架的实时行为位姿；然后，控制器根据液压支架操作人员动作指令及工作面顶板倾角，计算出液压支架所要达到的最终位姿，并对比液压支架当前行为位姿得出位移差，再根据最优动作规则表，以液压支架动作速度最快、用液量最省原则智能判定出液压支架最优动作方案，控制器根据位移差及设定的步进次数，计算出液压支架阶段步进值，并将阶段步进值设定为控制器的阶段目标参数值；

(3)PID控制运算：控制器将阶段目标参数值与行程传感器和激光测距仪的实时检测数据对比，获取阶段目标参数值与实时检测数据的差值；

(4)指令输出：控制器根据步骤(3)所述的差值，输出第一电磁比例阀、第二电磁比例阀的控制信号，控制信号控制第一电磁比例阀、第二电磁比例阀的运动方向和开口度大小以改变进出平衡千斤顶及立柱的流量，从而改变平衡千斤顶及立柱的长度，实现液压支架的位姿误差反馈控制，使液压支架逐步到达最终位姿。

优选的，步骤(1)中，所述滤波处理是指数据采集装置采用中值滤波法对第一加速度传感器、第二加速度传感器、行程传感器和激光测距仪对传输给数据采集装置的数据进行滤波处理，其具体过程包括：连续采样N个数据，然后用堆序法原理对采集的N个数据进行排序，找出N个数据中的最大值和最小值，去掉最大值和最小值，然后计算剩余的N-2个数据的算术平均值，N取为3-14。

优选的，步骤(2)中，控制器计算出液压支架实时行为位姿，具体包括以下过程：

(a)读取液压支架后连杆第二加速度传感器输出的α_h及底座第一加速度传感器输出的α_d，变换得出液压支架后连杆的绝对倾角β及底座的绝对倾角α：

β＝arcsina_h/g，α＝arcsina_d/g

液压支架后连杆相对底座的倾角：θ₀＝β+α(I)；

(b)当步骤(a)计算出液压支架后连杆相对底座的倾角θ₀后，通过读取液压支架平衡千斤顶和立柱的长度，将平衡千斤顶和立柱的长度带入液压支架的实时行为位姿状态方程(II)，可得出液压支架的实时位姿参数，最后在显示器上输出液压支架当前位姿状态，完成液压支架实时位姿状态监测；

$\left[\begin{array}{c}\mathrm{\ϵ}\\ {\mathrm{\θ}}_0\\ {\mathrm{\θ}}_1\\ {\mathrm{\θ}}_2\\ {\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4\\ {\mathrm{\θ}}_5\\ {\mathrm{\θ}}_6\\ {\mathrm{\θ}}_7\\ {\mathrm{\θ}}_9\\ {\mathrm{\θ}}_{10}\\ {\mathrm{sin\θ}}_{11}\end{array}\right]=\left(\begin{array}{ccccccccccc}-1& 0& 0& 1& 1& 1& 0& 0& 1& 1& 1\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ -1& 0& -1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ -1& 0& 0& 1& 1& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\\ \frac{l_1}{l_z}& 0& 0& 0& 0& 0& \frac{l_5}{l_z}& 0& 0& 0& 0\end{array}\right)\left[\begin{array}{c}{\mathrm{\θ}}_0\\ {\mathrm{\θ}}_1\\ {\mathrm{\θ}}_2\\ {\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4\\ {\mathrm{\θ}}_5\\ {\mathrm{\θ}}_6\\ {\mathrm{\θ}}_7\\ {\mathrm{\θ}}_8\\ {\mathrm{\θ}}_9\\ {\mathrm{\θ}}_{10}\end{array}\right]+\left[\begin{array}{c}-\frac{3}{2}\mathrm{\π}\\ \mathrm{\β}+\mathrm{\α}\\ \mathrm{\π}\\ 0\\ \arccos \frac{l_1^2+z^2-l_2^2}{2l_1\·z}\\ \arccos \frac{l_4^2+z^2-l_3^2}{2l_4\·z}\\ 0\\ 0\\ 0\\ \arccos \frac{l_6^2+l_7^2-l_q^2}{2l_6\·l_7}\\ 0\\ \frac{z_2}{l_z}\end{array}\right]---\left(II\right)$

其中z₂＝h₁-h₃+h₄cosε-h₆cosε+l₈sinε，ε为液压支架顶梁相对底座的倾角，θ₆为液压支架掩护梁与底座的相对倾角，θ₉为液压支架平衡千斤顶与顶梁、掩护梁铰接点形成的张角，θ₁₁为立柱倾角；

优选的，步骤(2)中，所述最优动作方案的确定过程如下：

(x)以工作面顶板倾角η、液压支架操作人员对液压支架的实际控制指令及求解的液压支架实时位姿参数为基础，基于平衡千斤顶长度、立柱长度与液压支架工作高度的变形协调方程(III)和平衡千斤顶长度、立柱长度与液压支架顶梁位姿角ε的变形协调方程(IV)求解液压支架目标位置位姿参数；

$\left\{\begin{array}{c}{l}_{z~H}=\frac{l_5\cos \arcsin \[(H-h_1-h_4-l_1{\mathrm{sin\θ}}_0)/l_5\]+l_8-l_1{\mathrm{cos\θ}}_0-l_9}{\cos \arcsin \[(H-h_6-h_3)/l_z\]}\\ l_{q~H}={\[l_6^2+{l_7}^2-2l_6\·l_7\·\cos (\frac{3}{2}\mathrm{\π}-\arcsin \[(H-h-h_4-l_1{\mathrm{sin\θ}}_0)/l_5\]-{\mathrm{\θ}}_8-{\mathrm{\θ}}_{10})\]}^{1/2}\end{array}\right.---\left(III\right)$

$\left\{\begin{array}{c}{l}_{z~\mathrm{\ϵ}}={(z_3^2+z_4^2)}^{1/2}\\ l_{q~\mathrm{\ϵ}}={\[l_6^2+{l_7}^2-2l_6\·l_{7}cos(\mathrm{\ϵ}+{\mathrm{\θ}}_0-{\mathrm{\θ}}_3-{\mathrm{\θ}}_4-{\mathrm{\θ}}_5-{\mathrm{\θ}}_8-{\mathrm{\θ}}_{10}+\frac{3}{2}\mathrm{\π})\]}^{1/2}\end{array}\right.---\left(IV\right)$

其中，

$z_3=h_1+l_1{\mathrm{sin\θ}}_0+l_5\sin \[\mathrm{\ϵ}-({\mathrm{\θ}}_8+{\mathrm{\θ}}_{10}+\arccos \frac{l_6^2+l_7^2-l_q^2}{2\·l_6\·l_7}-\frac{3}{2}\mathrm{\π})\]+h_4\cos \mathrm{\ϵ}+l_8\sin \mathrm{\ϵ}-h_3-h_6\cos \mathrm{\ϵ},$

$z_4=l_{5}cos\[\mathrm{\ϵ}-({\mathrm{\θ}}_8+{\mathrm{\θ}}_{10}+\arccos \frac{l_6^2+l_7^2-l_q^2}{2\·l_6\·l_7}-\frac{3}{2}\mathrm{\π})\]-h_{4}sin\mathrm{\ϵ}+l_{8}cos\mathrm{\ϵ}+h_{6}sin\mathrm{\ϵ}-l_1{\mathrm{cos\θ}}_0-l_9.$

(y)将(x)中求解的液压支架目标位置位姿参数，与液压支架当前位姿参数比较，以液压支架位姿调整速度最快、用液量最省为原则从液压支架最优动作规则表中智能决策出平衡千斤顶和立柱的最优动作方案；液压支架最优动作规则表如表一所示：

表一：液压支架最优动作规则表

其中

优选的，步骤(4)中，控制器比较输出的最终位姿的信号l_q0和l_z0以及液压支架实时监测信号l_q和l_z，根据液压支架最优动作方案确定PID控制量的阶段目标额定值l′_q0和l′_z0，并将数据传递给控制器，通过比较阶段目标额定值l′_q0和l′_z0与液压支架实时监测信号l_q和l_z，得出目标位姿参数与当前位姿参数的差值；控制器根据差值，控制平衡千斤顶和立柱连接的电磁比例阀的运动方向和开口度大小以改变进出立柱及平衡千斤顶的流量，从而改变液压支架立柱及平衡千斤顶的长度，在改变液压支架立柱及平衡千斤顶的长度过程中，立柱及平衡千斤顶的具体动作如表二所示，

表二：立柱及平衡千斤顶的具体动作表

优选的，所述使用方法还包括输入预设定参数及预设定参数处理过程，在液压支架下井前，通过键盘和显示器进行液压支架初始参数设定，包括液压支架后连杆长度l₁和定位尺寸高度h₁，液压支架前连杆的长度l₃和定位尺寸高度h₂，底座与连杆铰接点距离l₂及其连接线与底座的夹角θ₂，，立柱下柱窝定位尺寸h₃和l₉及上柱窝定位尺寸h₆和l₈，掩护梁与连杆铰接点连线的距离l₄以及其与掩护梁的夹角θ₅，掩护梁长度l₅及上定位尺寸h₅，平衡千斤顶下定位尺寸l₆、θ₈及上定位尺寸l₇和θ₁₀，掩护梁与顶梁铰接点到顶梁最上方的距离h₄。

优选的，所述检控系统还包括输入装置，输入装置与控制器连接，所述输入装置选用显示器和键盘，或选用触摸屏显示/输入模块。

优选的，所述控制器选用MSP430F5438A单片机。

优选的，所述第一加速度传感器、第二加速度传感器均选用飞思卡尔MMA7361LC加速度传感器。

优选的，所述行程传感器选用米朗MIRAN MTL3-2000mm油缸内置磁致伸缩位移传感器。

优选的，所述激光测距仪选用康力华KLH-01T-20hz激光测距仪。

优选的，所述触摸屏显示/输入模块选用迪文DMT80480T070_06WT+触摸屏。

优选的，所述第一电磁比例阀选用型号为4WRZe32W9-520-7X/6EG24N9ETK4F1/M的三位四通比例换向阀。

优选的，所述第二电磁比例阀选用型号为4WRZe52W9-1000-7X/6EG24N9ETK4F1/M的三位四通比例换向阀。

本发明的有益效果在于：

1.本发明液压支架位姿检测方法，有别于传统的液压支架位姿检测方法，通过检测底座倾角、液压支架后连杆加速度、驱动件立柱和辅助位姿平衡千斤顶4个参数，即可实现工作面液压支架的实时位姿检测，该方法利用液压支架固有的变形协调方程求解，求解结果精确。

2.本发明液压支架位姿控制方法，采用PID控制规则对液压支架位姿进行实时全姿态控制，实现液压支架升架过程的快速水平升、降架以及工作面顶板倾斜时的智能贴顶，提升了液压支架的升、移架速度及跟机速度；且PID控制系统对系统模型依赖少，适应性强，具有较强的鲁棒性。

3.本发明液压支架位姿检控系统，安装使用方便，可移植性强，亦可用于两柱掩护式、两柱放顶煤等支架，借助本发明的检控方法可以有效实现液压支架的智能位姿的检测与控制，且作用明显，效果显著，具有良好的经济效益和社会效益。

附图说明

图1为本发明中检控系统在液压支架上的安装结构图；

图2为本发明中检控系统各组成部分的连接关系图；

图3为实施例1中液压支架的力学关系图Ⅰ；

图4为实施例1中液压支架的力学关系图Ⅱ；

图5为实施例1中液压支架的力学关系图III；

图6为本发明位姿检测与控制方法的工作流程图；

其中：1、第一加速度传感器；2、激光测距仪；3、底座；4、第二加速度传感器；5、前连杆；6、掩护梁；7、第一电磁比例阀；8、行程传感器；9、平衡千斤顶；10、立柱；11、顶梁；12、第二电磁比例阀；13、控制器；14、上位机；15、显示器；16、键盘；17、后连杆。

α_d：底座横向加速度(平行底座方向)；

α_h：后连杆横向加速度(平行后连杆方向)；

g:重力加速度。

l₁：液压支架后连杆长度；

l₂：底座与连杆铰接点距离；

l₃：液压支架前连杆的长度；

l₄：掩护梁与连杆铰接点连线的距离；

l₅：掩护梁长度；

l₆：平衡千斤顶下定位尺寸；

l₇：平衡千斤顶上定位尺寸；

l₈：立柱上柱窝定位尺寸；

l₉：立柱下柱窝定位尺寸；

h₁：液压支架后连杆定位尺寸高度；

h₂：液压支架前连杆定位尺寸高度；

h₃：立柱下柱窝定位尺寸(h₃和l₉都是下柱窝的定位尺寸，一个高度方向一个长度方向)；

h₄：掩护梁与顶梁铰接点到顶梁最上方的距离；

h₅：掩护梁上定位尺寸；

h₆：上柱窝定位尺寸；

H：液压支架顶梁到底座的高度。

θ₀：液压支架后连杆相对底座的倾角；

θ₁：液压支架后连杆与前后连杆下铰点连线夹角；

θ₂：底座与连杆铰接点连接线与底座的夹角；

θ₃：前连杆下铰点与后连杆上铰点连线与后连杆夹角；

θ₄：前连杆下铰点与后连杆上铰点连线与前后连杆上铰点连线夹角；

θ₅：前后连杆上铰点连线与掩护梁夹角；

θ₆：液压支架掩护梁与底座的相对倾角；

θ₇：掩护梁背角；

θ₈：平衡千斤顶下定位角度；

θ₉：液压支架平衡千斤顶与顶梁、掩护梁铰接点形成的张角；

θ₁₀：平衡千斤顶上定位角度；

θ₁₁：立柱倾角；

ε：液压支架顶梁相对底座的倾角。

具体实施方式

下面通过实施例并结合附图对本发明做进一步说明，但不限于此。

实施例1：

如图1、图2所示，一种用于二柱式液压支架位姿检测与控制的方法，该方法包括主要由控制器、数据采集装置、第一加速度传感器、第二加速度传感器、行程传感器、激光测距仪、第一电磁比例阀和第二电磁比例阀组成的检控系统；其中，第一加速度传感器安装在液压支架的底座上，第二加速度传感器安装在液压支架的后连杆上，行程传感器安装在液压支架的平衡千斤顶内部，激光测距仪安装在液压支架的立柱上，第一电磁比例阀连接在液压支架平衡千斤顶液压控制回路上，第二电磁比例阀连接在液压支架立柱液压控制回路上，第一加速度传感器、第二加速度传感器、行程传感器、激光测距仪均与数据采集装置连接，数据采集装置与控制器连接，控制器控制连接第一电磁比例阀和第二电磁比例阀。

该检控系统还包括输入装置，输入装置与控制器连接，所述输入装置选用显示器和键盘，即显示器和键盘与控制器连接。

控制器具体型号选用MSP430F5438a单片机，单片机内置有数据采集装置。第一加速度传感器、第二加速度传感器均选用飞思卡尔MMA7361LC加速度传感器。行程传感器选用米朗MIRAN MTL3-2000mm油缸内置磁致伸缩位移传感器。激光测距仪选用康力华KLH-01T-20hz激光测距仪。第一电磁比例阀、第二电磁比例阀均选用力士乐的三位四通比例换向阀，其中，第一电磁比例阀的型号为4WRZe32W9-520-7X/6EG24N9ETK4F1/M的三位四通比例换向阀，第二电磁比例阀的型号为4WRZe52W9-1000-7X/6EG24N9ETK4F1/M的三位四通比例换向阀。

该方法具体包括信号采集、数据处理、PID控制运算和指令输出四个步骤，其中，

(1)信号采集：将第一加速度传感器测量的底座加速度、第二加速度传感器测量的后连杆加速度、行程传感器测取的液压支架平衡千斤顶的长度、激光测距仪测取的立柱的长度传输给数据采集装置，数据采集装置对接收的信号进行滤波处理得到加速度、长度原始数据，并将加速度、长度原始数据传输给MSP430F5438a单片机；

其中滤波处理是指，数据采集装置连续采样N个数据，然后用堆序法原理对采集的N个数据进行排序，找出N个数据中的最大值和最小值，去掉最大值和最小值，然后计算剩余的N-2个数据的算术平均值，N取为3-14。

另外，事先在单片机内写入有关最优动作规则表的程序和PID控制规则的程序，使该单片机具有PID控制器的功能。

(2)数据处理：单片机根据接收到的加速度、长度原始数据，计算出液压支架底座的实时倾角、后连杆的实时倾角、后连杆与底座的夹角以及液压支架实时行为位姿，并在显示器上输出液压支架的实时行为位姿；然后，单片机根据液压支架操作人员动作指令(通过键盘输入的动作指令)及工作面顶板倾角，计算出液压支架所要达到的最终位姿，并对比液压支架当前行为位姿得出位移差，再根据最优动作规则表，以液压支架动作速度最快、用液量最省原则智能判定出液压支架最优动作方案，单片机根据位移差及操作人员设定的步进次数，计算出液压支架阶段步进值(位移差/步进次数)，并将阶段步进值设定为单片机的阶段目标参数值；

其中，单片机计算液压支架实时行为位姿的具体过程如下：

(a)读取液压支架后连杆第二加速度传感器输出的α_h及底座第一加速度传感器输出的α_d，变换得出液压支架后连杆的倾角β及底座的倾角α：

β＝arcsina_h/g，α＝arcsina_d/g

液压支架后连杆相对底座的倾角：θ₀＝β+α(I)；

(b)当步骤(a)计算出液压支架后连杆相对底座的倾角θ₀后，通过读取液压支架平衡千斤顶和立柱的长度，将平衡千斤顶和立柱的长度带入液压支架实时行为位姿状态方程(II)，可得出液压支架的实时位姿参数(计算得出方程(II)左侧的所有角度，即液压支架的位姿参数)，最后在显示器上显示液压支架当前位姿状态(即在显示器上显示出液压支架位姿参数的所有角度)，完成液压支架的实时位姿状态监测；

$\left[\begin{array}{c}\mathrm{\ϵ}\\ {\mathrm{\θ}}_0\\ {\mathrm{\θ}}_1\\ {\mathrm{\θ}}_2\\ {\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4\\ {\mathrm{\θ}}_5\\ {\mathrm{\θ}}_6\\ {\mathrm{\θ}}_7\\ {\mathrm{\θ}}_9\\ {\mathrm{\θ}}_{10}\\ {\mathrm{sin\θ}}_{11}\end{array}\right]=\left(\begin{array}{ccccccccccc}-1& 0& 0& 1& 1& 1& 0& 0& 1& 1& 1\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ -1& 0& -1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ -1& 0& 0& 1& 1& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\\ \frac{l_1}{l_z}& 0& 0& 0& 0& 0& \frac{l_5}{l_z}& 0& 0& 0& 0\end{array}\right)\left[\begin{array}{c}{\mathrm{\θ}}_0\\ {\mathrm{\θ}}_1\\ {\mathrm{\θ}}_2\\ {\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4\\ {\mathrm{\θ}}_5\\ {\mathrm{\θ}}_6\\ {\mathrm{\θ}}_7\\ {\mathrm{\θ}}_8\\ {\mathrm{\θ}}_9\\ {\mathrm{\θ}}_{10}\end{array}\right]+\left[\begin{array}{c}-\frac{3}{2}\mathrm{\π}\\ \mathrm{\β}+\mathrm{\α}\\ \mathrm{\π}\\ 0\\ \arccos \frac{l_1^2+z^2-l_2^2}{2l_1\·z}\\ \arccos \frac{l_4^2+z^2-l_3^2}{2l_4\·z}\\ 0\\ 0\\ 0\\ \arccos \frac{l_6^2+l_7^2-l_q^2}{2l_6\·l_7}\\ 0\\ \frac{z_2}{l_z}\end{array}\right]---\left(II\right)$

其中z₂＝h₁-h₃+h₄cosε-h₆cosε+l₈sinε，ε为液压支架顶梁相对底座的倾角，θ₆为液压支架掩护梁与底座的相对倾角，θ₉为液压支架平衡千斤顶与顶梁、掩护梁铰接点形成的张角，θ1₁₁为立柱倾角；

所述的最优动作方案的确定过程如下：

(x)以工作面顶板倾角η、液压支架操作人员对液压支架的实际控制指令(通过键盘输入，例如升架指令)及求解的液压支架实时位姿参数为基础，基于平衡千斤顶长度、立柱长度与液压支架工作高度的变形协调方程(III)和平衡千斤顶长度、立柱长度与液压支架顶梁位姿角ε的变形协调方程(IV)求解液压支架目标位置位姿参数；

$\left\{\begin{array}{c}{l}_{z~H}=\frac{l_5\cos \arcsin \[(H-h_1-h_4-l_1{\mathrm{sin\θ}}_0)/l_5\]+l_8-l_1{\mathrm{cos\θ}}_0-l_9}{\cos \arcsin \[(H-h_6-h_3)/l_z\]}\\ l_{q~H}={\[l_6^2+{l_7}^2-2l_6\·l_7\·\cos (\frac{3}{2}\mathrm{\π}-\arcsin \[(H-h-h_4-l_1{\mathrm{sin\θ}}_0)/l_5\]-{\mathrm{\θ}}_8-{\mathrm{\θ}}_{10})\]}^{1/2}\end{array}\right.---\left(III\right)$

$\left\{\begin{array}{c}{l}_{z~\mathrm{\ϵ}}={(z_3^2+z_4^2)}^{1/2}\\ l_{q~\mathrm{\ϵ}}={\[l_6^2+{l_7}^2-2l_6\·l_{7}cos(\mathrm{\ϵ}+{\mathrm{\θ}}_0-{\mathrm{\θ}}_3-{\mathrm{\θ}}_4-{\mathrm{\θ}}_5-{\mathrm{\θ}}_8-{\mathrm{\θ}}_{10}+\frac{3}{2}\mathrm{\π})\]}^{1/2}\end{array}\right.---\left(IV\right)$

其中，

$z_3=h_1+l_1{\mathrm{sin\θ}}_0+l_5\sin \[\mathrm{\ϵ}-({\mathrm{\θ}}_8+{\mathrm{\θ}}_{10}+\arccos \frac{l_6^2+l_7^2-l_q^2}{2\·l_6\·l_7}-\frac{3}{2}\mathrm{\π})\]+h_4\cos \mathrm{\ϵ}+l_8\sin \mathrm{\ϵ}-h_3-h_6\cos \mathrm{\ϵ},$

$z_4=l_{5}cos\[\mathrm{\ϵ}-({\mathrm{\θ}}_8+{\mathrm{\θ}}_{10}+\arccos \frac{l_6^2+l_7^2-l_q^2}{2\·l_6\·l_7}-\frac{3}{2}\mathrm{\π})\]-h_{4}sin\mathrm{\ϵ}+l_{8}cos\mathrm{\ϵ}+h_{6}sin\mathrm{\ϵ}-l_1{\mathrm{cos\θ}}_0-l_9.$

(y)将(x)中求解的液压支架目标位置位姿参数，与液压支架当前位姿参数比较，以液压支架位姿调整速度最快、用液量最省为原则从液压支架最优动作规则表中智能决策出平衡千斤顶和立柱的最优动作方案；液压支架最优动作规则表如表一所示：

表一：液压支架最优动作规则表

其中

(3)PID控制运算：单片机将阶段目标参数值与行程传感器和激光测距仪的实时检测数据对比，获取实时检测数据与阶段目标参数值的差值，单片机对该差值进行PID控制运算；

(4)指令输出：单片机根据步骤(3)所述的差值，根据已确定的最优动作方案，输出第一电磁比例阀、第二电磁比例阀的控制信号，控制信号控制第一电磁比例阀、第二电磁比例阀的运动方向和开口度大小以改变进出平衡千斤顶及立柱的流量，从而改变平衡千斤顶及立柱的长度，实现液压支架的位姿误差反馈控制，使液压支架遵循最优动作方案逐步到达最终位姿。

具体地，单片机比较输出的最终位姿信号l_q0(平衡千斤顶)和l_z0(立柱)以及液压支架实时监测信号l_q和l_z，根据液压支架最优动作方案确定PID控制量的阶段目标额定值l′_q0和l′_z0，通过比较阶段目标额定值l′_q0和l′_z0与液压支架实时监测信号l_q和l_z，得出目标位姿参数与当前位姿参数的差值；单片机根据差值，控制平衡千斤顶和立柱连接的电磁比例阀的运动方向和开口度大小以改变进出立柱及平衡千斤顶的流量，从而改变液压支架立柱及平衡千斤顶的长度，在改变液压支架立柱及平衡千斤顶的长度过程中，立柱及平衡千斤顶的具体动作如表二所示，

表二：立柱及平衡千斤顶的具体动作表

本实施例的技术方案，由控制器、加速度传感器、行程传感器、激光测距仪及电磁比例阀组成液压支架的检控系统，检控系统可以很方便地安装在液压支架上，借助该检控系统的液压支架位姿检测与控制的方法，通过采用PID控制规则对液压支架位姿进行实时全姿态控制，可实现液压支架升降架过程的快速水平升、降架以及工作面顶板倾斜时的智能贴顶，同时避免了液压支架升降架过程中立柱、平衡千斤顶反复动作的过程，提升了液压支架的升、移架速度及跟机速度；且PID控制系统对系统模型依赖少，适应性强，具有较强的鲁棒性。

实施例2：

一种用于二柱式液压支架位姿检测与控制的方法，方法步骤如实施例1所述，其不同之处在于：检控系统中的输入装置选用触摸屏显示/输入模块，触摸屏显示/输入模块与控制器连接，触摸屏可以省略键盘，同时操作更方便、更直观。

如图2所示，控制器还可以外接上位机，上位机需通过外置的数据采集装置与第一加速度传感器、第二加速度传感器、行程传感器和激光测距仪连接进行数据采集，其中上位机为PC机，数据采集装置为研华PCI-1711U数据采集卡，上位机与研华PCI-1711U数据采集卡连接，研华PCI-1711U数据采集卡与第一加速度传感器、第二加速度传感器、行程传感器、激光测距仪连接，研华PCI-1711U数据采集卡将接收到数据传输给PC机，地面操作人员可通过PC机在地面进行操控。

实施例3：

一种用于二柱式液压支架位姿检测与控制的方法，方法步骤如实施例1所述，其不同之处在于：该系统的使用方法还包括输入预设定参数及预设定参数处理过程，在液压支架下井前，通过键盘和显示器进行液压支架初始参数设定(以后连杆与底座铰接为基准点)，包括液压支架后连杆长度l₁和定位尺寸高度h₁，液压支架前连杆的长度l₃和定位尺寸高度h₂，底座与连杆铰接点距离l₂及其连接线与底座的夹角θ₂，立柱下柱窝定位尺寸h₃和l₉及上柱窝定位尺寸h₆和l₈，掩护梁与连杆铰接点连线的距离l₄以及其与掩护梁的夹角θ₅，掩护梁长度l₅及上定位尺寸h₅，平衡千斤顶下定位尺寸l₆、θ₈及上定位尺寸l₇和θ₁₀，掩护梁与顶梁铰接点到顶梁最上方的距离h₄。

Claims (10)

1.一种用于二柱式液压支架位姿检测与控制的方法，所述方法包括主要由控制器、数据采集装置、第一加速度传感器、第二加速度传感器、行程传感器、激光测距仪、第一电磁比例阀和第二电磁比例阀组成的检控系统，所述第一加速度传感器设置在液压支架的底座上，第二加速度传感器设置在液压支架的后连杆上，行程传感器设置在液压支架的平衡千斤顶内部，激光测距仪设置在液压支架的立柱上，第一电磁比例阀设置在液压支架平衡千斤顶液压控制回路上，第二电磁比例阀设置在液压支架立柱液压控制回路上，所述第一加速度传感器、第二加速度传感器、行程传感器、激光测距仪均与数据采集装置连接，数据采集装置与控制器连接，控制器还连接第一电磁比例阀和第二电磁比例阀，所述方法包括以下步骤，

(1)信号采集：将第一加速度传感器测量的底座加速度、第二加速度传感器测量的后连杆加速度、行程传感器测取的平衡千斤顶长度、激光测距仪测取的立柱长度传输给数据采集装置，数据采集装置对接收的信号进行滤波处理后得到加速度、长度原始数据，并将加速度、长度原始数据传输给控制器；

(2)数据处理：控制器根据接收到的加速度、长度原始数据，计算出液压支架底座的实时倾角、后连杆与底座的夹角以及液压支架实时行为位姿，并输出液压支架的实时行为位姿；然后，控制器根据液压支架操作人员动作指令及工作面顶板倾角，计算出液压支架所要达到的最终位姿，并对比液压支架当前行为位姿得出位移差，再根据最优动作规则表，以液压支架动作速度最快、用液量最省原则智能判定出液压支架最优动作方案，控制器根据位移差及设定的步进次数，计算出液压支架阶段步进值，并将阶段步进值设定为控制器的阶段目标参数值；

(3)PID控制运算：控制器将阶段目标参数值与行程传感器和激光测距仪的实时检测数据对比，获取阶段目标参数值与实时检测数据的差值；

(4)指令输出：控制器根据步骤(3)所述的差值，输出第一电磁比例阀、第二电磁比例阀的控制信号，控制信号控制第一电磁比例阀、第二电磁比例阀的运动方向和开口度大小以改变进出平衡千斤顶及立柱的流量，从而改变平衡千斤顶及立柱的长度，实现液压支架的位姿误差反馈控制，使液压支架逐步到达最终位姿。

2.如权利要求1所述用于二柱式液压支架位姿检测与控制的方法，其特征在于，步骤(1)中，所述滤波处理是指数据采集装置采用中值滤波法对第一加速度传感器、第二加速度传感器、行程传感器和激光测距仪对传输给数据采集装置的数据进行滤波处理，其具体过程包括：连续采样N个数据，然后用堆序法原理对采集的N个数据进行排序，找出N个数据中的最大值和最小值，去掉最大值和最小值，然后计算剩余的N-2个数据的算术平均值，N取为3-14。

3.如权利要求1所述用于二柱式液压支架位姿检测与控制的方法，其特征在于，步骤(2)中，控制器计算出液压支架实时行为位姿，具体包括以下过程：

(a)读取液压支架后连杆第二加速度传感器输出的α_h及底座第一加速度传感器输出的α_d，变换得出液压支架后连杆的绝对倾角β及底座的绝对倾角α：

β＝arcsin a_h/g，α＝arcsin a_d/g

液压支架后连杆相对底座的倾角：θ₀＝β+α (I)；

(b)当步骤(a)计算出液压支架后连杆相对底座的倾角θ₀后，通过读取液压支架平衡千斤顶和立柱的长度，将平衡千斤顶和立柱的长度带入液压支架的实时行为位姿状态方程(II)，可得出液压支架的实时位姿参数，最后在显示器上输出液压支架当前位姿状态，完成液压支架实时位姿状态监测；

$\left[\begin{array}{c}\mathrm{\ϵ}\\ {\mathrm{\θ}}_0\\ {\mathrm{\θ}}_1\\ {\mathrm{\θ}}_2\\ {\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4\\ {\mathrm{\θ}}_5\\ {\mathrm{\θ}}_6\\ {\mathrm{\θ}}_7\\ {\mathrm{\θ}}_9\\ {\mathrm{\θ}}_{10}\\ {\mathrm{sin\θ}}_{11}\end{array}\right]=\left(\begin{array}{ccccccccccc}-1& 0& 0& 1& 1& 1& 0& 0& 1& 1& 1\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ -1& 0& -1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ -1& 0& 0& 1& 1& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\\ \frac{l_1}{l_z}& 0& 0& 0& 0& 0& \frac{l_5}{l_z}& 0& 0& 0& 0\end{array}\right)\left[\begin{array}{c}{\mathrm{\θ}}_0\\ {\mathrm{\θ}}_1\\ {\mathrm{\θ}}_2\\ {\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4\\ {\mathrm{\θ}}_5\\ {\mathrm{\θ}}_6\\ {\mathrm{\θ}}_7\\ {\mathrm{\θ}}_8\\ {\mathrm{\θ}}_9\\ {\mathrm{\θ}}_{10}\end{array}\right]+\left[\begin{array}{c}-\frac{3}{2}\mathrm{\π}\\ \mathrm{\β}+\mathrm{\α}\\ \mathrm{\π}\\ 0\\ \arccos \frac{l_1^2+z^2-l_2^2}{2l_1\·z}\\ \arccos \frac{l_4^2+z^2-l_3^2}{2l_4\·z}\\ 0\\ 0\\ 0\\ \arccos \frac{l_6^2+l_7^2-l_q^2}{2l_6\·l_7}\\ 0\\ \frac{z_2}{l_z}\end{array}\right]---\left(II\right)$

其中z₂＝h₁-h₃+h₄cosε-h₆cosε+l₈sinε，ε为液压支架顶梁相对底座的倾角，θ₆为液压支架掩护梁与底座的相对倾角，θ₉为液压支架平衡千斤顶与顶梁、掩护梁铰接点形成的张角，θ₁₁为立柱倾角。

4.如权利要求1所述用于二柱式液压支架位姿检测与控制的方法，其特征在于，步骤(2)中，所述最优动作方案的确定过程如下：

(x)以工作面顶板倾角η、液压支架操作人员对液压支架的实际控制指令及求解的液压支架实时位姿参数为基础，基于平衡千斤顶长度、立柱长度与液压支架工作高度的变形协调方程(III)和平衡千斤顶长度、立柱长度与液压支架顶梁位姿角ε的变形协调方程(IV)求解液压支架目标位置位姿参数；

$\left\{\begin{array}{c}{l}_{z~H}=\frac{l_5\cos \arcsin \[(H-h_1-h_4-l_1{\mathrm{sin\θ}}_0)/l_5\]+l_8-l_1{\mathrm{cos\θ}}_0-l_9}{\cos \arcsin \[(H-h_6-h_3)/l_z\]}\\ l_{q~H}={\[l_6^2-{l_7}^2-2l_6\·l_7\·\cos (\frac{3}{2}\mathrm{\π}-\arcsin \[(H-h_1-h_4-l_1{\mathrm{sin\θ}}_0)/l_5\]-{\mathrm{\θ}}_8-{\mathrm{\θ}}_{10})\]}^{1/2}\end{array}\right.---\left(III\right)$

$\left\{\begin{array}{c}{l}_{z~\mathrm{\ϵ}}={(z_3^2+z_4^2)}^{1/2}\\ l_{q~\mathrm{\ϵ}}={\[l_6^2+{l_7}^2-2l_6\·l_{7}cos(\mathrm{\ϵ}+{\mathrm{\θ}}_0-{\mathrm{\θ}}_3-{\mathrm{\θ}}_4-{\mathrm{\θ}}_5-{\mathrm{\θ}}_8-{\mathrm{\θ}}_{10}+\frac{3}{2}\mathrm{\π})\]}^{1/2}\end{array}\right.---\left(IV\right)$

其中，

$z_3=h_1+l_1{\mathrm{sin\θ}}_0+l_{5}sin\[\mathrm{\ϵ}-({\mathrm{\θ}}_8+{\mathrm{\θ}}_{10}+\arccos \frac{l_6^2+l_7^2-l_q^2}{2\·l_6\·l_7}-\frac{3}{2}\mathrm{\π})\]+h_{4}cos\mathrm{\ϵ}+l_8\sin \mathrm{\ϵ}-h_3-h_{6}cos\mathrm{\ϵ},$

$z_4=l_{5}cos\[\mathrm{\ϵ}-({\mathrm{\θ}}_8+{\mathrm{\θ}}_{10}+\arccos \frac{l_6^2+l_7^2-l_q^2}{2\·l_6\·l_7}-\frac{3}{2}\mathrm{\π})\]-h_4\sin \mathrm{\ϵ}+l_{8}cos\mathrm{\ϵ}+h_{6}sin\mathrm{\ϵ}-l_1{\mathrm{cos\θ}}_0-l_9.$

(y)将(x)中求解的液压支架目标位置位姿参数，与液压支架当前位姿参数比较，以液压支架位姿调整速度最快、用液量最少为原则从液压支架最优动作规则表中智能决策出平衡千斤顶和立柱的最优动作方案；液压支架最优动作规则表如表一所示：

表一：液压支架最优动作规则表

其中

5.如权利要求1所述用于二柱式液压支架位姿检测与控制的方法，其特征在于，步骤(4)中，控制器比较输出的最终位姿的信号l_q0和l_z0以及液压支架实时监测信号l_q和l_z，根据液压支架最优动作方案确定PID控制量的阶段目标额定值l'_q0和l'_z0，并将数据传递给控制器，通过比较阶段目标额定值l'_q0和l'_z0与液压支架实时监测信号l_q和l_z，得出目标位姿参数与当前位姿参数的差值；控制器根据差值，控制平衡千斤顶和立柱连接的电磁比例阀的运动方向和开口度大小以改变进出立柱及平衡千斤顶的流量，从而改变液压支架立柱及平衡千斤顶的长度，在改变液压支架立柱及平衡千斤顶的长度过程中，立柱及平衡千斤顶的具体动作如表二所示，

表二：立柱及平衡千斤顶的具体动作表

6.如权利要求1所述用于二柱式液压支架位姿检测与控制的方法，其特征在于，所述使用方法还包括输入预设定参数及预设定参数处理过程，在液压支架下井前，通过键盘和显示器进行液压支架初始参数设定，包括液压支架后连杆长度l₁和定位尺寸高度h₁，液压支架前连杆的长度l₃和定位尺寸高度h₂，底座与连杆铰接点距离l₂及其连接线与底座的夹角θ₂，立柱下柱窝定位尺寸h₃和l₉及上柱窝定位尺寸h₆和l₈，掩护梁与连杆铰接点连线的距离l₄以及其与掩护梁的夹角θ₅，掩护梁长度l₅及上定位尺寸h₅，平衡千斤顶下定位尺寸l₆、θ₈及上定位尺寸l₇和θ₁₀，掩护梁与顶梁铰接点到顶梁最上方的距离h₄。

7.如权利要求1所述用于二柱式液压支架位姿检测与控制的方法，其特征在于，所述检控系统还包括输入装置，输入装置与控制器连接，所述输入装置选用显示器和键盘，或选用触摸屏显示/输入模块。

8.如权利要求1所述用于二柱式液压支架位姿检测与控制的方法，其特征在于，所述控制器选用MSP430F5438A单片机。

9.如权利要求1所述用于二柱式液压支架位姿检测与控制的方法，其特征在于，所述行程传感器选用米朗MIRAN MTL3-2000mm油缸内置磁致伸缩位移传感器。

10.如权利要求7所述用于二柱式液压支架位姿检测与控制的方法，其特征在于，所述激光测距仪选用康力华KLH-01T-20hz激光测距仪；所述触摸屏显示/输入模块选用迪文DMT80480T070_06WT+触摸屏；所述第一电磁比例阀选用型号为4WRZe32W9-520-7X/6EG24N9ETK4F1/M的三位四通比例换向阀；所述第二电磁比例阀选用型号为4WRZe52W9-1000-7X/6EG24N9ETK4F1/M的三位四通比例换向阀。