基于D-H 法的果園作業平臺工作空間分析與試驗

李 釗，樊桂菊 ，梁 昭，牛成強

（1. 山東農業大學機械與電子工程學院，泰安，271018；2. 山東省園藝機械與裝備重點實驗室，泰安，271018；3. 山東省農業裝備智能化工程實驗室，泰安，271018）

0 引 言

中國果園種植面積和水果產量居世界首位，但水果產業屬于勞動密集型產業，尤其剪枝、疏花疏果、采摘等環節人工勞動強度大、效率低[1-3]。近年來，由于農村勞動力的轉移和人工成本的提高，輔助人工作業的果園平臺的研究與應用越來越多[4-6]，但主要集中在結構與功能方面，對其運動學與工作空間的研究比較少。隨著現代化果園種植面積的擴大和人們對農業機械性能要求的提高，農機農藝有機融合成為研究的重點和難點，果園作業平臺工作空間代表了其活動范圍，反映了平臺與果樹種植模式的融合程度，直接影響作業人員的工作舒適度，是其運動靈活性的重要衡量指標[7-8]。因此對果園作業平臺進行運動學與空間分析具有重要的意義。

許多學者開展了農業機械或農業機器人運動學與工作空間分析方面的研究。胡建平等[9]采用幾何法分析其約束方程及邊界方程求解了Delta并聯機構移栽機器人可達工作空間，并以機器尺寸及運動學性能為綜合進行優化；李國利等[10]采用D-H 法建立了蘋果采摘機械手正運動學方程，通過蒙特卡洛方法模擬了機械手工作空間；吳超宇等[11]提出了極坐標變步長迭代搜索法分析了并聯機器人的工作空間，以對空間利用率最大為目標進行了參數優化；陽涵疆等[12]基于旋量理論構建了一種混聯采摘機器人運動學方程；權龍哲等[13]采用D-H 法建立了立體苗盤管理機器人運動學模型，通過幾何圖解法得到了工作空間及約束關系，進而優化機械臂參數。

綜上所述，目前分析機構的工作空間主要有幾何法、解析法和蒙特卡洛方法，幾何法和解析法僅適用于自由度或桿件數目較少的機構工作空間分析[14-15]；蒙特卡洛方法算法比較簡單，適用范圍廣泛，但邊界提取精度依賴于邊界點分布狀況，模擬點多出現在非邊界處，造成點浪費且邊界不清等[16]。

針對以上問題，本文在前期研制的果園作業平臺基礎上，通過D-H 法建立坐標系，推導其運動學模型，采用U形反正弦分布函數對蒙特卡洛方法改進，利用MATALB 得到參考點工作空間點云圖，采用網格化算法分析改進前后的工作空間邊界清晰度，并通過仿真及樣機試驗進行驗證，為原型結構參數優化和智能控制提供理論依據。

1 果園作業平臺結構

1.1 結構簡介

果園作業平臺整體結構包括動力裝置、行走機構、回轉機構、升降機構、調平機構、工作臺和控制系統等[17]，輔助人工完成剪枝、疏花疏果、套袋及果實采摘等環節。為適應果園復雜地形，采用履帶式底盤，其工作原理為：回轉機構使工作臺在果樹行間左右擺動；升降機構通過油缸伸縮帶動工作臺上升或下降，滿足果樹不同高度的操作要求；調平機構主要由調平油缸、調平液壓回路和調平控制系統組成，通過電磁閥控制調平油缸伸縮改變工作臺橫坡和縱坡傾角，實現平臺坡地作業時工作臺保持水平。因此，該平臺既適用于喬砧密植的紡錘形平地果園，也適用于坡度為15°以內的山區果園。

根據上述結構和工作原理，果園作業平臺的一系列動作與串聯機器人相似，回轉機構相當于回轉關節，升降機構相對于立柱的運動構成旋轉關節，工作臺的橫坡調平運動和縱坡調平運動分別構成 2 個旋轉關節，因此該平臺可看作串聯開鏈機構。為便于作業平臺運動學模型求解、工作空間分析和試驗驗證，以縱坡調平油缸和工作臺的連接點作為作業平臺執行端參考點，簡稱平臺參考點。

