多點標識下的挖掘機工作裝置姿態(tài)測量

王杰棟，黃家海，2，蘭 媛，2，熊曉燕，2

（1.太原理工大學機械與運載工程學院，山西 太原 030000；2.太原理工大學新型傳感器與智能控制教育部重點實驗室，山西 太原 030000）

1 引言

挖掘機廣泛應(yīng)用于各種施工場合，在工程機械中占有十分重要的地位。挖掘機作業(yè)環(huán)境復(fù)雜，常常伴有噪聲、粉塵，甚至存在塌方、輻射等危險，不利于操作人員長時間工作。挖掘機智能化、自主化成為未來發(fā)展趨勢［1-2］。

挖掘機自主智能化研究，一直以來都是很具挑戰(zhàn)的課題，國內(nèi)外研究人員為此做了大量的研究工作，文獻［3-4］采用激光測距儀對作業(yè)環(huán)境進行感知，以此實現(xiàn)挖掘作業(yè)過程中對挖掘點和運載目標的準確可靠識別；文獻［5-6］為實現(xiàn)挖掘機自主化挖掘，在挖掘機上安裝激光掃描儀來檢測環(huán)境、障礙和土質(zhì)；文獻［7-8］利用普通相機、立體相機和激光測距儀來進行環(huán)境感知實現(xiàn)挖掘機避障和挖掘目標的檢測；文獻［9-10］利用激光雷達建立了一套具備站點建模能力的挖掘機機器人。這些挖掘機的自主智能化大多體現(xiàn)在環(huán)境感知、避障和挖掘目標識別等方面，在挖掘機工作裝置姿態(tài)測量方面，更多的是以電位計、關(guān)節(jié)角度編碼器等傳統(tǒng)接觸式傳感器來實現(xiàn)。工作裝置姿態(tài)信息作為控制系統(tǒng)的重要參數(shù)，實時精準地獲取姿態(tài)信息對自主智能化作業(yè)起著至關(guān)重要的作用。文獻［11-12］針對姿態(tài)測量中傳統(tǒng)接觸式傳感器容易碰撞損壞等問題，提出了一種基于視覺測量技術(shù)的姿態(tài)測量方法。在挖掘機工作裝置上固定人工靶標，以工業(yè)相機獲取圖像信息，經(jīng)圖像處理后檢測到人工靶標上的角點特征信息或其他特征標識，并將這些特征作為關(guān)鍵點，根據(jù)其約束條件獲得工作裝置的姿態(tài)信息［13-14］。在此過程中，由于挖掘機作業(yè)環(huán)境復(fù)雜及其本身具有的棱角，因此相機捕獲的圖像中元素和噪點過多，從而特征識別算法檢測到較多無效的特征點，導(dǎo)致姿態(tài)檢測失敗率高且耗時比較嚴重；同時，人工靶標上的角點信息在作業(yè)過程中不可避免地會被泥土等污染，這也是導(dǎo)致姿態(tài)檢測失敗的原因之一。

因此提出，利用基于深度學習的目標檢測算法實現(xiàn)對工作裝置上關(guān)鍵點特征的檢測。不同于傳統(tǒng)機器視覺算法對特定對象特征和人工制作特征的提取，深度神經(jīng)網(wǎng)絡(luò)可以從眾多樣本中提取更多廣義特征［15］。基于深度學習的目標檢測算法大致可以分為基于候選區(qū)域和基于回歸兩類，與前者相比后者不需要候選區(qū)域生成分支，可直接對給定的圖像回歸出目標類別和候選框［16］。YOLO系列算法作為基于回歸類目標檢測算法，經(jīng)過多次改進，YOLOv3已能夠以高精度和高速度檢測多個標簽對象，是現(xiàn)階段目標檢測中，滿足實時性和精確性的首要選擇之一［17］。YOLOv3算法以其對小目標識別率高、高精度、快響應(yīng)等優(yōu)勢，可應(yīng)用于無人駕駛汽車行駛過程中對交通標志的檢測［18］。YOLOv3算法對不同顏色目標有較高的識別率，可以此實現(xiàn)對交通信號燈的實時檢測，有效地減小光照及汽車尾燈對檢測的不良影響［19］。因此利用YOLOv3算法可以有效地提取工作裝置上較小關(guān)鍵點的特征，對其進行識別和定位，即使關(guān)鍵點被污染遮擋也可實現(xiàn)檢測。

