真空手的插值算法与动力学建模研究*

胡文静，甘　戈，于吉鲲，姜芳芳

(1.大连海洋大学 应用技术学院，辽宁 大连　116300；2.沈阳新松机器人自动化股份有限公司，沈阳　110168；3.中国重型机械研究院股份有限公司，西安　710032)



真空手的插值算法与动力学建模研究*

胡文静1，甘戈2，于吉鲲1，姜芳芳3

(1.大连海洋大学 应用技术学院，辽宁 大连116300；2.沈阳新松机器人自动化股份有限公司，沈阳110168；3.中国重型机械研究院股份有限公司，西安710032)

摘要：文章针对真空手需要在高速运动的条件下需要保持手臂稳定的问题。首先利用MDH法建立了坐标系，得到了各关节连杆的相对关系得到了正、反运动学描述。其次，讨论了三次样条插值在机器人路径规划中的应用，利用该算法对示教路径进行插值以消除了机器人运动过程中的不连续现象。再次用Newton-Euler进行多次迭代计算出力矩的数值方程求解，最后利用仿真工具软件对真空手机器人力矩进行了仿真，验证了真空手运动学和动力学的正确性。实验表明，运动学可以让机器人准确的进行运动，动力学和插值算法可以有效的增加真空手机器人的运动性能和稳定性。

关键词：真空机械手；三次样条插值；动力学；牛顿-欧拉

0引言

真空手在晶圆加工过程中承担着定位和传输晶圆到指定腔室两项关键任务，是IC装备的核心关键技术之一[1]。因为加工晶圆的工作环境和生产加工的工序要求,机器人持有晶圆在高速的条件下运行时必须保持手臂稳定，避免由于加速度的原因而造成硅片的飞出损坏，从而影响作业的正常执行和生产率，导致真空设备的洁净度下降报废率增加。鉴于此，单臂真空手机器人要求特别苛刻，特别是清洁度、控制精度、运动性能、稳定性和可靠性。因此，对加工晶圆传输的机械手研究其运动学和动力学等其关键技术是对IC产业的蓬勃发展的重要保证。

近年来，动力学技术已经从理论阶段逐步走向应用阶段。Degani A等研发了一种基于动力学单传动装置垂直攀爬机器人[2]。Swevers J等提出了一种工业机器人的动力学模型识别[3]。Zomaya A Y研究了在机器人动态模型:理论和应用程序中提取和计算参数识别的问题[4]。Dongsun Kim等提出了一种管理家庭服务机器人软件的应用动力学软件架构[5]。Sung Jin Yoo等研究了利用柔性关节机器人的神经网络反馈控制自适应输出：动力学表面设计方法[6]。

本文首先将单臂机械手分为上升轴、旋转轴和伸缩轴三个部分。然后详尽介绍了单臂真空手的运动学模型、插值算法和动力学模型，经过仿真试验表明，动力学和插值算法的正确性[6]。

1真空手运动学

如图1所示，为真空手坐标系分布，机器人的MDH参数如表1所示[8]。

图1　真空手机械手连杆坐标系关系图

通常把描述一个连杆和下一个连杆间相对关系的齐次变换叫做T矩阵[9]。本文首先采用MDH方法建立坐标系，进而用齐次变换矩阵来建立机器人末端执行器坐标系相对于基坐标系的数学模型[10]。根据以上这些参数可以得到表1杆件坐标系之间的关系，同时获得各个关节之间的变换矩阵。

表1　真空手机器人的MDH杆件坐标系

1.1机械臂几何法运动学正解

本文提出了一种几何法求解运动学方程，真空手运动学正解如下：

(1)

1.2机械臂运动学反解

机械臂运动学反解：根据真空手机的位置、姿态和连杆长度，求解出机械臂各个轴应该旋转多少角度，以满足已知的位置和姿态[11]。

通过逆运动求解得各个关节角度，可以实现对机器人进行运动学分析、编程和轨迹规划等工作。用逆矩阵求解关节角度虽然看起来简单但是计算过程复杂[12]。本文根据MDH构建的运动学方程，使用常规的方法来求解机器人位置值到机器人管家值的转换，通过推到可以得到：

(2)

1.3运动学验证

机器人末端轨迹为一螺旋线，根据运动学逆解公式计算出各关节角度值。将上面螺旋线逆解求取出的一系列关节值代入正解运算公式中，求解出对应的一系列正解值，将正解值与上面规划的螺旋线对比如下图2所示。

图2　运动学正反解验证

图中蓝色曲线代表规划的螺旋线，红色曲线代表规划螺旋线经一次反解→正解后计算得到的螺旋线，从图中可看出两条曲线处于重合状态，证明了机器人运动学计算的正确性。

