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

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

Info

Publication number
CN113340265A
CN113340265A CN202110681258.2A CN202110681258A CN113340265A CN 113340265 A CN113340265 A CN 113340265A CN 202110681258 A CN202110681258 A CN 202110681258A CN 113340265 A CN113340265 A CN 113340265A
Authority
CN
China
Prior art keywords
coal mining
mining machine
accelerometer
coordinate system
sin
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110681258.2A
Other languages
English (en)
Inventor
党宏涛
汤瑛琦
陶志豪
杨建业
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xijing University
Original Assignee
Xijing University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xijing University filed Critical Xijing University
Priority to CN202110681258.2A priority Critical patent/CN113340265A/zh
Publication of CN113340265A publication Critical patent/CN113340265A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G01C1/02—Theodolites
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C5/00—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，通过在采煤机机身上安装姿态测量装置和采煤机机轮上设置的里程计，数据处理单元对接收数据进行初始装订，然后构造采煤机姿态矩阵，计算采煤机的经度、纬度和高度，实现采煤机的高精度定位定姿，同时标定出了陀螺的常值漂移和加速度计的偏置，使其满足综采工作面自动化、远程自动控制的需求，具有定位和定姿精度高、综采工作效率高及远程自动控制的优点。

Description

一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法
技术领域
本发明属于采煤机定位定姿技术领域，具体涉及一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法。
背景技术
采煤机、液压支架和刮板输送机是井下综采工作面最重要的三种设备，相互配合完成割煤、运煤和支护工作。其中采煤机为主导设备，是综采工作面割煤和装煤的主要设备，是一种高集成度综采设备。采煤机在工作时，沿刮板输送机的轨道进行往复式割煤，液压支架支护顶板并推进工作面。为了实现综采工作面自动化、远程自动控制，需要对采煤机精确动态定位，而且，针对自动化采煤需求，需要同时对采煤机进行定姿，所以采煤机定位定姿技术是煤矿生产装备自动化的关键技术。煤矿综采工作面工况复杂，同时空间封闭，因此采煤机定位定姿是一个典型的复杂封闭环境下室内定位定姿问题。
目前采煤机所用的定位定姿方法主要包括捷联惯导法、红外定位法、超声波定位法、齿轮计数定位法、无线传感器网络定位法等。红外定位法易受粉尘影响，且存在固有定位盲区，定位精度不高，因此使用有局限性。超声波定位法由于工作面较长，信号失信严重，其定位精度不高，因此使用有局限性。齿轮计数定位法只能用于定位采煤机沿轨道方向的一维位置，不能满足三维定位需要。无线传感器网络定位法由于工作面环境复杂，无线定位数据不稳定，且锚节点随液压支架移动后会导致自身位置变化，需要更新锚节点位置信息，同时不能进行采煤机定姿，不能满足实时定位定姿需求。捷联惯导方法定位定姿精度误差随时间累积，长时间工作精度难以保障。
专利申请CN111879280A公开了一种采煤机定位定姿装置及方法，通过数据处理单元利用采煤机姿态矩阵将其转化为导航坐标系上的位移，计算采煤机的经度、纬度和高度，实现采煤机的定位定姿，定位、定姿精度及综采效率有了很大提升，但是未考虑陀螺的常值漂移和加速度计的偏置，对于姿态角的估计精度尚存在不足。
发明内容
为了克服上述现有技术存在的问题，本发明的目的是提供一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，充分考虑了陀螺的常值漂移和加速度计的偏置，使姿态角的估计精度有了很大提高，同时还能标定出陀螺的常值漂移和加速度计的偏置；具有定位定姿精度高、综采工作效率高及远程自动控制的优点。
为了实现上述目的，本发明采用如下的技术方案：
一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，具体包括以下步骤：
步骤一、在采煤机3机身上安装姿态测量装置1，采煤机3机轮上设置里程计2；
步骤二、利用步骤一安装的姿态测量装置1及里程计2定义坐标系；
步骤三、初始装订；
步骤四、计算采煤机3航向和姿态信息；
步骤五、计算采煤机3位置信息。
步骤一中所述姿态测量装置1包括转位机构4，转位机构4上近轴心向外分别安装有可随转位机构4转动的陀螺5、第一加速度计6及与第一加速度计6夹角呈90度的对应位置安装的第二加速度计7；转位机构4、陀螺5、第一加速度计6、第二加速度计7和里程计2的信息分别传输至外接的数据处理单元8，由数据处理单元8计算采煤机的姿态和位置。
步骤二所述定义坐标系的具体方法为：
定义采煤机坐标系b，OXbYbZb：坐标系原点O固连在姿态测量装置1中心，Xb轴正向指向采煤机3前进方向，Yb轴垂直于Xb轴向上，Zb轴与Xb轴、Yb轴构成右手坐标系，Xb轴、Yb轴、Zb轴构成前上右坐标系；
定义里程计2坐标系p与采煤机3坐标系b重合；
定义转位机构4坐标系m，OXmYmZm：采煤机3坐标系b绕Yb轴旋转角度α而得的坐标系；当转动角度α＝0时，转位机构4坐标系m与采煤机3坐标系b重合，陀螺5测量轴与Xm轴重合，第一加速度计6测量轴与Xm轴重合，第二加速度计7测量轴与Zm轴重合；
定义导航坐标系n，OXnYnZn：北天东地理坐标系，Xn轴指向地理北向，Yn轴指向天向，Zn轴指向地理东向；
定义导航坐标系n经过三次旋转后与采煤机3坐标系b重合，三次旋转的角度即为机身航向角ψ、俯仰角θ和滚动角γ。
步骤三所述的初始装订具体为：
通过数据处理单元8接收初始装订信息：初始位置P(0)＝[λ(0) φ(0) h(0)]T，其中λ，φ，h分别表示采煤机3的经度、纬度和高度；同时由于采煤机3初始处于停止状态，采煤机3的初始北向速度、天向速度和东向速度均为0，即V(0)＝[0 0 0]T。
步骤四所述的计算采煤机3航向和姿态信息具体方法为：
数据处理单元8接收并记录转位机构4、陀螺5、第一加速度计6、第二加速度计7的信息，计算采煤机3俯仰角θ、滚动角γ和航向角ψ：
1)姿态测量装置1的转位机构4连续匀速旋转，旋转角速度为ω；
2)数据处理单元8接收转位机构4测量的旋转角度α＝ω×t，α∈[0，2π)；
3)在α1＝0，
Figure BDA0003122646510000041
Figure BDA0003122646510000042
时，数据处理单元8接收并记录陀螺5测量值ωg(1)、ωg(2)、ωg(3)、ωg(4)、ωg(5)、ωg(6)、ωg(7)、ωg(8)
设陀螺的常值漂移为ε：
ωg(i)＝ε+cos(αi)[cos(θ)cos(ψ)ΩN+sin(θ)ΩU]+sin(αi)[cos(θ)sin(γ)ΩU-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ΩN] (1)
其中，ΩN和ΩU为地球自转角速度ωie的北向分量和天向分量，即
ΩN＝ωiecos(φ)
ΩU＝ωiesin(φ) (2)
4)在α1＝0，
Figure BDA0003122646510000051
Figure BDA0003122646510000052
时，数据处理单元8接收并记录第一加速度计6测量值fa1(1)、fa1(2)、fa1(3)、fa1(4)、fa1(5)、fa1(6)、fa1(7)、fa1(8)；第二加速度计7测量值fa2(1)、fa2(2)、fa2(3)、fa2(4)、fa2(5)、fa2(6)、fa2(7)、fa2(8)；下标a1表示第一加速度计6，下标a2表示第二加速度计7；
5)根据加速度计测量值，数据处理单元8计算采煤机3俯仰角θ和滚动角γ，计算过程如下：
设第一加速度计6的偏置为
Figure BDA0003122646510000053
第二加速度计7的偏置为
Figure BDA0003122646510000054
Figure BDA0003122646510000055
Figure BDA0003122646510000056
其中，g表示重力加速度；
令A1＝sin(θ)g，A2＝cos(θ)sin(γ)g，
Figure BDA0003122646510000057
Figure BDA0003122646510000058
将公式(3)和(4)整理改写为，
F＝HA (5)
其中，
F＝[fa1(1) fa1(2) … fa1(8) fa2(1) fa2(2) … fa2(8)]T
Figure BDA0003122646510000059
Figure BDA00031226465100000510
Figure BDA00031226465100000511
求解公式(5)可得，
A＝(HTH)HTF (6)
至此，第一加速度计6的偏置
Figure BDA0003122646510000061
和第二加速度计的偏置
Figure BDA0003122646510000062
已经得出，下面进行θ和γ的计算，根据A的定义，可知：
Figure BDA0003122646510000063
根据公式(6)的计算结果A和公式(7)，可得：
Figure BDA0003122646510000064
Figure BDA0003122646510000065
6)数据处理单元8根据陀螺5测量值、俯仰角θ和滚动角γ，计算采煤机3航向角ψ，计算过程如下：
令C1＝cos(θ)cos(ψ)ΩN+sin(θ)ΩU，
C2＝cos(θ)sin(γ)ΩU-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ΩN
将公式(1)整理改写为，
Figure BDA0003122646510000066
求解可得：
Figure BDA0003122646510000071
根据C1定义，可得采煤机3航向角ψ：
Figure BDA0003122646510000072
采煤机3在初始位置，φ取值初始装订值φ(0)；采煤机3移动后，φ取值实时更新值φ(k)；
7)数据处理单元8根据采煤机3俯仰角θ、滚动角γ，计算采煤机3航向角ψ，构造采煤机3姿态矩阵
Figure BDA0003122646510000073
Figure BDA0003122646510000074
步骤五所述的计算采煤机3位置信息具体方法为：
里程计2测量采煤机3坐标系上的位移Sb：
Figure BDA0003122646510000075
数据处理单元8接收并记录位移Sb，利用姿态矩阵
Figure BDA0003122646510000076
和Sb，计算采煤机3在导航坐标系的位移Sn：
Figure BDA0003122646510000077
令
Figure BDA0003122646510000078
数据处理单元8计算采煤机3的实时经度、纬度和高度：
Figure BDA0003122646510000079
Figure BDA0003122646510000081
Figure BDA0003122646510000082
本发明的有益效果是：提供了一种采煤机3高精度定位定姿装置及其方法，首先进行初始装订；然后，姿态测量装置1的转位机构4以设定的角速度转动，在转角0、
Figure BDA0003122646510000083
π、
Figure BDA0003122646510000084
时，数据处理单元8记录转位机构4、陀螺5、第一加速度计6、第二加速度计7的测量值，计算采煤机3航向角、俯仰角和滚动角及陀螺5的常值漂移、第一加速度计6和第二加速度计7的偏置，构造采煤机3姿态矩阵；里程计2测量采煤机3坐标系b上的位移，数据处理单元8利用采煤机3姿态矩阵将其转化为导航坐标系n上的位移，计算采煤机3的经度、纬度和高度，实现采煤机3的高精度定位定姿，使其满足综采工作面自动化、远程自动控制的需求，具有定位、定姿精度高、综采工作效率高及远程自动控制的优点。
附图说明
图1为本发明的工作流程图。
图2为本发明的安装示意图。
图3为陀螺5、第一加速度计6、第二加速度计7在转位机构4上的安装示意图。
图4为坐标系示意图。
图中：1、姿态测量装置；2、里程计；3、采煤机；4、转位机构；5、陀螺；6、第一加速度计；7、第二加速度计；8、数据处理单元。
具体实施方式
下面结合附图对本发明做进一步详细说明。
参见图1，一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，具体包括以下步骤：
步骤一、在采煤机3机身上安装姿态测量装置1，采煤机3机轮上设置里程计2；参见图2；
步骤二、利用步骤一安装的姿态测量装置1及里程计2定义坐标系；参见图2；
步骤三、初始装订；
步骤四、计算采煤机3航向和姿态信息；
步骤五、计算采煤机3位置信息。
步骤一中所述姿态测量装置1包括转位机构4，转位机构4上近轴心向外分别安装有可随转位机构4转动的陀螺5、第一加速度计6及与第一加速度计6夹角呈90度的对应位置安装的第二加速度计7；转位机构4、陀螺5、第一加速度计6、第二加速度计7和里程计2的信息分别传输至外接的数据处理单元8，数据处理单元8计算采煤机的姿态和位置；参见图3。
步骤二所述定义坐标系的具体方法为：
参见图4，定义采煤机坐标系b，OXbYbZb：坐标系原点O固连在姿态测量装置1中心，Xb轴正向指向采煤机3前进方向，Yb轴垂直于Xb轴向上，Zb轴与Xb轴、Yb轴构成右手坐标系，Xb轴、Yb轴、Zb轴构成前上右坐标系；
定义里程计2坐标系p与采煤机3坐标系b重合；
定义转位机构4坐标系m，OXmYmZm：采煤机3坐标系b绕Yb轴旋转角度α而得的坐标系；当转动角度α＝0时，转位机构4坐标系m与采煤机3坐标系b重合，陀螺5测量轴与Xm轴重合，第一加速度计6测量轴与Xm轴重合，第二加速度计7测量轴与Zm轴重合；
定义导航坐标系n，OXnYnZn：北天东地理坐标系，Xn轴指向地理北向，Yn轴指向天向，Zn轴指向地理东向；
定义导航坐标系n经过三次旋转后与采煤机3坐标系b重合，三次旋转的角度即为机身航向角ψ、俯仰角θ和滚动角γ。
步骤三所述的初始装订具体为：
通过数据处理单元8接收初始装订信息：初始位置P(0)＝[λ(0) φ(0) h(0)]T，其中λ，φ，h分别表示采煤机3的经度、纬度和高度；同时由于采煤机3初始处于停止状态，采煤机3的初始北向速度、天向速度和东向速度均为0，即V(0)＝[0 0 0]T。
步骤四所述的计算采煤机3航向和姿态信息具体方法为：
数据处理单元8接收并记录转位机构4、陀螺5、第一加速度计6、第二加速度计7的信息，计算采煤机3俯仰角θ、滚动角γ和航向角ψ：
1)姿态测量装置1的转位机构4连续匀速旋转，旋转角速度为ω；
2)数据处理单元8接收转位机构4测量的旋转角度α＝ω×t，α∈[0，2π)；
3)在α1＝0，
Figure BDA0003122646510000101
Figure BDA0003122646510000102
时，数据处理单元8接收并记录陀螺5测量值ωg(1)、ωg(2)、ωg(3)、ωg(4)、ωg(5)、ωg(6)、ωg(7)、ωg(8)
设陀螺的常值漂移为ε：
ωg(i)＝ε+cos(αi)[cos(θ)cos(ψ)ΩN+sin(θ)ΩU]+sin(αi)[cos(θ)sin(γ)ΩU-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ΩN] (1)
其中，ΩN和ΩU为地球自转角速度ωie的北向分量和天向分量，即
ΩN＝ωiecos(φ)
ΩU＝ωiesin(φ) (2)
4)在α1＝0，
Figure BDA0003122646510000111
Figure BDA0003122646510000112
时，数据处理单元8接收并记录第一加速度计6测量值fa1(1)、fa1(2)、fa1(3)、fa1(4)、fa1(5)、fa1(6)、fa1(7)、fa1(8)；第二加速度计7测量值fa2(1)、fa2(2)、fa2(3)、fa2(4)、fa2(5)、fa2(6)、fa2(7)、fa2(8)；下标a1表示第一加速度计6，下标a2表示第二加速度计7；
5)根据加速度计测量值，数据处理单元8计算采煤机3俯仰角θ和滚动角γ，计算过程如下：
设第一加速度计6的偏置为
Figure BDA0003122646510000113
第二加速度计7的偏置为
Figure BDA0003122646510000114
Figure BDA0003122646510000115
Figure BDA0003122646510000116
其中，g表示重力加速度；
令A1＝sin(θ)g，A2＝cos(θ)sin(γ)g，
Figure BDA0003122646510000117
Figure BDA0003122646510000118
将公式(3)和(4)整理改写为，
F＝HA (5)
其中，
F＝[fa1(1) fa1(2) … fa1(8) fa2(1) fa2(2) … fa2(8)]T
Figure BDA0003122646510000121
Figure BDA0003122646510000122
Figure BDA0003122646510000123
求解公式(5)可得，
A＝(HTH)HTF(6)
至此，第一加速度计6的偏置
Figure BDA0003122646510000124
和第二加速度计的偏置
Figure BDA0003122646510000125
已经得出，下面进行θ和γ的计算，根据A的定义，可知：
Figure BDA0003122646510000126
根据公式(6)的计算结果A和公式(7)，可得：
Figure BDA0003122646510000127
Figure BDA0003122646510000131
6)数据处理单元8根据陀螺5测量值、俯仰角θ和滚动角γ，计算采煤机3航向角ψ，计算过程如下：
令C1＝cos(θ)cos(ψ)ΩN+sin(θ)ΩU，
C2＝cos(θ)sin(γ)ΩU-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ΩN
将公式(1)整理改写为，
Figure BDA0003122646510000132
求解可得：
Figure BDA0003122646510000133
根据C1定义，可得采煤机3航向角ψ：
Figure BDA0003122646510000134
采煤机3在初始位置，φ取值初始装订值φ(0)；采煤机3移动后，φ取值实时更新值φ(k)；
7)数据处理单元8根据采煤机3俯仰角θ、滚动角γ，计算采煤机3航向角ψ，构造采煤机3姿态矩阵
Figure BDA0003122646510000135
Figure BDA0003122646510000136
步骤五所述的计算采煤机3位置信息具体方法为：
里程计2测量采煤机3坐标系上的位移Sb：
Figure BDA0003122646510000141
数据处理单元8接收并记录位移Sb，利用姿态矩阵
Figure BDA0003122646510000142
和Sb，计算采煤机3在导航坐标系的位移Sn：
Figure BDA0003122646510000143
令
Figure BDA0003122646510000144
数据处理单元8计算采煤机3的实时经度、纬度和高度：
Figure BDA0003122646510000145
Figure BDA0003122646510000146
Figure BDA0003122646510000147
本发明的工作原理为：在采煤机3的采煤作业中，采煤机3沿轨道移动。首先进行初始装订；然后，姿态测量装置1的转位机构4以设定的角速度转动，在转角0、
Figure BDA0003122646510000148
π、
Figure BDA0003122646510000149
时，数据处理单元8记录陀螺5和第一加速度计6、第二加速度计7的测量值，计算采煤机3航向角、俯仰角和滚动角及陀螺5的常值漂移、第一加速度计6和第二加速度计7的偏置，构造采煤机3姿态矩阵；里程计2测量采煤机坐标系b上的位移，数据处理单元8利用采煤机3姿态矩阵将其转化为导航坐标系n上的位移，计算采煤机3的经度、纬度和高度，实现采煤机3的高精度定位定姿。
显然，以上具体实施方式中实施例仅用于说明本发明的技术方案而非对其限制，尽管参照上述具体实施方式对本发明进行了详细说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求范围当中。

