CN110285810B - 一种基于惯性导航数据的采煤机自主定位方法及装置 - Google Patents

Abstract

本发明提供了一种基于惯性导航数据的采煤机自主定位方法及装置，所述方法包括：根据安装在采煤机上的惯性传感仪，在载体坐标系下测量得到采煤机的第一定位数据，其中所述第一定位数据包括：采煤机在载体坐标系下的平移加速度和采煤机在载体坐标系下的旋转角速度值；对所述第一定位数据中的所述平移加速度值结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据；以及在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵；根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置。

Description

一种基于惯性导航数据的采煤机自主定位方法及装置

技术领域

本发明涉及煤矿作业技术领域，尤其是涉及一种基于惯性导航数据的采煤机自主定位方法及装置。

背景技术

煤炭是我国的基础资源和能源，是国民经济发展的重要保障，煤炭在未来较长时间内仍将是我国主体能源。2014年，我国煤炭产量38.7亿吨，约占一次能源生产总量的70％；我国煤炭消费占一次能源消费的比重为65％，预计到2020年煤炭产能约为40-42亿吨。

我国煤炭开采以井工为主，综合机械化生产是当前广泛采用的生产方式。采煤工作面存在着“水、火、瓦斯、煤尘、顶板”五大自然灾害，时刻威胁着职工的生命安全和职业健康，只有将人从工作面解放出来，才能彻底改变这种局面。

采煤工作面的智能化、无人化是实现煤矿安全开采的必由之路，是践行“无人则安”先进安全理念的关键突破点。井下封闭空间内的采煤机器人高精度定位、定姿是其实现自主作业的关键技术，对应的采煤装备安全运行控制是整个无人化工作面的大脑。

目前美国和澳大利亚在自动化采煤技术研究方面处于领先地位，主要以JOY国际为代表的采煤机械厂商主导的以采煤机司机跟随操作采煤机，支架等与采煤机随动的跟机自动化模式。澳大利亚联邦科学与工业研究组织(CSIRO)推出了基于陀螺仪导向定位(LASC技术)的自动化采煤方法。但尚没有采煤工作面内无人操作的自动化采煤应用案例。国内近年来，由于煤矿安全高效生产的迫切需要，依托国家863计划“煤炭智能化掘采技术与装备”和国家发改委“煤炭综采成套装备智能系统”等重点专项支撑，以中国煤炭科工集团为代表的多家煤炭集团、采煤机械厂商、高校等开展了自动化无人开采技术的研发工作。其采煤机定位及工作面取直关键技术引进国外LASC系统，暂未实现关键设备国产化。

国内外对采煤机的位置监测先后尝试了多种方法，如无线电回声技术、光栅位移监测技术、倾角传感器技术、编码器和倾角传感器组合技术等，这些技术的测量精度均较低，远不能满足综采自动化开采的要求。

目前国际上并没有实现对采煤机的位置进行有效监测。

发明内容

针对现有技术存在的无法实现对采煤机的位置进行有效监测的缺陷，第一方面，本发明提供了一种基于惯性导航数据的采煤机自主定位方法，所述方法包括：

根据安装在采煤机上的惯性传感仪，在载体坐标系下测量得到采煤机的第一定位数据，其中所述第一定位数据包括：采煤机在载体坐标系下的平移加速度和采煤机在载体坐标系下的旋转角速度值；

在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵；

对所述第一定位数据中的所述平移加速度值结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据；

根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；

根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置。

进一步，所述在载体坐标系下构建第一姿态矩阵，包括：

通过采煤机的初态航向角、俯仰角和横滚角构建第一姿态矩阵，通过如下公式构建第一姿态矩阵：

其中/>

表示n系与b系之间旋转角度，/>

分别表示n系与b系之间的三维坐标系下的转轴方向；设u是n系中的旋转矢量，载体绕u旋转，旋转角度为β，l，m，n分别表示为u在n系中三个转轴方向的投影；

所述对所述第一姿态矩阵进行微分包括：所述第一姿态矩阵和所述载体坐标系下的旋转角速度值的乘积作为所述微分的结果。

进一步，所述第二定位数据包括：采煤机在导航坐标系下的速度值和采煤机在载体坐标系下的位移值。

进一步，所述惯性传感仪包括：用于测量所述第一定位数据的平移加速度值的摆式加速度计和用于测量所述第一定位数据的旋转角速度值的激光陀螺仪。

