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

(華南農(nóng)業(yè)大學(xué) a.工程學(xué)院；b.南方農(nóng)業(yè)機(jī)械與裝備關(guān)鍵技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，廣州 510642)

智能堆垛叉車三向?qū)倬叩倪\(yùn)動(dòng)學(xué)建模和驗(yàn)證

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

(華南農(nóng)業(yè)大學(xué) a.工程學(xué)院；b.南方農(nóng)業(yè)機(jī)械與裝備關(guān)鍵技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，廣州 510642)

為優(yōu)化智能堆垛叉車的三向?qū)倬叩目刂品椒?，?duì)三向?qū)倬哌M(jìn)行運(yùn)動(dòng)學(xué)建模及仿真分析。搭建三向?qū)倬叩膶?shí)驗(yàn)平臺(tái)，基于D-H建模方法，建立了三向?qū)倬叩臄?shù)學(xué)模型，運(yùn)用Robotics Toolbox作為分析工具，通過(guò)仿真和實(shí)驗(yàn)開(kāi)展平臺(tái)的正、逆運(yùn)動(dòng)學(xué)分析及驗(yàn)證。實(shí)驗(yàn)結(jié)果表明：逆運(yùn)動(dòng)學(xué)結(jié)果與正運(yùn)動(dòng)學(xué)設(shè)定的關(guān)節(jié)變量中垂直方向的移動(dòng)關(guān)節(jié)變量誤差為0.1%、水平方向的移動(dòng)關(guān)節(jié)變量誤差為1.5%、轉(zhuǎn)動(dòng)變量誤差為1.1%，驗(yàn)證了所建的三向?qū)倬咂脚_(tái)數(shù)學(xué)模型的正確性。該研究為智能叉車三向?qū)倬邷?zhǔn)確有效控制提供了參考。

智能堆垛叉車；三向?qū)倬撸贿\(yùn)動(dòng)學(xué)建模；D-H表示法

0 引言

三向堆垛叉車無(wú)需考慮叉車的最小直角堆垛通道距離，可以減少叉車作業(yè)所需要的通道寬度，從而提升倉(cāng)庫(kù)的空間利用率，降低倉(cāng)儲(chǔ)成本[1]。如何實(shí)現(xiàn)智能堆垛叉車三向?qū)倬叩闹悄芸刂?，成為?guó)內(nèi)外研究的重點(diǎn)。

目前文獻(xiàn)表明已成功研發(fā)出傳統(tǒng)前向式自動(dòng)導(dǎo)引叉車，但是未能達(dá)到三向堆垛叉車的窄巷道功能。周奇才[1]等人通過(guò)在叉車和貨架上安裝傳感器，實(shí)現(xiàn)貨叉與貨物的定位，完成存、取貨過(guò)程；但此方法需要較多傳感器，實(shí)際操作較復(fù)雜。研究表明，將D-H方法[2]應(yīng)用在不同類型的多自由度機(jī)械手上[3-12]，建立了機(jī)械手模型并實(shí)現(xiàn)了機(jī)械手的運(yùn)動(dòng)學(xué)仿真分析。智能堆垛叉車三向?qū)倬呤怯蓛蓚€(gè)移動(dòng)關(guān)節(jié)和1個(gè)旋轉(zhuǎn)關(guān)節(jié)組成，搭建了三向?qū)倬叩膶?shí)驗(yàn)平臺(tái)，基于D-H建模方法，建立了三向?qū)倬叩臄?shù)學(xué)模型，并運(yùn)用Robotics Toolbox作為分析工具，對(duì)實(shí)驗(yàn)平臺(tái)進(jìn)行運(yùn)動(dòng)學(xué)分析。仿真分析和平臺(tái)驗(yàn)證結(jié)果表明：正、逆運(yùn)動(dòng)學(xué)結(jié)果相互吻合。同時(shí)，驗(yàn)證了建模方法的正確性，為智能叉車三向?qū)倬邷?zhǔn)確、有效的控制提供了參考。

1 叉車三向?qū)倬哌\(yùn)動(dòng)學(xué)建模及驗(yàn)證

