基于惯性技术的端帮采煤机姿态测量仪设计*

孙　伟，闫慧芳，李瑞豹，丁　伟

（辽宁工程技术大学测绘与地理科学学院，辽宁阜新123000）



基于惯性技术的端帮采煤机姿态测量仪设计*

孙伟*，闫慧芳，李瑞豹，丁伟

（辽宁工程技术大学测绘与地理科学学院，辽宁阜新123000）

摘要：针对端帮采煤机特殊的机械构造及工作运动模式对自身姿态信息获取时效性和准确性以及无人自主化提出的更高要求，设计一种基于惯性解算方法的姿态测量设备。利用惯导系统中的水平加速度计敏感重力分量完成载体水平对准，根据加速度计敏感的采煤机运动状态实现载体静止状态的辨别，引入零速修正的闭环卡尔曼滤波完成惯导解算姿态及惯性器件偏差信息的修正。搭建基于DSP+FPGA的导航信息采集与处理平台并开展室内转台实验和户外模拟车载实验，结果表明：采用零速修正的闭环卡尔曼组合方法可有效准确地完成载体姿态信息的自主求取。

关键词：端帮采煤；惯性导航；姿态；实验测试

端帮采煤技术是在传统露天煤矿开采技术和井工开采技术之外，发展出的一种先进、成熟的煤矿开采方式［1-3］。上世纪80年代开始，在螺旋钻开采技术的基础上不断的进行研究和改进，端帮开采技术因其安全、高效的特点，已经成为露天煤、边坡煤以及露天矿端帮压覆煤开采的第一选择。作为世界上最大的端帮开采设备生产商，美国比塞罗斯国际公司旗下的SHM露天联合端帮开采系统就是这一领域的领军者，其设备已在世界40多个地区平稳运行，经过十多年的不断研发、改进、其掘进深度已由最初的30 m提高到300 m以上，端帮开采技术理念逐步得到广大用户接受，成为现代矿山开采不可或缺的一部分［4-5］。

端帮采煤技术对于提高我国煤矿资源的回采率以及薄煤层露头煤的开采，具有重大意义。我国拥有数量众多的露天煤矿、端帮资源及露头煤资源由于目前开采技术条件的限制不能有效地回收这些资源，造成资源的大量浪费。近年来，国内已有单位正在开展端帮采煤原理样机的研制工作，但是端帮采煤机因工作环境复杂、恶劣，采煤路线和煤岩性质的连续变化改变了设备运行过程中的预定轨迹，而端帮采煤机采用直进式采掘模式，特有的机械结构及运行模式对其自身行进姿态信息的掌握提出了更高要求。

惯性技术依靠陀螺仪和加速度计完成对载体姿态信息的实时测取，以其具有工作独立、实时连续的特点可实现端帮采煤机工作过程中姿态信息的获取［6-8］。论文结合端帮采煤机工作时的运动特征，设计采用零速修正的组合卡尔曼滤波姿态测量方法，根据端帮采煤机工作特点，开展户外车载实验模拟端帮采煤机工作模式，并利用实验结果验证方案的可行性和有效性。

1　端帮采煤机运动特征分析

端帮采煤机由采掘头车、多节链接式被动运输车、运输车对接平台和能源动力及操控平台组成。采掘头车具备开采动力和牵引能力，通过拖拽运输车沿直线前行实现煤炭开采及输送。一个完整采掘环节可描述为：采掘头车静止后掘进臂伸展性开采，达到掘进臂伸展极限长度时，复位于采掘头车初始位置，头车拖动运输车前进到掘进臂伸展长度后，开展下一周期采掘工作。由于依靠伸缩臂实现采煤机滚筒推动工作面前进，机体本身处于静止的时间远超于行进时间。采掘头车与运输车采用铰链连接，当头车探入煤层深度达到几十米时，其运行姿态的变化如果得不到及时修正将会阻碍设备前行甚至引起垮塌事故。

图1　端帮采掘头车工作示意

2　测姿系统总体方案设计

2.1系统初始化与姿态解算

由于MEMS导航系统中的陀螺仪精度低，无法有效敏感地球自转角速度，传统自对准方案无法实现初始方位角的获取［12-14］。而且端帮采煤机需要确定载体自身相对初始时刻夹角，因此可将初始方位角装订为0度，完成惯导系统初始化。

图2　惯导系统姿态解算方案

惯性导航系统的解算过程主要集中在捷联矩阵的更新，根据图2描述的惯导系统姿态解算方案可以看出，姿态基准更新过程实质是依据三轴陀螺仪输出信息完成基于四元数的更新过程，设采煤机坐标系相对导航坐标系的转动四元数为：

其中，q0、q1、q2、q3为4个实数；ib、jb、kb表示与采煤机坐标系相一致的四元数基，Q的即时修正可通过四元数微分方程来实现：