1.2 自由度計算

機構自由度計算公式[18-19]為

式中F表示機構自由度，n表示零件個數，pl表示平面低副，ph表示平面高副，p'表示虛約束，F'表示局部自由度。

由果園作業平臺結構和工作原理可知，其構件個數n為 11，將履帶式底盤看作移動副，則該平臺的平面低副pl=14（轉動副10 個、移動副4 個），平面高副ph=0，虛約束p’=0，局部自由度F’=0，將以上數據代入式（1），得果園作業平臺的自由度為5。

1.3 果園作業平臺運動學模型

1.3.1 基于D-H 法建立坐標系

D-H 法[20-23]通過α、ɑ、d、θ4 個獨立參數描述相鄰桿件之間的坐標方向和參數，可以有效的確定開鏈式串聯機器人手臂關節參數和關節變量。

為描述果園作業平臺工作臺的運動，對其作如下假設：地面為剛性，各構件為剛體，各油缸質量忽略不計，忽略鉸鏈間隙與摩擦力的影響。根據假設，立柱支承的上鉸接點至底盤為連桿1，立柱為連桿2，兩立柱支承連線中點至立柱為連桿3，橫梁為連桿4，工作臺與橫梁、縱坡調平油缸鉸接點間的桿為連桿5。以立柱到地面的投影為初始坐標系原點O0，如圖1 采用D-H 法建立果園作業平臺連桿坐標系，相應的D-H 參數如表1 所示。

表1 果園作業平臺D-H 參數Table 1 D-H parameter of orchard platform

圖1 果園作業平臺坐標系Fig.1 Coordinate system of orchard platform

1.3.2 正運動學方程

相鄰兩坐標系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，下同。

平臺參考點的正運動學方程為

式中R為姿態矩陣，P為位置矩陣，n，o，a，p均為di與θi的函數，具體如下

2 工作空間分析

2.1 理想工作空間

果園作業平臺的工作空間是指平臺參考點所能達到的所有位置空間點集的集合，表示了其工作范圍，是作業平臺結構設計和優化的重要依據，其理想工作空間與果樹種類、種植模式和操作人員自身因素密切相關。中國水果種植種類繁多，其中蘋果種植面積位居前列，目前種植模式主要有：喬砧稀植、喬砧密植和矮砧密植[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]，各參數取值為：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。

2.2 蒙特卡洛方法改進

蒙特卡洛方法是一種基于大數定理和中心極限定理[30-31]的應用較廣的數值法，通過人為構造合理的隨機概率模型，在機構關節空間內隨機產生關節空間點，利用坐標變換矩陣T逐一映射到工作空間內，得到一定數量的平臺參考點位置隨機點，將這些點以點云圖形式顯示，即可直觀地描述工作空間情況。

一般采用均勻分布模型R(a,b)產生隨機點，其概率密度函數為

式中a、b表示樣本在區間[a,b]服從均勻分布。

因此，果園作業平臺關節空間點為

式中θimax與θimin分別為關節角θi的最大值、最小值。本文關節角取值為包括關節角范圍內所有值，a=0，b=1，下同。

根據表 1 的作業平臺參數，通過蒙特卡洛方法模擬105個點得平臺參考點的工作空間位置點云圖如圖 3a 所示，為更清晰表達工作空間邊界處點云分布情況，截取Z>1 500 mm 部分如圖3b 所示。

圖3 平臺參考點工作空間（均勻分布）Fig.3 Platform reference point workspace（uniform distribution）

由圖 3 可知，平臺參考點的工作空間邊界位置點云稀疏，大多數點分布于非邊界位置，原因在于：1）關節空間邊界取值不理想，模擬105次服從均勻分布R(0,1)的隨機數抽樣，得到隨機數集合，如圖4 所示，以[0, 0.01]和[0.99, 1]為邊界域，其范圍內的分布點占比pbo，由式（8）計算得2.00 %；2）作業平臺正運動學方程為非線性方程，將關節空間取值在映射過程中“拉伸”或者“壓縮”，導致工作空間邊界處點分布較少，非邊界部分點分布集中，造成點浪費、邊界不清晰。因此需要對蒙特卡洛方法進行改進。

