智能堆垛叉車三向屬具的運動學建模和驗證

邱全勝，陸華忠,b，趙俊宏，阮清松，呂恩利,b

(華南農業大學 a.工程學院；b.南方農業機械與裝備關鍵技術教育部重點實驗室，廣州 510642)

智能堆垛叉車三向屬具的運動學建模和驗證

邱全勝a，陸華忠a,b，趙俊宏a，阮清松a，呂恩利a,b

(華南農業大學 a.工程學院；b.南方農業機械與裝備關鍵技術教育部重點實驗室，廣州 510642)

為優化智能堆垛叉車的三向屬具的控制方法，對三向屬具進行運動學建模及仿真分析。搭建三向屬具的實驗平臺，基于D-H建模方法，建立了三向屬具的數學模型，運用Robotics Toolbox作為分析工具，通過仿真和實驗開展平臺的正、逆運動學分析及驗證。實驗結果表明：逆運動學結果與正運動學設定的關節變量中垂直方向的移動關節變量誤差為0.1%、水平方向的移動關節變量誤差為1.5%、轉動變量誤差為1.1%，驗證了所建的三向屬具平臺數學模型的正確性。該研究為智能叉車三向屬具準確有效控制提供了參考。

智能堆垛叉車；三向屬具；運動學建模；D-H表示法

0 引言

三向堆垛叉車無需考慮叉車的最小直角堆垛通道距離，可以減少叉車作業所需要的通道寬度，從而提升倉庫的空間利用率，降低倉儲成本[1]。如何實現智能堆垛叉車三向屬具的智能控制，成為國內外研究的重點。

目前文獻表明已成功研發出傳統前向式自動導引叉車，但是未能達到三向堆垛叉車的窄巷道功能。周奇才[1]等人通過在叉車和貨架上安裝傳感器，實現貨叉與貨物的定位，完成存、取貨過程；但此方法需要較多傳感器，實際操作較復雜。研究表明，將D-H方法[2]應用在不同類型的多自由度機械手上[3-12]，建立了機械手模型并實現了機械手的運動學仿真分析。智能堆垛叉車三向屬具是由兩個移動關節和1個旋轉關節組成，搭建了三向屬具的實驗平臺，基于D-H建模方法，建立了三向屬具的數學模型，并運用Robotics Toolbox作為分析工具，對實驗平臺進行運動學分析。仿真分析和平臺驗證結果表明：正、逆運動學結果相互吻合。同時，驗證了建模方法的正確性，為智能叉車三向屬具準確、有效的控制提供了參考。

1 叉車三向屬具運動學建模及驗證

1.1 三向屬具的D-H模型

智能堆垛叉車的三向屬具主要用途是用于倉庫內貨物的搬運和堆垛，在叉車位置固定的情況下，可以叉取左右兩側的貨物。因此，相對于傳統型叉車而言，其所需要的工作空間更小，工作效率較高。三向屬具實物如圖1所示。替換了傳統型叉車的貨叉，其結構是由兩個移動關節和1個轉動關節組成，實現屬具的升降、橫移和旋轉功能，這種結構形式與多自由度機械手的結構形式相似。

圖1 叉車三向屬具實物Fig.1 Three-way fork of forklift

機械手一般有一系列關節和連桿按任意順序連接而成，這些關節的形式可能是滑動(線性)的或旋轉(轉動)的，可能處于不同的平面，且連桿也可以是任意長度的，也可能位于任意的平面上。

D-H模型[13-15]是描述對機械手連桿和關節進行建模的一種方法，可用于任何機械手結構，與機器人的結構順序和復雜程度無關。對于任意一個機械手，需要對其每一個關節指定一個坐標系，坐標系在參考坐標系中可以用3個表示方向的單位向量和一個原點位置向量表示為一個4×4的矩陣；然后，用4×4的齊次變換矩陣來描述相鄰兩連桿的空間關系，確定從一個關節到下一個關節的進行變換，直至最后一個關節的所有變換結合起來，就得到了機械手的總變換矩陣。D-H建模方法就是通過依次變換可最終推導出末端執行器相對于基坐標系的位姿，從而建立機器人的運動學方程。

