一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法

1.本发明属于采煤机定位定姿技术领域，具体涉及一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法。


背景技术：

2.采煤机、液压支架和刮板输送机是井下综采工作面最重要的三种设备，相互配合完成割煤、运煤和支护工作。其中采煤机为主导设备，是综采工作面割煤和装煤的主要设备，是一种高集成度综采设备。采煤机在工作时，沿刮板输送机的轨道进行往复式割煤，液压支架支护顶板并推进工作面。为了实现综采工作面自动化、远程自动控制，需要对采煤机精确动态定位，而且，针对自动化采煤需求，需要同时对采煤机进行定姿，所以采煤机定位定姿技术是煤矿生产装备自动化的关键技术。煤矿综采工作面工况复杂，同时空间封闭，因此采煤机定位定姿是一个典型的复杂封闭环境下室内定位定姿问题。
3.目前采煤机所用的定位定姿方法主要包括捷联惯导法、红外定位法、超声波定位法、齿轮计数定位法、无线传感器网络定位法等。红外定位法易受粉尘影响，且存在固有定位盲区，定位精度不高，因此使用有局限性。超声波定位法由于工作面较长，信号失信严重，其定位精度不高，因此使用有局限性。齿轮计数定位法只能用于定位采煤机沿轨道方向的一维位置，不能满足三维定位需要。无线传感器网络定位法由于工作面环境复杂，无线定位数据不稳定，且锚节点随液压支架移动后会导致自身位置变化，需要更新锚节点位置信息，同时不能进行采煤机定姿，不能满足实时定位定姿需求。捷联惯导方法定位定姿精度误差随时间累积，长时间工作精度难以保障。
4.专利申请cn111879280a公开了一种采煤机定位定姿装置及方法，通过数据处理单元利用采煤机姿态矩阵将其转化为导航坐标系上的位移，计算采煤机的经度、纬度和高度，实现采煤机的定位定姿，定位、定姿精度及综采效率有了很大提升，但是未考虑陀螺的常值漂移和加速度计的偏置，对于姿态角的估计精度尚存在不足。


技术实现要素：

