曲块搬运翻曲悬挂式机械手运动学求解方法研究

田建平， 刘伟， 代文旭， 宛家明， 沈思思

(四川理工学院机械工程学院， 四川自贡643000)



曲块搬运翻曲悬挂式机械手运动学求解方法研究

田建平， 刘伟， 代文旭， 宛家明， 沈思思

(四川理工学院机械工程学院， 四川自贡643000)

鉴于所研究的悬挂式白酒曲块搬运翻曲机械手存在平行四边形辅助机构，不能运用一般的运动学求解方法对其轨迹直接解算的问题，提出了一种基于DH法的改进型逆向运动学求解方法。该算法不需要了解末端执行器相对参考坐标系的姿态，仅考虑其空间位置便可完成对各运动副参数的几何运动学求解。在Matlab中采用新算法对关节的位姿矩阵进行修正，结果表明：所得位姿曲线数据满足机械手腕部的运动精度要求。提出的新算法解决了在常规机械手位姿求解过程中所使用的逆运动学数值方法计算慢、甚至在某些情况下不能求出所有可能解的问题。

悬挂式机械手；运动学；算法修正；算法优化

引言

根据白酒固态发酵的搬运翻曲工艺要求，曲块需按照一定的规律摆放在曲房中进行发酵，并根据发酵情况对曲块进行翻转搬运。目前企业完全依靠人工搬运翻曲，存在着工作环境恶劣、工作量大、效率低等问题。考虑到生产工艺要求及企业现有曲房空间的布置，研究设计悬挂式翻曲机械手以替代人工进行作业。机械手的腰部设计在底座的上方，且通过轮滑装置安装在曲房空间上部的轨道上，以使机械手能够沿着曲房上部的轨道在曲房中运动。

机械手的运动学求解通常采用DH方法，但当机械手带有平行四边形辅助机构时，使用此方法进行求解便存在一定的问题[1]。正向求解时，本文采用对机械手连杆的位姿修正的方法以解决带平行四边形辅助机构的机械手正向求解问题[2-3]。逆向求解时，碍于传统计算方法的繁琐，在基于DH法的基础上提出了一种新的算法，实验表明，该算法不仅能准确高效的解决悬挂式机械手运动的逆向求解问题，而且具有较高的适应性及效率性，具有一定的应用价值。

1 机械手主体结构

图1 为自主研发的QFMD010型机械手结构。该机械手为6自由度机械手，腰部能沿上方轨道的两个方向进行滑移(未表达)，腰部的旋转，大臂的旋转，小臂的旋转，末端的旋转以及执行器的动作(未表达)，本文实际针对4个自由度进行研究。

1.末端、2.连杆1、3.三角形支架、4.连杆2、5.连杆3、6.腰部、7.腰部悬挂支撑、8.平衡缸、9.平衡配重、10.连杆6、11.支座、12.大臂、13.小臂图1 机械手本体结构及末端执行器

机械手的各个杆件的尺寸数据：大臂700 mm，小臂650 mm，腰部回转部分到最顶端的距离900 mm，腕部伸出长度250 mm，如图2 为工作位置。机械手各关节的仰俯角参数见表1。

图2 机械手工作位置(单位：mm)表1 机械手各轴转动角度参数

驱动轴关节类型关节角运动空间/°一轴整体旋转-180～+180二轴大臂俯仰0～+25三轴小臂俯仰-30～+25四轴腕部旋转-180～+180

2 正向运动学分析

机械手的运动学理论分析常用方法是Denavit-Hartenberg(DH)法，使用坐标的齐次变换表达机械手各个杆件在参考系的空间集合关系，通过4×4矩阵表达各进杆件间空间集合关系，由此求出机械手爪在参考系中空间位姿[4-6]。

本文主要针对带平行四边形辅助机构的机械手，不再适用传统DH法进行计算，根据平行四边形辅助机构的特性对算法进行修正，根据前一关节的位姿矩阵进行修正，从而得到机械手腕部的位姿。

图3 为机械手转动关节连杆坐标系建立的示意图。腰部为l1，大臂为l2，小臂为l3，腕部为l4。采用右手螺旋法判定旋转角度的正负号，其中：腰部机构绕固定坐标系Z0轴旋转θ1；l2绕l1的坐标系X1轴旋转θ2；杆l3绕杆l2的坐标系Z2轴旋转θ3；末端机构绕自身坐标系Z4轴旋转θ4。

图3 机械手连杆坐标系

设定机械手的大臂小臂都处于水平状态时(图4)为零状态。

图4机械手初始状态

对以上运动过程进行求解，根据仰俯角度对应到机械手的坐标系，得到机械手的DH参数(表2 )。

