葡萄套袋机器人机械臂运动学分析与轨迹规划仿真

李诗荣 毛文华 胡小安

（中国农业机械化科学研究院，北京，100083）

葡萄套袋机器人机械臂运动学分析与轨迹规划仿真

李诗荣 毛文华 胡小安

（中国农业机械化科学研究院，北京，100083）

本文针对葡萄套袋机器人机械臂，建立基于Denavit-Hartenberg法则的运动学模型，对机械臂的正运动学与逆运动学进行求解与分析，利用MATLAB Robotics Toolbox对其结果进行仿真，验证了模型的正确性。利用Robotics Toolbox对机械臂进行了在关节空间的轨迹规划仿真分析，为葡萄套袋机器人机械臂的控制研究提供了基础。

机器人，D-H法则，运动学，轨迹规划，仿真

葡萄是我国水果生产的重要品种，近十年来，种植面积和产量一直呈上升趋势，并已在世界葡萄产业中占有一席之地[1]。但是，现阶段我国的葡萄生产作业仍然以人工为主，其中人工葡萄套袋工作劳动强度大、效率低，需要耗费大量人力财力。因此，研发适合生产实际的葡萄套袋机器人具有广阔应用前景。

运动学是机械臂控制的研究基础，研究方法主要有图解法，利用矢量分析、矩阵和二元数等数学工具的解析法，Denavit和Hartenberg提出使用标准矩阵表示法来表达任意空间机构的运动方程，该方法经过Paul等人的适当修正后被广泛应用于机器人机构的运动学问题[2]。

本文基于标准矩阵表示法建立运动学模型，对葡萄套袋机器人机械臂的正运动学和逆运动学进行了求解分析，并且利用基于MATLAB的 Robotics Toolbox机器人工具箱进行了仿真验证和关节空间的轨迹规划仿真。

1 运动学模型

葡萄套袋机器人机械臂采用4关节自由度，如图1所示。根据Denavit-Hartenberg法则（简称为D-H法则）建立坐标系，得到4个关节的D-H参数，如表1所示。

图1 葡萄套袋机器人机械臂D-H坐标系

表1：葡萄套袋机器人机械臂4个关节的D-H参数

2 正运动学求解

机械臂的末端位姿可以表示为[3]：

机械臂相邻两个关节坐标系的齐次变换矩阵的通式为：

则由：

可以得到：

由于（2）式与（3）式相等，可得正运动学解为：

3 正运动学仿真

使用基于Matlab 平台的机器人工具箱—Robotics Toolbox，可实现机械臂运动学仿真。此仿真过程，不仅可以直观地查看机器人的运动情况，还可将所需的数据以图形显示出来[4]。仿真步骤如下：

（1）创建名为 fangzhen的m文件，利用 link( )函数输入表1所提供的葡萄套袋机器人机械臂杆件参数，link( )函数调用格式为：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个元素依次为α，a，θ，d，最后一个为'standard'旋转关节；

（2）创建名为“4R葡萄套袋机械臂”，r.name='4R葡萄套袋机械臂'；

（3）调用 drivebot(r)命令。运行该程序，得到机器人的三维位姿仿真图和控制界面图，如图2所示。通过在控制界面图上输入各关节角度来驱动机器人运动，显示各种运动位姿。

图2-(a)机械臂位姿1仿真与控制界面

图2-(b)机械臂位姿2仿真与控制界面

图2-(c)机械臂位姿3仿真与控制界面

图2-(d) 机械臂位姿4仿真与控制界面

葡萄套袋机器人机械臂位姿的仿真结果表明：如图2所示，机械臂的初始位姿仿真图2-(a)与所建葡萄套袋机器人机械臂坐标系图1是一致的。因此亦可判定建立的D-H坐标系是正确的。为了进一步验证正运动学解的正确性，取 3组关节变量值带入正运动学方程，得到运动学正解数据，具体见表2所示。

表2：葡萄套袋机器人机械臂正运动学解

比较仿真控制界面的数值（图2）和正运动学方程解的数值（表2），可知：在关节角相同的情况下，正运动学方程解和Robotics Toolbox 仿真结果差值在10-2mm级别。因此，所建立正运动学模型是正确的。

4 逆运动学求解

逆运动学求解问题是将工作空间内机械人末端执行器的位姿转化成各关节变量。由于逆运动学是机器人运动规划和轨迹控制的基础，所以相对正运动而言显得更为重要。

对于串联机器人，正运动学的解是唯一的，而逆运动学的解是不确定的，可能无解，也可能有唯一解或多解。

葡萄套袋机器人机械臂只有4自由度，其中肩、肘、腕三个相邻关节轴平行，存在封闭解。因此，采用传统的解析法来求解葡萄套袋机器人机械臂的逆运动学问题：在正运动学方程两边左乘齐次变换逆矩阵，分离出关节变量进行逐个求解。这个解法的特点是计算速度快、效率高，便于实时控制。