Claims (6)

1.一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，其特征在于：具体包括以下步骤：
步骤一、在采煤机(3)机身上安装姿态测量装置(1)，采煤机(3)机轮上设置里程计(2)；
步骤二、利用步骤一安装的姿态测量装置(1)及里程计(2)定义坐标系；
步骤三、初始装订；
步骤四、计算采煤机(3)航向和姿态信息；
步骤五、计算采煤机(3)位置信息。
2.根据权利要求1所述的一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，其特征在于：步骤一中所述姿态测量装置(1)包括转位机构(4)，转位机构(4)上近轴心向外分别安装有可随转位机构(4)转动的陀螺(5)、第一加速度计(6)及与第一加速度计(6)夹角呈90度的对应位置安装的第二加速度计(7)；转位机构(4)、陀螺(5)、第一加速度计(6)、第二加速度计(7)和里程计(2)的信息分别传输至外接的数据处理单元(8)，数据处理单元(8)计算采煤机的姿态和位置。
3.根据权利要求1和2所述的一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，其特征在于：步骤二所述定义坐标系的具体方法为：
定义采煤机坐标系b，OXbYbZb：坐标系原点O固连在姿态测量装置(1)中心，Xb轴正向指向采煤机(3)前进方向，Yb轴垂直于Xb轴向上，Zb轴与Xb轴、Yb轴构成右手坐标系，Xb轴、Yb轴、Zb轴构成前上右坐标系；
定义里程计(2)坐标系p与采煤机(3)坐标系b重合；
定义转位机构(4)坐标系m，OXmYmZm：采煤机(3)坐标系b绕Yb轴旋转角度α而得的坐标系；当转动角度α＝0时，转位机构(4)坐标系m与采煤机(3)坐标系b重合，陀螺(5)测量轴与Xm轴重合，第一加速度计(6)测量轴与Xm轴重合，第二加速度计(7)测量轴与Zm轴重合；
定义导航坐标系n，OXnYnZn：北天东地理坐标系，Xn轴指向地理北向，Yn轴指向天向，Zn轴指向地理东向；
定义导航坐标系n经过三次旋转后与采煤机(3)坐标系b重合，三次旋转的角度即为机身航向角ψ、俯仰角θ和滚动角γ。
4.根据权利要求1和2所述的一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，其特征在于：步骤三所述的初始装订具体为：
通过数据处理单元(8)接收初始装订信息：初始位置P(0)＝[λ(0) φ(0) h(0)]T，其中λ，φ，h分别表示采煤机(3)的经度、纬度和高度；同时由于采煤机(3)初始处于停止状态，采煤机(3)的初始北向速度、天向速度和东向速度均为0，即V(0)＝[0 0 0]T。
5.根据权利要求1和2所述的一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，其特征在于：步骤四所述的计算采煤机(3)航向和姿态信息具体方法为：
数据处理单元(8)接收并记录转位机构(4)、陀螺(5)、第一加速度计(6)、第二加速度计(7)的信息，计算采煤机(3)俯仰角θ、滚动角γ和航向角ψ：
1)姿态测量装置(1)的转位机构(4)连续匀速旋转，旋转角速度为ω；
2)数据处理单元(8)接收转位机构(4)测量的旋转角度α＝ω×t，α∈[0，2π)；
3)在α1＝0，
Figure FDA0003122646500000031
α5＝π，
Figure FDA0003122646500000032
Figure FDA0003122646500000033
时，数据处理单元(8)接收并记录陀螺(5)测量值ωg(1)、ωg(2)、ωg(3)、ωg(4)、ωg(5)、ωg(6)、ωg(7)、ωg(8)
设陀螺的常值漂移为ε：
ωg(i)＝ε+cos(αi)[cos(θ)cos(ψ)ΩN+sin(θ)ΩU]+sin(αi)[cos(θ)sin(γ)ΩU-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ΩN]
(1)
其中，ΩN和ΩU为地球自转角速度ωie的北向分量和天向分量，即
ΩN＝ωiecos(φ)
ΩU＝ωiesin(φ)
(2)
4)在α1＝0，
Figure FDA0003122646500000041
α5＝π，
Figure FDA0003122646500000042
Figure FDA0003122646500000043
时，数据处理单元(8)接收并记录第一加速度计(6)测量值fa1(1)、fa1(2)、fa1(3)、fa1(4)、fa1(5)、fa1(6)、fa1(7)、fa1(8)；第二加速度计(7)测量值fa2(1)、fa2(2)、fa2(3)、fa2(4)、fa2(5)、fa2(6)、fa2(7)、fa2(8)；下标a1表示第一加速度计(6)，下标a2表示第二加速度计(7)；
5)根据加速度计测量值，数据处理单元(8)计算采煤机(3)俯仰角θ和滚动角γ，计算过程如下：
设第一加速度计6的偏置为
Figure FDA0003122646500000051
第二加速度计7的偏置为
Figure FDA0003122646500000052
Figure FDA0003122646500000053
Figure FDA0003122646500000054
其中，g表示重力加速度；
令A1＝sin(θ)g，A2＝cos(θ)sin(γ)g，
Figure FDA0003122646500000055
Figure FDA0003122646500000056
将公式(3)和(4)整理改写为，
F＝HA (5)
其中，
F＝[fa1(1) fa1(2) … fa1(8) fa2(1) fa2(2) … fa2(8)]T
Figure FDA0003122646500000057
Figure FDA0003122646500000058
Figure FDA0003122646500000059
求解公式(5)可得，
A＝(HTH)HTF (6)
至此，第一加速度计(6)的偏置
Figure FDA0003122646500000061
和第二加速度计(7)的偏置
Figure FDA0003122646500000062
已经得出，下面进行θ和γ的计算，根据A的定义，可知：
Figure FDA0003122646500000063
根据公式(6)的计算结果A和公式(7)，可得：
Figure FDA0003122646500000064
Figure FDA0003122646500000065
6)数据处理单元(8)根据陀螺(5)测量值、俯仰角θ和滚动角γ，计算采煤机(3)航向角ψ，计算过程如下：
令C1＝cos(θ)cos(ψ)ΩN+sin(θ)ΩU，
C2＝cos(θ)sin(γ)ΩU-(sin(θ)sin(γ)cos(ψ)+cos(γ)sin(ψ))ΩN
将公式(1)整理改写为，
Figure FDA0003122646500000071
求解可得：
Figure FDA0003122646500000072
根据C1定义，可得采煤机(3)航向角ψ：
Figure FDA0003122646500000073
采煤机(3)在初始位置，φ取值初始装订值φ(0)；采煤机(3)移动后，φ取值实时更新值φ(k)；
7)数据处理单元(8)根据采煤机(3)俯仰角θ、滚动角γ，计算采煤机(3)航向角ψ，构造采煤机(3)姿态矩阵
Figure FDA0003122646500000074
Figure FDA0003122646500000075
6.根据权利要求1和2所述的一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法，其特征在于：步骤五所述的计算采煤机(3)位置信息具体方法为：
里程计(2)测量采煤机(3)坐标系上的位移Sb：
Figure FDA0003122646500000081
数据处理单元(8)接收并记录位移Sb，利用姿态矩阵
Figure FDA0003122646500000082
和Sb，计算采煤机(3)在导航坐标系的位移Sn：
Figure FDA0003122646500000083
令
Figure FDA0003122646500000084
数据处理单元(8)计算采煤机(3)的实时经度、纬度和高度：
Figure FDA0003122646500000085
Figure FDA0003122646500000086
Figure FDA0003122646500000087
CN202110681258.2A 2021-06-18 2021-06-18 一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法 Pending CN113340265A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110681258.2A CN113340265A (zh) 2021-06-18 2021-06-18 一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110681258.2A CN113340265A (zh) 2021-06-18 2021-06-18 一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法

