CN111879280B - 一种采煤机定位定姿装置及方法 - Google Patents

Abstract

一种采煤机定位定姿装置及方法，其装置包括在采煤机机身上安装的姿态测量装置和采煤机机轮上设置的里程计，姿态测量装置包括安装在转位机构上的1个转位机构、1只陀螺、第一加速度计、第二加速度计和数据处理单元，陀螺和第一加速度计及第二加速度计可随转位机构转动；首先进行初始装订，然后构造采煤机姿态矩阵，计算采煤机的经度、纬度和高度，实现采煤机的高精度定位定姿，使其满足综采工作面自动化、远程自动控制的需求，具有定位和定姿精度高、综采工作效率高及远程自动控制的优点。

Description

一种采煤机定位定姿装置及方法

技术领域

本发明属于采煤机定位定姿技术领域，具体涉及一种采煤机定位定姿装置及方法。

背景技术

采煤机、液压支架和刮板输送机是井下综采工作面最重要的三种设备，相互配合完成割煤、运煤和支护工作。其中采煤机为主导设备，是综采工作面割煤和装煤的主要设备，是一种高集成度综采设备。采煤机在工作时，沿刮板输送机的轨道进行往复式割煤，液压支架支护顶板并推进工作面。为了实现综采工作面自动化、远程自动控制，需要对采煤机精确动态定位，而且，针对自动化采煤需求，需要同时对采煤机进行定姿，所以采煤机定位定姿技术是煤矿生产装备自动化的关键技术。煤矿综采工作面工况复杂，同时空间封闭，因此采煤机定位定姿是一个典型的复杂封闭环境下室内定位定姿问题。

目前采煤机所用的定位定姿方法主要包括捷联惯导法、红外定位法、超声波定位法、齿轮计数定位法、无线传感器网络定位法等。

红外定位法由安装在采煤机上红外发射装置发射信号，液压支架上安装的接收装置接收信号，利用红外测距定位采煤机的位置，但其红外信号易受粉尘影响，且存在固有定位盲区，定位精度不高，因此使用有局限性。

超声波定位法将超声波发射装置安装在工作面巷道中，当采煤机经过时，机身发射超声波，根据各位置超声波接收装置接收信号，利用超声波测距定位采煤机的位置，超声波的优点是可以穿透粉尘，但由于工作面较长，信号失信严重，其定位精度不高，因此使用有局限性。

齿轮计数定位法是对采煤机行走齿轮的转动圈数进行计数，根据转动圈数和齿轮圆周长计算采煤机沿输送机轨道方向上的位移；但该方法只能用于定位采煤机沿轨道方向的一维位置，不能满足三维定位需要。

无线传感器网络定位法是在液压支架上布置多个位置已知的无线传感器(称为锚节点)，在采煤机上布置待定位节点(称为移动节点)，移动节点发射无线信号，锚节点接收无线信号监测采煤机与液压支架间的位置关系，解算出采煤机的位置；但由于工作面环境复杂，无线定位数据不稳定，且锚节点随液压支架移动后会导致自身位置变化，需要更新锚节点位置信息，同时不能进行采煤机定姿，不能满足实时定位定姿需求。

捷联惯导方法是一种全自主导航定位方法，无需借助外部信息，利用捷联惯导装置的三轴陀螺和三轴加速度计实时测量采煤机的角速度和线加速度，结合初始装订信息，通过姿态更新算法先解算出采煤机的运动姿态，然后将加速度根据姿态信息投影到导航坐标系，通过积分和二次积分获得采煤机的速度和位置等信息；捷联惯导方法定位定姿精度误差随时间累积，长时间工作精度难以保障。

发明内容

针对现有技术存在的问题，本发明的目的是提供一种采煤机定位定姿装置及方法，具有定位定姿精度高、综采工作效率高及远程自动控制的优点。

为了实现上述目的，本发明采用如下的技术方案：

一种采煤机定位定姿装置，包括在采煤机3机身上安装的姿态测量装置1和采煤机3机轮上设置的里程计2；所述姿态测量装置1包括1个转位机构4，转位机构4上近轴心向外分别安装有可随转位机构4转动的陀螺5、第一加速度计6及与第一加速度计6夹角呈90度的对应位置安装的第二加速度计7；转位机构4、陀螺5、第一加速度计6、第二加速度计7和里程计2的信息分别传输至外接的数据处理单元8，数据处理单元8计算采煤机的姿态和位置。

基于上述的采煤机定位定姿装置的定位定姿方法，具体包括以下步骤：

