一种基于嵌入式裁床控制器的多轴联动运动控制方法与流程

本发明涉及一种运动控制方法，尤其是一种基于嵌入式裁床控制器的多轴联动运动控制方法，属于运动控制领域。

背景技术：

目前我国服装，鞋子、床上用品、等相关产业发展迅速，用于对布料、皮革等原材料裁割的裁床种类繁多。传统自动化程度低、功能单一、控制精度低的裁床逐渐被淘汰，自动化程度较高、多功能、高精度的新型裁床受到越来越广泛的关注。

裁床主要是由伺服电机(分别为X轴、Y轴和C轴以及D轴伺服电机)、X轴导轨、Y轴导轨、移动横梁、传动带、裁割机头、机头刀具、复位信号传感器等组成。裁床控制器根据上位机下发的轨迹文件，通过控制X轴和Y轴伺服电机，带动横梁上的裁割机头及机头刀具沿X轴导轨和Y轴导轨进行平面运动；通过C轴伺服电机控制机头刀具的旋转，使机头刀具的方向随着机头运动方向的改变而改变；通过D轴伺服电机控制机头刀具的抬落，从而完成轨迹文件的裁割操作。然而，以伺服电机为控制对象的嵌入式裁床控制器在高速运行及裁割连续小线段时仍然存在着控制精度较低、抖动比较严重等问题。这不仅会影响裁床生产出的产品的质量，而且会降低裁床使用寿命。

技术实现要素：

为了解决现有嵌入式裁床控制器在高速运行及裁割连续小线段时存在的抖动比较严重、稳定性较差、精度不高等问题，本发明提供了一种能有效减少裁床抖动、提升裁床稳定性和裁割精度的基于嵌入式裁床控制器的多轴联动运动控制方法。

本发明解决其技术问题所采用的技术方案如下：

一种基于嵌入式裁床控制器的多轴联动运动控制方法，所述方法包括以下步骤：

1)求得加速度表、速度表、阶梯步长表、裁床允许的最大加速度以及最大非抬刀旋转角度

采用S曲线型加减速控制算法，得到控制裁床X轴和Y轴伺服电机运动的加速度表(a₁,...,a_j)；然后根据所用X轴和Y轴伺服电机的速度特性建立X轴和Y轴伺服电机运动所需的速度表(v₁，...，v_j)；伺服电机每一次的速度变化都需要运行一定步数作缓冲，不同速度下所需要运行的缓冲步数也不同，结合伺服电机的运动特性计算出对应的阶梯步长表(l₁,...,l_j)；根据裁床机械结构特性及X轴和Y轴伺服电机参数，通过调试确定裁床工作所允许的最大加速度a_max；

上位机下发的轨迹文件由离散后的坐标点组成，相邻坐标点相连形成的密集线段即为裁床所需运行的轨迹，相邻线段之间的点称为拐点；在拐点处转刀时，若C轴所需旋转角度比预设角度小，采用直接转刀方式，在到达拐点处时通过C轴伺服电机直接控制刀具旋转相应的角度；若所需旋转角度比预设角度大，C轴伺服电机控制刀具旋转时会引起裁床抖动从而降低裁床裁割精度，因此采用转角抬刀方式，此时将该拐点速度降为零并通过D轴伺服电机抬起刀具，将刀具旋转相应角度后再落下刀具继续裁割；根据裁床机械结构特性通过测试得出C轴所允许的最大非抬刀旋转角度θ；若裁割轨迹是连续小线段，则采用三轴联动方式，处理连续小线段的裁割过程；

2)连续线段的速度规划以及加减速过程如下：

n+1个坐标点(x₀,y₀),(x₁,y₁),...,(x_n,y_n)组成了n条线段；由相邻两个坐标点坐标，分别计算出n条线段与X轴方向的夹角θ₁,θ₂...,θ_n，由θ₁,θ₂...,θ_n分别计算得到各个拐点处C轴所需要旋转的角度即拐角Δθ₁,...,Δθ_n-1；

2.1)结合步骤1)中的加速度表(a₁,...,a_j)和速度表(v₁，...，v_j)，设裁床启动速度为v_q并将起点和终点速度设为启动速度，在满足X轴方向和Y轴方向上速度变化不超过a_max的条件下，计算得到在各个拐点处的最大速度(v_x1,v_y1)_max,...,(v_x(n-1),v_y(n-1))_max；

2.2)采用速度回溯法计算得到各拐点目标速度，过程如下：

将各个拐点处的最大速度(v_x1,v_y1)_max,...,(v_x(n-1),v_y(n-1))_max作为裁割轨迹上各个拐点的初始目标速度(v'_x1,v'_y1),...,(v'_x(n-1),v'_y(n-1))。设n-1点的初始目标速度的合速度为ν'_n-1，终点n的速度的合速度为ν_n；根据步骤(1)中阶梯步长表从n点进行加速计算，算得运动到点n-1时的速度为(v″_x(n-1),v″_y(n-1))，合速度为ν″_n-1。若满足ν'_n-1＞ν″_n-1，则选取(v″_x(n-1),v″_y(n-1))作为在n-1点的目标速度(v_x(n-1),v_y(n-1))；反之若ν'_n-1≤ν″_n-1，则将(v'_x(n-1),v'_y(n-1))作为在n-1点的目标速度(v_x(n-1),v_y(n-1))。以此类推，得到所有拐点的目标速度(v_x1,v_y1),...,(v_x(n-1),v_y(n-1))；