2三次样条插值

在数学上，光滑程度的定量描述就是函数的k阶导数存在且连续，函数曲线具有k阶光滑性，满足的阶数越高，就表现为光滑性越好[8]。分段线性插值具有零阶光滑性，分段Hermite插值具有一阶光滑性，三次样条插值则具有二阶光滑性[9]。

2.1三次样条插值的原理

因此，由天然的约束条件可给出3(k-1)+k+1=4k-2个方程。而k个分段共有k个三次方程，共需4k个约束条件。

样条本质上是多段三次多项式拼合而成的曲线，在拼接处连续，且一阶和二阶导数均连续[11]。样条最初是基于弹性条设计的工具，这是因为样条函数是受到k+1个点约束的弹性条的最小能量形状，而弹性条总是倾向于稳定在能量最小的位置。

2.2三次样条插值的实现

其中hi=xi+1-xi，i=0,1,…,k-1。积分两次，再利用插值条件确定积分常数，可求得样条函数如下：

(3)

鉴于此，借助MATLAB图形工具箱，实现三次样条插值的样条曲线，得到图3和图4的曲线示图，分别对应最近邻插值、分段线性插值和三次样条插值的对比效果曲线[5]。从图4可以看出，三次样条插值曲线较光滑，保持了两插值节点间的单调性。

图3　最近邻和分段线性插值

图4　三次样条插值

3真空手的动力学建模

在控制精度和动态控制指标的要求下，传统的控制效果很差，难以满足较高的性能指标的晶圆传送需求[9]。要实现优异的运动效果，必须要对机器人本机进行动力学分析[10-11]。鉴于此，引入动力学的控制是一条有效的途径。采用力矩前馈控制(动力学前馈补偿)法，可效的改善机器人的性能[12-13]。

在真空手传送晶圆的工作过程中，升降运动、旋转运动和水平伸缩运动都是单独进行运动的，也就是说其中一个运动模块运动时，其它模块是保持不运动的。运用牛顿-欧拉动力学将上升、旋转和伸缩进行了动力学的三个部分解耦，可以有效的简化动力学方程的复杂度[9-10]。下面本文将从三个方面来分别计算动力学的方程。

3.1机械参数

如表2所示，为真空手的实际机械参数。

表2　真空手机器人的机械参数

3.2牛顿-欧拉动力学

牛顿-欧拉外推法

外推：i：0~5

(4)

牛顿-欧拉内推法

内推：i：6~1

(5)

3.3升降轴动力学

升降轴运动：当单臂SCAR机器人进行升降运动时，旋转和伸缩是不运动的。一般来说对于升降轴，使用公式(5)计算动力学：

(6)

3.4旋转轴动力学

一般来说对于旋转轴，使用如下公式(7)计算动力学：

(7)

其中c包含静摩擦力和克服真空隔离装置的摩擦力系数，τ为旋转关节电机提供的扭矩。

3.5伸缩轴动力学(封闭形式)

牛顿-欧拉动力学计算包括向外迭代和向内迭代量部分：

外推i(2～4)：

对连杆2用向外迭代法求解如下：

(8)

对连杆3用向外迭代法求解如下：

(9)

对连杆4用向外迭代法求解如下：

(10)

内推i(5～3)：

对连杆5用向外迭代法求解如下：

(11)

对连杆4用向外迭代法求解如下：

(12)

对连杆3用向外迭代法求解如下：

(13)

4计算结果及分析

首先规划一段有的位置曲线值作为算法的输入端，然后将动力学计算值作为算法的输出端。最后对各个轴的动力学数值数据进行采集和统计，如图5所示，为牛顿-欧拉动力学计算动力学数值曲线，如图6所示，为MATLAB机器人工具箱动力学数值曲线。经过对比分析，曲线一致。

经过将牛顿-欧拉法计算得到的动力学数值与MATLAB机器人工具箱中的动力学算法得到的各个轴动力数值相互比较，说明该算法适用于真空机械手，并且可以推广到各类机器人当中去，从而验证了算该算法的结果可靠性、再现性和普遍性。

图5　牛顿-欧拉动力学计算动力学数值曲线

图6　MATLAB机器人工具箱动力学数值曲线

5结束语

本文针对真空手机器人必须在高速的条件运行的下保持手臂稳定的问题，详尽的介绍了运动学建模以及路径曲线插值的过程。验证了机械臂数学模型以及正、逆运动学分析的正确性，同时与最近邻插值相比，三次样条插值能较好地保留原函数信息，插值函数比较光滑。将其动力学技术应用到该机器人当中去。同时采用力矩前馈控制(动力学前馈补偿)法，对机械手的动力学进行了仿真求解。仿真实验表明，运动学、插值算法赫尔动力学的正确性。