表2 机械手DH参数

A1=Rot(Z,θ1)Trans(0,0,d1)=

(1)

A2=Rot(X,θ2)Trans(0,a2,0)Rot(Y,α2)=

A3=Rot(Z,θ3)Trans(0,a3,0)=

(3)

由于悬挂式QFMD010型机械手的末端执行器采用平行四边性机构，对于连杆4的姿态以及空间位置不再适用于传统的DH算法，基于平行四边形辅助机构的特性，对连杆3相对于固定坐标系的位置及姿态T3进行修正，得到连杆4的姿位矩阵T4。

则QFMD010机械手的前三个关节的运动方程为：

T3=A1A2A3=

(4)

A4=Rot(Z,θ4)=

(5)

则QFMD010型机械手的运动方程为：

(6)

nx=cosθ1cosθ4-sinθ1sinθ4

ny=sinθ1cosθ4+cosθ1sinθ4

nz=0

ox=-cosθ1sinθ4-sinθ1cosθ4

oy=cosθ1cosθ4-sinθ1sinθ4

oz=0

ax=0,ay=0,az=1

Px=(-cosθ1sinθ4-sinθ1cosθ4)

H-a3sinθ1sinθ2sinθ3-a3

sinθ1cosθ2cosθ3-a2sinθ1cosθ2

Py=(cosθ1sinθ4-sinθ1cosθ4)H+a3cosθ1

sinθ2sinθ3+a3cosθ1cosθ2cosθ3+

a2cosθ1cosθ2

Pz=-a3cosθ2sinθ3+a3sinθ2cosθ3+

d1+a3sinθ2

设QFMD010机械手的起始位置为零位，机械手由图4所示的位置(工作零位)运动，将坐标系的参数带入计算：a2=700,a3=650,H=250，取三组数据进行计算：θ1=-19.8°,θ2=5.5 °,θ3=-6.6 °,θ4=19.8°。

带入运动方程，求得数值解：

θ1=-49.5 °,θ2=13.6 25°,θ3=-16.3 5°,

带入运动方程，求得数值解：

θ1=-90°，θ2=25°，θ3=-30°,θ4=90°

带入运动方程，求得数值解：

3 逆向运动求解分析

运动学逆问题，通常是根据给定的满足工作要求时末端执行器相对参考坐标系的位置和姿态以及杆件的结构参数，求各运动副参数[7]。

逆向运动学解决的问题是已知手部的位姿，求各个关节的变量。在机械手的控制中，往往已知手部到达的目标姿位，需要求出关节变量，以驱动各个关节的电机，使手部的位姿得到满足[8-10]。

本文主要针对旋转式两臂机械手(QFMD010型)为例，设置了专门的算法，能够不需要末端执行器相对参考坐标系的姿态，只需要位置的情况下完成各运动副参数的计算，对于轨迹控制编程及计算可节省大量的运算时间。

由于带平行四边形辅助机构的机械手的末端姿态保持恒定不变，所以无法直接根据最后一关节的位姿求得各个关节变量[11-12]。以QFMD010型机械手为例，只能用关节3的姿位进行计算。且利用此方法，可同时避免末端连杆坐标系与前一坐标原点不重合的问题，降低逆解的难度。

(7)

用得到的数据进行逆向求解，方便以验证其结果的正确性。采用建立在DH逆向求解算法基础上的简化算法，这种方法可以用于解决同类型两臂的机械手运动求解问题。

第一步：采用DH逆向算法求解θ1：

得：

θ1=arctan2(-Px,Py)

第二步：两臂永远处于同一平面，且两臂饶腰部旋转，那么腰部也处于这一平面内。因此，机械手在任意状态下，都能找到一个平面，满足大臂、小臂、腰部都这一平面上。在这个特定的平面内建立几何关系。利用几何关系：

(8)

第三步：剔除多余的解。

带入正解时得到的第三组数据进行求解：

Px=1197.3 ,Py=250,Pz=-929.2

带入方程求得数值解：

θ1=-90°

θ2=27.8219°，θ2=25.0271°

θ2=120.0339°，θ2=117.2 390°

θ3=64.9729°，θ3=117.8219°

θ3=30.0339°，θ3=-27.2 390°

由于取Px,Py,Pz的值是保留了小数点后1位小数，导致θ角存在0.1 °的误差。根据给定的转角范围，可以剔除多余的解，确定θ2=25.0271°，θ3=30.0339°。

4 Matlab仿真

