CN103968856B - 一种液压支架位姿的实时检测方法 - Google Patents

Abstract

本发明公开一种液压支架位姿的实时检测方法，尤其涉及一种基于三轴加速度传感器和三轴陀螺仪检测液压支架位姿的方法。随着工作面的推进，支架需要不断移动，支护位置不是固定不变的，在支架移架过程中主要的主动动作包括：降架、拉架、升架。液压支架顶梁直接与顶板接触，是用于支护的主要部件，同时也是主要的运动部件，本发明使用三轴加速度传感器和三轴陀螺仪实时测量支架顶梁位置、倾角的变化量，通过积分的方式获得液压支架任意时刻的位姿。

Description

一种液压支架位姿的实时检测方法

技术领域

本发明涉及一种液压支架位姿的实时检测方法，尤其涉及一种基于三轴加速度传感器和三轴陀螺仪测量液压支架位姿的方法。

背景技术

煤矿开采环境恶劣，工人工作劳动强度高、健康危害大，有时还会危及生命。随着煤矿行业科技投入，自动化程度逐渐提高，工人工作条件得到了一定程度的改善，但工作面开采时工作环境恶劣的情况，始终无法得到改善，煤尘、水雾以及岩层冲击无时无刻不危害着工人的身体健康。实现工作面无人、少人化开采可以有效的避免上述危害。

在综采工作面中，液压支架负责支撑顶板，以及对顶板进行有效的控制，保证顶板的稳定性。随着工作面的推进，液压支架需要不断移架，然而在移架过程中，支架顶梁脱离顶板的高度、时间均会影响顶板的稳定性。在实现无人、少人工作面的进程中，液压支架姿态的实时检测是无法回避的问题。

根据液压支架机械机构，只要测得支架主动油缸的实时长度便可以获得支架姿态，但是在井下的恶劣环境下以及条件限制，无法通过传感器直接测得主动油缸的长度。

发明内容

为了克服上述缺陷，本发明提供一种通过测量支架顶梁运动情况，来获得液压位姿的实时检测方法，尤其涉及一种基于三轴加速度传感器和三轴陀螺仪测量液压支架位姿的方法。

本发明为了解决其技术问题所采用的技术方案是：一种液压支架位姿的实时检测方法，其基于三轴加速度传感器以及三轴陀螺仪测量液压支架移架过程中支架顶梁运动的具体实现过程，所述移架过程包括降架、拉架和升架三个过程，在降架和升架过程中支架只有顶梁位姿发生变化，即获得顶梁位姿变化情况便可获得支架姿态；在拉架过程中支架本身姿态不发生变化，顶梁的位姿变化情况就是支架整体位姿的变化情况；

三轴加速传感器和三轴陀螺仪测量轴重合，在初始状态时两个传感器测量坐标系与参考坐标系OXYZ重合，经过i-1次移架过程后，测量坐标系变更为OX_iY_iZ_i，将坐标系OX_iY_iZ_i看作由坐标系OXYZ经坐标变换后得到，其变换方法如下：

由坐标系OXYZ绕Z轴旋转α角，得到OX’Y’Z’；

由坐标系OX’Y’Z’绕X’轴旋转β角，得到OX”Y”Z”；

由坐标系OX”Y”Z”绕Z”轴旋转γ角，得到OX_iY_iZ_i；

坐标变换矩阵为T(α,β,γ)，且：

$T=\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\β}\sin \mathrm{\α}& -\sin \mathrm{\β}\\ \sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}-\cos \mathrm{\γ}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}+\cos \mathrm{\γ}\cos \mathrm{\α}& \sin \mathrm{\γ}\cos \mathrm{\β}\\ \cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\β}+\sin \mathrm{\γ}\sin \mathrm{\α}& \cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}-\sin \mathrm{\γ}\cos \mathrm{\α}& \cos \mathrm{\γ}\cos \mathrm{\β}\end{array}\right]$

下面分别通过三个步骤，即：降架、拉架和升架来对移架过程中顶梁位姿的变化来进行测量：

（1）降架，在降架开始△t₁后，顶梁测量坐标系变为OX_i1Y_i1Z_i1：

a、通过三轴陀螺仪测得顶梁在降架过程中绕X_i1、Y_i1、Z_i1轴的瞬时角速率