式中nbo表示邊界域內分布點個數；N表示總模擬點個數，105個。

圖4 均勻分布的分布模擬Fig.4 Distribution simulation of uniform distribution

根據上述分析，為增加平臺參考點工作空間邊界分布點，得到清晰的工作空間，關節空間取值分布需要邊界處高而中心處低，故采用U 形的反正弦分布，其概率密度函數為

模擬105次服從該分布的隨機數抽樣，如圖5 所示，該隨機數邊界處分布點數明顯增多，以[0, 0.01]和[0.99, 1]為邊界域，由式（8）計算得其范圍內的分布點占比為12.76%，較均勻分布提高了10.76 %。

圖5 反正弦分布的分布模擬Fig.5 Distribution simulation of arcsine distribution

采用改進蒙特卡洛方法得作業平臺參考點的工作空間位置點云圖如圖6，可知其工作空間邊界部分點分布增加，邊界線明顯清晰。

圖6 平臺參考點工作空間（反正弦分布）Fig.6 Platform reference point workspace（arcsine distribution）

2.3 網格化算法

為進一步量化改進后的工作空間邊界清晰度，采用網格化算法[32]分別對改進前后的工作空間點云圖進行分析，具體如下：

1）外包長方體設計：根據工作空間點云圖得到平臺參考點在三維方向的最值，以略大于這些最值設計外包長方體，使得工作空間的所有點都分布在該長方體內。設該長方體在3 個坐標軸方向上的最值分別為xmin、ymin、zmin、xmax、ymax、zmax，則長方體表示為

2）工作空間網格化：將上述長方體沿3 個坐標軸方向按照2Δ的步長間隔，劃分為若干個小立方體，定義為網格化子空間，如圖 7 所示。假設某個子空間中心坐標為(x0,y0,z0)，則子空間頂點坐標為(x0±Δ,y0±Δ,z0±Δ)。

圖7 網格化空間示意圖Fig.7 Schematic diagram of gridded space

3）子空間有序化：基于網格化子空間，沿Z方向按步長2Δ劃分為若干層，然后沿Y方向按步長2Δ劃分為若干行，以“層-行”編號將子空間有序化處理。

4）子空間初篩：為節約計算時間，按“層-行”搜索所有子空間，初步篩除內部不含點的子空間，保留內部含有點的子空間。

5）邊界子空間搜索：對于保留的子空間，仍然按照“層-行”的邏輯搜索，若子空間連續，則每一行的最大值及最小值所在的子空間為邊界子空間；若子空間不連續，則空間內有點而相鄰子空間無點的子空間為邊界子空間。

6）清晰度評價指標：以邊界分布點總個數和最底層和最頂層邊界層面積為衡量清晰度的評價指標。其中邊界分布點總個數指所有邊界子空間的包含點數總和，邊界層面積指第i層邊界點封閉擬合曲線的面積，可分別用式（11）和式（12）表示。

式中nend為邊界分布點總個數；fi(y)、gi(y)為第i層邊界點在XOY平面投影的擬合曲線（4 次多項式擬合）；Si為該封閉擬合曲線的面積，m2；ymin與ymax分別為外包長方體在Y軸的最大值、最小值，mm。

根據算法分析改進前后的平臺參考點工作空間，考慮程序運行速度與計算精度，步長2Δ取10 mm，模擬點105個時邊界分布點總個數分別為 2.85×104和 3.53×104；將改進前后的工作空間在同一邊界層投影如圖8，根據式（12）計算，改進前后最底層和最頂層邊界層面積分別為0.057、0.16 和0.11、0.22 m2。由此可知改進后工作空間邊界分布點個數提高23.74 %，最底層和最頂層的邊界層面積增大180.70 %和102.69 %，表明改進后工作空間邊界清晰度提高，網格化算法有效。

