多点标识下的挖掘机工作装置姿态测量

王杰栋，黄家海，2，兰 媛，2，熊晓燕，2

（1.太原理工大学机械与运载工程学院，山西 太原 030000；2.太原理工大学新型传感器与智能控制教育部重点实验室，山西 太原 030000）

1 引言

挖掘机广泛应用于各种施工场合，在工程机械中占有十分重要的地位。挖掘机作业环境复杂，常常伴有噪声、粉尘，甚至存在塌方、辐射等危险，不利于操作人员长时间工作。挖掘机智能化、自主化成为未来发展趋势［1-2］。

挖掘机自主智能化研究，一直以来都是很具挑战的课题，国内外研究人员为此做了大量的研究工作，文献［3-4］采用激光测距仪对作业环境进行感知，以此实现挖掘作业过程中对挖掘点和运载目标的准确可靠识别；文献［5-6］为实现挖掘机自主化挖掘，在挖掘机上安装激光扫描仪来检测环境、障碍和土质；文献［7-8］利用普通相机、立体相机和激光测距仪来进行环境感知实现挖掘机避障和挖掘目标的检测；文献［9-10］利用激光雷达建立了一套具备站点建模能力的挖掘机机器人。这些挖掘机的自主智能化大多体现在环境感知、避障和挖掘目标识别等方面，在挖掘机工作装置姿态测量方面，更多的是以电位计、关节角度编码器等传统接触式传感器来实现。工作装置姿态信息作为控制系统的重要参数，实时精准地获取姿态信息对自主智能化作业起着至关重要的作用。文献［11-12］针对姿态测量中传统接触式传感器容易碰撞损坏等问题，提出了一种基于视觉测量技术的姿态测量方法。在挖掘机工作装置上固定人工靶标，以工业相机获取图像信息，经图像处理后检测到人工靶标上的角点特征信息或其他特征标识，并将这些特征作为关键点，根据其约束条件获得工作装置的姿态信息［13-14］。在此过程中，由于挖掘机作业环境复杂及其本身具有的棱角，因此相机捕获的图像中元素和噪点过多，从而特征识别算法检测到较多无效的特征点，导致姿态检测失败率高且耗时比较严重；同时，人工靶标上的角点信息在作业过程中不可避免地会被泥土等污染，这也是导致姿态检测失败的原因之一。

因此提出，利用基于深度学习的目标检测算法实现对工作装置上关键点特征的检测。不同于传统机器视觉算法对特定对象特征和人工制作特征的提取，深度神经网络可以从众多样本中提取更多广义特征［15］。基于深度学习的目标检测算法大致可以分为基于候选区域和基于回归两类，与前者相比后者不需要候选区域生成分支，可直接对给定的图像回归出目标类别和候选框［16］。YOLO系列算法作为基于回归类目标检测算法，经过多次改进，YOLOv3已能够以高精度和高速度检测多个标签对象，是现阶段目标检测中，满足实时性和精确性的首要选择之一［17］。YOLOv3算法以其对小目标识别率高、高精度、快响应等优势，可应用于无人驾驶汽车行驶过程中对交通标志的检测［18］。YOLOv3算法对不同颜色目标有较高的识别率，可以此实现对交通信号灯的实时检测，有效地减小光照及汽车尾灯对检测的不良影响［19］。因此利用YOLOv3算法可以有效地提取工作装置上较小关键点的特征，对其进行识别和定位，即使关键点被污染遮挡也可实现检测。

针对现在挖掘机工作装置姿态测量的一些问题，提出一种基于多点标识的挖掘机工作装置姿态测量方法。以不同类型的关键点标识作为工作装置各杆件的关键点，用YOLOv3目标识别算法直接分类并定位这些关键点标识，无需特征筛选，减少检测耗时、提高测量实时性。YOLOv3算法可以在关键点标识被污染遮挡的情况下，实现识别和定位，提高工作装置姿态检测的成功率。

2 YOLOv3算法回顾

YOLOv3算法的核心结构是一个能够一次预测多类目标的卷积神经网络。它可以实现真正意义上的端到端目标检测，具有检测准确率高、速度快的优点。YOLOv3算法使用回归方法代替Softmax方法，实现检测多种类别目标，该算法的神经网络使用了Darknet-53作为整个网络的分类骨干部分，采用残差网络思想，允许构建更深的网络来改善网络中的非线性，并显著改善分类和检测效果。YOLOv3调出从不同比例提取的三个特征图来预测对象。小特征图提供语义信息，大特征图提供更细粒度的信息，YOLOv3结构全貌，如图1所示。

图1 Darknet-53结构与Yolov3网络结构全貌Fig.1 Overview of Darknet-53 Structure and Yolov3 Network Structure

