EtherCAT运动控制——电子凸轮的实现（原理介绍、附代码）

大家好，欢迎来到IT知识分享网。

一、电子凸轮简介

1.1 组成部分

电子凸轮（ECAM）是利用构造的凸轮曲线来模拟机械凸轮，以达到机械凸轮系统相同的凸轮轴于主轴之间相对运动的软件系统。通过控制器控制伺服电机来模拟机械凸轮的功能，不需要另外安装机械结构。

1.2 工作原理

电子凸轮属于多轴同步运动。与机械齿轮一样，电子凸轮需要主轴和从轴。若定义凸轮为主轴，推杆为从轴，那么凸轮的实质就是从轴对应主轴的一种函数关系。

如下图，机械凸轮按照凸轮的轮廓可以得出一段转动角度与加工位置运动轨迹，此轨迹为弧线，将该段弧线分解成无数个直线轨迹，组合起来得到一串趋近于弧线运动轨迹，电子凸轮直接将此段轨迹运动参数装入运动指令，即可控制轴走出目标轨迹。

下面对电子凸轮中的几个概念解释一下：

1） 主轴

一般将一个运行周期内，以固定速度进行传动的轴作为主轴，主轴是在凸轮曲线中作为X轴的变量函数。电子凸轮的主轴可以是实际的轴（实轴）、或虚拟的轴（虚轴）。

2） 从轴

从轴是根据凸轮曲线进行运转的，在凸轮曲线中作为Y轴的变量函数，从轴的作用类似机械凸轮机构中从动轴作用，因此只能是实际的轴，但可以有多个从轴。

3）电子凸轮曲线

如下图所示，电子凸轮曲线是主轴和从轴之间的位置关系，是一个二维坐标系。

电子凸轮的两种表达形式：

1. 采用主轴作为X轴，从轴作为Y轴，建立的直角坐标系建立的XY点对点关系。
2. 采用主轴和从轴两者的函数关系式，即轴位置运算函数。根据函数，测量出主轴的位置，就能计算出从轴的需要到达的位置。

凸轮曲线设计的基本原则是在循环周 期中位置、速度、加速度必须平滑过渡，不引起电机抖动。实际应用中多采用五次多项式来表示。

1.3 电子凸轮的实现过程

本文介绍的电子凸轮运行在CSP模式，其实现过程可以概括为以下几个步骤：

1）上位机设定电子齿轮参数

主轴选择X轴，从轴选择Y轴，设定主轴转动一周需要6000puu，从轴转动一周需要6000puu，当主轴转动10周时从轴转动1周，主轴速度为1000puu/s；并设定从轴经过图中所示的5个点。

2）下位机接收电子齿轮参数，通过样条插值扩充凸轮表数据点

 下位机接收上位机发送的凸轮表信息，使用DSP库里的样条插值，将凸轮表扩充至1000个数据点，形成离散的电子凸轮曲线。

3）启动电子凸轮运行

1. 读取主轴的位置，更新写入下一个周期的位置
2. 基于主轴下一周期的位置，读取凸轮表中临近的3组对应数据，进行一次样条插值，求取从轴的目标位置

二、电子凸轮的实现

2.1 前期准备

2.1.1 DSP数学库

在电子齿轮实现过程中，使用到了DSP数学库。所谓DSP库就是为了让MCU实现DSP（数字信号处理的芯片）功能弄的一些官方库函数。

DSP 库是包含在 CMSIS（Cortex Microcontroller Software Interface Standard）里面的，所以下 载 DSP 库也就是下载 CMSIS。 STM32F4系列的官方下载链接如下，建议下载最新的版本，函数齐全。

STSW-STM32065 – STM32F4 DSP和标准外设库 – 意法半导体STMicroelectronics

DSP的获取及移植可以参考以下博主的介绍： 

推荐一种非常好的新版DSP库源码移植方式，含V7，V6和V5的IAR以及MDK5的AC5和AC6版本 – STM32H7 – 硬汉嵌入式论坛 – Powered by Discuz! (armbbs.cn)https://www.armbbs.cn/forum.php?mod=viewthread&tid=94659&extra=page%3D1

2.1.2 三次样条曲线介绍

在计算拟合凸轮表时，使用到了DSP库中的三次样条曲线插值（Cubic spline curve），样条曲线是一种常用的数学插值方法，用于平滑地拟合一组给定的离散数据点。

三次样条曲线的特点是由多个三次多项式组成，每个多项式在相邻数据点之间的区间上进行插值。这些多项式要满足插值条件和平滑条件，使得曲线在数据点之间变得光滑。

在DSP库中，样条曲线主要由 arm_spline_init_f32 和 arm_spline_f32 两个函数实现。

