CN104990549B - 一种采煤机‑液压支架相对定位的方法及装置 - Google Patents

一种采煤机‑液压支架相对定位的方法及装置 Download PDF

Info

Publication number
CN104990549B
CN104990549B CN201510330344.3A CN201510330344A CN104990549B CN 104990549 B CN104990549 B CN 104990549B CN 201510330344 A CN201510330344 A CN 201510330344A CN 104990549 B CN104990549 B CN 104990549B
Authority
CN
China
Prior art keywords
mrow
msub
mtd
mover
msubsup
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.)
Expired - Fee Related
Application number
CN201510330344.3A
Other languages
English (en)
Other versions
CN104990549A (zh
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.)
China University of Mining and Technology CUMT
Original Assignee
China University of Mining and Technology CUMT
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 China University of Mining and Technology CUMT filed Critical China University of Mining and Technology CUMT
Priority to CN201510330344.3A priority Critical patent/CN104990549B/zh
Publication of CN104990549A publication Critical patent/CN104990549A/zh
Application granted granted Critical
Publication of CN104990549B publication Critical patent/CN104990549B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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

Landscapes

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

Abstract

本发明公开了一种采煤机‑液压支架相对定位装置和方法，装置包括红外传感器、轴编码定位装置、惯性导航定位装置和中央系统处理单元，惯性导航定位装置、轴编码定位装置和红外传感器分别电连接于中央系统处理单元；惯性导航定位装置安装在采煤机的重心位置，轴编码定位装置放置在采煤机行走部行星减速器的低速输出轴上，红外传感器包括安装在采煤机中心位置的红外线发射装置以及安装在各个液压支架上的复数个红外线接收装置。有益效果是通过红外传感器定位、轴编码器定位以及惯性导航定位三种定位策略融合对采煤机进行定位，提高定位的可靠性和准确性。

Description

