基于云臺(tái)相機(jī)的番茄主莖跟蹤控制方法*

張萬豪， 張文愛， 馮青春， 成偉， 王博文

1. 太原理工大學(xué) 信息與計(jì)算機(jī)學(xué)院， 山西 晉中 030600；2. 北京農(nóng)業(yè)智能裝備技術(shù)研究中心， 北京100097； 3. 國家農(nóng)業(yè)智能裝備工程技術(shù)研究中心， 北京100097

中國是世界上番茄栽培面積最大、 生產(chǎn)總量最多的國家， 常年產(chǎn)量在5 000 t以上， 并呈現(xiàn)增長趨勢(shì)[1]． 目前， 番茄已經(jīng)成為我國種植面積和產(chǎn)量最大的蔬菜品種之一， 2020年番茄產(chǎn)量為6 515萬t， 占全國蔬菜總產(chǎn)量的7.1%[2]． 在番茄種植周期中， 整枝打葉、 授粉、 采摘以及噴藥等環(huán)節(jié)人工作業(yè)頻次高、 勞動(dòng)強(qiáng)度大． 近年來， 隨著從業(yè)勞動(dòng)力的流失， 番茄種植管理的人工費(fèi)用已達(dá)到總生產(chǎn)成本的35%以上， 成為制約番茄種植產(chǎn)業(yè)效益增長的重要因素[3]． 因此， 研發(fā)替代或者輔助人工作業(yè)的番茄機(jī)器人， 對(duì)于我國番茄安全供應(yīng)、 種植產(chǎn)業(yè)健康發(fā)展具有重要意義．

由于種植管理作業(yè)對(duì)象(果、 葉、 花)都是交錯(cuò)生長在番茄主莖上[4]， 這類作業(yè)有著相似的作業(yè)路徑， 都是沿著番茄主莖的生長方向進(jìn)行作業(yè)． 因此對(duì)番茄植株主莖視覺信息的獲取是實(shí)現(xiàn)對(duì)番茄智能化管理作業(yè)的重要前提， 針對(duì)這類作業(yè)環(huán)節(jié)， 借鑒人工作業(yè)方式， 沿著番茄主莖生長方向自下而上主動(dòng)搜索作業(yè)目標(biāo)， 是提高對(duì)作業(yè)對(duì)象定位效率和識(shí)別精度的有效方法．

國內(nèi)外學(xué)者針對(duì)果蔬的莖稈形態(tài)跟蹤定位問題做了很多研究． Botterill等[5]設(shè)計(jì)的葡萄藤修剪機(jī)器人采用3個(gè)圍繞葡萄藤固定安裝在箱體內(nèi)部的立體相機(jī)獲取葡萄藤的視覺信息以實(shí)現(xiàn)修剪工作． 黃彪[6]設(shè)計(jì)的枇杷剪枝機(jī)器人采用2個(gè)超廣角相機(jī)在3個(gè)固定高度對(duì)枇杷樹做3輪掃描獲取枇杷枝條的視覺信息， 攝像頭的畸變導(dǎo)致采集的圖像存在較大失真． Cuevas等[7]設(shè)計(jì)的玫瑰整枝機(jī)器人采用立體相機(jī)在9個(gè)固定位姿對(duì)玫瑰灌木進(jìn)行掃描獲取整株玫瑰的點(diǎn)云圖， 根據(jù)相鄰作業(yè)點(diǎn)的實(shí)時(shí)更新實(shí)現(xiàn)對(duì)整株玫瑰的作業(yè)． 馮青春等[8]提出的離散小視場(chǎng)番茄主莖測(cè)量方法， 實(shí)現(xiàn)了對(duì)番茄主莖的測(cè)量和信息獲取． 荷蘭Tomas[9]團(tuán)隊(duì)設(shè)計(jì)的Kompano Deleaf-Line機(jī)器人采用可控的Chameleon 3相機(jī)實(shí)現(xiàn)了對(duì)番茄側(cè)枝的修剪． Jon等[10]借助安裝于機(jī)械手末端的深度相機(jī)獲取視覺信息設(shè)計(jì)了一款番茄采摘機(jī)器人 ． Arad等[11]設(shè)計(jì)的甜椒收獲機(jī)器人采用隨末端移動(dòng)的深度相機(jī)識(shí)別目標(biāo)和果梗． 這些研究極大促進(jìn)了玫瑰、 番茄和果樹等作物整枝和收獲技術(shù)的進(jìn)步， 但這些研究中， 以固定視角被動(dòng)搜索目標(biāo)的設(shè)計(jì)方案使得設(shè)備的體積過大， 得到大量的冗余信息； 以可控視角主動(dòng)搜索的方式， 設(shè)備輕巧靈活， 獲取信息可用性更大， 可以提高信息的處理效率．