步骤一、定义坐标系

1)采煤机3坐标系b，OX_bY_bZ_b：坐标系原点O固连在姿态测量装置1中心，X_b轴正向指向采煤机3前进方向，Y_b轴垂直于X_b轴向上，Z_b轴与X_b轴、Y_b轴构成右手坐标系，X_b轴、Y_b轴、Z_b轴构成前上右坐标系；

2)里程计2坐标系p与采煤机3坐标系b重合；

3)转位机构4坐标系m，OX_mY_mZ_m：采煤机3坐标系b绕Y_b轴旋转角度α而得的坐标系；当转动角度α＝0时，转位机构4坐标系m与采煤机3坐标系b重合，陀螺5测量轴与X_m轴重合，第一加速度计6测量轴与X_m轴重合，第二加速度计7测量轴与Z_m轴重合；

4)导航坐标系n，OX_nY_nZ_n：北天东地理坐标系，X_n轴指向地理北向，Y_n轴指向天向，Z_n轴指向地理东向；

5)导航坐标系n经过三次旋转后与采煤机3坐标系b重合，三次旋转的角度即为机身航向角ψ、俯仰角θ和滚动角γ；

步骤二、初始装订

姿态测量装置1的数据处理单元8接收初始装订信息：初始位置

其中λ,

h分别表示采煤机3的经度、纬度和高度；同时由于采煤机3初始处于停止状态，采煤机3的初始北向速度、天向速度和东向速度均为0，即V(0)＝[000]^T；

步骤三、计算采煤机3航向和姿态信息

数据处理单元8接收并记录转位机构4、陀螺5、第一加速度计6、第二加速度计7的信息，计算采煤机3俯仰角θ、滚动角γ和航向角ψ：

1)姿态测量装置1的转位机构4连续匀速旋转，旋转角速度为ω；

2)数据处理单元8接收转位机构4测量的旋转角度α＝ω×t，α∈[0,2π)；

3)在α₁＝0，

α₃＝π，

时，数据处理单元8接收并记录陀螺5测量值ω_g(1)、ω_g(2)、ω_g(3)、ω_g(4)；

ω_g(i)＝cos(α_i)[cos(θ)cos(ψ)Ω_N+sin(θ)Ω_U]+

sin(α_i)[cos(θ)sin(γ)Ω_U-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))Ω_N]

(1)

其中，Ω_N和Ω_U为地球自转角速度ω_ie的北向分量和天向分量，即

4)在α₁＝0，

α₃＝π，

时，数据处理单元8接收并记录第一加速度计6测量值f_a1(1)、f_a1(2)、f_a1(3)、f_a1(4)；第二加速度计7测量值f_a2(1)、f_a2(2)、f_a2(3)、f_a2(4)；下标a1表示第一加速度计6，下标a2表示第二加速度计7；

5)根据加速度计测量值,数据处理单元8计算采煤机3俯仰角θ和滚动角γ,计算过程如下:

f_a1(i)＝cos(α_i)[sin(θ)g]+sin(α_i)[cos(θ)sin(γ)g] (3)

其中，g表示重力加速度；

令A₁＝sin(θ)g，A₂＝cos(θ)sin(γ)g，

将公式(3)和(4)整理改写为，

F＝HA (5)

其中，

F＝[f_a1(1) f_a1(2) f_a1(3) f_a1(4) f_a2(1) f_a2(2) f_a2(3) f_a2(4)]^T

A＝[A₁ A₂ A₃ A₄]^T

求解公式(5)可得,

A＝(H^TH)H^TF (6)

根据A的定义，可知：

根据公式(6)的计算结果A和公式(7)，可得：

6)数据处理单元8根据陀螺5测量值、俯仰角θ和滚动角γ，计算采煤机3航向角ψ，计算过程如下：

令C₁＝cos(θ)cos(ψ)Ω_N+sin(θ)Ω_U，

C₂＝cos(θ)sin(γ)Ω_U-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))Ω_N

将公式(1)整理改写为，

求解可得：

根据C₁定义，可得采煤机3航向角ψ：

采煤机3在初始位置，

取值初始装订值

采煤机3移动后，

取值实时更新值

7)数据处理单元8根据采煤机3俯仰角θ、滚动角γ，计算采煤机3航向角ψ，构造采煤机3姿态矩阵

步骤四、计算采煤机3位置信息

里程计2测量采煤机3坐标系上的位移S^b：

数据处理单元8接收并记录位移S^b，利用姿态矩阵