进一步，在所述对所述第一定位数据中的所述平移加速度值结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据之前，所述方法还包括：

利用扩展卡尔曼滤波对所述第一定位数据的旋转角速度和平移加速度进行误差补偿。

进一步，将微分后得到所述第二姿态矩阵中的方向余弦元素作为矩阵中的元素对所述第一姿态矩阵中的元素进行更新，采用更新后的第一姿态矩阵结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据。

第二方面，本发明提供了一种基于惯性导航数据的采煤机自主定位装置，所述装置包括：

测量模块，用于根据安装在采煤机上的惯性传感仪，在载体坐标系下测量得到采煤机的第一定位数据，其中所述第一定位数据包括：采煤机在载体坐标系下的平移加速度和采煤机在载体坐标系下的旋转角速度值；

积分运算模块，用于对所述第一定位数据中的所述平移加速度值结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据；

微分运算模块，用于在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵；

逆运算模块，用于根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；

位置确定模块，用于根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置/。

进一步，所述微分运算模块中所述在载体坐标系下构建第一姿态矩阵，包括：

通过采煤机的初态航向角、俯仰角和横滚角构建第一姿态矩阵，通过如下公式构建第一姿态矩阵：

其中/>

表示n系与b系之间旋转角度，/>

分别表示n系与b系之间的三维坐标系下的转轴方向；设u是n系中的旋转矢量，载体绕u旋转，旋转角度为β，l，m，n分别表示为u在n系中三个转轴方向的投影；

所述对所述第一姿态矩阵进行微分包括：所述第一姿态矩阵和所述载体坐标系下的旋转角速度值的乘积作为所述微分的结果。

进一步，所述第二定位数据包括：采煤机在导航坐标系下的速度值和采煤机在载体坐标系下的位移值。

进一步，所述惯性传感仪包括：用于测量所述第一定位数据的平移加速度值的摆式加速度计和用于测量所述第一定位数据的旋转角速度值的激光陀螺仪。

本发明的有益效果是：

产品致力于煤矿智能化无人开采领域，针对核心关键装置技术进行攻关，以综采设备自动化控制为基础，深入考量生产现场实际因素，引入军用惯导核心关键技术，结合煤矿应用实际需求特性，从惯性器件、系统集成、软件开发，拥有自主知识产权，实现采煤机定位设备国产化。配套自主研发设计的采煤机自主定位导航和工作面自动取直为核心技术的智能综采作业自导向控制模块，实现煤矿智能综采作业自导向采煤。

本发明首次自主研发试制惯性导航装置及配套系统，实现核心关键设备及技术国产化，减少国外核心关键技术依赖度，实现全系统及装置国产化率100％的目标；

本发明首次采用北斗导航定位领域的高精度惯性导航技术，自主研发高精度组合定位算法；

本发明研发采用的数据实时融合处理技术和嵌入式系统集成技术，可将高精度惯性导航系统、里程仪数据进行同步、采集、传输，对信息进行综合处理后，绘出精准的三维空间坐标信息，实现采煤工作面信息数据三维可视化呈现。

附图说明

图1为本发明的一种基于惯性导航数据的采煤机自主定位方法的流程示意图；

图2为本发明采用的采煤机自主定原理框图；

图3为本发明采用的激光陀螺仪的原理框图；

图4为激光陀螺仪的环形光路示意图；

图5为本发明采用的摆式平衡加速度计的原理框图；

图6为本发明采用的摆式加速度计机电部件图；

图7为本发明采用的摆式加速度计的结构示意图；

图8为本发明采煤机机身姿态角的示意图；

图9为本发明坐标变换的示意图；

图10为本发明的载体坐标系与导航坐标系之间的转换过程示意图；

图11为本发明采煤机自主定位实施流程示意图；

图12为本发明的一种基于惯性导航数据的采煤机自主定位装置的结构示意图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定装备结构、接口、技术之类的具体细节，以便透彻理解本发明。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。

如图1所示，本发明公开了一种采煤机自主定位方法，所述方法包括：

S1：根据安装在采煤机上的惯性传感仪，在载体坐标系下测量得到采煤机的第一定位数据，其中所述第一定位数据包括：采煤机在载体坐标系下的平移加速度和采煤机在载体坐标系下的旋转角速度值；

