基于3D視覺的青飼機拖車車斗自動識別與定位方法

苗中華 陳蘇躍 何創新 金稱雄 馬世偉 徐雙喜

(1.上海大學機電工程與自動化學院， 上海 200072； 2.勇猛機械股份有限公司， 天津 301802)

0 引言

隨著計算機、自動控制以及人工智能技術的不斷發展，農業機械作業的智能化升級受到越來越多的關注。農機作業智能化對提升勞動生產效率和舒適性、節約生產成本、改善農產品生產質量具有重要作用[1]。青飼機是一種青貯飼料收獲機械，主要用于收獲大麥、燕麥、牧草、玉米和高粱等作物，可一次完成對作物的收割、切碎、揉搓，并將碎作物拋送至運輸車。近年來，隨著我國規模化、自動化、集約化畜牧養殖業的快速發展，青飼機具有廣闊的市場需求，然而，國內青飼機技術水平較低，尤其在自動化、智能化方面，與國外相比差距較大[2-4]。

機器視覺技術是人工智能快速發展的一個重要分支，與圖像處理、模式識別等技術結合，可實現特定物體的識別，以及多物體間的相互定位與位置跟蹤。基于機器視覺的目標辨識、空間定位與位置跟蹤技術已成為農機裝備智能控制領域競相探究的熱點和難點，具有廣泛的應用前景[5-15]。

目前，我國青飼機的青飼料收集作業主要為人工遙控操作，操作員需要通過肉眼識別車斗的位置，手動控制噴頭的高度和角度。這種操作存在噴射不精準、反應不及時、物料浪費、人力勞頓等各種缺陷。因此，研究青飼機和拖車之間的協同作業和青飼料自動精準噴裝技術具有實用價值。

青飼機和拖車之間的自動物料填充過程是一個雙機協同作業控制問題[3-4]。首先需要自動識別行進中的拖車車斗外框邊沿，獲取車框與收獲車噴料機械臂的相對位置，在此基礎上自動控制機械臂和噴頭的高度和角度，使拋出的青飼料恰好落入車斗中。

針對青飼機拖車車斗自動識別與定位問題，本文利用車斗外框上沿位于世界坐標系的局部最高處的特點，提出一種基于RGB-D相機三維視覺信息的運動車斗識別與定位方法。

1 運動車斗自動識別與定位方法

1.1 方法概述

本文所提方法采用“眼在手”工作模式[16]，通過在機械臂上安裝相機，采集拖車區域的視頻信息。在地面保持水平的理想條件下(實際操作中可以通過加載車體陀螺儀信息來近似達到水平)，利用車斗外框上沿位于世界坐標系局部最高處(垂直地面方向)的特點，獲取車斗所在位置。因此，為了實現“定位”，即獲取機械臂噴頭與車斗的相對位置關系，需選擇一個合適的世界坐標系原點。本文所建立的世界坐標系與相機坐標系如圖1所示。

圖1 坐標系示意圖Fig.1 Schematic of coordinate system

世界坐標系中Oxy平面平行于地面，坐標系原點為機械臂基座旋轉中心，z軸垂直地面向上，y軸指向機械臂在Oxy平面上的投影方向，即機械臂中軸線位于Oyz平面。飼料填充過程中世界坐標系跟隨機械臂繞z軸轉動，因此，飼料落點一直維持在Oyz平面。

車斗角點與飼料理想落點的世界坐標系坐標將通過定位算法獲得，又由于機械臂噴頭在該世界坐標系中位置已知，飼料的噴射速度已知，因此可獲取飼料實際落點。當獲取噴頭與車斗的相對位置以及理想落點位置后，通過規劃機械臂運動控制實際落點去接近理想落點位置。

識別與定位方法主要步驟如下：

(1)選擇世界坐標系，并用奇異值分解(Singular value decomposition, SVD)求解其與相機坐標系之間的旋轉平移矩陣。

