Tripod精密平台动力学分析及控制

赵 罡，张 佳，肖文磊

(北京航空航天大学 机械工程及自动化学院，北京 100191)

0 引言

目前，高精密运动平台主要采用超声电机驱动方案，来实现亚微米或纳米级的定位精度，国内外对其研究已经取得一定成果[1-3]。采用直线超声电机驱动的Tripod精密平台是由Hunt[4]提出的3-RPS少自由度并联机构发展而来的混联机构，通过3-RPS并联机构实现Z轴、A轴及B轴的运动；通过串联平台实现X轴和Y轴的运动，使X轴与Y轴具有较大的运动范围与运动速度。

动力学建模是实施基于动力学模型控制、提高控制精度的基础。郑魁敬[5]对3-RPS并联机构运动学和静力学特性进行分析；刘善曾[6]对3-RRS并联机器人进行运动学和动力学分析，并基于Lagrange方程建立其动力学模型；STAICUCAI[7]采用虚功原理和拉格朗日乘子法两种方法对3-RPS机构进行逆动力学分析；薛邦灿[8]根据拉格朗日方程建立三自由度并联机器人的动力学模型，并对其动力学特性进行仿真分析，为仿生肘关节的变刚度控制奠定理论基础；贾凯凯[9]利用虚功原理建立了四自由度并联机构的完备刚体动力学模型，并利用ADAMS校验该模型的正确性；李永刚[10]采用牛顿欧拉法对3-RPS机构进行了逆动力学分析,求解机构所需的驱动力与约束力矩。

本文以控制精度高、响应速度快的直线超声电机驱动的Tripod精密平台为对象，针对其串并联混合机构的特点，建立动力学模型，并基于PLC语言及C++混合编程开发控制系统，实现平台按照预定轨迹运动，为Tripod精密平台精密运动控制奠定基础。

1 Tripod精密平台简介及坐标系建立

如图1所示，Tripod精密平台由精密二维平台和Tripod并联平台两部分组成，其中Tripod并联平台为3-RPS结构，具有两个旋转自由度和一个平移自由度，精密二维平台具有两个平移自由度，实现X轴、Y轴、Z轴、A轴及B轴五轴联动。采用超声电机直驱的方式驱动负载，不经过传动机构，可以提高机械系统的传动刚度和传动精度。精密平台内部安装有高精度的Micro-E光栅尺，以检测运动平台的当前位置，以及作为伺服控制系统的反馈信号。

图1 Tripod精密平台

通过支持工业以太网EtherCAT总线的压电伺服驱动器，实现对超声电机的网络化伺服控制。采用德国的基于PC的支持EtherCAT工业以太网协议的实时控制的TwinCAT软件系统，作为支持EtherCAT接口的超声电机控制器，实现对Tripod精密平台的控制。Tripod精密平台系统结构如图2所示。

Tripod精密平台为混联结构，为了求解方便，将3-RPS与XY平面运动平台分开考虑。优先考虑3-RPS机构部分，由动平台、静平台及3个支链组成，3-RPS机构结构简图如图3所示。ai和bi(i=1,2,3)分别为支链i球面副(R副)和转动副(S副)的中心，构成外接圆半径分别为r和R的等边三角形ΔA1A2A3和ΔB1B2B3。

以B1、B2、B3的分布圆圆心为中心点建立定平台坐标系{W}：OW-XYZ，Z轴垂直向上，X轴指向B1；以A1、A2、A3的分布圆的圆心为中心点建立动平台坐标系{P}:Op-X′Y′Z′，Z′ 轴垂直向上，X′轴指向A1。

图3 Tripod平台3-RPS结构简图

2 机构逆运动学分析

对Tripod精密平台进行逆运动学建模，对其进行位置分析、速度分析及加速度分析，为动力学建模奠定基础。

2.1 位置分析

