一种掘进机的导向方法、系统和存储介质技术方案

本发明专利技术提供了一种掘进机的导向方法、系统和存储介质，其方法包括：通过陀螺仪获取掘进机的姿态角；通过里程计获取输出脉冲数；根据输出脉冲数、姿态角以及预先获取的初始标定关系矩阵，计算得到掘进机的机头中心点、机尾中心点在导航坐标系下的位置坐标；根据位置坐标和计划线计算得到掘进机的姿态偏差。本发明专利技术实现全自主、高精度的导航定位，有效抑制定位误差，有效提高了掘进机前进方向的准确度。有效提高了掘进机前进方向的准确度。有效提高了掘进机前进方向的准确度。

【技术实现步骤摘要】
一种掘进机的导向方法、系统和存储介质


[0001]本专利技术涉及惯性测量
，尤指一种掘进机的导向方法、系统和存储介质。

技术介绍

[0002]煤炭作为我国主体能源和重要工业原料，是我国经济健康发展的重要支撑。然而，由于煤炭埋藏在地层深处，存在开采条件复杂、作业环境恶劣、劳动强度大以及危险系数高等特点，所以煤炭开采一直以来都被人们视为高危行业，对智能技术的需求尤为迫切。
[0003]由于掘进机工作环境的封闭性，类似于GPS等在陆地上常用到的定位方法无法使用，另外类似于红外对射法等常规的定位方法不能够同时兼顾对掘进机位置、姿态的同时测量。而且，由于粉尘遮挡的原因，掘进机的司机不能实时正确判断掘进机的姿态和掘进方向，掘偏、掘错的事情时有发生，不仅影响了巷道的施工质量，还影响了巷道的正常高效掘进。

技术实现思路