1.1 三向?qū)倬叩腄-H模型

智能堆垛叉車的三向?qū)倬咧饕猛臼怯糜趥}(cāng)庫(kù)內(nèi)貨物的搬運(yùn)和堆垛，在叉車位置固定的情況下，可以叉取左右兩側(cè)的貨物。因此，相對(duì)于傳統(tǒng)型叉車而言，其所需要的工作空間更小，工作效率較高。三向?qū)倬邔?shí)物如圖1所示。替換了傳統(tǒng)型叉車的貨叉，其結(jié)構(gòu)是由兩個(gè)移動(dòng)關(guān)節(jié)和1個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)組成，實(shí)現(xiàn)屬具的升降、橫移和旋轉(zhuǎn)功能，這種結(jié)構(gòu)形式與多自由度機(jī)械手的結(jié)構(gòu)形式相似。

圖1 叉車三向?qū)倬邔?shí)物Fig.1 Three-way fork of forklift

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

D-H模型[13-15]是描述對(duì)機(jī)械手連桿和關(guān)節(jié)進(jìn)行建模的一種方法，可用于任何機(jī)械手結(jié)構(gòu)，與機(jī)器人的結(jié)構(gòu)順序和復(fù)雜程度無(wú)關(guān)。對(duì)于任意一個(gè)機(jī)械手，需要對(duì)其每一個(gè)關(guān)節(jié)指定一個(gè)坐標(biāo)系，坐標(biāo)系在參考坐標(biāo)系中可以用3個(gè)表示方向的單位向量和一個(gè)原點(diǎn)位置向量表示為一個(gè)4×4的矩陣；然后，用4×4的齊次變換矩陣來(lái)描述相鄰兩連桿的空間關(guān)系，確定從一個(gè)關(guān)節(jié)到下一個(gè)關(guān)節(jié)的進(jìn)行變換，直至最后一個(gè)關(guān)節(jié)的所有變換結(jié)合起來(lái)，就得到了機(jī)械手的總變換矩陣。D-H建模方法就是通過(guò)依次變換可最終推導(dǎo)出末端執(zhí)行器相對(duì)于基坐標(biāo)系的位姿，從而建立機(jī)器人的運(yùn)動(dòng)學(xué)方程。

假設(shè)機(jī)械手有任意多的連桿和關(guān)節(jié)以任意形式構(gòu)成，如圖2所示。圖2中表示了3個(gè)順序的關(guān)節(jié)和兩個(gè)連桿，每個(gè)關(guān)節(jié)都是可以轉(zhuǎn)動(dòng)或平移的。第1個(gè)關(guān)節(jié)指定為關(guān)節(jié)n,第2個(gè)關(guān)節(jié)為關(guān)節(jié)n+1，第3個(gè)關(guān)節(jié)n+2。連桿也是如此，連桿n位于關(guān)節(jié)n和n+1之間，連桿n+1位于關(guān)節(jié)n+1和n+2之間，因此關(guān)節(jié)n+1用來(lái)轉(zhuǎn)動(dòng)或移動(dòng)連桿n+1。為了用D-H方法對(duì)機(jī)械手建模，為每個(gè)關(guān)節(jié)指定一個(gè)本地的參考坐標(biāo)系。對(duì)于每個(gè)關(guān)節(jié)，都必須指定一個(gè)z軸和x軸，通常并不需要指定y軸，因?yàn)閥軸總是垂直于x軸和z軸的。

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

所有關(guān)節(jié)都用z軸表示。如果關(guān)節(jié)是旋轉(zhuǎn)的，z軸位于按右手規(guī)則旋轉(zhuǎn)的方向；如果關(guān)節(jié)是滑動(dòng)的，那么z軸是沿直線運(yùn)動(dòng)的方向。通常在zn和zn-1軸公垂線方向上定義為xn軸，假如兩相鄰的z軸相交，將垂直于兩條z軸構(gòu)成的平面的直線定義為x軸。