一种采煤机-液压支架相对定位的方法及装置
技术领域
本发明涉及一种能够精确定位采煤机的方法及装置，尤其是一种采煤机-液压支架相对定位的方法及装置。
背景技术
综采自动化的核心是“三机”协同动作，其他两种设备的自动控制需要依靠采煤机的工作位置及工作状态等相关信息，综采自动化研究的首要任务是研究适合复杂地质条件的采煤机智能监控系统，而其中最为关键的就是采煤机的自主精确定位技术。目前，红外线采煤机位置检测装置和轴编码器定位装置为井下工业现场最常用的两种定位方式，单独运用一种定位方式都存在一定的问题，比如在使用红外线采煤机位置检测装置时，会出现由于现场一些物品等遮挡红外信号，使其产生误判；在使用轴编码器定位时，采煤机的行走轮同刮板输送机的销排啮合不是严格意义上的齿轮传动，只能算是一种“类齿轮齿排传动”，由于轮齿同销排啮合时，有较大的空隙这样会在计算过程中形成累积误差，该误差会随着时间累积而变大，因此采用这些单一的方法对采煤机的定位可靠性和准确性不高。
发明内容
针对上述现有技术存在的问题，本发明提供一种采煤机-液压支架相对定位的方法及装置，通过红外传感器定位、轴编码器定位以及惯性导航定位三种定位策略融合对采煤机进行定位，提高定位的可靠性和准确性。
为了实现上述目的，本发明采用的技术方案是：一种采煤机-液压支架相对定位装置，包括红外传感器、轴编码定位装置、惯性导航定位装置和中央系统处理单元，惯性导航定位装置、轴编码定位装置和红外传感器分别电连接于中央系统处理单元；惯性导航定位装置安装在采煤机的重心位置，轴编码定位装置放置在采煤机牵引部行星减速器的低速输出轴上，红外传感器包括安装在采煤机中心位置的红外线发射装置以及安装在各个液压支架上的复数个红外线接收装置。
一种采煤机-液压支架相对定位方法，包括以下步骤：
红外线发射装置随着采煤机行走连续的发射红外信号，位于各液压支架上的红外线接收装置会接收到强度不等的红外信号，靠近采煤机中心位置的红外线接收装置信号强度最强，确定该信号最强的红外线接收装置所在的液压支架为采煤机正对的液压支架；
轴编码定位装置根据行走部齿轮的传动比计算出采煤机的行走距离，通过对行走距离进行计算求出该采煤机的位置；
通过惯性导航定位装置上的三轴陀螺仪和三轴加速度计测得角速度和加速度后，发送至中央系统处理单元，求得相应的位置参数，再进行计算得到采煤机的位置；
分别通过红外线发射装置、轴编码定位装置和惯性导航定位装置确定采煤机的位置后，进行比较处理，最终确定采煤机的精确定位位置。
红外传感器的定位方法为：
通过安装在采煤机上的红外线发射装置发射信号，安装在液压支架上的红外线接收装置接收信号，随着采煤机行走连续的发射红外信号，位于附近的红外线接收装置将会接收到强度不等的红外信号，将信号发送至中央系统处理单元，根据信号强度确定出目前采煤机所在位置[w,w+a]，信号最强的接收装置所在的液压支架即为红外传感器确定的采煤机正对的液压支架；
轴编码器定位装置的定位方法为：
设输出轴到行走轮的齿轮传动比为i，轴编码器旋转一周生成的脉冲数目为N，某一时刻所累计的脉冲数目为n(ti)，行走轮的分度圆半径为R，采煤机的当前运行的距离为：
结合液压支架的宽度以及液压支架在工作面上的分布情况进行采煤机和液压支架的相对定位，设工作面液压支架宽度为β，相邻液压支架之间的平均距离为Δβ，液压支架从采煤机行走开始端编号1～n，采煤机正对的液压支架的支架号nc(ti)为j，采煤机行走距离同液压支架编号之间的关系为：
S(ti)＝Δβ·[nc(ti)-1]+β·nc(ti)
由此得出采煤机正对的支架号
惯性导航定位装置的定位方法为：
通过惯性导航定位装置中的三轴陀螺仪和三轴加速度计测得对应的角速度和加速度后，发送到中央系统处理单元，通求得相应的位置参数，随后计算后得出采煤机的当前位置；
由采煤机机身上安装的三轴加速度计直接测得沿采煤机机身坐标系的比例分量设采煤机机身坐标系为固联在采煤机机身中部的一点Oxbybzb，三个坐标轴的初始方位同平台坐标系重合，实现由采煤机机身至平台坐标系的坐标转换的采煤机姿态矩阵T应满足如下的坐标转换方程：
通过实时解算算法获得采煤机的姿态矩阵T，
进而计算得到平台坐标系内的测量参数
在平台坐标系下对三个方向的加速度进行两次积分计算，从而获得采煤机机身安装点在惯性下的位移情况；通过四元数法进行采煤机姿态矩阵的实时求解，设采煤机机身坐标系相对平台坐标系做定轴转动的转动四元数为：
Q(t)＝q0(t)+q1(t)ib+q2(t)jb+q3(t)
三轴陀螺仪测得角速度ω，Q随时间变化而变化，Q的实时求解通过解四元数微分方程
得到，然后通过最佳正交化纠偏得到：
由此，计算得到采煤机姿态矩阵T的实时值为：
采煤机机身坐标系下加速度的坐标转换：
在平台坐标系下对三个方向的加速度再加上重力加速度：
一次积分求出对应的速度
二次积分求出对应的位移
其中各速度位移的初始值均为0；
计算当前采煤机的位置k，结合液压支架的宽度以及液压支架在工作面上的分布情况进行采煤机和液压支架的相对定位，设工作面液压支架宽度为β，相邻液压支架之间的平均距离为Δβ，液压支架从采煤机行走开始端编号1～n，采煤机正对的液压支架的支架号nc(ti)为k，采煤机行走距离同支架号之间的关系为：
S(ti)＝Δβ·[nc(ti)-1]+β·nc(ti)
由此得出采煤机正对的液压支架号
对三种定位方式进行比较：计算j、k是否都在[w,w+a]内，若只有j在范围内，那么此时采煤机的位置就是在j处，若只有k在范围内，那么此时采煤机的位置就是在k处，若j、k都在范围内，且j＝k，此时位置为j或者k,若j≠k则采煤机位置取[w,w+a]中间架号w+a/2；若j、k都不在范围内，那么采煤机位置取[w,w+a]中间架号w+a/2。
本发明的有益效果是：解决现有采煤机方法定位不准确的问题，通过几种定位技术的融合模型，增强了融合校正系统的准确性、可靠性和稳定性。
附图说明
图1为本发明的原理框图；
图2为本发明的流程图；
图3为惯性导航定位装置系统原理图；
具体实施方式
下面结合附图对本发明作进一步说明。
如图1至图3所示：本发明的采煤机-液压支架相对定位装置，包括红外传感器、轴编码定位装置、惯性导航定位装置和中央系统处理单元，惯性导航定位装置、轴编码定位装置和红外传感器分别电连接于中央系统处理单元；惯性导航定位装置安装在采煤机的重心位置，轴编码定位装置放置在采煤机牵引部行星减速器的低速输出轴上，红外传感器包括安装在采煤机中心位置的红外线发射装置以及安装在各个液压支架上的复数个红外线接收装置。
一种采煤机-液压支架相对定位方法，包括以下步骤：
红外线发射装置随着采煤机行走连续的发射红外信号，位于各液压支架上的红外线接收装置会接收到强度不等的红外信号，靠近采煤机中心位置的红外线接收装置信号强度最强，确定该信号最强的红外线接收装置所在的液压支架为采煤机正对的液压支架；
轴编码定位装置根据行走部齿轮的传动比计算出采煤机的行走距离，通过对行走距离进行计算求出该采煤机的位置；
通过惯性导航定位装置上的三轴陀螺仪和三轴加速度计测得角速度和加速度后，发送至中央系统处理单元，求得相应的位置参数，再进行计算得到采煤机的位置；
分别通过红外线发射装置、轴编码定位装置和惯性导航定位装置确定采煤机的位置后，进行比较处理，最终确定采煤机的精确定位位置。
红外传感器的定位方法为：
通过安装在采煤机上的红外线发射装置发射信号，安装在液压支架上的红外线接收装置接收信号，随着采煤机行走连续的发射红外信号，位于附近的红外线接收装置将会接收到强度不等的红外信号，将信号发送至中央系统处理单元，根据信号强度确定出目前采煤机所在位置[w,w+a]，信号最强的接收装置所在的液压支架即为红外传感器确定的采煤机正对的液压支架；
轴编码器定位装置的定位方法为：
设输出轴到行走轮的齿轮传动比为i，轴编码器旋转一周生成的脉冲数目为N，某一时刻所累计的脉冲数目为n(ti)，行走轮的分度圆半径为R，采煤机的当前运行的距离为：
结合液压支架的宽度以及液压支架在工作面上的分布情况进行采煤机和液压支架的相对定位，设工作面液压支架宽度为β，相邻液压支架之间的平均距离为Δβ，液压支架从采煤机行走开始端编号1～n，采煤机正对的液压支架的支架号nc(ti)为j，采煤机行走距离同液压支架编号之间的关系为：
S(ti)＝Δβ·[nc(ti)-1]+β·nc(ti)
由此得出采煤机正对的支架号
惯性导航定位装置的主要原理就是利用固定在运载体上的三轴陀螺仪和三轴加速度计分别测量采煤机相对惯性空间的3个转动角速度和3个线性加速度沿采煤机坐标系的分量，经过坐标变换，把加速度信息转化为沿惯性坐标系的加速度。分别经过运算获得采煤机的位移、速度以及姿态等各种导航信息；
惯性导航定位装置的定位方法为：
通过惯性导航定位装置中的三轴陀螺仪和三轴加速度计测得对应的角速度和加速度后，发送到中央系统处理单元，通求得相应的位置参数，随后计算后得出采煤机的当前位置，将信息传至上位机或其他控制单元；
由采煤机机身上安装的三轴加速度计直接测得沿采煤机机身坐标系的比例分量设采煤机机身坐标系为固联在采煤机机身中部的一点Oxbybzb，三个坐标轴的初始方位同平台坐标系重合，实现由采煤机机身至平台坐标系的坐标转换的采煤机姿态矩阵T应满足如下的坐标转换方程：
通过实时解算算法获得采煤机的姿态矩阵T，
进而计算得到平台坐标系内的测量参数
此时加速度是去除了重力加速度的影响，然后在平台坐标系下对三个方向的加速度再加上重力加速度后进行两次积分计算，从而获得采煤机机身安装点在惯性下的位移情况；通过四元数法进行采煤机姿态矩阵的实时求解，设采煤机机身坐标系相对平台坐标系做定轴转动的转动四元数为：
Q(t)＝q0(t)+q1(t)ib+q2(t)jb+q3(t)
三轴陀螺仪测得角速度ω，Q随时间变化而变化，Q的实时求解通过解四元数微分方程
得到，然后通过最佳正交化纠偏得到：
由此，计算得到采煤机姿态矩阵T的实时值为：
采煤机机身坐标系下加速度的坐标转换：
在平台坐标系下对三个方向的加速度再加上重力加速度：
一次积分求出对应的速度
二次积分求出对应的位移
其中各速度位移的初始值均为0；
计算当前采煤机的位置k，结合液压支架的宽度以及液压支架在工作面上的分布情况进行采煤机和液压支架的相对定位，设工作面液压支架宽度为β，相邻液压支架之间的平均距离为Δβ，液压支架从采煤机行走开始端编号1～n，采煤机正对的液压支架的支架号nc(ti)为k，采煤机行走距离同支架号之间的关系为：
S(ti)＝Δβ·[nc(ti)-1]+β·nc(ti)
由此得出采煤机正对的液压支架号
对三种定位方式进行比较：计算j、k是否都在[w,w+a]内，若只有j在范围内，那么此时采煤机的位置就是在j处，若只有k在范围内，那么此时采煤机的位置就是在k处，若j、k都在范围内，且j＝k，此时位置为j或者k,若j≠k则采煤机位置取[w,w+a]中间架号w+a/2；若j、k都不在范围内，那么采煤机位置取[w,w+a]中间架号w+a/2。