[0004]本专利技术的目的是提供一种掘进机的导向方法、系统和存储介质，在惯性导航理论的基础上，结合采煤机运动特性进行导向分析，具有导向精度高、数据更新率高、维护成本低、体积小等特点。
[0005]本专利技术提供的技术方案如下：本专利技术提供一种掘进机的导向系统，包括：测量装置，包括陀螺仪和里程计；所述陀螺仪安装于防爆控制盒内，所述防爆控制盒安装于掘进机的顶部；所述陀螺仪，用于获取所述掘进机的姿态角；所述里程计，安装于所述掘进机的行走部，用于获取输出脉冲数；处理器，分别与所述陀螺仪和里程计连接，用于根据所述输出脉冲数、姿态角以及预先获取的初始标定关系矩阵，计算得到所述掘进机的机头中心点、机尾中心点在导航坐标系下的位置坐标，再根据所述位置坐标和计划线计算得到掘进机的姿态偏差。
[0006]本专利技术还提供一种掘进机的导向方法，包括步骤：通过陀螺仪获取掘进机的姿态角；通过里程计获取输出脉冲数；根据所述输出脉冲数、姿态角以及预先获取的初始标定关系矩阵，计算得到所述掘进机的机头中心点、机尾中心点在导航坐标系下的位置坐标；根据所述位置坐标和计划线计算得到掘进机的姿态偏差。
[0007]本专利技术还提供一种存储介质，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现如所述的掘进机的导向方法所执行的操作。
[0008]通过本专利技术提供的一种掘进机的导向方法、系统和存储介质，能够实现全自主、高精度的导航定位，有效抑制定位误差，有效提高了掘进机前进方向的准确度。
附图说明
[0009]下面将以明确易懂的方式，结合附图说明优选实施方式，对一种掘进机的导向方法、系统和存储介质的上述特性、技术特征、优点及其实现方式予以进一步说明。
[0010]图1是本专利技术一种掘进机的导向方法的一个实施例的流程图；图2是本专利技术一种掘进机的导向系统及其对应的坐标系定义的一个实施例的结构示意图；图3是本专利技术一种掘进机的导向系统的另一个实施例的结构示意图；图4是本专利技术一种掘进机的导向方法的一个实施例的姿态偏差示意图；图5是本专利技术一种掘进机的导向方法的另一个实施例的姿态偏差示意图。
具体实施方式
[0011]以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其他实施例中也可以实现本申请。在其他情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。
[0012]应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所述描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其他特征、整体、步骤、操作、元素、组件和/或集合的存在或添加。
[0013]为使图面简洁，各图中只示意性地表示出了与本专利技术相关的部分，它们并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部件，仅示意性地绘示了其中的一个，或仅标出了其中的一个。在本文中，“一个”不仅表示“仅此一个”，也可以表示“多于一个”的情形。
[0014]还应当进一步理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
[0015]另外，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
[0016]为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对照附图说明本专利技术的具体实施方式。显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。
[0017]本专利技术的一个实施例，一种掘进机101的导向系统102，包括：测量装置，包括陀螺仪104和里程计106；陀螺仪104安装于防爆控制盒内，防爆控制盒安装于掘进机101的顶部；陀螺仪104，用于获取掘进机101的姿态角；里程计106，安装于掘进机101的行走部，用于获取掘进机101的输出脉冲数；具体的，测量装置包括陀螺仪104和里程计106，其中，陀螺仪104为三轴光纤陀螺仪104。里程计106的工作原理是根据安装在掘进机101的行走部的光电编码器来检测行走部的行走履带轴在一定时间内转过的弧度，进而推算掘进机101的相对位置的变化。优选的，将陀螺仪104和里程计106分别安装在掘进机101的顶部和行走部。
[0018]处理器，分别与陀螺仪104和里程计106连接，用于根据输出脉冲数、姿态角以及预先获取的初始标定关系矩阵，计算得到掘进机101的机头中心点B、机尾中心点A在导航坐标系下的空间坐标，再根据位置坐标和计划线计算得到掘进机101的姿态偏差。
[0019]具体的，如图2和图3所示，该导向系统由掘进机101和导向系统102组成，导向系统102所包括的测量装置均安装在掘进机101上。其中测量装置主要由陀螺仪104、里程计106组成。陀螺仪104通过防爆外壳安装在掘进机101平面上，用于提供航向角、横滚角、俯仰角等姿态角；里程计106安装在掘进机101的行走履带轴107上，用于提供掘进机101前进的输出脉冲数。
[0020]处理器安装在防爆控制盒内，负责将测量单元获取的数据融合，得到实时的矢量里程，并通过组合导航算法及其他误差剔除算法进行实时位姿的解算，并提供掘进机101的姿态数据和位置数据在显示屏109上进行动态显示。
[0021]如图2所示，本专利技术提供的一种基于捷联惯导系统的掘进机101导向方法进行掘进机101位姿解算时，需要建立与其定位所需要用到的相关坐标系的相对关系，其中坐标系定义如图3所示：导航坐标系OnXnYnZn（下文简称导航坐标系n或n系），n系作为导向系统的导航基准的坐标系，根据实际施工地选取，其中各坐标轴方向和当地的东北天坐标系（亦称为站心坐标系）相同。
[0022]载体坐标系OmXmYmZm（下文简称载体坐标系m或m系），m系以陀螺仪104的质心为坐标原点，以陀螺仪104的横轴本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种掘进机的导向系统，其特征在于，包括：测量装置，包括陀螺仪和里程计；所述陀螺仪安装于防爆控制盒内，所述防爆控制盒安装于掘进机的顶部；所述陀螺仪，用于获取所述掘进机的姿态角；所述里程计，安装于所述掘进机的行走部，用于获取输出脉冲数；处理器，分别与所述陀螺仪和里程计连接，用于根据所述输出脉冲数、姿态角以及预先获取的初始标定关系矩阵，计算得到所述掘进机的机头中心点、机尾中心点在导航坐标系下的位置坐标，再根据所述位置坐标和计划线计算得到掘进机的姿态偏差。2.根据权利要求1所述的掘进机的导向系统，其特征在于，所述处理器包括：标定模块，用于获取在初始状态从全站仪获取所述掘进机的机头中心点、机尾中心点分别在导航坐标系下的刚体坐标，并获取在所述初始状态下所述掘进机的初始标定关系矩阵；其中，所述初始标定关系矩阵为载体坐标系在所述初始状态下相对于掘进机坐标系的矩阵。3.根据权利要求2所述的掘进机的导向系统，其特征在于，所述标定模块包括：第一获取子模块，用于在初始状态利用全站仪获取机尾中心点、机头中心点分别在导航坐标系下的初始坐标，并从所述陀螺仪处获取初始姿态角；所述初始姿态角包括初始俯仰角、初始横滚角和初始航向角；第一计算子模块，用于根据所述初始坐标、计划线计算得到初始旋转矩阵，并根据所述初始姿态角计算得到零位旋转矩阵，再根据所述初始旋转矩阵和零位旋转矩阵计算得到所述初始标定关系矩阵；述初始标定关系矩阵；述初始标定关系矩阵；；其中，l
s
为计划线对应的第一直线方向矢量，l
t
为利用机尾中心点A、机头中心点B分别在导航坐标系下的初始坐标和计算得到导航坐标系下的第二直线方向矢量，为零位旋转矩阵，为陀螺仪在初始状态获取到的在载体坐标系下的初始俯仰角，为陀螺仪在初始状态获取到的在载体坐标系下的初始横滚角，为陀螺仪在初始状态获取到的在载体坐标系下的初始航向角，为载体坐标系到掘进机坐标系的初始标定关系矩阵，为零位旋转矩阵的逆矩阵，为初始状态下在导航坐标系下的初始旋转矩阵。4.根据权利要求3所述的掘进机的导向系统，其特征在于，所述处理器还包括：初始对准模块，用于根据从所述陀螺仪处获取的姿态角，计算得到导航坐标系相对于载体坐标系的初始姿态转换矩阵；
导向规划模块，用于根据所述掘进机在导航坐标系下的初始空间坐标和预先设定的目标空间坐标，拟合生成所述掘进机在所述导航坐标系下的计划线；实时测量模块，用于获取所述陀螺仪测量得到的实时姿态角，并获取所述里程计测量得到的输出脉冲数；所述实时姿态角包括实时航向角、实时横滚角、实时俯仰角；里程计算模块，用于根据所述输出脉冲数和所述里程计的出厂参数，计算得到所述掘进机的里程增量；实时对准模块，用于根据实时姿态角计算得到所述导航坐标系相对于所述载体坐标系的实时姿态转换矩阵；偏差计算模块，用于根据所述初始标定关系矩阵、实时姿态转换矩阵、里程增量，以及所述陀螺仪在所述导航坐标系下的刚体坐标计算得到所述位置坐标，再根据所述位置坐标和计划线计算得到掘进机的姿态偏差；
ꢀꢀꢀ
其中，为初始姿态转换矩阵，为初始姿态转换矩阵的逆矩阵，为导航坐标系相对于载体坐标系的姿态转换矩阵，为姿态转换矩阵的转置矩阵，Ψ为载体坐标系相对于导航坐标系的航向角，θ为载体坐标系相对于导航坐标系的俯仰角，φ为载体坐标系相对于导航坐标系的横滚角，是陀螺仪于当前采样周期t在载体坐标系下的实时俯仰角，是陀螺仪于当前采样周期t在载体坐标系下的实时横滚角，是陀螺仪于当前采样周期t在载体坐标系下的实时航向角，为导航坐标系相对于载体坐标系的实时姿态转换矩阵。5.根据权利要求4所述的掘进机的导向系统，其特征在于，所述里程计算模块包括：第二计算子模块，用于根据所述输出脉冲数和出厂参数，计算得到所述掘进机在掘进机坐标系下的上一采样周期和当前采样周期分别对应的位移增量；所述第二计算子模块，还用于根据所述上一采样周期和当前采样周期分别对应的位移增量对应的位移矩阵计算得到上一采样周期至当前采样周期之间的里程增量；
其中，里程计在初始采样周期内测得位移矩阵为，在当前采样周期t内测得位移矩阵为，s0为初始采样周期对应的位移增量，s
t
为当前采样周期对应的位移增量，，，为在初始采样周期至当前采样周期t之间在掘进机坐标系下的里程增量。6.根据权利要求5所述的掘进机的导向系统，其特征在于：所述第一获取子模块，还用于在初始状态利用全站仪获取陀螺仪中心点在导航坐标系下的初始坐标；所述第一计算子模块，还用于根据机尾中心点、机头中心点和陀螺仪中心点分别在导航坐标系下的初始坐标，以及所述零位旋转矩阵的逆矩阵计算得到所述机头中心点、机尾中心点分别在载体坐标系下的刚体坐标；其中，是在初始状态从全站仪获取机尾中心点A在导航坐标系下的初始坐标，是在初始状态从全站仪获取机头中心点B在导航坐标系下的初始坐标，是在初始状态从全站仪获取陀螺仪中心点在导航坐标系下的初始坐标，是在初始状态机尾中心点A在载体坐标系下的刚体坐标，是在初始状态机头中心点B在载体坐标系下的刚体坐标，为在初始状态载体坐标系到导航坐标系的零位旋转矩阵，为零位旋转矩阵的逆矩阵。7.根据权利要求6所述的掘进机的导向系统，其特征在于，所述偏差计算模块包括：第三计算子模块，用于根据所述初始标定关系矩阵，实时姿态转换矩阵，里程增量，以及所述陀螺仪中心点在导航坐标系下的初始坐标，计算得到陀螺仪中心点在导航坐标系下的实时坐标；坐标求解子模块，还用于根据所述实时坐标，刚体坐标，实时姿态转换矩阵，机尾中心点、机头中心点分别在导航坐标系下的初始坐标，计算得到当前采样周期机尾中心点、机头中心点分别在导航坐标系下的空间坐标；投影子模块，用于将所述机头中心点分别在垂直轴和水平轴进行投影得到对应的机头投影点，将机尾中心点分别在垂直轴和水平轴进行投影得到对应的机尾投影点，并将所述计划线的起点分别在垂直轴和水平轴进行投影得到对应的起点投影点，将所述计划线的终
点分别在垂直轴和水平轴进行投影得到对应的终点投影点；所述垂直轴为以导航坐标系中的X轴、Y轴以及原点组成的平面，所述水平轴为以导航坐标系中的Y轴、Z轴以及原点组成的平面；偏差计算子模块，用于根据所述机头、机尾投影点的坐标值，以及起点、终点投影点的坐标值，通过空间求距公式计算得到所述机头中心点相对于所述计划线的水平偏移量和垂直偏移量，以及所述机尾中心点相对于所述计划线的水平偏移量和垂直偏移量；直偏移量，以及所述机尾中心点相对于所述计划线的水平偏移量和垂直偏移量；直偏移量，以及所述机尾中心点相对于所述计划线的水平偏移量和垂直偏移量；其中，是陀螺仪中心点在导航坐标系下的实时坐标，是在初始状态从全站仪获取陀螺仪中心点在导航坐标系下的初始坐标，是在当前采样周期导航坐标系相对于所述载体坐标系的实时姿态转换矩阵，为载体坐标系到掘进机坐标系的初始标定关系矩阵，为里程计在掘进机坐标系下初始采样周期至当前采样周期之间的里程增量；P
at
为当前采样周期机尾中心点A在导航坐标系的空间坐标，P
bt
为当前采样周期机头中心点B在导航坐标系的空间坐标；A、B分别是机尾中心点和机头中心点，A
´
、A
´´
分别是机尾中心点在水平轴和垂直轴进行投影的机尾投影点，B
´
、B
´´
分别是机头中心点在水平轴和垂直轴进行投影的机头投影点，C、D分别是计划线起点和计划线终点，C
´
、C
´´
分别是计划线起点在水平轴和垂直轴进行投影的起点投影点，D
´
、D
´´
分别是计划线终点在水平轴和垂直轴进行投影的终点投影点，E、E
´
和E
´´
分别是平面A
´
AA
´´
与线段CD、线段C
´
D
´
和线段C
´´
D
´´
分别相交的交点，F、F
´
和F
´´
分别是平面B
´
BB
´´
与线段CD、线段C
´
D
´
和线段C
´´
D
´´
分别相交的交点；和
分别为机尾中心点相对于计划线的水平偏差量和垂直偏差量，和分别为机头中心点相对于计划线的水平偏差量和垂直偏差量。8.根据权利要求1
‑
7任一项所述的掘进机的导向系统，其特征在于，还包括：显示器；所述显示器，与所述处理器连接，用于将掘进机的姿态偏差进行图形化显示，以指导掘进机司机朝向正确的方向掘进。9.一种掘进机的导向方法，其特征在于，应用于权利要求1

【专利技术属性】
技术研发人员：伦华江，张晓日，黄喆，戴璐璐，
申请(专利权)人：力信测量上海有限公司，
类型：发明
国别省市：