(2)讀入一幀三維點云數據、一幀深度圖像，并對點云的格式進行轉換。

(3)點云降采樣。實際并不需要如此稠密的點云數據，降采樣可成倍提升后續步驟的執行效率。

(4)使用步驟(1)所得旋轉平移矩陣將相機坐標系下點云變換到世界坐標系下。

(5)對點云數據進行閾值操作獲取車斗所在大致區域，并將保留值投射至二維平面。

(6)用隨機抽樣一致性算法(Random sample consensus，RANSAC)找出二維平面下的車斗角點定位。

(7)在像素坐標系下找到車斗角點，生成可視化界面。

識別與定位方法流程圖如圖2所示。

圖2 拖車車斗識別與定位方法流程圖Fig.2 Flow chart of method for trailer hopper positioning

1.2 位姿變換矩陣求解

圖3 青飼機與拖車協同作業示意圖Fig.3 Cooperative operation diagram of green forage harvester and trailer1.運輸車 2.車斗 3.青飼料 4.噴頭 5.機械臂 6.深度相機 7.青飼機 8.前方割臺

如圖3所示，相機固定于機械臂前端，確保較好的視角以便能夠拍攝到收獲車側后方來車。相機固定后，相機坐標系與世界坐標系之間的相對關系得到確定。定位過程最終需得到機械臂噴頭與拖車車斗之間的位置關系，所以需解算以上兩個坐標系之間的旋轉平移矩陣，即“手眼”標定過程。

為此需研究3D-3D的位姿估計問題，該問題的數學描述如下：

假設有一組配對好的3D點

(1)

需要找到一個歐氏變換R、t，使得

(2)

式中pi——點陣P中一個點的坐標

針對此類問題，視覺即時定位與視覺構建(Simultaneous localization and mapping，SLAM)中涉及較多相關應用，借助其思想求解本方案中所需的位姿變換矩陣。在視覺SLAM問題的研究中，構造視覺里程計(Visual odometry, VO)[17]是至關重要的步驟之一。VO的基本任務是通過圖像估計相機運動。其中基于特征提取的視覺里程計較為成熟，是應用比較廣泛的主流方法[18]。

此類視覺里程計需通過對前后兩幀圖像進行特征提取和匹配，從而得到兩幀圖像間對應特征點。通過特征匹配獲取兩組對應點，并通過三角化計算或者直接通過RGB-D相機的深度圖確定特征點的3D位置，此時得到的兩組點即為P與P′，最終根據相機類型選擇求解運動估計的方法解決式(2)提出的問題。此處求得兩組特征點間R、t矩陣為相機的姿態變換矩陣，即兩個相機坐標系之間的旋轉、平移矩陣。

本文相當于構建一個兩幀間視覺里程計，得到世界坐標系與相機坐標系之間的變換關系。此處需要進行匹配的兩組特征點分別是參考物在世界坐標系下的參考點與相機坐標系下的參考點。世界坐標系下參考點坐標通過實際測量獲得，相機坐標系下的參考點坐標通過編寫相應的程序在點云中指定。通過應用視覺SLAM中求取位姿變換矩陣的核心算法，解算兩個三維坐標系之間的R、t矩陣。

視覺SLAM求解運動估計矩陣有兩種方法：利用線性代數求解(主要是SVD)，以及利用非線性優化方式求解(類似Bundle adjustment)，本文將采用SVD算法完成解算。

通過構建最小二乘問題，求使得誤差平方和達到極小值的R、t[19]。

(3)

式中J——兩個點陣的誤差平方和

位姿估計3D-3D求解過程如下

Algorithm 1 pose_estimation_3d3d

∥center of mass

∥remove the center

∥compute q1*q2^T

∥SVD onW

W=UΣVT,R=UVT,t=p-Rp′

至此，解得兩組點陣之間的旋轉矩陣R與平移矩陣t。所得結果為相機坐標系與世界坐標系之間的轉換關系。