S2：在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵；

S3：对所述第一定位数据中的所述平移加速度值结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据；

S4：根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；

S5：根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置。

如图2所示，示出了本发明的采煤机自主定位原理图。

在步骤S1中，通过加速度计，可测量得到采煤机的在工作平面三方向加速度。加速度计在载体坐标系下测出来的加速度原始值为a_b(t)＝(a_bx(t),a_by(t),a_bz(t))^T,其中a_bx(t),a_by(t),a_bz(t)分别为加速度计在载体坐标系(b系)x轴、y轴、z轴三个方向的加速度值，a_b(t)为转置后的列向量。

陀螺仪通过绕采煤机三方向轴的角速度，测得的旋转角速度的原始值为ω_b(t)＝(ω_bx(t),ω_by(t),ω_bz(t)),其中，ω_bx(t),ω_by(t),ω_bz(t)分别为陀螺仪在载体坐标系(b系)x轴、y轴、z轴三个方向的角速度，其中，t为时间单位。

所述采煤机三方向轴角速度时表示当前时刻测量得到的ω_b(t)，先存储在系统计算机中，在后续采煤机姿态基准更新计算的过程中调用的平台旋转速率是滞后时刻的ω_b(t)。

在实施例中，导航加速度计和激光陀螺直接安装在载体上。用激光陀螺测量的角速度信息减去计算的导航坐标系相对惯性空间的角速度，得载体坐标系相对导航坐标系的角速度，利用该信息计算姿态矩阵。可把载体坐标系轴向加速度信息转换到导航坐标系轴向，再进行导航计算。利用姿态矩阵元素，提取姿态和航向信息。姿态矩阵计算、加速度信息的坐标变换、姿态与航向角计算可代替导航平台的计算导航数据的功能。计算导航坐标系的角速度信息则相对平台坐标系上陀螺旋矩信息。

在一些说明性实施例中，所述惯性传感仪包括：用于测量所述第一定位数据的平移加速度值的摆式加速度计和用于测量所述第一定位数据的旋转角速度值的激光陀螺仪。

如图3所示，示出了本发明所使用的激光陀螺的原理框图。

所述激光陀螺仪通过测量旋转闭合光路中顺、逆时针的两束光的频率差，由频率差计算得到光路角速度ω。在闭合光路中，由同一光源发出的沿顺时针方向和逆时针方向传输的两束光和光干涉，利用检测相位差或干涉条纹的变化，就可以测量闭合光路旋转角速度ω。激光陀螺仪的基本元件是环形激光器，环形激光器由三角形或正方形的石英制成的闭合光路组成，内有一个或几个装有混合气体(氦氖气体)的管子，两个不透明的反射镜和一个半透明镜。用高频电源或直流电源激发混合气体，产生单色激光。为维持回路谐振，回路的周长应为光波波长的整数倍。用半透明镜将激光导出回路，经反射镜使两束相反传输的激光干涉，通过光电探测器和电路输入与输出角度成比例的数字角速度信号。

激光陀螺采用三个反射镜组成环形谐振腔，即闭合回路。激光管沿光轴传播的光子向两侧经过透镜M4和M5射出，再分别由M₁→M₂→M₃和M₃→M₂→M₁从另一端反射回来，回路中形成传播方向相反的两路光束。对每一光束来说，只有经过一圈返回原处时相位差为2Π整数倍的光子才能诱发出与之相应的第二代光子，并以此规律逐渐增强，对于相位差不满足2Π整数倍的光子，则逐渐衰减直至消失，增强的光子多于衰减的光子，闭合光路工作在谐振状态。

如图4所示，示出了本发明的激光陀螺的环形光路。

如图5所示，示出了本发明的摆式力平衡加速度计的原理框图。

所述加速度计是惯性测量单元中的重要元件之一,用来测量沿其输入轴作用的常值和低频加速度。加速度计利用闭路系统的负反馈原理把检测质量悬浮在其结构的某一固定位置上。加速度计都由以下五部分组成:

①惯性质量,被加速时其惯性能产生一个力；②弹性铰链；③敏感惯性力做功的传感器；④力发生器；⑤电子放大器。同时各部分置于密闭的壳体中。各部分组合起来能将由加速度引起的机械力变换成相应的电流或电压,然后再把这个电流或电压变换成平衡加速度的输入惯性所需要的机械力。

