CN103809520B - 用于多轴联动动态修正插补位置的全闭环运动控制方法 - Google Patents

Abstract

本发明涉及一种用于多轴联动动态修正插补位置的全闭环运动控制方法。在第一个命令位置坐标上，根据采集到的各轴实际位置与电机实际位置之间的偏差，调整各轴命令位置；对调整后的各轴命令位置矢量在被加工轨迹上投影，得到投影长度，确定修正后的命令位置；当满足全闭环条件时，停止调整；当程序段处于混联且有后续运动段时，则将本运动段与下一运动段进行速度连接处理；否则，不对插补位置进行调整，直到当前运动段插补完成，再在系统提供的定位时长内，通过PID控制算法使轴的实际位置运动到命令位置上。本发明可以在满足多轴联动轮廓精度、定位精度、重复定位精度的同时，保证伺服电机稳定运行，保证工件加工精度且延长机床的使用寿命。

Description

用于多轴联动动态修正插补位置的全闭环运动控制方法

技术领域

本发明涉及到数控机床控制领域，具体地说是针对带外接位置传感装置反馈机床轴的多轴联动动态修正运动插补的全闭环运动控制方法。

背景技术

当今数控机床已被广泛应用，同时对数控机床多轴联动轮廓精度、定位精度、重复定位精度也日益提高，原来丝杠加编码器式的半闭环控制系统已不能完全满足轴复杂联动装置结构多样化所带来的运动控制要求。半闭环控制系统无法控制机床轴传动机构所产生的传动误差、高速运转时传动机构所产生热变形误差以及加工过程中传动系统磨损而产生的误差，而这些误差已经严重影响到数控机床的加工精度及其稳定性。光栅尺等外接传感装置对数控机床各坐标轴进行全闭环控制，消除上述误差，提高机床的定位精度、重复定位精度以及精度可靠性，作为提高数控机床位置精度的关键部件日益受到用户的青睐。但对于机械传动结构复杂、机械间隙较大或轴行程范围内传动线性较差的机床坐标轴来说，如果采用传统全闭环控制方式，虽然一定程度上能够满足轴定位精度、重复定位精度，但存在运动控制过程中伺服电机转速不平稳、易引起机床轴振动情况，一方面造成加工精度受到影响，另一方面还会造成加快机床传动装置的磨损。

随着现代制造业的迅速发展，机床结构也在不断发生变化，比如出现了带分配传动装置的伺服电机控制多机床坐标轴的机械结构，传统的运动控制方法已经不能很好的适用于此类机床。而此类机床传动结构复杂、机械间隙较大、轴行程范围内传动线性不稳定等，同样需要外界位置传感装置实现定位。

发明内容

针对现有运动运动控制的处理方法存在加工速度不平稳、极易产生机床振动等问题，严重影响了工件的加工精度并且降低了机床的使用寿命。本发明的目的是提供一种可根据光栅尺等外接位置传感装置及伺服电机编码器反馈动态修正运动轨迹插补位置，实现动态规划运动轨迹的方法。

本发明为实现上述目的所采用的技术方案是：一种用于多轴联动动态修正插补位置的全闭环运动控制方法，包括以下步骤：

在第一个命令位置坐标上，根据采集到的各轴实际位置与电机实际位置之间的偏差，调整各轴命令位置；

对调整后的各轴命令位置矢量在被加工轨迹上投影，得到投影长度，确定修正后的命令位置；

重复上述步骤，顺序调整其他命令坐标；

当满足全闭环条件，即满足零件加工的轮廓误差所要求的程序段最大剩余长度时，停止调整；

当程序段处于混联且有后续运动段时，则将本运动段与下一运动段进行速度连接处理；否则，不对插补位置进行调整，直到当前运动段插补完成，再在系统提供的定位时长内，通过PID控制算法使轴的实际位置运动到命令位置上，完成定位。

所述定位完成后或速度连接处理后，如果有后续运动段，则继续下一段加工，如果有增轴、减轴的情况，则通过PID控制算法使与下一运动段无关的轴运动到命令位置上，其他轴继续下一段加工；如果没有后续运动段，则各轴进入半闭环控制。

所述各轴实际位置通过在移动轴外接光栅尺、球栅尺，在旋转轴外接编码器实现。

