吴战武，马 可，袁 潇，许开州

(1.上海航天技术研究院, 上海 201109； 2.上海航天化工应用研究所, 浙江 湖州 313002)

火箭是国防科技的核心技术之一，固体发动机为火箭提供强大动力，火箭的性能取决于固体发动机质量的好坏[1-3]。氧化剂作为火箭固体发动机必不可少的原料，在固体火箭发动机生产过程中，需要考虑氧化剂原料的运输和转运方式，确保生产过程的安全性和高效性[4]。传统的发动机氧化剂转运主要是由人工与机器辅助共同完成，效率低下，存在危险性，难以满足生产线要求；又因氧化剂具有易燃易爆的化学性质，故转运过程中容易对人身安全造成隐患[5-6]。

龙门桁架机器人大范围直线运动采用齿轮齿条传动，具有运动行程大、结构刚度高、负载能力强、性能稳定等优点[7-9]，因而采用桁架机器人进行固体火箭发动机氧化剂料桶的快速上下料，可满足上下料运动范围大和生产节拍快的要求。

氧化剂上下料龙门桁架机器人跨距较大，由于机械结构限制驱动机构只能位于一侧，采用单电机伺服驱动的方式，其负载能力(驱动力矩)不能满足要求，因而采用采用双电机驱动的方式可均匀机构负载，并有效提高系统的负载能力与动态响应性能。固体火箭发动机氧化剂料桶高效上下料对运动速度和位置精度要求较高，而电机机械电气件存在离散性，难以保证较好的高速同步运动等性能。

本文为提高伺服系统的控制性能和同步运动位置精度，以固体火箭发动机氧化剂X轴双电机驱动伺服运动单元为研究对象，提出基于速度和转矩的电机混合驱动控制方法，构建双驱动控制系统模型，并通过仿真验证了控制算法的有效性，从而保证氧化剂料桶的高效快速精准上下料。

1 X轴双电机驱动机构设计及工作原理

龙门桁架机器人用于固体火箭发动机生产线氧化剂上下料，具有X向、Y向、Z向直线运动和旋转动作4个自由度，并具备氧化剂料桶的抓取以及翻转倒料功能。龙门桁架机器人结构如图1所示。

图1 龙门桁架机器人

机器人的X轴运动是由双伺服驱动单元驱动完成，两伺服驱动单元电机分别位于X轴移动滑台的两侧，并分别配置有伺服电机。伺服电机经减速器与齿轮连接，通过齿轮齿条啮合传动，并通过导轨滑块机构作导向。桁架机器人的X轴伺服驱动单元的传动结构如图2所示。

图2 桁架机器人X轴伺服驱动单元传动结构

在氧化剂上下料过程中，运动负载较大，对于X轴的运动速度和位置运动要求也较高，X轴运动机构采用双电机伺服驱动结构(转矩伺服驱动单元和速度伺服驱动单元)，传动机构机械结构均相似，通过对驱动电机进行速度和转矩的混合驱动控制，保证氧化剂上下料过程中X轴运动的同步性及动态运动特性。采用双电机伺服驱动结构可以取得较好的运动性能，首先由于生产节拍对运动速度的要求更高，包括运动速度的最大值及速度精度等，对运动速度的控制是主要矛盾；其次，位移可通过速度的积分得到，较好的速度控制精度可以保证较好的位置控制精度；最后，采用X轴机构采用双电机驱动，采用位置控制难以保证运动的同步性，由于运动速度较大，同步运动在较短的时间内会产生较大偏差，导致X轴运动卡滞，严重会导致X轴机构损坏。

固体火箭发动机氧化剂上下料桁架机器人X轴速度与转矩混合控制的工作原理为：对上下料运动的X轴运动速度和加速度进行规划，将X轴速度指令通过机构运动学转化为速度控制侧电机角速度指令，同时将该指令经过转换后作为伺服电机的转矩控制指令，X轴驱动单元上位机接收到运动目标指令后，通过闭环控制驱动伺服电机运动，速度控制侧伺服电机通过编码器进行速度闭环反馈，当系统运动状态发生变化后，作用于转矩控制侧电机的负载力矩也发生变化，该负载转矩通过机构动力学模型实时计算得到，进而进行电机转矩实时控制。X轴双电机驱动机构两伺服电机均通过整定PID调节器参数进行运动性能调节，X轴双电机驱动机构在混合驱动控制方法下进行同步快速运动，从而保证氧化剂的快速精准上下料。

2 速度伺服驱动单元建模

设氧化剂料桶上下料规划速度和加速度分别为Vr0和ar0，速度控制侧减速比分别为is，齿轮分度圆直径为D0，直线运动的速度为Vs，电机编码器反馈角速度为ωr。为保证X轴直线运动的同步性，速度控制侧驱动电机的控制目标ωs为

(1)

速度伺服驱动单元电机的模型包括电机开环模型、PWM驱动器模型及速度、电流双闭环模型。

2.1 电机基本数学模型