当沿③敏感惯性力做功的传感器中的敏感轴有加速度作用时,检测质量的位置发生变化,位置检测器检测这一变化,然后将信号输入放大器,放大器驱动力发生器,使检测质量恢复到零位。加速度计的输出是流过力发生器与输入加速度成比例的电流。

如图6所示，示出了本发明的摆式加速度计的机电部件图。图中的标记1，3-分别为定接线块和动接线块；2-软导线带；4-轴承支架；5-浮子；6-力矩线圈；7-套筒；8，9-力矩器和导磁体；10-摆锤；11-宝石轴承；12，13-分别为角度传感器的转子和定子。

如图7所示，示出了本发明的摆式加速度计结构示意图。

图中7-1为挠性支承，7-2为壳体，7-3为轭铁，7-4为力矩器动圈，7-5为永磁体，7-6为摆件，7-7为信号器激磁线圈，7-8为信号器动圈，7-9为放大器。

所述加速度计由摆组件、信号传感器、推挽式永磁铁力矩器和挠性杆组成，表内充有硅油作为阻尼液体，并设有精密温控装置，以保证仪表在恒定的工作温度下工作。挠性杆一端固定在表壳上，另一端粘贴有信号器动圈，形成悬臂梁，在摆片上固定有力矩器动圈，永久磁铁一端固定在表壳上，另一端空套在力矩器动圈内。当表壳(基座)具有沿IA轴方向的加速度a时，挠性杆在惯性力矩作用下绕挠性接头中心位置旋转，产生输出角θ0，信号器检测出θ0并转换成电压信号，经放大后加入力矩器动圈，产生的推挽力矩抵消惯性力矩，使θ0回零，再平衡回路中的电流经采样电阻变换成输出电压，从此电压可提取出加速度a。

在步骤S2、S3中，并没有时序上的步骤先后顺序，在实施例中，可以先执行步骤S2后，执行步骤S3，或者也可以先执行步骤S3，再执行步骤S2。

例如，步骤S2中，在载体坐标系下构建第一姿态矩阵，在构建过程中，需要确定在不同坐标系下的采煤机的航向角、俯仰角和横滚角。通过惯性导航装置进行相应的坐标变换，得到相应的角度。

1)坐标变换

惯性导航装置通过解算得到的载体的姿态、速度、位置等导航信息必须是在一定的坐标系下,不同的导航系统根据不同的导航需求会选择不同的坐标系。而采煤机的姿态主要是靠俯仰角、横滚角和航向角表示的，如图8所示。

其中，采煤机沿工作面方向上与正北方向的夹角表示采煤机的偏航角，表示采煤机在工作面方向上与当地水平面夹角表示采煤机的俯仰角，采煤机在工作面推进方向上与当地水平面的夹角表示采煤机的横滚角。惯导技术是依靠安装在采煤机上的惯性传感器，测量采煤机的加速度并进行两次积分运算，从而得到采煤机的位置状态。由于传感器与采煤机固连，通过积分运算直接得到的采煤机信息是在载体坐标系下的位置信息，为了整个综采工作面信息统一，还需进行坐标系变换将采煤机的位置信息由载体坐标系变换到导航坐标系下。设载体的航向角为ψ(习惯上以北偏东为正)，俯仰角为θ，横滚角为γ，取地理坐标系g为导航坐标系，选取载体坐标系为右前上坐标系，导航坐标系为东-北-天方向，它们的变换关系如图9所示。

如上图10所述，是载体坐标系(b系)与导航坐标系(n系)之间的转换过程。

2)姿态描述

惯性导航装置利用激光陀螺和加速度计构造惯性测量单元(IMU)，惯性测量单元(IMU)固定安装于采煤机载体，故激光陀螺和加速度计测得的都是基于载体坐标系的物理量，即激光陀螺基于载体坐标系测量采煤机三轴旋转角速度，加速度计基于载体坐标系测量采煤机三轴平移运动的加速度。而惯性导航装置的导航计算是基于导航坐标系，因此需完成1)中的坐标变换过程。在三维空间中,采煤机在移动过程中位姿的变换都可以用一组旋转矩阵和平移向量表征。其中,平移向量表征的是导航坐标系原点到载体坐标系原点的向量,而旋转矩阵也称姿态矩阵,实时表征采煤机车体的姿态。