針對現(xiàn)在挖掘機工作裝置姿態(tài)測量的一些問題，提出一種基于多點標識的挖掘機工作裝置姿態(tài)測量方法。以不同類型的關(guān)鍵點標識作為工作裝置各桿件的關(guān)鍵點，用YOLOv3目標識別算法直接分類并定位這些關(guān)鍵點標識，無需特征篩選，減少檢測耗時、提高測量實時性。YOLOv3算法可以在關(guān)鍵點標識被污染遮擋的情況下，實現(xiàn)識別和定位，提高工作裝置姿態(tài)檢測的成功率。

2 YOLOv3算法回顧

YOLOv3算法的核心結(jié)構(gòu)是一個能夠一次預(yù)測多類目標的卷積神經(jīng)網(wǎng)絡(luò)。它可以實現(xiàn)真正意義上的端到端目標檢測，具有檢測準確率高、速度快的優(yōu)點。YOLOv3算法使用回歸方法代替Softmax方法，實現(xiàn)檢測多種類別目標，該算法的神經(jīng)網(wǎng)絡(luò)使用了Darknet-53作為整個網(wǎng)絡(luò)的分類骨干部分，采用殘差網(wǎng)絡(luò)思想，允許構(gòu)建更深的網(wǎng)絡(luò)來改善網(wǎng)絡(luò)中的非線性，并顯著改善分類和檢測效果。YOLOv3調(diào)出從不同比例提取的三個特征圖來預(yù)測對象。小特征圖提供語義信息，大特征圖提供更細粒度的信息，YOLOv3結(jié)構(gòu)全貌，如圖1所示。

圖1 Darknet-53結(jié)構(gòu)與Yolov3網(wǎng)絡(luò)結(jié)構(gòu)全貌Fig.1 Overview of Darknet-53 Structure and Yolov3 Network Structure

3 關(guān)鍵點坐標變換

關(guān)鍵點是挖掘機工作裝置上用于工作裝置姿態(tài)計算的特征點，工作裝置各桿件姿態(tài)角是根據(jù)關(guān)鍵點的世界坐標計算的，關(guān)鍵點坐標的轉(zhuǎn)換模型，如圖2所示。

圖2 坐標變換模型Fig.2 Coordinate Transformation Model

假設(shè)P(Xw，Yw，Zw)為世界坐標系OwXwYwZw上任意一點，投影到成像平面為點p(x，y)，該點在像素平面坐標系的坐標為(u，v)，將像素坐標(u，v)轉(zhuǎn)換為世界坐標P(Xw，Yw，Zw)的轉(zhuǎn)換過程如下：

（1）像素平面坐標Ouvuv與圖像坐標系Oxy

像素平面上的像經(jīng)過放大處理得到數(shù)字圖像，像素平面上的成像點(u，v)轉(zhuǎn)換成為圖像點(x，y)，則：

式中：(u0，v0)—光軸中心線與成像平面交點的坐標。

將其寫成矩陣形式為：

式中：Min—內(nèi)參數(shù)矩陣，該參數(shù)矩陣含有4個參數(shù)。

（2）圖像平面坐標系Oxy與相機坐標系OcXcYcZc

點P在相機坐標系內(nèi)的坐標為(Xc，Yc，Zc)，是世界坐標在相機坐標系下的投影，是一個三維坐標，將其經(jīng)投影變換得到其圖像坐標(x，y)，則：

經(jīng)變換可得：

將其寫成矩陣形式為：

式中：f—相機焦距。

（3）相機坐標系和世界坐標系

相機坐標與世界坐標的轉(zhuǎn)換關(guān)系，可以看成是攝像機的外參數(shù)模型，是世界坐標系在相機坐標系中的描述。坐標系(Xw，Yw，Zw)在坐標系OcXcYcZc中的表示，構(gòu)成攝像機的外參數(shù)矩陣：

