扬州大学机械工程学院
????1??r??R/d (4-10)
?x?L?cos??2?r?sin(?/2)?cos(?/2)?r?sin(?)??y?L?sin??2?r?sin(?/2)?sin(?/2)?r?(1?cos?)?(?l??r)2??l??r??d?sin???ll??r??R/d? (4-11)
(?l??r)2??l??r??d?(1?cos?????r??R/d? (4-12)
第 16 页
扬州大学机械工程学院
第五章 果树采摘机器人机械臂运动学分析
果树采摘机器人的运动学问题包括正运动学以及逆运动学两个问题，它们之间的关系如图 5-1所示：
杆件参数关节角θ1，θ2……θn运动学正问题杆件参数末端执行器位姿关节角θ1，θ2……θn运动学逆问题
图5-1 机器人运动学关系示意图
在多数文献中，对机器人运动学进行建模时使用D-H法，在对机器人进行控制时，需要求解雅克比矩阵，其中包括图像雅克比矩阵和机械人雅克比矩阵，本文根据果树采摘机器人的机械机构，采用了一种新的机器人正、逆运动学求解方法。
5.1机械臂正运动学分析
在采摘作业中，采摘机器人首先要确定目标果实位置信息与机器人各关节的关系，这个环节有两部分组成：首先确定目标果实3D坐标与其在摄像机平面上的2D坐标之间的关系；其次确定摄像机坐标系与机器人坐标系之间的关系。 图5-2所示为机器人结构关系，采摘机器人的摄像机采用eye-in-hand方式安装, 摄像机坐标系C(X,Y,Z)与机器人基座坐标系(Xo,Yo,Zo)的各坐标轴相互平行, 假设目标果实在基坐标系内的坐标为(Xbo,Ybo,Zbo)，其在摄像机坐标系内的坐标为(Xg,Yg,Zg)。由摄像机成像基本原理得到目标在摄像机坐标系内的3D坐标
(Xg,Yg,Zg)与其在摄像机成像平面上的
X1?Xg?fZg2D坐标(X1,Y1)之间的关系：
Y1?Yg?fZg， (5-1)
式中 f — 摄像机焦距。
第 17 页
扬州大学机械工程学院
ZoL2X2X3ZX1XoZ1Z2L1L3L4XC（X,Y,Z）Z3o
图5-2采摘机器人结构示意图
对于摄像机坐标系与机器人基座坐标系之间的变换关系可由图5-3所示的几何解法求得：
C（X,Y,Z）L3θ2腰部Y X（X,Y）oL2大臂 小臂 θ3θ2ZL22?L32?2?L2?L3?cos(???3)?(L2?sin?2?L3?sin(?2??3))2X? (5-2)
1?tg2?1 Y?X?tg?1 (5-3) Z?L1?L2?sin?2?L3?sin?(2??3) (5-4)
式中 L1、L2、L3分别为腰部、大臂、小臂的长度；?1、?2、?3分别为机械手臂第二、三、四自由度关节角度。在此，设小臂延伸部分在机器人初期寻找目标果实过程中是收缩回小臂内的。
由式(5-2)、(5-3)、(5-4)可以得到目标果实在基坐标系的坐标(Xbo,Ybo,Zbo)与果实在摄像机坐标系的坐标(Xg,Yg,Zg)之间的关系：
第 18 页
L1 Xθ1Y
图5-3 采摘机器人关节几何关系
扬州大学机械工程学院
Xg=Xbo-X ，Yg =Ybo-Y ，Zg=Zbo-Z (5-5)
5.2机械臂逆运动学建立
采用移动平台的采摘机器人之所以得到广泛，很大的原因是它的灵活性好，可以到达的空间比较大，当单独的机械手不能到达指定的位置时，移动平台可以通过向x,y方向的混合移动来实现末端操作器的运动。因此在知道移动平台的位移时，移动平台的运动偏移角就是已知的，在进行果树采摘机器人逆运动学求解时，只考虑机械手的逆运动学。 根据式（5-3）可得：
?1?arctgY (5-6) X由（5-4）得：
Z?l1?l2sin?2?l3sin??2??3? (5-7)
将式（5-6）、（5-7）带入式（2-14）并平方得：
22l2?l3?2l2l3cos?3??Z?l1? (5-8) 2X?Y2l?2X2整理（5-8）得：
22X2l?Y2?l2?l3??Z?l1? (5-9) cos?3?2l2l32所以：
22X2l?Y2?l2?l3??Z?l1? (5-10) ?3?arccos2l2l32由（5-3）得：
Z=l1?l2sin?2?l3?sin?2cos?3?cos?2sin?3? =l1??l2?l3cos?3?sin?2?l3cos?2sin?3 (5-11)
即： ?Z?1 i n (5-12) l???lcs?3si?n2?l3c?os2?s2?l3o?322两端平方： ? l???lcs?3si?nos2?si n??l3c??2?l3o?3??Z?1?2?(5-13)
整理的：
第 19 页
扬州大学机械工程学院
?l22?l32?2l2l3cos?3?sin2?2?2?Z?l1??l2?l3cos?3?sin?2??Z?l1??l32?1?cos2?3??0 (5-14)
22 (5-15) Z?? 1l222由(5-10)知： l2?l3?2l2lcs3?Xl?2Y??3o?代入（5-14）式得：
?X2l?Y2??Z?l1?2?sin2?2?2?Z?l1??l2?l3cos?3?sin?2??Z?l1?2?l32?1?cos2?3??0 ?? (5-16)
解得：
sin?2??Z?l1??l2?l3cos?3???Z?l1??l2?l3cos?3?222??X2l?Y2??Z?l1????Z?l1??l32?1?cos2?3???????X2l?Y2??Z?l1?2???2
(5-17)
?2?arcsin(Z?l1)(l2?l3cos?3)?(Z?l1)2(l2?l3cos?3)2?[X2l?Y2?(Z?l1)2][(Z?l1)2?l32(1?cos2?3)][X2l?Y2?(Z?l1)2]
(5-18)
22X2l?Y2?l2?l3??Z?l1?(其中cos?3?)。
2l2l32
第 20 页