2.3)采用定插补周期的插补方式，加减速过程如下：

根据相邻两点坐标计算得到轨迹线段在X轴和Y轴上投影长度，选取较长的轴为插补计算的主轴，另一轴为副轴。结合两点目标速度、速度表和阶梯步长表可计算得到主轴加速、匀速、减速阶段所走的脉冲数N₁、N₂、N₃。选择固定的插补周期T，计算主轴在第m个插补周期中所需的脉冲数并将其放入主轴脉冲缓冲区；然后根据主轴所需脉冲数计算得到副轴在第m个插补周期中所需脉冲数并将其放入副轴脉冲缓冲区。在计算下一个插补周期时，将此之前每一个插补周期主轴走的脉冲数之和与N₁、N₂、N₃相比较，以确定下一个插补周期主轴所处的运动阶段。

进一步，所述多轴联动运动控制方法还包括以下步骤：

3)在拐点处转刀时，若拐角Δθ_i小于θ，采用直接转刀方式；若拐角Δθ_i大于θ，采用转角抬刀方式，过程如下：

3.1)判断在某一拐点(x_i,y_i)处拐角Δθ_i的大小，在拐点处转刀时，若拐角Δθ_i小于θ，则在到达拐点处时采用直接转刀方式，通过C轴伺服电机直接控制刀具旋转相应的角度；若拐角Δθ_i大于θ，则在步骤(2)中将该拐点处的最大速度设为零，因此求得该拐点的目标速度(v_xi,v_yi)也为零；当裁床运行到拐点(x_i,y_i)时先通过D轴伺服电机将刀具抬起，再通过C轴伺服电机以速度v_c驱动刀具旋转角度Δθ_i，最后通过D轴伺服电机驱动刀具落下继续裁割操作；

3.2)采用直接转刀和转角抬刀方式时，C轴伺服电机在控制机头刀具旋转时根据伺服电机特性选择一固定速度即可；结合伺服电机特性并经过测试，得到C轴伺服电机在直接转刀和转角抬刀时的运行速度v_c；在裁割过程中，D轴伺服电机控制刀具起落时，裁割机头运行速度为零，同样只需要根据伺服电机特性选择一固定速度v_d即可。

再进一步，所述多轴联动运动控制方法还包括以下步骤：

4)当裁割轨迹为连续小线段时，在拐点处转刀采用三轴联动方式，过程如下：

4.1)在对连续小线段裁割时，在步骤2)中对主轴和副轴进行插补计算的同时同步计算每个插补周期中C轴所需要旋转的角度及脉冲数，并将其放入C轴脉冲缓冲区，实现三轴联动插补，从而实现小线段的快速裁割。

4.2)采用三轴联动方式时，C轴伺服电机跟随X轴和Y轴同步运动，需要根据X轴、Y轴在每个插补周期内的速度以及脉冲数和拐角大小计算出C轴在对应插补周期内的速度以及脉冲个数。

本发明的优点在于：(1)根据S曲线型加减速控制算法，采用相应的加速度表、速度表以及阶梯步长表的速度控制方法，使裁床加减速过程中运行更加平稳高效；(2)采用回溯法计算到各拐点的目标速度，使得裁床在各拐点既能以较高的速度运行又能避免过切，速度规划更加的合理；(3)在拐点处转刀时，根据C轴所需旋转角度以及裁割轨迹是否为连续小线段，分别采用直接转刀、转角抬刀和三轴联动方式转刀，既减少了裁床工作过程中的抖动，又兼顾了裁床工作效率；(4)控制方法基于STM32F407实现，控制器资源丰富，功能强大。

附图说明

图1为S曲线型加减速控制算法的速度曲线图。

图2为长轴和拐点角度示意图。

具体实施方式

下面结合附图对本发明的实施方式作进一步描述。

参照图1和图2，一种基于嵌入式裁床控制器的多轴联动运动控制方法，包括以下步骤：

1)图1为S曲线型加减速控制算法下的速度曲线图；设伺服电机步距角为α，驱动脉冲的频率为f_s，对应的脉冲周期为t_s，则伺服电机的转速v为：

v＝60/[(360/α)t_s](r/min) (1)

步距角α由所选伺服电机型号决定，因此通过控制f_s即可控制伺服电机的转速。根据S型曲线加减速控制算法建立X轴和Y轴伺服电机运动所需要的加速度表(a₁,...,a_j)，结合伺服电机的速度特性，建立相应的速度表(v₁，...，v_j)；再由公式(2)计算出对应的驱动脉冲频率表(f₁，...，f_j)。结合速度表(v₁,...,v_j)和伺服电机本身的运动特性计算得出对应的阶梯步长表(l₁,...,l_j)。根据裁床的机械结构特性和所选用的伺服电机的型号并通过测试，确定X轴和Y轴伺服电机允许运行的最大加速度a_max以及C轴所允许的最大非抬刀旋转角度θ。

