1、煤矿机械Coal Mine MachineryVol.44 No.5May.2023第44卷第5期2023年5月doi:10.13436/j.mkjx.2023050200引言悬臂式掘进机是煤矿井下巷道掘进的核心装备，实现其自动位姿检测和纠偏是煤矿生产智能化、无人化的关键技术之一。传统的掘进机定向方法依靠激光指向仪，掘进机司机通过观察巷道断面激光光斑位置来调整机器在巷道中的位姿，进而控制截割头实现特定轨迹的运动。由于现场环境恶劣、光线差，断面成形质量完全依赖司机经验，且无法实现自动截割。对于井下掘进作业，近年来相关企业和科研院所对掘进机位姿测量方法的研究取得了一定的成果。文献4研究了基于视觉和
2、惯导的掘进机实时位姿组合测量方法，通过捷联惯导测得掘进机姿态信息、单目视觉测量位置信息。该方法虽然避免了惯导位置测量累积误差大等缺点，但对于姿态的测量是基于掘进机静态环境下所得，且视觉算法相对复杂。文献5提出基于超宽带技术的定位、定向方法，并采用基站群自主标定策略，提高了有效测量范围。该方法对基站群布置有较高要求，应用于狭长式巷道测量精度有待进一步提高。现有研究方法应用各种不同类别的测量仪器，试图得到掘进机在巷道中完整的六自由度位姿，虽然取得一定的效果，但由于应用场景的复杂性，难以稳定、持续地在现场实施应用。掘进机自动化作业过程中，定位的实质是确定机器在巷道中的位置和姿态，特别是机器在巷道中的
3、方位角和中线偏移距离。获得上述参数后，自动控制机身向中线附近矫正，可为自动化作业提供必要的技术支撑。本文正是基于上述考虑，采用三维激光雷达识别导向杆，用于替代传统的激光手动指引，实现关键参数的自动测量。基于三维激光雷达的掘进机实时位姿纠偏系统石勇（上海创力集团股份有限公司，上海201706）摘要：为实现煤矿掘进作业自动化，针对煤矿巷道掘进作业现状，提出一种基于3D激光雷达的动态测距、测向方法，用于实时控制掘进机位于巷道中线位置。以高反射率导向杆模拟巷道走向，通过设定雷达反射率阈值将导向杆从低反射率环境提取。建立导向杆空间坐标转换模型，并采用线性最小二乘拟合得到机身相对导向杆偏移距离和角度，以此
4、引导掘进机在巷道中自动位姿调整。算法过程增加滤波处理，有效降低干扰影响，提高系统稳定性。该系统测量精度高，能够有效替代传统的人工指向方法，为掘进机自动化作业奠定基础。关键词：掘进机自动化；3D激光雷达；导向杆；实时位姿纠偏中图分类号：TD421.5文献标志码：A文章编号：1003 0794（2023）05 0064 03Real-time Pose Correction System of Roadheader Based on Three-dimensional LidarShi Yong(Shanghai Chuangli Group Co.,Ltd.,Shanghai 201706,Ch
5、ina)Abstract:In order to realize the automation of coal mine driving operation,a dynamic ranging anddirection testing method based on 3D lidar was proposed according to the driving operation status ofroadheader in coal mine,used for real-time control the position of roadheader at the center line ofr
6、oadway.Taking the guide rod with high reflectivity to simulate the trend of roadway,the lidarreflectivity threshold was set to extract it from low reflectivity environment.The space coordinatetransformation model of the guide rod was established,and the offset distance and angle of the fuselagerelat
7、ive to the guide rod were obtained by linear least square fitting,so as to guide the automaticposition and posture adjustment of the roadheader in the roadway.Adding filtering to the algorithmprocess can effectively reduce the interference and improve the measurement stability.The system hashigh mea
8、surement accuracy,which can effectively replace the traditional manual pointing method andlay the foundation for automatic cutting of roadheader.Key words:roadheader automation；3D lidar；guide rod；real-time pose correction641掘进机实时位姿纠偏系统总体设计掘进机实时位姿纠偏系统构成如图1所示，包括掘进机、三维激光雷达、导向杆和控制单元等。三维激光雷达安装于机身末端右侧，通过持
9、续发射多线激光束，然后比较从目标反射回来的信号，获得目标距离和反射率等信息。导向杆作为巷道走向指引线由人工安装于距巷道顶端一定距离，位于巷道中线下方。巷道掘进过程中掘进机间歇式往复行走，为导向杆布置提供了时间和空间的便利性。控制单元安装于机身电控箱，用于处理激光雷达所测数据，并给出控制参数。图1实时位姿纠偏系统构成示意图基于上述原理的掘进机自动纠偏控制系统采用闭环控制，激光雷达检测到导向杆信号后，通过算法处理得到掘进机机身相对于导向杆的偏移参数。当偏移值大于给定的控制阈值，则持续通过纠偏算法控制机器行走，整体控制框图如图2所示。图2掘进机纠偏控制框图2位姿解算模型（1）偏移参数定义在机器截割回
10、转中心建立机器坐标系X1Y1Z1，在巷道断面中轴线机器回转中心等高处建立巷道坐标系XYZ。机器坐标系与巷道坐标系在XY面的投影关系决定了机器在巷道中的轴向偏移距离x和方位角，如图3所示。图3巷道坐标系和机器坐标系投影1.导向杆2.激光雷达（2）雷达-机器坐标系转换激光雷达作为感知元件安装于掘进机末端，其固有坐标系X1Y1Z1与机器坐标系X0Y0Z0相对关系如图3所示。当激光雷达采集到导向杆数据后，需从雷达固有坐标系转换至机身坐标系，转换关系为P0=RP1+t（1）由雷达在机器安装位置可知R=RyRxRz（2）机器坐标系相对雷达坐标系的姿态旋转矩阵Rycos 0 sin；0 1 0；-sin 0
11、 cos Rx1 0 0；0 cos -sin；0 sin cos Rzcos -sin 0；sin cos 0；0 0 1|（3）式中P1雷达测得的导向杆三维坐标向量；P0转换至机器坐标系中的三维向量；t机器坐标系相对雷达坐标系的平移向量；雷达坐标系绕机器坐标系Y0轴姿态角；雷达坐标系绕机器坐标系X0轴姿态角；雷达坐标系绕机器坐标系Z0轴姿态角。（3）参数计算经过上述坐标变换得到导向杆雷达点云数据在机器坐标系中向量P0，将坐标点投影到X0Y0平面得到xi，yi，i=0，1，n。采用最小二乘线性拟合，得到导向杆在机器坐标系X0Y0平面线性方程y=a+bx（4）a=（xi2yi-xixiyi）/
12、nxi2-（xi）2（5）b=（nxiyi-xiyi）/nxi2-（xi）2（6）进而计算得到机器相对于巷道中线偏移角度=/2-arctan b（7）机器相对于巷道中线偏移距离x=-a/bcos（8）3软件算法分析本文研究给出的掘进机巷道纠偏方法，能够根据机器在巷道中不同位置实时自动操作，现将导向杆特征点提取和机身位姿自动纠偏原理分析如下。（1）导向杆特征点提取除了距离和角度信息，激光雷达数据包给出了被测物体反射率信息。反射率与物体本身材质有很大关系，煤矿井下主要介质为黑色的煤层，因此采用白色高反射率导向杆即可将其从环境中分离出来，导向杆提取原理如图4所示。图4导向杆提取原理第44卷第5期Vo
13、l.44 No.5基于三维激光雷达的掘进机实时位姿纠偏系统石勇是否雷达信号开始机器位姿信息机器行走机构阈值内纠偏算法结束+每一线按照一定规则划分若干区间按照points垂直角度分成16线points反射率大于给定阈值雷达points水平旋转角位于给定区域各个区间中的points求取均值得到keypoints根据相邻关系从keypoints中提取每一线的导向杆keypoints根据16个导向杆keypoints拟合线性方程掘进机导向杆激光雷达X012XY0X1Z1YX65（2）机身位姿纠偏原理根据上述提取的导向杆数据，即可以依据上述计算得到机身与巷道中线的偏移参数。通过偏移参数调整机身自动回正原
14、理如图5所示。图5机身纠偏原理图5中机身角度和距离的正负定义在巷道坐标系中，将掘进机在巷道中按位姿划分成4个象限，通过控制机器行走机构实现机身偏移纠正。4纠偏方案验证（1）现场布置基于本文方法搭建的掘进机巷道偏移检测现场如图6所示，掘进机位于模拟巷道中，在巷道顶端中轴线处悬挂白色导向杆。激光雷达安装于机器后方，通过雷达扫描并提取导向杆点云数据，通过上述方法计算机器相对于巷道中轴线的偏移距离和角度。当机器存在偏移时，根据纠偏算法实时控制机器向中轴线方向移动。图6系统方案现场图（2）数据分析处理本文采用16线激光雷达检测导向杆，测量精度和稳定性高。将现场实测得到的雷达点云数据处理后，得到机器相对中
15、轴线偏移距离和角度，数据统计如图7所示。依据图7中数据求得导向杆偏移距离和角度均值为-0.07 m、-24.68，且数据近似符合正态分布，与实际情况符合良好。说明本文提出的基于激光雷达的导向杆测量方案能够有效测出导向杆偏移中轴线的距离和角度，且对干扰噪声抑制能力较好。5结语针对煤矿巷道掘进工作面作业现状，提出了一种基于3D激光雷达的掘进机自动纠偏方法。通过人工布置导向杆模拟巷道中轴线，可实时动态检测掘进机相对于巷道中轴线的偏移距离和角度，结合掘进机纠偏控制算法，能够有效调整机器在巷道中的相对位姿，为掘进机自动化作业提供了便利。偏移距离/m（a）偏移距离偏移角度/（）（b）偏移角度图7实验测试结
16、果参考文献：1王国法,王虹,任怀伟,等.智慧煤矿2025情景目标和发展路径J.煤炭学报,2018,43(2):295-305.2葛世荣.智能化采煤装备的关键技术J.煤炭科学技术,2014,42(9):7-11.3方立霞，王彤,李一鸣,等.基于机构拓扑结构的悬臂式掘进机空间位姿偏差分析J.煤炭学报,2020,45(11):3940-3951.4黄东,杨凌辉,罗文,等.基于视觉/惯导的掘进机实时位姿测量方法研究J.激光技术,2017,41(1):19-23.5符世琛,李一鸣,杨健健,等.基于超宽带技术的掘进机自主定位定向方法研究J.煤炭学报,2015,40(11):2603-2610.6童敏明,杜雨馨,李高军,等.多传感器的掘进机定位系统研究J.煤矿机械,2013,34(6):146-148.7符世琛,成龙,陈慎金,等.面向掘进机的超宽带位姿协同检测方法J.煤炭学报,2018,43(10):2918-2925.作者简介：石勇（1981-），江苏淮安人，工程师，硕士，现主要从业煤矿采掘装备智能化研发工作，电子信箱：.责任编辑：赵荣收稿日期：20220907第44卷第5期Vol.44 No.5