針對(duì)番茄智能化作業(yè)需要， 本文設(shè)計(jì)了基于云臺(tái)相機(jī)的番茄主莖形態(tài)視覺信息獲取系統(tǒng)， 提出了對(duì)番茄主莖的視覺跟蹤控制方法． 根據(jù)攝像機(jī)視場(chǎng)內(nèi)的番茄主莖形態(tài)， 實(shí)時(shí)調(diào)整云臺(tái)姿態(tài)使番茄主莖上的跟蹤參考點(diǎn)始終位于相機(jī)視場(chǎng)中心． 該研究可以為番茄整枝、 授粉、 噴藥和采摘等智能化作業(yè)環(huán)節(jié)提供技術(shù)支撐．

1 溫室環(huán)境與跟蹤原理

1.1 溫室番茄栽培環(huán)境

溫室番茄栽培逐漸趨于工廠化和標(biāo)準(zhǔn)化， 這將有利于機(jī)器人的智能作業(yè)． 選擇試驗(yàn)的溫室栽培環(huán)境如圖1所示． 番茄栽培在離地面600 mm高的培養(yǎng)槽中， 番茄種植行間距約為1 200 mm， 株距為300 mm． 距地面600～1 500 mm的區(qū)域?yàn)榉颜Α?打葉、 授粉和收獲的作業(yè)區(qū)域． 隨著番茄植株的生長， 通過釋放纏繞于主莖的吊線進(jìn)行落蔓， 使得作業(yè)區(qū)域相對(duì)地面高度保持不變， 因此本文以此高度區(qū)域內(nèi)的番茄植株為跟蹤的對(duì)象．

圖1 溫室番茄栽培環(huán)境

1.2 主莖跟蹤原理

如圖2所示， 果實(shí)和側(cè)枝是番茄整枝和采摘環(huán)節(jié)的作業(yè)對(duì)象． 番茄的側(cè)枝和果實(shí)是沿著主莖生長的， 由于作業(yè)對(duì)象交錯(cuò)生長在番茄主莖兩側(cè)， 大視場(chǎng)下會(huì)出現(xiàn)信息冗余和遮擋， 采用離散小視場(chǎng)的方法可以獲取更加精確的信息． 借鑒人工作業(yè)方式， 沿著番茄主莖自下而上地搜索可以提高作業(yè)對(duì)象(花、 果和側(cè)枝)的探測(cè)效率． 主動(dòng)跟蹤就是不斷調(diào)整相機(jī)姿態(tài)使番茄主莖上變化的跟蹤參考點(diǎn)始終位于視場(chǎng)中心進(jìn)而實(shí)現(xiàn)模仿人工作業(yè)時(shí)獲取視覺信息的一種目標(biāo)跟蹤方法．

圖2 目標(biāo)生長規(guī)律

2 云臺(tái)視覺系統(tǒng)設(shè)計(jì)

2.1 硬件構(gòu)成

視覺信息是支撐機(jī)器人作業(yè)的重要依據(jù)， 為了避免大視場(chǎng)帶來的信息冗余以及對(duì)目標(biāo)定位和識(shí)別精度的影響， 采用離散小視場(chǎng)方式采集信息． 本次試驗(yàn)設(shè)計(jì)的視覺獲取系統(tǒng)如圖3所示．

