悬臂式掘进机位姿检测方法研究现状

马源， 符世琛， 张子悦， 王东杰

(1.长安大学 汽车学院， 陕西 西安 710018；2.中国矿业大学(北京) 机电与信息工程学院， 北京 100083)

0 引言

井下综掘工作面是煤矿灾害事故的高发区，实现综掘工作面无人化迫在眉睫。悬臂式掘进机(以下简称掘进机)是综掘工作面的核心设备，掘进机的实时自主导航对实现综掘工作面无人化至关重要[1-3]。掘进机导航的核心是掘进机位姿检测。在空间狭小、装备繁杂的巷道内，掘进机位姿检测受干涉影响大、定位参照差、标定工序复杂等制约；当掘进机掘进距离超过系统有效测量距离时，仍需要人工进行标定；位姿检测参数不完全，位姿检测精度低。为解决这些问题，许多学者将各种先进技术应用于掘进机位姿检测中，提出了多种位姿检测方法。基于前人的研究，本文介绍了掘进机位姿检测原理，分析了目前较成熟的掘进机位姿检测方案原理及优缺点。

1 掘进机位姿检测原理

掘进机位姿参数主要包括掘进机机身的三维坐标及三轴姿态角(航向角、俯仰角及横滚角)，掘进机位姿检测即掘进机三维坐标(掘进机定位)及三轴姿态角(掘进机定姿)的测量或推算。掘进机姿态角可通过多个机身节点三维坐标推导得出，亦可通过倾角传感器、电子罗盘及惯性测量单元等测得。但掘进机定位需要建立巷道基准坐标系与机身坐标系的几何关系，由于巷道环境特殊，该几何关系往往不易获得，所以掘进机定位是掘进机位姿检测的关键。

掘进机定位原理如图1所示。首先，通过基站与终端设备间的信号收发，获得机身目标点与巷道基准坐标系间的原始物理参量(频率、振幅、相位、时间等)；然后，原始物理参量经处理后得到定位参量(来波方向、空间距离、高程等)，再根据定位参量建立定位观测方程组；最后，通过求解观测方程组得到位置面(在一个特定三维空间中，保持某一个定位参量恒定的点的轨迹)的交点或最优估计值，即机身目标点三维坐标[4]。

图1 掘进机定位原理Fig.1 Principle of roadheader positioning

按照原始物理参量来划分，目前掘进机位姿检测方法主要包括基于电磁波的掘进机位姿检测方法、基于机器视觉的掘进机位姿检测方法、基于惯导的掘进机位姿检测方法等。其中基于电磁波的掘进机位姿检测方法根据电磁波的频率和传播特性的不同，又分为基于可见激光(扇面激光)的掘进机位姿检测方法、基于非可见激光(iGPS(Indoor GPS，室内GPS)、全站仪)的掘进机位姿检测方法、基于无线电波(UWB(Ultra Wide-Band，超宽带))的掘进机位姿检测方法。

2 基于扇面激光的掘进机位姿检测系统

李睿等[5]提出了一种基于扇面激光的掘进机位姿检测系统，如图2所示。该系统核心设备为1台扇面激光发射器和2部激光标靶：扇面激光发射器安装在巷道后方的顶板；2部激光标靶平行安装在掘进机机身上方，并且垂直于机身中线。系统工作时，扇面激光沿巷道中线发射，机身上方2部激光标靶接收到光信号，根据简单几何推导，可实时获得掘进机机身中线相对于巷道中线的偏向角和偏向位移。扇面激光发射器保证自身位置与巷道后方的点激光指向仪处于巷道设计中线上，扇面激光的照射距离为30～100 m。此外，该系统搭配双轴倾角传感器，以测量机身的俯仰角、横滚角。

图2 基于扇面激光的掘进机位姿检测系统Fig.2 Pose detection system of roadheader based on sector laser

该系统在有效测量距离内，偏向角误差小于0.1°，偏向位移小于82.2 mm，俯仰角及横滚角误差小于0.05°；但无法检测掘进机机身Y轴、Z轴坐标。

3 基于iGPS的掘进机位姿检测系统

贾文浩等[6]提出了一种基于iGPS的掘进机位姿检测系统，如图3所示。将3台激光接收器安装在掘进机机身上，其在机身上的位置固定已知；2台扇形激光发射站安装在掘进机后方的煤巷中，扇形激光发射站三维坐标由地测人员事先标定。通过测量掘进机机身上激光接收器在巷道基准坐标系的三维坐标，根据几何推导解算出机身三轴姿态角。

图3 基于iGPS的掘进机位姿检测系统Fig.3 Pose detection system of roadheader based on iGPS