一般把連桿看作為定義兩個(gè)相鄰關(guān)節(jié)軸的空間關(guān)系的剛體，一個(gè)連桿的運(yùn)動(dòng)可用兩個(gè)參數(shù)來(lái)表示，即長(zhǎng)度a和轉(zhuǎn)角α。關(guān)節(jié)也是由兩個(gè)參數(shù)表示的，d表示與關(guān)節(jié)相連的兩個(gè)連桿沿公共軸線方向的距離，θ表示兩相鄰連桿繞公共軸線的夾角[16-17]，通常只有θ和d是關(guān)節(jié)變量。

將D-H方法應(yīng)用在三向?qū)倬咂脚_(tái)建模上，為了描述三向?qū)倬呦噜彈U件之間的旋轉(zhuǎn)和移動(dòng)關(guān)系，搭建三向?qū)倬邔?shí)驗(yàn)平臺(tái)，其三維模型如圖3所示。

圖3 三向?qū)倬咂脚_(tái)三維模型Fig.3 3D model of three-way fork platform

利用D-H方法對(duì)三向?qū)倬邔?shí)驗(yàn)平臺(tái)進(jìn)行建模，首先為每個(gè)關(guān)節(jié)指定一個(gè)本地的參考坐標(biāo)系。對(duì)于每個(gè)關(guān)節(jié)，都必須指定一個(gè)z軸和x軸。對(duì)于垂直和水平方向的移動(dòng)關(guān)節(jié)，z軸分別為沿直線運(yùn)動(dòng)方向，沿z軸的連桿長(zhǎng)度d是關(guān)節(jié)變量；對(duì)于旋轉(zhuǎn)關(guān)節(jié)，z軸位于按右手規(guī)則旋轉(zhuǎn)的方向，繞z軸的旋轉(zhuǎn)角θ是關(guān)節(jié)變量。x軸則是按照上述D-H建模方法規(guī)定相應(yīng)的方向，為了計(jì)算方便，設(shè)定垂直方向移動(dòng)關(guān)節(jié)的x軸與本地坐標(biāo)系的x軸方向相同。因此，可得出三向?qū)倬咂脚_(tái)的連桿坐標(biāo)系，如圖4所示。

依據(jù)D-H參數(shù)表示法，可得出叉車三向?qū)倬叩腄-H參數(shù)表，如表1所示。依據(jù)連桿參數(shù)，利用Robotics Toolbox工具箱建立三向?qū)倬咂脚_(tái)的運(yùn)動(dòng)學(xué)三維模型，如圖5所示。

坐標(biāo)系1為垂直方向坐標(biāo)系，坐標(biāo)系2為水平方向坐標(biāo)系，坐標(biāo)系3為旋轉(zhuǎn)坐標(biāo)系。

圖4 三向?qū)倬咂脚_(tái)連桿坐標(biāo)系

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

1.2 三向?qū)倬咂脚_(tái)的運(yùn)動(dòng)學(xué)分析

三向?qū)倬咂脚_(tái)的運(yùn)動(dòng)學(xué)就是要建立各運(yùn)動(dòng)構(gòu)件與末端執(zhí)行器空間的位置、位姿之間的關(guān)系。三向?qū)倬咂脚_(tái)可認(rèn)為是由2個(gè)移動(dòng)關(guān)節(jié)和1個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)連接起來(lái)的開(kāi)鏈?zhǔn)竭B桿系統(tǒng)，開(kāi)鏈的一端固連在垂直基座上，另一端是貨叉，即末端執(zhí)行器。平臺(tái)共有3個(gè)自由度，包括2個(gè)移動(dòng)自由度和1個(gè)旋轉(zhuǎn)自由度。其連桿坐標(biāo)系如圖4所示。

(1)

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

(2)

(3)

(4)

由此可以得出三向?qū)倬吣┒藞?zhí)行器與平臺(tái)之間的總變換為

(5)

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

由式(1)、～式(5)可得出末端執(zhí)行器位置坐標(biāo)與各關(guān)節(jié)轉(zhuǎn)角和位移的關(guān)系，即

(6)

表1 D-H參數(shù)表