[参考文献]

[1]赵彬,刘振宇,邹风山,等.单臂SCARA手的动力学建模与参数辨识[J].机械设计与制造,2016,3(6):153-165.

[2] Degani A, ShapiroA, Choset H, et al. A dynamic single actuator vertical climbing robot[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems, 2007:2901-2906.

[3] Swevers J, Verdonck W, De Schutter J. Dynamic Model Identification for Industrial Robots[J]. IEEE Control Systems, 2007, 27(5): 58-71.

[4] Zomaya A Y. Extraction and computation of identifiable parameters in robot dynamic models: theory and application[J]. Control Theory and Applications, 2002, 141(1): 48-56.

[5] Dongsun Kim, Sooyong Park, Mun-Taek Choi, et al. Applying Dynamic Software Architecture Management to Home Service Robot Software[C]. The 16th IEEE International Symposium on Robot and Human interactive Communication, 2007: 285-290.

[6] Sung Jin Yoo, Jin Bae Park, Yoon Ho Choi. Adaptive Output Feedback Control of Flexible-Joint Robots Using Neural Networks: Dynamic Surface Design Approach[J]. IEEE Transactions on Neural Networks, 2008, 19(10): 1712-1726.

第3期

[7] Zhang Bin, Xiong Rong, Wu Jun. Kinematics analysis of a novel 7-DOF humanoid manipulator for table tennis[C].Electronics, Communications and Control (ICECC), 2011, 33(10):9-11.

[8] 赵彬. 基于机器视觉的工业机器人分拣技术研究[D]. 沈阳：沈阳工业大学，2013.

[9] Zhao Dingxuan, Wei Hailong, Zhang Hongyan. Explicit solution for inverse kinematics of 3-RPS parallel link manipulator[C]. Computer, Mechatronics, Control and Electronic Engineering (CMCE), 2010 (2):425-429.

[10] Zhao Jia-xin, Luo Qing-sheng, Mo Yang, et al. The analysis of kinematics and gait planning of a segmental appendage octopod robot[J]. Machinery Design & Manufacture, 2013 (10): 171-174.

[11] Nandy S, Shome S N, Chakraborty G. A modular approach to detailed dynamic formulation and control of wheeled mobile robot[C]. 2011 International Conference on Mechatronics and Automation, 2011:1471-1478.

[12] Jaeoh Lee, Seongik Han, Jangmyung Lee. Decoupled Dynamic Control for Pitch and Roll Axes of the Unicycle Robot[J]. IEEE Transactions on Industrial Electronics, 2012, 60(9):3814-3822.

[13] 郑泽钿,陈银清,林文强,等. 工业机器人上下料技术及数控车床加工技术组合应用研究[J]. 组合机床与自动化加工技术, 2013(7):105-109.

[14] 石磊,金忠全. 四自由度机器人的建模和仿真[J]. 组合机床与自动化加工技术, 2012(3):37-39.

(编辑李秀敏)

Research Interpolation Algorithm and Dynamics Modeling of Single Vacuum Manipulator

HU Wen-jing1,GAN Ge2,YU Ji-kun1,JIANG Fang-fang3

(1.College of Applied Technology, Dalian Marine University, Dalian Liaoning 116300, China;2.Siasun Robot & Automation Co., Ltd, Shenyang 110168, China)

Abstract：Aimed at vacuum manipulator need to maintain stability of arms problem under the condition of high speed movement. Firstly, use MDH method to establish coordinate system, and obtain the joints connecting rod relative relationship of positive and inverse kinematics description. Then, bicubic interpolation is discussed in robot path planning, and the application of algorithm is used to analyze the teaching path interpolation to eliminate the discontinuous phenomenon in the process of robot movement. Finally，calculate the dynamic equation with Lagrange method. Using the Matlab robot toolbox executes kinematics and dynamics simulation experiment. The experimental results show that the kinematic algorithm, interpolation and dynamic algorithm has good robustness for the precise motion of robot and dynamic control has important significance.

Key words：vacuum manipulator;cubic spline interpolation;dynamic;Newton-Euler

中图分类号：TH166;TG506

文献标识码：A

作者简介：胡文静(1981—)，女，武汉人，大连海洋大学讲师，硕士， 研究方向为机器人及自动化，(E-mail) huwenjingAa@126.com。

*基金项目：国家863计划(2012AA041405) ；国家自然科学基金(11126028)

收稿日期：2015-09-01；修回日期：2015-10-09

文章编号：1001-2265(2016)03-0029-04

DOI:10.13462/j.cnki.mmtamt.2016.03.008