東梁，李迪，粟明，李松

（華南理工大學(xué)機(jī)械與汽車(chē)工程學(xué)院，廣東廣州510640）

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

東梁，李迪，粟明，李松

（華南理工大學(xué)機(jī)械與汽車(chē)工程學(xué)院，廣東廣州510640）

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

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

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

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

1　插補(bǔ)離散化

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

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

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

2.1直線與直線間過(guò)渡方案

如圖1所示，P1、P2兩段軌跡相交于點(diǎn)B。C（C'）是過(guò)渡段在P2上轉(zhuǎn)接點(diǎn)，BE（E'）是點(diǎn)B到過(guò)渡段AC（C'）的距離，即輪廓誤差。AF為當(dāng)前速度，即AF=CurSpeed，AB為P1段剩余距離，即AB=RemainL。連續(xù)軌跡過(guò)渡中，P1段的結(jié)束速度與P2段的起始速度相等，均為CurSpeed。直線與直線間單周期過(guò)渡求轉(zhuǎn)接點(diǎn)常見(jiàn)的方案有兩種——①過(guò)渡段保持速度不變，相當(dāng)于以A點(diǎn)為圓心，以AF為半徑作圓，與P2交于點(diǎn)C；②以B點(diǎn)為圓心，以BF為半徑作圓，與P2交于點(diǎn)C'。方案②可以看作是對(duì)方案①的近似處理。

2.1.1以點(diǎn)A為圓心過(guò)渡

該方案中有AC=CurSpeed。以二維直線插補(bǔ)為例，要完成過(guò)渡段插補(bǔ)輸出，需要計(jì)算AC在X，Y軸上的分速度，計(jì)算流程如下：

1）計(jì)算拐角（或拐角的余弦值）

設(shè)v1為加工段P1的單位向量，v2為加工段P2的單位向量，則根據(jù)向量的乘法法則可知

其中v1和v2可以根據(jù)當(dāng)前段軌跡的起始點(diǎn)和結(jié)束點(diǎn)求得，同時(shí)還可以求出各個(gè)軸上的方向余弦。

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

2）根據(jù)幾何關(guān)系求BC段長(zhǎng)度

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

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

2.1.2以點(diǎn)B為圓心過(guò)渡

以點(diǎn)B為圓心，以BF為半徑作圓，與P2交于點(diǎn)C'，則有

將式（4）代入公式（3），即求出過(guò)渡段在各軸方向上的分速度和轉(zhuǎn)接點(diǎn)C'坐標(biāo)。

2.1.3方案對(duì)比

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

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

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

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

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

則在圖1中容易推導(dǎo)出

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

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

2.2直線與圓弧間過(guò)渡方案

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

2.2.1求根公式求轉(zhuǎn)接點(diǎn)

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

兩方程聯(lián)立得

將（8）代入（7）中的第一個(gè)方程，得到關(guān)于xC的一元二次方程。將該方程寫(xiě)成ax2+bx+c=0的標(biāo)準(zhǔn)形式，各項(xiàng)參數(shù)為

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

如圖2所示，兩圓相交有兩個(gè)交點(diǎn)C1、C2。根據(jù)圓弧的旋向和兩圓心的位置關(guān)系，可以唯一確定直線與圓弧的交點(diǎn)。記圓弧旋向?yàn)閍rcdir，逆時(shí)針arcdir=1，順時(shí)針arcdir=-1。記式中符號(hào)為sign，以下分3種情況討論如何確定sign。

1）當(dāng)yA＜yO時(shí)，即如圖2所示的情況。

當(dāng)圓弧順時(shí)針，arcdir=-1，交點(diǎn)為C1，式（10）中應(yīng)取負(fù)號(hào)；當(dāng)圓弧逆時(shí)針，arcdir=1，交點(diǎn)為C2，式（10）中應(yīng)取正號(hào)；因此，式（10）中符號(hào)與圓弧旋向arcdir一致，有sign=arcdir。

2）當(dāng)yA＞yO時(shí)，即如圖3所示的情況。

圖3　直線與圓弧過(guò)渡示意圖（yA＞yO）Fig.3Schematic diagram of line-arc transition（yA＞yO）

當(dāng)圓弧順時(shí)針，arcdir=-1，交點(diǎn)為C1，式（10）中應(yīng)取正號(hào)；當(dāng)圓弧逆時(shí)針，arcdir=1，交點(diǎn)為C2，式（10）中應(yīng)取正號(hào)；因此，式（10）中符號(hào)與圓弧旋向arcdir相反，有sign=-arcdir。

3）當(dāng)yA=yO時(shí)，即兩圓圓心在一條直線上，兩個(gè)交點(diǎn)的橫坐標(biāo)相同，此時(shí)可按sign=arcdir處理。

綜合以上，由此可得直線與圓弧的交點(diǎn)為

將（11）代入（8），可以得到y(tǒng)C，即可以求得yA≠yO時(shí)點(diǎn)C坐標(biāo)。

2.2.2方案對(duì)比

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

表1　求兩圓交點(diǎn)算法對(duì)比Tab.1Comparison of algorithms

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

2.3圓弧與直線/圓弧間過(guò)渡方案

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

3　實(shí)驗(yàn)驗(yàn)證

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

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

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

圖5　點(diǎn)位加工速度曲線Fig.5PTP machining velocity curve

從速度曲線的對(duì)比可以得到如下結(jié)論：

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

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

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

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

4　結(jié)論

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

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

［2］葉佩青，趙慎良.微小直線段的連續(xù)插補(bǔ)控制算法研究［J］.中國(guó)機(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］呂強(qiáng)，張輝，楊開(kāi)明，等.基于實(shí)時(shí)性要求的多段預(yù)讀速度規(guī)劃及實(shí)驗(yàn)研究［J］.機(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］.組合機(jī)床與自動(dòng)化加工技術(shù)，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)小線段高速加工中的“拐點(diǎn)問(wèn)題”［J］.北京交通大學(xué)學(xué)報(bào)，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］趙志偉，陳學(xué)有，潘瓊.采用特征值法和Prony法相結(jié)合的PSS自適應(yīng)控制［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ī)組給水控制策略優(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稿件編號(hào)：201501053

廣東省科技計(jì)劃項(xiàng)目（2012A010702004；2012A090100012）

東梁（1989—），男，河南南陽(yáng)人，碩士。研究方向：高性能嵌入式控制系統(tǒng)。