单发射站测量原理如图4所示。将扇形激光发射站抽象为2束扇形激光平面绕一固定转轴旋转至固定位置向四周空间发射LED脉冲信号的仪器，设扇形激光发射站的激光发射器几何中心为O点，激光接收器为P点，扇形激光发射站旋转方向上第1束扇形激光平面与垂直于固定转轴且过激光平面交点的平面的交线为X轴，掘进机掘进方向为Y轴，扇形激光发射站固定转轴为Z轴，扇形激光发射站转速固定且已知。激光接收器的相对高程可由两扇形激光平面扫过激光接收器的时间差及角度θ，φ1，φ2(θ为两扇形激光平面在中心平面上的投影夹角，φ1，φ2分别为两扇形激光平面与固定转轴的夹角)来描述。当确定激光接收器的相对高程及扇形激光平面与固定转轴的夹角φ1后，根据图4(b)所示的几何关系，可进一步推导得到P点相对于O点的水平角α1及俯仰角φ1。

(a) 两扇形激光平面相对位置关系

(b) 扇形激光平面扫描至激光接收器的姿态

为了解算激光接收器的三维坐标，至少需要2台扇形激光发射站与激光接收器之间的角度定位参量，双发射站测量原理如图5所示。当激光接收器P点相对于另一台扇形激光发射站几何中心点Q的水平角α2、俯仰角φ2和双发射站之间的距离lOQ已知时，联立水平角α1及俯仰角φ1计算得到激光接收器P点的三维坐标(x，y，z)。

(1)

图5 双发射站测量原理Fig.5 Measuring principle of dual-station

该系统在扇形激光发射站间距为1.2 m时，具有较高测量精度，在有效测量距离17 m内，三维坐标测量误差小于7.4 cm，姿态角测量误差小于1°；但在实际使用中所需测量距离通常大于17 m，此时系统精度无法保证。

4 基于全站仪的掘进机位姿检测系统

陶云飞[7]提出了一种基于全站仪的掘进机位姿检测系统，如图6所示。3台机身棱镜安装在掘进机机身且相对位置关系已知，单台自动全站仪安装在巷道后方顶板处，单台后视棱镜安装在巷道初始位置且三维坐标已知。系统工作时，由自动全站仪根据后视棱镜标定自身三维坐标，之后对掘进机单台机身棱镜进行扫描，当接收到机身棱镜返回的光信号后，确定机身棱镜相对于自动全站仪的水平角、俯仰角及空间距离，由此推算出机身棱镜三维坐标，再根据3台机身棱镜间的确定位置关系推算出掘进机机身姿态角。

图6 基于全站仪的掘进机位姿检测系统Fig.6 Pose detection system of roadheader based on total station

全站仪测量原理如图7所示。设P1点为单台机身棱镜位置，O点为全站仪位置。当全站仪对P1点进行扫描时，记录全站仪内扫描发射器的水平角α3及俯仰角φ3，同时对棱镜进行空间测距，得到距离信息l1。根据式(2)可推导出机身棱镜相对于全站仪的三维坐标(x1，y1，z1)。

图7 全站仪测量原理Fig.7 Measuring principle of total station

(2)

该系统在60 m的有效测量距离内，三维坐标测量最大误差不超过9 cm，搭配倾角传感器时姿态角测量最大误差不超过0.01°；但全站仪在巷道顶板上的安装、标定、整平及移站工作较为复杂。

5 基于机器视觉的掘进机位姿检测系统

基于机器视觉的掘进机位姿检测系统利用各种视觉传感器在井下感测周围空间环境，具有复杂环境适应性好、精度高等优势[8]。

杜雨馨等[9-10]提出了一种基于十字激光与机器视觉的掘进机位姿检测系统，如图8所示。2台激光标靶安装在掘进机机身且相对位置已知；2台机器视觉摄像仪分别安装在激光标靶后方；2台十字激光发射器安装在巷道后方，其三维坐标由地测人员事先标定。系统工作时，2台十字激光发射器向掘进机掘进方向发射2组十字激光，分别照射在2台激光标靶上，单组十字激光成像与单台激光标靶四周会产生4个交点和1个中心交点，2台激光标靶共产生10个交点并由机器视觉摄像仪记录，根据各交点的相对关系可推导出掘进机相对于十字激光发射器的3个姿态角及X轴、Z轴坐标偏差。

图8 基于十字激光和机器视觉的掘进机位姿检测系统Fig.8 Pose detection system of roadheader based on cross laser and machine vision

该系统在100 m的有效测量距离内，机身姿态角解算误差不超过0.16°，机身X轴、Z轴坐标最大误差小于10 mm；但无法检测掘进机Y轴坐标。