Claims (1)

1.一种采煤机-液压支架相对定位装置的定位方法，其特征在于，所述定位装置包括：红外传感器、轴编码定位装置、惯性导航定位装置和中央系统处理单元，惯性导航定位装置、轴编码定位装置和红外传感器分别电连接于中央系统处理单元；惯性导航定位装置安装在采煤机的重心位置，轴编码定位装置放置在采煤机行走部行星减速器的低速输出轴上，红外传感器包括安装在采煤机中心位置的红外线发射装置以及安装在各个液压支架上的复数个红外线接收装置，所述定位方法包括以下步骤：
红外线发射装置随着采煤机行走连续的发射红外信号，位于各液压支架上的红外线接收装置会接收到强度不等的红外信号，靠近采煤机中心位置的红外线接收装置信号强度最强，确定该信号最强的红外线接收装置所在的液压支架为采煤机正对的液压支架；
轴编码定位装置根据行走部齿轮的传动比计算出采煤机的行走距离，通过对行走距离进行计算求出该采煤机的位置；
通过惯性导航定位装置中的三轴陀螺仪和三轴加速度计测得对应的角速度和加速度后，发送至中央系统处理单元，求得相应的位置参数，再进行计算得到采煤机的位置；
分别通过红外线发射装置、轴编码定位装置和惯性导航定位装置确定采煤机的位置后，进行比较处理，最终确定采煤机的精确定位位置；
其中，红外传感器的定位方法具体为：
通过安装在采煤机上的红外线发射装置发射信号，安装在液压支架上的红外线接收装置接收信号，随着采煤机行走连续的发射红外信号，位于附近的红外线接收装置将会接收到强度不等的红外信号，将信号发送至中央系统处理单元，根据信号强度确定出目前采煤机所在位置[w,w+a]，信号最强的接收装置所在的液压支架即为红外传感器确定的采煤机正对的液压支架；
轴编码器定位装置的定位方法具体为：
设输出轴到行走轮的齿轮传动比为i，轴编码器旋转一周生成的脉冲数目为N，某一时刻所累计的脉冲数目为n(ti)，行走轮的分度圆半径为R，采煤机的当前运行的距离为：
<mrow> <mi>S</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mn>2</mn> <mi>&amp;pi;</mi> <mi>R</mi> <mi>n</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mi>N</mi> <mi>i</mi> </mrow> </mfrac> </mrow>
结合液压支架的宽度以及液压支架在工作面上的分布情况进行采煤机和液压支架的相对定位，设工作面液压支架宽度为β，相邻液压支架之间的平均距离为Δβ，液压支架从采煤机行走开始端编号1～n，采煤机正对的液压支架的支架号nc(ti)为j，采煤机行走距离同液压支架编号之间的关系为：
S(ti)＝Δβ·[nc(ti)-1]+β·nc(ti)
由此得出采煤机正对的支架号
<mrow> <msub> <mi>n</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mfrac> <mrow> <mi>S</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>&amp;beta;</mi> </mrow> <mrow> <mi>&amp;Delta;</mi> <mi>&amp;beta;</mi> <mo>+</mo> <mi>&amp;beta;</mi> </mrow> </mfrac> <mo>&amp;rsqb;</mo> <mo>;</mo> </mrow>
惯性导航定位装置的定位方法具体为：
通过惯性导航定位装置中的三轴陀螺仪和三轴加速度计测得对应的角速度和加速度后，发送到中央系统处理单元，通求得相应的位置参数，随后计算后得出采煤机的当前位置；
由采煤机机身上安装的三轴加速度计直接测得沿采煤机机身坐标系的比例分量设采煤机机身坐标系为固联在采煤机机身中部的一点Oxbybzb，三个坐标轴的初始方位同平台坐标系重合，实现由采煤机机身至平台坐标系的坐标转换的采煤机姿态矩阵T应满足如下的坐标转换方程：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mi>p</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mi>p</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>z</mi> <mi>p</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mi>T</mi> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mi>b</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mi>b</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>z</mi> <mi>b</mi> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
通过实时解算算法获得采煤机的姿态矩阵T，
<mrow> <mi>T</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>T</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>T</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>T</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
进而计算得到平台坐标系内的测量参数
在平台坐标系下对三个方向的加速度进行两次积分计算，从而获得采煤机机身安装点在惯性下的位移情况；通过四元数法进行采煤机姿态矩阵的实时求解，设采煤机机身坐标系相对平台坐标系做定轴转动的转动四元数为：
Q(t)＝q0(t)+q1(t)ib+q2(t)jb+q3(t)
三轴陀螺仪测得角速度ω，Q随时间变化而变化，Q的实时求解通过解四元数微分方程
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mover> <mrow> <msub> <mi>q</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <mrow> <msub> <mi>q</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <mrow> <msub> <mi>q</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <mrow> <msub> <mi>q</mi> <mn>3</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>2</mn> </mtd> </mtr> </mtable> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>x</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>y</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>z</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>x</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>z</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>y</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>y</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>z</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>x</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>z</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>y</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>b</mi> <mi>x</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mn>3</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
得到，然后通过最佳正交化纠偏得到：
<mrow> <mover> <mi>Q</mi> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <msub> <mi>q</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msub> <mi>i</mi> <mi>b</mi> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msub> <mi>j</mi> <mi>b</mi> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msub> <mi>k</mi> <mi>b</mi> </msub> </mrow> <msqrt> <mrow> <msub> <mi>q</mi> <mn>0</mn> </msub> <msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mfrac> <mo>=</mo> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msub> <mi>i</mi> <mi>b</mi> </msub> <mo>+</mo> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msub> <mi>j</mi> <mi>b</mi> </msub> <mo>+</mo> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msub> <mi>k</mi> <mi>b</mi> </msub> </mrow>
由此，计算得到采煤机姿态矩阵T的实时值为：
<mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msup> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <msup> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <msup> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msup> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <msup> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <msup> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mn>2</mn> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msup> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <msup> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <msup> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>~</mo> </mover> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>
采煤机机身坐标系下加速度的坐标转换：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>f</mi> <mi>x</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>f</mi> <mi>y</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>f</mi> <mi>z</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mi>T</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>f</mi> <mi>x</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>f</mi> <mi>y</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>f</mi> <mi>z</mi> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
在平台坐标系下对三个方向的加速度再加上重力加速度：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mover> <mrow> <msubsup> <mi>V</mi> <mi>x</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <mrow> <msubsup> <mi>V</mi> <mi>y</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <mrow> <msubsup> <mi>V</mi> <mi>z</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>f</mi> <mi>x</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>f</mi> <mi>y</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>f</mi> <mi>z</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mi>g</mi> </mtd> </mtr> </mtable> </mfenced> </mrow>
一次积分求出对应的速度
<mrow> <msubsup> <mi>V</mi> <mi>x</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>V</mi> <mi>x</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <mover> <mrow> <msubsup> <mi>V</mi> <mi>x</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> <mi>d</mi> <mi>t</mi> </mrow> 2
<mrow> <msubsup> <mi>V</mi> <mi>y</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>V</mi> <mi>y</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <mover> <mrow> <msubsup> <mi>V</mi> <mi>y</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> <mi>d</mi> <mi>t</mi> </mrow>
<mrow> <msubsup> <mi>V</mi> <mi>z</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>V</mi> <mi>z</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <mover> <mrow> <msubsup> <mi>V</mi> <mi>z</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;CenterDot;</mo> </mover> <mi>d</mi> <mi>t</mi> </mrow>
二次积分求出对应的位移
<mrow> <msubsup> <mi>S</mi> <mi>x</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>S</mi> <mi>x</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <msubsup> <mi>V</mi> <mi>x</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>t</mi> </mrow>
<mrow> <msubsup> <mi>S</mi> <mi>y</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>S</mi> <mi>y</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <msubsup> <mi>V</mi> <mi>y</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>t</mi> </mrow>
<mrow> <msubsup> <mi>S</mi> <mi>z</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>S</mi> <mi>z</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mo>&amp;Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <msubsup> <mi>V</mi> <mi>z</mi> <mi>p</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>d</mi> <mi>t</mi> </mrow>
其中各速度位移的初始值均为0；
计算当前采煤机的位置k，结合液压支架的宽度以及液压支架在工作面上的分布情况进行采煤机和液压支架的相对定位，设工作面液压支架宽度为β，相邻液压支架之间的平均距离为Δβ，液压支架从采煤机行走开始端编号1～n，采煤机正对的液压支架的支架号nc(ti)为k，采煤机行走距离同支架号之间的关系为：
S(ti)＝Δβ·[nc(ti)-1]+β·nc(ti)
由此得出采煤机正对的液压支架号
<mrow> <msub> <mi>n</mi> <mi>c</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mfrac> <mrow> <mi>S</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>&amp;beta;</mi> </mrow> <mrow> <mi>&amp;Delta;</mi> <mi>&amp;beta;</mi> <mo>+</mo> <mi>&amp;beta;</mi> </mrow> </mfrac> <mo>&amp;rsqb;</mo> </mrow>
对三种定位方式进行比较：计算j、k是否都在[w，w+a]内，若只有j在范围内，那么此时采煤机的位置就是在j处，若只有k在范围内，那么此时采煤机的位置就是在k处，若j、k都在范围内，且j＝k，此时位置为j或者k,若j≠k则采煤机位置取[w,w+a]中间架号w+a/2；若j、k都不在范围内，那么采煤机位置取[w,w+a]中间架号w+a/2。
CN201510330344.3A 2015-06-15 2015-06-15 一种采煤机‑液压支架相对定位的方法及装置 Expired - Fee Related CN104990549B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510330344.3A CN104990549B (zh) 2015-06-15 2015-06-15 一种采煤机‑液压支架相对定位的方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510330344.3A CN104990549B (zh) 2015-06-15 2015-06-15 一种采煤机‑液压支架相对定位的方法及装置

