基于D-H法的農業采摘機器人運動協作控制系統設計

閔小翠，李 鵬

(廣州華立科技職業學院，廣州 511325)

0 引言

農業機器人是農產品收成工作中的重要輔助工具，農業機器人以牽引車為核心，在定位、導航等技術的支持下，通過傳感器、嵌入式智能系統來控制農用機械等機械的運動，從而實現多種農田作業。采摘機器人是農業機器人中的一種，是以農產品采摘為主要工作內容的機器人，采摘機器人的研制和應用，對提高勞動生產率具有重要的現實意義。作物的生長是在時空上發生變化的，其生長的環境是多變的、未知的。因此，農業采摘機器人既要具備與生物的柔性相適應的加工能力，又要適應多變的自然環境。同時，部分農作物表皮較為脆弱，且形態多樣，生長的程度也各不相同。采摘機器人一般是同時進行采摘與移動作業，為了提高農產品的采摘效率，一般采摘工作都由多個采摘機器人共同完成，這種工作方式雖能有效節省工作時間，但也提高了機器人之間的碰撞發生概率，為了降低工作狀態下農業采摘機器人之間的產生的負面影響，設計并開發了機器人運動協作控制系統。

運動控制是一種自動化的分支，它利用伺服機構來控制機械的位置和速度等運動參數。文獻[1]中提出的基于EtherCAT和TwinCAT3的機器人控制系統，通過以太網控制自動化技術實現系統的基本控制功能，并以TwinCAT3作為上位監控工具，實現實時控制指令的調節。文獻[2]中提出了基于無線網絡的機器人控制系統，該系統通過無線網絡進行控制指令的傳輸，而文獻[3]中提出的基于模糊PID的機器人控制系統，以模糊PID技術作為控制原理，通過模糊控制理論與調節控制理論的結合，實現系統完整的控制邏輯。文獻[4]提出考慮氣象因素的農業采摘機器人軌跡控制方法。提出了一種基于云存儲技術的目標果實動態監測方法。基于 RBF神經網絡設計了最優運動模型，并采用該模型的最優運動模型，實現了基于 RBF神經網絡的采摘機器人軌跡控制。

上述系統為現階段發展較為成熟的研究成果，然而在實際的運行過程中，傳統控制系統存在明顯的控制效果不佳的問題，主要體現在控制誤差與機器人碰撞事故兩個方面，為此引入D-H法。D-H方法是將一個坐標系固定于機械臂的各個連桿上，再利用4x4齊次轉換矩陣來表示兩個相鄰的連桿之間的空間關系。利用D-H法對農業采摘機器人運動協作控制系統進行優化設計。利用D-H法構建農業采摘機器人運動協作數學模型，利用傳感器設備實時量化機器人的位姿信息，并通過模擬采摘流程，分配機器人的運動協作任務。從位置和姿態等多個方面，確定機器人的運動協作控制目標，通過受力分析來求解機器人實際作用力，計算控制量，實現農業采摘機器人的運動協作控制功能。

1 農業采摘機器人運動協作控制硬件系統設計

1.1 農業采摘機器人傳感器

裝設運動傳感器的目的是獲取農業采摘機器人的實時運動信息，為運動協作控制量的計算提供初始數據。

1.1.1 位置傳感器

優化設計的位置傳感器采用光電轉換器作定位器件，只提供高、低兩種信號，使用的振蕩器是由高速CMOS電路74HC00組成，光電轉換器則選擇3 mm直徑的紅外發射接收二極。優化設計位置傳感器的實物圖及工作電路如圖1所示。

圖1 位置傳感器

1.1.2 碰撞傳感器

為了避免農業采摘機器人末端在執行采摘作業過程中被障礙物阻礙，在終端操作機構的前端裝設了一個碰撞傳感器來感知障礙。利用FSR-408型力敏電阻器制造用于機器人避障的碰撞傳感器，并將FSR-408型多個碰撞傳感器裝在機械手前臂上，實現對作業中遇到的障礙物進行實時檢測。在采摘機器人的操作中，主要是對大臂和小臂進行操作，以完成對作物的采集[5]。通過碰撞傳感器的安裝，可以在運動時檢測到障礙物。同時，對各方向的障礙物進行實時探測，若有障礙物出現，則按該方向的感應器進行探測，并采取避障措施。在信號獲取過程中，每個傳感器都要進行編碼，以便于在程序設計中進行信號的處理。優化設計碰撞傳感器的特點是自動復位，高重復精度，扭矩大。

