張晴暉 孔德肖 李俊萩 鐘麗輝

(西南林業(yè)大學(xué)大數(shù)據(jù)與智能工程學(xué)院， 昆明 650224)

0 引言

實(shí)現(xiàn)對(duì)機(jī)械臂運(yùn)動(dòng)控制和果實(shí)空間坐標(biāo)定位是采摘設(shè)備設(shè)計(jì)與實(shí)現(xiàn)的重要環(huán)節(jié)。隨著現(xiàn)代科技的發(fā)展，智能識(shí)別算法、機(jī)械領(lǐng)域新技術(shù)也越來(lái)越多地應(yīng)用到果蔬采摘設(shè)備設(shè)計(jì)中，這些新技術(shù)在提高農(nóng)業(yè)生產(chǎn)自動(dòng)化和智能化的同時(shí)，也降低了生產(chǎn)成本、提高了工作效率。目前，國(guó)內(nèi)外學(xué)者對(duì)采摘設(shè)備進(jìn)行了大量的研究和實(shí)驗(yàn)[1-12]。文獻(xiàn)[4]設(shè)計(jì)了一種采摘機(jī)器人，該機(jī)器人通過(guò)激光測(cè)距檢測(cè)到水果，再通過(guò)計(jì)算機(jī)精確定位，并控制機(jī)械系統(tǒng)實(shí)現(xiàn)采摘；文獻(xiàn)[5]基于單目視覺(jué)與超聲檢測(cè)對(duì)振蕩果實(shí)采摘進(jìn)行了識(shí)別與定位研究；文獻(xiàn)[6]結(jié)合ARM開(kāi)發(fā)技術(shù)設(shè)計(jì)了一套機(jī)械臂運(yùn)動(dòng)控制和避障系統(tǒng)，并通過(guò)Matlab進(jìn)行三維仿真實(shí)驗(yàn)，結(jié)果表明，系統(tǒng)具有較好的實(shí)時(shí)性、可靠性和穩(wěn)定性；文獻(xiàn)[7]設(shè)計(jì)了一種芒果采摘機(jī)，該設(shè)備可確定正確的莖部切割位置，以減少乳膠滲出，保護(hù)果實(shí)免受傷害，并防止果實(shí)掉落；文獻(xiàn)[8]以ARM92440處理器為核心，利用BP神經(jīng)網(wǎng)絡(luò)進(jìn)行機(jī)器人視覺(jué)識(shí)別和定位，研制了一種蘋果采摘機(jī)器人系統(tǒng)，實(shí)驗(yàn)表明，該系統(tǒng)能有效識(shí)別果實(shí)，并完成抓取工作；文獻(xiàn)[9]采用多尺度多層感知器(MLP)和卷積神經(jīng)網(wǎng)絡(luò)(CNN)對(duì)蘋果特征進(jìn)行學(xué)習(xí)，再使用分水嶺分割(WS)和循環(huán)霍夫變換(CHT)算法對(duì)蘋果圖像進(jìn)行分割，提高了檢測(cè)精度。文獻(xiàn)[10]針對(duì)采摘過(guò)程中番茄收獲機(jī)器人果實(shí)串采摘點(diǎn)位置難以確定的問(wèn)題，提出基于果梗骨架角點(diǎn)計(jì)算方法，利用雙目視覺(jué)圖像采集系統(tǒng)采集了60組果實(shí)串圖像，并獲取果梗采摘點(diǎn)位置信息，結(jié)果表明，采摘點(diǎn)位置成功率為90%；文獻(xiàn)[11]針對(duì)蘋果采摘定位問(wèn)題提出基于YOLO v3(You only look once)深度卷積神經(jīng)網(wǎng)絡(luò)的蘋果定位方法，為機(jī)器人在復(fù)雜環(huán)境下快速識(shí)別蘋果提供了理論依據(jù)；文獻(xiàn)[12]為實(shí)現(xiàn)溫室環(huán)境下農(nóng)業(yè)采摘機(jī)器人對(duì)番茄果實(shí)的快速、精確識(shí)別，提出了一種改進(jìn)型多尺度YOLO算法 (IMs-YOLO)，并通過(guò)番茄溫室大棚采摘實(shí)驗(yàn)驗(yàn)證了模型的可行性和準(zhǔn)確性。上述研究針對(duì)采摘方法、果實(shí)識(shí)別準(zhǔn)確率以及果實(shí)采摘過(guò)程保護(hù)進(jìn)行了研究，但關(guān)于多種果實(shí)識(shí)別采摘及設(shè)備遠(yuǎn)程控制的研究相對(duì)較少。

