基于3D視覺(jué)的青飼機(jī)拖車(chē)車(chē)斗自動(dòng)識(shí)別與定位方法

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

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

0 引言

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

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

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

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

針對(duì)青飼機(jī)拖車(chē)車(chē)斗自動(dòng)識(shí)別與定位問(wèn)題，本文利用車(chē)斗外框上沿位于世界坐標(biāo)系的局部最高處的特點(diǎn)，提出一種基于RGB-D相機(jī)三維視覺(jué)信息的運(yùn)動(dòng)車(chē)斗識(shí)別與定位方法。

1 運(yùn)動(dòng)車(chē)斗自動(dòng)識(shí)別與定位方法

1.1 方法概述

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

圖1 坐標(biāo)系示意圖Fig.1 Schematic of coordinate system

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

車(chē)斗角點(diǎn)與飼料理想落點(diǎn)的世界坐標(biāo)系坐標(biāo)將通過(guò)定位算法獲得，又由于機(jī)械臂噴頭在該世界坐標(biāo)系中位置已知，飼料的噴射速度已知，因此可獲取飼料實(shí)際落點(diǎn)。當(dāng)獲取噴頭與車(chē)斗的相對(duì)位置以及理想落點(diǎn)位置后，通過(guò)規(guī)劃?rùn)C(jī)械臂運(yùn)動(dòng)控制實(shí)際落點(diǎn)去接近理想落點(diǎn)位置。

識(shí)別與定位方法主要步驟如下：

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

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

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

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

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

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

(7)在像素坐標(biāo)系下找到車(chē)斗角點(diǎn)，生成可視化界面。

識(shí)別與定位方法流程圖如圖2所示。

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

1.2 位姿變換矩陣求解

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

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

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

假設(shè)有一組配對(duì)好的3D點(diǎn)

(1)

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

(2)

式中pi——點(diǎn)陣P中一個(gè)點(diǎn)的坐標(biāo)

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

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

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

視覺(jué)SLAM求解運(yùn)動(dòng)估計(jì)矩陣有兩種方法：利用線性代數(shù)求解(主要是SVD)，以及利用非線性?xún)?yōu)化方式求解(類(lèi)似Bundle adjustment)，本文將采用SVD算法完成解算。

通過(guò)構(gòu)建最小二乘問(wèn)題，求使得誤差平方和達(dá)到極小值的R、t[19]。

(3)

式中J——兩個(gè)點(diǎn)陣的誤差平方和

位姿估計(jì)3D-3D求解過(guò)程如下

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′

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

1.3 點(diǎn)云處理

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

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

(4)

式中Pw——世界坐標(biāo)系下點(diǎn)云坐標(biāo)

Pc——相機(jī)坐標(biāo)系下點(diǎn)云坐標(biāo)

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

tcw——相機(jī)坐標(biāo)系到世界坐標(biāo)系的平移矩陣

X、Y、Z——Pc點(diǎn)云中點(diǎn)的x、y、z軸坐標(biāo)值

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

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

1.4 定位算法

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

圖5 理想飼料落點(diǎn)定位示意圖Fig.5 Schematic of ideal point location

圖6 RANSAC算法車(chē)斗邊沿?cái)M合圖Fig.6 Edge fitting of bucket by RANSAC

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

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

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

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

二維平面上的定位得到車(chē)斗角點(diǎn)在世界坐標(biāo)系下的x、y軸坐標(biāo)值，另外通過(guò)拖車(chē)車(chē)斗上沿與機(jī)械臂基座的高度差獲取角點(diǎn)z軸坐標(biāo)值，完成三維世界坐標(biāo)系下的定位。

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

1.5 可視化反饋

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

將世界坐標(biāo)系下找到的角點(diǎn)坐標(biāo)變換到相機(jī)坐標(biāo)系下，進(jìn)行逆變換

(5)

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

(6)

式中Puv——像素坐標(biāo)系下角點(diǎn)的齊次坐標(biāo)

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

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

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

2 現(xiàn)場(chǎng)實(shí)驗(yàn)

2.1 實(shí)驗(yàn)設(shè)計(jì)

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

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

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

圖8 現(xiàn)場(chǎng)實(shí)驗(yàn)平臺(tái)Fig.8 Experiment platform1.車(chē)斗 2.青飼料 3.機(jī)械臂 4.RGB-D相機(jī)

2.2 實(shí)驗(yàn)結(jié)果分析

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

(7)

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

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

現(xiàn)場(chǎng)實(shí)驗(yàn)檢測(cè)效果如圖9a所示，對(duì)應(yīng)的RGB參考圖如圖9b所示，圖9c為IoU計(jì)算區(qū)域。其中綠色框線區(qū)域?yàn)闄z測(cè)算法獲取的目標(biāo)區(qū)域，即候選區(qū)；藍(lán)色框線區(qū)域?yàn)槭謩?dòng)標(biāo)記的目標(biāo)區(qū)域，即原標(biāo)記區(qū)；紫色填充區(qū)域?yàn)橐陨蟽蓚€(gè)區(qū)域的重疊區(qū)。根據(jù)以上信息可計(jì)算出每次檢測(cè)結(jié)果的IoU，由此定量評(píng)判算法精度，具體數(shù)據(jù)如表1所示。表1中呈現(xiàn)了6幀圖像的交并比，并得出其平均值為0.931，且總體檢測(cè)效果穩(wěn)定，無(wú)明顯檢測(cè)不到車(chē)斗的幀。

圖9 現(xiàn)場(chǎng)實(shí)驗(yàn)結(jié)果量化分析示意圖Fig.9 Quantitative analysis schematic of field experiment results

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

3 結(jié)論

(1)本文方法能對(duì)不同高度、不同類(lèi)型拖車(chē)車(chē)斗進(jìn)行準(zhǔn)確識(shí)別，無(wú)需使用專(zhuān)用的配套拖車(chē)或人工添加標(biāo)記點(diǎn)。

(2)通過(guò)“手眼”標(biāo)定建立各坐標(biāo)系之間的關(guān)系，不僅能從圖像中識(shí)別運(yùn)動(dòng)車(chē)斗，而且能在空間上定位車(chē)斗角點(diǎn)與飼料落點(diǎn)，作為機(jī)械臂控制的輸入信號(hào)。

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