基于云臺相機的番茄主莖跟蹤控制方法*

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

1. 太原理工大學 信息與計算機學院， 山西 晉中 030600；2. 北京農業智能裝備技術研究中心， 北京100097； 3. 國家農業智能裝備工程技術研究中心， 北京100097

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

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

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

針對番茄智能化作業需要， 本文設計了基于云臺相機的番茄主莖形態視覺信息獲取系統， 提出了對番茄主莖的視覺跟蹤控制方法． 根據攝像機視場內的番茄主莖形態， 實時調整云臺姿態使番茄主莖上的跟蹤參考點始終位于相機視場中心． 該研究可以為番茄整枝、 授粉、 噴藥和采摘等智能化作業環節提供技術支撐．

1 溫室環境與跟蹤原理

1.1 溫室番茄栽培環境

溫室番茄栽培逐漸趨于工廠化和標準化， 這將有利于機器人的智能作業． 選擇試驗的溫室栽培環境如圖1所示． 番茄栽培在離地面600 mm高的培養槽中， 番茄種植行間距約為1 200 mm， 株距為300 mm． 距地面600～1 500 mm的區域為番茄整枝、 打葉、 授粉和收獲的作業區域． 隨著番茄植株的生長， 通過釋放纏繞于主莖的吊線進行落蔓， 使得作業區域相對地面高度保持不變， 因此本文以此高度區域內的番茄植株為跟蹤的對象．

圖1 溫室番茄栽培環境

1.2 主莖跟蹤原理

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

圖2 目標生長規律

2 云臺視覺系統設計

2.1 硬件構成

視覺信息是支撐機器人作業的重要依據， 為了避免大視場帶來的信息冗余以及對目標定位和識別精度的影響， 采用離散小視場方式采集信息． 本次試驗設計的視覺獲取系統如圖3所示．

① Realsense D435i相機； ② 俯仰機構； ③ 擺動機構； ④ 控制模塊與電源．圖3 視覺獲取系統

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

表1 Realsense D435i相機參數

2.2 結構參數標定

明確云臺與相機的結構位置關系是根據圖像信息控制云臺姿態的重要依據[14]． 通過D435i相機可以直接得到目標點在相機坐標系下的坐標， 但為了實現對目標的跟蹤和云臺的控制， 必須將其坐標轉換到云臺坐標系下． 如圖3所示， 云臺坐標系為OT-XTYTZT， 相機坐標系為OC-XCYCZC， 則云臺坐標與相機坐標存在式(1)的轉換關系[15]．

(1)

T為云臺與相機坐標系之間的轉換矩陣． 由式(1)可知， 對云臺相機做標定就是求解旋轉矩陣R和平移分量t．

如圖3所示， 以云臺垂直自由度的轉軸與云臺上平面的交點O0為原點， 并以左右擺動轉軸向上的方向作為Z軸的正方向， 以水平指向目標的方向作為X軸正方向， 建立基坐標系O0-X0Y0Z0． 以云臺兩個旋轉軸的交點OT為原點， 以俯仰旋轉軸為Y軸， 以與相機光軸平行的方向為X軸， 建立云臺坐標系OT-XTYTZT． 以左紅外相機的光心為原點OC建立相機坐標系OC-XCYCZC，ZC軸與光軸重合． 根據坐標系建立規則， 基坐標系O0-X0Y0Z0與云臺坐標系OT-XTYTZT的原點共線．

已知， 坐標系繞X軸和Z軸旋轉具有式(2)的旋轉矩陣關系[16]．

(2)

θ，γ分別是坐標系繞X軸和Z軸的旋轉角度．

(3)

設空間中一點q在基坐標系下為q0， 在云臺坐標系下為qT， 根據坐標系轉換原理可得式(4)的關系．

(4)

(5)

Δθ和Δγ分別是云臺左右擺動和上下俯仰兩個自由度的增量．

(6)

(7)

(8)

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

3 主莖跟蹤控制算法

3.1 跟蹤參考點坐標轉化

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

(9)

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

(10)

根據式(10)可以求得期望點BC，i+1(0， 0，zc，i+1)在相機坐標系下的三維坐標．

根據圖4的跟蹤方案， 已經確定了跟蹤參考點BC，i和期望點BC，i+1在相機坐標系下的空間坐標．

圖4 番茄主莖主動跟蹤原理

3.2 云臺姿態伺服控制

(11)

將式(11)求得的云臺坐標點代入到式(6)并作齊次化處理可得：

(12)

將參考點BT，i和BT，i+1的坐標代入到式(12)并化簡可得：

(13)

其中c代表cos函數，s代表sin函數．

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

(14)

θ是每一次位姿變換前云臺坐標系OT-XTYTZT相對于基坐標系O0-X0Y0Z0的上下俯仰角度， 可以通過云臺驅動電機編碼器實時反饋得到．

4 試驗與結果分析

4.1 現場試驗

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

圖5 人工選取主莖跟蹤起始點

4.2 結果分析

(15)

式(15)中，D0為番茄主莖的像素寬度， 根據試驗環境測得番茄主莖像素寬度平均為15個像素．

跟蹤結果如圖6所示， 是同一植株在相同姿態下不同跟蹤步長的跟蹤結果． 當跟蹤步長Δl=200 mm時， 跟蹤參考點不在當前視場， 為了直觀體現誤差， 試驗中可人為標記．

圖6 同一姿態下不同步長跟蹤結果

表2 跟蹤偏差

圖7和圖8分別是不同跟蹤步長下平均水平偏差和平均垂直偏差． 觀察每組跟蹤步長下的跟蹤偏差， 隨著云臺姿態調整次數的增加， 跟蹤的平均偏差也在變大， 這是由于隨著跟蹤的進行， 跟蹤對象與云臺相機的距離在變大， 相機的畸變變大， 導致相機測距產生偏差． 比較同一次云臺姿態調整后的跟蹤偏差可以發現， 隨著跟蹤步長的增加， 同等姿態下跟蹤偏差也在變大， 因為隨著跟蹤步長的變大， 對擬合的主莖直線L的準確率要求增加． 但是番茄主莖客觀上不是一條規則的空間直線， 隨著跟蹤步長的增加， 依賴于擬合直線確定的跟蹤參考點誤差增大， 導致跟蹤的偏差也會變大． 比較每一組數據的跟蹤偏差變化曲線可以發現在云臺姿態第4次調整后， 跟蹤偏差較前3次有明顯的增加， 這是由于第4次姿態調整后， 跟蹤距離變化較大， 相機畸變變大， 導致跟蹤效果變差．

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

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

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

圖9 同一植株不同步長跟蹤結果

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

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

5 結論

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