连续轨迹平滑过渡控制算法研究

东梁，李迪，粟明，李松

（华南理工大学机械与汽车工程学院，广东广州510640）

连续轨迹平滑过渡控制算法研究

东梁，李迪，粟明，李松

（华南理工大学机械与汽车工程学院，广东广州510640）

为了解决传统的点位控制算法在加工复杂轨迹时速度频繁变化造成的机床抖动、加工效率低等问题，提出连续轨迹平滑过渡算法。文中首先指出插补离散化导致连续轨迹需要跨段过渡，然后提出两种连续轨迹单周期过渡方案并对比其算法执行效率和轮廓误差。为实现直线与圆弧过渡间平滑过渡，对比求根公式、用弧长近似弦长等多种算法的执行效率。仿真结果表明，在保证轮廓误差的前提下，实现了多段轨迹的连续加工，有效地提高了加工效率。

插补离散化；动态前瞻；指令周期；轮廓误差

根据运动控制的应用领域和特点不同，运动控制可以分为点位运动控制、连续轨迹运动控制等。点位运动控制只对终点位置有要求，其主要的性能参数是定位速度和定位精度。连续轨迹运动控制又称为轮廓控制，需要实现各段轨迹间的连续平滑过渡，既要保证系统加工的轮廓精度，还要保证较高的加工速度［1］。运动控制中，速度与精度是一对矛盾，如何在满足加工精度的前提下提高加工速度是数控加工的关键问题［2］。在多段轨迹加工，尤其是连续微小线段加工中，如果采用与点位运动控制相同的控制策略，即每一段轨迹都从零加速，匀速运行一段时间后减速到零，虽然能够保证各转接点的位置精度，但会造成电机频繁起停、加工表面质量差、加工效率低等问题［3］。连续轨迹控制采用的控制策略是插补一段新的轨迹跨过拐点，牺牲转接点处的部分精度，提高轨迹间的衔接速度。

文中首先分析插补离散化，指出跨段过渡的必要性，然后提出直线与直线、直线与圆弧等单周期过渡的算法并比较其优劣，提出执行效率高、轮廓误差小的轨迹间过渡方案。

1　插补离散化

电机输出的脉冲数目必须是整数，这决定了数控加工是一个离散的过程，采用连续函数规划得到的加速度、速度、轨迹长度与实际加工存在误差［4］。由于实际插补过程中将连续的轨迹进行离散化处理，当加减速参数固定后，轨迹段内的最后一个插补点很难与线段终点自然重合，即当前速度（记为CurSpeed）和剩余距离（记为RemainL）总是不相等的。如果继续以当前速度进行插补，则会引起过冲；如果改变速度，强行将线段终点作为最后插补点，则可能导致加速度超过允许值，产生速度冲击［5］。在点位控制中，解决方案是在插补结束阶段按剩余距离发送脉冲，补偿插补离散化带来的误差，以保证准确到达终点。在连续轨迹控制中，当CurSpeed＞RemainL时，则直接过渡到下一段轨迹。

2　连续轨迹过渡方案

连续轨迹处理中，由于不经过当前段终点而直接过渡到下一段，需要确定下一段轨迹上的转接点。本文研究直线与直线、直线与圆弧、圆弧与圆弧、圆弧与直线过渡求转接点的方案。

2.1直线与直线间过渡方案

如图1所示，P1、P2两段轨迹相交于点B。C（C'）是过渡段在P2上转接点，BE（E'）是点B到过渡段AC（C'）的距离，即轮廓误差。AF为当前速度，即AF=CurSpeed，AB为P1段剩余距离，即AB=RemainL。连续轨迹过渡中，P1段的结束速度与P2段的起始速度相等，均为CurSpeed。直线与直线间单周期过渡求转接点常见的方案有两种——①过渡段保持速度不变，相当于以A点为圆心，以AF为半径作圆，与P2交于点C；②以B点为圆心，以BF为半径作圆，与P2交于点C'。方案②可以看作是对方案①的近似处理。

2.1.1以点A为圆心过渡

该方案中有AC=CurSpeed。以二维直线插补为例，要完成过渡段插补输出，需要计算AC在X，Y轴上的分速度，计算流程如下：

1）计算拐角（或拐角的余弦值）

设v1为加工段P1的单位向量，v2为加工段P2的单位向量，则根据向量的乘法法则可知

其中v1和v2可以根据当前段轨迹的起始点和结束点求得，同时还可以求出各个轴上的方向余弦。

图1　直线与直线过渡示意图Fig.1Schematic diagram of line-line transition

2）根据几何关系求BC段长度

3）计算过渡段各轴分速度，放入插补脉冲队列，控制硬件发出脉冲根据向量运算规则

其中xRatioP2，yRatioP2分别为P2段轨迹在X轴、Y轴方向的方向余弦。

2.1.2以点B为圆心过渡

以点B为圆心，以BF为半径作圆，与P2交于点C'，则有

将式（4）代入公式（3），即求出过渡段在各轴方向上的分速度和转接点C'坐标。