1.3 點云處理

考慮到課題的實際需求，處理點云數據之前進行數據降采樣可成倍提升后續步驟的執行效率。降采樣使用體素化網格的方法實現，在減少點云數據量的同時保留點云的形狀特征，能有效提升算法速度。降采樣時對輸入的點云數據創建一個三維體素柵格，即把點云所在的三維空間劃分成一些微小的空間三維立方體的集合，然后在每個體素中用該體素內所有點的重心來近似代表體素中所有的點，最終單個體素只剩一個重心點。

隨后，在對實際情況進行合理分析的基礎上進行閾值操作。如圖3所示，左側車輛為農用拖車，其高大的車斗為整車中最高的部分，利用這個限制條件可進行閾值處理，提取出車斗所在區域的點陣。此時需要用到世界坐標系下點云，即要求點云z軸垂直于地面，其值表示車斗高度。因此，使用1.2節所求R、t矩陣對相機坐標系下的點云數據進行旋轉平移。計算公式為

(4)

式中Pw——世界坐標系下點云坐標

Pc——相機坐標系下點云坐標

Rcw——相機坐標系到世界坐標系的旋轉矩陣

tcw——相機坐標系到世界坐標系的平移矩陣

X、Y、Z——Pc點云中點的x、y、z軸坐標值

然后根據Z通道提供的車斗高度信息進行閾值操作，取出車斗頂部的點云，并將Z值去除，僅保留X、Y值組成的Mat矩陣，完成降維處理。圖4為同一物體在同一時刻相機坐標系下的點云圖、像素坐標系(相機成相平面對應的坐標系)下的depth圖及世界坐標系下閾值操作后的2D點陣圖。

圖4 參考物在同一時刻下的3種圖像Fig.4 Three kinds of images of reference at the same time

1.4 定位算法

利用2D點陣圖進行車斗角點定位，從而確定車斗的中軸線，車斗中軸線即飼料噴灑的落點所在直線。該直線與機械臂垂直投影所在直線(世界坐標系y軸)的交點即飼料理想落點。車斗角點與飼料落點關系如圖5所示。

圖5 理想飼料落點定位示意圖Fig.5 Schematic of ideal point location

圖6 RANSAC算法車斗邊沿擬合圖Fig.6 Edge fitting of bucket by RANSAC

根據車斗實際情況，需要從圖4c中擬合出多條直線，找到車斗邊沿。此時最小二乘法顯然不適用。對于每條邊而言，都必須排除一些局外點，才能擬合出合理的直線。因此引入隨機抽樣一致性算法[20-21]來完成這個工作。針對車斗點陣進行邊沿直線擬合示意圖如圖6所示，具體步驟為：

(1)使用RANSAC算法編程，擬合出4條直線，即車斗邊沿所在直線。

(2)對4條直線產生的6個交點進行篩選(車斗雖然為矩形，但由于直線由算法擬合產生，擬合所得的矩形對邊不可能平行)。篩選依據為交點所在坐標的絕對值，即交點偏移坐標原點的距離，將距離最大的兩個點排除(矩形車斗兩組對邊擬合直線的交點距原點最遠)。

(3)根據矩形特征，將保留的4個交點進行排序。將同一邊沿的交點連結，得到車斗邊沿。

二維平面上的定位得到車斗角點在世界坐標系下的x、y軸坐標值，另外通過拖車車斗上沿與機械臂基座的高度差獲取角點z軸坐標值，完成三維世界坐標系下的定位。

現在已經獲得世界坐標系下的車斗角點坐標，進一步根據圖5車斗角點與飼料落點關系，可以獲取飼料理想落點所在范圍(即車斗中軸線)，而實際落點坐標則由噴頭擋板角度和噴射速度決定。自動噴裝系統的控制算法需要根據車斗的位置信息以及落點的滿溢程度進行合理的噴裝路徑規劃。該部分內容不在本文的討論范圍。