在實際的裝設過程中，將位置傳感器安裝在農業采摘機器人的中心位置以及各個關節位置，而碰撞傳感器主要安裝在農業采摘機器人操作元件末端。

1.2 農業采摘機器人運動協作控制器

運動協作控制器是系統控制功能運行的主要部件，控制器選用LPC11U6x作為核心部件，能根據定位盤的坐標進行控制。運動協作控制器的實物圖及內部組成結構如圖2所示。

圖2 協作控制器內部組成框圖

從圖2中可以看出，優化設計的運動協作控制器采用“陀螺儀+編碼器”模式，產生PWM信號作為電機驅動電路的基極信號，通過對4個FETIRF3205構成H橋電路。信號反饋部分由電流反饋和電壓反饋兩個部分組成，其中電流反饋電路主要包括運算放大器、電壓比較器和AD模塊。通過采樣電阻，將電動機的電流直接輸入到dsPIC30F4012的AD模塊中，并將其作為電流回饋[6]。電壓反饋的基本原理是：當電樞電流超出設定值時，比較器會輸出一個低電平，并在此過程中，會發出一種提示系統出現故障的中斷信號。電壓電路具有電流檢測、電流限制及過流保護等特點。

1.3 農業采摘機器人運動驅動器

在運動控制系統中，必須以作業要求和生產環境為前提，以價格和技術水平作為評判依據，進行驅動器設備的選擇。由于采摘作業一般都是野外作業，安裝液壓、氣壓等都比較困難，而且會給步行系統帶來負擔，所以采取了電動驅動方式[7]。優化設計控制系統中選擇EC45150W直流電機，該電機屬于永磁同步電機。根據控制信號與驅動信號的作用原理，實現運動協作控制器與驅動器之間的連接。

1.4 農業采摘機器人運動協作通信模塊

系統通信模塊設計的目的是為控制指令的傳輸提供硬件支持，同時也為實時傳感數據以及機器人之間的信息傳輸提供通信環境[8]。優化設計控制系統中布設的通信環境支持同步傳輸和異步傳輸兩種方式，同步模式是一種連續傳送多個字符的串口通訊方法，在通訊過程中，收發端的字節和比特之間存在著嚴格的時序關系，而發送和接收信息的兩方則依靠同步信號來實現接收和接收的同步。在傳送過程中，每一個字節所占用的時間間隔都是相等的，不僅字節間沒有間隙，而且字節必須是連續的，當沒有數據需要傳送時，發送者要將剩下的時間用一個字節同步序列來填補。在多主機模式中，接收端的位同步信號是由發送者來提供的，而組同步信號則是通過接收端在完成了位同步之后捕獲的。在主從模式下，通過主控制器生成位同步和組同步[9]。異步通訊沒有嚴格的時序關系，而且沒有固定的時序。字節是逐幀傳輸，每個幀的傳輸都是從開始位到終止位。接收端通過檢測起始位和終止位來與發送者進行同步。除了路由器以及普通通信節點外，將優化的傳感器接入到通信網絡中，作為移動通信節點。

2 農業采摘機器人運動協作控制系統軟件功能設計

在控制硬件系統的支持下，利用D-H法構建農業采摘機器人的數學模型，并對采摘機器人的運動狀態進行求解。根據機器人的采摘任務，將采摘任務分配給多個機器人，通過對農業采摘機器人采摘流程的模擬，確定每個機器人實際的運動控制目標[10]。根據機器人的采摘目標，計算機器人的實際作用力，利用傳感器設備采集采摘機器人的實時運動數據，通過當前運動數據與控制目標數據的對比，計算得出機器人的運動協作控制量，在控制指令的作用下，完成系統的控制功能。

2.1 基于D-H法的農業采摘機器人數學模型構建

利用D-H法得出農業采摘機器人中任意兩個關節之間的連接模型如圖3所示。

圖3 農業采摘機器人關節連桿D-H模型表示法

按照圖3表示的D-H法得出機器人中所有結構的模型構建結構[11]，按照機器人的組成結構，實現多個組成部件的連接。D-H法支持下，得出農業采摘機器人末端執行器位姿為：

(1)

式中，(ψx，ψx，ψz)、(λx，λy，λz)和(δx，δy，δz)分別對應的是法線向量、方位向量和接近向量，(px，py，pz)表示采摘機器人末端執行器在D-H基準坐標系中的位置坐標[12]。由此可以得出任意兩個組成元素之間的變換矩陣為：

