机械臂优化设计及控制仿真

286
第六届中国 CAE 工程分析技术年会论文集
增大而增大。
1
1
2
P(820,18)
P(820,18)
2
Q(200,18)
Q(200,18)
3
3
(a) L0 对关节转角范围的影响 图6
(b) x 对关节转角范围的影响 对关节转角范围的影响
L0 、 x
2.3 优化设计结果
为了防止大臂与小臂发生干涉，夹角 必须满足条件 15o ＃a
（3）
（4）
上式中：
m = cos q1 px + sin q1 py , n = pz - d1,
s1 = sin q1 , c1 = cos q1 , s2 = sin q2 , c2 = cos q2 ,
s3 = sin q3 , c3 = cos q3 ,c23=cos(q2 +q3 ),s23=sin(q2 +q3 ) ；
2
1
1 4 3
操
操
4 3 2
(a) L0 对臂长及空间的影响 图 5 L0 、 x 对臂长及空间的影响
(b) x 对臂长及空间的影响
图 5 分别显示了 L0 、 x 对各臂长及操作空间和漏失空间的大小。其中， “操作空间”指 平面内机械臂运行所需回转空间的面积； “漏失空间”指未达到的区域面积。 图中曲线 2、1、4、3 分别表示 L0 、 x 变化时，对应所求得的大臂、小臂、操作空间和 漏失空间的大小。通过对图 5 分析，可得以下结论： 大臂长度和漏失空间大小只与 L0 有关， L0 越大，大臂长度越小，但同时漏失空间增大， 即需要优化 L0 的值，使臂长和漏失空间均较小；小臂长度只与 x 有关，且 x 越大，小臂所 需长度越小；操作空间大小与 L0 和 x 无关。 图 6 曲线 1、2、3 分别代表大臂转角、小臂转角和两臂夹角分别随 L0 和 x 变化时的曲 线图。通过对图 6 的分析，可得以下结论： 大臂转角范围只与 L0 有关， 且随着 L0 增大而增大； 小臂转角范围只与 x 有关， 且随着 x 的增大而减小；两臂夹角随 L0 增大而减小，随着 x 的
（1）确定要改变的设计变量是基座的高度 L0 和基座与采摘区的水平距离 x ，优化的目标参 数是大臂、小臂长度，转角范围及大、小臂最小夹角，即我们要研究目标参数与 L0 和 x 的关系； （2）运用 Matlab7.0 依次绘出目标参数与设计变量 L0 、 x 的关系曲线，找出与各个目标参 数最相关的设计变量； （3）根据曲线关系，在满足设计原则的前提下，确定目标参数的最优解。
第六届中国 CAE 工程分析技术年会论文集
机械臂优化设计及控制仿真
李吉，雷键长，章乐平，李伟
（中国运载火箭技术研究院, 北京 100076）
摘
要：本文结合苹果果树实际生长环境，采用参数化设计和分析方法，对采摘机器人机械臂结构参数进行
优化设计。采用 D-H 空间机构描述法，求解位移正逆解和雅克比矩阵。通过 ADAMS 和 Matlab 建立集三维 实体设计、动力学建模、PD 控制于一体的交互式仿真系统，实现了对机械臂的速度控制和定位控制，并在 此基础上对机械臂进行动力学分析，确定驱动参数。仿真结果表明，该机械臂能够收获目标区域内 90.2%以 上的果实，满足设计要求。 关键词：参数化分析; 机械臂; ADAMS; PD 控制
（1）
该机械臂在 x-y 平面的收获区域由四条曲线围成，如图 4 所示：曲线 PP 1 2 是当小臂夹角 为 1max 时，大臂夹角从 2min 到 2max 变化时得到的曲线；曲线 P 3P 2 是当大臂关节为 2max ， 小臂夹角从 1max 到 1min 变化时得到的曲线；曲线 P 3P 4 是当小臂夹角为 1min ，大臂夹角从
机械臂的收获空间与基座高度 L0 和距采摘区域的水平距离 x 有关。通过参数化设计方 法， 以“多移动小关节， 少移动大关节”为设计原则， 优化选择基座高度 L0 和水平距离 x 的值， 从而确定最优的 P 1 ( x1 , y1 ), P 2 ( x2 , y2 ), P 3 ( x3 , y3 ), P 4 ( x4 , y4 ) ；将其中的 3 点代入式（2）获得 收取该采摘空间的臂长及转角最优值，从而确定机械臂工作空间最优解。
0.前 言
果实采摘机器人是工作在非结构环境下的机器人系统。其通过各种传感器，对被采摘对 象进行识别、定位。利用机械臂与末端执行器实现果实的抓取与分割。机械臂是果实采摘机 器人的最重要组成部分， 其结构及运动精度直接决定了末端执行器的工作范围和采摘成功率。 而在非结构化的农业环境中， 对机械臂的自由度及灵敏度要求与工业机器人相比更高。 因此， 机械臂的优化设计在采摘机器人的设计中，占据重要地位[1]。 Pons （ 1996 ）通过对机械臂进行运动学和动力学分析，确定机械臂的最优结构 [2] 。 Yoshikama（1992）提出了机械臂可操纵性指标，以对末端执行器的指向和定位进行优化[3]。 Kondo 运用可操纵性指标作为机器人结构选择标准， 制作了 7 自由度番茄采摘机器人[4]。 Ceres （1998）提出了全局可操纵性能指标 GMI，并据此对 Agribot 机器人机构及尺寸参数进行优 化设计[5]。Babu Siraraman（2005）提出了机械臂灵活度的方向恒定特性，并据此对柑桔收获 机器人结构进行设计[6]。 本文结合苹果果树特点，首次采用参数化设计方法对苹果采摘机械臂的采摘空间、机械 臂参数进行优化设计，并建立非结构环境下的机器人仿真系统，对设计结果进行仿真检验， 并实现了机械臂的速度和定位控制。
2.1 优化设计条件
o （1）大小臂夹角 mm 15 ＃a 1 6o 0 （2）基座高度 mm 550 ＃H 850
285
第六届中国 CAE 工程分析技术年会论文集
（3）机器人距采摘区距离
0 ＃x
mm 500
Hale Waihona Puke 结合机械臂的机械结构及苹果园果实实际生长环境，在满足以上条件的前提下，进行优 化设计。
2.2 优化设计过程
1.2 机械臂参数的确定
该苹果采摘机械臂为 3 自由度关节型，其结构简图见图 2。对机械臂进行参数化：基座、 大臂、小臂长依次为 L0 、 L1 、 L2 ；机械臂基座的旋转位
3 2
1
图 1 机械臂采摘范围
图 2 机械臂结构简图
1.基座旋转 2.肩关节俯仰 3.肘关节俯仰
移为 θ1 ，逆时针方向时为正，顺时针方向时为负；机械臂大臂的旋转位移为 θ2 ，大臂位于左 侧时为正，位于右侧时为负；小臂旋转位移为 3 ，位于上侧时为正，位于下侧时为负；目标 采摘点为 P(x,y,z)，以基座底部中心为坐标原点，建立坐标系，如图 3。
ì q1min = tg - 1 ((sin a - n) / (cos a - m)) ï ï ï ï ï q1max = a - tg - 1 ((sin a - n) / (cos a + m)) ï ï ï ï L1 = ( x2 - x1 ) / (sin q1max - sin q1min ) （2） ï ï í 1 ï ï L2 = [( x2 + L1 sin q1min ) 2 + ( y2 - L0 - L1 cos q1min ) 2 ] 2 ï ï ï ï q2min = tg - 1 (( y3 - L0 - L1 cos q1min ) / ( y1 - L0 - L1 cos q1max )) ï ï ï ï - 1 ï î q2max = tg (( y1 - L0 - L1 cos q1max ) / ( x1 - L1 sin q1max ))
- 283 -
第六届中国 CAE 工程分析技术年会论文集
1 机械臂结构参数的计算
1.1 目标采摘空间的确定
对苹果果树种植环境和果树树冠的调研结果表明：树冠顶部和底部苹果分布较少，树冠 中部苹果分布较多。统计表明，约 80%以上的苹果分布在 y 轴方向距地面 1000~2000mm、x 轴方向距树干 1000~2000 的空间；苹果的分布在阴阳面没有明显差异。本文最终确定 x-y 面 果树的采摘区域为图 1 所示矩形剖面线区域，其宽度和高度均为 1000mm。
2max 到 2min 变化时得到的曲线；曲线 PP 1 4 是大臂夹角为 2min ，小臂夹角从 1min 到 1max 变
化时得到的曲线；交点依次为 P 3 ( x3 , y3 ) 、 P 1 ( x1 , y1 ) 、 P 2 ( x2 , y2 ) 、 P 4 ( x4 , y4 ) 。将交点坐标 代入式（1） ，对 1min、1max、L1、L2、2min、2max 进行反解可得：
160o ；由图 6 中Ｐ、Ｑ点
可知，当两臂的夹角最小取到 18° 时，基座 L0 、水平距离 x 分别为 820mm、 200mm ,均符合 各自的取值范围。 据此，最后优化值见表 1：
表 1 参数优化结果 坐标点
P1
200 1820
P2
1200 1820
P3
1200 800
P4
200 800
L0
287
第六届中国 CAE 工程分析技术年会论文集
ì px = d 2c1s 2 + d 3c1s 23 ï ï ï ï í p y = d 2s1s 2 + d 3s1s 23 ï ï ï ï î pz = d1 + d2c2+ d3c23
禳 py 镲 镲 q1 = tan 镲 镲 px 镲 镲 镲 镲 镲 m 2 + n 2 + d 22 - d 32 m 镲 q2 = arcsin - arccos 镲 镲 2 2 2 镲 2 m + n d 2 m + n2 镲 镲 睚 镲 镲 m 2 + n 2 + d 22 - d 32 m 镲 或p - arcsin - arccos 镲 2 2 2 镲 2 m + n d2 m + n2 镲 镲 镲 镲 镲 c1 px + sin q1 p y - s 2d 2 镲 镲 q3 = + arcsin( ) - q2 镲 d3 镲 镲 铪
L1 =707.1mm L2 =860.2mm
3 ADAMS 与 MATLAB 联合仿真
通过 ADAMS 建立苹果采摘机械臂虚拟样机，并与 MATLAB 联合仿真。仿真系统结构 见图 7。该控制系统分为 A、B、C、D4 个模块： A 模块可通过运动学正逆解方程，求得各关节角位移向量组，实现机械臂末端空间位移 量与机械臂各关节角位移量的转换，该机械臂的运动学正逆解方程分别见式 3 和式 4：
2
L 1
L 2
P 1
L 3
P2
3
P(x,y,z)
1
Z
图 3 机械臂参数化
P3
P4
图 4 机械臂平面采摘区域
则小臂末端点 P(x,y,z)坐标满足方程组（1） ：
284
第六届中国 CAE 工程分析技术年会论文集
ì x = - L1 sin θ1 + L2 cos θ2 ï ï ï ï y = L cos θ + L sin θ í 0 1 2 2 ï ï ï z= 0 ï î
B 模块可根据角位移向量组，按照设定的角加速度，通过 PD 控制，生成各关节角速度 控制指令； ADAMS 控制模块 C 根据角速度指令对虚拟样机驱动电机进行控制； 模块 D 以曲线或数组的形式显示不同时刻各关节角速度和小臂末端点的空间坐标。
A
D
B
图 7 联合仿真系统搭建 A、运动学逆解模块 B、速度 PD 控制模块 C、ADAMS 模块
式（2）含有 6 个未知数，因此，只要给定目标采摘区域边界点 P 1 ( x1 , y1 ) 、 P 2 ( x2 , y2 ) 、
P3 ( x3 , y3 ) 、 P4 ( x4 , y4 ) 中任意三点的坐标值，即可通过式（2）求得该采摘空间对应的臂长
及转角，从而确定机械臂的工作空间。
2 机械臂参数的优化设计
820
x（mm） y（mm）
从所求得的目标采摘区域四个边界点 P 1 ( x1 , y1 ), P 2 ( x2 , y2 ), P 3 ( x3 , y3 ), P 4 ( x4 , y4 ) 中任意 取三个点的坐标值，代入式（2）求解方程，即可求得该采摘空间臂长及转角的最优值，见表 2：
表 2 机械臂结构参数 关节 基座 肩关节 肘关节 转角范围 -90° ~90° -45° ~45° 18° ~160° 臂长