b、根据之前设定的坐标变换流程可以获得到与之间的关系：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\\ \stackrel{\·}{\mathrm{\γ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin \mathrm{\β}& \cos \mathrm{\γ}& 0\\ \sin \mathrm{\β}& 0& 1\\ \cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\ω}}_x^{i1}\\ {\mathrm{\ω}}_y^{i1}\\ {\mathrm{\ω}}_z^{i1}\end{array}\right];$

c、降架过程中顶梁只有俯仰角α和翻滚角β发生变化，通过上式积分便可求出降架开始经过任意时间△t后，顶梁转动情况，即俯仰角和翻滚角变化了Δα、Δβ， Δγ=0，变化后：α’=α+Δα、β’=β+Δβ、γ’=γ；

d、更新坐标变换矩阵T(α,β,γ)，新坐标变换矩阵为T’(α’,β’,γ’)；

e、通过三轴加速度传感器测得顶梁在降架时X_i1、Y_i1、Z_i1轴方向的瞬时加速度

f、利用坐标变换矩阵将加速度变换到参考坐标系OXYZ，求得X、Y、Z轴方向的瞬时加速度a_x、a_y、a_z，

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i1}\\ a_y^{i1}\\ a_z^{i1}\end{array}\right];$

g、滤掉重力加速度g， $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i1}\\ a_y^{i1}\\ a_z^{i1}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right];$

h、在△t₁时间内，顶梁在X、Y、Z方向的位移可以用二重积分的方法求得△s_x1、△s_y1、△s_z1，Δs_x1=∫∫a_x1dt、Δs_y1=∫∫a_y1dt、Δs_z1=∫∫a_z1dt；

经过上述运算可以求得顶梁在降架结束后，俯仰角变化值α₁、翻滚角变化值β₁、X方向位移s_x1、Y方向位移s_y1、Z方向位移s_z1；

（2）拉架，在拉架开始△t₂后，顶梁测量坐标系变为OX_i2Y_i2Z_i2：

a、通过三轴陀螺仪测得顶梁在降架过程中绕X_i2、Y_i2、Z_i2轴的瞬时角速率

b、根据之前设定的坐标变换流程可以获得到 与之间的关系：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\\ \stackrel{\·}{\mathrm{\γ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin \mathrm{\β}& \cos \mathrm{\γ}& 0\\ \sin \mathrm{\β}& 0& 1\\ \cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\ω}}_x^{i2}\\ {\mathrm{\ω}}_y^{i2}\\ {\mathrm{\ω}}_z^{i2}\end{array}\right];$

c、拉架过程中顶梁可能俯仰角α、翻滚角β和导航角γ同时发生变化，通上式积分便可求出降架开始经过任意时间△t后，顶梁转动情况，即俯仰角和翻滚角变化了Δα、Δβ， $\mathrm{\Δ\α}=\∫\stackrel{\·}{\mathrm{\α}}\mathrm{dt},\mathrm{\Δ\β}=\∫\stackrel{\·}{\mathrm{\β}}\mathrm{dt},\mathrm{\Δ\γ}=\∫\stackrel{\·}{\mathrm{\γ}}\mathrm{dt},$ 变化后：α’=α+Δα、β’=β+Δβ、γ’=γ+Δγ；

d、更新坐标变换矩阵T(α,β,γ)，新坐标变换矩阵为T’(α’,β’,γ’)

e、通过三轴加速度传感器测得顶梁在拉架时X_i2、Y_i2、Z_i2轴方向的瞬时加速度

f、利用坐标变换矩阵将加速度变换到参考坐标系OXYZ，求得X、Y、Z轴方向的瞬时加速度a_x、a_y、a_z，

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i2}\\ a_y^{i2}\\ a_z^{i2}\end{array}\right];$

g、滤掉重力加速度g， $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i2}\\ a_y^{i2}\\ a_z^{i2}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right];$

h、在△t₂时间内，顶梁在X、Y、Z方向的位移可以用二重积分的方法求得△s_x2、△s_y2、△s_z2；

Δs_x2=∫∫a_x2dt、Δs_y2=∫∫a_y2dt、Δs_z2=∫∫a_z2dt；