式中：R—3×3旋轉(zhuǎn)矩陣；T—三維的平移向量；

旋轉(zhuǎn)矩陣R、平移向量T通過相機標定獲得，cMw為外參數(shù)矩陣。

（4）將以上式子整合為：

式（6）為其坐標轉(zhuǎn)換過程，將點P的圖像坐標帶入可得到其對應(yīng)的世界坐標。式中f、u0、v0為相機的物理參數(shù)，取決于相機成像芯片以及鏡頭，R、T經(jīng)過相機標定可以獲得。

4 挖掘機工作裝置姿態(tài)測量系統(tǒng)

設(shè)計的工作裝置姿態(tài)測量系統(tǒng)，不依賴任何姿態(tài)傳感器，僅利用一臺RGB攝像機實現(xiàn)對工作裝置姿態(tài)的測量。為工作裝置設(shè)置易于識別和定位的關(guān)鍵點標識，捕獲工作裝置圖像并標注建立數(shù)據(jù)集，以此數(shù)據(jù)集訓練獲得視覺模型實現(xiàn)關(guān)鍵點標識的檢測；根據(jù)關(guān)鍵點間的約束關(guān)系和相機成像原理還原關(guān)鍵點標識的三維信息，并計算對應(yīng)機械桿件的姿態(tài)角。在背景環(huán)境與挖掘機過度融合、關(guān)鍵點標識被污染遮擋的情況下，實現(xiàn)關(guān)鍵點的識別和定位，并完成姿態(tài)測量。

4.1 系統(tǒng)概述

為一臺無傳感器的挖掘機模型配備一個基于多點標識的工作裝置視覺測量系統(tǒng)。在姿態(tài)測量過程中，工作裝置始終在其側(cè)面所在平面內(nèi)運動，定義此平面為目標平面，對目標的檢測即對目標平面的檢測。挖掘機工作裝置姿態(tài)測量系統(tǒng)組成包括計算機、RGB攝像機、挖掘機（目標平面）、數(shù)據(jù)線等。其安裝布置，如圖3所示。

圖3 姿態(tài)測量硬件布置Fig.3 Hardware Layout of Attitude Measurement

如圖4所示，測量系統(tǒng)由數(shù)據(jù)采集模塊和姿態(tài)估計模塊M兩部分組成。數(shù)據(jù)采集模塊包括訓練圖像采集和實時圖像采集，姿態(tài)估計模塊M，用數(shù)學方式表達為：

圖4 姿態(tài)估計系統(tǒng)Fig.4 Attitude Estimation System

該模型接收實時圖像x并輸出定義的姿態(tài)向量p。

式中：p—工作裝置在圖像中的位置參數(shù)，包括9個維度：6個相機外部參數(shù)（平移、旋轉(zhuǎn)）和3個姿態(tài)角度；x—實時從視覺傳感器輸入的圖像（圖片或視頻）；θ—可學習參數(shù)。

關(guān)鍵點標識在現(xiàn)實中都遵循一定的約束，為了充分利用這些約束，姿態(tài)估計模塊M：h()可分解為兩個分量：二維關(guān)鍵點檢測模塊M1、三維姿態(tài)估計模塊M2，用數(shù)學方式表達為

二維關(guān)鍵點檢測模塊

M1：y=f(x；θ)

三維姿態(tài)估計模塊

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

式中：y∈R8×2、z∈R8×3—關(guān)鍵點在像素平面坐標系和世界坐標系中的位置；θ—可學習參數(shù)；τ—對應(yīng)于幾何約束的固定方程組，例如兩個關(guān)鍵點之間的長度。也就是說，M1—被訓練來優(yōu)化θ的，而M2則是一種無參數(shù)算法，它涉及擬合幾個固定的算術(shù)方程。