2.4 可達工作空間

根據蒙特卡洛方法原理，通過該方法得到工作空間的解是平臺參考點到達位置的統計參量，是實際位置的近似值，為更加逼近平臺參考點實際位置，采用 2 種方法多次生成工作空間，取其工作空間X、Y、Z方向的平均值，記為平臺參考點的可達工作空間，即

式中Dkx、Dky、Dkz為平臺參考點可達工作空間在X、Y、Z方向的距離，mm；與分別為工作空間點云在X、Y和Z方向取10 次最大值、最小值的平均值，mm。

以105個點模擬出平臺參考點的工作空間位置點，分別采用2 種方法進行10 次，得其3 個方向的坐標最值如表 2 所示，引入空間差異系數r描述可達工作空間與理想工作空間的誤差。

圖8 邊界擬合曲線Fig.8 Boundary fit curve

表2 平臺參考點工作空間坐標最值Table 2 The minimum and the maximum of platform reference point workspace

根據表 2 數據和平臺參考點的理想工作空間計算空間差異系數，改進后平臺參考點X、Y、Z三個方向的空間差異系數分別為2.53 %、9.58 %、4.15 %，較改進前分別降低了6.92 %、8.99 %、2.64 %，表明改進后生成的工作空間更接近理想工作空間。

3 樣機試驗

3.1 試驗設備

基于前期研制的果園作業平臺，進行其運動位置跟蹤與工作空間分析試驗。平臺以柴油機為動力源，主要參數如表3 所示。

試驗采用北京約克科技有限公司的VEO410L型高速攝像機（拍攝分辨率 1 280×720，幀率 1 000 幀/s，曝光時間40μs）跟蹤平臺參考點位置；采用上海直川電子科技有限公司的ZCT230M 傾角儀（精度0.05°，數據通過RS485 連續輸出）測量油缸旋轉角；采用日本Panasonic公司 HG-C1100 激光位移傳感器測量油缸位移量，精度0.01 mm；此外，還包括余姚索普電子科技有限公司的VC-400HSS 高速攝影燈（400 Ws）、杰科斯JK-100F 系列土壤水分儀（分辨率0.1 %）、卷尺、秒表等。

表3 果園作業平臺主要技術參數Table 3 Main parameters of orchard platform

3.2 試驗內容及方法

3.2.1 平臺參考點位置跟蹤試驗

根據《農業機械生產試驗方法》（GB/T 5667—2008）[33]進行平臺參考點位置跟蹤試驗，驗證平臺運動學模型的準確性。由于果園地形復雜和種植模式限制，高速相機不能很好捕捉平臺參考點完整運動，因此在山東農業大學農學實驗站空曠硬質地面上進行試驗，如圖 9 所示為位置跟蹤試驗現場，試驗時間為2019 年10 月。

圖9 位置跟蹤試驗現場Fig.9 Position tracking test site

因樣機液壓回路不能實現多個油缸同時穩定給油，將回轉支承和橫坡調平油缸保持中位，縱坡調平油缸縮短至最小位移，升降油缸由最小行程伸長到最大行程的過程中，通過傾角儀實時測量橫梁旋轉角（θ4）和橫梁與工作臺之間旋轉角（θ5），利用高速攝像機跟蹤平臺參考點，拍攝過程共5 681 幀，以間隔1 136 幀截取圖像，如圖10 所示。

圖10 高速攝像圖像Fig.10 High-speed camera image

3.2.2 平臺參考點工作空間試驗

為獲得平臺參考點在實際果園的工作空間，2020 年6 月在山東農業大學園藝實驗基地果園進行現場試驗。該果園主要種植蘋果樹，喬砧密植，樹齡 8a，樹形為自由紡錘形，株行距為2 m×3 m，平均株高為3.5 m，平均冠徑為2.4 m。試驗時環境溫度為32 ℃，園地內地形較為平整，淺層土壤含水率約為18.7 %。