经过上述运算可以求得顶梁在拉架结束后，俯仰角变化值α₂、翻滚角变化值β₂、导航角变化值γ₂、X方向位移s_x2、Y方向位移s_y2、Z方向位移s_z2；

（3）升架，在升架开始△t₃后，顶梁测量坐标系变为OX_i3Y_i3Z_i3：

a、通过三轴陀螺仪测得顶梁在升架过程中绕X_i3、Y_i3、Z_i3轴的瞬时角速率

b、根据之前设定的坐标变换流程可以获得到与之间的关系：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\\ \stackrel{\·}{\mathrm{\γ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin \mathrm{\β}& \cos \mathrm{\γ}& 0\\ \sin \mathrm{\β}& 0& 1\\ \cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\ω}}_x^{i3}\\ {\mathrm{\ω}}_y^{i3}\\ {\mathrm{\ω}}_z^{i3}\end{array}\right];$

c、升架过程中顶梁只有俯仰角α和翻滚角β发生变化，通上式积分便可求出降架开始经过任意时间△t后，顶梁转动情况，即俯仰角和翻滚角变化了Δα、Δβ，Δγ=0，变化后：α’=α+Δα、β’=β+Δβ、γ’=γ；

d、更新坐标变换矩阵T(α,β,γ)，新坐标变换矩阵为T’(α’,β’,γ’)；

e、通过三轴加速度传感器测得顶梁在升架时X_i3、Y_i3、Z_i3轴方向的瞬时加速度

f、利用坐标变换矩阵将加速度变换到参考坐标系OXYZ，求得X、Y、Z轴方向的瞬时加速度a_x、a_y、a_z，

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i3}\\ a_y^{i3}\\ a_z^{i3}\end{array}\right];$

g、滤掉重力加速度g， $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i3}\\ a_y^{i3}\\ a_z^{i3}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right];$

h、在△t₃时间内，顶梁在X、Y、Z方向的位移可以用二重积分的方法求得△s_x3、△s_y3、△s_z3；

Δs_x3=∫∫a_x3dt、Δs_y3=∫∫a_y3dt、Δs_z3=∫∫a_z3dt；

经过上述运算可以求得顶梁升架完成后，俯仰角变化值α₃、翻滚角变化值β₃、X方向位移s_x3、Y方向位移s_y3、Z方向位移s_z3；

综上所述：支架在第i次移架过程中，支架顶梁在参考坐标系X方向位移s_xi，Y方向位移s_yi，Z方向位移s_zi；

s_xi=s_x1+s_x2+s_x3，s_yi=s_y1+s_y2+s_y3，s_zi=s_z1+s_z2+s_z3；

支架在第i次移架过程中，顶梁俯仰角变化α_i，横滚角变化β_i，航向角变化γ_i；

α_i＝α₁+α₂+α₃、β_i＝β₁+β₂+β₃、γ_i＝γ₁+γ₂+γ₃；

支架在第i次移架后，在参考坐标系下，顶梁法向量n=(x_n,y_n,z_n)^T；

$\left[\begin{array}{c}{x}_n\\ y_n\\ z_n\end{array}\right]=T\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\β}\sin \mathrm{\α}\\ \sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}+\cos \mathrm{\γ}\cos \mathrm{\α}\\ \cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}-\sin \mathrm{\γ}\cos \mathrm{\α}\end{array}\right];$

已知第i次移架前液压支架的姿态，便可以通过上述方法获得第i次移架后液压支架的姿态；已知支架在井下安装完成后的姿态可以通过上述方法检测液压支架任意时刻的姿态。

本发明的有益效果是：通过安装在顶梁的三轴加速度传感器和三轴陀螺仪传感器，可以实时检测液压支架在进行降架、拉架、升架动作时，液压支架位姿的变化情况，为液压支架的姿态检测提供有效的依据。可以将获得的姿态数据导入液压支架监控系统中，并转换为可视化的图形界面，直观的显示给远程监控操作人员，操作人员根据支架的实时姿态可以更加有效的对其进行控制。

附图说明

图1传感器安装位置及测量轴方向的示意图；

图2顶梁俯仰角、横滚角、航向角的分解示意图；

图3液压支架位姿实时检测方法流程图。