进一步可求取采煤机方位角φ、俯仰角和横滚角的计算形式：

2.2闭环卡尔曼滤波器设计

由端帮采煤机运动特征分析可知，设备工作过程中处于静止和运动状态更替，且机体静止过程中的时间远长于运动状态。根据这一运动特征，提出采用零速修正技术实现对惯导系统中惯性器件偏差和解算姿态信息的修正。系统采用“速度匹配”卡尔曼滤波方案，对各误差参数进行实时估计并闭环修正。

导航系统状态方程为：

该方案采用15阶导航误差模型，选取15个误差状态变量为：

式中：δVF、δVU、δVR分别表示惯导系统导航系3个方向的速度误差；δF、δU、δR分别表示3个方向的位移误差；ϕF、ϕU、ϕR分别表示3个方向的失准角；∇x、∇y、∇z分别表示载体坐标系内x、y、z方向的加速度计零偏；εx、εy、εz分别表示载体坐标系内x、y、z 3个方向的陀螺漂移。

状态转移矩阵F表达式为

W为导航系统状态噪声向量，主要为陀螺仪和加速度计噪声经矩阵在导航系的投影；考虑各陀螺及加速度计的噪声水平相同，认为在导航系下各方向的等效陀螺和加速度计噪声相同，因此作出如下简化：

式中，ωa表示加速度计随机游走零偏；ωg表示陀螺随机游走漂移。

导航系统量测方程为

其中，Z为导航系三个方向的速度测量信息，即

量测矩阵H为

V表示导航系统量测噪声向量。工程应用中，通常都无法准确知道卡尔曼滤波状态变量的真实值，在此情况下，卡尔曼滤波初始状态变量初值可设置为零。

零速修正由静止检测触发，即通过在检测为静止的时间区间内将速度计算结果重置为0，达到修正速度误差的目的，这是零速修正的最直接目的也是最简单的方法。为充分利用静止检测的检测结果估计更多的误差参数，结合零速修正工作原理，对卡尔曼滤波器做出改良：在卡尔曼更新时刻，若静态检测结果为运动状态，则滤波器只进行时间更新；若静态检测结果为静止状态，则滤波器做完整更新（即时间更新+量测更新），并闭环修正导航系统的姿态误差及器件误差。

式中，

得到更新后的捷联矩阵：

当加速度计零偏和陀螺漂移进行闭环修正时，将每步预测的惯性器件偏差状态量X相叠加，由叠加后的值对加速度计输出量fi(i=x,y,z)和陀螺仪输出量ωi(i=x,y,z)进行修正，即

修正后将修正所对应的一步预测状态量设为0。

3　系统解算平台设计

3.1惯性信号采集电路设计

惯性导航系统中的陀螺仪和加速度计均采用MEMS器件，其输出的加速度、温补和陀螺仪信号均为模拟电流量，需要将模拟信号经过信号调理后转换为数字量后才可进行载体姿态信息的解算。

ADS1258提供单/双极性输入模式，针对本系统中陀螺仪和加速度计的输出信号特征，选用图4所示单极性输入模式，八路模拟输入信号的正向端“AINP”接经信号调理电路输出的模拟信号，信号使能端“AINCOM”接+2.5 V；该+2.5 V是由电压基准芯片AD780输出的+ 2.5 V，经过运算放大器OPA2365跟随后提供。ADS1258的管脚连线如图4所示。

图3　惯性器件输出信息的模数转换

图4　ADS1258管脚连接图

3.2导航计算机设计

为满足惯性系统对于运动信息获取响应速度快、体积小、功耗低、可靠性高的要求，提出基于DSP+FPGA的实时信号处理平台设计方案，系统结构如图5。

采用TI公司32位高性能浮点数字信号处理器TMS320C6713B；外扩1片NOR Flash-AM29LV800B作为程序存储器，采用引导加载方式，系统上电后TMS320C6713B将该Flash中的程序自动加载到片内高速存储器RAM中，程序在片内RAM中高速运行；FPGA选用Altera公司的EP2C35F484C8N；外接1片1 G×8 Bit的NAND Flash-K9K8G08U0M作为数据存储器；数据回收模块由Altera公司的EP2C35F484C8N（FGPA）和Cypress公司的CY7C68013组成。

图5　导航计算机结构框图

3.3上位机软件设计

为提高系统可操作性，利用MFC对话编辑框中的变量实时获取接收的数据，通过调用UpdateData（）函数刷新界面实现解算数据的实时显示功能，其中包括三维陀螺仪、三维加速度计、三维磁力计信息，以及惯导解算后得到的三维姿态信息（如图6）。

要先在MFC中构建OPENGL框架：