(2)

式中，θij和?ij分別為關節轉角和扭角，Lij為關節之間的連桿長度，χij表示關節偏距。同理可以得出所有連接機構之間的變換結構，并由此反映出農業采摘機器人的正向運動機理[13]。在此基礎上，通過對機器人模型的逆運動學求解，即利用矩陣的逆對公式(2)進行多種變形，即可得出機器人中各個關節角的求解結果。另外，假設農業采摘機器人的旋轉中心與幾何中心重合，則其動力學模型可以量化描述為：

M(p)a+Q(p)v+Ff(p)+G(p)=c

(3)

式中，M(p)、Q(p)、Ff(p)和G(p)對應的是慣性矩陣、哥氏力與離心力作用合力、摩擦力以及重力，c為機器人的控制輸入，該變量包括位置控制分量和角度控制分量兩個部分[14]。將運動學求解結果以及動力學方程融合到機器人連接結構中，實現農業采摘機器人數學模型的構建。

2.2 農業采摘機器人實時運動位姿描述

在構建的農業采摘機器人數學模型下，利用硬件系統中的傳感器設備實現對運動位姿的實時采集，并以量化形式輸出。

(4)

式中，(x(t)，y(t)，z(t))表示機器人運動實時位置，(x0，y0，z0)表示機器人的初始位置，φ為姿態角，L為機器人的尺寸長度，t為時刻，v0和vvehicle分別為機器人的初始移動速度和當前移動速度。在實際位姿數據采集過程中，需要根據傳感器的工作頻率對機器人的位姿數據進行更新，更新結果如下：

b(t+1)=b(t)+κgainR

(5)

式中，b(t)和b(t+1)分別為t和t+1時刻的位姿數據，可分為位置數據和姿態角數據兩個部分，R為位姿觀測的協方差矩陣，κgain為卡爾曼增益參數[15]。按照上述流程完成農業采摘機器人中所有組成構件的實時運動位姿數據采集結果。

2.3 農業采摘機器人運動協作任務分配

根據農業采摘機器人的基本組成結構，農作物的采摘任務采用主-從作業模式，即主作業手用來采集農作物目標，而從作業手主要用來切斷農作物果實與樹枝之間的連接，根據作業手的工作內容，對機器人的運動任務進行劃分，分配給主、從作業手，并消解作業手運動過程中可能存在的沖突現象[16]。假設輸入到農業采摘機器人中的運動作業為U，則分配給主、從作業手的運動任務可以分別表示為：

(6)

式中，Umain、Ufrom分別為分配給主、從作業手的運動任務，d()為距離求解函數，Pmain，0和Pfrom，0分別表示機器人上主、從作業手的初始位置，Pcrops為采摘農作物目標的位置信息，Pbreakpoint為采摘過程中農作物的切斷位置[17]。農業采摘機器人運動協作分配任務的約束條件為：

Pmain(t)-Pfrom(t)≠0

(7)

公式(5)表示的是農業采摘機器人作業過程中，任意時刻主作業手和從作業手的位置均不重合，由此實現機器人運動沖突的消解。

2.4 農業采摘機器人運動協作控制目標確定

充分考慮農業采摘機器人作業任務，通過機器人采摘作業流程的模擬，確定各個機械手以及機械手之間的控制目標。圖4表示的是農業采摘機器人的采摘作業流程。

圖4 農業采摘機器人作業流程圖

在工作狀態下的農業采摘機器人首先利用內置的視覺元件收集周圍的環境信息，通過對圖像特征的提取與匹配，確定當前環境中是否存在采摘對象，并確定農作物采集目標的實際位置，標記為(xcrops，ycrops，zcrops)，則采集目標位置即為主作業手的控制目標，而從作業手的控制目標可以表示為：

(8)

式中，(xbreakpoint，ybreakpoint，zbreakpoint)表示從作業手的控制目標位置，變量lcrops、wcrops和hcrops分別表示采摘目標的長度、寬度和高度，上述變量可通過視覺圖像識別直接得出[18]。在已知起始節點與目標節點的情況下，得出主、從作業手的移動軌跡為：

(9)