① Realsense D435i相機(jī)； ② 俯仰機(jī)構(gòu)； ③ 擺動(dòng)機(jī)構(gòu)； ④ 控制模塊與電源．圖3 視覺獲取系統(tǒng)

視覺獲取系統(tǒng)主要由一個(gè)二自由度的云臺(tái)和Intel公司的Realsense D435i深度相機(jī)組成． 云臺(tái)有左右擺動(dòng)和上下俯仰兩個(gè)自由度， 其范圍分別為(-75°～+75°)和(-30°～+60°)， 其精度均為0.01°． Realsense D435i相機(jī)是一款深度相機(jī)， 采用即用型USB供電形式， 搭載Intel公司的D400系列深度模塊， 具備完整的深度光學(xué)解決方案． 它主要由兩個(gè)紅外傳感器、 一個(gè)1 080 P的色彩傳感器和一個(gè)紅外激光投射器3個(gè)部分組成[12]． D435i模仿人眼的視覺成像原理， 通過兩個(gè)紅外傳感器同時(shí)追蹤由紅外激光投射器發(fā)出的紅外光位置， 然后利用三角定位原理計(jì)算得到圖像中的深度信息． 主要參數(shù)[13]如表1所示．

表1 Realsense D435i相機(jī)參數(shù)

2.2 結(jié)構(gòu)參數(shù)標(biāo)定

明確云臺(tái)與相機(jī)的結(jié)構(gòu)位置關(guān)系是根據(jù)圖像信息控制云臺(tái)姿態(tài)的重要依據(jù)[14]． 通過D435i相機(jī)可以直接得到目標(biāo)點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)， 但為了實(shí)現(xiàn)對(duì)目標(biāo)的跟蹤和云臺(tái)的控制， 必須將其坐標(biāo)轉(zhuǎn)換到云臺(tái)坐標(biāo)系下． 如圖3所示， 云臺(tái)坐標(biāo)系為OT-XTYTZT， 相機(jī)坐標(biāo)系為OC-XCYCZC， 則云臺(tái)坐標(biāo)與相機(jī)坐標(biāo)存在式(1)的轉(zhuǎn)換關(guān)系[15]．

(1)

T為云臺(tái)與相機(jī)坐標(biāo)系之間的轉(zhuǎn)換矩陣． 由式(1)可知， 對(duì)云臺(tái)相機(jī)做標(biāo)定就是求解旋轉(zhuǎn)矩陣R和平移分量t．

如圖3所示， 以云臺(tái)垂直自由度的轉(zhuǎn)軸與云臺(tái)上平面的交點(diǎn)O0為原點(diǎn)， 并以左右擺動(dòng)轉(zhuǎn)軸向上的方向作為Z軸的正方向， 以水平指向目標(biāo)的方向作為X軸正方向， 建立基坐標(biāo)系O0-X0Y0Z0． 以云臺(tái)兩個(gè)旋轉(zhuǎn)軸的交點(diǎn)OT為原點(diǎn)， 以俯仰旋轉(zhuǎn)軸為Y軸， 以與相機(jī)光軸平行的方向?yàn)閄軸， 建立云臺(tái)坐標(biāo)系OT-XTYTZT． 以左紅外相機(jī)的光心為原點(diǎn)OC建立相機(jī)坐標(biāo)系OC-XCYCZC，ZC軸與光軸重合． 根據(jù)坐標(biāo)系建立規(guī)則， 基坐標(biāo)系O0-X0Y0Z0與云臺(tái)坐標(biāo)系OT-XTYTZT的原點(diǎn)共線．

已知， 坐標(biāo)系繞X軸和Z軸旋轉(zhuǎn)具有式(2)的旋轉(zhuǎn)矩陣關(guān)系[16]．

(2)