Publications (2)

Publication Number Publication Date
CN104990549A CN104990549A (zh) 2015-10-21
CN104990549B true CN104990549B (zh) 2017-12-15

Family

ID=54302391

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510330344.3A Expired - Fee Related CN104990549B (zh) 2015-06-15 2015-06-15 一种采煤机‑液压支架相对定位的方法及装置

Country Status (1)

Country Link
CN (1) CN104990549B (zh)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105371871B (zh) * 2015-12-02 2018-01-02 中国矿业大学 井下采煤机捷联惯导系统的组合初始对准系统及对准方法
CN106401585A (zh) * 2016-09-13 2017-02-15 陕西陕煤黄陵矿业有限公司 一种基于采煤机和支架协同控制的记忆割煤控制方法
DE102017213148B4 (de) * 2017-07-31 2020-01-23 Carl Freudenberg Kg Gleitringdichtungsanordnung eines hydrodynamischen Retarders sowie hydrodynamischer Retarder
CN107907078B (zh) * 2017-10-24 2020-12-11 北京天地玛珂电液控制系统有限公司 一种综采工作面集成传感器
CN107976192B (zh) * 2017-11-16 2021-03-23 太原理工大学 一种综采工作面采运装备的姿态求解与预测方法
CN108661637A (zh) * 2018-03-26 2018-10-16 太重煤机有限公司 用于采煤机位置定位的感知控制方法
CN108731669B (zh) * 2018-04-13 2022-03-04 深圳一清创新科技有限公司 定位终端
CN109826660B (zh) * 2019-01-18 2021-01-08 中国矿业大学 一种液压支架远程定位通讯错误支架的系统和方法
CN109931107B (zh) 2019-03-14 2020-05-19 中国矿业大学 一种液压支架与采煤机截割部干涉保护装置与方法
CN110989580A (zh) * 2019-11-25 2020-04-10 国网上海市电力公司 一种隧道内电缆敷设的输送机的定位控制系统
CN111003577A (zh) * 2019-11-25 2020-04-14 国网上海市电力公司 一种融合轴编码器定位和红外传感器定位的惯性导航系统
CN111472841B (zh) * 2020-03-05 2021-11-05 天地科技股份有限公司 一种综采工作面设备群位姿统一方法
CN111442759B (zh) * 2020-03-05 2023-10-31 天地科技股份有限公司 一种综采工作面设备位姿统一监测系统
CN111323044A (zh) * 2020-04-14 2020-06-23 中国矿业大学(北京) 掘进机二维位移测量装置及测量方法
CN111426315A (zh) * 2020-05-14 2020-07-17 淮北矿业股份有限公司 一种采煤机定位装置及方法
CN112033399A (zh) * 2020-09-08 2020-12-04 太原理工大学 一种采煤机多信号融合的定位方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4192632A (en) * 1977-07-16 1980-03-11 Gewerkschaft Eisenhutte Westfalia Support systems for mineral mining installations
CN102914304A (zh) * 2012-10-31 2013-02-06 天津华宁电子有限公司 一种基于can总线技术的采煤机位置检测方法
CN103410512A (zh) * 2013-08-15 2013-11-27 中国矿业大学 融合地质环境信息的采煤机绝对定位装置与方法
CN103742140A (zh) * 2013-12-26 2014-04-23 中国矿业大学 一种精确定位的采煤机及其定位方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4192632A (en) * 1977-07-16 1980-03-11 Gewerkschaft Eisenhutte Westfalia Support systems for mineral mining installations
CN102914304A (zh) * 2012-10-31 2013-02-06 天津华宁电子有限公司 一种基于can总线技术的采煤机位置检测方法
CN103410512A (zh) * 2013-08-15 2013-11-27 中国矿业大学 融合地质环境信息的采煤机绝对定位装置与方法
CN103742140A (zh) * 2013-12-26 2014-04-23 中国矿业大学 一种精确定位的采煤机及其定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
一种采用捷联惯导的采煤机动态定位方法;樊启高等;《煤炭学报》;20111031;第36卷(第10期);第1758-1761页 *
红外线采煤机位置检测装置的研究;李俊士等;《机械工程与自动化》;20131231;第143-147页 *

