基于多项式插值的倒料机构协同运动规划方法

沈伟民，周岳松，张英杰，俞成蛟

(上海航天化工应用研究所，浙江 湖州 313000)

火箭是国防工业的基础产品之一，固体发动机作为火箭提供动力的核心部件，其质量好坏决定了火箭的性能[1-4]。在火箭发动机生产过程中，需要对氧化剂进行转运、装填。传统的发动机氧化剂装填主要是由人工来实现的，效率低下。因为氧化剂具有易燃易爆的化学性质，所以转运过程中有防爆、防潮、环境清洁等要求，物料的转运比较困难，对人身安全也带来了巨大隐患。国外主要发达国家的发动机倒料机构设备已具备很高的自动化、信息化和模块化水平。国内的设备相对落后，自动化水平较低，存在定位精度低，装填效率低等问题，为提高生产效率和装填精度，对一台四自由度跟随装填倒料机构的协同运动进行了规划[5-6]。通过对原料桶出料口进行矢量建模、建立末端出料口的运动方程。根据目标轨迹对每个自由度驱动电机进行五次多项式插值规划，通过翻转、升降、水平移动和旋转自由度驱动电机的协同运动确保氧化剂原料取料倒料操作的正确性和精准性。仿真分析结果表明倒料机构能够满足装药精度要求，对设备的冲击较小。

1 氧化剂原料倒料机构结构设计及工作原理

氧化剂倒料机构用于火箭发动机生产线上发动机氧化剂装填工作。需要完成氧化剂药筒的抓取以及随动装药工作。其具有X向、Y向直线运动和旋转、翻转回转运动四个自由度。倒料机构结构的三维模型如图1所示，旋转机构齿轮传动结构如图2所示。

设备的X向移动是由外部轴驱动完成，伺服电机经减速器与滚珠丝杠连接完成驱动与传动，导轨滑块机构实现导向。设备的Y向运动同样由固定于旋转轴顶部的伺服电机和滚珠丝杠通过连接摆动横梁实现。设备的旋转运动是由电机减速器驱动，齿轮啮合传动完成，从动齿轮与旋转轴底部相连，传动精确可靠。翻转运动通过电机减速器驱动，经齿轮齿条的传递至翻转机械手，从而实现翻转操作。

机构的刚度和精度是设计的关键指标，针对以上指标设计的倒料机构具有以下结构特点：

1)X向移动机构为外部轴形式，刚度大、运动空间大，有利用X运动的快速实现，提高运动精度。

2) 旋转和翻转机构采用低背隙的间隙机构和角度编码器实现位置双闭环控制，提高运动精度以及平稳性。

3) 设备四自由度之间采用串联结构形式，每个自由度都具有独立性、便于控制。

氧化剂倒料机构的工作原理：四自由度倒料机构外部轴与生产线体平行放置。四自由度机械手先抓取氧化剂药筒，放置生产线发动机装药口上端，初始位姿及各自由度参数可有位置及角度编码器测量。装药过程中为保证装药质量和精度，通过控制各自由度运动参数保证装药速率相同，出药口位置高度不变，且随发动机运动。

2 氧化剂原料倒料点位置建模

为了精确控制倒料机构的倒料精度和质量，必须对其建立运动方程来准确描述其空间位姿。常用的机器人运动描述采用D-H矩阵法和矢量法。由于本设备具有四个自由度，翻转机构的自由度参数已确定(为了保证倒料均匀，旋转角速度满足一定的变化规律)自由度较少，容易求解，所以采用向量法[7-8]。

装药时机械手需要夹持氧化剂药筒，抓取药筒示意图如图3。

Db和Hb分别为氧化剂料桶直径和高度，D为氧化剂出料位置，yi为夹手夹持点与料筒底部距离。

为保证氧化剂原料桶倒料口的空间位置满足目标轨迹路径，并尽量减小设备运动过程中的冲击，倒料机构X轴(水平移动)、Y轴(竖直运动)、旋转和翻转等自由度需要进行协同运动，下面对氧化剂原料倒料点位置进行建模，得到倒料过程中多自由度联动需要满足的运动关系，倒料机构的位置矢量关系在X-Z、X-Y方向的投影关系如图4、图5所示。

(1)

(2)

(3)

(4)

(5)

(6)

(7)

由以上公式可以得出原料口出料点的运动学方程：

Px=xi-Pri1cosθi-Pbi1sinθi=

xi-Lcosθi-Pbisinξicosφi

(8)

Py=Pr1sinθi-Pbicosθi=Lsinθi-Pbicosξicosφi

(9)

