李 釗,樊桂菊 ,梁 昭,牛成強
(1. 山東農(nóng)業(yè)大學(xué)機械與電子工程學(xué)院,泰安,271018;2. 山東省園藝機械與裝備重點實驗室,泰安,271018;3. 山東省農(nóng)業(yè)裝備智能化工程實驗室,泰安,271018)
中國果園種植面積和水果產(chǎn)量居世界首位,但水果產(chǎn)業(yè)屬于勞動密集型產(chǎn)業(yè),尤其剪枝、疏花疏果、采摘等環(huán)節(jié)人工勞動強度大、效率低[1-3]。近年來,由于農(nóng)村勞動力的轉(zhuǎn)移和人工成本的提高,輔助人工作業(yè)的果園平臺的研究與應(yīng)用越來越多[4-6],但主要集中在結(jié)構(gòu)與功能方面,對其運動學(xué)與工作空間的研究比較少。隨著現(xiàn)代化果園種植面積的擴大和人們對農(nóng)業(yè)機械性能要求的提高,農(nóng)機農(nóng)藝有機融合成為研究的重點和難點,果園作業(yè)平臺工作空間代表了其活動范圍,反映了平臺與果樹種植模式的融合程度,直接影響作業(yè)人員的工作舒適度,是其運動靈活性的重要衡量指標[7-8]。因此對果園作業(yè)平臺進行運動學(xué)與空間分析具有重要的意義。
許多學(xué)者開展了農(nóng)業(yè)機械或農(nóng)業(yè)機器人運動學(xué)與工作空間分析方面的研究。胡建平等[9]采用幾何法分析其約束方程及邊界方程求解了Delta并聯(lián)機構(gòu)移栽機器人可達工作空間,并以機器尺寸及運動學(xué)性能為綜合進行優(yōu)化;李國利等[10]采用D-H 法建立了蘋果采摘機械手正運動學(xué)方程,通過蒙特卡洛方法模擬了機械手工作空間;吳超宇等[11]提出了極坐標變步長迭代搜索法分析了并聯(lián)機器人的工作空間,以對空間利用率最大為目標進行了參數(shù)優(yōu)化;陽涵疆等[12]基于旋量理論構(gòu)建了一種混聯(lián)采摘機器人運動學(xué)方程;權(quán)龍哲等[13]采用D-H 法建立了立體苗盤管理機器人運動學(xué)模型,通過幾何圖解法得到了工作空間及約束關(guān)系,進而優(yōu)化機械臂參數(shù)。
綜上所述,目前分析機構(gòu)的工作空間主要有幾何法、解析法和蒙特卡洛方法,幾何法和解析法僅適用于自由度或桿件數(shù)目較少的機構(gòu)工作空間分析[14-15];蒙特卡洛方法算法比較簡單,適用范圍廣泛,但邊界提取精度依賴于邊界點分布狀況,模擬點多出現(xiàn)在非邊界處,造成點浪費且邊界不清等[16]。
針對以上問題,本文在前期研制的果園作業(yè)平臺基礎(chǔ)上,通過D-H 法建立坐標系,推導(dǎo)其運動學(xué)模型,采用U形反正弦分布函數(shù)對蒙特卡洛方法改進,利用MATALB 得到參考點工作空間點云圖,采用網(wǎng)格化算法分析改進前后的工作空間邊界清晰度,并通過仿真及樣機試驗進行驗證,為原型結(jié)構(gòu)參數(shù)優(yōu)化和智能控制提供理論依據(jù)。
果園作業(yè)平臺整體結(jié)構(gòu)包括動力裝置、行走機構(gòu)、回轉(zhuǎn)機構(gòu)、升降機構(gòu)、調(diào)平機構(gòu)、工作臺和控制系統(tǒng)等[17],輔助人工完成剪枝、疏花疏果、套袋及果實采摘等環(huán)節(jié)。為適應(yīng)果園復(fù)雜地形,采用履帶式底盤,其工作原理為:回轉(zhuǎn)機構(gòu)使工作臺在果樹行間左右擺動;升降機構(gòu)通過油缸伸縮帶動工作臺上升或下降,滿足果樹不同高度的操作要求;調(diào)平機構(gòu)主要由調(diào)平油缸、調(diào)平液壓回路和調(diào)平控制系統(tǒng)組成,通過電磁閥控制調(diào)平油缸伸縮改變工作臺橫坡和縱坡傾角,實現(xiàn)平臺坡地作業(yè)時工作臺保持水平。因此,該平臺既適用于喬砧密植的紡錘形平地果園,也適用于坡度為15°以內(nèi)的山區(qū)果園。
根據(jù)上述結(jié)構(gòu)和工作原理,果園作業(yè)平臺的一系列動作與串聯(lián)機器人相似,回轉(zhuǎn)機構(gòu)相當(dāng)于回轉(zhuǎn)關(guān)節(jié),升降機構(gòu)相對于立柱的運動構(gòu)成旋轉(zhuǎn)關(guān)節(jié),工作臺的橫坡調(diào)平運動和縱坡調(diào)平運動分別構(gòu)成 2 個旋轉(zhuǎn)關(guān)節(jié),因此該平臺可看作串聯(lián)開鏈機構(gòu)。為便于作業(yè)平臺運動學(xué)模型求解、工作空間分析和試驗驗證,以縱坡調(diào)平油缸和工作臺的連接點作為作業(yè)平臺執(zhí)行端參考點,簡稱平臺參考點。
機構(gòu)自由度計算公式[18-19]為
式中F表示機構(gòu)自由度,n表示零件個數(shù),pl表示平面低副,ph表示平面高副,p'表示虛約束,F(xiàn)'表示局部自由度。
由果園作業(yè)平臺結(jié)構(gòu)和工作原理可知,其構(gòu)件個數(shù)n為 11,將履帶式底盤看作移動副,則該平臺的平面低副pl=14(轉(zhuǎn)動副10 個、移動副4 個),平面高副ph=0,虛約束p’=0,局部自由度F’=0,將以上數(shù)據(jù)代入式(1),得果園作業(yè)平臺的自由度為5。
1.3.1 基于D-H 法建立坐標系
D-H 法[20-23]通過α、ɑ、d、θ4 個獨立參數(shù)描述相鄰桿件之間的坐標方向和參數(shù),可以有效的確定開鏈式串聯(lián)機器人手臂關(guān)節(jié)參數(shù)和關(guān)節(jié)變量。
為描述果園作業(yè)平臺工作臺的運動,對其作如下假設(shè):地面為剛性,各構(gòu)件為剛體,各油缸質(zhì)量忽略不計,忽略鉸鏈間隙與摩擦力的影響。根據(jù)假設(shè),立柱支承的上鉸接點至底盤為連桿1,立柱為連桿2,兩立柱支承連線中點至立柱為連桿3,橫梁為連桿4,工作臺與橫梁、縱坡調(diào)平油缸鉸接點間的桿為連桿5。以立柱到地面的投影為初始坐標系原點O0,如圖1 采用D-H 法建立果園作業(yè)平臺連桿坐標系,相應(yīng)的D-H 參數(shù)如表1 所示。
表1 果園作業(yè)平臺D-H 參數(shù)Table 1 D-H parameter of orchard platform
圖1 果園作業(yè)平臺坐標系Fig.1 Coordinate system of orchard platform
1.3.2 正運動學(xué)方程
相鄰兩坐標系i和i-1 之間的位姿變換矩陣Tii-1[24-26]為
式中cθi=cosθi,sθi=sinθi,c iα=cosiα,s iα=siniα,i=1,2 ...6,下同。
平臺參考點的正運動學(xué)方程為
式中R為姿態(tài)矩陣,P為位置矩陣,n,o,a,p均為di與θi的函數(shù),具體如下
果園作業(yè)平臺的工作空間是指平臺參考點所能達到的所有位置空間點集的集合,表示了其工作范圍,是作業(yè)平臺結(jié)構(gòu)設(shè)計和優(yōu)化的重要依據(jù),其理想工作空間與果樹種類、種植模式和操作人員自身因素密切相關(guān)。中國水果種植種類繁多,其中蘋果種植面積位居前列,目前種植模式主要有:喬砧稀植、喬砧密植和矮砧密植[27]。本文以喬砧密植的紡錘形蘋果園為研究對象,其株行距為(3~4)m×(4~5)m,株高 2.8~3.5 m,冠徑1.4~3.0 m[28]。
綜合蘋果園種植模式、平臺通用性和操作人員自身因素,平臺參考點的理想工作空間如圖2 所示。
圖2 平臺參考點理想工作空間示意圖Fig.2 Schematic diagram of orchard platform reference point ideal workspace
平臺參考點的理想工作空間X、Y、Z三個方向的距離Dlx、Dly、Dlz為
式中Ls為冠徑,m;Lr為成人胳膊長度,m;Hr為成人雙臂功能上舉高,m;Lz為靈活長度,m。
由文獻[28],各參數(shù)取值為:D0、L0、Hs分別取最大值 4、5、3.5 m,Ls取最小值 1.4 m;由文獻[29],Lr= 0.6 m,Hr= 2 m,Lz= 0.1 m。代入式(5)可得Dlx= 2.5 m、Dly=1.5 m、Dlz= 1.6 m,平臺參考點理想工作空間大小為2.5 m×1.5 m×1.6 m。
蒙特卡洛方法是一種基于大數(shù)定理和中心極限定理[30-31]的應(yīng)用較廣的數(shù)值法,通過人為構(gòu)造合理的隨機概率模型,在機構(gòu)關(guān)節(jié)空間內(nèi)隨機產(chǎn)生關(guān)節(jié)空間點,利用坐標變換矩陣T逐一映射到工作空間內(nèi),得到一定數(shù)量的平臺參考點位置隨機點,將這些點以點云圖形式顯示,即可直觀地描述工作空間情況。
一般采用均勻分布模型R(a,b)產(chǎn)生隨機點,其概率密度函數(shù)為
式中a、b表示樣本在區(qū)間[a,b]服從均勻分布。
因此,果園作業(yè)平臺關(guān)節(jié)空間點為
式中θimax與θimin分別為關(guān)節(jié)角θi的最大值、最小值。本文關(guān)節(jié)角取值為包括關(guān)節(jié)角范圍內(nèi)所有值,a=0,b=1,下同。
根據(jù)表 1 的作業(yè)平臺參數(shù),通過蒙特卡洛方法模擬105個點得平臺參考點的工作空間位置點云圖如圖 3a 所示,為更清晰表達工作空間邊界處點云分布情況,截取Z>1 500 mm 部分如圖3b 所示。
圖3 平臺參考點工作空間(均勻分布)Fig.3 Platform reference point workspace(uniform distribution)
由圖 3 可知,平臺參考點的工作空間邊界位置點云稀疏,大多數(shù)點分布于非邊界位置,原因在于:1)關(guān)節(jié)空間邊界取值不理想,模擬105次服從均勻分布R(0,1)的隨機數(shù)抽樣,得到隨機數(shù)集合,如圖4 所示,以[0, 0.01]和[0.99, 1]為邊界域,其范圍內(nèi)的分布點占比pbo,由式(8)計算得2.00 %;2)作業(yè)平臺正運動學(xué)方程為非線性方程,將關(guān)節(jié)空間取值在映射過程中“拉伸”或者“壓縮”,導(dǎo)致工作空間邊界處點分布較少,非邊界部分點分布集中,造成點浪費、邊界不清晰。因此需要對蒙特卡洛方法進行改進。
式中nbo表示邊界域內(nèi)分布點個數(shù);N表示總模擬點個數(shù),105個。
圖4 均勻分布的分布模擬Fig.4 Distribution simulation of uniform distribution
根據(jù)上述分析,為增加平臺參考點工作空間邊界分布點,得到清晰的工作空間,關(guān)節(jié)空間取值分布需要邊界處高而中心處低,故采用U 形的反正弦分布,其概率密度函數(shù)為
模擬105次服從該分布的隨機數(shù)抽樣,如圖5 所示,該隨機數(shù)邊界處分布點數(shù)明顯增多,以[0, 0.01]和[0.99, 1]為邊界域,由式(8)計算得其范圍內(nèi)的分布點占比為12.76%,較均勻分布提高了10.76 %。
圖5 反正弦分布的分布模擬Fig.5 Distribution simulation of arcsine distribution
采用改進蒙特卡洛方法得作業(yè)平臺參考點的工作空間位置點云圖如圖6,可知其工作空間邊界部分點分布增加,邊界線明顯清晰。
圖6 平臺參考點工作空間(反正弦分布)Fig.6 Platform reference point workspace(arcsine distribution)
為進一步量化改進后的工作空間邊界清晰度,采用網(wǎng)格化算法[32]分別對改進前后的工作空間點云圖進行分析,具體如下:
1)外包長方體設(shè)計:根據(jù)工作空間點云圖得到平臺參考點在三維方向的最值,以略大于這些最值設(shè)計外包長方體,使得工作空間的所有點都分布在該長方體內(nèi)。設(shè)該長方體在3 個坐標軸方向上的最值分別為xmin、ymin、zmin、xmax、ymax、zmax,則長方體表示為
2)工作空間網(wǎng)格化:將上述長方體沿3 個坐標軸方向按照2Δ的步長間隔,劃分為若干個小立方體,定義為網(wǎng)格化子空間,如圖 7 所示。假設(shè)某個子空間中心坐標為(x0,y0,z0),則子空間頂點坐標為(x0±Δ,y0±Δ,z0±Δ)。
圖7 網(wǎng)格化空間示意圖Fig.7 Schematic diagram of gridded space
3)子空間有序化:基于網(wǎng)格化子空間,沿Z方向按步長2Δ劃分為若干層,然后沿Y方向按步長2Δ劃分為若干行,以“層-行”編號將子空間有序化處理。
4)子空間初篩:為節(jié)約計算時間,按“層-行”搜索所有子空間,初步篩除內(nèi)部不含點的子空間,保留內(nèi)部含有點的子空間。
5)邊界子空間搜索:對于保留的子空間,仍然按照“層-行”的邏輯搜索,若子空間連續(xù),則每一行的最大值及最小值所在的子空間為邊界子空間;若子空間不連續(xù),則空間內(nèi)有點而相鄰子空間無點的子空間為邊界子空間。
6)清晰度評價指標:以邊界分布點總個數(shù)和最底層和最頂層邊界層面積為衡量清晰度的評價指標。其中邊界分布點總個數(shù)指所有邊界子空間的包含點數(shù)總和,邊界層面積指第i層邊界點封閉擬合曲線的面積,可分別用式(11)和式(12)表示。
式中nend為邊界分布點總個數(shù);fi(y)、gi(y)為第i層邊界點在XOY平面投影的擬合曲線(4 次多項式擬合);Si為該封閉擬合曲線的面積,m2;ymin與ymax分別為外包長方體在Y軸的最大值、最小值,mm。
根據(jù)算法分析改進前后的平臺參考點工作空間,考慮程序運行速度與計算精度,步長2Δ取10 mm,模擬點105個時邊界分布點總個數(shù)分別為 2.85×104和 3.53×104;將改進前后的工作空間在同一邊界層投影如圖8,根據(jù)式(12)計算,改進前后最底層和最頂層邊界層面積分別為0.057、0.16 和0.11、0.22 m2。由此可知改進后工作空間邊界分布點個數(shù)提高23.74 %,最底層和最頂層的邊界層面積增大180.70 %和102.69 %,表明改進后工作空間邊界清晰度提高,網(wǎng)格化算法有效。
根據(jù)蒙特卡洛方法原理,通過該方法得到工作空間的解是平臺參考點到達位置的統(tǒng)計參量,是實際位置的近似值,為更加逼近平臺參考點實際位置,采用 2 種方法多次生成工作空間,取其工作空間X、Y、Z方向的平均值,記為平臺參考點的可達工作空間,即
式中Dkx、Dky、Dkz為平臺參考點可達工作空間在X、Y、Z方向的距離,mm;與分別為工作空間點云在X、Y和Z方向取10 次最大值、最小值的平均值,mm。
以105個點模擬出平臺參考點的工作空間位置點,分別采用2 種方法進行10 次,得其3 個方向的坐標最值如表 2 所示,引入空間差異系數(shù)r描述可達工作空間與理想工作空間的誤差。
圖8 邊界擬合曲線Fig.8 Boundary fit curve
表2 平臺參考點工作空間坐標最值Table 2 The minimum and the maximum of platform reference point workspace
根據(jù)表 2 數(shù)據(jù)和平臺參考點的理想工作空間計算空間差異系數(shù),改進后平臺參考點X、Y、Z三個方向的空間差異系數(shù)分別為2.53 %、9.58 %、4.15 %,較改進前分別降低了6.92 %、8.99 %、2.64 %,表明改進后生成的工作空間更接近理想工作空間。
基于前期研制的果園作業(yè)平臺,進行其運動位置跟蹤與工作空間分析試驗。平臺以柴油機為動力源,主要參數(shù)如表3 所示。
試驗采用北京約克科技有限公司的VEO410L型高速攝像機(拍攝分辨率 1 280×720,幀率 1 000 幀/s,曝光時間40μs)跟蹤平臺參考點位置;采用上海直川電子科技有限公司的ZCT230M 傾角儀(精度0.05°,數(shù)據(jù)通過RS485 連續(xù)輸出)測量油缸旋轉(zhuǎn)角;采用日本Panasonic公司 HG-C1100 激光位移傳感器測量油缸位移量,精度0.01 mm;此外,還包括余姚索普電子科技有限公司的VC-400HSS 高速攝影燈(400 Ws)、杰科斯JK-100F 系列土壤水分儀(分辨率0.1 %)、卷尺、秒表等。
表3 果園作業(yè)平臺主要技術(shù)參數(shù)Table 3 Main parameters of orchard platform
3.2.1 平臺參考點位置跟蹤試驗
根據(jù)《農(nóng)業(yè)機械生產(chǎn)試驗方法》(GB/T 5667—2008)[33]進行平臺參考點位置跟蹤試驗,驗證平臺運動學(xué)模型的準確性。由于果園地形復(fù)雜和種植模式限制,高速相機不能很好捕捉平臺參考點完整運動,因此在山東農(nóng)業(yè)大學(xué)農(nóng)學(xué)實驗站空曠硬質(zhì)地面上進行試驗,如圖 9 所示為位置跟蹤試驗現(xiàn)場,試驗時間為2019 年10 月。
圖9 位置跟蹤試驗現(xiàn)場Fig.9 Position tracking test site
因樣機液壓回路不能實現(xiàn)多個油缸同時穩(wěn)定給油,將回轉(zhuǎn)支承和橫坡調(diào)平油缸保持中位,縱坡調(diào)平油缸縮短至最小位移,升降油缸由最小行程伸長到最大行程的過程中,通過傾角儀實時測量橫梁旋轉(zhuǎn)角(θ4)和橫梁與工作臺之間旋轉(zhuǎn)角(θ5),利用高速攝像機跟蹤平臺參考點,拍攝過程共5 681 幀,以間隔1 136 幀截取圖像,如圖10 所示。
圖10 高速攝像圖像Fig.10 High-speed camera image
3.2.2 平臺參考點工作空間試驗
為獲得平臺參考點在實際果園的工作空間,2020 年6 月在山東農(nóng)業(yè)大學(xué)園藝實驗基地果園進行現(xiàn)場試驗。該果園主要種植蘋果樹,喬砧密植,樹齡 8a,樹形為自由紡錘形,株行距為2 m×3 m,平均株高為3.5 m,平均冠徑為2.4 m。試驗時環(huán)境溫度為32 ℃,園地內(nèi)地形較為平整,淺層土壤含水率約為18.7 %。
以D-H 坐標原點為實際坐標原點,調(diào)整回轉(zhuǎn)支承和各油缸狀態(tài),分別測量平臺參考點實際工作空間在X、Y、Z三個方向所能達到的最大值、最小值,如圖 11 所示。
圖11 平臺參考點工作空間極限位置Fig.11 Limit position of platform reference point workspace
3.3.1 平臺參考點位置分析
根據(jù)高速攝像機得到的平臺參考點運動跟蹤圖像和傾角儀測得的角度,得到圖10 中對應(yīng)各幀圖像中的平臺參考點Y和Z方向坐標值和相應(yīng)的關(guān)節(jié)角θ4與θ5,因回轉(zhuǎn)支承和橫坡調(diào)平油缸保持中位,故平臺參考點在X方向上無變化。由樣機參數(shù)和試驗條件確定d1=0 mm、θ2=90°、θ3=90°,代入運動學(xué)模型得平臺參考點Y和Z方向的計算值,如表4。
表4 各幀對應(yīng)的平臺參考點Y 和Z 方向坐標實測值與計算值Table 4 Measured and calculated values of Y and Z coordinates of platform reference points corresponding to each frame
由表4 可知,所選幀對應(yīng)的Y方向和Z方向的最大差值分別為0.8 和5.5 mm。
為描述升降油缸整個伸長過程平臺參考點的運動,利用Tema 軟件導(dǎo)出拍攝圖像的平臺參考點位置數(shù)據(jù),以改進后蒙特卡洛方法求得的運動學(xué)正解為對照組,采用MATLAB 繪制平臺參考點運動軌跡,如圖12 所示。
圖12 平臺參考點實測值與計算值軌跡Fig.12 Measured value and calculated value track of platform reference point
由圖12 可知,平臺參考點的實測值與計算值基本吻合,當(dāng)Z為1314.4 mm 時,Y值差值最大為1.2 mm;當(dāng)Y為763.6 mm 時,Z值差值最大為6.2 mm,表明運動學(xué)模型和求解方法正確。
3.3.2 平臺參考點實際工作空間分析
平臺參考點實際工作空間為
式中Dsx、Dsy、Dsz為平臺參考點實際工作空間在X、Y、Z三個方向距離,mm;與分別為實際工作空間在X方向、Y方向和Z方向取10 次最大值、最小值的平均值,mm。
調(diào)整回轉(zhuǎn)支承和各油缸狀態(tài)進行10 次試驗,測量平臺參考點實際工作空間在X、Y、Z三個方向的最大值、最小值,并求取最大值、最小值的平均值代入式(15),得:Dsx=2 430.2 mm、Dsy=1 346.0 mm、Dsz=1 533.7 mm。
利用空間差異系數(shù)r′=( |Dsj-Dlj| /Dlj)×100%(j=x、y、z,下同)和r″=( |Dsj-Dkj| /Dkj)×100%分別描述實際工作空間與理想工作空間和可達工作空間的誤差,則平臺參考點實際工作空間與理想工作空間和可達工作空間的對比如表5 所示。
表5 實際工作空間與理想工作空間和可達工作空間對比Table 5 Actual workspace compared with ideal workspace and reachable workspace
由表 5 可知,平臺參考點實際工作空間與理想工作空間差異較大,Y方向最大,達到10.27 %,主要原因在于設(shè)計樣機時側(cè)重行距和株高,株距考慮的較少,但因Y方向為前進方向,對實際作業(yè)影響不大;與可達工作空間的差異較小,X、Y、Z三個方向的空間差異系數(shù)r″分別為0.27 %、0.76 %和0.01 %,表明通過改進的蒙特卡洛方法分析工作空間有效。
另外試驗過程中,偶爾會出現(xiàn)運動不連續(xù)、參考點出現(xiàn)晃動等現(xiàn)象,分析原因主要有:安裝鉸鏈存在間隙和摩擦;升降速度過快,作業(yè)平臺由于慣性產(chǎn)生擺動。
1)本文以前期研制的果園作業(yè)平臺為研究對象,以簡化的作業(yè)平臺模型建立D-H 坐標系,推導(dǎo)了果園作業(yè)平臺平臺參考點的正向運動學(xué)方程。
2)利用U 形反正弦分布函數(shù)改進了蒙特卡洛方法,采用網(wǎng)格化算法對改進前后生成的工作空間進行分析,改進后最底層與最頂層邊界面積分別增大 180.70 %和102.69 %,與理想工作空間X、Y、Z三個方向的差異系數(shù)較改進前分別降低了6.92 %、8.99 %、2.64 %,表明改進后工作空間邊界清晰度明顯提高,更接近理想工作空間。
3)對平臺進行位置跟蹤和實際空間測試:平臺參考點位置的實測值與計算值基本吻合,最大差值僅為6.2 mm,表明運動學(xué)模型正確;平臺參考點實際工作空間與理想工作空間差異較大,而與可達工作空間差異較小,與可達工作空間差異系數(shù)最大僅為0.76 %,表明改進方法有效,為下一步作業(yè)平臺結(jié)構(gòu)和參數(shù)優(yōu)化提供理論基礎(chǔ)。