θ，γ分別是坐標(biāo)系繞X軸和Z軸的旋轉(zhuǎn)角度．

(3)

設(shè)空間中一點(diǎn)q在基坐標(biāo)系下為q0， 在云臺(tái)坐標(biāo)系下為qT， 根據(jù)坐標(biāo)系轉(zhuǎn)換原理可得式(4)的關(guān)系．

(4)

(5)

Δθ和Δγ分別是云臺(tái)左右擺動(dòng)和上下俯仰兩個(gè)自由度的增量．

(6)

(7)

(8)

采用Tsai兩步法[17]求解式(8)； 已知矩陣A和H的旋轉(zhuǎn)軸和旋轉(zhuǎn)角可以求出轉(zhuǎn)換矩陣T的旋轉(zhuǎn)矩陣R， 再將R代入式(8)則可求出平移分量t， 最后求得相機(jī)坐標(biāo)系與云臺(tái)坐標(biāo)系的轉(zhuǎn)換矩陣T．

3 主莖跟蹤控制算法

3.1 跟蹤參考點(diǎn)坐標(biāo)轉(zhuǎn)化

根據(jù)番茄主莖生長趨勢(shì)實(shí)時(shí)調(diào)整云臺(tái)姿態(tài)使番茄主莖上的跟蹤參考點(diǎn)始終位于視場(chǎng)中心， 是實(shí)現(xiàn)對(duì)番茄主莖主動(dòng)跟蹤的基本思路． 如何確定跟蹤參考點(diǎn)BC，i和跟蹤點(diǎn)的期望位置BC，i+1是實(shí)現(xiàn)對(duì)番茄主莖主動(dòng)跟蹤的關(guān)鍵． 在番茄工廠化種植中， 番茄主莖都做了吊繩落蔓處理， 根據(jù)實(shí)地考察后發(fā)現(xiàn)， 這些番茄植株除在根部有較大彎曲外， 主莖其他部分生長趨勢(shì)比較規(guī)則， 在較小視場(chǎng)下可近似為一條空間曲線． 依據(jù)這些， 提出了跟蹤參考點(diǎn)BC，i和跟蹤點(diǎn)期望位置BC，i+1的確定方法．

(9)

式中， 向量l(m，n，p)為直線L的單位方向向量．

(10)

根據(jù)式(10)可以求得期望點(diǎn)BC，i+1(0， 0，zc，i+1)在相機(jī)坐標(biāo)系下的三維坐標(biāo)．

根據(jù)圖4的跟蹤方案， 已經(jīng)確定了跟蹤參考點(diǎn)BC，i和期望點(diǎn)BC，i+1在相機(jī)坐標(biāo)系下的空間坐標(biāo)．

圖4 番茄主莖主動(dòng)跟蹤原理

3.2 云臺(tái)姿態(tài)伺服控制

(11)

將式(11)求得的云臺(tái)坐標(biāo)點(diǎn)代入到式(6)并作齊次化處理可得：

(12)

將參考點(diǎn)BT，i和BT，i+1的坐標(biāo)代入到式(12)并化簡(jiǎn)可得：

(13)

其中c代表cos函數(shù)，s代表sin函數(shù)．

方程(13)中包含的未知數(shù)Δθ，Δγ是云臺(tái)依次繞X軸和Z軸旋轉(zhuǎn)的角度增量． 解得：

(14)

θ是每一次位姿變換前云臺(tái)坐標(biāo)系OT-XTYTZT相對(duì)于基坐標(biāo)系O0-X0Y0Z0的上下俯仰角度， 可以通過云臺(tái)驅(qū)動(dòng)電機(jī)編碼器實(shí)時(shí)反饋得到．

4 試驗(yàn)與結(jié)果分析

4.1 現(xiàn)場(chǎng)試驗(yàn)

