基于三维点云处理的辅助作业机器人位姿控制方法

摘 要：煤矿井下辅助作业机器人位姿控制是提高机器人作业效率和质量的重要手段，目前控制效果不佳，机器人控制误差较大，因此本文提出基于三维点云处理的煤矿井下辅助作业机器人位姿控制方法。对煤矿井下进行三维激光扫描，获取井下三维点云数据，进行三维点云处理，搭建井下空间三维场景模型，提供识别障碍物和作业目标，确定机器人位姿调控量，基于三维点云处理对煤矿井下辅助作业机器人进行位姿控制。试验证明，当采用本文方法时，机器人避障效果较好，位置跟踪误差较低，机器人作业位置与目标位置误差仅为0.1 m，可以对煤矿井下辅助作业机器人位姿进行精准控制。

关键词：三维点云处理；辅助作业机器人；位姿控制；三维激光扫描；障碍物；作业目标

中图分类号：TP 242" " " " " 文献标志码：A

随着煤炭工业快速发展，煤矿井下作业效率和安全性成为行业内关注的焦点。为了提升井下作业的效率和安全性，在煤矿生产中采用智能化设备。煤矿井下辅助作业机器人是一种新型的智能化设备，其具有自主导航、作业执行以及信息感知等功能，其可以通过传感器和定位系统获取周围环境的信息，利用计算机视觉和人工智能技术进行识别和判断。当执行作业任务时，机器人可以利用预设路径或者自主探索方式进行移动，完成各种井下作业任务，例如钻孔、搬运、喷浆和巡检等，节约生产成本，降低事故发生的概率，保障井下作业安全。机器人还可以为矿工提供更好的工作环境，减轻劳动强度，提高工作效率。目前，煤矿井下辅助作业机器人在采矿行业中广泛应用，位姿控制是煤矿井下辅助作业机器人高效、安全作业的关键技术之一。

1 井下作业空间三维激光扫描

煤矿井下作业空间狭窄，环境恶劣，巷道崎岖不平，障碍物较多，光线条件较差，提高了机器人定位建图难度。因此，本文采用三维点云处理技术虚拟化井下作业空间场景，识别场景内机器人作业目标和周围障碍物，为机器人位姿控制提供依据。为实现该目标，需要对煤矿井下作业空间进行三维激光扫描，获取井下作业空间的三维点云数据[1]。将型号为OYTAS-A7F7的三维激光扫描仪安装在机器人上方，探头与地面平行，采用串并联的方式将其接入机器人电源总线上，使两者同步运行。根据实际情况设定三维激光扫描仪扫描范围、周期以及频率等技术方面参数。在扫描过程中，激光测距仪发射一束激光脉冲信号，当接触物体表面时，信号发生漫反射并沿着原路径反向传导至信号接收器，可以得到机器人与扫描点之间的距离，如公式（1）所示[2]。

（1）

式中：h为激光扫描仪与扫描点之间的距离，即机器人与扫描点之间的距离；v为激光脉冲信号传播速度；t1、t2分别为激光脉冲信号发射时间与反射时间。电机驱动三维激光扫描仪上部转动，根据扫描仪竖向镜垂直方向与水平方向转过的角度，得到三维激光扫描仪观测角度值，将测量值映射至扫描仪坐标系中，得到扫描点的坐标，如公式（2）所示[3]。

（2）

式中：Q为扫描仪坐标系；X、Y和Z分别为扫描点在三维激光扫描仪中的横坐标、纵坐标和斜坐标；α为水平方向观测角度；β为垂直方向观测角度。采集煤矿井下作业空间三维点云数据，上传至计算机，用于三维点云处理。

2 基于三维点云处理的作业三维场景搭建

处理三维激光扫描得到的井下作业空间三维点云数据，搭建井下作业三维虚拟场景。将采集的三维点云数据中的重复部分删除，对异常和缺失数据进行插值处理，保证三维点云数据完整性[4]。当三维激光扫描时会受井下作业噪声干扰，采集的点云数据存在噪声。为了保证控制精度，采用均值滤波法对三维点云滤波进行去噪处理，如公式（3）所示。

（3）

