連續(xù)軌跡平滑過渡控制算法研究

東梁，李迪，粟明，李松

（華南理工大學機械與汽車工程學院，廣東廣州510640）

連續(xù)軌跡平滑過渡控制算法研究

東梁，李迪，粟明，李松

（華南理工大學機械與汽車工程學院，廣東廣州510640）

為了解決傳統(tǒng)的點位控制算法在加工復雜軌跡時速度頻繁變化造成的機床抖動、加工效率低等問題，提出連續(xù)軌跡平滑過渡算法。文中首先指出插補離散化導致連續(xù)軌跡需要跨段過渡，然后提出兩種連續(xù)軌跡單周期過渡方案并對比其算法執(zhí)行效率和輪廓誤差。為實現(xiàn)直線與圓弧過渡間平滑過渡，對比求根公式、用弧長近似弦長等多種算法的執(zhí)行效率。仿真結果表明，在保證輪廓誤差的前提下，實現(xiàn)了多段軌跡的連續(xù)加工，有效地提高了加工效率。

插補離散化；動態(tài)前瞻；指令周期；輪廓誤差

根據(jù)運動控制的應用領域和特點不同，運動控制可以分為點位運動控制、連續(xù)軌跡運動控制等。點位運動控制只對終點位置有要求，其主要的性能參數(shù)是定位速度和定位精度。連續(xù)軌跡運動控制又稱為輪廓控制，需要實現(xiàn)各段軌跡間的連續(xù)平滑過渡，既要保證系統(tǒng)加工的輪廓精度，還要保證較高的加工速度［1］。運動控制中，速度與精度是一對矛盾，如何在滿足加工精度的前提下提高加工速度是數(shù)控加工的關鍵問題［2］。在多段軌跡加工，尤其是連續(xù)微小線段加工中，如果采用與點位運動控制相同的控制策略，即每一段軌跡都從零加速，勻速運行一段時間后減速到零，雖然能夠保證各轉接點的位置精度，但會造成電機頻繁起停、加工表面質量差、加工效率低等問題［3］。連續(xù)軌跡控制采用的控制策略是插補一段新的軌跡跨過拐點，犧牲轉接點處的部分精度，提高軌跡間的銜接速度。

文中首先分析插補離散化，指出跨段過渡的必要性，然后提出直線與直線、直線與圓弧等單周期過渡的算法并比較其優(yōu)劣，提出執(zhí)行效率高、輪廓誤差小的軌跡間過渡方案。

1　插補離散化

電機輸出的脈沖數(shù)目必須是整數(shù)，這決定了數(shù)控加工是一個離散的過程，采用連續(xù)函數(shù)規(guī)劃得到的加速度、速度、軌跡長度與實際加工存在誤差［4］。由于實際插補過程中將連續(xù)的軌跡進行離散化處理，當加減速參數(shù)固定后，軌跡段內的最后一個插補點很難與線段終點自然重合，即當前速度（記為CurSpeed）和剩余距離（記為RemainL）總是不相等的。如果繼續(xù)以當前速度進行插補，則會引起過沖；如果改變速度，強行將線段終點作為最后插補點，則可能導致加速度超過允許值，產生速度沖擊［5］。在點位控制中，解決方案是在插補結束階段按剩余距離發(fā)送脈沖，補償插補離散化帶來的誤差，以保證準確到達終點。在連續(xù)軌跡控制中，當CurSpeed＞RemainL時，則直接過渡到下一段軌跡。

2　連續(xù)軌跡過渡方案

連續(xù)軌跡處理中，由于不經過當前段終點而直接過渡到下一段，需要確定下一段軌跡上的轉接點。本文研究直線與直線、直線與圓弧、圓弧與圓弧、圓弧與直線過渡求轉接點的方案。

2.1直線與直線間過渡方案

如圖1所示，P1、P2兩段軌跡相交于點B。C（C'）是過渡段在P2上轉接點，BE（E'）是點B到過渡段AC（C'）的距離，即輪廓誤差。AF為當前速度，即AF=CurSpeed，AB為P1段剩余距離，即AB=RemainL。連續(xù)軌跡過渡中，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的單位向量，則根據(jù)向量的乘法法則可知

其中v1和v2可以根據(jù)當前段軌跡的起始點和結束點求得，同時還可以求出各個軸上的方向余弦。

圖1　直線與直線過渡示意圖Fig.1Schematic diagram of line-line transition

2）根據(jù)幾何關系求BC段長度

3）計算過渡段各軸分速度，放入插補脈沖隊列，控制硬件發(fā)出脈沖根據(jù)向量運算規(guī)則

其中xRatioP2，yRatioP2分別為P2段軌跡在X軸、Y軸方向的方向余弦。

2.1.2以點B為圓心過渡

以點B為圓心，以BF為半徑作圓，與P2交于點C'，則有

將式（4）代入公式（3），即求出過渡段在各軸方向上的分速度和轉接點C'坐標。

2.1.3方案對比

以上兩種方案的思路基本相同，都需要先計算BC（C'），然后根據(jù)向量運算規(guī)則計算過各軸分速度。兩種方案的區(qū)別如下：

1）方案2與方案1相比，計算BC（C'）的算法大為簡化，因此算法執(zhí)行效率高，占用CPU指令周期少。

2）方案2輪廓誤差較小。

上述算法中，實際上犧牲了兩端直線段的連接點B處的部分精度，主要的理論誤差在于兩直線段的連接點B處的輪廓誤差。方案1的輪廓誤差為BE，方案2的輪廓誤差為BE'。

根據(jù)三角形“兩邊之和大于第三邊”，有

則在圖1中容易推導出

即方案2的輪廓誤差比方案1小。

