2-TPR/2-TPS并联机构的精密运动控制*

陈明方，张祥，胡康康，陈中平，张永霞

昆明理工大学机电工程学院，云南 昆明 650031

并联机构是一个高度非线性的多输入多输出系统，各关节间存在着耦合惯性和干扰，且随时变位姿参数而变化。动平台位姿难以测量，难以搭建全闭环控制系统等因素均会影响系统的控制精度和响应特性［1-3］。为了提高并联机构的轨迹跟踪精度和动态性能，国内外学者针对其控制系统做了大量研究工作。鲁开讲等［2］提出基于模型的参考自适应控制策略，将平面3自由度并联机构运动过程中的参数变化和未建模误差视为干扰信号，并引入前馈和反馈环节，提高了动平台轨迹跟踪精度和抗干扰能力。崔学良等［4］针对冗余驱动并联机构中冗余支链力阻抗控制模型随位姿变化的问题，设计了一种模糊PID控制器，与非冗余驱动支链的位置控制模型组成整机的力/位混合控制模型，实现了对冗余驱动并联机构的柔顺性控制。赵劲松等［5］针对液压并联机构多维力加载系统非线性、时变和强耦合的系统特性，提出了一种模糊PID与小脑模型神经网络并行的控制策略，并通过实验证明输出信号与指令信号几乎没有力/力矩幅值衰减与相位滞后，控制精度明显提高。孔德庆等［6］针对一种3 自由度平动并联机构因模型简化和外部干扰对控制系统的不确定性影响，在考虑交流伺服电机动力学特性的基础上设计了一种鲁棒轨迹跟踪策略，仿真表明该控制策略可有效抑制其不确定影响，保证系统稳定性。高国琴等［7］针对虚拟轴机床并联机构系统模型复杂且准确建模难度大的问题，提出了一种新型自适应动态滑模控制方法，通过仿真和试验证明其控制系统具有较好的自适应能力、较强的鲁棒性和良好的动静态性能。

PID 控制具有控制简单、无需对被控对象建模、易于实现、可靠性高等优点，广泛应用于机器人运动控制中。因并联机构具有高度非线性、难以求解精确数学模型的特点，PID 控制在并联机构控制中具有良好的适用性。随着智能控制理论的发展，与智能算法结合的先进PID控制展现了优越的控制性能［8-10］。冯李航等［11］以Delta 系列机器人为对象，对并联机器人控制策略进行了综述，指出模糊控制在非线性、大干扰、无明确数学模型的并联机构控制上具有显著优势。本文以2-TPR/2-TPS 并联机构［12］为研究对象，考虑系统动态性能、各支链丝杠驱动力矩的时变特性以及直流无刷电机变负载条件下的运动特性等因素，搭建了基于SIMSCAPE的Electronics-Multibody速度-位移双闭环联合仿真控制系统，进行了传统PID、分段PID和自适应模糊PID控制的仿真与分析。

2-TPR/2-TPS 并联机构是一种新型空间4 自由度非对称并联机构， 其结构如图1 所示。R1、R2、S1、S2分别为动平台的两个转动副和两个球副，T1、T2、T3、T4分别为定平台的4 个虎克铰，P1、P2、P3、P4为连接动平台和定平台的4 条移动副支链，定坐标系O1x1y1z1原点O1和动坐标系O2x2y2z2原点O2分别位于线段T1T2和R1R2的中点，y1轴和y2轴分别和两线段重合，z1轴和z2轴分别与定平台和动平台所在平面垂直。

图1 2-TPR/2-TPS并联机构简图Fig.1 2-TPR/2-TPS parallel mechanism diagram

实物样机采用伺服电机驱动丝杠的方式控制4条支链的运动，实物模型如图2所示。

图2 并联机构实物样机Fig.2 Parallel mechanism physical prototype

1.1 路径规划

机器人末端路径规划包括3 个任务［13］：定义末端执行器在两点之间的一条几何曲线；
定义两个方向之间的旋转运动；
定义坐标变化在两个给定值之间的一个时间函数。

对并联机构进行路径规划，不仅要保证动平台根据期望位姿进行运动，还要避免动平台加速度突变带来的关节冲击和机构抖振，使动平台按期望轨迹精确平滑地运动［10］。基于运动学反解的关节空间控制策略通过控制驱动杆跟踪期望杆长位移曲线来间接跟踪动平台期望位姿，驱动杆位移跟踪精度直接决定了动平台运动精度。为了使动平台在一定时间内精确地跟踪期望轨迹，且各个位姿分量的速度、加速度曲线平滑，需要对动平台运动轨迹进行合理的路径规划，并通过运动学反解映射到关节空间，作为并联机构期望输入信号。