所述根据采集到的各轴实际位置与电机实际位置之间的偏差，调整各轴命令位置，具体为：

各轴的偏差计算如下：

根据以上偏差，各轴调整后命令位置坐标计算如下：

其中，P(x,y),P'(x_m,y_m)分别为轨迹上命令位置坐标、各轴调整后命令位置坐标，P_Xaxis,P_Xmotor分别为X向轴的实际位置及电机的实际位置，P_Yaxis,P_Ymotor分别为Y向轴的实际位置及电机的实际位置。

所述对调整后的各轴命令位置矢量在被加工轨迹上投影，得到投影长度，确定修正后的命令位置，具体为：

在待加工轨迹为直线的情况下，修正后轨迹上命令位置坐标P”(x_c,y_c)的坐标计算为：

(u_x,u_y)为待加工直线的单位方向矢量，L为投影长度，(x₀,y₀)为待加工直线的初始点Ps的坐标；

在待加工轨迹为圆弧的情况下，通过半径R、根据公式(5)所示，确定

根据及由公式(6)确定夹角α：

补偿方向的确定：根据公式(7)确定圆弧的旋转方向

利用公式(8)计算d值，然后根据d值的符号，判断调整的圆弧方向：

如果d值大于0，说明位置点调整方向为正向调整；如果d值小于0，说明位置点调整方向为负向调整；

根据圆弧长度修正量，确定修正后轨迹上命令位置坐标：根据公式(9)，确定圆弧长度修正量，

ΔL＝α·R (9)

根据确定的补偿方向及圆弧长度修正量，通过圆弧插补算法，确定修正后轨迹上命令位置坐标P”(x_c,y_c)。

所述程序段最大剩余长度为预先设置的参数。

所述速度连接处理为：在连接过程中，下一运动段进行动态修正插补位置的全闭环运动控制，当前运动段不需要调整。

所述半闭环控制不需要机床轴执行终端的直接反馈，直接外接位置传感装置进行反馈。

本发明具有以下优点：

1.适应性强。凡是采用带全闭环反馈装置的机床，具备执行终端位置反馈，不管是线性坐标轴，还是旋转轴，不管是采用光栅尺、球栅尺，还是外接位置编码器，都可以采用本发明的方法；适用于两轴及三轴联动直线插补、两轴联动圆弧插补，并适用于全闭环反馈、全闭环反馈与半闭环反馈混合配制情形。

2.机床轴定位精度、重复定位精度可得到保障。在定位完成阶段采用全闭环控制，可保证各轴定位精度，而传统全闭环控制方法需要全程全闭环控制，虽然也能够保证定位精度，但要求机床轴机械传动性好，对于传动性较差的机床轴易引起震动，甚至出现过动情形。

3.轮廓定位精度高、速度平滑度高、动态性好。本发明在不同运动阶段采用不同的运动控制方式，在速度规划过程，引入运动轨迹点的动态修正机制，既避免了为保证速度规划的平稳性而带来的实际电机运转的不稳定，也保证轴运动速度的平稳性，同时较完全采用全闭环控制而言，可调高伺服环速度比例增益，减小随动误差，提高加工精度。

4.运动安全性高。通过引入运动轨迹点的动态修正机制，在提高速度平滑性的同时，可有效降低轴运动超差、超载的可能性；同时当出现外接传感装置信号异常的状态下，比如光栅尺读数紊乱等原因，可避免轴运动失常甚至飞车的情况，及早报告超差，保护机床不被损坏。

附图说明

图1为本发明方法流程图；

图2为本发明适用的系统结构框图；

图3为本发明单轴位置计算流程图；

图4为本发明轨迹向量映射图；

图5为本发明直线插补命令位置调整示意图；

图6为本发明圆弧插补命令位置调整示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

多轴联动动态修正的全闭环运动控制包括：1)根据采集到的各轴实际位置与电机实际位置之间的偏差，调整各轴命令位置；2)将调整后的各轴命令位置映射为轨迹上的命令位置。