挖掘機工作裝置姿態(tài)測量系統(tǒng)具體實現(xiàn)過程為RGB攝像機在不同環(huán)境下捕獲大量具有關(guān)鍵點標識的挖掘機圖像數(shù)據(jù)，目標檢測算法利用這些數(shù)據(jù)訓練獲得關(guān)鍵點檢測模塊M1；挖掘機作業(yè)過程中RGB攝像機實時捕獲工作裝置的圖像，作為姿態(tài)估計模塊M的輸入，模塊M1負責檢測該狀態(tài)下關(guān)鍵點二維坐標，通過模塊M2獲得關(guān)鍵點的世界坐標和三維姿態(tài)；挖掘機控制系統(tǒng)將估計的三維姿態(tài)作為輸入，控制工作裝置變化，變化后的姿態(tài)又作為下一輪循環(huán)的輸入，實現(xiàn)挖掘機工作裝置姿態(tài)的實時測量。

4.2 關(guān)鍵點標識

工作裝置姿態(tài)就是工作裝置各機械桿件的姿態(tài)角，分別為挖掘機動臂與水平基座的夾角α2、斗桿與動臂夾角α3、鏟斗（斗尖）與斗桿的夾角α4，如圖5 所示。其中，a1、a2、a3分別為挖掘機動臂、斗桿、鏟斗。

圖5 工作裝置姿態(tài)角分布Fig.5 Attitude Angle Distribution of Work Device

為了實時準確地測量工作裝置各桿件姿態(tài)角，關(guān)鍵點的選取要易于識別和定位，由于挖掘機作業(yè)環(huán)境復(fù)雜，挖掘機及其工作裝置與背景環(huán)境過度融合，沒有可以明顯區(qū)分二者的關(guān)鍵點標識。顏色特征和形狀幾何特征是深度學習算法易于學習和提取的特征信息，用不同顏色和大小的貼片作為關(guān)鍵點標識，既易于識別和定位又能明顯區(qū)分工作裝置和背景環(huán)境。設(shè)計了8種類型的關(guān)鍵點標識，關(guān)鍵點標識為顏色不同大小相同的圓形標識，具體參數(shù)，如表1所示。工作裝置各桿件和水平基座上各布置兩種，桿件上兩關(guān)鍵點標識中心連線與對應(yīng)桿件鉸點連線平行，其布置方式如圖3所示，關(guān)鍵點標識的中心即為關(guān)鍵點，用于姿態(tài)角的測量。

表1 關(guān)鍵點標識位置布置與參數(shù)Tab.1 Key Point Mark Location Layout and Parameters

4.3 數(shù)據(jù)采集

為挖掘機模型配備工作裝置姿態(tài)測量系統(tǒng)，主要是訓練獲得一個姿態(tài)估計模型M，使給定輸入圖像x，輸出姿態(tài)預(yù)測p。訓練這樣的模型通常需要大量的標記數(shù)據(jù)，收集大量不同環(huán)境下的挖掘機圖像，用于姿態(tài)測量模型的訓練。數(shù)據(jù)采集包括訓練圖像采集和實時圖像采集。訓練數(shù)據(jù)D1采集于實驗室環(huán)境下模擬挖掘機在不同背景環(huán)境、光照、障礙物下作業(yè)的圖像，最大限度地保證工作裝置的姿態(tài)、背景環(huán)境和相機視角的多變性，以確保獲得準確的預(yù)測模型。采集到圖像后，利用標注軟件LabelImg對每幅圖像上的8類關(guān)鍵點標識進行標注，訓練數(shù)據(jù)樣本容量為1300。訓練樣本按比例6：2：2劃分為訓練集、驗證集、測試集。訓練集用于訓練或者擬合模型，用于建立模型；驗證集用來測試不同模型和參數(shù)的表現(xiàn)，借此調(diào)整模型；測試集用來評估模型在實際使用時的泛化能力。

實時圖像D2是姿態(tài)測量系統(tǒng)的輸入，是挖掘機在實際作業(yè)過程中RGB攝像機捕獲的圖像。

4.4 相機標定

數(shù)學軟件MATLAB 具有強大的視覺算法包，其內(nèi)置的標定方法操作簡單，獲得的標定精度高，被廣泛應(yīng)用于各種相機標定。相機標定分為兩部分：內(nèi)部參數(shù)的校核、外出參數(shù)的標定。

4.4.1 內(nèi)部參數(shù)校核

