掘进机精准定位方法与掘进机器人系统

卢新明，闫长青，袁照平

（1.山东科技大学山东省智慧矿山信息技术重点实验室，山东 青岛 266590；2.山东蓝光软件有限公司山东省智慧矿山软件工程技术研究中心，山东 泰安 271000；3.山东科技大学信息工程系，山东 泰安 271019；4.山东科技大学机械与电子工程系，山东 泰安 271019）

1 引言

悬臂式掘进机是目前煤矿开采和井下巷道掘进的主要设备，掘进机的远程化、智能化、机器人化直接关系到煤矿的安全开采和矿工的生命安全。

研究人员对掘进机自动化和智能化进行了大量研究[1-2]。目前，掘进机已经能够实现远程控制[3-5]，但由于未能实现地下空间的自主测量和定位，无法建立掘进机自身位姿和巷道之间的位置关系，无法自动感知和定位周围环境，因此只能依靠视频进行作业，无法实现精确开采。掘进机机器人化的核心问题——定位和导航已有了大量的研究[6-13]，主要发展了基于超宽带技术的定位方法[10]、基于激光交汇的定位方法[2,12-13]和基于惯性导航技术的定位方法[14-15]。但超宽带方法本身存在精度问题，很难精确控制切割精度。基于激光交汇技术的定位安装过于复杂，需要测量多个点进行定位，难以在复杂的井下环境中配置应用。基于惯性导航的方法需要计算姿态角和距离，存在累积误差问题。上述方法目前普遍处于理论研究和系统仿真阶段，尚未发现智能掘进机器人在井下的实际应用。另外，不少定位仿真算法对姿态角的使用存在误区，混淆了惯性实测结果和预定旋转角的因果关系。惯性导航仪输出的是实测姿态角，而使用实测姿态角对原坐标系旋转变换无法得到具有同样姿态角的坐标系统，如对3 个轴均旋转45°，得到的坐标系的姿态角并非45°，从而使这些算法结果的可信度存疑。受定位方法的制约，目前没有可以应用于生产的可精确定位掘进机器人系统，极大地制约了掘进机器人的应用和煤矿安全精确开采问题的解决[3]。

制约掘进机机器人化的另一个关键问题是对周围环境进行感知需要传感器设备联网。现代物联网技术的核心可以实现设备入网和互联，实时传输状态数据至处理设备，目前该技术已成为实现感知矿山、智慧矿山的关键支撑技术[16-18]。借助物联网技术可以使掘进机实时感知矿山的状态，实现掘进机机器人化[19]。

针对上述问题，本文结合惯性导航仪、指北仪和具有跟踪功能的全站仪等设备，构建物联网，研发了新的高精度定位模型和解算方法，并据此研发了掘进机器人系统。通过惯性导航仪、指北仪和全站仪的结合，解决了惯性导航的累积误差问题；建立了只需测量一点即可实现精准定位的一点定位模型，解决了现有方法井下配置复杂的问题；在解算时抛弃了旋转姿态角定位的方式，确保了算法的可靠性；通过物联网实现了对掘进机周围环境的精确感知，研发了掘进机器人系统，实现了掘进机机器人化。

2 掘进机器人物联网构建

由于掘进机在矿井下工作，要实现掘进机机器人化需要实时采集掘进机自身的状态信息和周围的环境信息，以确定自身的位姿及其与巷道的关系，进而感知周围环境，因此本文在互联网的基础上引入了物联网，以便相关状态采集设备可以连入网络。

本文所提掘进机器人系统的主要设备包括远程遥控设备、全站仪、指北仪、惯性导航仪、远程工控机、现场工控机、可编程逻辑控制器（PLC,programmable logic controller），以及配置了多个传感器的掘进机。这些设备分别接入远程工控机和现场工控机，并由远程工控机通过互联网和现场工控机交换信息。

远程遥控设备即遥控器，提供掘进机运行所需要的指令，通过串口与远程工控机相连。2 个工控机通过TCP/IP 连接，共处于一个局域网内。全站仪和指北仪通过网络与现场工控机连接，全站仪源源不断地提供测量数据以计算坐标；指北仪内置陀螺仪，以一定的频率连续测量全站仪的姿态角并发送数据。掘进机油缸安装油缸行程传感器，测定油缸行程，掘进机共配置8 条油缸，其中4 条负责炮头的运动，另外4 条负责掘进机位姿的调整。根据炮头运动相关的油缸行程可以计算出炮头坐标信息。油缸行程传感器和其他相应设备与PLC 连接，惯性导航仪通过网络实时传送掘进机的位姿数据至工控机，PLC 通过Modbus TCP 协议与工控机连接。整个系统形成了一个物联网络，整体架构如图1 所示。

