AMR果蔬自動收獲機器人動力學建模與實時控制

曹 東，田富洋，宋占華，閆銀發，李法德，蔡占河

AMR果蔬自動收獲機器人動力學建模與實時控制

曹 東1，田富洋1，宋占華1，閆銀發1，李法德1，蔡占河2

(1.山東農業大學 機械與電子工程學院/山東省園藝機械與裝備重點實驗室，山東 泰安 271018；2.菏澤市產品檢驗檢測研究院，山東 菏澤 274000)

現代農業的高速發展促使我國農業向精細農業、精準農業的方向深入發展，由此果蔬自動收獲機器人的發展進入快車道。為此，設計研發了帶有6自由度機械臂的果蔬自動收獲機器人AMR1。該機器人為通用型果蔬自動收獲機器人，可改裝各種專用末端執行器以實現專用采摘。為了驗證本機器人結構的合理性與實用性，通過利用SolidWorks軟件進行了實體建模，并建立了相關的運動學、動力學模型，進一步設計了6自由度機械臂的PID-SMC控制算法，實時控制機械臂末端的軌跡跟蹤。通過試驗仿真，驗證了所提出的動力學和控制模型，為果蔬自動收獲機器人AMR1的進一步應用提供了參數數據。

機器人；自動收獲；建模；仿真

0 引言

經濟的高速發展與科技的不斷進步促使農業生產模式的變革與生產新技術的不斷融合，智能農業機械的技術條件日趨成熟，是當今農業機械發展的主流方向。農業機器人的應用與推廣是現代農業向精細農業、精準農業發展的必由之路，而果蔬自動收獲機器人是智能農業機械裝備的典型。果蔬自動收獲機器人是一種高度智能化和自動化的農業機械裝備，集傳感技術、圖像識別技術、自動導航技術、通訊技術、計算機技術及系統集成技術等多種前沿科學技術于一身，代表了機電一體化的最高水平[1]。果蔬自動收獲機器人作為新一代的生產工具，在提高果蔬生產力、改變果蔬收獲模式及降低勞動力成本等方面彰顯出極大的優越性，果蔬自動收獲機器人的應用將帶來農業生產的又一次技術革命。本文設計研發了果蔬自動收獲機器人AMR1，并通過SolidWorks實體建模與ADAMS運動學、動力學仿真驗證AMR1果蔬自動收獲機器人結構的合理性與實用性，為后續的智能控制算法與基于機器視覺的自動收獲程序設計做準備。

1 果蔬自動收獲機器人AMR的建模

1.1 總體結構

本文設計的果蔬自動收獲機器人AMR1整體機構由智能行走裝置與自動收獲裝置構成。智能行走裝置為四輪移動平臺，采用后輪驅動、前輪轉向的結構。自動收獲裝置由果蔬存儲箱與6DOF(6R)采摘收獲機械手構成[2]，整體機構如圖1所示。

1.前輪轉向機構 2.機械手末端夾持器 3.采摘機械臂 4.自動控制室 5.果實存儲廂 6.后輪驅動機構圖1 AMR果蔬自動收獲機器人結構簡圖Fig.1 Sketch of Automatic harvesting robot modeling diagram

1.2 主要工作部件設計

根據調查結果顯示，田壟間距因果蔬品種而異。以黃瓜、茄子、西紅柿、蕓豆為例，壟距一般在300～700mm之間，果蔬高平均在300～1 800mm之間，AMR1自動收獲機器人車輪間距取均值500mm，車體高500mm。其前后輪均采用冗余伺服驅動，即每個車輪獨立懸掛驅動，采用伺服電機配減速機輸出驅動力是為了獲取更高的位置精度。冗余驅動是考慮伺服電機相對步進電機、普通直流電機而言輸出扭矩偏小，雙驅以獲取足夠動力。后驅采用鏈傳動以獲取足夠傳動強度來適應田間作業，前驅與自動收獲機械手的轉向均采取同步帶傳動，以獲取足夠轉向精度，達到自動收獲機器人田壟間自動轉向與機械手精確定位的目的，來提高收獲效率。果蔬存儲廂由果蔬存儲室與自動控制室構成，自動收獲機械手為6DOF型機械手，前3個關節確定機械手的方位，后3個關節模擬腕關節確定自動收獲機械手的姿態。為了簡化D-H參數，提高運動學逆解效率。6個旋轉關節可串聯在同一條直線上而不存在關節偏置，既增大了可操作空間，又節省了機器成本。底部腰關節600mm，可360°轉向；腰關節上部為大臂和小臂，其長度分別為700、400mm；大臂和小臂都可270°轉向。為了模擬腕部關節，實現末端執行器依據果實位置調整最佳姿態高效采摘，由文獻[3]可知：自動收獲機械手運動學逆解封閉,腕部三關節的旋轉軸必須相交于一點，因此在小臂上部依次串聯相互垂直的3個旋轉微關節，旋轉角度度依次為360°、270°和360°。

