张金尧， 李威， 杨海， 司卓印， 应葆华
(中国矿业大学 机电工程学院， 江苏 徐州 221116)
摘要：针对综采工作面采煤机定位精度较低的问题，提出了一种基于Rodrigues参数法的采煤机捷联惯导定位方法。该方法运用捷联惯导的三轴加速度计和三轴陀螺仪输出采煤机的加速度和角速度信息，采用无冗余度的Rodrigues参数法对其进行位姿解算。仿真结果表明，在采煤机斜切进刀过程中，采煤机最大定位误差为0.291 4 m，姿态误差最大值为0.602 2 °，满足采煤机的定位精度要求。
关键词：采煤机定位； 捷联惯导； Rodrigues参数法
煤炭是我国的主要能源，其需求量高，但事故频发，实现综采工作面的无人化和少人化是提高煤炭开采效率和安全性的必要保障。其中，实现综采工作面“三机”自动化、智能化是关键技术之一，而采煤机定位技术又是首先需要解决的问题[1]。现有的采煤机定位技术有齿轮计数法、红外对射法以及超声波反射法和无线传感器网络法等[2]。齿轮计数法只能确定沿工作面方向行走的距离，而无法确定垂直于工作面的位移，且存在累积误差。红外对射法是把红外接收装置安装在液压支架上，根据采煤机发来的广角信号的强度来判断采煤机的位置，其只能定性，不能定量。超声波反射法和无线传感器网络法都存在信号在井下衰减太快的问题，具体实施困难。上述4种方法定位精度都较低，且易受环境影响。捷联惯导直接安装在采煤机上，不需要接收外部信息，直接采集采煤机的三轴加速度和三轴角速度信息，通过解算算法求解采煤机的速度、位置和姿态[3]。捷联惯导不依赖于外部环境，适用于煤矿井下的复杂环境。
传统的捷联惯导解算算法有欧拉角法、方向余弦矩阵法和四元数法[4]。欧拉角法涉及到大量的三角运算，计算量大，不适用于实时性要求高的场合。方向余弦矩阵法的约束条件比较多，其计算效率难以保证。而四元数法存在一个冗余约束，为非最小实现。经典Rodrigues参数法由于存在等效旋转角为180 °时出现奇异值的现象，故不适用于飞行器、航天器、船舶等的姿态解算。但采煤机在运行时，其航向角、俯仰角、横滚角均远小于180 °，不会出现奇异值的问题，且Rodrigues参数法计算更为简单，没有冗余度，占用内存和储存空间少，具有较好的实用价值。故本文采用Rodrigues参数法进行采煤机姿态解算。
1.1 采煤机的Rodrigues参数描述
刚体绕单位矢量a旋转α角，则定义Φ为描述其转动的Rodrigues参数[4]：
(1)
对于采煤机，导航坐标系选取地理坐标系O -XnYnZn，其中Xn，Yn，Zn三轴分别指向东向、北向、天向。载体坐标系O′-XbYbZb原点位于采煤机的质心，Xb轴指向采煤机的前进方向，Yb轴指向采煤机的垂直推进方向，Zb轴指向上，如图1所示。
图1 采煤机定位坐标系
设采煤机的姿态角为航向角ψ、俯仰角θ、横滚角γ。初始时刻，载体坐标系与导航坐标系重合，经3次旋转后，分别绕Z轴旋转ψ，绕X轴旋转γ，绕Y轴旋转θ，如图2所示。
载体坐标系绕3轴旋转的Rodrigues参数为
(2)
将3次旋转的等效旋转矢量合成，形成1次旋
图2 采煤机姿态角
转的Rodrigues参数：
Φ=
(3)
式中：φ1，φ2，φ3分别为Φ的3个元素。
设采煤机的角速度为ω，则其刚体姿态运动学微分方程的Rodrigues参数描述为
(4)
根据Rodrigues参数微分方程，对其积分，实现其姿态更新。设t时刻采煤机的姿态为Φ(t)，则t+h时刻的姿态为
(5)
式中：p为[t,t+h]时段内捷联惯导的输出角增量，p为p的模；*表示Rodrigues参数乘法。
1.2 采煤机姿态解算
Rodrigues参数Φ包含采煤机的3个姿态角信息，根据Φ得出采煤机载体坐标系(b系)到导航坐标系(n系)之间的姿态转换矩阵Cnb为[5]
(6)
式中φ=ΦTΦ，为Φ模的平方。
由采煤机的姿态转换矩阵Cnb，得到采煤机的三轴姿态角：
(7)
1.3 采煤机位置解算
根据捷联惯导解算模型[6]，得到采煤机速度更新的微分方程为
(8)
式中：fb为比力向量，即捷联惯导的加速度计输出，通过姿态转换矩阵把其从载体坐标系转换到导航坐标系；
为地球自转角速度在导航坐标系的投影；
为采煤机移动速度Vn引起的相对于导航坐标系的转动角速度;gn为重力加速度在导航坐标系下的投影。
采煤机位置更新的微分方程为
(9)
式中：L，λ，h分别为采煤机所在位置的纬度、经度和高度；vx，vy，vz分别为采煤机在导航坐标系下各方向的速度分量；RM，RN分别为当地子午圈半径和卯酉圈半径。
对式( 9) 积分即可得到采煤机所在位置的纬度、经度和高度。
2.1 采煤机斜切轨迹描述
采煤机割煤时通常采用斜切进刀法，斜切进刀法又分为端部斜切式进刀法和中部斜切式进刀法，本文以中部斜切式进刀法为例进行仿真。整个仿真过程分为空载和割煤2个阶段。设空载行走速度为1 m/s，割煤行走速度为0.5 m/s；空载直行，在1 s内，速度从0加速到1 m/s，匀速行走6 s；从第7 s开始减速，准备斜切进刀割煤，8 s时速度降为0.5 m/s，开始斜切进刀沿x方向前进3 m，y方向前进0.6 m，14 s后直行割煤。斜切进刀时航向角为11.309 9 °。
由于捷联惯导在高度通道具有发散性，所以仿真时根据采煤机的运动特性，限定采煤机在高度方向的速度最大值为0.05 m/s。
2.2 仿真结果与分析
在仿真过程中模拟加速度计和陀螺仪的数据输出，并叠加上相应的噪声，采样时间为0.01 s。图3和图4 分别为采煤机的捷联惯导定位轨迹和定位误差。从图3可看到，采煤机捷联惯导定位轨迹能够较好地跟踪基准轨迹。从图4可知，x，y，z方向最大定位误差分别为0.250 8，0.041 7，0.015 4 m，总体定位误差最大值为0.291 4 m。
图3 采煤机捷联惯导定位轨迹
(a) x方向位置误差
(b) y方向位置误差
(c) z方向位置误差
图4 采煤机定位位置误差
采煤机姿态角误差如图5所示。从图5可看出，采煤机姿态角在航向角、俯仰角、横滚角3个方向的误差最大值分别为0.602 2，0.031 9，0.154 2°，采煤机的姿态角具有很稳定的定位精度。综合采煤机的位置误差和姿态角误差可知，基于Rodrigues算法的采煤机捷联惯导定位能够满足井下采煤机定位精度要求。
(a) 航向角误差
(b) 俯仰角误差
(c) 横滚角误差
图5 采煤机姿态角误差
针对井下采煤机自主定位的问题，提出了一种基于Rodrigues算法的采煤机捷联惯导定位方法。该方法利用Rodrigues参数法对采煤机斜切进刀工作过程进行定位仿真，其仿真轨迹能够较好地跟踪基准轨迹，姿态角误差最大值为0.602 2 °，位置定位误差最大值为0.291 4 m，定位精度能够满足井下采煤机定位要求。
参考文献：
[1] 张世洪. 我国综采采煤机技术的创新研究[J].煤炭学报,2010,35(11):1898-1902.
[2] 杨海, 李威, 罗成名, 等. 基于捷联惯导的采煤机定位定姿技术实验研究[J]. 煤炭学报, 2014,39(12): 2550-2556.
[3] 樊启高, 李威, 王禹桥, 等. 一种采用捷联惯导的采煤机动态定位方法[J].煤炭学报,2011,36(10): 1758-1761.
[4] 周江华, 苗育红, 王明海. 姿态运动的Rodrigues参数描述[J]. 宇航学报, 2004,25(5): 514-519.
[5] 王勇军, 秦永元, 杨波. 四元数、Rodrigues参数在卫星姿态解算上的对比研究[J]. 中国空间科学技术, 2007,27(3): 18-23.
[6] 秦永元. 惯性导航[M]. 北京: 科学出版社, 2014.
ZHANG Jinyao， LI Wei， YANG Hai， SI Zhuoyin， YING Baohua
(School of Mechatronic Engineering, China University of Mining and Technology, Xuzhou 221116, China)
Abstract：For problem that shearer on fully mechanized coal face has lower positioning accuracy, a shearer positioning method using strapdown inertial navigation system based on Rodrigues parametric method was proposed. The tri-axial accelerometer and suspended gyroscope in the SINS were used to output information of acceleration and angular velocity of the shearer, and Rodrigues parametric method with no-redundancy was applied to calculate the position and attitude data. The simulation result shows that in the process of beveling feed, the maximum positioning error is 0.291 4 m and the maximum error of attitude angle is 0.602 2 °, it meets the requirements of positioning accuracy of shearer.
Key words:shearer positioning; strapdown inertial navigation system; Rodrigues parametric method
中图分类号：TD421.6
文献标志码：A 网络出版时间：2016-03-07 15:20
作者简介：张金尧(1992-)，男，四川广元人，硕士研究生，研究方向为采煤机定位和惯性导航，E-mail：502300523@qq.com。通信作者：李威(1964-)，男,江苏徐州人，教授，博士研究生导师，研究方向为机电系统智能控制及检测应用，E-mail：cmeecumt512@yahoo.com。
基金项目：国家高技术研究发展计划(863计划)资助项目(2013AA06A411)；江苏省“333工程”科研资助项目(BRA2015300)；江苏省研究生培养创新工程(KYLX_1374)；江苏省高校优势学科建设工程资助项目。
收稿日期：2015-11-23；修回日期：2016-01-06；责任编辑：胡娴。
文章编号：1671-251X(2016)03-0052-04
DOI:10.13272/j.issn.1671-251x.2016.03.012
网络出版地址：http://www.cnki.net/kcms/detail/32.1627.TP.20160307.1520.012.html
张金尧，李威，杨海，等.采煤机捷联惯导定位方法研究[J].工矿自动化，2016,42(3)：52-55.