2-TPR/2-TPS 并联机构动平台的6 个位姿参数具有耦合性［14］，进行路径规划时要根据对动平台位姿的变化需求选取相应的独立位姿参数。为了验证其在笛卡尔坐标系中的平动能力，选取[x，y，z，θx]为动平台独立位姿参数，并在其工作空间［15］内规划一条圆形路径，轨迹方程为

式中x(t)，y(t)，z(t)为动坐标系原点O2相对于定坐标系原点O1的坐标分量；
r0为圆轨迹半径；
θ(t)为半径r0与定坐标系x1轴的夹角；
θx(t)，θy(t)，θz(t)分别为动坐标系相对于定坐标系x1轴、y1轴、z1轴的旋转角度。

在此路径中，仅x(t)和y(t)是时变量，为了避免动平台的速度、加速度突变，需对θ进行边界条件限制，其边界条件为

式中θt0为起始时刻θ值；
θ˙t0为起始时刻速度值；
θ¨t0为起始时刻加速度值；
θtf为停止时刻θ值；
θ˙tf为停止时刻速度值；
θ¨tf为停止时刻加速度值。

五次多项式路径可以满足式（2）所示边界条件，设θ(t)的表达式为

将式（2）代入式（3），可求得θ(t)和式（1）所示动平台轨迹方程的时变函数。将式（1）代入2-TPR/2-TPS 并联机构反解方程［14］，求得4 条驱动支链的位移时变函数及速度时变函数，作为控制系统期望输入信号。

1.2 丝杠驱动力矩分析

2-TPR/2-TPS并联机构各支链滑块通过虎克铰与定平台连接，采用滑块相对位置固定、丝杠旋转的设计方案控制各支链的长度。机构运动过程中，各支链电机负荷力矩会随着丝杠驱动力矩和丝杠角加速度的变化而变化［16-17］，整个机构是由4个子系统组成的变负载系统。

丝杠驱动力矩TD是丝杠总轴向负载Fa的函数，如式（4）所示

式中TD为丝杠驱动力矩；
Fa为总轴向负载；
Ph为丝杠导程；
η1为丝杠和滑块之间的传动效率。

电机总负荷力矩TM为

式中TM为电机总负荷力矩；
JM为驱动电机轴上的转动惯量；
ω˙为丝杠进给角加速度；
TPL为滚珠丝杠阻力矩；
Tf为滚珠丝杠副摩擦力矩；
ib为电机轴与丝杠间减速转置传动比；
η2为电机轴与丝杠间减速转置传动效率。

并联机构控制分为路径规划级控制和驱动执行级控制。路径规划级控制本质上是基于机构反解，根据末端执行器的期望路径求解各关节理论运动指令；
驱动执行级控制则是基于各关节理论运动指令，精准、快速完成实际驱动系统的伺服控制［1］。

基于动力学的控制策略对并联机构动力学模型的准确性要求较高，对于复杂的并联机构而言，建立精确动力学模型比较困难，由简化模型得出的动力学反解，其关节信号与理论值存在偏差，必须通过复杂的控制算法补偿，控制系统复杂且不易达到理想控制效果。基于运动学的控制策略通过机构运动学反解，可以得到关节驱动信号解析解，验证后可直接作为驱动系统期望输入信号，将并联机构精度控制问题简化为对驱动执行级的伺服系统误差控制问题。

2-TPR/2-TPS并联机构通过伺服电机驱动，为了满足动平台及丝杠运动过程中的精度和动态性能要求，采用速度驱动方式，进行速度-位移双闭环反馈控制。控制系统总框图如图3所示。

图3 控制系统总图Fig.3 General block diagram of control system

基 于SIMSCAPE 的Electronics-Multibody 联 合仿真控制系统如图4所示。

图4 Electronics-Multibody总仿真模型Fig.4 Electronics-Multibody total simulation model

2.1 分段PID控制

分段PID 控制器由多个传统PID 控制器组成，每个子PID控制器参数需要人为整定，并根据触发条件选择相应的子PID控制器接入控制回路中，以实现PID控制器的自适应参数调整，满足系统在多状态下的稳定、快速和准确性要求，使系统总体性能达到最优，其运行机制表示为

2.2 自适应模糊PID控制

针对2-TPR/2-TPS 并联机构运动控制过程中由位姿变化、路径规划函数、支链耦合等因素导致的系统动态特性时变的问题，需引入智能算法进行在线参数辨识，实时修改控制器参数以达到最优控制效果。自适应模糊PID控制器能根据误差信号e和误差变化率e˙的实时变化，基于模糊控制规则实时修改PID参数，以适应系统动态特性的变化。模糊PID控制器分为常规式和增量式，常规式直接将模糊控制器输出作为最终PID参数；
增量式在初始PID值的基础上，将模糊控制器输出值和初始值相加，得到最终PID 参数［18-19］，二者模糊规则不同。为了和传统定PID控制器进行对比，选用增量式模糊PID 控制器，在相邻采样时间间隔内，Kp(t)、Kd(t)、Ki(t)和Δkp(t)、Δki(t)、Δkd(t)之间的关系如式（7）所示，其控制流程图如图5所示。