1.3 工作原理

自動收獲機器人通過獲取車體上部雙目視覺圖像信息確定果實位置，由控制程序根據反饋信息自動計算運動學逆解及相關動力學，如雅克比力距矩陣解算。向自動收獲機械手發送采摘信號，調整每個關節姿態對果實進行精確采摘；然后，調整各控制參數(關節運動角度，伺服電機力矩等)，將果實放置后部的果蔬存儲廂，依次循環采摘收獲；到達田壟邊界依據軌跡規劃實現自動轉向進行下一采摘收獲工作。通過下一階段虛擬樣機在ADAMS中進行相關運動學、動力學仿真，以模擬較為復雜的機械系統運轉，提高系統設計性能、節約時間和成本，同時驗證本設計結構的合理性與高效性。

2 農業機器人操作臂運動學反解算法

將機械臂各關節變量表示為qi(i=1,2,…,qn)，該值由各關節傳感器輸出獲得。根據文獻[1],應用Denavit-Hartenberg方法，可以得到n自由度的農業機器人操作臂運動學方程為[3]

(1)

逆運動學的解法有多種，Pieper解法適用于本機器人，但機器人的強耦合性、強非線性及噪聲等一系列不確定因素給精確的數學模型建立帶來影響。由于BP神經網絡具有較強的非線性映射能力，可以解決操作臂運動學反解問題，因此本機器人選用BP神經網絡去擬合非線性的反解問題。

(2)

其中，Atan2(y,x)是雙變量反正切函數。神經網絡訓練樣本取關節變量qi(i=1,2,…,6)代入公式(1)隨機獲得。BP神經網絡結構簡圖如圖2所示。

神經網絡在訓練時選取2 000組數據，每組數據包括6個輸入矢量和6個輸出矢量；神經網絡結構中輸入矢量為(?,β,γ,px,py,pz)；輸出矢量為(q1,q2,q3,q4,q5,q6)；輸出矢量根據各關節允許轉動范圍隨機取數并組合在一起，輸入矢量根據正運動學公式計算得出。神經網絡選用單隱層，隱層神經元個數為21，訓練誤差曲線圖如圖3所示。

由圖3可以看出：神經網絡經過77次迭代均方根誤差下降到目標誤差0.015以內，5次迭代之后下降趨勢較明顯，耗時短、效率高且繼續進行多次訓練誤差還可以進一步減小。根據實驗結果，該方法基本滿足本農業機器人操作臂控制要求。

圖3 神經網絡訓練誤差曲線圖Fig.3 Error of Neural network training curve

3 農業機器人廣義動力學模型

該農業機器人在摘取、噴霧及施肥等操作過程中，驅動輪需要進行欠驅動達到最優的前進路線，并且機械手需要更加靈活地到達目標位置，需要考慮手臂在運動過程中快速地避開農業作物，因此機器人系統中既含有驅動輪又含有被動輪。機械臂在抓取農產品目標過程中既含有主動鉸又含有被動鉸，該類機器人系統稱為欠驅動農業機器人。

根據文獻[4]，欠驅動農業機器人系統動力學方程可表述為

其中，Maa為主動關節系統的質量矩陣；Mpp為被動關節質量矩陣；Map為耦合質量矩陣。

對公式(3)按照空間算子代數理論對樹型機械多體系統的質量矩陣進行分解，即

可得系統的動力學微分-代數方程組為

(4)

其中

相關算子參數可以參考文獻[5]。

4 PID滑模變結構(PID-SMC)控制設計

根據方程(4)本文控制可以表示為

(5)

xd(0)=x(0)

(6)

式(6)可以說明一個可以執行的期望軌跡線，其初始的位置、速度應該與系統的初始位置、速度相同，這樣避免了一個跟蹤的過渡時間，提高了效率。

跟蹤誤差矢量記為

(7)

狀態空間Rn中的滑動曲面S(x)記為

(8)

其中，λ是正常數，為進一步追蹤減小誤差，將積分部分用于滑動面，描述為以下公式，即

(9)

這種方法的主要目標是讓滑動面s(x,t)斜率趨于0，因此可以選擇公式(5)中的控制u，使在滑動面s(x,t)之外滿足以下關系，即

(10)

其中，ζ是正常數，這就使得s(x,t)恒為0簡化為一階問題。式(10)表達的是：選s2(x,t)作為度量標準到滑動面的平方距離沿著系統的全部軌線減小并且當軌線進入滑面后就會一直停留在該滑面。

如果

(11)

消除倒數項，用到了一個從t=0到t=trch的積分