具体实施方式

下面结合附图对本发明进行进一步的阐述。

如图1至图3所示，一种液压支架位姿的实时检测方法，其基于三轴加速度传感器以及三轴陀螺仪测量液压支架移架过程中支架顶梁运动的具体实现过程，所述移架过程包括降架、拉架和升架三个过程，在降架和升架过程中支架只有顶梁位姿发生变化，即获得顶梁位姿变化情况便可获得支架姿态；在拉架过程中支架本身姿态不发生变化，顶梁的位姿变化情况就是支架整体位姿的变化情况；

三轴加速传感器和三轴陀螺仪测量轴重合，在初始状态时两个传感器测量坐标系与参考坐标系OXYZ重合，经过i-1次移架过程后，测量坐标系变更为OX_iY_iZ_i，将坐标系OX_iY_iZ_i看作由坐标系OXYZ经坐标变换后得到，其变换方法如下：

由坐标系OXYZ绕Z轴旋转α角，得到OX’Y’Z’；

由坐标系OX’Y’Z’绕X’轴旋转β角，得到OX”Y”Z”；

由坐标系OX”Y”Z”绕Z”轴旋转γ角，得到OX_iY_iZ_i；

坐标变换矩阵为T(α,β,γ)，且：

$T=\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\β}\sin \mathrm{\α}& -\sin \mathrm{\β}\\ \sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}-\cos \mathrm{\γ}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}+\cos \mathrm{\γ}\cos \mathrm{\α}& \sin \mathrm{\γ}\cos \mathrm{\β}\\ \cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\β}+\sin \mathrm{\γ}\sin \mathrm{\α}& \cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}-\sin \mathrm{\γ}\cos \mathrm{\α}& \cos \mathrm{\γ}\cos \mathrm{\β}\end{array}\right]$

下面分别通过三个步骤，即：降架、拉架和升架来对移架过程中顶梁位姿的变化来进行测量：

（1）降架，在降架开始△t₁后，顶梁测量坐标系变为OX_i1Y_i1Z_i1：

a、通过三轴陀螺仪测得顶梁在降架过程中绕X_i1、Y_i1、Z_i1轴的瞬时角速率

b、根据之前设定的坐标变换流程可以获得到与之间的关系：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\\ \stackrel{\·}{\mathrm{\γ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin \mathrm{\β}& \cos \mathrm{\γ}& 0\\ \sin \mathrm{\β}& 0& 1\\ \cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\ω}}_x^{i1}\\ {\mathrm{\ω}}_y^{i1}\\ {\mathrm{\ω}}_z^{i1}\end{array}\right];$

c、降架过程中顶梁只有俯仰角α和翻滚角β发生变化，通过上式积分便可求出降架开始经过任意时间△t后，顶梁转动情况，即俯仰角和翻滚角变化了Δα、Δβ， Δγ=0，变化后：α’=α+Δα、β’=β+Δβ、γ’=γ；

d、更新坐标变换矩阵T(α,β,γ)，新坐标变换矩阵为T’(α’,β’,γ’)；

e、通过三轴加速度传感器测得顶梁在降架时X_i1、Y_i1、Z_i1轴方向的瞬时加速度

f、利用坐标变换矩阵将加速度变换到参考坐标系OXYZ，求得X、Y、Z轴方向的瞬时加速度a_x、a_y、a_z，

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i1}\\ a_y^{i1}\\ a_z^{i1}\end{array}\right];$

g、滤掉重力加速度g， $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i1}\\ a_y^{i1}\\ a_z^{i1}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right];$

h、在△t₁时间内，顶梁在X、Y、Z方向的位移可以用二重积分的方法求得△s_x1、△s_y1、△s_z1，Δs_x1=∫∫a_x1dt、Δs_y1=∫∫a_y1dt、Δs_z1=∫∫a_z1dt；

经过上述运算可以求得顶梁在降架结束后，俯仰角变化值α₁、翻滚角变化值β₁、X方向位移s_x1、Y方向位移s_y1、Z方向位移s_z1；

（2）拉架，在拉架开始△t₂后，顶梁测量坐标系变为OX_i2Y_i2Z_i2：