交流伺服电机实质上是一个由定子和转子两部分组成的感应电动机，可通过等效的方法将该伺服驱动单元进行简化[10]。设伺服电机电枢电压为U，电枢回路总电阻为Rr，电机回路总电感为Lr，反电动势系数为Ke1，电机转矩系数为Kt1，电机阻尼系数为Br，则根据电机电压方程和磁链方程可得到速度控制侧伺服电机的电流环控制框图如图3所示。

图3 驱动电机电流环控制框图

由上述控制框图可得电机电流环控制模型传递函数为

(2)

式(2)中:θr为速度伺服驱动单元电机输出角位移;Jr为等效转动惯量。

2.2 PWM驱动环节

PWM脉宽调节装置由脉宽调制器和PWM变换器组成[9]，虽然PWM调制的频率很高，但是PWM的变换电压要到下一个周期才能改变，因此PWM脉宽调节装置相当于延时环节。

设PWM脉宽调节装置的开关频率为fpwm，对应的周期TPWM即为延时时间，其传递函数为

GPWM=KPWMe-TPWMs

(3)

(4)

其中，KPWM为放大系数，ud为PWM变换器的输出电压，uc为脉宽调制器的控制电压。

由于上式中包含指数函数，使得系统为非最小相位系统。为便于分析，将其按泰勒级数展开，由于分母的高阶泰勒展开式相对于分子是高阶无穷小，故可得出近似值：

GPWM=KPWMe-TPWMs=

(5)

由于PWM周期很小，常用的开关频率为10 kHz，因此忽略上式中的高阶项，PWM脉宽调节装置近似为一阶惯性环节，其传递函数为

(6)

2.3 双闭环电机控制模型

速度伺服驱动单元采用外环速度和内环电流双闭环控制结构，其中转速调节器是调速系统的主导调节器，它使转速很快跟随给定电压变化，稳态时可减小转速误差，对负载变化起抗扰作用，其输出限幅值决定电动机允许的最大电流；电流调节器为内环调节器，其作用是使电流紧紧跟随给定电压变化，对电网电压的波动起及时抗扰的作用，并保证获得电机允许的最大电流，从而加快动态响应过程。

速度伺服驱动单元双闭环调节器均采用PID控制器，其双闭环控制框图如图4所示。

图4 速度伺服驱动单元双闭环模型

3 转矩伺服驱动单元建模

转矩伺服驱动单元采用转矩闭环控制结构，在转矩伺服驱动单元接收到运动指令后，先将其转化为力矩指令，再实时计算作用于转矩伺服驱动单元电机上的扰动力矩，通过整定PID参数改善转矩伺服驱动单元控制性能，从而满足固体火箭发动机氧化剂上下料同步运动的要求。

3.1 电机扰动转矩建模

电机扰动转矩模型是转矩伺服驱动单元建模的基础，由于扰动转矩随着X轴机构的运动状态实时变化，因而需要通过机构运动动力学获取该扰动转矩。设氧化剂料桶及X轴运动组件的质量为M0，转矩控制侧减速器减速比为it，转矩控制侧驱动电机的扰动转矩为Ttd，则通过X轴双电机驱动机构动力学方程可得到Ttd的表达式如下：

(7)

(8)

其中，Vr0采用对X轴双电机驱动机构无刚性冲击和柔性冲击的五次多项式规划，其在运动初始时刻t0和结束时刻t1满足：

(9)

设转矩伺服驱动单元电机的电流和转矩系数分别为It和Kt2，则转矩控制侧电机的输出转矩Ttt为

Ttt=Kt2It

(10)

3.2 微分控制器设计

由转矩伺服驱动单元电机扰动转矩模型可知，扰动转矩值与速度伺服驱动单元的直线运动速度的微分值相关，但在在氧化剂料桶上下料过程中，由于速度控制侧驱动电机的角速度反馈信号中存在较大的噪声和干扰，即使采用低通滤波器后滤波效果也难以达到运动要求，且直接进行速度差分容易引入较大运算干扰，甚至引起角速度信号的失真，造成转矩伺服驱动单元运动震荡甚至控制发散。

离散型跟踪微分器可实现对信号的无颤振快速跟踪和微分，具有较好的滤波去燥效果和较高的稳态精度[11-12]，因而在转矩反馈回路中使用跟踪微分器对角速度微分信号进行提取，离散型跟踪微分器表达式为

ωr0(n+1)=ωr1(n+1)+r0·h·ωr2(n+1)

(11)

ωr1(n+1)=ωr1(n)+h·ωr2(n)

(12)

ωr2(n+1)=ωr2(n)+h·fstr(ωr1(n)-

ωr(n),ωr2(n),ωr2(n),r1,h1)

(13)

其中，ωr0为滤波后的角速度指令，ωr1(n+1)和ωr2(n+1)分别为ωr的跟踪信号及微分信号，r0、r1、h和h1均为可调参数，fstr函数的定义如下：

fstr=-r1·satr(gr(n),δr)

(14)

δr=h1·r1，δr1=h1·δr1

(15)

er(n)=ωr1(n)-ωr(n)

(16)

yr(n)=er(n)+h1·ωr2(n)

(17)

(18)

(19)