本文使用YOLO v4目標(biāo)檢測(cè)識(shí)別算法對(duì)果實(shí)進(jìn)行像素定位和識(shí)別，訓(xùn)練多種果實(shí)的識(shí)別模型，并結(jié)合Android開(kāi)發(fā)技術(shù)、服務(wù)器和網(wǎng)絡(luò)通信技術(shù)，對(duì)多種果實(shí)進(jìn)行智能識(shí)別及采摘，為降低機(jī)械臂逆運(yùn)動(dòng)學(xué)求解時(shí)的分析難度和計(jì)算量，通過(guò)降維方式進(jìn)行機(jī)械臂逆運(yùn)動(dòng)學(xué)解算。

1 系統(tǒng)總體設(shè)計(jì)

1.1 總體框架

圖1為果實(shí)采摘系統(tǒng)的連接及功能框圖，系統(tǒng)中樹(shù)莓派和手機(jī)端APP使用Socket通信協(xié)議與服務(wù)器進(jìn)行連接，實(shí)現(xiàn)數(shù)據(jù)的互傳。樹(shù)莓派對(duì)實(shí)時(shí)圖像拍攝和上傳，完成機(jī)械臂逆運(yùn)動(dòng)學(xué)解算和運(yùn)動(dòng)軌跡規(guī)劃，通過(guò)GPIO管腳與六自由度機(jī)械臂連接，輸出控制信號(hào)，控制機(jī)械臂運(yùn)動(dòng)到指定位置，實(shí)現(xiàn)果實(shí)的采摘。服務(wù)器使用YOLO v4識(shí)別算法對(duì)樹(shù)莓派上傳的圖像數(shù)據(jù)進(jìn)行果實(shí)目標(biāo)檢測(cè)和像素定位，并對(duì)果實(shí)空間坐標(biāo)進(jìn)行解算，再將解算結(jié)果發(fā)送到樹(shù)莓派。手機(jī)端APP可以實(shí)現(xiàn)與采摘設(shè)備的遠(yuǎn)程連接，控制采摘設(shè)備的啟動(dòng)和停止，可實(shí)時(shí)查看采摘設(shè)備的現(xiàn)場(chǎng)采摘視頻，還可以通過(guò)手機(jī)端選擇需要采摘的果實(shí)類型。

圖1 果實(shí)采摘系統(tǒng)連接及功能框圖Fig.1 System connection and function diagram of fruit picking system

1.2 設(shè)計(jì)流程

系統(tǒng)的設(shè)計(jì)流程如圖2所示。打開(kāi)手機(jī)APP以及采摘設(shè)備開(kāi)機(jī)上電，APP和采摘設(shè)備自動(dòng)連接至服務(wù)器，服務(wù)器端將采摘設(shè)備的IP信息發(fā)送到手機(jī)APP，在APP選中IP地址即可進(jìn)行采摘設(shè)備的連接。設(shè)備連接成功后，可通過(guò)手機(jī)APP對(duì)采摘設(shè)備進(jìn)行遠(yuǎn)程控制，此時(shí)樹(shù)莓派將采摘現(xiàn)場(chǎng)的實(shí)時(shí)圖像上傳至服務(wù)器進(jìn)行圖像識(shí)別和空間定位，而后服務(wù)器將空間定位坐標(biāo)返回樹(shù)莓派。樹(shù)莓派根據(jù)空間坐標(biāo)進(jìn)行機(jī)械臂逆運(yùn)動(dòng)學(xué)解算和機(jī)械臂運(yùn)動(dòng)軌跡規(guī)劃，最終由樹(shù)莓派輸出控制信號(hào)，控制機(jī)械臂進(jìn)行果實(shí)采摘。采摘過(guò)程中，可以點(diǎn)擊APP中的查看按鈕查看采摘現(xiàn)場(chǎng)的實(shí)時(shí)視頻。

圖2 系統(tǒng)總體流程圖Fig.2 System flow chart

2 采摘系統(tǒng)機(jī)械臂設(shè)計(jì)

2.1 硬件結(jié)構(gòu)

對(duì)果實(shí)進(jìn)行采摘的是一個(gè)六自由度的機(jī)械臂，圖3a為其實(shí)物圖。該機(jī)械臂由6個(gè)6125型數(shù)字舵機(jī)構(gòu)成旋轉(zhuǎn)關(guān)節(jié)，單個(gè)舵機(jī)的扭矩為25 kg·cm，6個(gè)舵機(jī)與其它金屬零件組成機(jī)械臂。

圖3 機(jī)械臂實(shí)物圖Fig.3 Physical drawing of mechanical arm

機(jī)械臂爪子部分可根據(jù)采摘不同的果實(shí)進(jìn)行更換，這樣可以使采摘機(jī)器人更好地完成果實(shí)采摘任務(wù)。當(dāng)采摘的果實(shí)直徑較小時(shí)，選用如圖3b所示的開(kāi)口較小的爪子進(jìn)行采摘，該爪子最大的開(kāi)口為55 mm。果實(shí)直徑較大時(shí)，選用如圖3c所示的開(kāi)口較大的爪子進(jìn)行采摘，該爪子最大的開(kāi)口為125 mm。該機(jī)械臂參數(shù)見(jiàn)表1。