void arm_spline_init_f32( arm_spline_instance_f32 * S, // 结构体变量 arm_spline_type type, // 样条类型选择 ARM_SPLINE_NATURAL 自然样条 // ARM_SPLINE_PARABOLIC_RUNOUT 表示抛物线样条 const float32_t * x, // 原始数据x轴坐标值 数据必须是递增方式 const float32_t * y, // 原始数据y轴坐标值 uint32_t n, // 原始数据个数 float32_t * coeffs, // 参数是插补因数缓存 缓冲大小大于等于3*（n-1） float32_t * tempBuffer) // 临时缓冲 缓冲大小大于等于2*n-1 

void arm_spline_f32( arm_spline_instance_f32 * S, // 结构体变量 const float32_t * xq, // 补后的x轴坐标值 由用户指定的递增数据 float32_t * pDst, // 经过插补计算后输出的y轴数值 uint32_t blockSize) // 数据输出个数

2.2 电子凸轮中的非实时任务

2.2.1 凸轮表数据生成

首先下位机要接收上位机发送的数据，并进行插值计算，例如将接收到的五个坐标点，通过样条插值扩充为1000个，作为凸轮表保存下来。

样条插值的函数如下，当下位机接收到上位机发送来的凸轮表数据时，在非实时任务循环中调用一次本函数，即可进行样条插值扩充凸轮表。

float ECAM_coeffs[3*(ECAM_INPUT_DATA_MAX - 1)] __attribute__((at(0XC0050000))); /* 插补系数缓冲 笔者定义在了SDRAM中 读者按需重定义*/ float ECAM_tempBuffer[2 * ECAM_INPUT_DATA_MAX - 1] __attribute__((at(0XC0070000))); /* 插补临时缓冲 笔者定义在了SDRAM中*/ // 该函数为三次样条插值 // data_x data_y 为输入数据 // data_xi data_yi 为输出数据 其中的 data_xi 数据要升序排序！！！ // data_in_num data_out_num 为数据量 void Spline_Interpolation(float *data_x, float *data_y, u16 data_in_num, float *data_xi, float *data_yi, u16 data_out_num) { arm_spline_instance_f32 S; /* 样条初始化 */ arm_spline_init_f32(&S, ARM_SPLINE_NATURAL , // 选用自然样条曲线 data_x, // 原始数据 data_y, // 原始数据 data_in_num, // 原始数据个数 ECAM_coeffs, // 插补系数缓存 ECAM_tempBuffer); // 临时缓冲 /* 样条计算 */ arm_spline_f32 (&S, data_xi, data_yi, data_out_num); }

2.3 电子凸轮中的实时任务

所谓的实时任务是指，EtherCAT在每一次中断循环中，都会调用一次本节的代码，更新一次目标位置。实时任务要求任务要在循环周期内完成，否则将引起系统错误。

一般来说可以通过定时器中断来设定并开启实时任务循环。

2.3.1 主轴位置更新

笔者设计的程序使用一个数组 int nPos[ ]，存放各个轴的位置，对于主轴位置的更新，简单写的话可以直接加上一个定值的脉冲数。

例如若主轴的速度是10000puu/s，同步周期为2ms，那么每个周期递增的脉冲数为20puu，即：

nPos[Master_ID] += 20; // 每周期 主轴位置递增固定的脉冲 

上面是简单的写法，但如果要考虑主轴的加减速过程，让主轴运动更加的平滑，可以采用梯型速度曲线（要求更高可以考虑使用S型速度曲线）

// 该函数返回下一周期的位置增量 每周期步进数从StartIncPos到EndIncPos // nAccStep nAccStepDivi：每nAccStepDivi次数调用该函数，返回值速度增加/减少nAccStep // StartIncPos 起始时每周期步进的脉冲数 // EndIncPos 目标每周期步进的脉冲数 int Cal2TarIncPos(int nAccStep, int nAccStepDivi, int StartIncPos, int EndIncPos) { static int ToTarIncPos = 0, ToTarAccCnt = 0, AddCnt = 0; // 目标步进小于起始步进时 if(StartIncPos < EndIncPos) { // 在加速运行阶段： 每nAccStepDivi次周期返回值增加 nAccStep if(ToTarIncPos + nAccStep < EndIncPos) { if(ToTarAccCnt == (nAccStepDivi-1)){ AddCnt ++; ToTarAccCnt = 0; }else{ ToTarAccCnt++; } ToTarIncPos = StartIncPos + AddCnt * nAccStep; // 比上一周期的返回值增加 nDir*nAcc } else // 加速到目标周期步进 { ToTarIncPos = EndIncPos; } } else // 目标步进大于起始步进时 { // 在加速运行阶段： 每nAccStepDivi次周期返回值增加 nAccStep if(ToTarIncPos - nAccStep > EndIncPos) { if(ToTarAccCnt == (nAccStepDivi-1)){ AddCnt ++; ToTarAccCnt = 0; }else{ ToTarAccCnt++; } ToTarIncPos = StartIncPos - AddCnt * nAccStep; // 比上一周期的返回值增加 nDir*nAcc } else // 加速到目标周期步进 { ToTarIncPos = EndIncPos; } } return ToTarIncPos; } nPos[Master_ID] += Cal2TarIncPos(1,10,0,20); // 加速度为每10周期增多加1个脉冲 起始脉冲速度0 目标脉冲速度20 

2.3.2 从轴位置更新

基于主轴下一周期的位置，读取凸轮表中临近的3组对应数据，进行一次样条插值，求取从轴的目标位置。下列代码为笔者的实现过程：

float ECAM_xi[4]; // 待插补后的数值 float ECAM_yi[4]; float ECAM_x[3]; // 读取待插值点左右共计3个的数值作为插值原始数据 float ECAM_y[3]; // 该指令用于关联主轴和从轴的关系 // Master: 主动轴 （数值为0-31） // Slave: 从动轴 （数值为0-31） // mode：同步运动模式（1 电子凸轮） int EtherCAT_Synchronous_Follow(u8 Master, u8 Slave, u8 Mode) { if(Mode == 1 ) // 使用电子凸轮模式 { int i = 0, k = 0; int CamTable_Num = 1001; // 原始数据点的个数 int MASTER_PLS_NUM = 10000; // 主轴10000个脉冲转动一圈 int ECAM_index; float xi_Data; xi_Data = (float)(nPos[Master] % MASTER_PLS_NUM) ; // 待插补x数值 ECAM_index = xi_Data / (MASTER_PLS_NUM / (CamTable_Num-1)); // 定位出该点离得最近的凸轮表数据 的指针 if(ECAM_index == 0) { ECAM_x[0] = CamTable_xi[0] - (MASTER_PLS_NUM / (CamTable_Num-1)); ECAM_x[1] = CamTable_xi[0]; ECAM_x[2] = CamTable_xi[1]; ECAM_y[0] = CamTable_yi[0] - CamTable_yi[CamTable_Num - 1] + CamTable_yi[CamTable_Num - 2]; ECAM_y[1] = CamTable_yi[0]; ECAM_y[2] = CamTable_yi[1]; } else if(ECAM_index == (CamTable_Num-1)) { ECAM_x[0] = CamTable_xi[ECAM_index -1]; ECAM_x[1] = CamTable_xi[ECAM_index]; ECAM_x[2] = CamTable_xi[ECAM_index] + (MASTER_PLS_NUM / (CamTable_Num-1)); ECAM_y[0] = CamTable_yi[ECAM_index -1]; ECAM_y[1] = CamTable_yi[ECAM_index]; ECAM_y[2] = CamTable_yi[ECAM_index] + CamTable_yi[1] - CamTable_yi[0]; } else { ECAM_x[0] = CamTable_xi[ECAM_index - 1]; ECAM_x[1] = CamTable_xi[ECAM_index]; ECAM_x[2] = CamTable_xi[ECAM_index + 1]; ECAM_y[0] = CamTable_yi[ECAM_index - 1]; ECAM_y[1] = CamTable_yi[ECAM_index]; ECAM_y[2] = CamTable_yi[ECAM_index + 1]; } // 待插补数列要升序排序 if( xi_Data < CamTable_xi[ECAM_index]) { ECAM_xi[0] = ECAM_x[0]; ECAM_xi[1] = xi_Data; ECAM_xi[2] = ECAM_x[1]; ECAM_xi[3] = ECAM_x[2]; k = 1; Spline_Interpolation(ECAM_x, ECAM_y, 3, ECAM_xi, ECAM_yi, 4); // 插补计算出数值 } else if ( xi_Data > CamTable_xi[ECAM_index]) { ECAM_xi[0] = ECAM_x[0]; ECAM_xi[1] = ECAM_x[1]; ECAM_xi[2] = xi_Data; ECAM_xi[3] = ECAM_x[2]; k = 2; Spline_Interpolation(ECAM_x, ECAM_y, 3, ECAM_xi, ECAM_yi, 4); // 插补计算出数值 } else { ECAM_xi[2] = xi_Data; ECAM_yi[2] = CamTable_yi[ECAM_index]; k = 2; } nPos[Slave] = ECAM_yi[k]; // ------------ 从轴位置更新 ----------- printf("下一周期主轴位置 %f 拟合出的从轴位置 %f \r\n",ECAM_xi[k],ECAM_yi[k]); return 0; } }

2.3.3 EtherCAT通讯更新数据

EtherCAT主站发起PDO通讯，写入伺服目标位置，这部分读者自行调用EtherCAT通讯的API即可。

三、实验验证

笔者使用台达的ASDA2-E系列伺服驱动器进行验证，电子凸轮的参数按照上述步骤进行设置，能直观的观察到4号伺服作为从轴，其运动轨迹与凸轮曲线曲线一致。

 以上是笔者电子凸轮的介绍，如有错误还请指正，后续有更好的电子凸轮算法，笔者也会在本文中进行更新。