3 关键点坐标变换

关键点是挖掘机工作装置上用于工作装置姿态计算的特征点，工作装置各杆件姿态角是根据关键点的世界坐标计算的，关键点坐标的转换模型，如图2所示。

图2 坐标变换模型Fig.2 Coordinate Transformation Model

假设P(Xw，Yw，Zw)为世界坐标系OwXwYwZw上任意一点，投影到成像平面为点p(x，y)，该点在像素平面坐标系的坐标为(u，v)，将像素坐标(u，v)转换为世界坐标P(Xw，Yw，Zw)的转换过程如下：

（1）像素平面坐标Ouvuv与图像坐标系Oxy

像素平面上的像经过放大处理得到数字图像，像素平面上的成像点(u，v)转换成为图像点(x，y)，则：

式中：(u0，v0)—光轴中心线与成像平面交点的坐标。

将其写成矩阵形式为：

式中：Min—内参数矩阵，该参数矩阵含有4个参数。

（2）图像平面坐标系Oxy与相机坐标系OcXcYcZc

点P在相机坐标系内的坐标为(Xc，Yc，Zc)，是世界坐标在相机坐标系下的投影，是一个三维坐标，将其经投影变换得到其图像坐标(x，y)，则：

经变换可得：

将其写成矩阵形式为：

式中：f—相机焦距。

（3）相机坐标系和世界坐标系

相机坐标与世界坐标的转换关系，可以看成是摄像机的外参数模型，是世界坐标系在相机坐标系中的描述。坐标系(Xw，Yw，Zw)在坐标系OcXcYcZc中的表示，构成摄像机的外参数矩阵：

式中：R—3×3旋转矩阵；T—三维的平移向量；

旋转矩阵R、平移向量T通过相机标定获得，cMw为外参数矩阵。

（4）将以上式子整合为：

式（6）为其坐标转换过程，将点P的图像坐标带入可得到其对应的世界坐标。式中f、u0、v0为相机的物理参数，取决于相机成像芯片以及镜头，R、T经过相机标定可以获得。

4 挖掘机工作装置姿态测量系统

设计的工作装置姿态测量系统，不依赖任何姿态传感器，仅利用一台RGB摄像机实现对工作装置姿态的测量。为工作装置设置易于识别和定位的关键点标识，捕获工作装置图像并标注建立数据集，以此数据集训练获得视觉模型实现关键点标识的检测；根据关键点间的约束关系和相机成像原理还原关键点标识的三维信息，并计算对应机械杆件的姿态角。在背景环境与挖掘机过度融合、关键点标识被污染遮挡的情况下，实现关键点的识别和定位，并完成姿态测量。

4.1 系统概述

为一台无传感器的挖掘机模型配备一个基于多点标识的工作装置视觉测量系统。在姿态测量过程中，工作装置始终在其侧面所在平面内运动，定义此平面为目标平面，对目标的检测即对目标平面的检测。挖掘机工作装置姿态测量系统组成包括计算机、RGB摄像机、挖掘机（目标平面）、数据线等。其安装布置，如图3所示。

图3 姿态测量硬件布置Fig.3 Hardware Layout of Attitude Measurement

如图4所示，测量系统由数据采集模块和姿态估计模块M两部分组成。数据采集模块包括训练图像采集和实时图像采集，姿态估计模块M，用数学方式表达为：

图4 姿态估计系统Fig.4 Attitude Estimation System

该模型接收实时图像x并输出定义的姿态向量p。

式中：p—工作装置在图像中的位置参数，包括9个维度：6个相机外部参数（平移、旋转）和3个姿态角度；x—实时从视觉传感器输入的图像（图片或视频）；θ—可学习参数。

关键点标识在现实中都遵循一定的约束，为了充分利用这些约束，姿态估计模块M：h()可分解为两个分量：二维关键点检测模块M1、三维姿态估计模块M2，用数学方式表达为

二维关键点检测模块

M1：y=f(x；θ)

三维姿态估计模块

M2：(p，z)=g(y；τ)

式中：y∈R8×2、z∈R8×3—关键点在像素平面坐标系和世界坐标系中的位置；θ—可学习参数；τ—对应于几何约束的固定方程组，例如两个关键点之间的长度。也就是说，M1—被训练来优化θ的，而M2则是一种无参数算法，它涉及拟合几个固定的算术方程。