其中，数据采集及偏差计算采用基于电机和机床位置双反馈方式实现。打破传统运动轨迹规划的概念，通过机床轴的最终执行端的光栅尺等外接位置传感装置得到的轴反馈位置，与伺服电机编码器反馈计算所得轴反馈位置，二者之差对轴运动规划位置点不断进行修正，重新计算运动规划剩余距离，但不影响轴运动规划速度，在运动规划周期内计算新的轨迹插补位置。通过外接位置传感装置可计算得到轴实际位置；通过伺服电机编码器获取读数，可计算得到伺服电机对应的轴位置；参见图3，通过这两个反馈位置，可得到当前伺服周期轴位置反馈修正量；通过此修正量，重新计算电机编码器与轴位置反馈间的位置偏移量，即电机反馈轴位置偏移，用于下一伺服周期计算伺服电机对应的轴位置；通过上一伺服周期的轴插补命令位置与当前获得的轴反馈位置，二者之差即为轴随动误差，用于判断轴运动过程中是否超差。

其中命令位置调整采用一种基于动态修正的调整算法，是本发明的一个关键。本部分涉及直线及圆弧两种轨迹形式，具体实现如下：

1)直线插补方式的修正算法

包括以下步骤：

(1)各轴命令位置的调整

如图5所示，以两轴联动为例，设P(x,y),P'(x_m,y_m),P”(x_c,y_c)分别为轨迹上命令位置坐标、各轴调整后命令位置坐标、修正后轨迹上命令位置坐标，P_Xaxis,P_Xmotor分别为X向轴的实际位置及电机的实际位置，P_Yaxis,P_Ymotor分别为Y向轴的实际位置及电机的实际位置，则各轴的偏差计算如公式(1)所示。

根据以上偏差，各轴调整后命令位置坐标计算如公式(2)所示。

(2)各轴命令位置矢量在被加工直线上的投影

矢量投影的示意图如图4所示。根据矢量点积的定义，如果一个任意矢量a与一个单位矢量u点乘，则其点积等于矢量a在单位矢量u上的投影，其计算如公式(3)所示。

a·u＝|u||a|cosθ＝|a|cosθ (3)

(3)根据投影长度L，确定修正后轨迹上命令位置坐标，设初始点Ps坐标为(x₀,y₀)，则P”(x_c,y_c)的坐标计算如公式(4)所示。

以上为两轴联动加工的命令位置调整算法，三轴联动插补加工直线与两轴联动插补加工直线的算法类似，在此不再赘述。

2)圆弧插补方式的修正算法

包括如下步骤：

(1)各轴命令位置的调整

如图6所示，设P(x,y),P'(x_m,y_m),P”(x_c,y_c)分别为轨迹上命令位置坐标、各轴调整后命令位置坐标、修正后轨迹上命令位置坐标，P_Xaxis,P_Xmotor分别为X向轴的实际位置及电机的实际位置，P_Yaxis,P_Ymotor分别为Y向轴的实际位置及电机的实际位置，则各轴的偏差计算如公式(1)所示。