Publications (1)

Publication Number Publication Date
CN113340265A true CN113340265A (zh) 2021-09-03

Family

ID=77477701

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110681258.2A Pending CN113340265A (zh) 2021-06-18 2021-06-18 一种在线补偿惯性仪表误差的综采煤机轨道定位测姿方法

Country Status (1)

Country Link
CN (1) CN113340265A (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110132309A (zh) * 2019-06-05 2019-08-16 西京学院 一种采煤机摇臂惯性/视觉组合定姿装置标定方法
CN110702109A (zh) * 2019-06-05 2020-01-17 西京学院 一种采煤机惯性导航/无线传感器网络组合定位方法
CN111879280A (zh) * 2020-08-10 2020-11-03 西京学院 一种采煤机定位定姿装置及方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110132309A (zh) * 2019-06-05 2019-08-16 西京学院 一种采煤机摇臂惯性/视觉组合定姿装置标定方法
CN110702109A (zh) * 2019-06-05 2020-01-17 西京学院 一种采煤机惯性导航/无线传感器网络组合定位方法
CN111879280A (zh) * 2020-08-10 2020-11-03 西京学院 一种采煤机定位定姿装置及方法

Similar Documents

Publication Publication Date Title
CN110702109B (zh) 一种采煤机惯性导航/无线传感器网络组合定位方法
CN101514899B (zh) 基于单轴旋转的光纤陀螺捷联惯性导航系统误差抑制方法
CN110631574B (zh) 一种惯性/里程计/rtk多信息融合方法
CN109540130A (zh) 一种连采机惯性导航定位定向方法
CN106246168A (zh) 一种近钻头钻具姿态随钻测量装置及测量方法
CN111307133B (zh) 一种竖井垂直度测量方法、竖井姿态检测设备及存储介质
CN101701825A (zh) 高精度激光陀螺单轴旋转惯性导航系统
CA2691034A1 (en) Reducing error contributions to gyroscopic measurements from a wellbore survey system
CN111207743B (zh) 基于编码器与惯性设备紧耦合实现厘米级精确定位的方法
CN201955092U (zh) 一种基于地磁辅助的平台式惯性导航装置
CN110887505A (zh) 一种冗余式惯性测量单元实验室标定方法
CN112378399B (zh) 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法
CN102071924B (zh) 一种陀螺测斜仪全方位连续测量方法
CN102798399A (zh) 基于双轴转动方案的捷联惯导系统误差抑制方法
CN113236363B (zh) 开采设备导航定位方法、系统、设备及可读存储介质
CN103256943A (zh) 一种在单轴旋转捷联惯导系统中刻度因数误差的补偿方法
CN114739425A (zh) 基于rtk-gnss及全站仪的采煤机定位标定系统及应用方法
CN114753841A (zh) 采煤机姿态修正方法及装置
CN111323011A (zh) 一种采煤机机身与摇臂协同定位装置及定位方法
CN111551164B (zh) 速率偏频激光陀螺寻北仪航向效应误差的补偿方法
CN101012745A (zh) 一种油气井井眼轨迹的测量方法
CN103940445A (zh) 一种单轴旋转惯导系统惯性器件误差补偿方法
CN110631573A (zh) 一种惯性/里程计/全站仪多信息融合方法
CN111879280B (zh) 一种采煤机定位定姿装置及方法
CN114001757A (zh) 一种磁力计零偏校准方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210903