圖5 三向?qū)倬咂脚_(tái)運(yùn)動(dòng)學(xué)三維模型Fig.5 3D kinematics model of three-way fork platform

1.2.1 正運(yùn)動(dòng)學(xué)分析

對(duì)于三向?qū)倬咂脚_(tái)，已知連桿的幾何參數(shù)和關(guān)節(jié)變量，計(jì)算平臺(tái)末端執(zhí)行器相對(duì)于參考坐標(biāo)系的位置和姿態(tài)稱為正運(yùn)動(dòng)學(xué)分析。

設(shè)定關(guān)節(jié)變量為(0.04, 0.2, -pi/4)，利用MatLab里的Robotics Toolbox工具箱，編寫(xiě)三向?qū)倬咂脚_(tái)的正運(yùn)動(dòng)學(xué)程序[18]，可得末端執(zhí)行器的位姿為

(7)

式(7)即為各關(guān)節(jié)按照關(guān)節(jié)變量變化后得出的末端執(zhí)行器的位姿。

1.2.2 逆運(yùn)動(dòng)學(xué)分析

對(duì)于三向?qū)倬咂脚_(tái)，已知連桿的幾何參數(shù)，給定末端執(zhí)行器的相對(duì)于參考坐標(biāo)系的位姿，計(jì)算三向?qū)倬咂脚_(tái)能夠達(dá)到預(yù)期位姿的關(guān)節(jié)變量，稱為逆運(yùn)動(dòng)學(xué)分析。

設(shè)定相對(duì)參考系的目標(biāo)位姿為上述正運(yùn)動(dòng)學(xué)的結(jié)果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工具箱，編寫(xiě)三向?qū)倬咂脚_(tái)的逆運(yùn)動(dòng)學(xué)程序，可計(jì)算出三向?qū)倬咂脚_(tái)達(dá)到預(yù)期位姿的關(guān)節(jié)變量為

(8)

與上述關(guān)節(jié)變量(0.04, 0.2, -pi/4)吻合，因此給予任意一個(gè)可以達(dá)到期望的位姿(工作空間內(nèi))，可以根據(jù)平臺(tái)的D-H模型準(zhǔn)確地計(jì)算出各關(guān)節(jié)變量。由此可以證明所建立的三向?qū)倬咂脚_(tái)的D-H模型是正確的。

1.2.3 正、逆運(yùn)動(dòng)學(xué)實(shí)驗(yàn)

利用實(shí)驗(yàn)平臺(tái)和上述仿真設(shè)定的關(guān)節(jié)變量和對(duì)應(yīng)的末端執(zhí)行器參考位置的位姿為(0.04, 0.2, -pi/4)和Tk，分別對(duì)正、逆運(yùn)動(dòng)學(xué)分析進(jìn)行實(shí)驗(yàn)。通過(guò)實(shí)際測(cè)量，正、逆運(yùn)動(dòng)學(xué)的關(guān)節(jié)變量和位姿如圖6所示。

圖6 三向?qū)倬咂脚_(tái)試驗(yàn)Fig.6 Experiment of three-way fork platform

1)正運(yùn)動(dòng)：測(cè)得三向?qū)倬咂脚_(tái)通過(guò)正運(yùn)動(dòng)學(xué)后末端執(zhí)行器參考位置的位姿為

(9)

2)逆運(yùn)動(dòng)：三向?qū)倬咂脚_(tái)達(dá)到設(shè)定目標(biāo)位姿Tk后，測(cè)得關(guān)節(jié)變量為(0.04, 0.197, -0.7767)，與正運(yùn)動(dòng)學(xué)設(shè)定的關(guān)節(jié)變量中垂直方向的移動(dòng)關(guān)節(jié)變量誤差為0.1%、水平方向的移動(dòng)關(guān)節(jié)變量誤差為1.5%、轉(zhuǎn)動(dòng)變量誤差為1.1%。

實(shí)驗(yàn)結(jié)果表明：三向?qū)倬咂脚_(tái)的數(shù)學(xué)模型正、逆運(yùn)動(dòng)學(xué)結(jié)果相互吻合。由此證明了該平臺(tái)的數(shù)學(xué)模型的正確性。