①将工程所需的OpenGL文件和库添加到工程（setupapi.lib、OpenGL32.lib、GLu32.lib、GLaux.lib），并在工程中加入头文件

②添加WM_CREATE、WM_DESTROY、 WM_SIZE、WM_ERASEBACKGROUND的消息处理函数

③定义SetWindowPixelFormat函数定义像素格式

BOOL CSanWeiView：：SetWindowPixelFormat（HDC hDC）

④定义InitOpenGL（）函数创建并初始化渲染环境

完成OPENGL框架的建立，将惯导系统解算得到的载体运动信息数据传递给Device context，实现载体三维姿态信息的实时显示。

图6　上位机系统控制软件

4　模拟实验结果及分析

4.1初始姿态获取实验

惯导系统的初始化精度和稳定性直接影响惯导系统的姿态解算精度，为验证本文设计的初始化方法，将MEMS惯性测量单元安置于三轴惯性测试转台中心后保持转台归零状态（如图7），分别采集21次惯性器件输出的数据进行初始水平姿态信息的求取，得到的姿态信息如图8。

通过对图8分析可看出，将21次对准得到的水平姿态信息求平均值和方差，得到的结果如表1。

图7　转台实验环境

图8　多次对准实验结果

表1　水平对准过程中姿态角均值和标准差

通过对图表的分析可以看出：基于水平加速度计敏感重力分量的对准方法产生的姿态角具有很好的稳定性，方差优于2.013 7×10-4，这样的对准结果满足MEMS惯导系统导航解算对于初始姿态信息精度的要求。

4.2载体零速检测与姿态求取实验

采用零速修正方法实现惯导系统误差参数修正是不依赖任何外界信息提高惯导姿态解算精度的有效方法。因此，载体零速信息获取的准确程度就显得尤为重要。采煤机应用环境复杂，为模拟端帮采煤机间歇性行进模式，使结果更具有说服力。为模拟端帮采煤机间歇性行进模式，将惯导系统安装于原理测试车后采用行进10 m、停顿1 min循环实验模式，测试车的行进速度为10 km/h。

采集惯性器件输出，得到载体运行状态下对应的加速度计输出信息（如图9），试验过程中载体的间歇性运动可以被有效的区分。其中，载体静止过程中的曲线相对平稳，保持在9.8 m/s2小幅度均匀波动；载体处于运动过程中，由于加减速过程的影响，其加速度变化剧烈，与载体静止状态的加速度信息产生明显比对。根据载体不同运动状态下的加速度变化特征，通过设计阈值可实现载体静止状态的检测。

图9　载体运行加速度特征

利用测试车处于静止状态真实参考速度为零的特点，将惯导解算速度与参考速度的差值作为卡尔曼滤波组合修正的观测量，通过闭环反馈实现惯性器件偏差和姿态误差修正，得到的姿态曲线如图10所示。

图10　系统姿态变化曲线

结合图9和图10可看出，由于测试车的运动状态变化过程中90%以上的时间段内是处于静止状态，此时惯导系统工作在采用零速修正闭环卡尔曼组合导航状态，因此惯性器件误差和姿态误差可得到连续的修正，有效地避免了姿态误差的发散。该实验结果验证了零速修正闭环卡尔曼滤波方法对于实现载体姿态信息提取的可行性。

5　结论

端帮采煤技术的应用使得露头煤、边坡煤以及露天矿端帮压覆煤的开采成为可能，论文设计零速修正的闭环卡尔曼组合测姿方案可保证端帮采煤机的安全连续施工，设计低功耗的导航解算平台实现了系统集成的同时又可满足矿井瓦斯安全的要求。实验室转台与车载实验结果表明，采用加速度计敏感重力分量可有效地确定水平姿态，实现惯导系统初始对准；通过加速度计检测载体不同运行状态下的加速度变化特征，实现零速信息的检测并采用闭环卡尔曼组合完成姿态信息的有效获取，避免了姿态信息的发散。该研究成果可为端帮采煤机姿态信息的获取提供技术支撑。开展实地采掘实验实现系统无人化操作将是本课题下一步工作重点。

参考文献：

［1］尚涛，才庆祥，张幼蒂，等.我国大型露天煤矿若干生产工艺问题分析［J］.中国矿业大学学报，2005，34（2）：138-142.

［2］李绍臣，周杰，李宏杰，等.井采方向对露井协采边坡稳定影响的分析及优化［J］.煤炭学报，2014，39（4）：666-672.

［3］丁鑫品，李绍臣，王俊，等.露天矿端帮煤柱回收井工开采工作面推进方向的优化［J］.煤炭学报，2013，38（11）：1923-1928.

［4］刘文岗，王雷石，富强.SHM端帮开采技术及其应用的关键问题［J］.煤炭工程，2012，6：1-4.