a、通过三轴陀螺仪测得顶梁在降架过程中绕X_i2、Y_i2、Z_i2轴的瞬时角速率

b、根据之前设定的坐标变换流程可以获得到 与之间的关系：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\\ \stackrel{\·}{\mathrm{\γ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin \mathrm{\β}& \cos \mathrm{\γ}& 0\\ \sin \mathrm{\β}& 0& 1\\ \cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\ω}}_x^{i2}\\ {\mathrm{\ω}}_y^{i2}\\ {\mathrm{\ω}}_z^{i2}\end{array}\right];$

c、拉架过程中顶梁可能俯仰角α、翻滚角β和导航角γ同时发生变化，通上式积分便可求出降架开始经过任意时间△t后，顶梁转动情况，即俯仰角和翻滚角变化了Δα、Δβ， $\mathrm{\Δ\α}=\∫\stackrel{\·}{\mathrm{\α}}\mathrm{dt},\mathrm{\Δ\β}=\∫\stackrel{\·}{\mathrm{\β}}\mathrm{dt},\mathrm{\Δ\γ}=\∫\stackrel{\·}{\mathrm{\γ}}\mathrm{dt},$ 变化后：α’=α+Δα、β’=β+Δβ、γ’=γ+Δγ；

d、更新坐标变换矩阵T(α,β,γ)，新坐标变换矩阵为T’(α’,β’,γ’)

e、通过三轴加速度传感器测得顶梁在拉架时X_i2、Y_i2、Z_i2轴方向的瞬时加速度

f、利用坐标变换矩阵将加速度变换到参考坐标系OXYZ，求得X、Y、Z轴方向的瞬时加速度a_x、a_y、a_z，

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i2}\\ a_y^{i2}\\ a_z^{i2}\end{array}\right];$

g、滤掉重力加速度g， $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i2}\\ a_y^{i2}\\ a_z^{i2}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right];$

h、在△t₂时间内，顶梁在X、Y、Z方向的位移可以用二重积分的方法求得△s_x2、△s_y2、△s_z2；

Δs_x2=∫∫a_x2dt、Δs_y2=∫∫a_y2dt、Δs_z2=∫∫a_z2dt；

经过上述运算可以求得顶梁在拉架结束后，俯仰角变化值α₂、翻滚角变化值β₂、导航角变化值γ₂、X方向位移s_x2、Y方向位移s_y2、Z方向位移s_z2；

（3）升架，在升架开始△₃后，顶梁测量坐标系变为OX_i3Y_i3Z_i3：

a、通过三轴陀螺仪测得顶梁在升架过程中绕X_i3、Y_i3、Z_i3轴的瞬时角速率

b、根据之前设定的坐标变换流程可以获得到与之间的关系：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\\ \stackrel{\·}{\mathrm{\γ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin \mathrm{\β}& \cos \mathrm{\γ}& 0\\ \sin \mathrm{\β}& 0& 1\\ \cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\ω}}_x^{i3}\\ {\mathrm{\ω}}_y^{i3}\\ {\mathrm{\ω}}_z^{i3}\end{array}\right];$

c、升架过程中顶梁只有俯仰角α和翻滚角β发生变化，通上式积分便可求出降架开始经过任意时间△t后，顶梁转动情况，即俯仰角和翻滚角变化了Δα、Δβ，Δγ=0，变化后：α’=α+Δα、β’=β+Δβ、γ’=γ；

d、更新坐标变换矩阵T(α,β,γ)，新坐标变换矩阵为T’(α’,β’,γ’)；

e、通过三轴加速度传感器测得顶梁在升架时X_i3、Y_i3、Z_i3轴方向的瞬时加速度

f、利用坐标变换矩阵将加速度变换到参考坐标系OXYZ，求得X、Y、Z轴方向的瞬时加速度a_x、a_y、a_z，

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i3}\\ a_y^{i3}\\ a_z^{i3}\end{array}\right];$

g、滤掉重力加速度g， $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{,-1}\left[\begin{array}{c}{a}_x^{i3}\\ a_y^{i3}\\ a_z^{i3}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right];$

h、在△t₃时间内，顶梁在X、Y、Z方向的位移可以用二重积分的方法求得△s_x3、△s_y3、△s_z3；