Also Published As

Publication number Publication date
CN104990549A (zh) 2015-10-21

Similar Documents

Publication Publication Date Title
CN104990549B (zh) 一种采煤机‑液压支架相对定位的方法及装置
CN105352504B (zh) 一种惯性导航与激光扫描融合的采煤机定位装置及方法
CN104089617B (zh) 一种移动机器人用定位装置及定位方法
CN109141410B (zh) Agv组合导航的多传感器融合定位方法
CN207087855U (zh) 用于煤矿巷道三维实景建模的移动机器人平台
CN105173940B (zh) 一种基于组合导航技术的超深矿井罐笼位姿测量系统及方法
CN205950750U (zh) 基于惯性导航的变电站巡检机器人导航控制系统
CN109540130A (zh) 一种连采机惯性导航定位定向方法
CN107560613A (zh) 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
CN107168186A (zh) 基于六轴组合传感器的四点自动调平系统及其工作方法
EP3045862B1 (en) Dump truck
CN205426176U (zh) 一种惯性导航与激光扫描融合的采煤机定位装置
CN107121082A (zh) 基于光纤惯性技术的煤矿巷道连续线形检测装置及方法
CN105005312B (zh) 一种基于最大角加速度和最大角速度卫星规划轨迹方法
Nagatani et al. Improvement of the odometry accuracy of a crawler vehicle with consideration of slippage
CN106325287A (zh) 一种基于惯性/磁传感器marg姿态检测的智能割草机直线行走控制系统
CN202451158U (zh) 一种刮板运输机机身调直装置
CN102749065A (zh) 基于惯性测量技术的罐笼轨道变形监测方法
CN103241656A (zh) 一种起重机遥控系统及其控制方法
CN106802322A (zh) 一种基于贴壁式检测车的隧道衬砌裂缝超声波检测系统
CN205825912U (zh) 一种用于集装箱识别定位的3d数据采集系统
CN108868772A (zh) 一种连采机快速准直控制方法
CN106524924A (zh) 一种基于光学惯性组合的轨检车位置姿态测量系统及方法
RU2019134846A (ru) Бюджетная инерциальная навигационная система
CN104251699B (zh) 一种室内空间定位方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CP02 Change in the address of a patent holder
CP02 Change in the address of a patent holder

Address after: 221000 research and development center of mine safety technology and equipment in Tongshan, Xuzhou, Jiangsu Province, China University of Mining and Technology

Patentee after: China University of Mining & Technology

Address before: 221000 Xuzhou high tech Industrial Development Zone,, Xuzhou Road, China University of Mining and Technology, mine safety technology and equipment research and Development Center

Patentee before: China University of Mining & Technology

CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20171215

Termination date: 20180615