挖掘机工作装置姿态测量系统具体实现过程为RGB摄像机在不同环境下捕获大量具有关键点标识的挖掘机图像数据，目标检测算法利用这些数据训练获得关键点检测模块M1；挖掘机作业过程中RGB摄像机实时捕获工作装置的图像，作为姿态估计模块M的输入，模块M1负责检测该状态下关键点二维坐标，通过模块M2获得关键点的世界坐标和三维姿态；挖掘机控制系统将估计的三维姿态作为输入，控制工作装置变化，变化后的姿态又作为下一轮循环的输入，实现挖掘机工作装置姿态的实时测量。

4.2 关键点标识

工作装置姿态就是工作装置各机械杆件的姿态角，分别为挖掘机动臂与水平基座的夹角α2、斗杆与动臂夹角α3、铲斗（斗尖）与斗杆的夹角α4，如图5 所示。其中，a1、a2、a3分别为挖掘机动臂、斗杆、铲斗。

图5 工作装置姿态角分布Fig.5 Attitude Angle Distribution of Work Device

为了实时准确地测量工作装置各杆件姿态角，关键点的选取要易于识别和定位，由于挖掘机作业环境复杂，挖掘机及其工作装置与背景环境过度融合，没有可以明显区分二者的关键点标识。颜色特征和形状几何特征是深度学习算法易于学习和提取的特征信息，用不同颜色和大小的贴片作为关键点标识，既易于识别和定位又能明显区分工作装置和背景环境。设计了8种类型的关键点标识，关键点标识为颜色不同大小相同的圆形标识，具体参数，如表1所示。工作装置各杆件和水平基座上各布置两种，杆件上两关键点标识中心连线与对应杆件铰点连线平行，其布置方式如图3所示，关键点标识的中心即为关键点，用于姿态角的测量。

表1 关键点标识位置布置与参数Tab.1 Key Point Mark Location Layout and Parameters

4.3 数据采集

为挖掘机模型配备工作装置姿态测量系统，主要是训练获得一个姿态估计模型M，使给定输入图像x，输出姿态预测p。训练这样的模型通常需要大量的标记数据，收集大量不同环境下的挖掘机图像，用于姿态测量模型的训练。数据采集包括训练图像采集和实时图像采集。训练数据D1采集于实验室环境下模拟挖掘机在不同背景环境、光照、障碍物下作业的图像，最大限度地保证工作装置的姿态、背景环境和相机视角的多变性，以确保获得准确的预测模型。采集到图像后，利用标注软件LabelImg对每幅图像上的8类关键点标识进行标注，训练数据样本容量为1300。训练样本按比例6：2：2划分为训练集、验证集、测试集。训练集用于训练或者拟合模型，用于建立模型；验证集用来测试不同模型和参数的表现，借此调整模型；测试集用来评估模型在实际使用时的泛化能力。

实时图像D2是姿态测量系统的输入，是挖掘机在实际作业过程中RGB摄像机捕获的图像。

4.4 相机标定

数学软件MATLAB 具有强大的视觉算法包，其内置的标定方法操作简单，获得的标定精度高，被广泛应用于各种相机标定。相机标定分为两部分：内部参数的校核、外出参数的标定。

4.4.1 内部参数校核

MATLAB相机标定程序原理为张正友棋盘格标定法，利用棋盘格标定板对相机标定，将棋盘格标定板置于相机视场范围内，改变标定板的角度、方向、位置等，摄像机捕获大量图像，导入MATLAB标定软件，角点检测算法检测标定板上各个棋盘格的角点，根据其图像坐标计算获得相机内部参数，完成对内部参数的校核，如图6所示。

图6 相机内部参数校核Fig.6 Camera Internal Parameter Check

4.4.2 外部参数标定

进行相机标定的过程中，我们以标定板边缘角点为世界坐标系的原点，如图7所示。标点板可以看作目标平面，目标平面的世界坐标系对应标定板的坐标系，此标定板的位置决定世界坐标系的位置。

图7 相机外部参数标定Fig.7 Camera External Parameter Calibration

如图7所示，目标平面内所有点Zw=0，固定相机与挖掘机的位置，在不考虑挖掘机回转的情况下，其世界坐标系和相机坐标系的相对位置固定不变，即旋转矩阵R和平移向量T位置固定不变。利用MATLAB标定软件，可获得其固定的旋转矩阵R和平移向量T。

4.5 姿态角的测量

工作装置姿态测量过程中，相机捕获挖掘机作业图像输入姿态测量系统，检测到关键点标识的位置并获得其世界坐标，根据世界坐标计算各杆件在目标平面的方向向量，其表达矩阵为：

其中，4列依次为水平基座、动臂、斗杆的方向向量。

因此，工作装置各杆件相应的姿态角为：

4.6 试验

以挖掘机模型为研究对象，为其配备工作装置姿态测量系统，挖掘机模型本身无任何传感器，为验证姿态测量的准确性，在挖掘机模型上布置传统接触式传感器，以此来获得工作装置的真实姿态，并以此为基准评估姿态估计模型的好坏。