根据所计算的各轴偏差，由公式(2)确定P`点的坐标

(2)调整角度的计算

通过半径R、根据公式(5)所示，确定

根据及由公式(6)确定夹角α

(3)补偿方向的确定

由解析几何知识可知，两个向量进行叉乘得到的是一个向量，方向垂直于这两个向量构成的平面，根据公式(7)确定

利用公式(8)计算d值，然后根据d值的符号，判断调整的圆弧方向

如果d值大于0，说明位置点调整方向为正向调整；如果d值小于0，说明位置点调整方向为负向调整。

(4)根据圆弧长度修正量，确定修正后轨迹上命令位置坐标

根据公式(9)，确定圆弧长度修正量。然后根据步骤(3)中确定的补偿方向，通过圆弧插补算法，确定修正后轨迹上命令位置坐标。

ΔL＝α·R (9)

参见图2，描述了本发明总体结构框图，包括数控系统装置、各轴轴数据采集、各轴轴控制、PID及驱动、反馈设备部分。此外，该结构图适用于采用轴切换的结构形式，即图中的N大于或等于M。

如图1所示，详细描述了本发明方法的控制流程。按流程所示，采用动态修正算法的运动过程可划分为以下七个阶段：

1)规划加速阶段；2)规划匀速阶段；3)规划减速阶段；4)程序段混联阶段；5)精准停阶段；6)定位完成阶段；7)各轴无运动阶段。针对不同的运动规划阶段，所述基于电机和机床位置双反馈的轴运动控制方法可划分为不同的控制方式：1)半闭环控制；2)全闭环控制；3)基于电机和机床位置双反馈动态修正的闭环控制。

按流程所示，轴运动过程可划分为以下六个阶段：

①规划加速阶段

在轨迹运动指令执行初期以及在轨迹规划运动过程中，剩余足够距离且规划速度未达到进给速度时，即处于规划加速阶段。该阶段的运动规划采用多轴联动动态修正的全闭环运动控制算法。

规划过程如下：

首先对上一周期插补命令位置点及规划剩余距离进行全闭环修正：

lastCmdPos＝LastCmdPos+posCorrection；

remDis＝remDis-posCorrection；

其中，lastCmdPos表示上一周期的插补命令位置点，posCorrection表示轴位置反馈修正量，remDis表示规划剩余距离。

根据加减速算法得出当前周期新的插补命令位置点cmdPos、规划速度cmdVel、加速度cmdAcc，以及剩余距离remDis等。

规划加速阶段完成进入规划匀速②或减速阶段③。

②规划匀速阶段

在轨迹规划运动过程中，剩余足够距离且规划速度达到进给速度时，即处于规划匀速阶段。该阶段采用运动规划采用多轴联动动态修正的全闭环运动控制算法。

规划过程同加速阶段①。

规划匀速阶段完成进入规划减速阶段③。

③规划减速阶段

在轴规划运动过程中，剩余距离不足以匀速或加速运行时，即处于规划减速阶段。在该阶段满足全闭环条件前，采用多轴联动动态修正的全闭环运动控制算法。

规划过程与①相比，在减速过程中，需要判断是否满足全闭环条件，即用参数指定的剩余长度大小，以保证零件加工的轮廓误差满足精度要求，若不满足全闭环条件，则继续进行减速规划，否则判断是否为混联模式且有后续运动段，如果满足条件，进入阶段④进行处理，否则，进入阶段⑤进行处理。

④程序段混联阶段

对于当前程序段，采用全闭环方式实现对各轴的位置控制，无插补命令位置点及规划剩余距离进行全闭环修正过程，同时引入下一运动段与当前运动段进行速度连接处理。在连接过程中，下一运动段进行正常的命令位置动态修正的运动控制，利用前后两个运动段间合成的增量位置矢量，通过与开始混联处的命令位置矢量进行矢量运算，进而确定连接时各插补点的命令位置，直到当前运动段规划完成。

然后判断在运动段切换过程中，是否存在增轴或减轴的情况。若存在则当前运动段规划完成后，下一运动段无关轴需要进入阶段⑥进行精确定位处理，同时，其余轴进入阶段①继续下一运动段的规划。

⑤精准停阶段

采用全闭环方式实现对各轴的位置控制，无插补命令位置点及规划剩余距离进行全闭环修正过程，当程序段速度规划完成后，进入阶段⑥进行精确定位处理。

判断有没有后续运动段，若没有后续运动段，则进入阶段⑦，若有后续运动段，然后判断在程序段切换过程中，是否存在增轴或减轴的情况，若存在则当前运动段规划完成后，下一运动段无关轴需要进入阶段⑥进行精确定位处理，同时，其余轴进入阶段①继续下一运动段的规划。

⑥定位完成阶段

在规划减速阶段完成后，插补命令位置点cmdPos与目标位置点相同，规划命令速度cmdVel为0，规划加速度cmdAcc为0，规划剩余距离为0，轴运动规划完成，此阶段为定位完成阶段。

此阶段为定位点全闭环控制，系统提供定位计时，时长由系统参数指定，保证定位精确。当达到定位计时时长，进入轴无运动阶段⑦；若此阶段有新运动指令命令，接收运动指令，进入规划加速阶段①。

⑦各轴无运动阶段

在轴运动指令执行完成，或无运动指令执行时，此阶段为轴无运动阶段。

此阶段采用半闭环控制，外接位置传感装置只用于轴实际位置的计算与随动误差的计算。

Claims (8)

1.一种用于多轴联动动态修正插补位置的全闭环运动控制方法，其特征在于，包括以下步骤：

在第一个命令位置坐标上，根据采集到的各轴实际位置与电机实际位置之间的偏差，调整各轴命令位置；

对调整后的各轴命令位置矢量在被加工轨迹上投影，得到投影长度，确定修正后的命令位置；

重复上述步骤，顺序调整其他命令位置坐标；

当满足全闭环条件，即参数指定的剩余长度大小满足零件加工的轮廓误差所要求的程序段最大剩余长度时，停止调整；

当程序段处于混联且有后续运动段时，则将本运动段与下一运动段进行速度连接处理；否则，不对插补位置进行调整，直到当前运动段插补完成，再在系统提供的定位时长内，通过PID控制算法使轴的实际位置运动到命令位置上，完成定位。

2.根据权利要求1所述的用于多轴联动动态修正插补位置的全闭环运动控制方法，其特征在于，所述定位完成后或速度连接处理后，如果有后续运动段，则继续下一段加工，如果有增轴、减轴的情况，则通过PID控制算法使与下一运动段无关的轴运动到命令位置上，其他轴继续下一段加工；如果没有后续运动段，则各轴进入半闭环控制。

3.根据权利要求1所述的用于多轴联动动态修正插补位置的全闭环运动控制方法，其特征在于，所述各轴实际位置通过在移动轴外接光栅尺、球栅尺，在旋转轴外接编码器实现。

4.根据权利要求1所述的用于多轴联动动态修正插补位置的全闭环运动控制方法，其特征在于，所述根据采集到的各轴实际位置与电机实际位置之间的偏差，调整各轴命令位置，具体为：

各轴的偏差计算如下：

$\left\{\begin{array}{c}\mathrm{\Δ}X=P_{Xaxis}-P_{Xmotor}\\ \mathrm{\Δ}Y=P_{Yaxis}-P_{Ymotor}\end{array}\right.---\left(1\right)$

根据以上偏差，各轴调整后命令位置坐标计算如下：

$\left\{\begin{array}{c}{x}_m=x+\mathrm{\Δ}X\\ y_m=y+\mathrm{\Δ}Y\end{array}\right.---\left(2\right)$

其中，P(x,y),P'(x_m,y_m)分别为轨迹上命令位置坐标、各轴调整后命令位置坐标，P_Xaxis,P_Xmotor分别为X向轴的实际位置及电机的实际位置，P_Yaxis,P_Ymotor分别为Y向轴的实际位置及电机的实际位置。

5.根据权利要求1所述的用于多轴联动动态修正插补位置的全闭环运动控制方法，其特征在于，所述对调整后的各轴命令位置矢量在被加工轨迹上投影，得到投影长度，确定修正后的命令位置，具体为：

在待加工轨迹为直线的情况下，修正后轨迹上命令位置坐标P”(x_c,y_c)的坐标计算为：

$\left\{\begin{array}{c}{x}_c=x_0+u_x\·L\\ y_c=y_0+u_y\·L\end{array}\right.---\left(4\right)$

(u_x,u_y)为待加工直线的单位方向矢量，L为投影长度，(x₀,y₀)为待加工直线的初始点Ps的坐标；

在待加工轨迹为圆弧的情况下，通过半径R、根据公式(5)所示，确定

根据及由公式(6)确定夹角α：

补偿方向的确定：根据公式(7)确定圆弧的旋转方向

利用公式(8)计算d值，然后根据d值的符号，判断调整的圆弧方向：

$d=\stackrel{\→}{V}\·\stackrel{\→}{N}---\left(8\right)$

如果d值大于0，说明位置点调整方向为正向调整；如果d值小于0，说明位置点调整方向为负向调整；

根据圆弧长度修正量，确定修正后轨迹上命令位置坐标：根据公式(9)，确定圆弧长度修正量，

ΔL＝α·R (9)

根据确定的补偿方向及圆弧长度修正量，通过圆弧插补算法，确定修正后轨迹上命令位置坐标P”(x_c,y_c)。

6.根据权利要求1所述的用于多轴联动动态修正插补位置的全闭环运动控制方法，其特征在于，所述程序段最大剩余长度为预先设置的参数。

7.根据权利要求1所述的用于多轴联动动态修正插补位置的全闭环运动控制方法，其特征在于，所述速度连接处理为：在连接过程中，下一运动段进行动态修正插补位置的全闭环运动控制，当前运动段不需要调整。

8.根据权利要求2所述的用于多轴联动动态修正插补位置的全闭环运动控制方法，其特征在于，所述半闭环控制不需要机床轴执行终端的直接反馈，直接外接位置传感装置进行反馈。