柑橘果園機器人仿生眼的目標跟蹤與定位系統(tǒng)

陳繼清, 徐關(guān)文, 強 虎, 劉 旭, 莫榮現(xiàn), 黃仁智

(1.廣西大學(xué)機械工程學(xué)院， 南寧 530007；2.廣西制造系統(tǒng)與先進制造技術(shù)重點實驗室， 南寧 530007)

柑橘是世界四大水果之一，同時也是中國水果類消費品的重要成員。其中，廣西連續(xù)幾年的柑橘產(chǎn)量都居全國之首，柑橘產(chǎn)業(yè)園有500多個[1]。柑橘園區(qū)機器人能完成觀測柑橘的生長情況，害蟲入侵預(yù)警等工作，并在柑橘成熟期裝載上柑橘采摘機械手采摘。柑橘園區(qū)機器人非常依賴與之匹配的視覺系統(tǒng)，通過對圖像的處理獲取周圍環(huán)境的信息，并執(zhí)行。在農(nóng)業(yè)機器人共性技術(shù)的研究中，視覺系統(tǒng)正在逐漸成為研究熱點[2-3]。Li等[4]等采用改進的大津法(OTSU)動態(tài)閾值分割方法實現(xiàn)了復(fù)雜環(huán)境下蘋果果實的識別和提取; 李文洋[5]研究了獼猴桃采摘機器人的視覺路徑導(dǎo)航路徑的生成方法；Xiang等[6]研究了基于雙目立體相機獲取的深度圖的聚類番茄識別方法。在300～500 mm的采集距離下，番茄群集的識別精度為87.9%；郭凱敏等[7]研究了基于Bumblebee雙目視覺系統(tǒng)的西紅柿采摘機器人，實現(xiàn)了對果實的識別與定位，并達到采摘工作要求；宋健等[8]研制的茄子采摘機器人結(jié)合基于亮度的閾值分割算法與四連通標記法提取果實感興趣特征，最后通過雙目匹配算法獲得采摘果實的三維信息；Mehta等[9]研制的柑橘采摘機器人基于閉環(huán)伺服控制，其雙目攝像頭一個安裝在固定位置，另一個安裝在末端執(zhí)行器，采摘成功率達到95%。

上述文獻都對園林機器人雙目視覺系統(tǒng)的研究取得了不錯的進展，但是都僅限于雙目為平行光軸下的圖像處理，其雙目平臺都為靜平臺，很少考慮為園林機器人視覺系統(tǒng)增添自由度，尤其是對多自由度雙目云臺的目標跟蹤與定位研究更是少之又少，不能使其像人類的頭眼結(jié)構(gòu)一樣靈活運動以獲得更大的視野范圍和目標定位效果。針對此，搭建柑橘園區(qū)機器人的仿生眼視覺平臺，提出了一種基于雙目仿生眼的柑橘果園機器人目標跟蹤與定位系統(tǒng)，實驗驗證該視覺系統(tǒng)能夠滿足機器人的精度和實時性要求。

1 仿生眼平臺

目前市場上絕大多數(shù)的果園機器人只配備主光軸平行的雙目系統(tǒng)，為固定云臺，大大限制了機器人感知環(huán)境能力[10]。人眼具有快速目標跟蹤定位的功能，機器人也需要相應(yīng)的圖像跟蹤。因此設(shè)計了基于人類頭眼運動特點和生理結(jié)構(gòu)的仿生眼機構(gòu)。該系統(tǒng)可以模仿人類視覺系統(tǒng)為了跟蹤目標而做出的平滑追蹤、掃視等動作，從而提高了機器人自身姿態(tài)變化和目標姿態(tài)變化對圖像質(zhì)量的影響。

圖1為設(shè)計的主動雙目視覺機構(gòu)，由左、右偏航子平臺(眼球)、俯仰平臺和旋轉(zhuǎn)底座(頸)組成。旋轉(zhuǎn)底座上的電機與中位軸、中位軸與俯仰子平臺、俯仰子平臺電機與左偏航子平臺、左右偏航子平臺電機與相機均用轉(zhuǎn)動副連接，參照人眼目標跟蹤時共軛運動的優(yōu)越性，將左、右偏航子平臺通過桿件相接，可做同步的俯仰運動，因此仿生視覺平臺具備空間4個自由度的運動，分別為左右相機的偏航運動、左右相機同步的俯仰運動以及底部仿人頸平臺的水平旋轉(zhuǎn)。