和S^b，计算采煤机3在导航坐标系的位移Sⁿ：

令

数据处理单元8计算采煤机3的实时经度、纬度和高度：

本发明的有益效果是：提供了一种采煤机3定位定姿装置及其方法，首先进行初始装订；然后，姿态测量装置1的转位机构4以设定的角速度转动，在转角0、

π和

时，数据处理单元8记录转位机构4、陀螺5、第一加速度计6、第二加速度计7的测量值，计算采煤机3航向角、俯仰角和滚动角，构造采煤机3姿态矩阵；里程计2测量采煤机3坐标系b上的位移，数据处理单元8利用采煤机3姿态矩阵将其转化为导航坐标系n上的位移，计算采煤机3的经度、纬度和高度，实现采煤机3的高精度定位定姿，使其满足综采工作面自动化、远程自动控制的需求，具有定位、定姿精度高、综采工作效率高及远程自动控制的优点。

附图说明

图1为本发明的采煤机定位定姿装置的示意图，其中：图1(a)为定位定姿装置安装示意图；图1(b)定位定姿装置结构框图；图1(c)为陀螺5、第一加速度计6、第二加速度计7在转位机构4上的安装示意图；图1(d)为定位定姿原理框图。

图2为坐标系示意图。

图3为采煤机定位定姿的工作流程图。

图中：1、姿态测量装置；2、里程计；3、采煤机；4、转位机构；5、陀螺；6、第一加速度计；7、第二加速度计；8、数据处理单元。

具体实施方式

下面结合附图对本发明做进一步详细说明。

参见图1，一种采煤机定位定姿装置，包括在采煤机3机身上安装的姿态测量装置1和采煤机3机轮上设置的里程计2；所述姿态测量装置1包括1个转位机构4，转位机构4上近轴心向外分别安装有可随转位机构4转动的陀螺5、第一加速度计6及与第一加速度计6夹角呈90度的对称位置安装的第二加速度计7；转位机构4、陀螺5、第一加速度计6、第二加速度计7和里程计2的信息分别传输至外接的数据处理单元8，数据处理单元8计算采煤机的姿态和位置。

基于上述的采煤机定位定姿装置的定位定姿方法，具体包括以下步骤：

步骤一、定义坐标系

1)采煤机3坐标系b，OX_bY_bZ_b：坐标系原点O固连在姿态测量装置1中心，X_b轴正向指向采煤机3前进方向，Y_b轴垂直于X_b轴向上，Z_b轴与X_b轴、Y_b轴构成右手坐标系，X_b轴、Y_b轴、Z_b轴构成前上右坐标系；

2)里程计2坐标系p与采煤机3坐标系b重合；

3)转位机构4坐标系m，OX_mY_mZ_m：采煤机3坐标系b绕Y_b轴旋转角度α而得的坐标系；当转动角度α＝0时，转位机构4坐标系m与采煤机3坐标系b重合；陀螺5测量轴与X_m轴重合，第一加速度计6测量轴与X_m轴重合，第二加速度计7测量轴与Z_m轴重合；

4)导航坐标系n，OX_nY_nZ_n：北天东地理坐标系，X_n轴指向地理北向，Y_n轴指向天向，Z_n轴指向地理东向；

5)导航坐标系n经过三次旋转后与采煤机3坐标系b重合，三次旋转的角度即为机身航向角ψ、俯仰角θ和滚动角γ；

步骤二、初始装订

姿态测量装置1的数据处理单元8接收初始装订信息：初始位置

其中λ,

h分别表示采煤机3的经度、纬度和高度；同时由于采煤机3初始处于停止状态，采煤机3初始北向速度、天向速度和东向速度为0，即V(0)＝[000]^T；

步骤三、计算采煤机3航向和姿态信息

数据处理单元8接收并记录转位机构4、陀螺5、第一加速度计6、第二加速度计7的测量信息，计算采煤机3俯仰角θ、滚动角γ和航向角ψ：

1)转位机构4连续匀速旋转，旋转角速度为ω；

2)数据处理单元8接收转位机构4测量的旋转角度α＝ω×t，α∈[0,2π)；

3)在α₁＝0，

α₃＝π，

时，数据处理单元8接收并记录陀螺5测量值ω_g(1)、ω_g(2)、ω_g(3)、ω_g(4)；

ω_g(i)＝cos(α_i)[cos(θ)cos(ψ)Ω_N+sin(θ)Ω_U]+sin(α_i)[cos(θ)sin(γ)Ω_U-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))Ω_N]

(1)