图5 增量式模糊PID控制原理框图Fig.5 The block diagram of incremental fuzzy PID control principle

式中Kp(t)，Ki(t)，Kd(t)为第t次采样时系统最终PID 参数；
Δkp(t)，Δki(t)，Δkd(t)为第t次采样时模糊PID 控制器输出参数；
Kp(t- 1)，Ki(t- 1)，Kd(t- 1)为第t-1次采样时系统最终PID参数。

2.2.1 数据模糊化 模糊控制器的输入和输出都是具有实际物理意义的控制信号，但模糊控制器进行运算时需要将输入信号变换到一定尺度范围内，输出信号时也需要将定尺度范围内的计算结果变换为实际物理信号，这两个过程分别为数据的模糊化和去模糊化。输入量e、e˙和输出量u称为模糊系统的语言变量，语言变量实际取值范围称为系统的基本论域。基本论域进行尺度变换后得到系统模糊论域，用以计算模糊向量，进行模糊运算。

为了提高调节精度，e、e˙、Δkp、Δki和Δkd的模 糊 论 域 分 别 取[-3，3]、[-3，3]、[-0.3，0.3]、[-0.06，0.06]、[-0.03，0.03]，模 糊 集 合 划 分 为7 级，各语言变量的模糊语言值集合均表示为

2.2.2 隶属度函数选择 隶属度函数的形状对模糊控制器的性能有很大影响，需根据误差大小、模糊概念界定等因素合理选用［20-21］。基于系统响应的快速性、稳定性和参数调节实时性的要求，在误差较大、模糊概念完整的区间，选用S型、Z型隶属度函数，避免参数突变，保证系统响应的平稳性；
在误差较小、具有中间模糊概念的区间，选用高斯型、三角型隶属度函数，参数调节较灵敏，确保系统响应的快速性。e、e˙、Δkp、Δki和Δkd的隶属度函数如图6～10。

图6 e隶属度函数Fig.6 Membership function of e

图7 e˙隶属度函数Fig.7 Membership function of e˙

图8 Δkp隶属度函数Fig.8 Membership function of Δkp

图9 Δki 隶属度函数Fig.9 Membership function of Δki

2.2.3 模糊规则 模糊控制规则是模糊控制的核心，基于专家经验和控制工程知识建立的模糊规则综合了实践经验和PID控制理论，具有广泛的使用场景［22］。本文选用的模糊规则如下：

图10 Δkd隶属度函数Fig.10 Membership function of Δkd

当|e|较大时，为使系统响应具有较好的快速跟踪性能，避免微分饱和及较大超调，应取较大的Kp和较小的Kd，令Ki= 0。

当|e|中等大小时，为使系统具有较小的超调量，应取较小的Kp，适中的Kd和Ki，保证系统的响应速度。

当|e|较小时，为使系统具有良好的稳态性能，应取较大的Kp和Ki。同时为了避免振荡，并考虑系统抗干扰性能，当|ec|较小时，Kd取中等值；
当|ec|较大时，Kd取较小值。

根据以上PID 参数调节规则，Δkp、Δki和Δkd模糊规则表［19，23］如表1所示。

表1 Δkp、Δki、Δkd模糊规则表1）Table 1 Δkp、Δki、Δkd fuzzy rule table

2.2.4 去模糊化 模糊推理结果需进行去模糊化，转化为模糊论域的精确量，再经尺度变化转换为基本论域中的实际控制量，才能作用于控制系统中。常用的去模糊化方法有最大隶属度法、加权平均法、重心法和中位数法。其中，重心法的输出推理机制更加平滑，信号调节灵敏度更高［20-21］。为了避免调节过程中的参数突变，采用重心法进行去模糊化。重心法取隶属度函数曲线与模糊论域轴之间的面积重心值作为该语言变量的输出值。对于连续论域，输出值为

式中z0为语言变量c的第i个模糊集合对应的模糊论域输出值；
μCi(z)为语言变量c的第i个模糊集合的隶属度函数；
z为隶属度函数的自变量；
a为第i个模糊集合隶属度函数对应的论域积分下限；
b为第i个模糊集合隶属度函数对应的论域积分上限。

取r0=50 mm，z0(t)=320 mm，θx0(t)=0 rad，代入式（1），由反解方程［14］得4 条驱动支链的位移时变函数及速度时变函数。丝杠运动轨迹由两部分组成，0～5 s 区间，丝杠由机构初始位置运动至目标轨迹起点位置；
5～20 s区间，丝杠由目标轨迹起点运动至终点。4 条驱动支链丝杠位移、速度、加速度曲线如图11～13所示。