表1 機(jī)械臂參數(shù)Tab.1 Parameters of mechanical arm

2.2 機(jī)械臂逆運(yùn)動(dòng)學(xué)降維求解

機(jī)械臂的運(yùn)動(dòng)學(xué)分為正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)兩種。正運(yùn)動(dòng)學(xué)是設(shè)定好機(jī)械臂各個(gè)關(guān)節(jié)角，來(lái)實(shí)現(xiàn)機(jī)械臂的運(yùn)動(dòng)控制；逆運(yùn)動(dòng)學(xué)是已知一個(gè)在機(jī)械臂運(yùn)動(dòng)范圍之內(nèi)的空間坐標(biāo)，根據(jù)已知的空間坐標(biāo)來(lái)求解得到機(jī)械臂各個(gè)關(guān)節(jié)所需旋轉(zhuǎn)的角度，最終控制機(jī)械臂運(yùn)動(dòng)到指定位置。在機(jī)械臂的實(shí)際運(yùn)用中，更多的是實(shí)現(xiàn)機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解。本文設(shè)計(jì)的采摘系統(tǒng)，通過(guò)圖像識(shí)別算法來(lái)指定果實(shí)的空間坐標(biāo)，進(jìn)行機(jī)械臂逆運(yùn)動(dòng)學(xué)求解使機(jī)械臂運(yùn)動(dòng)到果實(shí)位置，最終實(shí)現(xiàn)采摘。

機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解，通常是在三維坐標(biāo)系中實(shí)現(xiàn)。首先需要通過(guò)右手定則確定機(jī)械臂各個(gè)關(guān)節(jié)的正旋轉(zhuǎn)方向。之后得到各個(gè)關(guān)節(jié)的旋轉(zhuǎn)矩陣和平移矩陣，將旋轉(zhuǎn)矩陣和平移矩陣相乘得到一個(gè)新的矩陣，該矩陣描述了機(jī)械臂末端在空間的坐標(biāo)及姿態(tài)。通過(guò)該矩陣就能得到機(jī)械臂末端和各個(gè)關(guān)節(jié)間旋轉(zhuǎn)角的關(guān)系，通過(guò)這個(gè)關(guān)系可實(shí)現(xiàn)機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解。但是這需要進(jìn)行大量的矩陣運(yùn)算和三角函數(shù)運(yùn)算，并且可能求得多個(gè)解，使得機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解變得很復(fù)雜。為了降低運(yùn)算量，該采摘系統(tǒng)中結(jié)合使用的機(jī)械臂，對(duì)機(jī)械臂各個(gè)關(guān)節(jié)進(jìn)行拆分，最終通過(guò)二維坐標(biāo)系實(shí)現(xiàn)機(jī)械臂逆運(yùn)動(dòng)學(xué)求解，大大簡(jiǎn)化了計(jì)算。

2.2.1機(jī)械手臂D-H模型與坐標(biāo)系建立

在分析機(jī)械手臂的運(yùn)動(dòng)過(guò)程時(shí)，需要先建立機(jī)械手臂中各個(gè)關(guān)節(jié)的坐標(biāo)，圖4a為根據(jù)D-H模型建立的機(jī)械臂各個(gè)關(guān)節(jié)的坐標(biāo)圖[13-16]。該圖中使用通用的機(jī)械手臂關(guān)節(jié)符號(hào)來(lái)表示機(jī)械臂的結(jié)構(gòu)。圓形關(guān)節(jié)表示此處的旋轉(zhuǎn)軸垂直于屏幕，圓柱形關(guān)節(jié)表示此處的旋轉(zhuǎn)軸平行于屏幕。該六自由度機(jī)械臂由3個(gè)圓形關(guān)節(jié)、2個(gè)圓柱關(guān)節(jié)以及1個(gè)控制機(jī)械臂末端爪子開(kāi)合的關(guān)節(jié)組成。圖4b為機(jī)械臂的三維建模圖[17]，通過(guò)Matlab中的Robotic Toolbox對(duì)機(jī)械臂進(jìn)行建模。

圖4 機(jī)械臂關(guān)節(jié)坐標(biāo)與建模圖Fig.4 Joint coordinates and modeling diagram of mechanical arm

2.2.2機(jī)械手臂逆運(yùn)動(dòng)學(xué)角度求解過(guò)程實(shí)現(xiàn)

為了提高機(jī)械臂控制的實(shí)時(shí)性，通過(guò)降低維度的方法來(lái)進(jìn)行分析得到逆運(yùn)動(dòng)學(xué)求解。求解過(guò)程中，先將底部云臺(tái)的旋轉(zhuǎn)關(guān)節(jié)和控制末端機(jī)械爪轉(zhuǎn)動(dòng)及開(kāi)合的關(guān)節(jié)去掉，這樣就可以在XOY二維平面進(jìn)行機(jī)械臂姿態(tài)的分析。