假設機械手有任意多的連桿和關節以任意形式構成，如圖2所示。圖2中表示了3個順序的關節和兩個連桿，每個關節都是可以轉動或平移的。第1個關節指定為關節n,第2個關節為關節n+1，第3個關節n+2。連桿也是如此，連桿n位于關節n和n+1之間，連桿n+1位于關節n+1和n+2之間，因此關節n+1用來轉動或移動連桿n+1。為了用D-H方法對機械手建模，為每個關節指定一個本地的參考坐標系。對于每個關節，都必須指定一個z軸和x軸，通常并不需要指定y軸，因為y軸總是垂直于x軸和z軸的。

圖2 典型D-H連桿參數的定義[13]Fig.2 Classical parameter definition of D-H linkage [13]

所有關節都用z軸表示。如果關節是旋轉的，z軸位于按右手規則旋轉的方向；如果關節是滑動的，那么z軸是沿直線運動的方向。通常在zn和zn-1軸公垂線方向上定義為xn軸，假如兩相鄰的z軸相交，將垂直于兩條z軸構成的平面的直線定義為x軸。

一般把連桿看作為定義兩個相鄰關節軸的空間關系的剛體，一個連桿的運動可用兩個參數來表示，即長度a和轉角α。關節也是由兩個參數表示的，d表示與關節相連的兩個連桿沿公共軸線方向的距離，θ表示兩相鄰連桿繞公共軸線的夾角[16-17]，通常只有θ和d是關節變量。

將D-H方法應用在三向屬具平臺建模上，為了描述三向屬具相鄰桿件之間的旋轉和移動關系，搭建三向屬具實驗平臺，其三維模型如圖3所示。

圖3 三向屬具平臺三維模型Fig.3 3D model of three-way fork platform

利用D-H方法對三向屬具實驗平臺進行建模，首先為每個關節指定一個本地的參考坐標系。對于每個關節，都必須指定一個z軸和x軸。對于垂直和水平方向的移動關節，z軸分別為沿直線運動方向，沿z軸的連桿長度d是關節變量；對于旋轉關節，z軸位于按右手規則旋轉的方向，繞z軸的旋轉角θ是關節變量。x軸則是按照上述D-H建模方法規定相應的方向，為了計算方便，設定垂直方向移動關節的x軸與本地坐標系的x軸方向相同。因此，可得出三向屬具平臺的連桿坐標系，如圖4所示。

依據D-H參數表示法，可得出叉車三向屬具的D-H參數表，如表1所示。依據連桿參數，利用Robotics Toolbox工具箱建立三向屬具平臺的運動學三維模型，如圖5所示。

坐標系1為垂直方向坐標系，坐標系2為水平方向坐標系，坐標系3為旋轉坐標系。

圖4 三向屬具平臺連桿坐標系

Fig.4 Connecting rod coordinate system of three-way fork platform no-till wheat planter

1.2 三向屬具平臺的運動學分析

三向屬具平臺的運動學就是要建立各運動構件與末端執行器空間的位置、位姿之間的關系。三向屬具平臺可認為是由2個移動關節和1個轉動關節連接起來的開鏈式連桿系統，開鏈的一端固連在垂直基座上，另一端是貨叉，即末端執行器。平臺共有3個自由度，包括2個移動自由度和1個旋轉自由度。其連桿坐標系如圖4所示。

(1)

i取1、2、3，依次可得

(2)

(3)

(4)

由此可以得出三向屬具末端執行器與平臺之間的總變換為

(5)

其中，nx、ny、nz為執行器法向矢量；ox、oy、oz為方向矢量；ax、ay、az為接近矢量；px、py、pz為位置坐標。

由式(1)、～式(5)可得出末端執行器位置坐標與各關節轉角和位移的關系，即

(6)

表1 D-H參數表

圖5 三向屬具平臺運動學三維模型Fig.5 3D kinematics model of three-way fork platform

