基于组合方式的掘进机导航系统研究

2022-05-06田立勇孙业新陈洪月马春莹

工程设计学报 2022年2期

田立勇，孙业新，于宁，陈洪月，马春莹

（辽宁工程技术大学机械工程学院，辽宁阜新 123000）

煤炭资源是支撑国家繁荣和经济可持续发展的基础，开采技术智能化是煤炭工业发展的重要方向［1-2］。随着我国科学技术的发展，对智能化煤炭开采技术的要求不断提高。掘进作业作为煤炭开采过程中的重要环节，其面临的主要难题是“采掘失调”。因此，亟须实现煤矿巷道掘进的自动化、智能化［3-4］，而自动定向掘进是实现掘进工作面自动化的核心技术［5］。

掘进机定向掘进的关键是实现掘进机位姿的自主测量。朱信平等［6］通过采用全站仪检测目标棱镜，实现了对掘进机机身位姿参数的实时测量。杨文娟等［7］利用机器视觉方法来测量掘进机机身及截割头的位姿，其以激光束与红外光斑为特征，通过防爆相机采集图像以实现掘进机位姿的测量。杜雨馨等［8］通过分析十字激光束在激光标靶上的成像特征，解算得到掘进机机身位姿的相关参数，完成了掘进机机身位姿的自主测量。吴淼等［9］利用空间交汇测量技术来获取掘进机的位姿，即通过激光接收器识别旋转激光平面交汇区域，解算得到机身位姿参数，从而获得掘进机的工作状态。张旭辉等［10］结合超声波传感器和惯性导航系统来测量掘进机机身的位姿信息。毛清华等［11］将激光测距传感器、超声波测距传感器、地磁传感器和惯性导航系统组合，用于测量掘进机的位姿参数。Hargrave等［12］利用雷达定位技术来测量掘进机的位姿，即通过雷达的反射信号得到掘进机的位置和运行轨迹信息。综上所述，井下掘进机位姿的自主测量大多以机器视觉技术和全站仪为主，但由于煤矿巷道的环境复杂、恶劣，照明不足，易对位姿测量结果造成影响。为此，笔者基于多传感器融合思想［13］，将多种测量技术相结合，以期能够进一步提高掘进机位姿的测量精度，从而实现其定向掘进。

结合上述学者的研究成果，笔者提出一种基于组合方式的掘进机导航系统（即组合导航系统）：将自主研制的光电传感器导航系统与光纤惯性导航系统组合，用于测量掘进机机身的位姿。首先，以十字激光束作为光电传感器导航系统的识别特征，建立掘进机机身位姿解算模型；然后，利用递推最小二乘（recursive least square,RLS）算法对光电传感器导航系统和光纤惯性导航系统的测量数据进行融合处理，以完成掘进机机身位姿解算；最后，利用PLC（programmable logic controller，可编程逻辑控制器）对掘进机机身进行纠偏控制，实现其定向掘进。

1 掘进机组合导航系统总体方案

鉴于光电传感器导航系统是基于绝对基准进行测量的，无误差累积影响，而光纤惯性导航系统对姿态角的测量精度高且不受外部信号干扰，但对位置的测量误差会随时间累积，本文将两者组合使用，以弥补各导航系统的不足，从而提高对掘进机机身位姿的测量精度。

掘进机组合导航系统的总体方案如图1所示，其硬件结构主要由光电传感器导航系统（包含十字激光指向仪和激光接收标靶）、光纤惯性导航系统（包含光纤陀螺仪和加速度计）和辅助配套设备构成；软件程序可完成位姿解算，数据存储、显示与PLC控制等功能，以实现掘进机机身位姿的自主测量与纠偏调控。

图1 掘进机组合导航系统总体方案Fig.1 Overall scheme of integrated navigation system of roadheader

该组合导航系统的组成模块及安装方式如下。

1）光电传感器导航系统。将激光接收标靶安装在掘进机机身上，十字激光指向仪固定于掘进机后方的巷道顶板上，利用激光接收标靶对定位激光束进行接收和识别。