為了驗(yàn)證對(duì)番茄主莖主動(dòng)跟蹤算法的實(shí)際效果， 在北京大興區(qū)宏福國際農(nóng)業(yè)生產(chǎn)基地番茄工廠化溫室內(nèi)開展了現(xiàn)場(chǎng)試驗(yàn)． 選擇5株番茄植株， 分別在跟蹤步長Δl為50 mm， 100 mm， 150 mm和200 mm下完成一次跟蹤試驗(yàn)． 將安裝有相機(jī)的云臺(tái)固定在與番茄植株種植槽等高的可移動(dòng)工作臺(tái)上， 云臺(tái)與試驗(yàn)對(duì)象的水平距離為400～600 mm， 跟蹤的番茄主莖區(qū)域?yàn)殡x地600～1 500 mm高度范圍內(nèi)． 根據(jù)相機(jī)視場(chǎng)大小和試驗(yàn)對(duì)象的高度， 需要調(diào)整云臺(tái)姿態(tài)4～5次以完成對(duì)單株番茄主莖的跟蹤． 為了避免引入主莖識(shí)別誤差， 本文以人眼對(duì)攝像機(jī)視場(chǎng)主莖的實(shí)時(shí)識(shí)別結(jié)果為依據(jù)， 在主莖中心線上選擇跟蹤起始點(diǎn)． 基于python 3開發(fā)了云臺(tái)相機(jī)的跟蹤交互程序， 將相機(jī)的視頻流實(shí)時(shí)顯示在界面上， 如圖5所示， 人工通過鼠標(biāo)在靠近視場(chǎng)中心的番茄主莖上選擇一個(gè)點(diǎn)AC，i作為跟蹤起始點(diǎn)， 沿著主莖的中心線在視場(chǎng)上邊緣主莖上再次選取一個(gè)點(diǎn)AC，i+1． 由深度相機(jī)可得點(diǎn)AC，i，AC，i+1空間坐標(biāo)， 以兩點(diǎn)連線AC，iAC，i+1的立體單位向量作為當(dāng)前視場(chǎng)主莖的單位方向向量l． 設(shè)定跟蹤步長Δl下， 根據(jù)式(9)可以求得跟蹤參考點(diǎn)BC，i在當(dāng)前視場(chǎng)下的坐標(biāo)． 求解云臺(tái)轉(zhuǎn)角位移， 調(diào)整云臺(tái)相機(jī)的姿態(tài)后， 比較相機(jī)的視場(chǎng)中心點(diǎn)與跟蹤期望點(diǎn)的像素偏差．

圖5 人工選取主莖跟蹤起始點(diǎn)

4.2 結(jié)果分析

(15)

式(15)中，D0為番茄主莖的像素寬度， 根據(jù)試驗(yàn)環(huán)境測(cè)得番茄主莖像素寬度平均為15個(gè)像素．

跟蹤結(jié)果如圖6所示， 是同一植株在相同姿態(tài)下不同跟蹤步長的跟蹤結(jié)果． 當(dāng)跟蹤步長Δl=200 mm時(shí)， 跟蹤參考點(diǎn)不在當(dāng)前視場(chǎng)， 為了直觀體現(xiàn)誤差， 試驗(yàn)中可人為標(biāo)記．

圖6 同一姿態(tài)下不同步長跟蹤結(jié)果

表2 跟蹤偏差