杨文娟等[11]提出了一种基于组合激光和机器视觉的掘进机位姿检测系统，如图9所示。在巷道顶部的轨道上安装2台点激光指向仪，使其发射的2条激光束平行且激光束方向沿掘进方向；在轨道上沿巷道掘进垂直方向另安装1台点激光指向仪，使其发射1条水平激光束且与上述2条平行激光束垂直。激光标靶安装在掘进机截割臂上；2台防爆相机安装在掘进机机身后方，其中一台面向点激光指向仪，采集水平激光束和平行激光束的图像信息来检测机身位姿，另一台面向激光标靶，采集激光标靶的图像信息来检测截割头位姿。

图9 基于组合激光和机器视觉的掘进机位姿检测系统Fig.9 Pose detection system of roadheader based on combined laser and machine vision

当掘进机正常工作时，面向点激光指向仪的防爆相机开始采集水平激光束和2条平行激光束的图像信息，并进行杂光过滤、图像增强、畸变校正等处理，利用Hough算法进行直线检测，提取直线特征，建立基于2个平行激光束发射点、3条激光束的位姿解算几何模型，从而获得掘进机坐标系与巷道基准坐标系间的确定关系，基于掘进机坐标系中防爆相机的视觉信息，推算获得巷道基准坐标系中机身位姿。此外，面向激光标靶的防爆相机采集激光标靶的图像信息，经图像处理和数学拟合运算后最终可解算得到截割头位姿信息。

该系统截割头位置测量误差不超过40 mm，姿态角测量误差不超过0.5°；但需要全站仪辅助测量，且掘进机机身坐标计算复杂，精度不高。

6 基于UWB的掘进机位姿检测系统

符世琛等[12-13]提出了一种基于UWB的掘进机位姿检测系统，如图10所示。3个UWB节点模块安装在掘进机机身；4台UWB定位基站布置在巷道后方，其相对于巷道基准坐标系的初始坐标由地测人员在巷道掘进工作开始之前标定。4台UWB定位基站分别对3个UWB节点模块进行UWB测距，得到12组UWB测距信息，从而建立定位观测方程组，通过求解方程组获得UWB节点模块的三维坐标，进而推导得到机身姿态角。当掘进机掘进距离超过UWB测距有效范围时，可通过4台UWB定位基站依次移动进行坐标系变换，单台UWB定位基站移动后通过其他UWB定位基站对其进行位置标定，基于UWB信号全向辐射的特点，便于进行自主标定，从而消除UWB定位基站移动造成的累计误差。

图10 基于UWB的掘进机位姿检测系统Fig.10 Pose detection system of roadheader based on UWB

UWB定位解算的核心难点是定位观测方程组求解。由于UWB信号的全向辐射特点，定位观测方程组一般为非线性方程组，在工程上采用的UWB定位基站数量较少时，存在无解情况；常用的求解方法是将非线性定位观测方程组线性化，进而采用最小二乘法及循环迭代法进行最优估计。

该系统在70 m的有效测量距离内，X轴坐标误差小于25 cm，Y轴坐标误差小于2 cm，Z轴坐标误差为10～65 cm，航向角误差小于5°，横滚角及俯仰角误差小于10°；但姿态角解算精度不高，需要搭配倾角传感器辅助测量，且Z轴坐标误差较大。

7 基于惯导的掘进机位姿检测系统

毛清华等[14-15]提出了一种基于惯导的掘进机位姿检测系统，原理如图11所示。通过三轴陀螺仪测量掘进机绕三轴旋转的角速度，通过三轴加速度计测量掘进机三轴方向上的线加速度，将以上6个参量对时间二次积分后与掘进机初始位姿联立进行航位推算，得到掘进机坐标及姿态计算初始值，再根据相应补偿策略及其他标定方法循环迭代进行精度优化，最终确定掘进机的三轴姿态角及三维坐标。

图11 基于惯导的掘进机位姿检测系统原理Fig.11 Principle of roadheader pose detection system based on inertial navigation

在2 h内的静态测量条件下，该系统的航向角误差小于0.2°，俯仰角及横滚角误差小于0.1°；但该系统本质上无法建立掘进机坐标系与巷道基准坐标系的联系，受航位推算原理限制，三轴坐标测量误差随着时间推移累计增大。

8 结语

为使掘进机位姿检测具有较大实用意义，应满足以下要求：有效测量距离达100 m以上；根据巷道成型标准，断面误差不得大于10 cm，因此在截割头理想作业条件下，三轴坐标误差应小于10 cm；姿态角误差应小于1°。而现有的单一掘进机位姿检测方法难以实现掘进机全自主、全参数、非接触位姿检测，无法满足上述全部要求。基于机器视觉的掘进机位姿检测方法具有掘进机截割头及掘进机机身姿态检测精度高、X轴和Z轴坐标测量精度高的优势；基于UWB的掘进机位姿检测方法具有自主移站便利、Y轴坐标测量精度高的优势；基于惯导的掘进机位姿检测方法具有姿态角测量精度高、独立非接触测量的优势。因此，机器视觉、UWB、惯导相结合的掘进机位姿组合检测方法值得进一步研究。