對每個關(guān)節(jié)做受力分析，進行最大力矩估算。圖2為機構(gòu)簡圖，圖2中αi(i=1,2,3,4) 為關(guān)節(jié)旋轉(zhuǎn)角度。為了簡化計算，將兩個相機看成一個質(zhì)點，質(zhì)量m為190 g。L為相機質(zhì)心到俯仰軸線的長度，L=205 mm。B為兩個相機質(zhì)心的距離，B=80 mm。

1為相機；2為左偏航子平臺；3為右偏航子平臺；4為俯仰平臺；5為旋轉(zhuǎn)平臺圖1 仿生云臺結(jié)構(gòu)設(shè)計Fig.1 Bionic head 3D structure design

圖2 機構(gòu)簡圖Fig.2 Mechanism diagram

以旋轉(zhuǎn)底座關(guān)節(jié)為例，估算其電機所需功率計算過程如下。

計算轉(zhuǎn)動慣量Jr：

(1)

計算最大力矩T：

T=Jα=m(Jr+Jl)α

(2)

計算所需功率P：

P=Tω=30 W

(3)

式中：Jr為右眼轉(zhuǎn)動慣量；Jl為左眼轉(zhuǎn)動慣量；α為角加速度；ω為角速度。

同理可估算出偏航電機所需功率為1.2 W，俯仰電機所需功率為7 W。選擇由Maxon公司生產(chǎn)的直流減速電機，電機自帶光電增量式傳感器。

2 仿生眼建模

首先介紹一個仿生眼圖像觀測模型[11]。仿生雙目機構(gòu)是一個4自由度的主動視覺云臺，觀測圖像為理想圖像經(jīng)過一系列空間轉(zhuǎn)換而產(chǎn)生，最重要的空間轉(zhuǎn)換因素為云臺關(guān)節(jié)的位姿改變。

2.1 坐標系設(shè)置

針對仿生眼平臺，定義如圖3所示5種坐標系，其中Ow-XwYwZw為世界坐標系，設(shè)置在仿生眼機構(gòu)底座中心。O1=X1Y1Z1為俯仰平臺坐標系。O4=X4Y4Z4和O2=X2Y2Z2和分別為左右子偏航平臺坐標系。O5-X5Y5Z5和O3-X3Y3Z3分別為左、右相機坐標系。根據(jù)機器人運動學(xué)原理，可以得到相鄰兩個坐標系之間的變換矩陣[6]：

(4)

式(4)中：L1、L2、L3為相鄰兩個坐標原點之間的長度：q1、q2、q3、q4、q5為坐標系進行變換所要轉(zhuǎn)的角度。

圖3 坐標系設(shè)置Fig.3 Coordinate system settings

2.2 相機內(nèi)參數(shù)標定

對于一個線性空間不變成像系統(tǒng)，其空間域成像過程可用式(5)描述[12]：

g(x)=k(x)*f(x)

(5)

式(5)中：g(x)表示像；f(x)表示物；k(x)為點擴展函數(shù)；*表示卷積運算。式(5)用齊次坐標表示，得

(6)

(7)

(8)

2.3 相機外參數(shù)自適應(yīng)標定

仿生眼云臺需要配合四個自由度的電機轉(zhuǎn)動對目標進行跟蹤，其相機外參數(shù)具有不確定性。針對此，提出一種相機外參數(shù)矩陣自適應(yīng)標定的方法。以偏航云臺左相機為例，設(shè)在T0時某點在世界坐標系Ow-XwYwZw上的坐標為Pw=[Xw,Yw,Zw,1]T，其經(jīng)過運動學(xué)轉(zhuǎn)換之后映射到左相機坐標系O4=X4Y4Z4上的點坐標為PL=[X4,Y4,Z4,1]T，可以得到

(9)