Δs_x3=∫∫a_x3dt、Δs_y3=∫∫a_y3dt、Δs_z3=∫∫a_z3dt；

经过上述运算可以求得顶梁升架完成后，俯仰角变化值α₃、翻滚角变化值β₃、X方向位移s_x3、Y方向位移s_y3、Z方向位移s_z3；

综上所述：支架在第i次移架过程中，支架顶梁在参考坐标系X方向位移s_xi，Y方向位移s_yi，Z方向位移s_zi；

s_xi=s_x1+s_x2+s_x3，s_yi=s_y1+s_y2+s_y3，s_zi=s_z1+s_z2+s_z3；

支架在第i次移架过程中，顶梁俯仰角变化α_i，横滚角变化β_i，航向角变化γ_i；

α_i＝α₁+α₂+α₃、β_i＝β₁+β₂+β₃、γ_i＝γ₁+γ₂+γ₃；

支架在第i次移架后，在参考坐标系下，顶梁法向量n=(x_n,y_n,z_n)^T；

$\left[\begin{array}{c}{x}_n\\ y_n\\ z_n\end{array}\right]=T\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\β}\sin \mathrm{\α}\\ \sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}+\cos \mathrm{\γ}\cos \mathrm{\α}\\ \cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}-\sin \mathrm{\γ}\cos \mathrm{\α}\end{array}\right];$

已知第i次移架前液压支架的姿态，便可以通过上述方法获得第i次移架后液压支架的姿态；已知支架在井下安装完成后的姿态可以通过上述方法检测液压支架任意时刻的姿态。

Claims (1)

1.一种液压支架位姿的实时检测方法，其特征在于：基于三轴加速度传感器以及三轴陀螺仪测量液压支架移架过程中支架顶梁运动的具体实现过程，所述移架过程包括降架、拉架和升架三个过程，在降架和升架过程中支架只有顶梁位姿发生变化，即获得顶梁位姿变化情况便可获得支架姿态；在拉架过程中支架本身姿态不发生变化，顶梁的位姿变化情况就是支架整体位姿的变化情况；

三轴加速传感器和三轴陀螺仪测量轴重合，在初始状态时两个传感器测量坐标系与参考坐标系OXYZ重合，经过i-1次移架过程后，测量坐标系变更为OX_iY_iZ_i，将坐标系OX_iY_iZ_i看作由坐标系OXYZ经坐标变换后得到，其变换方法如下：

由坐标系OXYZ绕Z轴旋转α角，得到OX’Y’Z’；

由坐标系OX’Y’Z’绕X’轴旋转β角，得到OX”Y”Z”；

由坐标系OX”Y”Z”绕Z”轴旋转γ角，得到OX_iY_iZ_i；

坐标变换矩阵为T(α,β,γ)，且：

$T=\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\β}\sin \mathrm{\α}& -\sin \mathrm{\β}\\ \sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}-\cos \mathrm{\γ}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}+\cos \mathrm{\γ}\cos \mathrm{\α}& \sin \mathrm{\γ}\cos \mathrm{\β}\\ \cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\β}+\sin \mathrm{\γ}\sin \mathrm{\α}& \cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}-\sin \mathrm{\γ}\cos \mathrm{\α}& \cos \mathrm{\γ}\cos \mathrm{\β}\end{array}\right]$

下面通过三个步骤，即：降架、拉架和升架来分别对移架过程中顶梁位姿的变化来进行测量获得：

(1)降架，在降架开始△t₁后，顶梁测量坐标系变为OX_i1Y_i1Z_i1：

a、通过三轴陀螺仪测得顶梁在降架过程中绕X_i1、Y_i1、Z_i1轴的瞬时角速率 ${\mathrm{\ω}}_x^{i1},{\mathrm{\ω}}_y^{i1},{\mathrm{\ω}}_z^{i1};$

b、根据之前设定的坐标变换流程可以获得到与之间的关系：

$\left[\begin{array}{c}\stackrel{.}{\mathrm{\α}}\\ \stackrel{.}{\mathrm{\β}}\\ \stackrel{.}{\mathrm{\γ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin \mathrm{\β}& \cos \mathrm{\γ}& 0\\ \sin \mathrm{\β}& 0& 1\\ \cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\ω}}_x^{i1}\\ {\mathrm{\ω}}_y^{i1}\\ {\mathrm{\ω}}_z^{i1}\end{array}\right];$