2 結(jié)論與討論

本文搭建了智能叉車三向?qū)倬叩膶?shí)驗(yàn)平臺(tái)，并建立了三向?qū)倬咂脚_(tái)的數(shù)學(xué)模型。利用MatLab中的RoboticsToolbox工具箱對(duì)該模型進(jìn)行正、逆運(yùn)動(dòng)學(xué)仿真，最后通三向?qū)倬咂脚_(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。

實(shí)驗(yàn)結(jié)果表明： 實(shí)驗(yàn)?zāi)孢\(yùn)動(dòng)學(xué)結(jié)果與正運(yùn)動(dòng)學(xué)設(shè)定的關(guān)節(jié)變量中垂直方向的移動(dòng)關(guān)節(jié)變量誤差為0，水平方向的移動(dòng)關(guān)節(jié)變量誤差為1.5%，轉(zhuǎn)動(dòng)變量誤差為1.1%，驗(yàn)證了所建的三向?qū)倬咂脚_(tái)數(shù)學(xué)模型的正確性。該研究可為進(jìn)一步研究三向?qū)倬叩膭?dòng)力學(xué)分析及軌跡規(guī)劃控制等問(wèn)題提供了參考。

值得討論的是：本文實(shí)驗(yàn)結(jié)果是通過(guò)手工測(cè)量的，結(jié)果難免會(huì)有誤差，如何改進(jìn)測(cè)量方式以提高實(shí)驗(yàn)結(jié)果精度，本課題組正在進(jìn)行深入研究。

[1] 周奇才,翁誠(chéng)霖.巷道堆垛叉車的自動(dòng)控制[J].起重運(yùn)輸機(jī)械,1993(4):3-6.

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

[3] 劉鵬,宋濤.焊接機(jī)器人運(yùn)動(dòng)學(xué)分析及軌跡規(guī)劃研究[J].機(jī)電工程,2013,30(4):390-394

[4] 陶智量.基于MATLAB的6自由度工業(yè)機(jī)器人仿真研究[D].長(zhǎng)春:吉林大學(xué),2009.

[5] 聶曉波,陳亞峰.焊接機(jī)器人運(yùn)動(dòng)學(xué)分析[J].自動(dòng)化儀表,2013,34(9):67-72.

[6] 扶宇陽(yáng),葛阿萍.基于MATLAB的工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)仿真研究[J].機(jī)械工程與自動(dòng)化,2013,26(3):40-42.

[7] 陳桂,王建紅,湯玉東.KUKA機(jī)器人運(yùn)動(dòng)學(xué)仿真與實(shí)驗(yàn)研究[J].組合機(jī)床與自動(dòng)化加工技術(shù),2014,22(8):94-97.

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

[9] 余曉蘭，牛玉會(huì)，郝明,等.肖像機(jī)器人運(yùn)動(dòng)學(xué)建模與路徑規(guī)劃[J].機(jī)械設(shè)計(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.機(jī)器人學(xué)導(dǎo)論—分析、控制及應(yīng)用[M]. 北京:電子工業(yè)出版社，2010:43-60.

[14] 謝斌，蔡自興.基于MatlabRoboticsToolbox的機(jī)器人學(xué)仿真實(shí)驗(yàn)教學(xué)[J].計(jì)算機(jī)教育,2010,19(10):140-143.

[15] 蔡自興.機(jī)器人學(xué)[M].北京:清華大學(xué)出版社,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語(yǔ)言基礎(chǔ)與應(yīng)用[M].北京: 電子工業(yè)出版社，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

"十二五"國(guó)家科技支撐計(jì)劃項(xiàng)目(2015BAD18B0301)；廣東省科技計(jì)劃項(xiàng)目(2016B090920092)；現(xiàn)代農(nóng)業(yè)產(chǎn)業(yè)技術(shù)體系建設(shè)專項(xiàng)(CARS-33-13)

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

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

S219.89

A

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