其中，Ω_N和Ω_U为地球自转角速度ω_ie的北向分量和天向分量，即

4)在α₁＝0，

α₃＝π，

时，数据处理单元8接收并记录第一加速度计6测量值f_a1(1)、f_a1(2)、f_a1(3)、f_a1(4)；第二加速度计7测量值f_a2(1)、f_a2(2)、f_a2(3)、f_a2(4)；下标a1表示第一加速度计6，下标a2表示第二加速度计7；

5)根据加速度计测量值,数据处理单元8计算采煤机3俯仰角θ和滚动角γ,计算过程如下:

f_a1(i)＝cos(α_i)[sin(θ)g]+sin(α_i)[cos(θ)sin(γ)g] (3)

其中，g表示重力加速度；

令A₁＝sin(θ)g，A₂＝cos(θ)sin(γ)g，

将公式(3)和(4)整理改写为，

F＝HA (5)

其中：

F＝[f_a1(1) f_a1(2) f_a1(3) f_a1(4) f_a2(1) f_a2(2) f_a2(3) f_a2(4)]^T

A＝[A₁ A₂ A₃ A₄]^T

求解公式(5)可得：

A＝(H^TH)H^TF (6)

根据A的定义，可知：

根据公式(6)的计算结果A和公式(7)，可得：

6)数据处理单元8根据陀螺5测量值、俯仰角θ和滚动角γ，计算采煤机3航向角ψ，计算过程如下：

令C₁＝cos(θ)cos(ψ)Ω_N+sin(θ)Ω_U，

C₂＝cos(θ)sin(γ)Ω_U-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))Ω_N

将公式(1)整理改写为：

求解可得：

根据C₁定义，可得采煤机3航向角ψ：

采煤机3在初始位置，

取值初始装订值

采煤机3移动后，

取值实时更新值

7)数据处理单元8根据采煤机3俯仰角θ、滚动角γ，计算采煤机3航向角ψ，构造采煤机3姿态矩阵

步骤四、计算采煤机3位置信息

里程计2测量采煤机3坐标系上的位移S^b：

数据处理单元8接收并记录位移S^b，利用姿态矩阵

和S^b，计算采煤机3在导航坐标系的位移Sⁿ：

令

计算采煤机3的实时经度、纬度和高度：

参见图3，本发明的工作原理为：

在采煤机3的采煤作业中，采煤机3沿轨道移动。首先进行初始装订；然后，姿态测量装置1的转位机构4以设定的角速度转动，在转角0、

π和

时，数据处理单元8记录陀螺5和第一加速度计6、第二加速度计7的测量值，计算采煤机3航向角、俯仰角和滚动角，构造采煤机3姿态矩阵；里程计2测量采煤机坐标系b上的位移，数据处理单元8利用采煤机3姿态矩阵将其转化为导航坐标系n上的位移，计算采煤机3的经度、纬度和高度，实现采煤机3的高精度定位定姿。

需要指出的是，本发明虽然以一种单陀螺+双加速度计+里程计的定位定姿装置方案作为具体实施实例，但本发明只需对步骤一至步骤四的解算方式稍作更改，同样适用于其它形式的采煤机定位装置。

显然，以上具体实施方式中实施例仅用于说明本发明的技术方案而非对其限制，尽管参照上述具体实施方式对本发明进行了详细说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求范围当中。

Claims (1)

1.一种采煤机定位定姿装置的定位定姿方法，其特征在于：使用的采煤机定位定姿装置，包括在采煤机(3)机身上安装的姿态测量装置(1)和采煤机(3)机轮上设置的里程计(2)；所述姿态测量装置(1)包括1个转位机构(4)，转位机构(4)上近轴心向外分别安装有可随转位机构(4)转动的陀螺(5)、第一加速度计(6)及与第一加速度计(6)夹角呈90度的对称位置安装的第二加速度计(7)；转位机构(4)、陀螺(5)、第一加速度计(6)、第二加速度计(7)和里程计(2)的信息分别传输至外接的数据处理单元(8)，数据处理单元(8)计算采煤机的姿态和位置；

基于上述采煤机定位定姿装置的定位定姿方法，具体包括以下步骤：

步骤一、定义坐标系

1)采煤机(3)坐标系b，OX_bY_bZ_b：坐标系原点O固连在姿态测量装置(1)中心，X_b轴正向指向采煤机(3)前进方向，Y_b轴垂直于X_b轴向上，Z_b轴与X_b轴、Y_b轴构成右手坐标系，X_b轴、Y_b轴、Z_b轴构成前上右坐标系；