1.5 可視化反饋

本文重點研究運動車斗的識別與定位，自動噴裝系統的控制算法將利用所檢測信息進行機械臂運動規劃。為了可視化識別與定位效果，進行逆向坐標變換，將世界坐標系下找到的角點坐標變換到相機坐標系，然后通過相機內參數將相機坐標系下的角點坐標映射到像素坐標系下，從而在RGB圖或者depth圖中標記出車斗所在區域，形成直觀的可視化反饋，便于操作人員監測青飼料填充情況。

將世界坐標系下找到的角點坐標變換到相機坐標系下，進行逆變換

(5)

然后通過相機內參數將相機坐標系下的角點坐標變換到像素坐標系下，此時依據相機的針孔模型[22-23]做轉換

(6)

式中Puv——像素坐標系下角點的齊次坐標

K——相機內參矩陣

fx、fy、cx、cy——像素坐標系與成像平面間縮放平移參數

通過以上步驟，將世界坐標系下定位的角點顯示在二維圖像上，方便工作人員觀察車斗識別的實際情況，及時做出必要的調整。

2 現場實驗

2.1 實驗設計

實驗采用圖漾公司的PERCIPIO.XYZ系列RGB-D相機，如圖7所示。該相機支持0.5～6 m測試距離，誤差達到測試距離的0.25%。測試用計算機型號為ThinkPad X230i，Intel內核(2.4 GHz)，10 GB內存。測試系統為Ubuntu 14.04 LTS，編程語言為C/C++。

圖7 RGB-D相機結構示意圖Fig.7 Schematic of camera structure1、3.紅外攝像頭 2.紅外激光發射器 4.彩色攝像頭 5.定位孔

車斗識別和自動定位系統在天津勇猛機械股份有限公司生產的青飼機上進行了田間實驗，實驗平臺如圖8所示，相機固定于機械臂上，拍攝車斗所在區域，并通過USB向計算機傳遞視覺信號。

圖8 現場實驗平臺Fig.8 Experiment platform1.車斗 2.青飼料 3.機械臂 4.RGB-D相機

2.2 實驗結果分析

在算法檢測效果評估中，采用交并比(Intersection over union，IoU)量化實驗結果。IoU表示算法得出的候選框與原標記框的交疊率，最理想的情況是完全重疊，即IoU為1。交并比計算公式如下

(7)

式中 area()——圖像區域面積函數

C——候選區G——原標記區

現場實驗檢測效果如圖9a所示，對應的RGB參考圖如圖9b所示，圖9c為IoU計算區域。其中綠色框線區域為檢測算法獲取的目標區域，即候選區；藍色框線區域為手動標記的目標區域，即原標記區；紫色填充區域為以上兩個區域的重疊區。根據以上信息可計算出每次檢測結果的IoU，由此定量評判算法精度，具體數據如表1所示。表1中呈現了6幀圖像的交并比，并得出其平均值為0.931，且總體檢測效果穩定，無明顯檢測不到車斗的幀。

圖9 現場實驗結果量化分析示意圖Fig.9 Quantitative analysis schematic of field experiment results

幀號area(C)∩area(G)/像素area(C)∪area(G)/像素IoU12430572697400.90122653782900760.91532576702599980.99142900983299710.87953115233311190.94162587192698710.959平均值0.931

3 結論

(1)本文方法能對不同高度、不同類型拖車車斗進行準確識別，無需使用專用的配套拖車或人工添加標記點。

(2)通過“手眼”標定建立各坐標系之間的關系，不僅能從圖像中識別運動車斗，而且能在空間上定位車斗角點與飼料落點，作為機械臂控制的輸入信號。

(3)本文方法降低了點云計算量，能滿足現場工作的實時性需求。對連續6幀檢測結果進行IoU分析表明，該算法滿足現場工作的精度需求。