圖5 機(jī)械臂XOY平面圖Fig.5 XOY plane diagram of mechanical arm

根據(jù)機(jī)械臂正運(yùn)動(dòng)學(xué)方程的DH表示法，建立圖5中坐標(biāo)系原點(diǎn)與機(jī)械臂末端的總變換矩陣為

(1)

(2)

其中cθ123表示cos(θ1+θ2+θ3)，sθ123表示sin(θ1+θ2+θ3)，sθ12表示sin(θ1+θ2)，cθ12表示cos(θ1+θ2)，sθ1表示sinθ1，cθ1表示cosθ1。

式(2)表示機(jī)械臂末端連桿坐標(biāo)系相對(duì)于機(jī)械臂的描述[20-23]。

由式(2)可以得到機(jī)械臂末端的坐標(biāo)為

Px=L0cθ1+L1cθ12+L2cθ123

(3)

Py=L0sθ1+L1sθ12+L2sθ123

(4)

根據(jù)幾何方法有

α=θ1+θ2+θ3

(5)

由式(3)～(5)組成方程組即可求解得到圖5所示的機(jī)械臂3個(gè)旋轉(zhuǎn)關(guān)節(jié)的姿態(tài)，求解過(guò)程如下：

將式(5)分別代入式(3)、(4)得

Px=L0cosθ1+L1cos(θ1+θ2)+L2cosα

(6)

Py=L0sinθ1+L1sin(θ1+θ2)+L2sinα

(7)

為了方便計(jì)算，令

m=L2cosα-Px
n=L2sinα-Py

化簡(jiǎn)式(6)、(7)有

(8)

通過(guò)求解一元二次方程得

其中

由此求得θ1，重復(fù)上述算法就可以求解得到θ2、θ3。

由于該機(jī)械臂中控制爪子開(kāi)合的關(guān)節(jié)和靠近爪子處的圓柱關(guān)節(jié)在轉(zhuǎn)動(dòng)過(guò)程中不會(huì)對(duì)機(jī)械臂末端的位置產(chǎn)生影響，本文求解機(jī)械臂逆運(yùn)動(dòng)學(xué)時(shí)，不對(duì)這2個(gè)關(guān)節(jié)進(jìn)行求解。所以在求解出θ1、θ2、θ3后，只需要求解得到底部舵機(jī)云臺(tái)處的旋轉(zhuǎn)關(guān)節(jié)的姿態(tài)，即可完成機(jī)械臂逆運(yùn)動(dòng)學(xué)的求解。

圖6是完成了圖4a機(jī)械臂坐標(biāo)建立之后的ZOX平面圖。通過(guò)該圖進(jìn)行求解得到機(jī)械臂底部舵機(jī)云臺(tái)處旋轉(zhuǎn)關(guān)節(jié)的姿態(tài)。根據(jù)給定坐標(biāo)中的Z和X坐標(biāo)即可得到θ

(9)

完成了θ、θ1、θ2、θ3求解之后，就完成了該項(xiàng)目中所使用的機(jī)械臂逆運(yùn)動(dòng)學(xué)角度的求解。

圖6 機(jī)械臂ZOX平面圖Fig.6 ZOX plane diagram of mechanical arm

2.3 果實(shí)識(shí)別與空間坐標(biāo)定位

2.3.1果實(shí)識(shí)別模型訓(xùn)練

實(shí)際采摘過(guò)程中，樹(shù)莓派攝像頭拍攝的視頻圖像中除了果實(shí)之外還有樹(shù)葉、樹(shù)枝和樹(shù)干等其它非果實(shí)物體。如圖7a所示，系統(tǒng)為了更好地在復(fù)雜環(huán)境中進(jìn)行果實(shí)檢測(cè)、識(shí)別以及像素定位，使用OpenCv計(jì)算機(jī)視覺(jué)和機(jī)器學(xué)習(xí)軟件庫(kù)來(lái)進(jìn)行視頻的處理，處理之后通過(guò)基于深度學(xué)習(xí)框架Darknet的YOLO[24]目標(biāo)檢測(cè)算法來(lái)進(jìn)行果實(shí)目標(biāo)檢測(cè)與果實(shí)像素定位。Darknet框架具有CNN底層實(shí)現(xiàn)以及YOLO(You only look once)目標(biāo)檢測(cè)的功能。Darknet框架通過(guò)C語(yǔ)言實(shí)現(xiàn)，具有容易安裝、移植性好和支持CPU、GPU(CUDA)2種計(jì)算方式的特點(diǎn)。在性能上YOLO v4的運(yùn)行速度比EfficientDet快2倍，并且性能相當(dāng)，將YOLO v3的平均精確率(AP)和幀率(FPS)分別提高了10%和12%。

圖7 果實(shí)識(shí)別與目標(biāo)檢測(cè)Fig.7 Recognition and target detection of fruit