2）光纤惯性导航系统。光纤陀螺仪和加速度计分别沿掘进机前进方向、偏移方向及垂直于底板方向安装，用于测量掘进机机身在这3个方向上的运动角速度和线加速度。

3）通信传输模块。测量数据通过485总线传输到PLC，由状态显示界面显示输出数据及误差提示，通过人工调控来控制掘进机的运行轨迹。

在开始测量前，测量人员将光电传感器导航系统中的激光接收标靶和光纤惯性导航系统固定安装在掘进机机身上，并根据煤矿巷道设计信息确定十字激光指向仪在掘进机巷道后方顶板上的安装位置，以保证定位激光束发射的精确度。十字激光指向仪发射2个相互垂直的激光束，分别为沿掘进方向的水平激光束和垂直激光束。掘进机组合导航系统的结构如图2所示。

图2 掘进机组合导航系统结构Fig.2 Structure of integrated navigation system of roadheader

光电传感器导航系统包含2个激光接收横向标靶和1个激光接收纵向标靶，分别用于接收垂直和水平激光束。每台激光接收标靶由若干个光电传感器线性阵列组成，各光电传感器的间距为3 mm，如图3所示。均匀排布的光电传感器将识别的光信号转化为电信号后输出，以确定激光束的落点位置，结合构建的位姿解算模型获取掘进机机身的位姿参数。

图3 激光接收标靶组成示意Fig.3 Composition of laser receiving target

光纤惯性导航系统用于测量掘进机沿不同方向的角速度和线加速度，通过解算可得到其机身的姿态角和实时位置。将光电传感器导航系统和光纤惯性导航系统的测量数据进行融合处理，可实现对掘进机机身位姿的自主、准确测量。

2 掘进机组合导航系统测量原理

2.1 坐标系定义

为实现掘进机机身位姿的测量，建立如图4所示的空间直角坐标系。其中：十字激光指向仪坐标系O0-X0Y0Z0以激光束发射点为原点；巷道坐标系O-XYZ以十字激光指向仪安装位置在巷道底板上的投影点为原点；掘进机机身坐标系Ob-XbYbZb以其中心为原点。掘进机机身坐标系与巷道坐标系的3个坐标轴方向在起始条件下保持一致，即：Yb轴沿机身中轴线指向掘进机前进方向，Zb轴垂直机身中轴线指向巷道顶板方向，Xb、Yb、Zb轴的关系符合左手定则。在巷道坐标系下，激光束发射点O0的坐标为(0，0，z0)，掘进机机身中心Ob的坐标为(x，y，z)。

图4 掘进机机身位姿测量相关坐标系定义Fig.4 Definition of coordinate systems related to position and attitude measurement of roadheader body

由于组合导航系统固定安装在掘进机机身上，须确定机身坐标系与巷道坐标系之间的转换关系。以掘进机机身姿态角为转换角，用欧拉角α、β、γ表示。其中：α为机身偏向角，β为机身俯仰角，γ为机身滚动角。则机身坐标系相对于巷道坐标系的转换矩阵Jb可表示为：

2.2 光电传感器导航系统的测量原理

在光电传感器导航系统中，激光接收标靶通过识别定位激光束来测量掘进机机身的位姿参数。首先，定义各个激光接收标靶在掘进机机身坐标系下的坐标：激光接收横向标靶1中心Ob1的坐标为(xb1，yb1，zb1)，激光接收横向标靶2中心Ob2的坐标为(xb2，yb2，zb2)，其中xb2=xb1，yb2=yb1+e1，zb2=zb1+e2，e1和e2分别为2个激光接收横向标靶之间的水平和竖直距离，其具体数值与激光接收标靶的安装位置有关；激光接收纵向标靶中心Ob3的坐标为(xb3，yb3，zb3)。光电传感器导航系统的测量原理如图5所示。

图5 光电传感器导航系统的测量原理示意Fig.5 Schematic diagram of measurement principle of photoelectric sensor navigation system

