李詩(shī)榮 毛文華 胡小安

（中國(guó)農(nóng)業(yè)機(jī)械化科學(xué)研究院，北京，100083）

葡萄套袋機(jī)器人機(jī)械臂運(yùn)動(dòng)學(xué)分析與軌跡規(guī)劃仿真

李詩(shī)榮 毛文華 胡小安

（中國(guó)農(nóng)業(yè)機(jī)械化科學(xué)研究院，北京，100083）

本文針對(duì)葡萄套袋機(jī)器人機(jī)械臂，建立基于Denavit-Hartenberg法則的運(yùn)動(dòng)學(xué)模型，對(duì)機(jī)械臂的正運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)進(jìn)行求解與分析，利用MATLAB Robotics Toolbox對(duì)其結(jié)果進(jìn)行仿真，驗(yàn)證了模型的正確性。利用Robotics Toolbox對(duì)機(jī)械臂進(jìn)行了在關(guān)節(jié)空間的軌跡規(guī)劃仿真分析，為葡萄套袋機(jī)器人機(jī)械臂的控制研究提供了基礎(chǔ)。

機(jī)器人，D-H法則，運(yùn)動(dòng)學(xué)，軌跡規(guī)劃，仿真

葡萄是我國(guó)水果生產(chǎn)的重要品種，近十年來(lái)，種植面積和產(chǎn)量一直呈上升趨勢(shì)，并已在世界葡萄產(chǎn)業(yè)中占有一席之地[1]。但是，現(xiàn)階段我國(guó)的葡萄生產(chǎn)作業(yè)仍然以人工為主，其中人工葡萄套袋工作勞動(dòng)強(qiáng)度大、效率低，需要耗費(fèi)大量人力財(cái)力。因此，研發(fā)適合生產(chǎn)實(shí)際的葡萄套袋機(jī)器人具有廣闊應(yīng)用前景。

運(yùn)動(dòng)學(xué)是機(jī)械臂控制的研究基礎(chǔ)，研究方法主要有圖解法，利用矢量分析、矩陣和二元數(shù)等數(shù)學(xué)工具的解析法，Denavit和Hartenberg提出使用標(biāo)準(zhǔn)矩陣表示法來(lái)表達(dá)任意空間機(jī)構(gòu)的運(yùn)動(dòng)方程，該方法經(jīng)過(guò)Paul等人的適當(dāng)修正后被廣泛應(yīng)用于機(jī)器人機(jī)構(gòu)的運(yùn)動(dòng)學(xué)問(wèn)題[2]。

本文基于標(biāo)準(zhǔn)矩陣表示法建立運(yùn)動(dòng)學(xué)模型，對(duì)葡萄套袋機(jī)器人機(jī)械臂的正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)進(jìn)行了求解分析，并且利用基于MATLAB的 Robotics Toolbox機(jī)器人工具箱進(jìn)行了仿真驗(yàn)證和關(guān)節(jié)空間的軌跡規(guī)劃仿真。

1 運(yùn)動(dòng)學(xué)模型

葡萄套袋機(jī)器人機(jī)械臂采用4關(guān)節(jié)自由度，如圖1所示。根據(jù)Denavit-Hartenberg法則（簡(jiǎn)稱(chēng)為D-H法則）建立坐標(biāo)系，得到4個(gè)關(guān)節(jié)的D-H參數(shù)，如表1所示。

圖1 葡萄套袋機(jī)器人機(jī)械臂D-H坐標(biāo)系

表1：葡萄套袋機(jī)器人機(jī)械臂4個(gè)關(guān)節(jié)的D-H參數(shù)

2 正運(yùn)動(dòng)學(xué)求解

機(jī)械臂的末端位姿可以表示為[3]：

機(jī)械臂相鄰兩個(gè)關(guān)節(jié)坐標(biāo)系的齊次變換矩陣的通式為：

則由：

可以得到：

由于（2）式與（3）式相等，可得正運(yùn)動(dòng)學(xué)解為：

3 正運(yùn)動(dòng)學(xué)仿真

使用基于Matlab 平臺(tái)的機(jī)器人工具箱—Robotics Toolbox，可實(shí)現(xiàn)機(jī)械臂運(yùn)動(dòng)學(xué)仿真。此仿真過(guò)程，不僅可以直觀地查看機(jī)器人的運(yùn)動(dòng)情況，還可將所需的數(shù)據(jù)以圖形顯示出來(lái)[4]。仿真步驟如下：