2.1.3方案对比

以上两种方案的思路基本相同，都需要先计算BC（C'），然后根据向量运算规则计算过各轴分速度。两种方案的区别如下：

1）方案2与方案1相比，计算BC（C'）的算法大为简化，因此算法执行效率高，占用CPU指令周期少。

2）方案2轮廓误差较小。

上述算法中，实际上牺牲了两端直线段的连接点B处的部分精度，主要的理论误差在于两直线段的连接点B处的轮廓误差。方案1的轮廓误差为BE，方案2的轮廓误差为BE'。

根据三角形“两边之和大于第三边”，有

则在图1中容易推导出

即方案2的轮廓误差比方案1小。

综上所述，方案2运算量小、算法执行效率高、耗费DSP指令周期少，且轮廓误差比方案1小，因此应采用方案2处理直线与直线间单周期过渡。

2.2直线与圆弧间过渡方案

图2　直线与圆弧过渡示意图（yA＜yO）Fig.2Schematic diagram of line-arc transition（yA＜yO）

2.2.1求根公式求转接点

根据圆的方程，可得到两个二元二次方程联立

两方程联立得

将（8）代入（7）中的第一个方程，得到关于xC的一元二次方程。将该方程写成ax2+bx+c=0的标准形式，各项参数为

公式中的各项都已知，因此容易求出a，b，c。求根公式如下

如图2所示，两圆相交有两个交点C1、C2。根据圆弧的旋向和两圆心的位置关系，可以唯一确定直线与圆弧的交点。记圆弧旋向为arcdir，逆时针arcdir=1，顺时针arcdir=-1。记式中符号为sign，以下分3种情况讨论如何确定sign。

1）当yA＜yO时，即如图2所示的情况。

当圆弧顺时针，arcdir=-1，交点为C1，式（10）中应取负号；当圆弧逆时针，arcdir=1，交点为C2，式（10）中应取正号；因此，式（10）中符号与圆弧旋向arcdir一致，有sign=arcdir。

2）当yA＞yO时，即如图3所示的情况。

图3　直线与圆弧过渡示意图（yA＞yO）Fig.3Schematic diagram of line-arc transition（yA＞yO）

当圆弧顺时针，arcdir=-1，交点为C1，式（10）中应取正号；当圆弧逆时针，arcdir=1，交点为C2，式（10）中应取正号；因此，式（10）中符号与圆弧旋向arcdir相反，有sign=-arcdir。

3）当yA=yO时，即两圆圆心在一条直线上，两个交点的横坐标相同，此时可按sign=arcdir处理。

综合以上，由此可得直线与圆弧的交点为

将（11）代入（8），可以得到yC，即可以求得yA≠yO时点C坐标。

2.2.2方案对比

求两圆交点的方法有求根公式、利用圆的几何性质、利用圆的参数方程、利用弧长近似替代弦长等方法，本文只给出使用求根公式求交点算法。为测试算法性能，搭建测试平台如下——硬件平台为TI TMS320C6748 DSP芯片，软件平台为CCSV5.5。DSP芯片主频为456 MHz。算法程序（.text字段）运行在片内的IRAM中。仿真器型号为Texas Instruments XDS100v2 USB Emulator。各种算法的指令周期如表1所示。

表1　求两圆交点算法对比Tab.1Comparison of algorithms

由表1可知，求根公式耗费指令周期最少。原因是TMS320C6748为定点和浮点型DSP，浮点运算速度非常快，两个单精度浮点数相乘仅需2个时钟周期，两个双精度浮点数相乘仅需4个时钟周期［6］。尽管求根公式运算较为复杂，但均为加法和乘法运算，执行效率较高。

2.3圆弧与直线/圆弧间过渡方案

直线插补中对线速度进行规划，类似地，圆弧插补对角速度进行规划。以圆弧起点和终点间的圆心角为角位移，根据S型或T型曲线，可以对角速度进行规划。圆弧与其他类型的轨迹过渡时，满足当前角速度大于剩余角位移时，即进行跨段过渡［7］。由圆弧插补的原理可知，圆弧插补可以保证插补点都在圆的轮廓上，但实际的轨迹是多边形，具体到每个插补周期都是以弦长代替弧长。因此，圆弧插补过渡处理时，也可以用弦长代替弧长处理。根据圆弧当前点坐标和结束点坐标，可以求出圆弧结束段的割线，从而圆弧与直线/圆弧单周期过渡问题转化为直线与直线/圆弧单周期过渡问题。

3　实验验证

通过Matlab仿真检验算法的执行效果。加工路径如图4所示，整个路径由七段轨迹组成，路径方向为逆时针方向，即按A-B-C-D-E-F-G-A的顺序进行加工［8］。位置、速度等参数均换算成脉冲（pulse）。主要加工参数为：插补周期100 μs，指令进给速度100 pulse/T，允许轮廓误差10 pulse，合成最大加速度和单轴的最大加速度10 pulse/T2。