≤-ζ(trch-0)

(12)

式(12)中，t=trch是堆積到達滑動面的時間，所以假設S(trch=0)定義為

(13)

如果

(14)

如果

Strch=S(0)→error(x-xd)=0

(15)

假設S的定義為

(16)

(17)

假設二階系統的定義為

(18)

(19)

(20)

其中，sgn(s)是符號函數，表達式為

(21)

(22)

通過使用積分控制，可以得到以下等式，即

(23)

用這種方法U的逼近值可以表示為

(24)

綜上所述，多自由度機械臂的滑模變結構控制公式可以表示為

τ=τeq+τdis

(25)

τeq是名義動力學系統參數，可以表示為

(26)

τdis可以表示為

τdis=K·sgn(S)

(27)

代入等式(23)得

τ=τeq+K·sgn(S)

(28)

圖4是本文六自由度機械臂的滑模變結構控制示意圖。由等式(25)～式(28)得該機械臂的滑模變結構控制的表達式為

(29)

圖4 六自由度機械臂的滑模變結構控制示意圖Fig.4 Schematic diagram of sliding mode variable structure control for Six-DOF Robot Manipulator

5 仿真分析

5.1ADAMS仿真

由于ADAMS在實體建模方面的效率與細節處理效能上遠不如SolidWorks/Proe/UG等專業三維設計軟件，因此本文依據上文設計參數在SolidWorks進行實體建模，虛擬樣機如圖5所示。為提高ADAMS的仿真效率，在將三維模型導入ADAMS之前進行必要的零件的合并，精簡后模型如圖6所示。AMR果蔬自動收獲機械手末端軌跡曲線如圖7所示。

圖5 AMR果蔬自動收獲機器人實體模型Fig.5 Entity model of vegetable and fruit automatic harvesting robot AMR

圖6 AMR果蔬自動收獲機器人精簡模型Fig.6 The simplified model of vegetable and fruit automaticharvesting robot AMR

圖7 AMR果蔬自動收獲機械手末端軌跡曲線Fig.7 The end-effector trajectory curve of vegetable and fruitautomatic harvesting robot AMR

模型自檢無干涉，進入后處理模塊觀測各關節角度、角速度、角加速度變化，如圖8～圖10所示。

圖8 AMR果蔬自動收獲機械手6R關節角速度變化圖Fig.8 The joint angular velocity variation of vegetable and fruitautomatic harvesting robot AMR

圖9 AMR果蔬自動收獲機械手6R關節角度變化圖Fig.9 The joint angle variation of vegetable and fruit automaticharvesting robot AMR

圖10 AMR果蔬自動收獲機械手主要關節角加速度變化圖Fig.10 The major joint angular acceleration variation of vegetable andfruit automatic harvesting robot AMR

整個收獲過程中，自動收獲機械手處于欠驅動狀態，即采摘收獲過程中機械手各關節順序動作是為了[8]：一方面，在抓取果實過程中調整最佳位姿，避免觸碰果實對果實造成損傷；另一方面，抓取后回收過程中障礙(避免自動收獲機械手碰撞果實存儲廂等)，順利將果實放入果蔬采集箱內。由圖8和圖9可看出：角度角速度變化平順，而角加速度力矩波動較大局部波幅較大。這是由于各關節順序動作造成的伺服電機在啟動的瞬時需克服阻力矩產生足夠加速度，因此造成波形震動現象的發生[10]。上述變化曲線不僅說明的仿真的正確性，更為伺服電機的選型提供了參數依據，可根據曲線變化的峰值乘以動載荷系數推算本自動收獲機器人各關節所需伺服電機的輸出扭矩。

5.2 控制算法仿真

為驗證本文提出的控制算法的效率和精度，利用如圖11所示機器人，采用滑模變結構控制、PID反饋控制、滑模變結構PID控制對6自由度機械臂末端軌跡進行控制，仿真結果如圖12～圖17所示。試驗結果表明：本文提出的控制算法高效有效，精度較高。

圖 11 農業輪式機器人實物圖Fig.11 Practicality picture of Agricultural Wheeled robot

圖12 PID控制機器人末端軌跡Fig.12 The trajectory tracking of end-effector based on PID control

圖13 PID控制機器人末端軌跡誤差Fig.13 The trajectory tracking error of end-effector based on PID control

圖14 滑模變結構控制機器人末端軌跡Fig.14 The trajectory tracking of end-effector based on SMC control

圖15 滑模變結構控制機器人末端軌跡誤差Fig.15 The trajectory tracking error of end-effector based on SMC control

圖16 滑模變結構-PID控制機器人末端軌跡Fig.16 The trajectory tracking of end-effector based on SMC-PID control