1.2.1 正運動學分析

對于三向屬具平臺，已知連桿的幾何參數和關節變量，計算平臺末端執行器相對于參考坐標系的位置和姿態稱為正運動學分析。

設定關節變量為(0.04, 0.2, -pi/4)，利用MatLab里的Robotics Toolbox工具箱，編寫三向屬具平臺的正運動學程序[18]，可得末端執行器的位姿為

(7)

式(7)即為各關節按照關節變量變化后得出的末端執行器的位姿。

1.2.2 逆運動學分析

對于三向屬具平臺，已知連桿的幾何參數，給定末端執行器的相對于參考坐標系的位姿，計算三向屬具平臺能夠達到預期位姿的關節變量，稱為逆運動學分析。

設定相對參考系的目標位姿為上述正運動學的結果Tk=[0.7071 -0.7071 0 0.1202;-0.7071 0.07071 0 0.0798;0 0 1 0.04;0 0 0 1] ，利用MatLab里的RoboticsToolbox工具箱，編寫三向屬具平臺的逆運動學程序，可計算出三向屬具平臺達到預期位姿的關節變量為

(8)

與上述關節變量(0.04, 0.2, -pi/4)吻合，因此給予任意一個可以達到期望的位姿(工作空間內)，可以根據平臺的D-H模型準確地計算出各關節變量。由此可以證明所建立的三向屬具平臺的D-H模型是正確的。

1.2.3 正、逆運動學實驗

利用實驗平臺和上述仿真設定的關節變量和對應的末端執行器參考位置的位姿為(0.04, 0.2, -pi/4)和Tk，分別對正、逆運動學分析進行實驗。通過實際測量，正、逆運動學的關節變量和位姿如圖6所示。

圖6 三向屬具平臺試驗Fig.6 Experiment of three-way fork platform

1)正運動：測得三向屬具平臺通過正運動學后末端執行器參考位置的位姿為

(9)

2)逆運動：三向屬具平臺達到設定目標位姿Tk后，測得關節變量為(0.04, 0.197, -0.7767)，與正運動學設定的關節變量中垂直方向的移動關節變量誤差為0.1%、水平方向的移動關節變量誤差為1.5%、轉動變量誤差為1.1%。

實驗結果表明：三向屬具平臺的數學模型正、逆運動學結果相互吻合。由此證明了該平臺的數學模型的正確性。

2 結論與討論

本文搭建了智能叉車三向屬具的實驗平臺，并建立了三向屬具平臺的數學模型。利用MatLab中的RoboticsToolbox工具箱對該模型進行正、逆運動學仿真，最后通三向屬具平臺進行實驗驗證。

實驗結果表明： 實驗逆運動學結果與正運動學設定的關節變量中垂直方向的移動關節變量誤差為0，水平方向的移動關節變量誤差為1.5%，轉動變量誤差為1.1%，驗證了所建的三向屬具平臺數學模型的正確性。該研究可為進一步研究三向屬具的動力學分析及軌跡規劃控制等問題提供了參考。

值得討論的是：本文實驗結果是通過手工測量的，結果難免會有誤差，如何改進測量方式以提高實驗結果精度，本課題組正在進行深入研究。

[1] 周奇才,翁誠霖.巷道堆垛叉車的自動控制[J].起重運輸機械,1993(4):3-6.

[2]CorkePI.ARoboticsToolboxforMatlab[J].IEEERoboticsandAutomationMagazine，1996,3(1):24-32．

[3] 劉鵬,宋濤.焊接機器人運動學分析及軌跡規劃研究[J].機電工程,2013,30(4):390-394

[4] 陶智量.基于MATLAB的6自由度工業機器人仿真研究[D].長春:吉林大學,2009.

[5] 聶曉波,陳亞峰.焊接機器人運動學分析[J].自動化儀表,2013,34(9):67-72.

[6] 扶宇陽,葛阿萍.基于MATLAB的工業機器人運動學仿真研究[J].機械工程與自動化,2013,26(3):40-42.