進(jìn)行模型訓(xùn)練時(shí)，對(duì)各種果實(shí)在無(wú)遮擋光照充足的條件下實(shí)景拍攝圖像500幅，對(duì)每種果實(shí)的500幅圖像進(jìn)行4種處理方法，包括90°、180°和270°旋轉(zhuǎn)以及進(jìn)行X軸鏡像處理，最終得到每種果實(shí)的訓(xùn)練數(shù)據(jù)集圖像為2 500幅，其中2 000幅圖像用來(lái)進(jìn)行模型訓(xùn)練，500幅圖像用來(lái)模型驗(yàn)證，訓(xùn)練集和驗(yàn)證集比例為4∶1。確定好訓(xùn)練集之后使用圖像標(biāo)注工具對(duì)圖像中果實(shí)進(jìn)行標(biāo)注，并生成PASCAL VOC格式的xml標(biāo)注文件，生成的標(biāo)記文件中使用了5個(gè)參數(shù)(class_fruit,x,y,w,h)對(duì)圖像中的果實(shí)進(jìn)行描述，class_fruit表示果實(shí)的種類，x和y表示果實(shí)在圖像的中心坐標(biāo)，w和h表示目標(biāo)框的長(zhǎng)和寬。之后，對(duì)YOLO v4中的配置文件進(jìn)行修改。該項(xiàng)目中對(duì)配置文件的修改如下：batch(每一批訓(xùn)練的樣本數(shù)量)為16，subdivisions(batch分割數(shù)量)為16，max_batches(迭代次數(shù))為6 000，steps(學(xué)習(xí)率)為4 800、5 400(0.8倍batch為4 800，0.9倍batch為5 400)。進(jìn)行模型訓(xùn)練的硬件環(huán)境為Intel(R)Core(TM)i7-7700HQ CPU 2.80 GHz(8CPUs)處理器，16 GB運(yùn)行內(nèi)存，顯存為8 GB的GTX 1080Ti顯卡，軟件環(huán)境為Ubuntu 19.10操作系統(tǒng)。該系統(tǒng)訓(xùn)練了蘋果、油桃、黃杏和李子4種果實(shí)的識(shí)別模型。

訓(xùn)練好模型之后即可對(duì)樹(shù)莓派上傳的實(shí)時(shí)圖像進(jìn)行果實(shí)目標(biāo)檢測(cè)，檢測(cè)之后輸出圖像中果實(shí)的置信度、種類以及像素點(diǎn)尺寸和中心像素位置，得到的像素尺寸和像素位置用于后期果實(shí)的空間坐標(biāo)定位。圖7b是識(shí)別后的圖像，從圖像中可以看到果實(shí)被藍(lán)綠色邊框框出，果實(shí)標(biāo)簽為“l(fā)izi”，置信度為100%。

2.3.2果實(shí)空間坐標(biāo)定位

得到果實(shí)空間坐標(biāo)是實(shí)現(xiàn)果實(shí)采摘非常關(guān)鍵的步驟，該系統(tǒng)通過(guò)得到果實(shí)相對(duì)于攝像頭的坐標(biāo)來(lái)確定果實(shí)坐標(biāo)，如圖8所示。圖中分別建立了果實(shí)和攝像頭的XOY平面圖和XOZ平面圖，P表示攝像頭拍攝到的圖像，A表示果實(shí)，a表示果實(shí)A在圖像中的尺寸，ay和ax表示a在圖像中x方向和y方向各占據(jù)的像素點(diǎn)數(shù)量。

圖8 果實(shí)空間位置定位Fig.8 Spatial location of fruit

在坐標(biāo)定位中首先要得到圖像中像素點(diǎn)與實(shí)際物體尺寸的轉(zhuǎn)換關(guān)系，使用攝像頭拍攝1元硬幣的方式來(lái)得到轉(zhuǎn)換比率，1元硬幣的直徑為3 cm，在拍攝距離為1 m時(shí)，其在圖像中占163像素，得到轉(zhuǎn)換比率k=163/3=54.3像素/cm。圖中Py和Pz表示a中心點(diǎn)在y軸和z軸到中軸線的偏移量，根據(jù)相似原理，k=Py/YL，k=Pz/ZL，由此可以得到果實(shí)空間坐標(biāo)中的y坐標(biāo)和z坐標(biāo)計(jì)算公式分別為

YL=Py/k

(10)

ZL=Pz/k

(11)

根據(jù)單目攝像頭測(cè)距原理，計(jì)算果實(shí)空間坐標(biāo)中的x坐標(biāo)，計(jì)算公式為

D=WF/P

(12)

式中D——果實(shí)與攝像頭的相對(duì)距離

W——果實(shí)寬度

F——攝像頭焦距

P——果實(shí)在圖像中占的像素?cái)?shù)

樹(shù)莓派攝像頭焦距F為543像素。如圖8所示W(wǎng)=ax/k，P=ax，D=XL，將計(jì)算公式修改為