將相關系數代入到公式(7)中，即可得出主作業手運動軌跡Ymain和從作業手運動軌跡Ybreakpoint的初始規劃結果。為避免機器人在采摘過程中出現的碰撞現象，需要對運動軌跡中的障礙物進行識別，若初始規劃軌跡中存在障礙物，則需要對該軌跡點進行移動處理，并重新連接移動軌跡點和原軌跡中的前后軌跡點，最終通過對規劃軌跡的平滑處理，將得出的軌跡處理結果作為農業采摘機器人運動協作的控制目標。通過上述算法模型，實現了農業采摘機器人運動協作控制系統軟件的運行，農業采摘機器人運動協作控制系統界面圖如圖5所示。

圖5 農業采摘機器人運動協作控制系統界面圖

2.5 農業采摘機器人作用力求解

當機器人協作采摘一個農作物時，機器人的兩個作業手與被采摘物體形成一個閉環，具體的受力情況如圖6所示。

圖6 機器人協作采摘對象受力關系圖

機器人主從作業手在采摘對象上施加的力Fresultant和力矩ζresultant可以分別表示為：

(10)

式中，Fmain和Ffrom分別為主從作業手的實際作用力，ζi為力矩分量，pi為機器人質心相對于機器人末端的位置向量[19]。在實際的求解過程中，作業手實際作用力的計算公式如下：

(11)

式中，mcrop為采摘作物目標的質量，o為作業手與采摘目標中心之間的夾角，κrigidity和rbranch分別為農作物連接樹枝的剛度系數和半徑值。將公式(11)的計算結果代入到公式(10)中，得出農業采摘機器人作用力以及力矩的具體求解結果。

2.6 農業采摘機器人運動協作控制功能實現

結合當前農業采摘機器人各個作業手的實際運動數據與采摘作業的目標數據，利用公式(12)計算出農業采摘機器人的運動協作控制量。

(12)

將實時位姿數據與運動目標求解結果參數代入到公式(12)中，即可得出機器人運動位置、姿態角以及作用力控制量的計算結果。根據控制量的計算結果，利用控制器設備生成控制指令，分別作用在機器人的各個關節上，控制指令的作用邏輯如圖7所示。

圖7 機器人運動協作控制指令作用邏輯圖

在實際控制任務執行過程中，首先執行機器人采摘放置程序初始化，然后驅動主作業手到達待采摘目標下方，張開末端主作業手，從作業手運行至待剪斷新梢位置，主作業手在力矩驅動下運行到收集臺后張開釋放采摘作物，釋放完成抓手復位，完成一次采摘任務后判斷分配的采摘任務是否全部執行完畢，若存在未執行的采摘任務，則繼續循環上述控制步驟，否則跳出循環，農業采摘機器人復位，將采摘完成指令反饋給控制器[20]。

3 系統測試

為了測試優化設計的基于D-H法的農業采摘機器人運動協作控制系統是否達到預期效果，設計仿真實驗。確定農業采摘機器人的運動協作控制目標，并在控制系統的作用下獲取機器人的實際運動數據，通過兩者之間的比對，得出反映系統控制精度功能的測試結果，除此之外，此次系統測試實驗還對機械臂的協作控制能力進行測試，最終從控制精度和協作能力兩個方面，體現出優化設計系統在控制功能方面的優勢。

3.1 配置農業采摘機器人樣機

此次系統測試實驗選擇GSCZ型號的果蔬采摘機器人作為控制對象，該機器人具有信息獲取、成熟度判別等功能，從組成結構方面來看，由機架、履帶移動機構、采摘操作裝置、傳輸機構、雙目視覺機構等部分組成，其中采摘操作部分中，主作業手為果實抓手，從作業手為切斷裝置。農業采摘機器人的外形尺寸為1 200 mm*800 mm*1 800 mm，整機質量為120 kg，機器人的最大移動速度為6.0 m/s，采摘模塊和移動模塊的總功率分別為80 W和120 W，主從作業手的工作空間均能達到800 mm*300 mm。

3.2 選擇機器人采摘對象

選擇哈密瓜、黃瓜、櫻桃和蘋果種植基地作為機器人的采摘作業環境，其中哈密瓜和黃瓜的果實均結在藤上，櫻桃和蘋果均長在樹上，因此上述4種果實的采摘機理相同，其采摘高度在準備機器人樣機的采摘范圍內。選擇果蔬種植基地內部分區域內成熟的果蔬作為采摘對象，在開始實驗之前統計采摘對象的數量，并確定各個對象的生長位置信息。

3.3 生成機器人運動協作控制任務

按照空間順序對采摘對象進行標號，并根據收集的位置信息生成機器人運動協作控制任務，其中部分任務的生成情況如表1所示。

表1 農業采摘機器人運動協作控制任務