图1 网络架构

物联网是互联网的延伸，从互联网的角度看，整个网络架构又可看作客户机/服务器模式。以远程工控机和地面工控机为中心的网络各自组成了客户机，以现场工控机为中心的网络和以PLC 为中心的网络共同组成了服务器系统，其中客户机系统可以根据不同的角色和职能配置多个。本文系统采用了三层架构，其中地面客户机系统用于地面监控。

3 掘进机位姿确定方法

掘进机实现精准掘进的关键是自身位姿的精准确定。为此，本文研发了一种基于惯性导航仪、指北仪和跟踪式全站仪的精确定位方法[20]，设计了一种精确定位算法。定位原理如图2 所示，在掘进机机身后部安装一个靶球，由全站仪测量结果计算靶球相对坐标，即靶球在全站仪坐标系下的坐标（简称全站仪坐标系），由指北仪测量全站仪位姿，进而得出全站仪所在的坐标系，从而定位靶球；由靶球位置和惯性导航仪可得掘进机自身坐标系，则实现掘进机任意一点定位。由于使用惯性导航仪定位会因二次积分而产生累积误差，本文方法仅使用惯性导航仪输出高精度姿态角，避免了二次积分的累积误差问题。本文方法仅使用一个靶球和一个激光发射源即全站仪，即可解决传统方法安装部署过于复杂的问题。

图2 定位原理

3.1 定位设备的安装

指北仪和全站仪的安装要求共底平面，且使指北仪的测量方向与全站仪的初始设定方向一致，二者固定在一个防爆箱内。

惯性导航仪安装在掘进机机身上，且保持惯性导航仪的底面与掘进机机身平面共面，惯性导航仪的测量方向与机身前进方向一致。

定位所需的激光靶球安装在掘进机机身后部某个固定位置处，手动测量并记录激光靶球到掘进机坐标系原点的坐标增量(Δxr,Δyr,Δzr)。由全站仪寻找并锁定激光靶球进行定位。

由惯性导航仪可得掘进机的实时姿态角，包括方位角yaw、俯仰角pitch 和滚动角roll。方位角yaw为掘进机前进方向在水平面的投影与北向的夹角，俯仰角pitch 为掘进机前进方向Y轴与水平面的夹角，而滚动角roll 为机身X轴与水平面的夹角。

指北仪测定全站仪的姿态角，将姿态角融入算法计算，从而避免了全站仪的调平处理，减小了人为误差的引入。

3.2 坐标系定义

绝对坐标的确定需设定4 个坐标系，包括大地坐标系OeXeYeZe、掘进机坐标系OrXrYrZr、巷道坐标系OlXlYlZl和全站仪坐标系OoXoYoZo。

所有坐标系均采用右手法则。大地坐标系采用东北高模式，即东为X轴正向，北为Y轴正向，向上方向为Z轴正向。

巷道坐标系采用从巷道起点沿中线掘进的方向为Y轴正向，向右方向为X轴正向，垂直于巷道底板向上方向为Z轴正向。

掘进机坐标系采用沿掘进机中线前进方向为Y轴正向，沿机身平面垂直于机身中线向右方向为X轴正向，垂直于机身平面向上方向为Z轴正向。

全站仪坐标系采用其本身固有的左手法则坐标系，即西为X轴正向，北为Y轴正向，向上方向为Z轴正向。

3.3 定位算法

经查阅大量文献发现，以往利用惯性导航的研究大都混淆了惯性导航仪输出的实时状态的姿态角和表示旋转过程的欧拉角，而给定一组相同的角度，按一定的顺序旋转无法得到相应的实时状态。导致这种问题的可能原因有两方面，一方面，因为旋转本身的复杂性，3 个角度的旋转顺序和坐标系定义的不同均会导致旋转结果的不同；另一方面，由于惯性导航近些年才逐渐走向民用且价格高昂，模拟实验并未用到真正的惯性导航，导致不能发现其中的问题。目前的研究算法都基于给定所谓姿态角的旋转，尚未发现可以正确利用惯导实时状态的姿态角进行正确定位的算法。因而本文提出了利用惯导的实时状态姿态角直接求算惯导坐标系的3 个向量，进而确定惯导坐标系进行定位的算法。

由图2 设计定位算法，原理如下。