在一些说明性实施例中，步骤S3中的，所述在载体坐标系下构建第一姿态矩阵的过程包括：

通过采煤机的初态航向角、俯仰角和横滚角构建第一姿态矩阵，通过如下公式构建第一姿态矩阵：

其中/>

表示n系与b系之间旋转角度，/>

分别表示n系与b系之间的三维坐标系下的转轴方向；设u是n系中的旋转矢量，载体绕u旋转，旋转角度为β，l，m，n分别表示为u在n系中三个转轴方向的投影。

具体是根据四元数算法对姿态矩阵进行构建。四元数是一个由一个实数和三个虚数单位组成的超复数，其中实数表示旋转角度，虚数单位表示转轴方向，设u是n系中的旋转矢量，载体绕u旋转，旋转角度为β，u在n系中的投影uⁿ(l,m,n)，l，m，n分别表示为u在n系中三个转轴方向的投影，则以Q(e₀,e₁,e₂,e₃)＝e₀+e₁i+e₂j+e₃ ^k构造的四元数表示的姿态矩阵为：

其中

表示n系与b系之间旋转角度，/>

分别表示n系与b系之间的三维坐标系下的转轴方向。

公式(1)与1)旋转角之间的关系表示如公式(2)所示：

其中，ψ表示载体在b系与n系之间等效旋转的航向角转角，θ表示载体在b系与n系之间等效旋转的俯仰角转角，γ表示载体在b系与n系之间等效旋转的横滚角转角。(2)中的内容为方向余弦元素。

在步骤S2中，还包括对进行微分得到导航坐标系下的第二姿态矩阵；所述对第一姿态矩阵进行微分包括：所述第一姿态矩阵和所述载体坐标系下的旋转角速度值的乘积作为所述微分的结果。

导航计算机对导航坐标系下的姿态进行微分更新，即微分方程为：

将上述微分方程的结果

作为第二姿态矩阵。上述采煤机姿态初始估值包括初态航向角、俯仰角和横滚角，采用初始估值构建的第一姿态矩阵为矩阵R，绕采煤机三方向轴角速度ω_b(t)，通过公式(5)实现图2中的姿态基准更新计算，得到的结果为第二姿态矩阵/>

上述微分方程中的微分表示符号可在教科书《惯性导航》作者秦永元或者教科书《GNSS与惯性及多传感器组合导航系统原理》译者练军想中找到，表示微分，下文中再出现同样的符号亦表示微分。

其中，

表示载体坐标系相对于导航坐标系的姿态矩阵，/>

表示载体坐标系相对于导航坐标系的角速度，对公式(5)变换得到：

其中，

表示载体坐标系b相对于导航坐标系n的姿态矩阵,/>

表示载体坐标系相对于导航坐标系的角速度，/>

分别表示导航坐标系、载体坐标系相对于惯性坐标系i的转动角速度在导航坐标系、载体坐标系中的投影。/>

表示载体坐标系相对于惯性坐标系的转动角速度在导航坐标系中的投影，/>

表示导航坐标系相对于载体坐标系的姿态矩阵。

故而，姿态更新的公式为：

其中，

和/>

分别为m-1和m时刻的姿态矩阵,i表示惯性坐标系，b表示载体坐标系，n表示导航坐标系，m和m-1分别表示时刻且是时间单位。

在上述微分计算的过程中，

通过以下步骤获得，惯性导航装置的定位定姿计算是基于导航坐标系(n系)，因此需完成上述的坐标变换。生成如公式(1)所示的姿态矩阵R,即公式(3)中重力加速度和地球自转角速度在导航坐标系下的表示为：

其中，gⁿ,ωⁿ分别表示导航坐标系(n系)下重力加速度和地球自转角速度；T为转置。

g^b，ω^b分别表示载体坐标系(b系)下的重力加速度和地球自转角速度。

上面详细S3中构建第一姿态矩阵，并进行微分得到第二姿态矩阵的过程，下面详细描述步骤S2中得到导航坐标系下的第二定位数据的过程，包括：对所述第一定位数据中的所述平移加速度值结合采煤机的速度和位置的初始估值进行积分计算。

采煤机在载体坐标系下的初始位置包括：速度初始值、和位置初始值，并分别表示为v_g(0)、p_g(0),其中重力加速度和地球自转角速度矢量在地理坐标系(初始对准参考坐标系，导航坐标系)的分量准确已知。如下：