根據機器人的位姿初始值，得出機器人的采摘軌跡，從而得出農業采摘機器人任意時刻運動控制目標的生成結果。

3.4 設置系統控制功能測試指標

從控制精度和協作控制能力兩個方面設置量化測試指標，其中控制精度測試指標具體包括：位置控制誤差、位置控制方差、姿態角控制誤差和作用力控制誤差，其中位置控制誤差的數值結果如下：

εp=|xactual-Δx|+|yactual-Δy|+|zactual-Δz|

(13)

式中，(xactual，yactual，zactual)為機器人的實際位置坐標。

位置控制方差的數值結果如下：

(14)

式中，n為獲取的機器人位置個數。

另外姿態角和作用力誤差的測試結果如下：

(15)

式中，φactual、φtarget、Factual和Ftarget分別為姿態角和作用力的實際值與目標值。另外協作控制能力的測試指標為機器人的碰撞次數，該指標的測試結果為：

Ncollision=Ntask+Nexternal

(16)

式中，Ntask和Nexternal分別表示機器人作業手之間的碰撞次數及其與外界環境之間的碰撞次數。最終計算得出控制誤差越小、碰撞次數越少，證明對應系統的控制功能越優。

3.5 系統測試過程與結果分析

根據系統的功能測試內容，將實驗分為控制精度測試和碰撞測試兩個部分，在實驗開始之前，首先需要對農業采摘機器人運動協作控制系統進行開發，并調試系統運行程序與環境。首先將優化設計系統的硬件部分裝設到機器人樣機上，利用編程工具實現系統軟件部分的開發，并導入到機器人內部的運行器件中。隨機生成控制指令，觀察機器人樣機是否能夠按照控制內容完成指定動作，從而完成系統硬件與軟件的調試。為了體現出優化設計系統在控制功能方面的優勢，設置傳統的基于無線網絡的機器人控制系統(文獻[2]方法)和基于模糊PID的機器人控制系統(文獻[3]方法)作為實驗的對比系統，分別標記為對比系統1和對比系統2，按照上述流程完成對比系統的開發與調試工作。

3.5.1 控制精度測試

將農業采摘機器人樣機放置在種植基地環境中，將生成的控制任務逐一輸入到控制系統中，通過數學建模、位姿描述、任務分配、控制量計算等步驟，完成相應的采摘工作。統計機器人作業手的位置數據，通過公式(13)的計算，得出不同系統作用下機器人位置控制誤差及方差的測試結果，如圖8所示。

圖8 農業采摘機器人位置控制誤差測試對比結果

從圖8中可以直觀地看出，與兩個對比系統相比，優化控制系統作用下機器人位置誤差更小，最大值為14 mm。機器人位置的方差也更小，約為9 mm。另外機器人姿態角和作用力控制誤差的測試結果，如表2所示。

表2 機器人姿態角和作用力控制誤差測試數據表

將表2中的數據代入到公式(13)中，綜合主、從作業手兩個部分，得出兩種對比系統下機器人姿態角控制誤差的平均值分別為0.25°和0.15°，平均作用力控制誤差分別為2.13 N和1.0 N，優化設計系統下，機器人姿態角和作用力的平均控制誤差分別為0.04°和0.4 N。

3.5.2 碰撞測試

在農業采摘機器人運動過程中，記錄作業手之間以及作業手與外界環境之間的碰撞次數，直接得出反映系統協作控制能力的測試結果，如表3所示。

表3 系統協作控制能力測試數據表

通過公式(15)的計算，得出3種控制系統作用下，機器人碰撞次數的平均值為5.75次、4.25次和1.25次。

4 結束語

農業采摘機器人是一種在農業機械化生產中非常重要的應用，具有廣闊的發展前景。在此次研究中，利用D-H法對采摘機器人運動協作控制系統進行優化設計，實驗結果表明，所設計基于D-H法的農業采摘機器人運動協作控制系統能夠實現對機器人采摘過程的精準控制，減小控制誤差和機器人碰撞概率，從而降低了機器人采摘失敗的概率，有效提高了農業采摘機器人的工作效率。

采用D-H法進行農業采摘機器人運動協作控制，具有工作效率高、采摘精度高的優點，但在實際應用過程中，對多種農產品的控制參數不同，難以完成多種農產品同時采摘的工作。因此，在未來研究中，將以同時實現不同種類農產品的采摘為目標，進一步完善基于D-H法的農業采摘機器人運動協作控制系統。