[7] 陳桂,王建紅,湯玉東.KUKA機器人運動學仿真與實驗研究[J].組合機床與自動化加工技術,2014,22(8):94-97.

[8] 張新敏,朱學軍,趙晨晨,等.基于MATLAB的HP20機器人運動學分析與仿真[J].制造業自動化,2014,36(13):12-15.

[9] 余曉蘭，牛玉會，郝明,等.肖像機器人運動學建模與路徑規劃[J].機械設計與制造,2016,22(1)235-237.

[10]El-Khasawneh,B.S.Thekinematicsandcalibrationofa5-degrees-offreedomhybridserial-parallelkinematicsmanipulator[C]//MechatronicsanditsApplications(ISMA), 2012 8thInternationalSymposiumon.Sharjah(AE):ISMA. 2012.

[11]PengJian-qing,XuWen-fu,WangZhiy-ing.Analyticalinversekinematicsandtrajectoryplanningfora6DOFgrindingrobot[C]//IEEEInternationalConferenceonInformationandAutomation(ICIA).Yinchuan(CN):IEEE,2013.

[12]DaiXiao-lin,HuangQi-tao,JiangHong-zhou.KinematicsAnalysisofa3-dofRotationalParallelMechanism[C]//Modelling,SimulationandOptimization,WMSO, 2008InternationalWorkshopon.HongKong:WMSO,2008.

[13]SaeedB.Niku.機器人學導論—分析、控制及應用[M]. 北京:電子工業出版社，2010:43-60.

[14] 謝斌，蔡自興.基于MatlabRoboticsToolbox的機器人學仿真實驗教學[J].計算機教育,2010,19(10):140-143.

[15] 蔡自興.機器人學[M].北京:清華大學出版社,2009:170-176.

[16]WenShu-hui,MaZhi-yuan,WenShu-huan.ThestudyofNAOrobotarmbasedondirectkinematicsbyusingD-Hmethod[C]//2014 10thUKACCInternationalConferenceonControl.Loughborough(GB):InternationalConferenceonControl,2014.

[17]JDuffy,CCrame.ADisplacementiAnalysisoftheGeneralApatialLink7RMechanism[J].MMT，1980(5)：153-169.

[18] 王永龍，張兆忠，張桂紅.Matlab語言基礎與應用[M].北京: 電子工業出版社，2010:56-84.

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

Kinematics Modeling and Validation for Three-way Fork of Intelligent Stacking Forklift

Qiu Quanshenga, Lu Huazhonga,b, Zhao Junhonga, Ruan Qingsonga, Lv Enlia,b

(a.College of Engineering; b.Key Laboratory of Key Technology on Agricultural Machine and Equipment，Ministry of Education, South China Agricultural University，Guangzhou 510642, China)

Abstract： In order to optimize the control method of three-way fork of intelligent stacking forklift, this article carried out a kinematic model of the three-way fork and did a simulation analysis. Based on the cross slide the experimental platform of the three-way fork was setting up. Three dimensional mathematical model was built by D-H method. The accuracy of model of the three-way fork was verified through the mutual authentication of the forward kinematics and inverse kinematics by the Robotics Toolbox. The results show that prismatic joint of vertical direction error in obverse and inverse kinematic analysis is 0, and prismatic joint of horizontal direction and rotate error respectively are 1.5% and 1.1%, which verifies the correction of the mathematical l model of the three-way fork. This paper provides a reliable reference for the research of controlling three-way fork.

intelligent stacking fork lift truck; three-way fork; kinematics modeling; D-H method

2016-12-01

"十二五"國家科技支撐計劃項目(2015BAD18B0301)；廣東省科技計劃項目(2016B090920092)；現代農業產業技術體系建設專項(CARS-33-13)

邱全勝(1991-),男，山東菏澤人，碩士研究生，(E-mail) qq_sheng@163.com。

呂恩利(1979-)，男，山東德州人，教授，博士生導師，(E-mail) 68273856@qq.com。

S219.89

A

1003-188X(2018)02-0019-05