2)读取上位机下发的裁割轨迹坐标点(x₀,y₀),(x₁,y₁),...,(x_n,y_n)，起点和终点分别为(x₀,y₀)、(x_n,y_n)。根据相邻两个坐标点的坐标，通过公式(3)、(4)可以计算出n条线段与X轴方向的夹角θ₁,θ₂...,θ_n。由公式(5)可计算出各个拐点处C轴所需要旋转的角度即拐角Δθ₁,...,Δθ_n-1，其中Δθ₁,...,Δθ_n-1的正负号代表C轴的旋转方向。

Δθ_i＝θ_i+1-θ_i(1≤i≤(n-1)) (5)

2.1)结合步骤(1)中的加速度表(a₁,...,a_j)和速度表(v1,...,v_j)，在满足X轴方向和Y轴方向上速度变化不超过a_max的条件下，可以计算得到在各个拐点处的最大速度(v_x1,v_y1)_max,...,(v_x(n-1),v_y(n-1))_max。

2.2)采用速度回溯法，将所述步骤(2)中得到的最大速度(v_x1,v_y1)_max,...,(v_x(n-1),v_y(n-1))_max作为裁割轨迹上各个拐点的初始目标速度(v'_x1,v'_y1),...,(v'_x(n-1),v'_y(n-1))。设n-1点的初始目标速度的合速度为ν'_n-1，终点n的速度的合速度为ν_n；根据步骤(1)中阶梯步长表从n点进行加速计算，算得运动到点n-1时的速度为(v″_x(n-1),v″_y(n-1))，合速度为ν″_n-1。若满足ν'_n-1＞ν″_n-1，则选取(v″_x(n-1),v″_y(n-1))作为在n-1点的目标速度(v_x(n-1),v_y(n-1))；反之若ν'_n-1≤ν″_n-1，则将(v'_x(n-1),v'_y(n-1))作为在n-1点的目标速度。以此类推，得到所有拐点的目标速度(v_x1,v_y1),...,(v_x(n-1),v_y(n-1))。

2.3)如图2所示，根据相邻两点坐标计算得到轨迹线段在X轴和Y轴上投影长度l_ix，l_iy，设l_ix≥l_iy，选取较长的X轴为插补计算的主轴，Y轴为副轴。结合两点目标速度和速度表、阶梯步长表可计算得到主轴加速、匀速、减速阶段所走的脉冲数N₁、N₂、N₃。选择固定的插补时间T，通过公式(6)和(7)可计算主轴在第m个插补周期中所需的脉冲数n_xTm，其中v_xTm为第m个插补周期主轴对应的运行速度，l_s为所用伺服电机步距，l_xTm为第m个插补周期主轴伺服电机走过的长度。最后将得到的脉冲数n_xTm放入主轴脉冲缓冲区；然后根据主轴脉冲数n_xTm和线段与X轴夹角θ_i通过公式(8)计算得到副轴在第m个插补周期中所需脉冲数n_yTm并将其放入副轴脉冲缓冲区。在计算下一个插补周期时，将此之前每一个插补周期主轴走的脉冲数之和与N₁、N₂、N₃相比较，以确定下一个插补周期主轴所处的运动阶段。

l_xTm＝T*v_xTm (6)

n_yTm＝n_xTm tanθ_i(8)

3)在步骤2)中，若在某一拐点(x_i,y_i)处拐角Δθ_i小于θ，为提高裁床工作效率，在到达拐点处时采用直接转刀方式，通过C轴伺服电机直接控制刀具旋转相应的角度；若拐角Δθ_i角度大于θ，为避免裁床在大角度转角时产生机械抖动，采用转角抬刀方式，将该拐点处的最大速度设为零，因此求得该点的目标速度(v_xi,v_yi)也为零。当裁床运行到拐点(x_i,y_i)时先通过D轴伺服电机将刀具抬起，再通过C轴伺服电机以速度v_c驱动刀具旋转角度Δθ_i，最后通过D轴伺服电机驱动刀具落下继续裁割操作。结合伺服电机特性并经过测试，得到C轴伺服电机在直接转刀和转角抬刀时的运行速度v_c以及D轴伺服电机的运行速度v_d。

4)在对连续小线段裁割时，在步骤(2)中对主轴进行插补计算的同时同步计算每个插补周期中C轴所需要旋转的角度及脉冲数，并将其放入C轴脉冲缓冲区，实现三轴联动插补，从而实现小线段的快速裁割。设X轴为主轴，第m个插补周期C轴所需旋转的角度和脉冲数计算公式如下：

其中θ_Tm为在第m个插补周期中C轴需要旋转的角度，n_cTm为在第m个插补周期中C轴伺服电机所需运行的脉冲数，α为伺服电机步距角。l_xTp为在第p个插补周期中主轴伺服电机走过的长度。