式中：X*、Y*和Z*为三维点云坐标；Qt-1、Qt+1分别为前一个时刻和下一个时刻扫描点坐标值。根据均值对三维点云数据中的离散点进行平滑处理，去除原始点云数据中的噪声分量。将处理好的三维点云数据上传至UFH三维建模软件中，利用转换矩阵将扫描仪坐标系转换为大地坐标系，对三维点云数据进行拼接处理，如公式（4）所示。

（4）

式中：S（x，y，z）为大地坐标系中扫描点位置坐标；x为水平方向坐标；y为竖直方向坐标；z为垂直于两者方向的坐标；E为旋转矩阵；F为平移矩阵。将三维点云数据映射至大地坐标系中，搭建煤矿井下辅助作业空间三维场景模型，为后续机器人位姿控制奠定基础。

3 作业目标定位及机器人位姿控制

在上述基础上识别三维空间场景的障碍物，在三维空间场景中，如果扫描点纵向坐标值高于机器人位置纵向坐标值，则视该点为障碍物，如公式（5）所示。

（5）

式中：g为障碍物识别结果；1为扫描点障碍物；0为扫描点无障碍物；a为机器人纵坐标。利用公式（5）识别得到三维场景中的障碍物，并将机器人作业目标标记在大地坐标系中，在未识别障碍物的情况下，机器人向作业目标成直线移动；当识别到障碍物时，根据机器人当前位姿以及障碍物位置，确定机器人位姿调控量。机器人运动学模型如公式（6）所示。

（6）

式中：b为机器人横坐标；γ为机器人姿态角。

机器人动力学模型如公式（7）所示。

（7）

式中：M为机器人移动惯性，即加速度；C为机器人速度与位置有关的向心力；μ为机器人约束反力。利用加速度传感器进行测量，得到机器人的移动惯性，为了改变机器人的运动方向，须改变机器人运动模型中的姿态角。机器人姿态控制可以改变机器人姿态角，越过障碍物向目标位置移动，因此其位姿控制量的计算方法如公式（8）所示。

（8）

式中：υγ为煤矿井下辅助作业机器人位姿控制量；δ为机器人作业目标位置；S*（x，y，z）为机器人前方障碍物坐标。对机器人进行位姿调控，改变机器人移动方向，越过障碍物。将以上计算的控制量发送至控制器中，利用控制器对机器人位姿进行调控，对基于三维点云处理的煤矿井下辅助作业机器人进行位姿控制。

4 试验论证

4.1 试验工况

煤矿井下采掘工作面风流温度为28 ℃～30 ℃，作业地点的风流速度为2.5 m/s～3.0 m/s，湿度为80%～100%，大气压为103 hPa～108 hPa，甲烷含量为0.1%～15.0%，井下辅助作业机器人承压为5.5 MPa，导热系数为0.02 W/（m·℃）。

4.2 试验准备与设计

为验证设计方法能否应用于煤矿井下作业环境，对煤矿井下辅助作业机器人进行试验，机器人型号为KHFA-SG455，移动速度为1.5 m/s，井下巷道宽度为5.14 m，长度为1 564.52 m，利用本文方法对煤矿井下辅助作业机器人进行位姿控制。试验过程为在井下辅助作业机器人上安装1台三维激光扫描仪，采集井下三维激光扫描数据0.26 GB，分析并处理三维激光扫描数据，搭建井下三维场景，控制机器人位姿，使其完成井下500 m作业任务，煤矿井下辅助作业机器人位姿作业任务如图1所示。从图1可以看出，在对机器人进行位姿控制的过程中，机器人能够绕开所有障碍物并按照规定路线向目标移动，说明本文方法基本可以完成煤矿井下辅助作业机器人位姿控制任务。

4.3 试验结果

4.3.1 煤矿井下辅助作业机器人避障效果测试

应用设计方法后的机器人位姿作业路径与煤矿井下辅助作业机器人位姿作业任务路径轨迹越一致，方法避障能力越强。具体测试结果如图2所示。

根据图2可知，设计方法采用三维点云处理技术，利用激光雷达传感器获取井下环境的点云数据，这些数据包括丰富的空间信息和几何特征，因此设计方法与煤矿井下辅助作业机器人位姿作业任务路径基本一致。处理并分析这些点云数据，得到包括周围障碍物的位置、形状和大小等准确的煤矿井下作业空间环境信息，构建井下作业空间的三维模型，识别机器人作业目标和周围障碍物。由于避障路径的规划基于准确的三维模型和障碍物信息，因此，当遇到障碍物时，煤矿井下辅助作业机器人能够做出快速、准确的反应，避免与障碍物发生碰撞。