XL=F/k

(13)

確定果實(shí)A的空間坐標(biāo)為(XL,YL,ZL)。

2.4 機(jī)械臂運(yùn)動(dòng)軌跡規(guī)劃與采摘控制

2.4.1五項(xiàng)式插值法推導(dǎo)

系統(tǒng)首先對(duì)果實(shí)空間坐標(biāo)進(jìn)行定位，然后進(jìn)行逆運(yùn)動(dòng)學(xué)求解，之后再對(duì)機(jī)械臂的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。系統(tǒng)中使用了五項(xiàng)式插值法對(duì)機(jī)械臂運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃，該方法在得到機(jī)械臂各關(guān)節(jié)的起始角和停止角的前提下，可實(shí)現(xiàn)對(duì)角度、角速度和角加速度的約束。該方法的數(shù)學(xué)推導(dǎo)過(guò)程如下：

在五項(xiàng)式插值法中設(shè)定了6個(gè)待定系數(shù)a0～a5，得到起始角和終止角、角速度和角加速度隨時(shí)間變換的函數(shù)關(guān)系。式中θ(t)表示角度隨時(shí)間變換的函數(shù)關(guān)系，θ′(t)和θ″(t)是對(duì)θ(t)進(jìn)行一次求導(dǎo)和二次求導(dǎo)之后的結(jié)果，分別表示角速度和角加速度隨時(shí)間變換的函數(shù)關(guān)系為

(14)

在已知起始角和終止角的前提下，使用θ0和θf(wàn)表示起始角和終止角，并且約束起始速度和終止速度，對(duì)式(14)進(jìn)行推導(dǎo)得到的函數(shù)約束關(guān)系為

(15)

將式(15)代入式(14)求解得到各個(gè)系數(shù)為

(16)

2.4.2五項(xiàng)式插值法實(shí)現(xiàn)單個(gè)電機(jī)運(yùn)動(dòng)控制

在完成五項(xiàng)插值法數(shù)學(xué)推導(dǎo)之后，通過(guò)Python編程語(yǔ)言編寫(xiě)五項(xiàng)式插值法程序。表2為電機(jī)運(yùn)動(dòng)角、角速度和角加速度的約束關(guān)系。運(yùn)行程序之后得到圖9所示的角度、角速度和角加速度隨約束時(shí)間的變化曲線。通過(guò)曲線圖可以看出電機(jī)旋轉(zhuǎn)的角度和角加速度均隨時(shí)間變化，這也意味著電機(jī)在旋轉(zhuǎn)過(guò)程中處于平穩(wěn)的轉(zhuǎn)動(dòng)狀態(tài)，這樣可以保證機(jī)械臂的關(guān)節(jié)電機(jī)在工作過(guò)程中損耗較小，且使得機(jī)械臂整體運(yùn)動(dòng)情況更加協(xié)調(diào)與穩(wěn)定。

表2 電機(jī)運(yùn)動(dòng)角度、角速度和角加速度約束關(guān)系Tab.2 Constraint of angle, angular velocity and angular acceleration of motor motion

圖9 電機(jī)運(yùn)動(dòng)軌跡規(guī)劃曲線Fig.9 Curves of motor motion trajectory planning

2.4.3單次采摘流程與速度設(shè)定

圖10為采摘系統(tǒng)的單次采摘流程圖，系統(tǒng)開(kāi)始工作后，樹(shù)莓派控制機(jī)械臂進(jìn)入初始狀態(tài)，同時(shí)樹(shù)莓派拍攝圖像并上傳，在服務(wù)器中使用訓(xùn)練好的識(shí)別模型對(duì)果實(shí)進(jìn)行識(shí)別，并根據(jù)空間坐標(biāo)定位算法計(jì)算果實(shí)的空間坐標(biāo)并返回到樹(shù)莓派中。之后樹(shù)莓派進(jìn)行逆運(yùn)動(dòng)學(xué)解算，得到機(jī)械臂從初始狀態(tài)運(yùn)動(dòng)到果實(shí)位置各個(gè)關(guān)節(jié)旋轉(zhuǎn)的角度，并且根據(jù)旋轉(zhuǎn)角使用五項(xiàng)式插值法進(jìn)行運(yùn)動(dòng)軌跡規(guī)劃。規(guī)劃后樹(shù)莓派輸出對(duì)應(yīng)的控制信號(hào)，控制機(jī)械臂運(yùn)動(dòng)以實(shí)現(xiàn)果實(shí)的采摘。采摘完成之后進(jìn)行果實(shí)投放，最終機(jī)械臂回到初始狀態(tài)，等待下一次采摘指令。

圖10 單次采摘過(guò)程的流程圖Fig.10 Flow chart of single picking process