由惯性导航仪测得姿态角α β、和γ，根据姿态角计算出掘进机的坐标系向量，从而解算出掘进机的坐标系OrXrYrZr向量。

由全站仪实时测量掘进机身安装的定位靶球，输出距离L、方位角αf和倾角αq，假设全站仪的基点为(xo,yo,zo)。可以根据式(1)计算全站仪坐标(xob,yob,zob)为

由指北仪测定的全站仪姿态角，解算出全站仪所在的坐标系OoXoYoZo向量。

由全站仪坐标系向量、全站仪基点和全站仪坐标(xob,yob,zob)，解算出靶球在大地坐标系OeXeYeZe的绝对坐标(xeb,yeb,zeb)。

根据靶球所在掘进机坐标系OrXrYrZr，向量(rx,ry,rz)和靶球在掘进机坐标系中的坐标增量(Δxr,Δyr,Δzr)解算出掘进机坐标系原点的绝对坐标(xr,yr,zr)，如式(2)所示。

算法1 具体描述如下。

算法1定位算法

1)惯性导航仪输出实时姿态角：方位角yaw、俯仰角 pitch 和横滚角roll。

2)假设掘进机Y轴向量在XOY平面上的投影长度为单位长度1。

3)由yaw 和 pitch 求出Y轴向量。

4)标准化Y轴向量。

5)由于坐标系正交，则X轴和Y轴垂直，内积为0。

6)已知滚动角roll，并假设长度为单位向量，求得X轴单位向量。

7)由坐标轴的正交性，根据X、Y轴向量差积，求得Z轴向量。

8)由已知相对坐标，根据所求坐标系，求得该坐标点在坐标系下的投影坐标增量。

9)考虑原点计算定位坐标。

本文定位算法首先根据3 个实时姿态角分别求出掘进机坐标系的3 个坐标轴X、Y、Z轴向量；然后计算掘进机相对坐标点在该坐标系中的坐标增量；最后考虑原点计算定位坐标进行定位。由于根据实时姿态角直接求算掘进机坐标系，因此避免了利用旋转矩阵带来的不确定性，保证了计算结果的正确性。

4 掘进机器人系统

本节基于前述物联网和定位算法，设计并实现掘进机器人系统。

4.1 系统组成

4.1.1 按照空间部署距离划分

本文系统按照空间部署距离可以划分为三层，第一层为地面监控子系统，第二层为井下远程监控子系统，第三层为现场作业子系统。

1)地面监控子系统部署在地面上，供地面观察和地面管理人员使用。地面监控子系统主要提供井下作业的监视支持，一般不用于实际作业控制，经授权后可以进行作业控制。

2)井下远程监控子系统部署在井下远离工作面的地方，供操作人员操作使用。井下远程监控子系统具体负责掘进作业的监控。

3)现场作业子系统部署在掘进机机身和掘进机附近，不需要人工操作，负责现场作业。

这种系统架构使操作人员远离采掘工作面，地面管理人员可以随时检查井下作业状况并根据需要控制井下作业。

4.1.2 按照任务职能划分

井下远程监控子系统和现场作业子系统合作完成掘进作业，按照任务职能又可以分为3 个子系统：监控子系统、定位子系统和执行子系统。

1)监控子系统的功能是实时监视并控制掘进机进行远程作业，由一台工控机连接遥控设备，并安装相应的监控软件构建。通过遥控器发送指令，经由网络传输至执行子系统，驱动掘进机进行作业，作业执行情况传回监控子系统实时图形化显示。监控软件实时高精度地显示作业的进展情况，保证作业安全可靠、高精度运行。

2)定位子系统包括指北仪、具跟踪功能的全站仪、安装于掘进机某固定部位的定位靶球和安装于机体的惯性导航仪。指北仪和全站仪固定在定位箱内，指北仪确定全站仪的位姿并输出3 个角度，即方位角、俯仰角和滚动角。指北仪安装方向与北向的夹角即方位角，指北仪安装方向与水平面的夹角即俯仰角，指北仪安装方向为Y轴，用右手法则确定的坐标系的X轴与大地水平面的夹角为滚动角。全站仪跟踪定位靶球并返回距离和2 个角度，具有自动跟踪功能。全站仪和指北仪共同测量并解算靶球的大地坐标。惯性导航仪安装在掘进机机体上，实时测量掘进机的位姿，输出3 个与指北仪输出角类似的角度，但角度的表示范围有所不同，这3 个姿态角表示掘进机的位姿。

3)执行子系统包括一台现场工控机、PLC 和配置了8 条油缸传感器的掘进机。