5.为了克服上述现有技术存在的问题，本发明的目的是提供一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，充分考虑了陀螺的常值漂移和加速度计的偏置，使姿态角的估计精度有了很大提高，同时还能标定出陀螺的常值漂移和加速度计的偏置；具有定位定姿精度高、综采工作效率高及远程自动控制的优点。
6.为了实现上述目的，本发明采用如下的技术方案：
7.一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，具体包括以下步骤：
8.步骤一、在采煤机3机身上安装姿态测量装置1，采煤机3机轮上设置里程计2；
9.步骤二、利用步骤一安装的姿态测量装置1及里程计2定义坐标系；
10.步骤三、初始装订；
11.步骤四、计算采煤机3航向和姿态信息；
12.步骤五、计算采煤机3位置信息。
13.步骤一中所述姿态测量装置1包括转位机构4，转位机构4上近轴心向外分别安装有可随转位机构4转动的陀螺5、第一加速度计6及与第一加速度计6夹角呈90度的对应位置安装的第二加速度计7；转位机构4、陀螺5、第一加速度计6、第二加速度计7和里程计2的信息分别传输至外接的数据处理单元8，由数据处理单元8计算采煤机的姿态和位置。
14.步骤二所述定义坐标系的具体方法为：
15.定义采煤机坐标系b，ox
b
y
b
z
b
：坐标系原点o固连在姿态测量装置1中心，x
b
轴正向指向采煤机3前进方向，y
b
轴垂直于x
b
轴向上，z
b
轴与x
b
轴、y
b
轴构成右手坐标系，x
b
轴、y
b
轴、z
b
轴构成前上右坐标系；
16.定义里程计2坐标系p与采煤机3坐标系b重合；
17.定义转位机构4坐标系m，ox
m
y
m
z
m
：采煤机3坐标系b绕y
b
轴旋转角度α而得的坐标系；当转动角度α＝0时，转位机构4坐标系m与采煤机3坐标系b重合，陀螺5测量轴与x
m
轴重合，第一加速度计6测量轴与x
m
轴重合，第二加速度计7测量轴与z
m
轴重合；
18.定义导航坐标系n，ox
n
y
n
z
n
：北天东地理坐标系，x
n
轴指向地理北向，y
n
轴指向天向，z
n
轴指向地理东向；
19.定义导航坐标系n经过三次旋转后与采煤机3坐标系b重合，三次旋转的角度即为机身航向角ψ、俯仰角θ和滚动角γ。
20.步骤三所述的初始装订具体为：
21.通过数据处理单元8接收初始装订信息：初始位置p(0)＝[λ(0) φ(0) h(0)]
t
，其中λ，φ，h分别表示采煤机3的经度、纬度和高度；同时由于采煤机3初始处于停止状态，采煤机3的初始北向速度、天向速度和东向速度均为0，即v(0)＝[0 0 0]
t
。
[0022]
步骤四所述的计算采煤机3航向和姿态信息具体方法为：
[0023]
数据处理单元8接收并记录转位机构4、陀螺5、第一加速度计6、第二加速度计7的信息，计算采煤机3俯仰角θ、滚动角γ和航向角ψ：
[0024]
1)姿态测量装置1的转位机构4连续匀速旋转，旋转角速度为ω；
[0025]
2)数据处理单元8接收转位机构4测量的旋转角度α＝ω
×
t，α∈[0，2π)；
[0026]
3)在α1＝0，3)在α1＝0，时，数据处理单元8接收并记录陀螺5测量值ω
g
(1)、ω
g
(2)、ω
g
(3)、ω
g
(4)、ω
g
(5)、ω
g
(6)、ω
g
(7)、ω
g
(8)
[0027]
设陀螺的常值漂移为ε：
[0028]
ω
g
(i)＝ε+cos(α
i
)[cos(θ)cos(ψ)ω
n
+sin(θ)ω
u
]+sin(α
i
)[cos(θ)sin(γ)ω
u
‑
(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ω
n
]
ꢀꢀ
(1)
[0029]
其中，ω
n
和ω
u
为地球自转角速度ω
ie
的北向分量和天向分量，即
[0030]
ω
n
＝ω
ie
cos(φ)
[0031]
ω
u
＝ω
ie
sin(φ)
ꢀꢀ
(2)
[0032]
4)在α1＝0，4)在α1＝0，时，数据处理单元8接收并记录第一加速度计6测量值f
a1
(1)、f
a1
(2)、f
a1
(3)、f
a1
(4)、f
a1
(5)、
f
a1
(6)、f
a1
(7)、f
a1
(8)；第二加速度计7测量值f
a2
(1)、f
a2
(2)、f
a2
(3)、f
a2
(4)、f
a2
(5)、f
a2
(6)、f
a2
(7)、f
a2
(8)；下标a1表示第一加速度计6，下标a2表示第二加速度计7；
[0033]
5)根据加速度计测量值，数据处理单元8计算采煤机3俯仰角θ和滚动角γ，计算过程如下：
[0034]
设第一加速度计6的偏置为第二加速度计7的偏置为第二加速度计7的偏置为
[0035][0036]
其中，g表示重力加速度；
[0037]
令a1＝sin(θ)g，a2＝cos(θ)sin(γ)g，＝cos(θ)sin(γ)g，将公式(3)和(4)整理改写为，
[0038]
f＝ha
ꢀꢀ
(5)
[0039]
其中，
[0040]
f＝[f
a1
(1) f
a1
(2)
ꢀ…ꢀ
f
a1
(8) f
a2
(1) f
a2
(2)
ꢀ…ꢀ
f
a2
(8)]
t
[0041][0042][0043][0044]
求解公式(5)可得，
[0045]
a＝(h
t
h)h
t
f
ꢀꢀ
(6)
[0046]
至此，第一加速度计6的偏置和第二加速度计的偏置已经得出，下面进行θ和γ的计算，根据a的定义，可知：
[0047][0048]
根据公式(6)的计算结果a和公式(7)，可得：
[0049][0050][0051]
6)数据处理单元8根据陀螺5测量值、俯仰角θ和滚动角γ，计算采煤机3航向角ψ，计算过程如下：
[0052]
令c1＝cos(θ)cos(ψ)ω
n
+sin(θ)ω
u
，
[0053]
c2＝cos(θ)sin(γ)ω
u
‑
(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ω
n
[0054]
将公式(1)整理改写为，
[0055][0056]
求解可得：
[0057][0058]
根据c1定义，可得采煤机3航向角ψ：
[0059][0060]
采煤机3在初始位置，φ取值初始装订值φ(0)；采煤机3移动后，φ取值实时更新值φ(k)；
[0061]
7)数据处理单元8根据采煤机3俯仰角θ、滚动角γ，计算采煤机3航向角ψ，构造采煤机3姿态矩阵
[0062][0063]
步骤五所述的计算采煤机3位置信息具体方法为：
[0064]
里程计2测量采煤机3坐标系上的位移s
b
：
[0065][0066]
数据处理单元8接收并记录位移s
b
，利用姿态矩阵和s
b
，计算采煤机3在导航坐标系的位移s
n
：
[0067][0068]
令数据处理单元8计算采煤机3的实时经度、纬度和高度：
[0069][0070][0071][0072]
本发明的有益效果是：提供了一种采煤机3高精度定位定姿装置及其方法，首先进行初始装订；然后，姿态测量装置1的转位机构4以设定的角速度转动，在转角0、π、时，数据处理单元8记录转位机构4、陀螺5、第一加速度计6、第二加速度计7的测量值，计算采煤机3航向角、俯仰角和滚动角及陀螺5的常值漂移、第一加速度计6和第二加速度计7的偏置，构造采煤机3姿态矩阵；里程计2测量采煤机3坐标系b上的位移，数据处理单元8利用采煤机3姿态矩阵将其转化为导航坐标系n上的位移，计算采煤机3的经度、纬度和高度，实现采煤机3的高精度定位定姿，使其满足综采工作面自动化、远程自动控制的需求，具有定位、定姿精度高、综采工作效率高及远程自动控制的优点。
附图说明
[0073]
图1为本发明的工作流程图。
[0074]
图2为本发明的安装示意图。
[0075]
图3为陀螺5、第一加速度计6、第二加速度计7在转位机构4上的安装示意图。
[0076]
图4为坐标系示意图。
[0077]
图中：1、姿态测量装置；2、里程计；3、采煤机；4、转位机构；5、陀螺；6、第一加速度计；7、第二加速度计；8、数据处理单元。
具体实施方式
[0078]
下面结合附图对本发明做进一步详细说明。
[0079]
参见图1，一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，具体包括以下步骤：
[0080]
步骤一、在采煤机3机身上安装姿态测量装置1，采煤机3机轮上设置里程计2；参见图2；
[0081]
步骤二、利用步骤一安装的姿态测量装置1及里程计2定义坐标系；参见图2；
[0082]
步骤三、初始装订；
[0083]
步骤四、计算采煤机3航向和姿态信息；
[0084]
步骤五、计算采煤机3位置信息。
[0085]
步骤一中所述姿态测量装置1包括转位机构4，转位机构4上近轴心向外分别安装有可随转位机构4转动的陀螺5、第一加速度计6及与第一加速度计6夹角呈90度的对应位置安装的第二加速度计7；转位机构4、陀螺5、第一加速度计6、第二加速度计7和里程计2的信息分别传输至外接的数据处理单元8，数据处理单元8计算采煤机的姿态和位置；参见图3。
[0086]
步骤二所述定义坐标系的具体方法为：
[0087]
参见图4，定义采煤机坐标系b，ox
b
y
b
z
b
：坐标系原点o固连在姿态测量装置1中心，x
b
轴正向指向采煤机3前进方向，y
b
轴垂直于x
b
轴向上，z
b
轴与x
b
轴、y
b
轴构成右手坐标系，x
b
轴、y
b
轴、z
b
轴构成前上右坐标系；
[0088]
定义里程计2坐标系p与采煤机3坐标系b重合；
[0089]
定义转位机构4坐标系m，ox
m
y
m
zm：采煤机3坐标系b绕y
b
轴旋转角度α而得的坐标系；当转动角度α＝0时，转位机构4坐标系m与采煤机3坐标系b重合，陀螺5测量轴与x
m
轴重合，第一加速度计6测量轴与x
m
轴重合，第二加速度计7测量轴与z
m
轴重合；
[0090]
定义导航坐标系n，ox
n
y
n
z
n
：北天东地理坐标系，x
n
轴指向地理北向，y
n
轴指向天向，z
n
轴指向地理东向；
[0091]
定义导航坐标系n经过三次旋转后与采煤机3坐标系b重合，三次旋转的角度即为机身航向角ψ、俯仰角θ和滚动角γ。
[0092]
步骤三所述的初始装订具体为：
[0093]
通过数据处理单元8接收初始装订信息：初始位置p(0)＝[λ(0) φ(0) h(0)]
t
，其中λ，φ，h分别表示采煤机3的经度、纬度和高度；同时由于采煤机3初始处于停止状态，采煤机3的初始北向速度、天向速度和东向速度均为0，即v(0)＝[0 0 0]
t
。
[0094]
步骤四所述的计算采煤机3航向和姿态信息具体方法为：
[0095]
数据处理单元8接收并记录转位机构4、陀螺5、第一加速度计6、第二加速度计7的信息，计算采煤机3俯仰角θ、滚动角γ和航向角ψ：
[0096]
1)姿态测量装置1的转位机构4连续匀速旋转，旋转角速度为ω；
[0097]
2)数据处理单元8接收转位机构4测量的旋转角度α＝ω
×
t，α∈[0，2π)；
[0098]
3)在α1＝0，＝0，时，数据处理单元8接收并记录陀螺5测量值ω
g
(1)、ω
g
(2)、ω
g
(3)、ω
g
(4)、ω
g
(5)、ω
g
(6)、ω
g
(7)、ω
g
(8)
[0099]
设陀螺的常值漂移为ε：
[0100]
ω
g
(i)＝ε+cos(α
i
)[cos(θ)cos(ψ)ω
n
+sin(θ)ω
u
]+sin(α
i
)[cos(θ)sin(γ)ω
u
‑
(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ω
n
]
ꢀꢀ
(1)
[0101]
其中，ω
n
和ω
u
为地球自转角速度ω
ie
的北向分量和天向分量，即
[0102]
ω
n
＝ω
ie
cos(φ)
[0103]
ω
u
＝ω
ie
sin(φ)
ꢀꢀ
(2)
[0104]
4)在α1＝0，＝0，时，数据处理单元8接收并记录第一加速度计6测量值f
a1
(1)、f
a1
(2)、f
a1
(3)、f
a1
(4)、f
a1
(5)、f
a1
(6)、f
a1
(7)、f
a1
(8)；第二加速度计7测量值f
a2
(1)、f
a2
(2)、f
a2
(3)、f
a2
(4)、f
a2
(5)、f
a2
(6)、f
a2
(7)、f
a2
(8)；下标a1表示第一加速度计6，下标a2表示第二加速度计7；
[0105]
5)根据加速度计测量值，数据处理单元8计算采煤机3俯仰角θ和滚动角γ，计算过程如下：
[0106]
设第一加速度计6的偏置为第二加速度计7的偏置为第二加速度计7的偏置为
[0107][0108]
其中，g表示重力加速度；
[0109]
令a1＝sin(θ)g，a2＝cos(θ)sin(γ)g，＝cos(θ)sin(γ)g，将公式(3)和(4)整理改写为，
[0110]
f＝ha
ꢀꢀ
(5)
[0111]
其中，
[0112]
f＝[f
a1
(1) f
a1
(2)
ꢀ…ꢀ
f
a1
(8) f
a2
(1) f
a2
(2)
ꢀ…ꢀ
f
a2
(8)]
t
[0113][0114][0115][0116]
求解公式(5)可得，
[0117]
a＝(h
t
h)h
t
f(6)
[0118]
至此，第一加速度计6的偏置和第二加速度计的偏置已经得出，下面进行θ和γ的计算，根据a的定义，可知：
[0119][0120]
根据公式(6)的计算结果a和公式(7)，可得：
[0121][0122][0123]
6)数据处理单元8根据陀螺5测量值、俯仰角θ和滚动角γ，计算采煤机3航向角ψ，计算过程如下：
[0124]
令c1＝cos(θ)cos(ψ)ω
n
+sin(θ)ω
u
，
[0125]
c2＝cos(θ)sin(γ)ω
u
‑
(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ω
n
[0126]
将公式(1)整理改写为，
[0127][0128]
求解可得：
[0129][0130]
根据c1定义，可得采煤机3航向角ψ：
[0131][0132]
采煤机3在初始位置，φ取值初始装订值φ(0)；采煤机3移动后，φ取值实时更新值φ(k)；
[0133]
7)数据处理单元8根据采煤机3俯仰角θ、滚动角γ，计算采煤机3航向角ψ，构造采
煤机3姿态矩阵
[0134][0135]
步骤五所述的计算采煤机3位置信息具体方法为：
[0136]
里程计2测量采煤机3坐标系上的位移s
b
：
[0137][0138]
数据处理单元8接收并记录位移s
b
，利用姿态矩阵和s
b
，计算采煤机3在导航坐标系的位移s
n
：
[0139][0140]
令数据处理单元8计算采煤机3的实时经度、纬度和高度：
[0141][0142][0143][0144]
本发明的工作原理为：在采煤机3的采煤作业中，采煤机3沿轨道移动。首先进行初始装订；然后，姿态测量装置1的转位机构4以设定的角速度转动，在转角0、π、时，数据处理单元8记录陀螺5和第一加速度计6、第二加速度计7的测量值，计算采煤机3航向角、俯仰角和滚动角及陀螺5的常值漂移、第一加速度计6和第二加速度计7的偏置，构造采煤机3姿态矩阵；里程计2测量采煤机坐标系b上的位移，数据处理单元8利用采煤机3姿态矩阵将其转化为导航坐标系n上的位移，计算采煤机3的经度、纬度和高度，实现采煤机3的高精度定位定姿。
[0145]
显然，以上具体实施方式中实施例仅用于说明本发明的技术方案而非对其限制，尽管参照上述具体实施方式对本发明进行了详细说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求范围当中。