4.3.2 煤矿井下辅助作业机器人位置跟踪误差测试

位置跟踪误差测试能够直接反映煤矿井下辅助作业机器人的定位精度，对评估机器人的导航和定位能力十分重要。利用EFly智能航测软件，采用自动数据采集终端采集设计方法的煤矿井下辅助作业机器人位置信息。获取机器人在X轴、Y轴和Z轴的平均位置跟踪误差，误差越低，煤矿井下辅助作业机器人位姿控制效果越好。具体测试结果如图3所示。

由图3可知，采用均值滤波法对三维点云数据进行滤波去噪处理，并对离散点进行平滑处理，有效去除原始数据中的噪声分量，减少数据中的随机误差和异常值，点云数据更平滑和连续。因此，利用本文方法控制的矿井下辅助作业机器人在X轴、Y轴和Z轴的平均位置跟踪误差均较低，三维轴向的误差波动较平缓。将处理好的三维点云数据上传至UFH三维建模软件中，及时调整机器人的路径和姿态，完成动态避障。利用转换矩阵将扫描仪坐标系转换为大地坐标系，对三维点云数据进行拼接处理，使机器人在井下作业过程中的位置数据与实际环境坐标系一致，提高了机器人位姿控制精度。

4.3.3 煤矿井下辅助作业机器人位姿控制精度测试

极值搜索算法是一种自适应控制方法，其可以在时变环境中寻找系统的最佳性能点。优化机器人位姿参数，利用极值搜索算法取得最佳作业效果。模糊PID控制算法是一种结合了模糊逻辑和传统PID控制的方法，利用模糊化处理系统输入和输出，改善传统PID控制器对非线性、时变系统的控制性能。模糊PID控制算法根据机器人当前位姿误差和误差变化率动态调整PID控制参数，提升机器人在复杂环境中的位姿控制性能。为了验证方法效果，试验选择极值搜索算法（传统方法一）和模糊PID控制算法（传统方法二）与本文方法进行对比。试验统计了7次机器人作业任务中目标点位置与实际位置的数据，机器人作业位置与目标位置越近，机器人位姿控制精度越高，效果越好，统计数据见表1。

根据表1的数据得出结论，应用本文方法，机器人作业位置与目标位置最大误差仅为0.1 m，传统方法一的最大误差为1.2 m，传统方法二的最大误差为1.8 m，大于本文方法的最大误差。试验结果说明采用本文方法，机器人位姿控制精度较高。

5 结语

本文提出一种基于三维点云处理的煤矿井下辅助作业机器人位姿控制方法。经过试验验证，本文方法能够对煤矿井下辅助作业机器人进行精确定位和姿态控制，提高机器人在井下作业的效率，保障安全。该方法还可以为其他类似的机器人系统提供一种有效的位姿控制方法。该方法在实际应用中仍然存在一些挑战。例如，井下环境的复杂性和动态性可能导致点云数据的获取和处理难度较大；机器人在井下的运动受到限制和约束，需要更精确的轨迹规划和姿态控制。为了解决这些问题，从以下2个方面进行研究。1）进一步研究点云数据的获取和处理技术，提高数据的准确性和实时性。2）探索更有效的位姿估计方法，提高机器人的定位和姿态控制精度。

参考文献

[1]赵喜江，杨志强，张宇.煤矿竖井井筒三维激光扫描的误差分析与控制[J].黑龙江科技大学学报，2022，32（4）：6.

[2]毛成林，于瑞强，宋爱国.基于各向异性空间投影的地铁列车底部巡检机器人避障研究[J].机械与电子， 2023，41（8）：76-80.

[3]李鸣昊，王辉，伍思欢，等.考虑执行器最优分配的空间站舱内机器人位姿耦合控制[J].宇航学报，2023，44（7）：1073-1083.

[4]韩勇，霍迎秋，费攀锋.基于模糊PID的电力铁塔攀爬机器人末端位姿控制研究[J].工业仪表与自动化装置，2023（3）：80-84，87.