（1）創(chuàng)建名為 fangzhen的m文件，利用 link( )函數(shù)輸入表1所提供的葡萄套袋機(jī)器人機(jī)械臂桿件參數(shù)，link( )函數(shù)調(diào)用格式為：L1=link([pi/2 0 0 375 ],'standard')，L2=link([0 650 0 0],'standard')，L3=link([0 614 0 0],'standard')，L4=link([pi/2 300 0 0],'standard')，前4個(gè)元素依次為α，a，θ，d，最后一個(gè)為'standard'旋轉(zhuǎn)關(guān)節(jié)；

（2）創(chuàng)建名為“4R葡萄套袋機(jī)械臂”，r.name='4R葡萄套袋機(jī)械臂'；

（3）調(diào)用 drivebot(r)命令。運(yùn)行該程序，得到機(jī)器人的三維位姿仿真圖和控制界面圖，如圖2所示。通過(guò)在控制界面圖上輸入各關(guān)節(jié)角度來(lái)驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)，顯示各種運(yùn)動(dòng)位姿。

圖2-(a)機(jī)械臂位姿1仿真與控制界面

圖2-(b)機(jī)械臂位姿2仿真與控制界面

圖2-(c)機(jī)械臂位姿3仿真與控制界面

圖2-(d) 機(jī)械臂位姿4仿真與控制界面

葡萄套袋機(jī)器人機(jī)械臂位姿的仿真結(jié)果表明：如圖2所示，機(jī)械臂的初始位姿仿真圖2-(a)與所建葡萄套袋機(jī)器人機(jī)械臂坐標(biāo)系圖1是一致的。因此亦可判定建立的D-H坐標(biāo)系是正確的。為了進(jìn)一步驗(yàn)證正運(yùn)動(dòng)學(xué)解的正確性，取 3組關(guān)節(jié)變量值帶入正運(yùn)動(dòng)學(xué)方程，得到運(yùn)動(dòng)學(xué)正解數(shù)據(jù)，具體見(jiàn)表2所示。

表2：葡萄套袋機(jī)器人機(jī)械臂正運(yùn)動(dòng)學(xué)解

比較仿真控制界面的數(shù)值（圖2）和正運(yùn)動(dòng)學(xué)方程解的數(shù)值（表2），可知：在關(guān)節(jié)角相同的情況下，正運(yùn)動(dòng)學(xué)方程解和Robotics Toolbox 仿真結(jié)果差值在10-2mm級(jí)別。因此，所建立正運(yùn)動(dòng)學(xué)模型是正確的。

4 逆運(yùn)動(dòng)學(xué)求解

逆運(yùn)動(dòng)學(xué)求解問(wèn)題是將工作空間內(nèi)機(jī)械人末端執(zhí)行器的位姿轉(zhuǎn)化成各關(guān)節(jié)變量。由于逆運(yùn)動(dòng)學(xué)是機(jī)器人運(yùn)動(dòng)規(guī)劃和軌跡控制的基礎(chǔ)，所以相對(duì)正運(yùn)動(dòng)而言顯得更為重要。

對(duì)于串聯(lián)機(jī)器人，正運(yùn)動(dòng)學(xué)的解是唯一的，而逆運(yùn)動(dòng)學(xué)的解是不確定的，可能無(wú)解，也可能有唯一解或多解。

葡萄套袋機(jī)器人機(jī)械臂只有4自由度，其中肩、肘、腕三個(gè)相鄰關(guān)節(jié)軸平行，存在封閉解。因此，采用傳統(tǒng)的解析法來(lái)求解葡萄套袋機(jī)器人機(jī)械臂的逆運(yùn)動(dòng)學(xué)問(wèn)題：在正運(yùn)動(dòng)學(xué)方程兩邊左乘齊次變換逆矩陣，分離出關(guān)節(jié)變量進(jìn)行逐個(gè)求解。這個(gè)解法的特點(diǎn)是計(jì)算速度快、效率高，便于實(shí)時(shí)控制。

由（2）式得相鄰關(guān)節(jié)的逆矩陣為：