同理可以得到世界坐標系O4=X4Y4Z4上的點Pw在右相機坐標系O3=X3Y3Z3中的坐標為PR：

(10)

(11)

(12)

式(11)、式(12)經(jīng)過最小二乘法處理后便可以得到目標的三維信息，實現(xiàn)目標定位[13]。

3 目標定位與追蹤

基于顏色直方圖的Camshift目標跟蹤算法是目前主流的目標跟蹤算法，該算法主要解決了基于核函數(shù)的固定窗口寬度會降低均值漂移(meanshift)算法準確率的缺點[14]。該算法本質(zhì)上是一種密度函數(shù)梯度估計的非參數(shù)方法，在局部搜索區(qū)域內(nèi)迭尋優(yōu)。Camshift算法與MeanShift算法相比，給出了一種新的觀測方向函數(shù)，具有更高的跟蹤精度與實時性。算法流程如下。

(1)初始化搜索窗口的大小和位置。

(2)將圖像從RGB空間轉(zhuǎn)換到HSV空間。選取搜索窗口中每個像素的H分量，得出目標的顏色直方圖。

(3)執(zhí)行MeanShift算法，由投影圖的灰度，計算搜索窗零階矩和一階矩：

(13)

搜索窗的質(zhì)心(x0y0)：

(14)

搜索窗長度：

(15)

式中：Ic(x,y)為圖像像素(x,y)。

(4)搜索窗的中心移動到質(zhì)心位置，若距離大于閾值，則重復(fù)步驟(2)、步驟(3)，直到收斂。

(5)計算搜索窗口的二階矩：

(16)

自適應(yīng)更新目標區(qū)域的大小和方向，l為搜索窗長度，w為搜索框高度，q為調(diào)整角度，其表達式為

(17)

(6)返回步驟(1)，實現(xiàn)對視頻下一幀的目標識別與追蹤。

4 仿生眼控制系統(tǒng)

研究的仿生眼平臺為基于圖像的視覺伺服(IBVS)控制系統(tǒng)[15]。采用基于目標特征點脫靶量為控制參數(shù)的PID運動控制器，目標特征點的脫靶量由其在圖像平面的像素與圖像平面的中心像素做差得到，輸出云臺在水平、俯仰、偏航方向所要旋轉(zhuǎn)的角度，使相對運動的目標始終處于兩條視軸的交匯處。脫靶量與云臺旋轉(zhuǎn)角度的轉(zhuǎn)換關(guān)系為

(18)

式(19)中：Δqx，Δqy分別為需要轉(zhuǎn)動的水平和俯仰角度；(u*,v*)為圖像中心像素坐標；(u,v)為目標的像素坐標；Fx、Fy為相機的焦距。

控制器的輸出和輸入之間存在非線性時，控制器PI/PID積分部分會產(chǎn)生一種不良現(xiàn)象即控制器的windup問題[16]。實際的仿生眼控制系統(tǒng)具有突出的飽和非線性特性。引入適當(dāng)?shù)难a償環(huán)節(jié)，使控制系統(tǒng)在出現(xiàn)飽和現(xiàn)象時仍能達到滿意系統(tǒng)響應(yīng)，提出一種變結(jié)構(gòu)PID控制器，其結(jié)構(gòu)如圖4所示。

e為誤差；es為飽和誤差項；un為PID輸出；us為前反饋輸出；r為系統(tǒng)輸入；y為系統(tǒng)輸出圖4 基于Anti-windup的PID控制器結(jié)構(gòu)Fig.4 PID controller structure based on Anti-windup

當(dāng)控制器輸出信號飽和時，飽和誤差項為un-ux，采用系數(shù)η實現(xiàn)積分項自適應(yīng)調(diào)整,其自適應(yīng)變化率為

(19)

5 實驗結(jié)果與分析

實驗在仿生眼平臺上進行。本系統(tǒng)中可通過控制云臺位姿變化實現(xiàn)圖像平面對于目標的跟蹤定位。以柑橘為研究對象，在自然光環(huán)境下開展試驗，以驗證本文仿生眼目標跟蹤與定位系統(tǒng)的有效性和可行性。