以ZYZ欧拉角(φ，θ，-φ)表示平台运动姿态，平台倾角大小由进动角θ决定，脊线位置完全有由章动角φ决定，则动坐标系Op-X′Y′Z′相对于定坐标系OW-XYZ的旋转矩阵表达式表示为：

(1)

其中,c为cos，s为sin。

(2)

图2 Tripod精密平台系统结构

由空间向量关系可得到3个驱动杆在{W}中矢量Li为：

Li=Wai-Wbi

(3)

(4)

动平台中心点的偏移量与动平台的进动角φ和章动角θ有关。精密平台X、Y、Z方向位移分别为X0、Y0、Z0。

串联平台实现X轴和Y轴的运动，使X轴与Y轴具有较大的运动范围与运动速度，平台X，Y方向位移分别为xp和yp，表达式如下：

(5)

2.2 速度分析

式(2)对时间求导，可得{W}系下R副中心Ai的速度：

(6)

驱动速度li的可表示为Vai在ni上的投影:

(7)

式(4)对时间求导，进而可得动平台中心点速度：

(8)

ZYZ型欧拉角参数一阶运动[12]表示为：

(9)

通过式(7)～式(9)，可得驱动杆速度：

(10)

式(5)对时间求导，即可得二维平台运动速度：

(11)

2.3 加速度分析

式(7)对时间求导，可得驱动杆加速度：

(12)

式(8)对时间求导可得动平台中心点加速度：

(13)

式(11)对时间求导，即可得二维平台运动加速度：

(14)

3 机构动力学分析

图4 支链简化结构图

3.1 支链动力学分析

在进行Tripod平台机构的动力学分析之前，首先需要确定连杆角速度和加速度的表达式，进而对支链进行动力学分析。

如图5所示, 支链坐标系可由{W}系绕z轴转动γi角，再绕yi′轴转动αi角得到[13]。

图5 支链欧拉角

支链坐标系相对定坐标系的旋转矩阵可表示为：

(15)

将支链角速度和角加速度、缸体和活塞质心的加速度转在支链坐标系描述为：

(16)

ZinAi为作用于ai点的合外力矩：

(17)

其中,ZiMAi ,x支链坐标系下机架作用于支链i的x方向的约束力矩。

根据支链欧拉方程[10]，有：

(18)

其中,I1iy和I2iy分别为上支链和下支链关于质心的主惯性距在yi轴方向的分量。

由式(18)可得动平台对支链x方向的作用力：

(19)

3.2 平台动力学分析

动平台受支链反作用力、自身重力、动平台所受外力FCW及动平台所受外力矩MCW。

平台力平衡方程为：

(20)

根据平台欧拉方程，有：

(21)

由式(20)和式(21)可得：

(22)

其中：

因此，可得动平台对支链i，在z方向和y方向的作用力：

(23)

由支链力平衡方程可得电机驱动力和约束力矩：

fi=m1a1i·ni-ifAi,z-m1G·ni

(24)

Mi=MAi,x=liifAi,y

(25)

二维精密平台驱动力为：

(26)

3.3 ADAMS动力学仿真分析

利用ADAMS软件对Tripod平台进行运动学及动力学仿真，并得出各杆轴向驱动力的理论变化曲线。利用SolidWorks建立Tripod精密平台三维模型，通过ADANS/Exchange模块从外部导入Parasolid格式输入模型文件并对模型材料、质心、约束和仿真环境等参数进行设置。

首先，给定动平台中心施加驱动，仿真生成各移动副位移、速度、加速度驱动样条曲线；将驱动样条曲线导入ADAMS模型驱动设置中，分别进行仿真得到动力学模型驱动力变化曲线。给定动平台运动运动轨迹为：

图6为按照定动平台中心运动轨迹驱动得到的各驱动位移、速度和加速度误差曲线，与MATLAB计算结果位移误差在1%以内。将MATLAB各移动副位移样条曲线作为ADAMS仿真驱动函数，平台目标运动轨迹仿真结果如图7所示，与平台设定轨迹基本吻合，证明了运动学模型的正确性。