以D-H 坐標原點為實際坐標原點，調整回轉支承和各油缸狀態，分別測量平臺參考點實際工作空間在X、Y、Z三個方向所能達到的最大值、最小值，如圖 11 所示。

圖11 平臺參考點工作空間極限位置Fig.11 Limit position of platform reference point workspace

3.3 試驗結果與分析

3.3.1 平臺參考點位置分析

根據高速攝像機得到的平臺參考點運動跟蹤圖像和傾角儀測得的角度，得到圖10 中對應各幀圖像中的平臺參考點Y和Z方向坐標值和相應的關節角θ4與θ5，因回轉支承和橫坡調平油缸保持中位，故平臺參考點在X方向上無變化。由樣機參數和試驗條件確定d1=0 mm、θ2=90°、θ3=90°，代入運動學模型得平臺參考點Y和Z方向的計算值，如表4。

表4 各幀對應的平臺參考點Y 和Z 方向坐標實測值與計算值Table 4 Measured and calculated values of Y and Z coordinates of platform reference points corresponding to each frame

由表4 可知，所選幀對應的Y方向和Z方向的最大差值分別為0.8 和5.5 mm。

為描述升降油缸整個伸長過程平臺參考點的運動，利用Tema 軟件導出拍攝圖像的平臺參考點位置數據，以改進后蒙特卡洛方法求得的運動學正解為對照組，采用MATLAB 繪制平臺參考點運動軌跡，如圖12 所示。

圖12 平臺參考點實測值與計算值軌跡Fig.12 Measured value and calculated value track of platform reference point

由圖12 可知，平臺參考點的實測值與計算值基本吻合，當Z為1314.4 mm 時，Y值差值最大為1.2 mm；當Y為763.6 mm 時，Z值差值最大為6.2 mm，表明運動學模型和求解方法正確。

3.3.2 平臺參考點實際工作空間分析

平臺參考點實際工作空間為

式中Dsx、Dsy、Dsz為平臺參考點實際工作空間在X、Y、Z三個方向距離，mm；與分別為實際工作空間在X方向、Y方向和Z方向取10 次最大值、最小值的平均值，mm。

調整回轉支承和各油缸狀態進行10 次試驗，測量平臺參考點實際工作空間在X、Y、Z三個方向的最大值、最小值，并求取最大值、最小值的平均值代入式（15），得：Dsx=2 430.2 mm、Dsy=1 346.0 mm、Dsz=1 533.7 mm。

利用空間差異系數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 %，主要原因在于設計樣機時側重行距和株高，株距考慮的較少，但因Y方向為前進方向，對實際作業影響不大；與可達工作空間的差異較小，X、Y、Z三個方向的空間差異系數r″分別為0.27 %、0.76 %和0.01 %，表明通過改進的蒙特卡洛方法分析工作空間有效。

另外試驗過程中，偶爾會出現運動不連續、參考點出現晃動等現象，分析原因主要有：安裝鉸鏈存在間隙和摩擦；升降速度過快，作業平臺由于慣性產生擺動。

4 結 論

1）本文以前期研制的果園作業平臺為研究對象，以簡化的作業平臺模型建立D-H 坐標系，推導了果園作業平臺平臺參考點的正向運動學方程。

2）利用U 形反正弦分布函數改進了蒙特卡洛方法，采用網格化算法對改進前后生成的工作空間進行分析，改進后最底層與最頂層邊界面積分別增大 180.70 %和102.69 %，與理想工作空間X、Y、Z三個方向的差異系數較改進前分別降低了6.92 %、8.99 %、2.64 %，表明改進后工作空間邊界清晰度明顯提高，更接近理想工作空間。

3）對平臺進行位置跟蹤和實際空間測試：平臺參考點位置的實測值與計算值基本吻合，最大差值僅為6.2 mm，表明運動學模型正確；平臺參考點實際工作空間與理想工作空間差異較大，而與可達工作空間差異較小，與可達工作空間差異系數最大僅為0.76 %，表明改進方法有效，為下一步作業平臺結構和參數優化提供理論基礎。