gⁿ＝[0 0 -g]^T

ωⁿ＝[0 ω_ecos L ω_esin L]^T (3)

其中，L、g和ω_e分别对应表示在当地纬度、重力加速度大小和地球自转角速率大小。

上述S3中积分计算的过程包括：

导航计算机依据姿态更新得到的姿态矩阵R的实时值，即公式(7)描述的姿态矩阵，加速度测得的加速度信息基于载体坐标系，需要减去重力加速度g得到地理坐标系下的加速度a_g，设加速度计的测量值是a＝[a_x a_y a_z]^T，加速度一次积分得到速度V_g，对速度积分得到位移P_g，故加速度，速度，位移关系如下：

a_g＝R^-1a-g

对惯导元件在周期T内进行离散化，即等价于公式：

V_gm＝V_gm-1+a_gm-1T

其中，V_gm，V_gm-1分别表示m时刻，m-1时刻地理坐标系下的速度，P_gm,P_gm-1分别表示m时刻，m-1时刻地理坐标系下的位移，a_gm-1表示m-1时刻地理坐标系下的加速度。

在一些说明性实施例中，所述第二定位数据包括：采煤机在导航坐标系下的速度值v_g(t)和采煤机在载体坐标系下的位移值p_g(t)。

将载体坐标系进行坐标变换到导航坐标系，故变换后的加速度a_n(t)、速度v_g(t)、位移值p_g(t)如下：

这里所说的积分计算为图2中的导航计算，对公式(1)方向余弦矩阵和采煤机的速度和位置的初始估值，所执行的导航计算，最终的计算结果为公式(10)中的采煤机位移p_g(t)和采煤机速度v_g(t)。

故采煤机可以通过自身惯性导航系统装置对陀螺仪的角速率进行微分更新，得到采煤机的姿态，对实时加速度进行积分得到速度以及位移，实现采煤机自主定姿定位。

在上述实施例中，说明了步骤S2的过程，下面说明步骤S4和步骤S5的过程。

步骤S4：根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；

首先，通过初试测量得到采煤机的航向角、俯仰角和横滚角，根据采煤机姿态初始估值表示为采煤机的第一姿态矩阵，经过公式(5)中的微分运算得到运算后的第二姿态矩阵

对第二姿态矩阵进行逆运算，重新获得采煤机在导航坐标系下的航向角、俯仰角和横滚角，作为采煤机导航坐标系下的姿态角。即图2中采用方向余弦元素逆运算(图2中的导航计算)后，重新获得采煤机在导航坐标系下的航向角、俯仰角和横滚角，作为图2中的采煤机的姿态数据。

步骤S5：根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置。

由于第二定位数据包括：公式(10)中采煤机在导航坐标系下的速度值和采煤机在载体坐标系下的位移值，且还具有步骤S4中逆运算得到的航向角、俯仰角和横滚角。从而得到采煤机在n系下位移后的位置，该位置为惯性导航后的位置。

在一些说明性实施例中，将微分后得到所述第二姿态矩阵中的方向余弦元素作为矩阵中的元素对所述第一姿态矩阵中的元素进行更新，采用更新后的第一姿态矩阵结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据。从而再次导航计算的积分运算后，再次获得采煤机位移p_g(t)和采煤机速度v_g(t)。

在一些说明性实施例中，在所述对所述第一定位数据中的所述平移加速度值结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据之前，所述方法还包括：

利用扩展卡尔曼滤波对所述第一定位数据的旋转角速度和平移加速度进行误差补偿。具体原理如图11所示。

对扩展卡尔曼滤波算法的初始值进行设定，未检测到采煤机静止状态时，滤波器仅进行预测而不执行更新过程；

静态检测时如果检测到采煤机处于静止状态，由于在数据采集模块已经对角速度、加速度数据进行了采集，同时完成了公式(1)所述坐标系变换的部分工作，相对零态修正的过程包括：通过零速修正将公式(9)解算得到的初始速度作为速度的误差观测值，同时控制扩展卡尔曼(Kalman)滤波算法进行更新过程；

对惯导元件在周期T内进行离散化，即等价于公式：

V_gm＝V_gm-1+a_gm-1T