圖5 控制系統(tǒng)框架Fig.5 Control system framework

仿生眼平臺控制系統(tǒng)的整體框架如圖5所示。視覺傳感器為兩臺相同型號為MER-132-30UC-L 的大恒CCD攝像機，和兩個型號為M1224-MPW2的鏡頭，分別安裝在雙目云臺上，通過USB3.0傳送數(shù)據(jù)。采用PC機作為控制系統(tǒng)上位機，主要負責(zé)處理由相機所拍攝的圖像序列信息，同時與運動控制器進行通信，其CPU為intel?coreTMi5-4200。運動控制器采用STM32F103型號的單片機，通過發(fā)送PWM脈寬調(diào)制信號到驅(qū)動模塊，信號經(jīng)功率放大后驅(qū)動電機工作，同時逆變器實時進行AD采樣，實現(xiàn)過流與電壓保護作用。上位機與運動控制模塊經(jīng)過RS232串口通信，4個電機的反饋信息通過串行端口傳輸?shù)娇刂颇K，實現(xiàn)閉環(huán)控制。圖像處理平臺基于Visual Studio 2017與OPENCV4.1.2搭建。

選取一張帶莖葉干擾的柑橘圖像作為研究對象，圖6為Camshift算法圖像預(yù)處理過程。如圖6(a)所示為原圖圖像。如圖6(b)、圖6(c)所示，將圖像從RGB顏色空間轉(zhuǎn)到HSV空間，圖6(d)為圖像H分量的直方圖。將直方圖做反向投影后得到色調(diào)H的概率分布，然后得到搜索窗口與窗口質(zhì)心位置。目標在圖像上的質(zhì)心像素相對圖像中心像素的偏移值作為云臺控制器控制參數(shù)，輸出云臺旋轉(zhuǎn)角度，使仿生眼跟蹤目標。圖7為目標跟蹤效果圖，模擬機器人移動場景，在跟蹤過程中有意讓葉子遮擋大部分目標，目標仍然被有效跟蹤。

圖7 跟蹤效果Fig.7 Tracking effects

圖6 圖像預(yù)處理過程Fig.6 Image pre-processing

選取圖像平面分辨率為400×300，則其中心像素坐標為O(200，150)。圖8為云臺跟蹤目標過程中目標在相機圖像平面的質(zhì)心像素，目標始終穩(wěn)定在左右圖像平面的中心點O(200，150)附近。仿生眼具有良好的跟蹤性能，且每幀圖像處理過程小于0.1 s，實用性高。

定義物理量中心位置像素誤差(error of center pixel，CPE)來作為云臺伺服跟蹤效果的標準，其表達式為

(20)

式(20)中：(u*,v*)為圖像中心像素坐標；(u，v)為提取的目標質(zhì)心實時像素坐標。

圖8 目標特征點像素值Fig.8 Target feature point pixel value

圖9 中心位置像素誤差Fig.9 Pixel error of center position

選取10 s的采樣時間段，如圖9所示的仿生眼中心位置像素誤差值。其中，左眼實時中心位置像素誤差(CPE)如圖9(a)所示，平均CPE為7.2，右眼實時CPE如圖9(b)所示，平均CPE為6.1。誤差主要是由于相機模型誤差與機械傳動誤差造成的。仿生眼平均CPE低于10，能夠達到本系統(tǒng)所需要的跟蹤效果。

6 結(jié)論

在仿生眼運動平臺上展開研究，得到以下結(jié)論。

(1)提出一種相機外參數(shù)自適應(yīng)標定方法對仿生眼系統(tǒng)建模，解決了仿生眼平臺相機外參數(shù)不確定性問題。

(2)采用基于Camshift的目標跟蹤算法，并結(jié)合基于Anti-windup的變結(jié)構(gòu)自適應(yīng)抗飽和PID控制器來設(shè)計仿生眼控制系統(tǒng)，大大降低了仿生眼系統(tǒng)飽和非線性特性。

(3)在仿生眼平臺上進行了相關(guān)實驗，實驗驗證了該仿生眼系統(tǒng)有較好的目標跟蹤效果并在一定程度上能達到實時性的要求。