分别以位移样条曲线、速度样条曲线和加速度样条曲线作为驱动函数，经ADAMS仿真得到的Tripod平台3-RPS部分各超声电机驱动力随时间变化的曲线如图8所示，速度和加速度驱动函数下得到的驱动力连续稳定。支链2驱动力ADAMS仿真结果与MATLAB结果对比曲线如图9所示，可知两者总体吻合良好，数值误差很小。由于MATLAB分析针对的是简化后的理想模型，与实际模型本身存在一定误差，故仿真结果与理论值存在一定误差是合理的，通过ADAMS动力学仿真证明了动力学分析的正确性，为动力学前馈补偿控制奠定基础。

(a)驱动位移误差曲线 (b) 驱动速度误差曲线 (c) 驱动加速度误差曲线图6 移动副位移、速度及加速度曲线对比

(a)驱动位移曲线 (b) 平台位移曲线 (c) 平台旋转曲线图7 ADAMS仿真驱动位移及平台目标运动轨迹

(a)位置驱动模式 3-RPS驱动力 (b) 速度驱动模式 3-RPS驱动力 (c) 加速度驱动模式 3-RPS驱动力图8 支链压电马达驱动力曲线

图9 支链2驱动力仿真结果对比

4 控制系统开发

采用基于Windows软硬件平台的支持PLC语言及C++混合编程的实时控制软件TwinCAT 3，实现对I/O模块和驱动器的实时控制。该软件PLC包含丰富的运动控制库可用于轨迹规划及对NC轴控制，此外C++编程可实现PLC不能实现的复杂算法的开发。

利用TwinCAT C++调用newmat库实现Tripod平台运动学及动力学算法，将运动学及动力学工程文件生成可以集成到TwinCAT3环境的具有输入输出变量的C++语言的TcCOM模型。

TwinCAT NC PTP把一个电机的运动控制分为三层：PLC轴、NC轴和物理轴。NC轴如果关联到物理轴，就是实轴，如果不关联到硬件，就是虚轴。PLC中的运动控制程序并不区分虚轴和实轴，在实际调试的阶段前采用虚轴进行控制系统实验。

在TwinCAT NC中添加关节轴虚轴，Tripod平台在笛卡尔坐标系下具有X、Y、Z、A、B 这5个自由度，需控制5个直线超声电机。给定的X、Y、Z、A、B位姿作为输入变量，经动力学TcCOM模块计算，得到驱动位置、速度、加速度和驱动力，作为关节轴控制输入信息，调用运动控制库中的凸轮、点动等功能块，实现平台点对点运动及连续循环运动。Tripod控制系统HMI界面如图10所示。

图10 Tripod控制系统HMI界面

将Tripod平台运动轨迹文件导入系统软件，进行连续运动测试。图11为虚拟轴运动轨迹，与图7a驱动位移曲线对比，可知虚拟轴运动轨迹与满足理论分析结果一致。通过测试Tripod平台控制系统可实现单轴点动、点对点定位运动、按照预置轨迹运动等操作，为 Tripod 精密平台精密运动控制奠定基础。

图11 虚拟轴运动轨迹

5 结论

(1)基于牛顿欧拉法对支链及平台进行动力学分析，构造6×6方阵，可求解对Tripod 混联机构的动力学逆解。

(2)通过ADAMS仿真分析结果与MATLAB计算结果对比，运动学分析计算误差在1%以内。仿真结果与平台设定轨迹基本吻合，证明了运动学模型的正确性。简化后的理想模型，与实际模型本身存在一定误差，动力学仿真结果与理论值存误差。在位置驱动模式下驱动力输出存在波动，证明在位置环控制模式下，加入动力学前馈补偿控制的必要性。

(3)通过 TwinCAT 3 开发Tripod精密平台控制系统，实现平台按照预定轨迹运动，为Tripod精密平台精密运动控制奠定基础。