由式（6）和式（7）的（3,4）元素對(duì)應(yīng)相等可得：

化簡(jiǎn)得到：

由(6)式和(7)式中的(1,4)和(2,4)項(xiàng)元素分別相等得到：

又因?yàn)樵谔状^(guò)程中，末端執(zhí)行器保持水平狀態(tài)，所以有：

即有：

帶入式（9）得：

將上式兩邊平方相加，化簡(jiǎn)整理可得：

5）驗(yàn)證：

選取4組關(guān)節(jié)角度變量q1、q3、q2、q4輸入到圖2控制面板中得到相應(yīng)的末端位姿，如表3所示。這些關(guān)節(jié)角度的選取有些特殊，應(yīng)當(dāng)滿(mǎn)足q3+q2+q4，因?yàn)檫@條件是本逆解的基礎(chǔ)。這時(shí)，末端執(zhí)行器保持水平，姿態(tài)為然后將位姿代入公式（8）、（14）、（16）、（10）分別求取得到的值與選取的q1、q3、q2、q4的值完全相同，印證了逆運(yùn)動(dòng)學(xué)解的正確性。

表3：運(yùn)動(dòng)學(xué)逆解變量參數(shù)

5 基于Robotics Toolbox的運(yùn)動(dòng)軌跡規(guī)劃

機(jī)器人軌跡規(guī)劃是根據(jù)機(jī)器人要完成的作業(yè)任務(wù)，計(jì)算出各個(gè)關(guān)節(jié)在運(yùn)動(dòng)過(guò)程中期望的位置、速度和加速度[2]。軌跡規(guī)劃主要有兩種方法：關(guān)節(jié)空間軌跡規(guī)劃，笛卡爾空間軌跡規(guī)劃。由于笛卡爾空間規(guī)劃的計(jì)算量很大，并且可能會(huì)出現(xiàn)奇異點(diǎn)無(wú)運(yùn)動(dòng)學(xué)逆解的問(wèn)題；而關(guān)節(jié)空間規(guī)劃直接用運(yùn)動(dòng)時(shí)的受控量進(jìn)行規(guī)劃，計(jì)算簡(jiǎn)單、容易，實(shí)時(shí)性較好，并且不會(huì)出現(xiàn)奇異性問(wèn)題；因此，這里選擇在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃。

關(guān)節(jié)空間法是以關(guān)節(jié)角度的函數(shù)來(lái)描述機(jī)器人的軌跡，在關(guān)節(jié)空間進(jìn)行規(guī)劃時(shí)，需要給定機(jī)械臂在起始點(diǎn)、終止點(diǎn)的形態(tài)、位姿。對(duì)關(guān)節(jié)進(jìn)行插值時(shí)，應(yīng)滿(mǎn)足一系列的約束條件，例如抓取物體時(shí)，手部運(yùn)動(dòng)方向（初始點(diǎn)），提升物體離開(kāi)的方向（提升點(diǎn)），放下物體（下放點(diǎn)）和停止點(diǎn)等節(jié)點(diǎn)上的位姿、速度和加速度的要求；與此相應(yīng)的各個(gè)關(guān)節(jié)位移、速度、加速度在整個(gè)時(shí)間間隔內(nèi)的連續(xù)性要求；其極值必須在各個(gè)關(guān)節(jié)變量的允許范圍之內(nèi)等[2]。

這里采用Robotics Toolbox工具箱的jtraj函數(shù)進(jìn)行關(guān)節(jié)空間插值，調(diào)用格式如下：

[q, qd, qdd]=jtraj(qa, qb ,t);

q是一個(gè)矩陣，其中：每行代表一個(gè)時(shí)間采樣點(diǎn)上各關(guān)節(jié)的轉(zhuǎn)動(dòng)角度，qd和qdd分別是對(duì)應(yīng)的關(guān)節(jié)速度向量以及關(guān)節(jié)加速度向量，qa與qb 分別是初始點(diǎn)位置和終止點(diǎn)位置，t是采樣時(shí)間。

jtraj函數(shù)采用的是7次多項(xiàng)式插值，即：

規(guī)劃的每個(gè)關(guān)節(jié)函數(shù)qi(t)滿(mǎn)足8個(gè)條件：初始點(diǎn)和終止點(diǎn)的位置、速度、加速度要求，提升點(diǎn)和下降點(diǎn)的位置要求，從而可以解出7次多項(xiàng)式參數(shù)。