其中，V_gm，V_gm-1分别表示m时刻，m-1时刻地理坐标系下的采煤机的速度，P_gm,P_gm-1分别表示m时刻，m-1时刻地理坐标系下的采煤机的位移，a_gm-1表示m-1时刻地理坐标系下的采煤机的加速度。

采用上述公式(9)对惯导元件在周期T内进行离散化处理可以实现方便计算的效果。

所述通过零速修正将公式(9)解算得到的初始速度作为速度的误差观测值指将初始速度v_g(0)代入公式(9)进行解算得到的初始速度作为速度的误差观测值。

所述图11中经过陀螺仪和加速度计输出的加速度为上文中提到的a_b(t)，角速度为上文中提到的ω_b(t)。

经过惯性导航定位解算即上文中提到的对所述第一定位数据中的所述平移加速度值结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据以及在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵的过程。

经过惯性导航定位解算得到的速度是上文中提到的v_g(t)，位置是上文中提到的p_g(t)，方向角是经过微分更新后的航向角、俯仰角和横滚角。

图11中所述修正反馈的过程是将测得的速度、位置及方向角等误差值反馈至当前惯导定位解算单元，作为解算完成后的未经过修正的速度、位置及方向角值处，修正反馈的过程可以理解为用未修正的速度、位置及方向角的值减去误差值得到精准的位姿信息的过程。误差，为经过扩展卡尔曼滤波得到的误差值，即图11中的系统误差。

图11中最终得到的位姿信息即是经过误差修正后的速度、位置及方向角的值。

图11中所述误差估计的过程是将惯导当前解算的速度作为扩展卡尔曼滤波的实测值，然后根据当下采煤机是否处于静止状态，进行判断扩展卡尔曼滤波算法执行的预测过程还是更新过程。

如果不进行图11中的扩展卡尔曼滤波也可以实现本发明的上述求解采煤机更新后的位姿信息的方法，只是采用扩展卡尔曼滤波可以一定程度上解决惯导在定位过程中不断积累的系统误差对定位精度的影响，从而提高了定位精度。

通过更新过程，扩展卡尔曼滤波算法对系统状态向量中的加速度、角速度、速度、位置的误差做最优估计，并将误差反馈至导航定位定姿解算单元；所述系统状态向量为一个包括加速度、角速度、速度等误差信息的向量。

惯性测量数据中，包括补偿后扩展卡尔曼滤波算法状态向量中的速度误差、位置误差、姿态角归零，保留加速度、角速度误差。

通过自主获取加速度、角速度定位数据，并对定位数据进行坐标转换、实时补偿纠偏等处理，再根据惯导纠偏误差参数输出导航坐标系下的惯导定位数据，因此，本发明提供的基于惯导的自主定姿定位方法不仅可以实现导航坐标系下的定位，还解决了电磁波定位覆盖信号弱或者盲区情况下无法定位的问题，且本发明提供的实时纠偏解决了惯导在定位过程中不断积累的系统误差对定位精度的影响，从而提高了定位精度。

如图12所示，本发明还公开了一种基于惯性导航数据的采煤机自主定位装置，所述装置包括：

测量模块100，用于根据安装在采煤机上的惯性传感仪，在载体坐标系下测量得到采煤机的第一定位数据，其中所述第一定位数据包括：采煤机在载体坐标系下的平移加速度和采煤机在载体坐标系下的旋转角速度值；

微分运算模块200，用于在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵；

积分运算模块300，用于对所述第一定位数据中的所述平移加速度值结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据；

逆运算模块400，用于根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；

位置确定模块500，用于根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置。

本发明的有益效果是：

产品致力于煤矿智能化无人开采领域，针对核心关键装置技术进行攻关，以综采设备自动化控制为基础，深入考量生产现场实际因素，引入军用惯导核心关键技术，结合煤矿应用实际需求特性，从惯性器件、系统集成、软件开发，拥有自主知识产权，实现采煤机定位设备国产化。配套自主研发设计的采煤机自主定位导航和工作面自动取直为核心技术的智能综采作业自导向控制模块，实现煤矿智能综采作业自导向采煤。

本发明首次自主研发试制惯性导航装置及配套系统，实现核心关键设备及技术国产化，减少国外核心关键技术依赖度，实现全系统及装置国产化率100％的目标；

本发明首次采用北斗导航定位领域的高精度惯性导航技术，自主研发高精度组合定位算法；