(10)

为了保证倒料的质量和效率，在生产之前对设备的目标轨迹进行了设计，并通过激光跟踪仪记录了目标末端轨迹S0。则X、Y、Z方向的轨迹方程可表示为S0x(x，y，θ，φ)、S0y(x，y，θ，φ)、S0z(x，y，θ，φ)。

3 电机运动轨迹规划

轨迹规划是轨迹跟踪控制的基础，对工业机器人的工作效率、平稳运行、降低能耗等均具有重要意义。通过氧化剂原料倒料点位置模型，可得到不同时刻倒料机构各自由度的位移，为了保证倒料过程的稳定性，防止氧化剂原料出现倒料过快、原料外倒现象，采用柔顺性能较好和运算效率较高的五次多项式运动插补算法对倒料机构各自由度电机的角位移轨迹进行规划，以避免驱动电机在取料和倒料过程中刚性冲击和柔性冲击过大。确定一个五次多项式插值函数需要有6个约束条件[9-11]。五次插值函数形式如下

S(t)=at5+bt4+ct3+dt2+et+f

(11)

以首末两位置处位移、速度、加速度为约束条件，其形式如下：

S(ti)=Si，S(tj)=Sj

S′(ti)=vi，S′(tj)=vj

S″(ti)=αi，S″(tj)=αj

(12)

由上两式可以确定求解矩阵：

(13)

其中，t∈[ti，tj]，a，b，c，d，e，f为插补轨迹因子，可通过起点位置和终点位置的边界条件得到，S1(t)、S2(t)、S3(t)和S4(t)分别为翻转运动、升降运动、水平移动和旋转运动电机角位移。

由翻转机构结构可知，驱动电机运动经由行星减速器和齿轮齿条的传递至翻转机械手，从而实现翻转操作，因而翻转运动电机角位移的轨S1(t)为：

(14)

其中，i1为翻转机构行星减速器减速比。

由Y轴移动结构和水平移动机构可知，驱动电机的旋转运动经过减速器和滚珠丝杠转化为升降柱的直线移动，升降机构的电机角位移S2(t)和水平移动机构电机角位移S3(t)为：

(15)

(16)

其中：i2和i3分别为升降机构和水平移动机构行星减速器减速比；d2和d3为升降机构和水平移动机构滚珠丝杠导程。

旋转自由度采用伺服电机驱动齿轮传动的方式实现旋转运动，设i4为齿轮减速比，则驱动电机角位移S4(t)的表达式为

(17)

4 仿真验证

首先在Simulink建立了系统规划的程序框图，如图6所示。

程序框图的原理为：对目标轨迹通过运动学求解，得到驱动电机位移，通过五次轨迹规划得到实际的各电机驱动位移，进而求出设备实际轨迹。

运行程序，可得到每个电机的速度、加速度曲线及末端位置的X、Y、Z方向上的轨迹曲线。如图7、图8、图9所示。

通过速度、加速度及末端位移曲线可知：通过协同规划之后，设备运动的速度最大为0.18 m/s，角速度为6 rad/s，最大加速度为0.06 m/s2，最大角加速度0.58 rad/s2，运动平缓，冲击较小。通过末端轨迹对比可知运动轨迹与目标轨迹之间误差较小，最大处为4.23 mm，在允许误差范围之内。

基于插值多项式建立的轨迹规划方法，误差来源主要由两部分原因。一是该方法能够准确设定确定位置处各电机的运动参数，中间位置的确定是基于确定位置点的运动参数插值模拟出来的，与实际的位置有一定偏差。二是串联四自由度机构的累计误差造成实际位置与理想位置有一定误差。这种误差与确定位置点的个数有关，已知位置点个数越多，误差越小。在无引入一些具体算法(如最小误差控制)时跟随误差较大，在大行程跟随中误差在3 mm左右，引入具体算法后误差较小，小于1 mm[12-13]。本研究的大行程轨迹跟随没有采取特殊跟随算法最大误差为3.24 mm，与国内水平相比，效果相当。

5 结论

针对火箭发动机氧化剂装填翻转设备跟随装填位置精度较差的问题，提出一种基于多项式插值的协同运动规划方法。先运用空间矢量叠加法建立原料筒出料口的矢量模型，求出出料口的运动方程，测定了目标轨迹，然后利用柔顺的五次多项式轨迹规划方法规划电机的驱动角位移，最后通过Simulink对建立的模型进行仿真分析，仿真结果表明该种方法可保证氧化剂原料落点位置处于不同接料目标区域内，实现对氧化剂原料倒料过程的柔顺及精准控制。