图4　仿真加工轨迹Fig.4Simulation machining path

采用点位控制方式，每段轨迹单独进行加减速，速度曲线如图5所示。采用本文提出的连续轨迹过渡算法，得到的速度曲线如图6所示。

图5　点位加工速度曲线Fig.5PTP machining velocity curve

从速度曲线的对比可以得到如下结论：

图6　连续加工速度曲线Fig.6Continuous machining velocity curve

1）连续轨迹过渡算法可有效减少加减速次数，实现轨迹间的平滑过渡。图5中，每一段轨迹都经历完整的加减速过程，共有7次加减速过程。图6中，由于在转节点B和D处转接速度不变，共有5次加减速过程。

2）连续轨迹过渡算法可以有效缩短加工时间，提高加工效率。图5中，加工整段轨迹需要528个时钟周期；图6中，加工整段轨迹需要494个时钟周期。连续轨迹过渡算法可节省34个时钟周期。

3）连续轨迹过渡算法中转接速度与轨迹间的夹角有关。点B、D处轨迹间夹角为0，转接速度等于指令进给速度。点G处轨迹间夹角小于点E处轨迹间夹角，因此点G处转接速度大于点E处转接速度。

4　结论

文中提出了连续轨迹单周期过渡算法，详尽分析了直线与直线、直线与圆弧间平滑过渡方案并对比不同方案的优劣。仿真结果表明该算法可实现轨迹间的平滑过渡，有效缩短加工时间，提高加工效率。

［1］庄文波.连续轨迹运动控制系统的研究与应用［D］.哈尔滨：哈尔滨工业大学，2008.

［2］叶佩青，赵慎良.微小直线段的连续插补控制算法研究［J］.中国机械工程，2004，15（15）：1354-1356. YE Pei-qing，ZHAO Shen-liang.Study on algorithm for micro-line continuous interpolation［J］.China Mechanical Engineering，2004，15（15）：1354-1356.

［3］吕强，张辉，杨开明，等.基于实时性要求的多段预读速度规划及实验研究［J］.机械与液压，2008，36（4）：1-4. LV Qiang，ZHANG Hui，YANG Kai-ming，et al.Look-ahead federate plan method based on the requirement of real time and its experimental study［J］.Machine Tool&Hydraulics，2008，36（4）：1-4.

［4］周刚，李岸，邬义杰.基于综合约束的微小直线段速度连续规划算法［J］.组合机床与自动化加工技术，2010（10）：20-25. ZHOU Gang，LI An，WU Yi-jie.Micro-lines continue federate scheduling based on integrate restriction［J］.Modular Machine Tool&Automatic Manufacturing Technique，2010（10）：20-25.

［5］叶伟，王小椿.数控系统小线段高速加工中的“拐点问题”［J］.北京交通大学学报，2004，15（15）：1354-1356. YE Wei，WANG Xiao-chun.Inflexion problem in high-speed machining of small line blocks［J］.Journal of Beijing Jiaotong University，2004，15（15）：1354-1356.

［6］Texas Instruments.TMS320C6748TM Fixed-and Floating-Point DSP［EB/OL］.［2014-12-20］.http：//www.ti.com/lit/ds/ symlink/tms320c6748.pdf.

［7］赵志伟，陈学有，潘琼.采用特征值法和Prony法相结合的PSS自适应控制［J］.陕西电力，2012（6）：49-52，62. ZHAO Zhi-wei，CHEN Xue-you，PAN Qiong.Adaptive control of pss based on eigen value analysis and prony analysis［J］. Shaanxi Electric Power，2012（6）：49-52，62.

［8］周丹，李浩.基于焓值控制的超临界机组给水控制策略优化与仿真研究［J］.陕西电力，2014（4）：70-73，77. ZHOU Dan，LI Hao.Optimization and simulation for feedwater control strategy of supercritical unit based on enthalpy control［J］.Shaanxi Electric Power，2014（4）：70-73，77.

Research on smooth transition control algorithm for continuous path

DONG Liang，LI Di，SU Ming，LI Song
（School of Mechanical and Automotive Engineering，South China University of Technology，Guangzhou 510640，China）

When machining complex counters，the conventional PTP control causes jitter of the machine，reduces machining efficiency due to the frequent changes of feed rate.A new control algorithm was put forward to realize smooth transition for continuous path.The existence of interpolation discretization was pointed out.The execution efficiency of two single-period transition solution was compared.In order to realize the smooth transition of line and arc，this paper compared the instruction cycle of radical formula and approximation method.The simulation result indicated that，while ensuring the precision of contour error，the continuous interpolation was realized，which effectively improved the machining efficiency.

interpolation discretization；dynamic look-ahead；instruction cycle；contour error

TH-39；TP273+.5

A

1674-6236（2015）20-0096-04

2015-01-07稿件编号：201501053

广东省科技计划项目（2012A010702004；2012A090100012）

东梁（1989—），男，河南南阳人，硕士。研究方向：高性能嵌入式控制系统。