其中，r0为跟踪步长；r1为速度因子，其决定了信号的跟踪快慢，其值越大跟踪速度越快；h为积分步长，其值越大跟踪精度越高，同时系统计算时间也越长；h1为滤波因子，选取合适的h1值能过滤信号噪声，获取良好的滤波效果；fstr为最速控制综合函数；er为跟踪信号与原始信号的误差。

3.3 转矩伺服驱动单元控制模型

在转矩伺服驱动单元电机控制模型的基础上，将速度伺服驱动单元控制模型中的Vs及微分控制器结合形成电机扰动转矩，并在此基础上增加PID调节器和一阶超前校正器Gc(s)，构建转矩伺服驱动单元的控制模型，其控制框图如图5。

图5 转矩伺服驱动单元的控制框图

图5中，微分控制器通过实时跟踪速度伺服驱动单元电机转速反馈信号，并提取其微分信号，一阶超前校正器与PID调节器串联，用于提高转矩伺服驱动单元的快速响应。

4 仿真验证

采用五次多项式规划X轴双电机驱动机构的运动速度，其包括加速、匀速和减速过程，X轴双电机驱动机构的运动速度和加速度曲线如图6所示。

图6 X轴双电机驱动机构运动速度和加速度曲线

图6中，X轴双电机驱动机构的直线运动行程为7.45 m，最大直线运动速度和角速度分别为0.5 m/s和0.19 m/s2，通过传动结构参数计算得到速度控制伺服电机的最大运动角速度和角加速度分别为238.10 rad/s和90.48 rad/s2。

将速度伺服驱动单元模型、转矩伺服驱动单元模型与X轴双电机驱动机构运动负载连接即可得到双电机驱动系统的控制系统模型。X轴双电机驱动机构Simulink仿真模型如图7所示。

图7 X轴双电机驱动机构Simulink仿真模型

图7中，X轴双电机驱动机构速度和角速度的.mat数据通过From Workspace模块导入；微分控制器的Simulink模型如图8所示。

图8 微分控制器的Simulink模型

由于在机构真实运动中，伺服电机反馈角速度指令会混杂干扰，因而选用白噪声作为角速度信号的噪声，设定白噪声的能量谱密度为0.008，相关时间为0.005 s。为验证所设计微分控制器的效果，选取正弦角信号作为伺服电机的原始角位移反馈信号，并在其基础上叠加白噪声干扰，叠加后的电机角速度信号如图9所示。

图9 叠加干扰后的伺服电机角速度信号

通过微分控制器对上述角速度信号进行微分并滤波，调整跟踪微分器各参数，当h=0.005，h1=8h、r=1 200时，速度伺服驱动单元电机的角加速度信号如图10所示。

图10 速度伺服驱动单元角加速度信号

将调整好后的微分控制器参数带入X轴双电机驱动Simulink仿真框图中，并对速度伺服驱动单元和转速伺服驱动单元双并闭环PID调整器进行整定，得到两电机的速度同步曲线如图11所示。

图11 两电机速度同步曲线

由图11可得，转矩伺服驱动单元电机角速度ωrt存在明显滞后，两伺服电机最大角位移偏差Δωr最大为42.01 rad/s，无法满足X轴双电机驱动机构同步运动要求。在转矩伺服驱动单元控制模型中设计超前校正环节，其传递函数Gc(s)如下：

(20)

调节超前校正Gc(s)参数c1和c2，当c1=0.8、c2=0.1时，转矩伺服驱动单元电机角速度响应明显提高，校正前后转矩伺服驱动电机角速度及两电机角速度偏差如图12。

图12 校正前后转矩伺服驱动单元电机角速度及两电机角速度偏差

由图12可知，校正后两驱动单元电机角速度最大偏差为0.39 rad/s，当it=1∶25时，X轴双电机驱动机构的最大速度偏差为8.10×10-3m/s，两驱动单元具备较强的动态响应性能和较高的速度同步精度。

将双驱动系统的速度偏差在全运动行程范围内进行积分，即可得到X轴双电机驱动机构的同步位置偏差ΔS，同步位置偏差随时间的变化关系如图13所示。

图13 X轴双电机驱动机构的同步位置偏差

由图13可知，在上下料加速运动阶段同步位置偏差随着时间增加先增大后减小，在匀速运动阶段同步位置偏差不断增大，在减速阶段位置偏差先减小后逐步增大，并在上下料结束时刻偏差达到最大值。由ΔS随时间变化曲线可知，X轴双电机驱动机构全运动行程范围内的最大位置同步误差为3.05 mm，具有较高的位置同步运动精度，同时也验证了速度与转矩混合控制方法的有效性。

5 结论

本文针对固体火箭发动机氧化剂料桶上下料的快速精准要求，设计了大跨距龙门桁架机器人，以机器人X轴双电机驱动机构为基础，提出基于速度和转矩的混合驱动控制方法，分别建立了速度伺服驱动单元和转矩伺服驱动单元的控制模型，并最终搭建了X轴双电机驱动机构的仿真模型。通过Simulink仿真结果可知，X轴双电机驱动机构具有较好的速度同步性及较高的位置运动精度，可实现固体火箭发动机氧化剂料桶的高效快速精准上下料，同时也验证了速度和转矩混合驱动控制方法的有效性。