MATLAB相機標定程序原理為張正友棋盤格標定法，利用棋盤格標定板對相機標定，將棋盤格標定板置于相機視場范圍內(nèi)，改變標定板的角度、方向、位置等，攝像機捕獲大量圖像，導(dǎo)入MATLAB標定軟件，角點檢測算法檢測標定板上各個棋盤格的角點，根據(jù)其圖像坐標計算獲得相機內(nèi)部參數(shù)，完成對內(nèi)部參數(shù)的校核，如圖6所示。

圖6 相機內(nèi)部參數(shù)校核Fig.6 Camera Internal Parameter Check

4.4.2 外部參數(shù)標定

進行相機標定的過程中，我們以標定板邊緣角點為世界坐標系的原點，如圖7所示。標點板可以看作目標平面，目標平面的世界坐標系對應(yīng)標定板的坐標系，此標定板的位置決定世界坐標系的位置。

圖7 相機外部參數(shù)標定Fig.7 Camera External Parameter Calibration

如圖7所示，目標平面內(nèi)所有點Zw=0，固定相機與挖掘機的位置，在不考慮挖掘機回轉(zhuǎn)的情況下，其世界坐標系和相機坐標系的相對位置固定不變，即旋轉(zhuǎn)矩陣R和平移向量T位置固定不變。利用MATLAB標定軟件，可獲得其固定的旋轉(zhuǎn)矩陣R和平移向量T。

4.5 姿態(tài)角的測量

工作裝置姿態(tài)測量過程中，相機捕獲挖掘機作業(yè)圖像輸入姿態(tài)測量系統(tǒng)，檢測到關(guān)鍵點標識的位置并獲得其世界坐標，根據(jù)世界坐標計算各桿件在目標平面的方向向量，其表達矩陣為：

其中，4列依次為水平基座、動臂、斗桿的方向向量。

因此，工作裝置各桿件相應(yīng)的姿態(tài)角為：

4.6 試驗

以挖掘機模型為研究對象，為其配備工作裝置姿態(tài)測量系統(tǒng)，挖掘機模型本身無任何傳感器，為驗證姿態(tài)測量的準確性，在挖掘機模型上布置傳統(tǒng)接觸式傳感器，以此來獲得工作裝置的真實姿態(tài)，并以此為基準評估姿態(tài)估計模型的好壞。

針對工作裝置視覺測量中存在的問題，進行兩組測試。第一組工作裝置上布置設(shè)計好的標識，測量其姿態(tài)；第二組工作裝置上布置被污染的標識，測量其姿態(tài)。兩組關(guān)鍵點標識，如圖8所示。

圖8 關(guān)鍵點標識（左：第一組 右：第二組）Fig.8 Key Point Identification（Left：Group 1，Right：Group 2）

關(guān)鍵點標識檢測模型訓練的超參數(shù)設(shè)置為：最大訓練輪數(shù)為100，前50 個epoch 初始學習率配置為0.001，后50 個epoch 初始學習率配置為0.0001，batch size設(shè)置為16，優(yōu)化器為Adam，經(jīng)訓練獲得最優(yōu)的網(wǎng)絡(luò)模型。在自然光照下，采用分辨率為（960×540）的RGB攝像機，相機幀率為30fps，采用USB接口，工作裝置以正常速度作業(yè)，姿態(tài)估計系統(tǒng)對兩組關(guān)鍵點標識的檢測，如圖9所示。各類關(guān)鍵點標識均可被檢測，其部分圖像的識別率，如表2所示。第一組檢測率均在0.97以上，第二組被污染靶標識別率有所下降，但大部分高于0.9。

表2 部分圖像關(guān)鍵點標識識別率Tab.2 Recognition Rate of Key Points in Some Images

圖9 二維關(guān)鍵點標識檢測（左：第一組 右：第二組）Fig.9 2D Key Point Mark Detection（Left：Group 1，Right：Group 2）

姿態(tài)測量系統(tǒng)將二維關(guān)鍵點經(jīng)坐標變換獲得其世界坐標，確定工作裝置各桿件姿態(tài)角，其動臂姿態(tài)角動態(tài)測量結(jié)果，如圖10所示。

圖10 動臂姿態(tài)角測量結(jié)果Fig.10 Measurement Results of Boom Attitude Angle