针对工作装置视觉测量中存在的问题，进行两组测试。第一组工作装置上布置设计好的标识，测量其姿态；第二组工作装置上布置被污染的标识，测量其姿态。两组关键点标识，如图8所示。

图8 关键点标识（左：第一组 右：第二组）Fig.8 Key Point Identification（Left：Group 1，Right：Group 2）

关键点标识检测模型训练的超参数设置为：最大训练轮数为100，前50 个epoch 初始学习率配置为0.001，后50 个epoch 初始学习率配置为0.0001，batch size设置为16，优化器为Adam，经训练获得最优的网络模型。在自然光照下，采用分辨率为（960×540）的RGB摄像机，相机帧率为30fps，采用USB接口，工作装置以正常速度作业，姿态估计系统对两组关键点标识的检测，如图9所示。各类关键点标识均可被检测，其部分图像的识别率，如表2所示。第一组检测率均在0.97以上，第二组被污染靶标识别率有所下降，但大部分高于0.9。

表2 部分图像关键点标识识别率Tab.2 Recognition Rate of Key Points in Some Images

图9 二维关键点标识检测（左：第一组 右：第二组）Fig.9 2D Key Point Mark Detection（Left：Group 1，Right：Group 2）

姿态测量系统将二维关键点经坐标变换获得其世界坐标，确定工作装置各杆件姿态角，其动臂姿态角动态测量结果，如图10所示。

图10 动臂姿态角测量结果Fig.10 Measurement Results of Boom Attitude Angle

由图10的两组测量结果可知，设计的姿态测量模型测量的结果与传感器测量系统测量的结果基本吻合。在挖掘机模型模拟作业的过程中，由于挖掘机微小振动，使所测数据上下浮动，在工作装置突然剧烈晃动时会出现突变的情况，如图10 中出现的脉冲现象，此类数据具有较大的误差。为避免由此类现象而导致的误差，对前后两次所捕获的图像获得的姿态估计结果进行比对。定义两次结果之差为ΔA，若实际差值a＞ΔA，则舍弃此组数据；若实际差值a＜ΔA，则保留此组数据。经处理后结果，如图11 所示。结果明显改善，且不影响姿态估计模型的测量准确性。

图11 舍弃突变数据后动臂姿态角测量结果Fig.11 Measurement of Boom Attitude Angle After Data Processing

同理，经数据处理后，斗杆姿态角测量结果，如图12 所示。对比两组测量结果传感器测量和姿态估计模型测量结果，姿态角测量偏差，如图13所示。工作装置各杆件姿态角偏差均在±2°之间，大部分偏差在±1.5°之间。设计的测量系统对每帧图像测量耗时，如图14所示。截取前100帧，初始位置由于加载模型，耗时较长，大于240ms，之后的图像耗时趋于稳定，稳定后的平均耗时为108.26ms，满足实时测量的条件。根据以上结果可知，第一组工作装置姿态测量偏差在±2°范围内，其动臂姿态角测量平均偏差为0.864°，斗杆姿态角测量平均偏差为0.942°；第二组测量偏差在±2°范围内，其动臂姿态角测量平均偏差为0.880°，斗杆姿态角测量平均偏差为0.804°，满足挖掘机作业过程中视觉伺服控制系统的姿态反馈。

图12 舍弃突变数据后斗杆姿态角测量结果Fig.12 Measurement of Stick Attitude Angle After Data Processing

图13 姿态角测量偏差Fig.13 Attitude Angle Measurement Deviation

图14 测量耗时Fig.14 Measurement Time

5 结论

（1）提出一种基于多点标识的视觉姿态测量方案，本方案一方面可直接应用于各种机械臂设备的位姿测量，包括挖掘机工作装置、机器人手臂等；另一方面，也可以将其作为配备传感器设备系统的补充信息，用于完善系统。这里以无任何传感器的挖掘机模型为研究对象，仅使用一台RGB摄像机，完成对其工作装置姿态测量。以视觉传感器取代传统接触型传感器，很大程度上克服了接触型传感器在工作装置作业中出现碰撞、剧烈振动造成的损坏现象，降低了维护成本。（2）深度学习算法的应用，改善了传统机器视觉姿态测量系统中靶标特征被污染遮挡导致的检测失败的情况。（3）试验表明，与传统传感器测量结果相比，基于多点标识和深度学习算法的视觉测量方法可以实现对挖掘机工作装置的实时测量，其姿态测量偏差均在±2°范围内。如配备相应的伺服系统，可实现挖掘机作业的实时控制。