图11 驱动支链位移Fig.11 Drive chain displacement

图12 驱动支链速度Fig.12 Drive chain speed

图13 驱动支链加速度Fig.13 Drive chain acceleration

4 条驱动支链电机负载力矩曲线如图14 所示。综合考虑转速跟踪精度、机构结构特点、电机末端负载等因素，选用最大扭矩为最大负载2倍的伺服电机进行仿真。电机扭矩-速度曲线如图15所示。

图14 驱动支链电机负载Fig.14 Drive branch motor load

图15 伺服电机扭矩-转速包络线Fig.15 Torque-speed envelope for the servomotor

基于临界比例度法进行PID参数整定，并进行适当调整，分别取速度环PID 参数kp= 0.005，ki= 0.001，kd= 0.000 5，位置环kp= 1。为了减小参数整定造成的不稳定性，降低分段PID控制器的复杂度，将误差e分为三段，分别整定对应的PID参数，其对应关系如式（9）所示。

根据传统PID仿真的速度误差和速度变化率误差结果，取e和e˙的实际论域为[-7，7]、[-9，9]，在多次调节并进行效果对比的基础上，取Δkp、Δki和Δkd的实际论域为[0.012，0.018]、[-0.001，0.001]和[0，0.005]。

传统PID 控制、切换PID 控制和自适应模糊PID 控制下，动平台轨迹跟踪效果如图16 所示。传统PID控制下，在丝杠反向旋转瞬间，由于转速为零且负载方向突变，电机需要较长时间达到期望转速，出现了明显的轨迹偏差。分段PID 控制下，在不同的误差区间切换相匹配的PID参数，轨迹偏差明显减小。自适应PID 控制器调节作用下，PID 参数随着误差变化实时修正，在负载突变时轨迹偏差大幅减小，实际轨迹与期望轨迹重合度较高，体现了良好的动态性能。

图16 动平台轨迹跟踪曲线Fig.16 Tracking curve of moving platform

为了更直观地对比三种控制策略的调节效果，将三种控制策略下的末端轨迹半径值和期望轨迹半径值进行比较，得出半径误差曲线。如图17 所示，传统PID 控制下，误差峰值为-0.96；
分段PID 控制下，误差峰值为0.51；
自适应模糊PID 控制下，误差峰值为0.47。相比传统PID 控制器，分段PID 控制下的误差峰值减小了47%，自适应模糊PID 控制下的误差峰值减小了51%。虽然分段PID 控制下的误差峰值和自适应模糊PID 控制下的相近，但是在误差调节过程中的快速性较差，如图17 所示，前者在两个误差较大的轨迹范围内出现了较长时间的轨迹偏移，自适应模糊PID控制器作用下的轨迹半径误差波动幅度较小，综合性能最优。

图17 动平台轨迹半径误差Fig.17 Trajectory radius error of moving platform

各支链丝杠位移误差如图18 所示，5～20 s 为各支链进行目标轨迹跟踪的运动区间，其位移跟踪精度直接决定了末端轨迹跟踪精度。如图11～14所示，5～20 s 区间内，支链1 和支链2 运动轨迹较平缓且电机负载相对较小，三种控制策略下的误差均较小；
支链3 和支链4 运动轨迹变化幅度较大且电机负载较大，三种策略的控制效果差异较大。传统PID 控制下，支链3 和支链4 误差峰值分别为3.3 和2.8 mm；
分段PID 控制下，误差峰值均为1.5 mm；
自适应模糊PID 控制下，误差峰值分别为0.9和1.2 mm。相对于传统PID，分段PID 和自适应模糊PID 下的支链3 位移误差分别减小了56%和72%，支链4 分别减小了46%和57%。自适应模糊PID下的支链位移误差曲线峰值更小，波动幅度更小，体现了良好的动态跟踪性能。

图18 丝杠位移误差Fig.18 Lead screw displacement error

通过对2-TPR/2-TPS 并联机构的运动学仿真，可得如下结论：2-TPR/2-TPS并联机构是一个受动平台时变位姿、驱动丝杠角加速度、支链耦合等因素影响的多输入多输出变负载系统，负载变化和驱动丝杠反向旋转均会降低转速跟踪精度，影响动平台轨迹跟踪精度。相比传统定PID 控制策略，分段PID 在不同的误差区间切换相匹配的PID控制器，具有一定的动态调节能力，自适应模糊PID 控制策略能根据电机转速误差e和误差变化率e˙，实时修正PID 参数，适应负载变化，末端轨迹跟踪精度较高，对负载时变的并联机构具有较好的控制效果。