2)里程计(2)坐标系p与采煤机(3)坐标系b重合；

3)转位机构(4)坐标系m，OX_mY_mZ_m：采煤机(3)坐标系b绕Y_b轴旋转角度α而得的坐标系；当转动角度α＝0时，转位机构(4)坐标系m与采煤机(3)坐标系b重合；陀螺5测量轴与X_m轴重合，第一加速度计(6)测量轴与X_m轴重合，第二加速度计(7)测量轴与Z_m轴重合；

4)导航坐标系n，OX_nY_nZ_n：北天东地理坐标系，X_n轴指向地理北向，Y_n轴指向天向，Z_n轴指向地理东向；

5)导航坐标系n经过三次旋转后与采煤机(3)坐标系b重合，三次旋转的角度即为机身航向角ψ、俯仰角θ和滚动角γ；

步骤二、初始装订

姿态测量装置(1)的数据处理单元(8)接收初始装订信息：初始位置

其中λ,

h分别表示采煤机(3)的经度、纬度和高度；同时由于采煤机(3)初始处于停止状态，采煤机(3)初始北向速度、天向速度和东向速度为0，即V(0)＝[0 0 0]^T；

步骤三、计算采煤机(3)航向和姿态信息

数据处理单元(8)接收并记录转位机构(4)、陀螺(5)、第一加速度计(6)、第二加速度计(7)的测量信息，计算采煤机(3)俯仰角θ、滚动角γ和航向角ψ：

1)转位机构(4)连续匀速旋转，旋转角速度为ω；

2)数据处理单元(8)接收转位机构(4)测量的旋转角度α＝ω×t，α∈[0,2π)；

3)在α₁＝0，

α₃＝π，

时，数据处理单元(8)接收并记录陀螺(5)测量值ω_g(1)、ω_g(2)、ω_g(3)、ω_g(4)；

ω_g(i)＝cos(α_i)[cos(θ)cos(ψ)Ω_N+sin(θ)Ω_U]+sin(α_i)[cos(θ)sin(γ)Ω_U-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))Ω_N] (1)

其中，Ω_N和Ω_U为地球自转角速度ω_ie的北向分量和天向分量，即

4)在α₁＝0，

α₃＝π，

时，数据处理单元(8)接收并记录第一加速度计(6)测量值f_a1(1)、f_a1(2)、f_a1(3)、f_a1(4)；第二加速度计7测量值f_a2(1)、f_a2(2)、f_a2(3)、f_a2(4)；下标a1表示第一加速度计6，下标a2表示第二加速度计7；

5)根据加速度计测量值,数据处理单元(8)计算采煤机(3)俯仰角θ和滚动角γ,计算过程如下:

f_a1(i)＝cos(α_i)[sin(θ)g]+sin(α_i)[cos(θ)sin(γ)g] (3)

其中，g表示重力加速度；

令A₁＝sin(θ)g，A₂＝cos(θ)sin(γ)g，

将公式(3)和(4)整理改写为：

F＝HA (5)

其中：

F＝[f_a1(1) f_a1(2) f_a1(3) f_a1(4) f_a2(1) f_a2(2) f_a2(3) f_a2(4)]^T

A＝[A₁ A₂ A₃ A₄]^T

求解公式(5)可得：

A＝(H^TH)H^TF (6)

根据A的定义，可知：

根据公式(6)的计算结果A和公式(7)，可得：

6)数据处理单元(8)根据陀螺(5)测量值、俯仰角θ和滚动角γ，计算采煤机(3)航向角ψ，计算过程如下：

令C₁＝cos(θ)cos(ψ)Ω_N+sin(θ)Ω_U，

C₂＝cos(θ)sin(γ)Ω_U-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))Ω_N

将公式(1)整理改写为：

求解可得：

根据C₁定义，可得采煤机(3)航向角ψ：

采煤机(3)在初始位置，

取值初始装订值

采煤机(3)移动后，

取值实时更新值

7)数据处理单元(8)根据采煤机(3)俯仰角θ、滚动角γ，计算采煤机(3)航向角ψ，构造采煤机(3)姿态矩阵

步骤四、计算采煤机(3)位置信息

里程计(2)测量采煤机(3)坐标系上的位移S^b：

数据处理单元(8)接收并记录位移S^b，利用姿态矩阵

和S^b，计算采煤机(3)在导航坐标系的位移Sⁿ：

令

根据下式

计算采煤机(3)的实时经度、纬度和高度。

Images