当掘进机开始作业后，激光接收标靶在巷道中的位置随掘进机的运动而发生变化。在掘进机行进过程中，2个激光接收横向标靶中心在巷道坐标系下的坐标（x1，y1，z1）、（x2，y2，z2）可分别表示为：

激光接收横向标靶1和2分别用矢量l1和l2来表示，垂直激光束所在平面的方程为X=p，则垂直激光束落点P1和P2分别为l1和l2与平面X=p的交点。l1和l2的表达式为：

式中：μk为比例系数。

将X=p代入式（4），求得落点P1与落点P2的坐标（xPk，yPk，zPk）（k=1，2），可表示为：

在巷道坐标系下，垂直激光束在激光接收横向标靶上的落点与该标靶中心之间的距离（d1和d2）可用两点间距离公式计算，为：

在掘进机行进过程中，激光接收横向标靶在巷道坐标系中的横向（X方向）位移等于其中心与垂直激光束落点的距离。联立式（2）、式（3）和式（6），可得偏向角α：

整理可得，在巷道坐标系下掘进机机身的横向、垂向位置分别为：

式中：d3为水平激光束在激光接收纵向标靶上的落点与该标靶中心之间的距离。

2.3 光纤惯性导航系统的测量原理

光纤惯性导航系统测量得到的是掘进机机身在地理坐标系下的位姿信息，则须进行掘进机机身坐标系与地理坐标系的转换，其转换矩阵可表示为［14］：

综合考虑姿态角解算的复杂度、计算量以及更新速率，本文采用四元数法［15］来解算掘进机机身的姿态角，并通过四阶龙格库塔方法实时求解四元数。定义四元数q0、q1、q2和q3，则转换矩阵可表示为：

通过解算可得掘进机机身的姿态角：

通过比力积分运算得到掘进机在地理坐标系下的运动速度，进一步对速度进行积分运算获得其地理位置［16-17］。掘进机机身在地理坐标系下的位置可表示为：

式中：T为解算周期；λ、L和h分别为地理坐标系下掘进机机身在经度、纬度和高度方向上的位置；λ0、L0和h0分别为地理坐标系下掘进机机身在经度、纬度和高度方向上的初始位置；RN、RM分别为地球卯酉圈和子午圈的曲率半径；vx、vy和vz分别为掘进机运动速度沿地理坐标系的3个坐标轴方向的分量。

设地理坐标系与巷道坐标系之间的转换矩阵为M，则掘进机机身在巷道坐标系下的位置可表示为：

3 基于RLS算法的数据融合处理

光电传感器导航系统在解算掘进机机身位姿时采用的俯仰角β和滚动角γ由光纤惯性导航系统测得，这是因为十字激光指向仪发射的激光束会被遮挡，从而无法测量这2个姿态角，结合光纤惯性导航系统可弥补该缺陷。因此须对2个导航系统的测量数据进行融合处理，实现掘进机机身位姿的准确测量。本文采用RLS算法对测量数据进行融合处理。

3.1 RLS算法原理

RLS算法是一种自适应高斯-牛顿算法，其可使实际及期望信号与融合后输出信号之差的平方和最小，每次融合过程均直接求解最佳权重系数［18］。

经RLS算法融合处理后，输出信号y(i)可表示为［19-20］：

式中：X(i)为i时刻的输入信号，i=1，2，…，n；w(n)为权重系数向量。

定义误差e(i)为i时刻期望信号d(i)与融合后输出信号y(i)之间的误差，构建RLS算法的代价函数J（n）：

式中：η为遗忘因子，0<η<1。

对代价函数关于权重求导，并令导数值为0。由此可得，权重系数向量的最优解为：

根据Q(n)定义增益向量K(n)：

则Q-1(n)可表示为：

则权重系数向量的最优解可表示为：

式中：e′(n)为先验估计误差。

综上，基于RLS算法的数据融合流程为：首先计算先验估计误差e′(n)，并将输入信号作为期望信号；然后更新增益向量K(n)，并更新权重系数向量w(n)；最后更新Q-1(n)。