［5］孙进步.SHM端帮采煤机在我国露天煤矿的应用前景［J］.神华科技，2011，9（4）：44-46.

［6］吕振，刘丹，李春光.基于捷联惯性导航的井下人员精确定位系统［J］.煤炭学报，2009，34（8）：1149-1152.

［7］孙伟，李松，李瑞豹.带有转动机构的捷联惯导初始对准方法分析［J］.传感技术学报，2014，27（8）：1082-1087.

［8］Sun Wei，Xu Aigong，Gao Yang.Strapdown Gyrocompass Algo⁃rithm for AUV Attitude Determination Using a Digital Filter［J］.Measurement，2013，46（1）：815-822.

［9］Tirerton D H，Weston J L.Strapdown Inertial Navigation Technol⁃ogy［M］.Second Edition.Reston：Copublished by the American institute of Aeronautics and Astronautics and the Institution of Electrical Engineers，2004：453-456.

［10］郭美凤，杨海军，腾云鹤，等.激光陀螺惯导系统扰动基座上的初始对准［J］.清华大学学报，2002，42（2）：179-181.

［11］Sun Wei，Gao Yang.Fiber-Based Rotary Strapdown Inertial Navi⁃gation System［J］.Optical Engineering，2013，52（7）：1-12.

［12］宋海宾，杨平，徐立波.MEMS传感器随机误差分析及处理［J］.传感技术学报，2013，26（12）：1719-1723.

［13］Heckman D W，Baretela M.Interferometric Fiber Optic Gyro Technology［J］.IEEE Aerospace and Electronic Systems Maga⁃zine，2000，15（2）：23-28.

［14］Hao Yanling，Xiong Zhilan，Gao Wei，et al.Study of Strapdown In⁃ertial Navigation Integration Algorithms［C］//Proceedings of the 2004 International Conference on Intelligent Mechatronocs and Automation.USA：IEEE，2004：751-754.

［15］秦永元.惯性导航［M］.北京：科学出版社，2006.

孙　伟（1984-），男，教授，博士生导师，黑龙江萝北县人。中国宇航学会会员，中国惯性技术协会会员；2007年于哈尔滨工程大学获得学士学位，2011年于哈尔滨工程大学获得博士学位。主持国家级、省部级科研项目8项；发表学术论文四十余篇，其中SCI收录6篇，EI收录20篇。长期从事惯性导航技术研究，sunwei-3775235@163.com；

闫慧芳（1991-），女，河南上蔡市人。现为辽宁工程技术大学测绘与地理科学学院学生。从事惯性数据处理方法研究。

The Design of Attitude Determination Instrument for End Slope Coal Mining Machine Based on Inertial Technology*

SUN Wei*，YAN Huifang，LI Ruibao，DING Wei
（School of Geomatics，Liaoning Technical University，Fuxin Liaoning 123000，China）

Abstract：According to the special mechanical structure，motion work mode and the requirement of timing and accuracy for un⁃manned attitude determination of the end slope coal mining.The attitude determined machine based on inertial resolution was designed in this paper.The level alignment with gravity sensed by accelerometers was carried out.The state identification for vehicle could be detected by sensationed of coal cutter motion state based on the inertial navigation system.The zero detected closed-loop Kalman filter was introduced to revise the inertial bias and attitude errors.Turntable and mobile vehicle experiments were carried out based on data collection and processing platform with DSP and FPGA.Experiment results show that，the zero detected closed loop Kalman filter could be used to finish the attitude detection effectively and accurately.

Key words：end slope coal mining；inertial navigation；attitude；experiment testing

doi：EEACC：7120；7230M；7320E10.3969/j.issn.1004-1699.2016.02.026

收稿日期：2015-09-12修改日期：2015-10-14

中图分类号：TP212

文献标识码：A

文章编号：1004-1699（2016）02-0306-07

项目来源：国家自然科学基金项目（41304032）；高等学校博士学科点专项科研基金项目（新教师类）（20132121120005）；第8批中国博士后科学基金特别项目（2015T80265）；对地观测技术国家测绘地理信息局重点实验室开放基金项目（K201401）；江西省数字国土重点实验室开放研究基金项目（DLLJ201501）；地球空间环境与大地测量教育部重点实验室开放基金项目（14-01-05）；第58批中国博士后科学基金面上项目（2015M581360）；辽宁省高等学校杰出青年学者成长计划项目（LJQ2015044）；辽宁省自然科学基金项目（2015020078）；辽宁省“百千万人才工程”培养经费项目（辽百千万立项［2015］76号）；航空遥感技术国家测绘地理信息局重点实验经费课题项目（2015B11）；精密工程与工业测量国家测绘地理信息局重点实验室开放基金项目（PF2015-13）；海岛（礁）测绘技术国家测绘地理信息局重点实验室项目（2014B05）