c、降架过程中顶梁只有俯仰角α和翻滚角β发生变化，通过对上式积分便可求出降架开始经过任意时间△t后，顶梁转动情况，即俯仰角和翻滚角变化了Δα、Δβ，Δγ＝0，变化后：α’＝α+Δα、β’＝β+Δβ、γ’＝γ；

d、更新坐标变换矩阵T(α,β,γ)，新坐标变换矩阵为T’(α’,β’,γ’)；

e、通过三轴加速度传感器测得顶梁在降架时X_i1、Y_i1、Z_i1轴方向的瞬时加速度 $a_x^{i1},a_y^{i1},a_z^{i1};$

f、利用坐标变换矩阵将加速度变换到参考坐标系OXYZ，求得X、Y、Z轴方向的瞬时加速度a_x、a_y、a_z，

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{\text{'}-1}\left[\begin{array}{c}{a}_x^{i1}\\ a_y^{i1}\\ a_z^{i1}\end{array}\right];$

g、滤掉重力加速度g， $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{\text{'}-1}\left[\begin{array}{c}{a}_x^{i1}\\ a_y^{i1}\\ a_z^{i1}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right];$

h、在△t₁时间内，顶梁在X、Y、Z方向的位移可以用二重积分的方法求得△s_x1、△s_y1、△s_z1，Δs_x1＝∫∫a_x1dt、Δs_y1＝∫∫a_y1dt、Δs_z1＝∫∫a_z1dt；

经过上述运算可以求得顶梁在降架结束后，俯仰角变化值α₁、翻滚角变化值β₁、X方向位移s_x1、Y方向位移s_y1、Z方向位移s_z1；

(2)拉架，在拉架开始△t₂后，顶梁测量坐标系变为OX_i2Y_i2Z_i2：

a、通过三轴陀螺仪测得顶梁在降架过程中绕X_i2、Y_i2、Z_i2轴的瞬时角速率 ${\mathrm{\ω}}_x^{i2},{\mathrm{\ω}}_y^{i2},{\mathrm{\ω}}_z^{i2};$

b、根据之前设定的坐标变换流程可以获得到与之间的关系：

$\left[\begin{array}{c}\stackrel{.}{\mathrm{\α}}\\ \stackrel{.}{\mathrm{\β}}\\ \stackrel{.}{\mathrm{\γ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin \mathrm{\β}& \cos \mathrm{\γ}& 0\\ \sin \mathrm{\β}& 0& 1\\ \cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\ω}}_x^{i1}\\ {\mathrm{\ω}}_y^{i1}\\ {\mathrm{\ω}}_z^{i1}\end{array}\right];$

c、拉架过程中顶梁可能俯仰角α、翻滚角β和导航角γ同时发生变化，通过对上式积分便可求出拉架开始经过任意时间△t后，顶梁转动情况，即俯仰角、翻滚角和导航角变化了Δα、Δβ、Δγ，变化后：α’＝α+Δα、β’＝β+Δβ、γ’＝γ+Δγ；

d、更新坐标变换矩阵T(α,β,γ)，新坐标变换矩阵为T’(α’,β’,γ’)

e、通过三轴加速度传感器测得顶梁在拉架时X_i2、Y_i2、Z_i2轴方向的瞬时加速度 $a_x^{i2},a_y^{i2},a_z^{i2};$

f、利用坐标变换矩阵将加速度变换到参考坐标系OXYZ，求得X、Y、Z轴方向的瞬时加速度a_x、a_y、a_z，

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{\text{'}-1}\left[\begin{array}{c}{a}_x^{i2}\\ a_y^{i2}\\ a_z^{i2}\end{array}\right];$

g、滤掉重力加速度g， $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{\text{'}-1}\left[\begin{array}{c}{a}_x^{i2}\\ a_y^{i2}\\ a_z^{i2}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right];$

h、在△t₂时间内，顶梁在X、Y、Z方向的位移可以用二重积分的方法求得△s_x2、△s_y2、△s_z2；

Δs_x2＝∫∫a_x2dt、Δs_y2＝∫∫a_y2dt、Δs_z2＝∫∫a_z2dt；

经过上述运算可以求得顶梁在拉架结束后，俯仰角变化值α₂、翻滚角变化值β₂、导航角变化值γ₂、X方向位移s_x2、Y方向位移s_y2、Z方向位移s_z2；

(3)升架，在升架开始△t₃后，顶梁测量坐标系变为OX_i3Y_i3Z_i3：

a、通过三轴陀螺仪测得顶梁在升架过程中绕X_i3、Y_i3、Z_i3轴的瞬时角速率 ${\mathrm{\ω}}_x^{i3},{\mathrm{\ω}}_y^{i3},{\mathrm{\ω}}_z^{i3};$

b、根据之前设定的坐标变换流程可以获得到与之间的关系：

$\left[\begin{array}{c}\stackrel{.}{\mathrm{\α}}\\ \stackrel{.}{\mathrm{\β}}\\ \stackrel{.}{\mathrm{\γ}}\end{array}\right]={\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin \mathrm{\β}& \cos \mathrm{\γ}& 0\\ \sin \mathrm{\β}& 0& 1\\ \cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}& 0\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\ω}}_x^{i3}\\ {\mathrm{\ω}}_y^{i3}\\ {\mathrm{\ω}}_z^{i3}\end{array}\right];$

c、升架过程中顶梁只有俯仰角α和翻滚角β发生变化，通过对上式积分便可求出升架开始经过任意时间△t后，顶梁转动情况，即俯仰角和翻滚角变化了Δα、Δβ，Δγ＝0，变化后：α’＝α+Δα、β’＝β+Δβ、γ’＝γ；

d、更新坐标变换矩阵T(α,β,γ)，新坐标变换矩阵为T’(α’,β’,γ’)；

e、通过三轴加速度传感器测得顶梁在升架时X_i3、Y_i3、Z_i3轴方向的瞬时加速度 $a_x^{i3},a_y^{i3},a_z^{i3};$

f、利用坐标变换矩阵将加速度变换到参考坐标系OXYZ，求得X、Y、Z轴方向的瞬时加速度a_x、a_y、a_z，

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{\text{'}-1}\left[\begin{array}{c}{a}_x^{i3}\\ a_y^{i3}\\ a_z^{i3}\end{array}\right];$

g、滤掉重力加速度g， $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{\text{'}-1}\left[\begin{array}{c}{a}_x^{i3}\\ a_y^{i3}\\ a_z^{i3}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right];$

h、在△t₃时间内，顶梁在X、Y、Z方向的位移可以用二重积分的方法求得△s_x3、△s_y3、△s_z3；

Δs_x3＝∫∫a_x3dt、Δs_y3＝∫∫a_y3dt、Δs_z3＝∫∫a_z3dt；

经过上述运算可以求得顶梁升架完成后，俯仰角变化值α₃、翻滚角变化值β₃、X方向位移s_x3、Y方向位移s_y3、Z方向位移s_z3；

综上所述：支架在第i次移架过程中，支架顶梁在参考坐标系X方向位移s_xi，Y方向位移s_yi，Z方向位移s_zi；

s_xi＝s_x1+s_x2+s_x3，s_yi＝s_y1+s_y2+s_y3，s_zi＝s_z1+s_z2+s_z3；

支架在第i次移架过程中，顶梁俯仰角变化α_i，横滚角变化β_i，航向角变化γ_i；

α_i＝α₁+α₂+α₃、β_i＝β₁+β₂+β₃、γ_i＝γ₁+γ₂+γ₃；

支架在第i次移架后，在参考坐标系下，顶梁法向量n＝(x_n,y_n,z_n)^T；

$\left[\begin{array}{c}{x}_n\\ y_n\\ z_n\end{array}\right]=T\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\β}\sin \mathrm{\α}\\ \sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}+\cos \mathrm{\γ}\cos \mathrm{\α}\\ \cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}-\sin \mathrm{\γ}\cos \mathrm{\α}\end{array}\right];$

已知第i次移架前液压支架的姿态，便可以通过上述方法获得第i次移架后液压支架的姿态；已知支架在井下安装完成后的姿态可以通过上述方法检测液压支架任意时刻的姿态。