现场工控机通过网络接收监控系统传来的指令，通过Modbus TCP 将指令传递给PLC，PLC 执行指令驱动掘进机各部件进行动作完成掘进作业。同时，现场工控机查询PLC 的状态信息并根据需要执行相应计算，然后把远程工控机所需数据分别返回给井下监控子系统和地面监控子系统。

掘进机机体需要确定所有的机体参数，配置8 条油缸的行程传感器。其中，2 条升降油缸和2条回转油缸负责掘进机炮头的摆动，2 条前铲板油缸和2 条后支撑油缸负责掘进机姿态的调整。前4 条油缸行程用于计算炮头相对于掘进机重心的位置，后4 条油缸行程作为掘进机姿态调整的依据。

控制系统选用PLC 的西门子1500。PLC 连接所有的掘进机传感器以及掘进机的其他部件，能够按指令驱动相关的部件运动并提供各种参数和状态信息。

4.2 工作流程

掘进机器人工作流程如图3 所示。其中，向右的虚线箭头表示掘进机接收从监控系统发来的指令信息流，向左的虚线箭头表示回传掘进机作业的所有相关状态信息流，并计算空间坐标回传。

图3 掘进机器人工作流程

具体工作原理如下。遥控器以一定频率发出指令，远程工控机接受遥控器指令并校验其完整性，将校验后的指令发送至现场工控机。现场工控机接受指令后，按照和PLC 的协议将该指令送入PLC 指定地址，PLC 根据该指令驱动掘进机进行作业。PLC发送指令驱动掘进机后，会实时读取掘进机状态信息并写至相应寄存器。现场工控机完成指令发送后，实时读取PLC 状态寄存器，根据掘进机油缸的状态信息计算炮头的相对坐标，根据惯导数据实时计算掘进机坐标系，根据掘进机坐标系把炮头的原坐标转换为大地坐标，并发送至远程工控机，由远程工控机完成坐标投影变换和实时显示。远程工控机遥控器操作用户根据软件系统的实时画面进行下一步操作，如此反复，最终完成掘进任务。工作流程可分为2 个主要部分，即炮头定位和实时精确显示。

4.2.1 炮头定位

炮头定位指炮头坐标信息的确定，是工作流程的核心。掘进机自身的位姿由第3 节所述掘进机位姿确定方法确定，具体为掘进机自身坐标系的求算和掘进机原点坐标的空间位置解算。炮头在掘进机坐标系下相对于掘进机坐标原点的坐标增量为(Δx, Δy,Δz)，可以根据掘进机运动学原理和掘进机控制炮头上下摆动和左右摆动的油缸行程确定[8]。根据3.3 节定位算法，将掘进机空间坐标系向量(rx,ry,rz)、掘进机原点坐标(xr,yr,zr)和炮头坐标增量(Δx,Δy,Δz)代入式(2)，可以解算炮头的绝对空间坐标点p，完成炮头定位。

4.2.2 实时精确显示

掘进机作业的状态需要实时准确地显示在监控屏幕上，以便根据实时工作状态进行高效精准的掘进作业。掘进机炮头在巷道的位置、掘进机相对于巷道的位置和姿态是需要精确显示的核心内容。为了准确直观地呈现上述信息，需要进行坐标投影变换。具体变换为，将掘进机炮头的大地坐标投影到巷道断面，并转换为巷道断面坐标系坐标，然后直观呈现巷道、炮头以及它们之间的相对位置关系。

具体步骤如下。

1)根据3.3 节定位算法，求得掘进机坐标系OrXrYrZr向量(rx,ry,rz)和原点坐标(xr,yr,zr)。

2)由于掘进机本身为一刚体，除去悬臂和炮头，机身可简化为一个立方体，因此可以根据定位算法和测量所得坐标增量求得掘进机机身的特征点，包括掘进机机身上平面的4 个点a、b、c、d和下平面的4个点a1、b1、c1、d1，如图4 所示。图4 中，f1f2f3f4为断面。由于3.2.1 节已经确定了炮头顶点p，则可以将掘进机炮头投影至当前截割面，显示实时位置关系。

图4 掘进机与断面示意

3)掘进机的位姿以三视图的形式呈现，在三视图中的坐标求解方法如下。对于侧视图，把掘进机的侧平面顶点b、c、c1、b1和炮头顶点p投影至巷道的左帮断面，则可以显示从侧面观察的实时精确位置关系。后视图把掘进机的上平面顶点a、b、c、d投影至巷道截割面，而俯视图则把前平面abb1a1投影至巷道底板。由此可以得到掘进机和巷道的掘进机的精确姿态和掘进机相对于巷道的精确位置关系。