为了保证上述运动学求解的正确性，对机械手进行运动仿真分析，设定机械手4轴同时运动，vθ1=-9 °/s,vθ2=2.5 °/s,vθ3=3 °/s,vθ4=9 °/s总运行时间10 s。

本文采用UG软件对机器人主要结构的运动学参数进行仿真分析。相关参数导入Matlab的分析结果如图5所示。

图5翻曲机械手末端位置随时间变化曲线

根据图5(a)，第一组数据的X方向、Y方向、Z方向位移分别为455.1 、1514、-970.6 ，与理论计算结果吻合；第二组数据的X方向、Y方向、Z方向位移分别为985.7、1105、-918.1 ，与理论结果吻合；第三组数据的X方向、Y方向、Z方向位移分别为1198、250、-929.2 ，与理论结果吻合。证明正向求解过程正确。

结合图5(a)、(b)中X轴为同一时间轴，比较可得当机械手的的空间位置为(1197.3 ,250，-929.2 )，关节2、3的转角θ2、θ3分别为25°、30°，结果与理论吻合，该运动学逆问题推导公式可正确进行求解。

5 结束语

针对现有运动学求解方法不能较好地解决所研究的白酒制曲车间曲块的搬运翻曲机械手运动轨迹规划算法的问题，提出了一种适用于带有平行四边形辅助机构的运动学逆解方法，在只考虑空间位置的情况下便可完成对机械手各运动副参数的几何运动学求解。结果表明，该方法可较好地解决悬挂式4自由度机械手的运动学求解问题，对制曲车间的自动化、智能化生产线的产业化有一定的实用价值。

[1] 李大和.白酒酿造培训教程全书[M].北京:中国轻工业出版社,2013.

[2] 丁磊,李恩.五自由度搬运机器人系统设计与运动学分析[J].华中科技大学学报:自然科学版,2015(S1):19-22.

[3] 李慧,郑相周.一种六自由度可折叠机械手的运动学分析[J].机械传动,2015,39(12):79-82.

[4] 郭万金,李瑞峰,曹雏清.五自由度机器人逆运动学解法与工作空间分析[J].华中科技大学学报:自然科学版,2015(S1):14-18.

[5] 杨玉维,赵新华,孙启湲,等.基于多体动力学特性的机械手时间最优轨迹规划[J].机械工程学报,2014,50(7):8-14.

[6] 陈金宝,韩冬,王小涛,等.灵巧机械手多指协调控制技术[J].机械工程学报,2014,50(5):42-47.

[7] 雷崇华,何庆中,李江.电驱微差6R关节型开链机械手工作空间运动学逆解[J].四川理工学院:自然科学版,2008,21(3):103-106.

[8] 孙文成.搬运码垛机械手传动系统设计及其运动学分析与仿真研究[D].武汉:武汉科技大学,2014.

[9] 朱素霞.IRB660型码垛机械手的运动学与动力学分析[D].北京:北京邮电大学,2013.

[10] 牛金磊.斜导面直线驱动码垛机械手结构设计及动力学性能研究[D].广州:广东工业大学,2014.

[11] 张小江.机械手仿真研究及运动学动力学分析[D].长春:吉林大学,2006.

[12] 芮延年.机械手技术及其应用[M].北京:化学工业出版社,2008.

Research on the Kinematic Method of Suspension Manipulator with the Koji Handling and Turning

TIANJianping,LIUWei,DAIWenxu,WANJiaming,SHENSisi

(School of Mechanical Engineering, Sichuan University of Science & Engineering, Zigong 643000, China)

The studied suspension manipulator of liquor Koji handling and turning exists parallelogram auxiliary mechanism.This trajectory can not be solved directly by General kinematic method.The improved inverse kinematics method based on DH methods have been proposed aiming at this problem.This algorithm does not need to understand the attitude of End-effector relative to the reference coordinate system.The geometry kinematics of each kinematic pair will be completed only consider their spatial location.The joint post matrix is corrected using the new algorithm in Matlab.The results showed that the pose curve data is able to meet motional precision of manipulator wrist.According to inverse kinematics numerical methods used in conventional manipulator pose solving process，the new algorithm solves the problem of the calculation slow, and in some cases can not obtain all possible solutions.

suspension manipulator；kinematics；algorithm modification；algorithm optimization

2016-07-11

酿酒生物技术及应用四川省重点实验室重点项目(NJ2014-04)；四川省大学生创新训练项目(201510622008)；泸州老窖科研奖学金重点项目(15ljzk03)

田建平(1965-)，男，四川自贡人，教授，主要从事计算机集成制造技术方面的研究，(E-mail)tjp893@126.com

1673-1549(2016)05-0033-05

10.11863/j.suse.2 016.05.08

TH69

A