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

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

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

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

1 仿生眼平臺(tái)

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

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

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

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

圖2 機(jī)構(gòu)簡(jiǎn)圖Fig.2 Mechanism diagram

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

計(jì)算轉(zhuǎn)動(dòng)慣量Jr：

(1)

計(jì)算最大力矩T：

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

(2)

計(jì)算所需功率P：

P=Tω=30 W

(3)

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

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

2 仿生眼建模

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

2.1 坐標(biāo)系設(shè)置

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

(4)

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

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

2.2 相機(jī)內(nèi)參數(shù)標(biāo)定

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

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

(5)

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

(6)

(7)

(8)

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

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

(9)

同理可以得到世界坐標(biāo)系O4=X4Y4Z4上的點(diǎn)Pw在右相機(jī)坐標(biāo)系O3=X3Y3Z3中的坐標(biāo)為PR：

(10)

(11)

(12)

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

3 目標(biāo)定位與追蹤

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

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

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

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

(13)

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

(14)

搜索窗長(zhǎng)度：

(15)

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

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

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

(16)

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

(17)

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

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

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

(18)

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

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

e為誤差；es為飽和誤差項(xiàng)；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)控制器輸出信號(hào)飽和時(shí)，飽和誤差項(xiàng)為un-ux，采用系數(shù)η實(shí)現(xiàn)積分項(xiàng)自適應(yīng)調(diào)整,其自適應(yīng)變化率為

(19)

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

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

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

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

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

圖7 跟蹤效果Fig.7 Tracking effects

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

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

定義物理量中心位置像素誤差(error of center pixel，CPE)來(lái)作為云臺(tái)伺服跟蹤效果的標(biāo)準(zhǔn)，其表達(dá)式為

(20)

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

圖8 目標(biāo)特征點(diǎn)像素值Fig.8 Target feature point pixel value

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

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

6 結(jié)論

在仿生眼運(yùn)動(dòng)平臺(tái)上展開(kāi)研究，得到以下結(jié)論。

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

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

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