圖17 滑模變結構-PID控制機器人末端軌跡誤差Fig.17 The trajectory tracking error of end-effector based onSMC-PID control

6 結論

本文通過SolidWorks建立果蔬自動收獲機器人的

虛擬樣機，利用BP神經網絡解決運動學反解問題，建立了機器人在工作工程中的動力學模型，并在ADAMS中進行相關運動學、動力學仿真，獲取采摘收獲機械手的各項參數尤其是采摘收獲過程中所需最大力矩。同時，進一步設計了農業機器人末端軌跡的滑模變結構PID控制算法。該研究為樣機伺服電機的選型提供了可靠依據，降低了生產成本，縮短了設計周期，大大降低了特定采摘對象的自動收獲機器人研發周期。通過對收獲機器人機械臂的控制研究，找到較好的控制方案，提高了自動收獲機器人采摘效率。

[1] 常勇,馬書根,王洪光,等.輪式移動機器人運動學建模方法[J].機械工程學報,2010(5):30-36.

[2] 谷侃鋒,趙明揚.輪式移動機器人沙地行駛控制建模與仿真研究[J].系統仿真學報,2008(18):5035-5039.

[3] 熊有倫.機器人技術基礎[M].武漢:華中科技大學出版社,2014.

[4] 田富洋，吳洪濤，趙大旭. 一類柔性宏剛性微空間機器人廣義動力學建模研究[J].宇航學報，2010,31(3):687-694.

[5]SunT,PeiH,PanY,etal.Neuralnetwork-basedslidingmodeadaptivecontrolforrobotmanipulators[J].Neurocomputing, 2011(74):2377-2384.

[6] 王燕,楊慶華,鮑官軍,等.關節型果蔬采摘機械臂優化設計與試驗[J].農業機械學報,2011(7):191-195.

[7]CYChen,THSLi,YCYeh.EP-basedkinematiccontrolandadaptivefuzzysliding-modedynamiccontrolforwheeledmobilerobots[J].InformationSciences,2009，179：180-195.

[8] 梁喜鳳,周濤,王斌銳.果實收獲機器人關節滑模控制系統設計與試驗[J]. 農業機械學報,2016,47(3)：1-7.

[9]RobertS.Twoapproachesforfeedforwardcontrolandoptimaldesignofunderactuatedmultibodysystems[J].MultibodysystemDynmics, 2012,27:75-93.

[10]OscarC,HéctorN,JoséS,etal.AnewapproachfordynamicfuzzylogicparametertuninginAntColonyOptimizationanditsapplicationinfuzzycontrolofamobilerobot[J].AppliedSoftComputing, 2015,28:150-159.

[11]FranciscoG,RossomandoN,CarlosS,etal.AutonomousmobilerobotsnavigationusingRBFneuralcompensator[J].ControlEngineeringPractice,2011,19:215-222.

[12]Nakazonok,Ohnishi,Kinsjoh,etal.VibrationcontrolofloadforrotarycranesystemusingneuralnetworkwithGA-basedtraining[J].ArtificialLifeandRobotics,2008,13:98-101．

ID:1003-188X(2018)02-0007-EA

The Dynamics and Control of AMR Vegetable and Fruit Automatic Harvesting Robot

Cao Dong1, Tian Fuyang1, Song Zhanhua1, Yan Yinfa1, Li Fade1, Cai Zhanhe2

(1.College of Mechanical and Electronic Engineering, Shandong Agricultural University/Shandong Provincial Key Laboratory of Horticultural Machineries and Equipments，Tai’an 271018, China；2.Product Inspection and Research Institute of Heze, Heze 274000,China)

Abstract： The rapid development of modern agriculture promote Chinese Precision Agriculture development deeply. Thus, the fruit and vegetable automatic harvesting robot have already entered quick development stage. This paper designed and developed the fruit and vegetable automatic harvesting robot AMR1. The robot is a universal automatic harvesting robot for fruits and vegetables, it can achieve special picking by various special end effectors in order to verify the practicality of the robot structure. In this paper, we use SolidWorks software to carry on the entity modeling in order to verify Special-purpose of harvesting.We also established the kinematics and dynamics models.The PID-SMC control algorithm of 6 degree of freedom manipulator is designed further to real-time control of the trajectory tracking of robot arm. The simulation results show that the proposed dynamic and control model is reasonable.Data of the simulation reduced production costs and improved manufacturing efficiency.

robot; automatic harvesting; dynamics; modeling; simulation

2016-11-28

國家自然科學基金項目(51205238)

曹 東(1991-),男，山東濟寧人，碩士研究生，(E-mail) sdaucd@126.com。

田富洋(1979-)，男，山東泰安人，副教授，碩士生導師，(E-mail)sdautfy@163.com。

S225;TP242

A

1003-188X(2018)02-0007-07