在該系統(tǒng)中為了防止機(jī)械臂運(yùn)動(dòng)過(guò)快導(dǎo)致系統(tǒng)不穩(wěn)定以及容易碰撞到果實(shí)，設(shè)定五項(xiàng)式插值法中進(jìn)行運(yùn)動(dòng)軌跡規(guī)劃的約束時(shí)間為5 s。根據(jù)采摘流程圖可以得到，進(jìn)行一次果實(shí)采摘需要通過(guò)五項(xiàng)式插值法進(jìn)行3次運(yùn)動(dòng)軌跡規(guī)劃。設(shè)3次五項(xiàng)式插值法消耗的時(shí)間為T1=15 s，其它各個(gè)流程步驟消耗的時(shí)間為T2，則采摘系統(tǒng)完成單次采摘任務(wù)的總時(shí)間為T=T1+T2=15+T2。由于時(shí)間T2包括了整個(gè)系統(tǒng)的各個(gè)環(huán)節(jié)的運(yùn)作時(shí)間，會(huì)受到網(wǎng)絡(luò)速度、服務(wù)器以及樹(shù)莓派的運(yùn)算速度的影響，無(wú)法計(jì)算得到一個(gè)精準(zhǔn)的時(shí)間，只有通過(guò)實(shí)際測(cè)試獲知。

3 采摘系統(tǒng)試驗(yàn)

圖11a為系統(tǒng)自動(dòng)采摘李子的現(xiàn)場(chǎng)測(cè)試。測(cè)試中通過(guò)APP界面中的“設(shè)備管理”對(duì)采摘設(shè)備進(jìn)行連接，圖11b所示是在手機(jī)APP中選擇采摘的果實(shí)為李子，點(diǎn)擊確定按鈕后采摘設(shè)備開(kāi)始工作。

圖11 果實(shí)實(shí)地采摘測(cè)試圖Fig.11 Test chart of fruit picked in field

與樹(shù)莓派連接的攝像頭進(jìn)行視頻拍攝并上傳到服務(wù)器中，服務(wù)器調(diào)用訓(xùn)練好的李子識(shí)別模型對(duì)上傳的視頻進(jìn)行識(shí)別并對(duì)果實(shí)進(jìn)行空間位置定位，然后將識(shí)別結(jié)果返回樹(shù)莓派，由樹(shù)莓派控制機(jī)械臂進(jìn)行采摘。當(dāng)用戶發(fā)送查看視頻請(qǐng)求指令到服務(wù)器時(shí)，服務(wù)器將樹(shù)莓派上傳的原始視頻提供到手機(jī)端進(jìn)行顯示。圖11c為手機(jī)端APP查看到當(dāng)前設(shè)備的IP、已采摘的果實(shí)數(shù)量、采摘的果實(shí)種類以及實(shí)時(shí)畫(huà)面。

表3為機(jī)械臂姿態(tài)解算測(cè)試結(jié)果，測(cè)試過(guò)程中通過(guò)樹(shù)莓派將實(shí)時(shí)圖像上傳到服務(wù)器中，服務(wù)器中調(diào)用YOLO v4識(shí)別算法訓(xùn)練得到的識(shí)別模型對(duì)上傳的圖像進(jìn)行果實(shí)目標(biāo)檢測(cè)以及像素定位，根據(jù)像素定位計(jì)算得出空間坐標(biāo)，并將果實(shí)的空間坐標(biāo)返

表3 機(jī)械臂姿態(tài)解算測(cè)試結(jié)果Tab.3 Test results of attitude calculation of manipulator

回到樹(shù)莓派中，樹(shù)莓派根據(jù)接收到的空間坐標(biāo)對(duì)機(jī)械臂進(jìn)行逆運(yùn)動(dòng)學(xué)解算，對(duì)不同空間坐標(biāo)實(shí)現(xiàn)機(jī)械臂逆運(yùn)動(dòng)學(xué)解算得到θ、θ1、θ2、θ3，如表3所示。

表4為使用該果實(shí)采摘設(shè)備采摘不同類別果實(shí)的測(cè)試結(jié)果。此次采摘測(cè)試在光照充足的條件下，對(duì)無(wú)遮擋且果實(shí)生長(zhǎng)稀疏的情況下進(jìn)行采摘測(cè)試。測(cè)試過(guò)程中根據(jù)不同果實(shí)尺寸來(lái)選擇爪子型號(hào)，對(duì)每種果實(shí)進(jìn)行采摘測(cè)試的次數(shù)均為60次，在此基礎(chǔ)上得到采摘過(guò)程中機(jī)械臂的最大和最小電流、完成不同種類果實(shí)的采摘總時(shí)間Ta。

表4 不同種類果實(shí)采摘測(cè)試結(jié)果Tab.4 Picking test results of different kinds of fruits