根据以上步骤，掘进机可以实时感知自身状态和炮头的精确位置，并和周围环境进行精确交互，从而实现精准掘进。

5 定位算法验证与掘进机器人应用

本文掘进机器人系统已经应用于实际煤矿开采和掘进作业，巷道宽4.2 m，高4 m，安装了全站仪的定位系统与掘进机上的定位靶球间距离约为15～40 m。

系统的运行界面如图5 所示。其中，左侧为模式选择和状态显示部分，中间主图显示了截割断面、炮头在断面中的位置及二者的相对位置关系，右侧表示掘进机和巷道的精确相对位置关系和精确位姿。

图5 右侧从上到下依次为掘进机实时姿态的侧视图、俯视图和后视图。侧视图显示了掘进机相对于巷道位置关系、炮头在断面的高度以及炮头距切割面的距离关系。俯视图显示了炮头在水平方向上和巷道的相对位置关系、炮头和巷道前方待切割断面的垂直距离以及炮头和当次截割的煤壁后端面的位置关系。在狭窄巷道作业时，还需要注意一种情况，即炮头看起来可以到达，但是实际上却因为悬臂被煤壁卡住无法到达，为了提醒掘进机司机，侧视图中标出了悬臂最宽处距离巷道两帮的距离（斜线的两端点为悬臂最宽处距离巷道两帮的距离）。后视图显示了从后方观察到的掘进机机体和巷道的位置关系。此外，图5 中的小图标分别表示掘进机的各种状态，如铲板升降、前后支撑和左右星轮运动等。

图5 运行界面

5.1 算法比较

为了对比本文所提定位算法与基于旋转姿态角的定位算法，分别测试了3 组数据样例。旋转矩阵方法分别对3 个角度，即方位角yaw、俯仰角pitch和滚动角roll，以一定的顺序旋转，所有可能的旋转顺序有6 种，所有可能的旋转顺序和相应的结果如表1 所示。

掘进机坐标系和大地坐标系均采用右手法则，向上为Z轴正向，向右为X轴正向。

根据表1 可知，只有当姿态角均为45°，且坐标在X轴或Y轴上时，才有一种旋转顺序可以得到和本文定位算法一致的正确结果，但用于X轴和Y轴时对3 个姿态角的旋转顺序不确定。而其他坐标的定位结果显示，无论采用什么角度，进行表1 中任一种顺序的姿态角旋转变化，得到的坐标和由实时姿态角求得的定位结果不一致。由此可见，用于旋转的姿态角和惯导输出的实时状态的姿态角没有关系，使用旋转惯导实时姿态角的方式进行定位无法得到正确的定位结果。

5.2 精度分析

为了测试本机器人的系统误差，用全站仪实测了大约100 个点，从中选择误差较大的能够体现定位精度的边界点和拐点测量结果，并与机器人系统得到的定位结果进行对比，如表2 所示。

表1 旋转矩阵方法与本文定位方法的结果对比

表2 定位坐标和实测坐标的对比

表2 中，mx、my和mz表示实际测量坐标的3个坐标分量，px、py和pz分别表示定位结果的坐标分量。把实测结果和定位结果代入式(3)，可进行均方根误差计算。

其中，mi,pi分别表示实测结果和定位结果，n表示坐标点的数目。计算得到3 个坐标分量的误差结果分别为0.049 m、0.021 m 和0.059 m，三者平均误差为0.043 m，达到了较高的截割精度。

本机器人系统测试运行时间已达90 天，系统运行稳定，能准确地按照巷道设计进行截割作业，初步达到了设计目标，为掘进工作面无人化奠定了坚实技术基础。

6 结束语

本文首先基于矿山物联网，提出了一种新的高精度的井下定位的方法，且配置方便，易于部署和使用，解决了掘进机机器人化的核心技术难题。为推动掘进机机器人化的快速发展和安全高效的井下掘进奠定了技术基础。然后矿山物联网和精准定位方法，研发了掘进机器人系统，应用本文机器人系统可以实时定位掘进机，精确感知周围环境，实现和周围环境的精准交互，从而实现高精度掘进。误差结果和系统应用验证了本系统的精度和高可用性。

未来的工作有两方面，一是研发三维真实镜像系统，使系统能实时显示掘进的真实场景，从而使掘进工作工作面透明化；二是在此基础上发展完全自动化的智能掘进系统。