多點標識下的挖掘機工作裝置姿態測量

王杰棟，黃家海，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°范圍內。如配備相應的伺服系統，可實現挖掘機作業的實時控制。