具體算法的實(shí)現(xiàn)過(guò)程如圖3所示。

圖3 關(guān)節(jié)空間軌跡規(guī)劃算法框圖

對(duì)葡萄套袋機(jī)器人機(jī)械臂進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃仿真，設(shè)：

初始點(diǎn)位置： qa= [0 pi/2 -pi/2 0]，

終止點(diǎn)位置： qb =[-pi/4 pi/6 -pi/2 pi/3]，

采樣時(shí)間t = 0：0.01：5。

正運(yùn)動(dòng)學(xué)求出的初始位置和終止位置的形態(tài)如圖4所示，仿真得到的機(jī)械臂各個(gè)關(guān)節(jié)的角度、角速度、角加速度曲線(xiàn)圖如圖5所示，其中第i行三個(gè)圖從左到右依次是關(guān)節(jié)i的角度、角速度、角加速度曲線(xiàn)圖。由圖5可知：仿真曲線(xiàn)都很光滑，其中第3個(gè)關(guān)節(jié)保持不動(dòng)。

圖4 軌跡規(guī)劃初始位置與目標(biāo)位置

圖5 機(jī)械臂各個(gè)關(guān)節(jié)的角度、角速度、角加速度曲線(xiàn)圖

結(jié)論

針對(duì)葡萄套袋機(jī)器人4自由度機(jī)械臂，本文利用Denavit-Hartenberg法則建立運(yùn)動(dòng)學(xué)模型，對(duì)其正運(yùn)動(dòng)學(xué)進(jìn)行了求解，并且利用MATLAB Robotics Toolbox對(duì)正運(yùn)動(dòng)學(xué)求解的結(jié)果進(jìn)行了仿真驗(yàn)證。其次，根據(jù)機(jī)械臂套袋時(shí)末端執(zhí)行器保持水平的特點(diǎn)，簡(jiǎn)化了逆運(yùn)動(dòng)學(xué)的求解，并利用Robotics Toolbox對(duì)逆運(yùn)動(dòng)學(xué)求解的結(jié)果進(jìn)行了驗(yàn)證。最后，初步探索了機(jī)械臂的軌跡規(guī)劃，利用Robotics Toolbox對(duì)機(jī)械臂進(jìn)行了在關(guān)節(jié)空間的軌跡規(guī)劃仿真，達(dá)到了良好的效果。

[1] 龐俊杰,勾賀明，寧書(shū)臣,等.我國(guó)葡萄生產(chǎn)機(jī)械化現(xiàn)狀及產(chǎn)品開(kāi)發(fā)方向[J].農(nóng)機(jī)質(zhì)量與監(jiān)督,2005(3).

[2] Niku S B．機(jī)器人學(xué)導(dǎo)論一分析、系統(tǒng)及應(yīng)用[M]．孫富春，朱紀(jì)洪，劉國(guó)棟，等，譯．武漢：華中科技大學(xué)出版社，1996：48-67．

[3] 蔡自興. 機(jī)器人學(xué)[M]. 北京：清華大學(xué)出版社, 2000.

[4] 羅家佳, 胡國(guó)清. 基于 MATLAB 的機(jī)器人運(yùn)動(dòng)仿真研究[J]. 廈門(mén)大學(xué)學(xué)報(bào), 2005, 44(5)： 640-644.

[5] 張毅，吳育理，羅元. 智能輪椅上的機(jī)械手運(yùn)動(dòng)學(xué)分析及軌跡規(guī)劃[J]. 計(jì)算機(jī)應(yīng)用研究, 2010,27(12)：4578-4591.

[6] 王仲民，蔡 霞，崔世鋼.一種四自由度機(jī)器人的運(yùn)動(dòng)學(xué)建模[J].天津職業(yè)技術(shù)師范學(xué)院學(xué)報(bào), 2003,13(4.)

[7] Corke P．Manual of Robotics Toolbox for MATLAB (Release 7.1) [EB]．http：//www.cat.csiro.au/cmst/staf f/pic/robot, 2002-04.

本次項(xiàng)目是“十二五”國(guó)家科技支撐計(jì)劃資助項(xiàng)目，項(xiàng)目編號(hào)：2011BAD20B07。