3.2 数据融合效果分析

构建初始模拟信号（数据个数m=2，遗忘因子η=0.97），融合当前时刻与前一时刻的数据，对应权重系数分别为w1和w2。采用二阶自回归方程生成非平稳随机输入信号，采样点数为3 000个，得到如图6所示的初始模拟信号和RLS融合信号的均方误差曲线。由图可见，相较于初始模拟信号，RLS融合信号的均方误差明显下降。

图6 初始模拟信号和RLS融合信号的均方误差对比Fig.6 Comparison of mean square error between initial analog signal and RLS fusion signal

鉴于RLS算法中遗忘因子η的取值不同时会得到不同的融合处理结果，设定3个不同的遗忘因子，通过仿真分析其对权重系数的影响（已知最优权重系数为1.4）。不同遗忘因子下权重系数的收敛曲线如图7所示。

图7 不同遗忘因子下权重系数的收敛曲线Fig.7 Convergence curves of weight coefficient under different forgetting factors

由图7可知：η取值越大，权重系数越收敛于最优权重系数；当η=0.99时，权重系数最接近于最优权重系数，故本文取η=0.99。仿真结果表明：RLS算法具有较高的收敛速度和估计精度，在迭代过程中产生的均方误差较小，能够精准快速地从带噪信号中恢复真值，对非平稳随机信号的适应性强。故选取RLS算法作为组合导航系统测量数据的融合处理方法可行。

4 掘进机组合导航系统实验研究

为验证所设计的组合导航系统的合理性与精确度，以陕煤集团张家峁煤矿15212掘进工作面为依托，以SANDNIK公司生产的MB670型掘锚一体机为掘进设备，开展组合导航系统测试实验，实验现场如图8所示。

图8 掘进机组合导航系统测试现场Fig.8 Test site of integrated navigation system of roadheader

按照MB670型掘锚一体机机身的空间尺寸，激光接收标靶的长度设定为600 mm，经过地面安装调试后与光纤惯性导航系统一起固定安装在机身上，保证其对定位激光束的接收功能；十字激光指向仪安装在机身后方的巷道顶板上，发射波长为650 nm的红色激光束。在组合导航系统测量的同时，利用全站仪对MB670型掘锚一体机进行位姿标定。

通过对比组合导航系统的测量结果与全站仪的标定结果，分析组合导航系统的测量误差。在实验过程中，MB670型掘锚一体机共行进50 m，每间隔10 m进行一次标定；组合导航系统在同一位置处的测量次数不少于30次。MB670型掘锚一体机机身位姿的标定值与测量值（平均值）的对比如表1所示。相同位置处MB670型掘锚一体机机身的位置测量误差和姿态角测量误差分别如图9和图10所示。考虑到在井下测量机身姿态角较为困难，故仅给出30组测量误差，结果由人机交互界面输出显示。

图9 MB670型掘锚一体机机身位置测量误差Fig.9 Measurement error of position of MB670 anchor digging machine body

图10 MB670型掘锚一体机机身姿态角测量误差Fig.10 Measurement error of attitude angle of MB670 anchor digging machine body

表1 MB670型掘锚一体机机身位姿的标定值与测量值对比Table 1 Comparison between calibrated and measured values of position and attitude of MB670 anchor digging machine body

由图9可知：对于MB670型掘锚一体机机身的位置，X方向的最大测量误差为17.5 mm，Y方向的最大测量误差为14.8 mm，Z方向的最大测量误差为13.9 mm，3个方向上的位置测量误差均在±20 mm内波动。由图10可知：对于MB670型掘锚一体机机身的姿态角，偏向角的最大测量误差为0.13°，俯仰角的最大测量误差为0.13°，滚动角的最大测量误差为0.14°，3个姿态角的测量误差均在±0.15°内波动。实验结果表明，所设计的组合导航系统可实现对掘进机机身位姿的实时、准确测量。