苗中華 陳蘇躍 何創(chuàng)新 金稱雄 馬世偉 徐雙喜

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

0 引言

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

機器視覺技術(shù)是人工智能快速發(fā)展的一個重要分支，與圖像處理、模式識別等技術(shù)結(jié)合，可實現(xiàn)特定物體的識別，以及多物體間的相互定位與位置跟蹤?；跈C器視覺的目標辨識、空間定位與位置跟蹤技術(shù)已成為農(nóng)機裝備智能控制領(lǐng)域競相探究的熱點和難點，具有廣泛的應(yīng)用前景[5-15]。

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

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

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

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

1.1 方法概述

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1.2 位姿變換矩陣求解

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

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

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

假設(shè)有一組配對好的3D點

(1)

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

(2)

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

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

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

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

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

通過構(gòu)建最小二乘問題，求使得誤差平方和達到極小值的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′

至此，解得兩組點陣之間的旋轉(zhuǎn)矩陣R與平移矩陣t。所得結(jié)果為相機坐標系與世界坐標系之間的轉(zhuǎn)換關(guān)系。

1.3 點云處理

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

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

(4)

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

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

Rcw——相機坐標系到世界坐標系的旋轉(zhuǎn)矩陣

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

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

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

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

1.4 定位算法

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

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

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

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

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

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

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

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

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

1.5 可視化反饋

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

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

(5)

然后通過相機內(nèi)參數(shù)將相機坐標系下的角點坐標變換到像素坐標系下，此時依據(jù)相機的針孔模型[22-23]做轉(zhuǎn)換

(6)

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

K——相機內(nèi)參矩陣

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

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

2 現(xiàn)場實驗

2.1 實驗設(shè)計

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

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

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

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

2.2 實驗結(jié)果分析

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

(7)

式中 area()——圖像區(qū)域面積函數(shù)

C——候選區(qū)G——原標記區(qū)

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

圖9 現(xiàn)場實驗結(jié)果量化分析示意圖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 結(jié)論

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

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

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