綜上所述，方案2運算量小、算法執(zhí)行效率高、耗費DSP指令周期少，且輪廓誤差比方案1小，因此應采用方案2處理直線與直線間單周期過渡。

2.2直線與圓弧間過渡方案

圖2　直線與圓弧過渡示意圖（yA＜yO）Fig.2Schematic diagram of line-arc transition（yA＜yO）

2.2.1求根公式求轉接點

根據(jù)圓的方程，可得到兩個二元二次方程聯(lián)立

兩方程聯(lián)立得

將（8）代入（7）中的第一個方程，得到關于xC的一元二次方程。將該方程寫成ax2+bx+c=0的標準形式，各項參數(shù)為

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

如圖2所示，兩圓相交有兩個交點C1、C2。根據(jù)圓弧的旋向和兩圓心的位置關系，可以唯一確定直線與圓弧的交點。記圓弧旋向為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），可以得到y(tǒng)C，即可以求得yA≠yO時點C坐標。

2.2.2方案對比

求兩圓交點的方法有求根公式、利用圓的幾何性質、利用圓的參數(shù)方程、利用弧長近似替代弦長等方法，本文只給出使用求根公式求交點算法。為測試算法性能，搭建測試平臺如下——硬件平臺為TI TMS320C6748 DSP芯片，軟件平臺為CCSV5.5。DSP芯片主頻為456 MHz。算法程序（.text字段）運行在片內的IRAM中。仿真器型號為Texas Instruments XDS100v2 USB Emulator。各種算法的指令周期如表1所示。

表1　求兩圓交點算法對比Tab.1Comparison of algorithms

由表1可知，求根公式耗費指令周期最少。原因是TMS320C6748為定點和浮點型DSP，浮點運算速度非常快，兩個單精度浮點數(shù)相乘僅需2個時鐘周期，兩個雙精度浮點數(shù)相乘僅需4個時鐘周期［6］。盡管求根公式運算較為復雜，但均為加法和乘法運算，執(zhí)行效率較高。

2.3圓弧與直線/圓弧間過渡方案

直線插補中對線速度進行規(guī)劃，類似地，圓弧插補對角速度進行規(guī)劃。以圓弧起點和終點間的圓心角為角位移，根據(jù)S型或T型曲線，可以對角速度進行規(guī)劃。圓弧與其他類型的軌跡過渡時，滿足當前角速度大于剩余角位移時，即進行跨段過渡［7］。由圓弧插補的原理可知，圓弧插補可以保證插補點都在圓的輪廓上，但實際的軌跡是多邊形，具體到每個插補周期都是以弦長代替弧長。因此，圓弧插補過渡處理時，也可以用弦長代替弧長處理。根據(jù)圓弧當前點坐標和結束點坐標，可以求出圓弧結束段的割線，從而圓弧與直線/圓弧單周期過渡問題轉化為直線與直線/圓弧單周期過渡問題。

3　實驗驗證

通過Matlab仿真檢驗算法的執(zhí)行效果。加工路徑如圖4所示，整個路徑由七段軌跡組成，路徑方向為逆時針方向，即按A-B-C-D-E-F-G-A的順序進行加工［8］。位置、速度等參數(shù)均換算成脈沖（pulse）。主要加工參數(shù)為：插補周期100 μs，指令進給速度100 pulse/T，允許輪廓誤差10 pulse，合成最大加速度和單軸的最大加速度10 pulse/T2。

圖4　仿真加工軌跡Fig.4Simulation machining path

采用點位控制方式，每段軌跡單獨進行加減速，速度曲線如圖5所示。采用本文提出的連續(xù)軌跡過渡算法，得到的速度曲線如圖6所示。

圖5　點位加工速度曲線Fig.5PTP machining velocity curve

從速度曲線的對比可以得到如下結論：

圖6　連續(xù)加工速度曲線Fig.6Continuous machining velocity curve

1）連續(xù)軌跡過渡算法可有效減少加減速次數(shù)，實現(xiàn)軌跡間的平滑過渡。圖5中，每一段軌跡都經歷完整的加減速過程，共有7次加減速過程。圖6中，由于在轉節(jié)點B和D處轉接速度不變，共有5次加減速過程。

2）連續(xù)軌跡過渡算法可以有效縮短加工時間，提高加工效率。圖5中，加工整段軌跡需要528個時鐘周期；圖6中，加工整段軌跡需要494個時鐘周期。連續(xù)軌跡過渡算法可節(jié)省34個時鐘周期。

3）連續(xù)軌跡過渡算法中轉接速度與軌跡間的夾角有關。點B、D處軌跡間夾角為0，轉接速度等于指令進給速度。點G處軌跡間夾角小于點E處軌跡間夾角，因此點G處轉接速度大于點E處轉接速度。

4　結論

文中提出了連續(xù)軌跡單周期過渡算法，詳盡分析了直線與直線、直線與圓弧間平滑過渡方案并對比不同方案的優(yōu)劣。仿真結果表明該算法可實現(xiàn)軌跡間的平滑過渡，有效縮短加工時間，提高加工效率。

［1］莊文波.連續(xù)軌跡運動控制系統(tǒng)的研究與應用［D］.哈爾濱：哈爾濱工業(yè)大學，2008.

［2］葉佩青，趙慎良.微小直線段的連續(xù)插補控制算法研究［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］呂強，張輝，楊開明，等.基于實時性要求的多段預讀速度規(guī)劃及實驗研究［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］周剛，李岸，鄔義杰.基于綜合約束的微小直線段速度連續(xù)規(guī)劃算法［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］葉偉，王小椿.數(shù)控系統(tǒng)小線段高速加工中的“拐點問題”［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］周丹，李浩.基于焓值控制的超臨界機組給水控制策略優(yōu)化與仿真研究［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—），男，河南南陽人，碩士。研究方向：高性能嵌入式控制系統(tǒng)。