由（2）式得相邻关节的逆矩阵为：

由式（6）和式（7）的（3,4）元素对应相等可得：

化简得到：

由(6)式和(7)式中的(1,4)和(2,4)项元素分别相等得到：

又因为在套袋过程中，末端执行器保持水平状态，所以有：

即有：

带入式（9）得：

将上式两边平方相加，化简整理可得：

5）验证：

选取4组关节角度变量q1、q3、q2、q4输入到图2控制面板中得到相应的末端位姿，如表3所示。这些关节角度的选取有些特殊，应当满足q3+q2+q4，因为这条件是本逆解的基础。这时，末端执行器保持水平，姿态为然后将位姿代入公式（8）、（14）、（16）、（10）分别求取得到的值与选取的q1、q3、q2、q4的值完全相同，印证了逆运动学解的正确性。

表3：运动学逆解变量参数

5 基于Robotics Toolbox的运动轨迹规划

机器人轨迹规划是根据机器人要完成的作业任务，计算出各个关节在运动过程中期望的位置、速度和加速度[2]。轨迹规划主要有两种方法：关节空间轨迹规划，笛卡尔空间轨迹规划。由于笛卡尔空间规划的计算量很大，并且可能会出现奇异点无运动学逆解的问题；而关节空间规划直接用运动时的受控量进行规划，计算简单、容易，实时性较好，并且不会出现奇异性问题；因此，这里选择在关节空间中进行轨迹规划。

关节空间法是以关节角度的函数来描述机器人的轨迹，在关节空间进行规划时，需要给定机械臂在起始点、终止点的形态、位姿。对关节进行插值时，应满足一系列的约束条件，例如抓取物体时，手部运动方向（初始点），提升物体离开的方向（提升点），放下物体（下放点）和停止点等节点上的位姿、速度和加速度的要求；与此相应的各个关节位移、速度、加速度在整个时间间隔内的连续性要求；其极值必须在各个关节变量的允许范围之内等[2]。

这里采用Robotics Toolbox工具箱的jtraj函数进行关节空间插值，调用格式如下：

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

q是一个矩阵，其中：每行代表一个时间采样点上各关节的转动角度，qd和qdd分别是对应的关节速度向量以及关节加速度向量，qa与qb 分别是初始点位置和终止点位置，t是采样时间。

jtraj函数采用的是7次多项式插值，即：

规划的每个关节函数qi(t)满足8个条件：初始点和终止点的位置、速度、加速度要求，提升点和下降点的位置要求，从而可以解出7次多项式参数。

具体算法的实现过程如图3所示。

图3 关节空间轨迹规划算法框图

对葡萄套袋机器人机械臂进行关节空间轨迹规划仿真，设：

初始点位置： qa= [0 pi/2 -pi/2 0]，

终止点位置： qb =[-pi/4 pi/6 -pi/2 pi/3]，

采样时间t = 0：0.01：5。

正运动学求出的初始位置和终止位置的形态如图4所示，仿真得到的机械臂各个关节的角度、角速度、角加速度曲线图如图5所示，其中第i行三个图从左到右依次是关节i的角度、角速度、角加速度曲线图。由图5可知：仿真曲线都很光滑，其中第3个关节保持不动。

图4 轨迹规划初始位置与目标位置

图5 机械臂各个关节的角度、角速度、角加速度曲线图

结论

针对葡萄套袋机器人4自由度机械臂，本文利用Denavit-Hartenberg法则建立运动学模型，对其正运动学进行了求解，并且利用MATLAB Robotics Toolbox对正运动学求解的结果进行了仿真验证。其次，根据机械臂套袋时末端执行器保持水平的特点，简化了逆运动学的求解，并利用Robotics Toolbox对逆运动学求解的结果进行了验证。最后，初步探索了机械臂的轨迹规划，利用Robotics Toolbox对机械臂进行了在关节空间的轨迹规划仿真，达到了良好的效果。

[1] 庞俊杰,勾贺明，宁书臣,等.我国葡萄生产机械化现状及产品开发方向[J].农机质量与监督,2005(3).

[2] Niku S B．机器人学导论一分析、系统及应用[M]．孙富春，朱纪洪，刘国栋，等，译．武汉：华中科技大学出版社，1996：48-67．

[3] 蔡自兴. 机器人学[M]. 北京：清华大学出版社, 2000.

[4] 罗家佳, 胡国清. 基于 MATLAB 的机器人运动仿真研究[J]. 厦门大学学报, 2005, 44(5)： 640-644.

[5] 张毅，吴育理，罗元. 智能轮椅上的机械手运动学分析及轨迹规划[J]. 计算机应用研究, 2010,27(12)：4578-4591.

[6] 王仲民，蔡 霞，崔世钢.一种四自由度机器人的运动学建模[J].天津职业技术师范学院学报, 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.

本次项目是“十二五”国家科技支撑计划资助项目，项目编号：2011BAD20B07。