本发明研发采用的数据实时融合处理技术和嵌入式系统集成技术，可将高精度惯性导航系统、里程仪数据进行同步、采集、传输，对信息进行综合处理后，绘出精准的三维空间坐标信息，实现采煤工作面信息数据三维可视化呈现。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，物流管理服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。

Claims (8)

1.一种基于惯性导航数据的采煤机自主定位方法，其特征在于，所述方法包括：

根据安装在采煤机上的惯性传感仪，在载体坐标系下测量得到采煤机的第一定位数据，其中所述第一定位数据包括：采煤机在载体坐标系下的平移加速度和采煤机在载体坐标系下的旋转角速度值；

在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵；

对所述第一定位数据中的所述平移加速度结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据；

根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；

根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置；

所述在载体坐标系下构建第一姿态矩阵，包括：

通过采煤机的初态航向角、俯仰角和横滚角构建第一姿态矩阵，通过如下公式构建第一姿态矩阵：

其中/>

表示n系与b系之间旋转角度，/>

分别表示n系与b系之间的三维坐标系下的转轴方向；设u是n系中的旋转矢量，载体绕u旋转，旋转角度为β，l，m，n分别表示为u在n系中三个转轴方向的投影；n表示导航坐标系；b表示载体坐标系；

所述对所述第一姿态矩阵进行微分包括：所述第一姿态矩阵和所述载体坐标系下的旋转角速度值的乘积作为所述微分的结果。

2.根据权利要求1所述的基于惯性导航数据的采煤机自主定位方法，其特征在于，

所述第二定位数据包括：采煤机在导航坐标系下的速度值和采煤机在载体坐标系下的位移值。

3.根据权利要求1所述的基于惯性导航数据的采煤机自主定位方法，其特征在于，所述惯性传感仪包括：用于测量所述第一定位数据的平移加速度的摆式加速度计和用于测量所述第一定位数据的旋转角速度值的激光陀螺仪。

4.根据权利要求2或3所述的基于惯性导航数据的采煤机自主定位方法，其特征在于，在所述对所述第一定位数据中的所述平移加速度结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据之前，所述方法还包括：

利用扩展卡尔曼滤波对所述第一定位数据的旋转角速度和平移加速度进行误差补偿。

5.根据权利要求1所述的基于惯性导航数据的采煤机自主定位方法，其特征在于，将微分后得到所述第二姿态矩阵中的方向余弦元素作为矩阵中的元素对所述第一姿态矩阵中的元素进行更新，采用更新后的第一姿态矩阵结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据。

6.一种基于惯性导航数据的采煤机自主定位装置，其特征在于，所述装置包括：

测量模块，用于根据安装在采煤机上的惯性传感仪，在载体坐标系下测量得到采煤机的第一定位数据，其中所述第一定位数据包括：采煤机在载体坐标系下的平移加速度和采煤机在载体坐标系下的旋转角速度值；

微分运算模块，用于在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵；

积分运算模块，用于对所述第一定位数据中的所述平移加速度结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据；

逆运算模块，用于根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；

位置确定模块，用于根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置；

所述微分运算模块中所述在载体坐标系下构建第一姿态矩阵，包括：

通过采煤机的初态航向角、俯仰角和横滚角构建第一姿态矩阵，通过如下公式构建第一姿态矩阵：

其中/>

表示n系与b系之间旋转角度，/>

分别表示n系与b系之间的三维坐标系下的转轴方向；设u是n系中的旋转矢量，载体绕u旋转，旋转角度为β，l，m，n分别表示为u在n系中三个转轴方向的投影；n系表示导航坐标系；b系表示载体坐标系；

所述对所述第一姿态矩阵进行微分包括：所述第一姿态矩阵和所述载体坐标系下的旋转角速度值的乘积作为所述微分的结果。

7.根据权利要求6所述的基于惯性导航数据的采煤机自主定位装置，其特征在于，所述第二定位数据包括：采煤机在导航坐标系下的速度值和采煤机在载体坐标系下的位移值。

8.根据权利要求6所述的基于惯性导航数据的采煤机自主定位装置，其特征在于，所述惯性传感仪包括：用于测量所述第一定位数据的平移加速度的摆式加速度计和用于测量所述第一定位数据的旋转角速度值的激光陀螺仪。

Images