表5為采摘系統(tǒng)的單次采摘時(shí)間，表中Ta為對(duì)各種果實(shí)采摘60次的總時(shí)間，單次果實(shí)采摘時(shí)間計(jì)算公式為T=T1+T2，由于T2是一個(gè)不確定的值，導(dǎo)致單次采摘具體時(shí)間T也無(wú)法準(zhǔn)確確定，這里采用平均時(shí)間Tn來(lái)代替T，Tn=Ta/60表示平均單個(gè)果實(shí)采摘時(shí)間，則有單次果實(shí)采摘時(shí)間為：T=Tn=T1+T2，由表5中可以看出，單次最長(zhǎng)采摘時(shí)間為17.98 s，最短單次采摘時(shí)間為16.58 s。

表5 單次采摘時(shí)間計(jì)算結(jié)果Tab.5 Calculation of single picking time

表6為對(duì)不同種類果實(shí)訓(xùn)練后的模型進(jìn)行測(cè)試的結(jié)果，每種果實(shí)的訓(xùn)練集圖像均為2 000幅，測(cè)試集圖像為500幅，在Ubuntu 19.10操作系統(tǒng)中使用GPU進(jìn)行模型訓(xùn)練，用訓(xùn)練后的模型進(jìn)行測(cè)試得到的各類平均精確率的平均值(mAP)、準(zhǔn)確率、召回率和交并比(IOU)如表6所示。

表6 不同種類果實(shí)的訓(xùn)練模型測(cè)試結(jié)果Tab.6 Training model test results of different kinds of fruits %

通過(guò)實(shí)際采摘測(cè)試，該采摘系統(tǒng)可以實(shí)現(xiàn)對(duì)蘋果、油桃、黃杏、李子4種果實(shí)的自動(dòng)采摘，系統(tǒng)工作穩(wěn)定，但是目前系統(tǒng)設(shè)計(jì)與研究處于實(shí)驗(yàn)階段，還有一些不足之處：

(1)果實(shí)識(shí)別模型采用的數(shù)據(jù)集均是在光照充足的條件下拍攝的無(wú)遮擋果實(shí)圖像，所以采摘系統(tǒng)目前只能在光照充足的條件下對(duì)無(wú)遮擋的果實(shí)進(jìn)行采摘。如果需要設(shè)計(jì)更好的采摘系統(tǒng)，則需要更多情況下的果實(shí)實(shí)景圖像來(lái)進(jìn)行模型訓(xùn)練。

(2)目前采摘系統(tǒng)中只是通過(guò)更改爪子型號(hào)來(lái)實(shí)現(xiàn)對(duì)直徑較大和直徑較小的果實(shí)進(jìn)行采摘，由于爪子是金屬類物體，采摘過(guò)程中會(huì)導(dǎo)致果實(shí)表皮受損，需要更換采摘和夾取方式來(lái)保護(hù)果實(shí)。

(3)目前該系統(tǒng)是通過(guò)人工方式移動(dòng)或安放采摘設(shè)備到某一個(gè)位置來(lái)進(jìn)行采摘測(cè)試，而實(shí)際的采摘系統(tǒng)因根據(jù)不同種類水果的果樹(shù)高度來(lái)設(shè)計(jì)采摘機(jī)械臂以及整個(gè)采摘系統(tǒng)。

4 結(jié)論

(1)基于樹(shù)莓派、六自由度機(jī)械臂、手機(jī)APP、服務(wù)器設(shè)計(jì)了智能果實(shí)采摘系統(tǒng)。

(2)相比于傳統(tǒng)的水果采摘系統(tǒng)，本文設(shè)計(jì)的水果采摘系統(tǒng)通過(guò)YOLO v4訓(xùn)練了多種果實(shí)識(shí)別模型，可以實(shí)現(xiàn)對(duì)多種果實(shí)的自動(dòng)采摘。

(3)該系統(tǒng)中結(jié)合實(shí)際使用的機(jī)械臂，對(duì)機(jī)械臂進(jìn)行關(guān)節(jié)拆分，可以通過(guò)降維的方式來(lái)實(shí)現(xiàn)機(jī)械臂逆運(yùn)動(dòng)學(xué)求解，降低了分析難度和運(yùn)算量。

(4)在機(jī)械臂運(yùn)動(dòng)控制中，使用了五項(xiàng)式插值法來(lái)實(shí)現(xiàn)對(duì)機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃，此法可使機(jī)械臂各個(gè)旋轉(zhuǎn)關(guān)節(jié)的電機(jī)平穩(wěn)轉(zhuǎn)動(dòng)，使機(jī)械臂運(yùn)動(dòng)更加協(xié)調(diào)與穩(wěn)定，減小機(jī)械臂工作過(guò)程中電機(jī)的損耗。

(5)該系統(tǒng)設(shè)計(jì)中結(jié)合了Android開(kāi)發(fā)技術(shù)以及網(wǎng)絡(luò)通信技術(shù)，可以通過(guò)手機(jī)APP端對(duì)采摘設(shè)備進(jìn)行遠(yuǎn)程控制以及查看實(shí)時(shí)采摘視頻。