圖7和圖8分別是不同跟蹤步長下平均水平偏差和平均垂直偏差． 觀察每組跟蹤步長下的跟蹤偏差， 隨著云臺(tái)姿態(tài)調(diào)整次數(shù)的增加， 跟蹤的平均偏差也在變大， 這是由于隨著跟蹤的進(jìn)行， 跟蹤對(duì)象與云臺(tái)相機(jī)的距離在變大， 相機(jī)的畸變變大， 導(dǎo)致相機(jī)測(cè)距產(chǎn)生偏差． 比較同一次云臺(tái)姿態(tài)調(diào)整后的跟蹤偏差可以發(fā)現(xiàn)， 隨著跟蹤步長的增加， 同等姿態(tài)下跟蹤偏差也在變大， 因?yàn)殡S著跟蹤步長的變大， 對(duì)擬合的主莖直線L的準(zhǔn)確率要求增加． 但是番茄主莖客觀上不是一條規(guī)則的空間直線， 隨著跟蹤步長的增加， 依賴于擬合直線確定的跟蹤參考點(diǎn)誤差增大， 導(dǎo)致跟蹤的偏差也會(huì)變大． 比較每一組數(shù)據(jù)的跟蹤偏差變化曲線可以發(fā)現(xiàn)在云臺(tái)姿態(tài)第4次調(diào)整后， 跟蹤偏差較前3次有明顯的增加， 這是由于第4次姿態(tài)調(diào)整后， 跟蹤距離變化較大， 相機(jī)畸變變大， 導(dǎo)致跟蹤效果變差．

圖7 不同步長跟蹤平均水平偏差

圖8 不同步長跟蹤平均垂直偏差

圖9是同一株番茄植株在不同跟蹤步長下的跟蹤偏差， 在跟蹤步長為50 mm， 100 mm， 150 mm時(shí)， 跟蹤偏差增加幅度較小， 但在跟蹤步長為200 mm時(shí)， 跟蹤偏差增加幅度較大． 說明跟蹤步長在不超過150 mm時(shí)可以保證跟蹤精度， 當(dāng)跟蹤步長達(dá)到或超過200 mm時(shí)， 跟蹤效果變差， 精度下降嚴(yán)重．

圖9 同一植株不同步長跟蹤結(jié)果

圖10是不同跟蹤步長下， 水平平均偏差占番茄主莖像素寬度的多少， 20組數(shù)據(jù)的最大水平平均偏差占主莖像寬的46.7%． 在跟蹤步長為150 mm時(shí)， 最大水平平均偏差占主莖像寬的40%； 在跟蹤步長為200 mm時(shí)， 最大水平平均偏差占主莖像寬的74.7%， 遠(yuǎn)高于平均水平； 說明當(dāng)跟蹤步長Δl超過200 mm時(shí)該算法不適合跟蹤番茄主莖． 可以發(fā)現(xiàn)當(dāng)跟蹤步長越小時(shí)， 跟蹤的精度就越高， 但是跟蹤的效率也會(huì)降低， 綜合考慮將跟蹤步長Δl設(shè)置為100 mm到150 mm之間最為合適．

圖10 不同步長水平偏差與主莖像寬之比

5 結(jié)論

為了提高對(duì)番茄智能管理作業(yè)對(duì)象的探測(cè)效率， 本研究提出了一種基于云臺(tái)相機(jī)的番茄主莖主動(dòng)跟蹤控制方法． 采用二自由度云臺(tái)相機(jī)， 實(shí)現(xiàn)了對(duì)主莖水平和垂直方向的跟蹤； 通過視覺系統(tǒng)結(jié)構(gòu)參數(shù)標(biāo)定， 建立了云臺(tái)與攝像機(jī)坐標(biāo)之間的關(guān)系； 提出了以主莖形態(tài)為引導(dǎo)的主莖跟蹤控制方法， 實(shí)現(xiàn)了攝像機(jī)視場(chǎng)中心對(duì)主莖的定向跟蹤． 試驗(yàn)結(jié)果表明， 在番茄植株離地600～1 500 mm高度范圍內(nèi)， 當(dāng)跟蹤步長Δl≤150 mm時(shí)， 平均水平偏差和平均垂直偏差不超過6個(gè)像素， 水平偏差占番茄主莖像寬不超過40%； 當(dāng)跟蹤步長Δl≥200 mm時(shí)， 跟蹤偏差變大， 水平偏差占番茄主莖像寬超過70%． 綜合考慮跟蹤精度和跟蹤效率， 將跟蹤步長設(shè)置為100～150 mm可以滿足對(duì)番茄主莖的跟蹤． 本研究可以為番茄的收獲、 打葉和授粉提供視覺信息參考．