由圖10的兩組測量結(jié)果可知，設(shè)計的姿態(tài)測量模型測量的結(jié)果與傳感器測量系統(tǒng)測量的結(jié)果基本吻合。在挖掘機模型模擬作業(yè)的過程中，由于挖掘機微小振動，使所測數(shù)據(jù)上下浮動，在工作裝置突然劇烈晃動時會出現(xiàn)突變的情況，如圖10 中出現(xiàn)的脈沖現(xiàn)象，此類數(shù)據(jù)具有較大的誤差。為避免由此類現(xiàn)象而導(dǎo)致的誤差，對前后兩次所捕獲的圖像獲得的姿態(tài)估計結(jié)果進行比對。定義兩次結(jié)果之差為ΔA，若實際差值a＞ΔA，則舍棄此組數(shù)據(jù)；若實際差值a＜ΔA，則保留此組數(shù)據(jù)。經(jīng)處理后結(jié)果，如圖11 所示。結(jié)果明顯改善，且不影響姿態(tài)估計模型的測量準確性。

圖11 舍棄突變數(shù)據(jù)后動臂姿態(tài)角測量結(jié)果Fig.11 Measurement of Boom Attitude Angle After Data Processing

同理，經(jīng)數(shù)據(jù)處理后，斗桿姿態(tài)角測量結(jié)果，如圖12 所示。對比兩組測量結(jié)果傳感器測量和姿態(tài)估計模型測量結(jié)果，姿態(tài)角測量偏差，如圖13所示。工作裝置各桿件姿態(tài)角偏差均在±2°之間，大部分偏差在±1.5°之間。設(shè)計的測量系統(tǒng)對每幀圖像測量耗時，如圖14所示。截取前100幀，初始位置由于加載模型，耗時較長，大于240ms，之后的圖像耗時趨于穩(wěn)定，穩(wěn)定后的平均耗時為108.26ms，滿足實時測量的條件。根據(jù)以上結(jié)果可知，第一組工作裝置姿態(tài)測量偏差在±2°范圍內(nèi)，其動臂姿態(tài)角測量平均偏差為0.864°，斗桿姿態(tài)角測量平均偏差為0.942°；第二組測量偏差在±2°范圍內(nèi)，其動臂姿態(tài)角測量平均偏差為0.880°，斗桿姿態(tài)角測量平均偏差為0.804°，滿足挖掘機作業(yè)過程中視覺伺服控制系統(tǒng)的姿態(tài)反饋。

圖12 舍棄突變數(shù)據(jù)后斗桿姿態(tài)角測量結(jié)果Fig.12 Measurement of Stick Attitude Angle After Data Processing

圖13 姿態(tài)角測量偏差Fig.13 Attitude Angle Measurement Deviation

圖14 測量耗時Fig.14 Measurement Time

5 結(jié)論

（1）提出一種基于多點標識的視覺姿態(tài)測量方案，本方案一方面可直接應(yīng)用于各種機械臂設(shè)備的位姿測量，包括挖掘機工作裝置、機器人手臂等；另一方面，也可以將其作為配備傳感器設(shè)備系統(tǒng)的補充信息，用于完善系統(tǒng)。這里以無任何傳感器的挖掘機模型為研究對象，僅使用一臺RGB攝像機，完成對其工作裝置姿態(tài)測量。以視覺傳感器取代傳統(tǒng)接觸型傳感器，很大程度上克服了接觸型傳感器在工作裝置作業(yè)中出現(xiàn)碰撞、劇烈振動造成的損壞現(xiàn)象，降低了維護成本。（2）深度學習算法的應(yīng)用，改善了傳統(tǒng)機器視覺姿態(tài)測量系統(tǒng)中靶標特征被污染遮擋導(dǎo)致的檢測失敗的情況。（3）試驗表明，與傳統(tǒng)傳感器測量結(jié)果相比，基于多點標識和深度學習算法的視覺測量方法可以實現(xiàn)對挖掘機工作裝置的實時測量，其姿態(tài)測量偏差均在±2°范圍內(nèi)。如配備相應(yīng)的伺服系統(tǒng)，可實現(xiàn)挖掘機作業(yè)的實時控制。