陳日東，李迪，王世勇，張孫亞（華南理工大學(xué) 機(jī)械與汽車工程學(xué)院，廣東 廣州510640）

連續(xù)軌跡動(dòng)態(tài)前瞻插補(bǔ)算法研究

陳日東，李迪，王世勇，張孫亞
（華南理工大學(xué) 機(jī)械與汽車工程學(xué)院，廣東 廣州510640）

傳統(tǒng)連續(xù)軌跡加工過(guò)程由于需要頻繁變速而存在加工效率低，精度差的缺點(diǎn)。為此，提出一種面向連續(xù)軌跡的動(dòng)態(tài)前瞻算法。該算法根據(jù)加工軌跡的幾何特征、誤差、加減速要求，計(jì)算出連續(xù)軌跡各段之間銜接處的最大允許速度，實(shí)現(xiàn)連續(xù)軌跡的平滑過(guò)渡。在基于ARM+DSP+FPGA多核架構(gòu)的獨(dú)立式控制器中對(duì)所提算法進(jìn)行了實(shí)現(xiàn)和驗(yàn)證。結(jié)果表明本文所提的動(dòng)態(tài)前瞻算法與傳統(tǒng)算法相比加工效率提高20%以上。

連續(xù)軌跡；動(dòng)態(tài)前瞻算法；平滑過(guò)渡；運(yùn)動(dòng)控制器

現(xiàn)代高端工業(yè)應(yīng)用中，運(yùn)動(dòng)控制系統(tǒng)所需要處理的加工軌跡越來(lái)越復(fù)雜，速度與精度要求也不斷提高。然而，在傳統(tǒng)的運(yùn)動(dòng)控制過(guò)程中，控制系統(tǒng)對(duì)每段軌跡進(jìn)行獨(dú)立插補(bǔ)，并且每段軌跡的速度規(guī)劃均由零加速到目標(biāo)速度，最終減速停止[1]。在復(fù)雜多軌跡的加工過(guò)程中，這種速度規(guī)劃?rùn)C(jī)制不僅給電機(jī)、機(jī)床帶來(lái)了巨大的沖擊，同時(shí)還大大降低了加工效率[2-3]。在復(fù)雜的軌跡運(yùn)動(dòng)控制中，希望在保證控制的精度前提下，提高加工效率，減少對(duì)電機(jī)的沖擊[4-6]。因此，在保證控制精度的前提下，如何實(shí)現(xiàn)軌跡銜接處的平滑過(guò)渡，是解決復(fù)雜軌跡加工的技術(shù)關(guān)鍵[7-8]。為此，文中提出了一種動(dòng)態(tài)前瞻控制算法。此算法能夠根據(jù)軌跡的幾何特征、加工精度要求以及加減速要求，預(yù)先規(guī)劃軌跡的加工速度以及各段之間銜接處的過(guò)渡速度，從而實(shí)現(xiàn)復(fù)雜軌跡的平滑過(guò)渡。

1 連續(xù)軌跡的基本控制過(guò)程

文中研究的運(yùn)動(dòng)控制系統(tǒng)的指令處理過(guò)程如圖1所示。用戶的NC程序經(jīng)過(guò)譯碼過(guò)程之后，運(yùn)動(dòng)軌跡、速度指令以及加減速指令等信息將被提取[9]，軌跡優(yōu)化模塊將根據(jù)軌跡信息和加工的輪廓誤差要求對(duì)軌跡中一些極端加工段進(jìn)行軌跡優(yōu)化處理。加工的速度指令、加速度指令以及經(jīng)過(guò)優(yōu)化后的運(yùn)動(dòng)軌跡信息進(jìn)入速度前瞻模塊動(dòng)態(tài)計(jì)算相鄰兩段軌跡銜接處的銜接速度，并將各加工軌跡的起始速度、結(jié)束速度傳送給速度插補(bǔ)模塊。速度插補(bǔ)模塊根據(jù)軌跡的起始速度、結(jié)束速度等信息進(jìn)行速度規(guī)劃，計(jì)算生成插補(bǔ)速度，最后由位置控制模塊確定各個(gè)電機(jī)位置控制指令。

圖1 連續(xù)軌跡的基本控制流程

由于在實(shí)際的運(yùn)動(dòng)控制中有實(shí)時(shí)性要求，對(duì)于上述的控制過(guò)程需要搭建實(shí)時(shí)的運(yùn)行環(huán)境，本文在TI公司TMS320C6748數(shù)字處理器中為上述具有實(shí)時(shí)性要求的任務(wù)構(gòu)建DSP/BIOS運(yùn)行環(huán)境，在此環(huán)境中使用SWI（軟件中斷）服務(wù)函數(shù)實(shí)現(xiàn)實(shí)時(shí)任務(wù)調(diào)度策略，保證了任務(wù)調(diào)度的實(shí)時(shí)性[10]。譯碼任務(wù)由于實(shí)時(shí)性要求不高，所以無(wú)需運(yùn)行在TMS320C6748 的DSP/BIOS環(huán)境中，在DSP/BIOS環(huán)境中任務(wù)調(diào)度主要包括了位置控制模塊、插補(bǔ)模塊、動(dòng)態(tài)前瞻模塊及數(shù)據(jù)接收和反饋模塊。動(dòng)態(tài)前瞻模塊同時(shí)處理上述的軌跡優(yōu)化模塊與速度前瞻模塊，同時(shí)，TMS320C6748處理器需要接收加工指令，所以增加了數(shù)據(jù)接收模塊，各模塊任務(wù)屬性如表1所示。其中位置控制任務(wù)是基準(zhǔn)任務(wù)，任務(wù)內(nèi)部有特定的循環(huán)機(jī)制并決定了TMS320C6748最終輸出周期[11]。

表1TMS320C6748任務(wù)屬性

2 動(dòng)態(tài)前瞻控制算法的實(shí)現(xiàn)

動(dòng)態(tài)前瞻算法作用在于執(zhí)行插補(bǔ)算法前，根據(jù)加工軌跡的幾何信息、最大加減速，最大加工速度等因素提前規(guī)劃好軌跡的連續(xù)段之間的銜接處的加工速度[12-13]。在插補(bǔ)加減速控制過(guò)程中，借助于前瞻銜接處的速度，知道每段軌跡的起始、結(jié)束速度，從而實(shí)現(xiàn)在軌跡銜接處的高速過(guò)渡。

2.1 銜接速度的計(jì)算

加工一道連續(xù)軌跡，其示意圖如圖2所示，動(dòng)態(tài)前瞻任務(wù)需要根據(jù)機(jī)床的固有特性及上文所提到的幾個(gè)加工因素算出軌跡各銜接處速度Vi。銜接點(diǎn)速度Vi計(jì)算主要取決于以下幾個(gè)條件限制。

1）運(yùn)動(dòng)指令中的目標(biāo)加工速度F，F(xiàn)不允許超過(guò)機(jī)床各軸所允許的最大速度，一旦超過(guò)按照機(jī)床最大的允許加工速度處理。

2）前推速度Vf。如圖2所示，前推速度是指由Vi-1以最大的允許加速度走完所達(dá)到的速度。由公式V2f-V1i-1=2amaxLi可得

3）回溯速度Vb。回溯速度是指在連續(xù)軌跡的終點(diǎn)，以允許的最大加速度回溯到銜接點(diǎn)?；厮莸木嚯x是從Li+1到最后一段的總和，假設(shè)有n段，則回溯的距離是Σnj=i+1Lj。由此可得回溯速度為

實(shí)驗(yàn)過(guò)程參考劉俊懷的一種調(diào)節(jié)氫氣露點(diǎn)還原氧化鉬的方法[4]，一段還原分別選用氫氣露點(diǎn)-20 ℃、10 ℃和20 ℃的3種氣氛，原料及產(chǎn)品編號(hào)對(duì)應(yīng)工藝見表2；兩段還原采用相同工藝，具體如表3、表4所示。

4）在銜接處，根據(jù)機(jī)床允許的最大加速度以及銜接處的所限制的速度Vα。如圖2所示，兩直線的夾角是α，機(jī)床最大加速度amax，差補(bǔ)周期T，根據(jù)動(dòng)力學(xué)特性

5）誤差速度Ve。給定最大允許輪廓誤差E，直接過(guò)渡造成的e=Vi/2tan。所以Ve=2Etan。

最終確定：Vi=min（F，Vf，Vb，Vα，Ve）。

以上過(guò)程給出了直線間的銜接速度的計(jì)算。對(duì)于直線與圓弧，圓弧與圓弧之間銜接速度計(jì)算，可以利用類似的公式算出。對(duì)于圓弧銜接的情況，圓弧在銜接處的向量是圓弧在此點(diǎn)的切線向量。

圖2 前瞻示意圖

2.2 動(dòng)態(tài)前瞻算法具體實(shí)現(xiàn)

實(shí)時(shí)的動(dòng)態(tài)前瞻算法具體實(shí)現(xiàn)如圖3所示。當(dāng)前的控制系統(tǒng)以實(shí)時(shí)周期性線程的形式執(zhí)行動(dòng)態(tài)前瞻算法。前瞻算法的主要步驟如下：

1）進(jìn)入前瞻線程，首先判斷接收指令的隊(duì)列是否為空，如果指令的隊(duì)列為空，判斷是否已經(jīng)開始前瞻，如果沒有，整個(gè)前瞻線程處于空閑狀態(tài)，結(jié)束當(dāng)前線程。

2）如果指令隊(duì)列不為空，則需要先判斷接收指令標(biāo)志ReFlag和開始前瞻標(biāo)志StartFlag，這是為了保證在預(yù)處理隊(duì)列的數(shù)據(jù)順利進(jìn)行動(dòng)態(tài)前瞻。只有在滿足ReFlag=1、StartFlag=0標(biāo)志位的情況下，才會(huì)從指令隊(duì)列中逐段取出運(yùn)動(dòng)軌跡信息。僅當(dāng)一次連續(xù)加工任務(wù)所有加工段進(jìn)入預(yù)處理隊(duì)列或者進(jìn)入預(yù)處理隊(duì)列的加工段數(shù)目達(dá)到設(shè)定的最大值，才會(huì)真正做下一步的速度前瞻計(jì)算工作。

3）開始計(jì)算銜接速度的時(shí)候，程序先尋找“拐點(diǎn)”，所謂的拐點(diǎn)指銜接處兩段軌跡的夾角小于所設(shè)定的值MinAngle。如果尋的拐點(diǎn)則將此銜接點(diǎn)加工處理為準(zhǔn)停形式，且在計(jì)算回溯速度的時(shí)候，Σnj=i+1Lj中 n的值是 k，即回溯距離是拐點(diǎn)的前 i+ 1～k段總和。如果沒有拐點(diǎn)，則以預(yù)處理中的所有線段的總長(zhǎng)作為回溯距離，回溯距離不同。接著計(jì)算出另外的幾個(gè)限制速度，最終確定銜接處的銜接速度。

圖3 動(dòng)態(tài)前瞻具體實(shí)現(xiàn)

4）如果預(yù)處理的隊(duì)列中數(shù)據(jù)全部處理完，則認(rèn)為此次前瞻結(jié)束，將處理標(biāo)志位ReFlag、StartFlag，為重新接收數(shù)據(jù)做準(zhǔn)備工作。否則，等待下次進(jìn)入前瞻線程，繼續(xù)進(jìn)行速度前瞻計(jì)算。

2.3 銜接處的插補(bǔ)處理

經(jīng)過(guò)動(dòng)態(tài)前瞻處理，軌跡中每加工段的起始速度和結(jié)束速度都已經(jīng)確定。經(jīng)過(guò)S/T型速度規(guī)劃，在每一加工段末端的最后一個(gè)插補(bǔ)周期無(wú)法保證位移與速度同時(shí)達(dá)到理想的值，因此，處理好銜接點(diǎn)軌跡的過(guò)渡顯得尤為重要[14-15]。本文采用直接過(guò)渡的方式進(jìn)行過(guò)渡。

如圖4所示，當(dāng)軌跡走到（XlYl）時(shí)候，剩余距離LR，銜接速度是Vnow，LR小于當(dāng)前速度Vnow，如果按照原來(lái)的速度大小和方向進(jìn)行加工，則會(huì)偏離加工軌跡。為了避免這類加工誤差，本文采用直接過(guò)渡方式改變當(dāng)前周期的目標(biāo)位置，讓其落入在下一段加工軌跡中，從而實(shí)現(xiàn)相鄰軌跡的銜接過(guò)渡。假設(shè)過(guò)渡到下一段的位置是（XnYn），則（XnYn）是以（XiYj）為圓心，以R=Vnow-LR為半徑的圓弧與下一段的交點(diǎn)。只需要求出（XnYn），就可以求出當(dāng)前周期各軸的速度。

計(jì)算（XnYn）。首先求出（XiYj）至（Xi-1Yj+1）單位向量（vivj），由此可以得知：

（XnYn）=（Xi+Ni*（Vnow-LR）Yj+vj*（Vnow-LR））。則該周期應(yīng)發(fā)送的速度（VxVy）=（Xn-XiYn-Yj）。如果是直線與圓弧或者圓弧與圓弧之間的過(guò)過(guò)渡，利用相同的原理同樣可以算出該周期各軸的速度。

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

為了驗(yàn)證動(dòng)態(tài)前瞻算法的可行性和優(yōu)越性，搭建了如圖5所示的實(shí)驗(yàn)平臺(tái)。此實(shí)驗(yàn)平臺(tái)是一個(gè)直角坐標(biāo)點(diǎn)膠機(jī)器人平臺(tái)，主要由示教盒、驅(qū)動(dòng)器、運(yùn)動(dòng)部件以及運(yùn)動(dòng)控制系統(tǒng)組成。此運(yùn)動(dòng)控制系統(tǒng)是基于ARM+DSP+FPGA多核架構(gòu)的獨(dú)立式控制器，控制器中ARM模塊采用了TI公司的AM3352芯片，DSP模塊采用了TI公司TMS320C6748芯片，其主要完成運(yùn)動(dòng)軌跡動(dòng)態(tài)前瞻算法，插補(bǔ)算法、位置控制、誤差控制等功能，研究的動(dòng)態(tài)前瞻算法在此處理器中實(shí)現(xiàn)。FPGA模塊采用了 ALTERA公司EP4CE15F23CN8。

圖4 直線間過(guò)渡

圖5 實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)在TMS320C6748處理器中采集一個(gè)加工文件的在位置控制模塊插補(bǔ)位移及其插補(bǔ)速度進(jìn)行分析，并通過(guò)Matlab描繪其運(yùn)動(dòng)控制曲線。加工文件的位移曲線和速度如圖6所示。軌跡加工位移控制曲線如圖6（a）所示，軌跡由直線和圓弧組合而成，順序是ABCDEA。給定的位移是以脈沖（pulse）為單位，相應(yīng)的速度已轉(zhuǎn)成以pulse/T單位，實(shí)驗(yàn)的位置控制模塊周期T=0.1 ms，目標(biāo)速度是10 prlse/T，加速度是0.05 pulse/T2。

實(shí)驗(yàn)分別對(duì)動(dòng)態(tài)前瞻加工軌跡和傳統(tǒng)模式下的加工軌跡速度曲線進(jìn)行分析，如圖6（b）曲線所示。

傳統(tǒng)連續(xù)模式加工軌跡的速度曲線如圖6（b）中準(zhǔn)停模式曲線所示。動(dòng)態(tài)前瞻加工軌跡的速度曲中連續(xù)運(yùn)動(dòng)曲線所示。對(duì)比兩種模式下的速度曲線，可以明顯看到傳統(tǒng)加工連續(xù)軌跡速度變化頻繁，幅度大，而進(jìn)行動(dòng)態(tài)前瞻加工軌跡可以實(shí)現(xiàn)軌跡間的高速過(guò)渡，速度變化平緩。在加工效率上，傳統(tǒng)模式加工消耗4 190個(gè)插補(bǔ)周期，而進(jìn)行前瞻模式加工消耗僅3 285個(gè)插補(bǔ)周期，效率提高了高達(dá)21.6%。

圖6 加工的位移曲線和速度曲線

4 結(jié) 論

文中在ARM+DSP+FPGA分布式運(yùn)動(dòng)控制系統(tǒng)上，對(duì)連續(xù)軌跡的動(dòng)態(tài)前瞻算法進(jìn)行了研究，并進(jìn)行了實(shí)驗(yàn)驗(yàn)證。通過(guò)分析可知，動(dòng)態(tài)前瞻算法不僅可以提高加工效率20%以上，還可以減少電機(jī)的頻繁變速，減少對(duì)電機(jī)的沖擊。

[1]陳錚杰.嵌入式數(shù)控系統(tǒng)微小線段平滑插補(bǔ)算法研究[D].南京：南京航空航天大學(xué)，2014.

[2]潘海鴻，楊增啟，陳琳，等.一種優(yōu)化軌跡段間銜接速度的自適應(yīng)前瞻控制 [J].機(jī)械工程學(xué)報(bào)，2015（5）:151-159.

[3]史中權(quán)，葉文華.多軸聯(lián)動(dòng)條件下插補(bǔ)速度實(shí)時(shí)可調(diào)的前瞻控制算法[J].航空學(xué)報(bào)，2014（2）:582-592.

[4]劉強(qiáng)，劉煥，周勝凱，等.連續(xù)多類型曲線段進(jìn)給速度前瞻規(guī)劃 [J].計(jì)算機(jī)集成制造系統(tǒng)，2015 （9）:2369-2377.

[5]張曉輝，于東，鄭飂默，等.面向高速加工的微小線段平滑過(guò)渡算法研究 [J].小型微型計(jì)算機(jī)系統(tǒng)，2011（1）:177-180.

[6]張力.NURBS曲線插補(bǔ)的速度前瞻控制算法的研究設(shè)計(jì)[D].沈陽(yáng)：中國(guó)科學(xué)院研究生院（沈陽(yáng)計(jì)算技術(shù)研究所），2014.

[7]王海濤.數(shù)控系統(tǒng)速度前瞻控制算法及其實(shí)現(xiàn)[D].南京：南京航空航天大學(xué)，2011.

[8]許東偉.多軸數(shù)控系統(tǒng)的運(yùn)動(dòng)控制算法研究與開發(fā)[D].廣州：廣東工業(yè)大學(xué)，2015.

[9]董靖川，王太勇，王自靜，等.通用型前瞻速度規(guī)劃算法[J].計(jì)算機(jī)集成制造系統(tǒng)，2013（3）:529-539.

[10]Texas Instruments.TMS320C6748TM Fixed-and Floating-Point DSP [EB/OL].[2014-12-20]. http://www.ti.com/lit/ds/symlink/tms320c6748.pdf

[11]李松，李迪，翟振坤.基于ARM+DSP+FPGA+MCU的高速嵌入式數(shù)控系統(tǒng)研究 [J].機(jī)床與液壓，2013（3）:89-92.

[12]卜凡，樊留群，齊黨進(jìn).數(shù)控加工速度前瞻及插補(bǔ)算法的研究[J].機(jī)械與電子，2013（8）:44-47.

[13]王海濤.數(shù)控系統(tǒng)速度前瞻控制算法及其實(shí)現(xiàn)[D].南京：南京航空航天大學(xué)，2011.

[14]東梁，李迪，粟明，等.連續(xù)軌跡平滑過(guò)渡控制算法研究[J].電子設(shè)計(jì)工程，2015（20）:96-99.

[15]馮強(qiáng)，胡毅，于東，等.基于多核嵌入式數(shù)控系統(tǒng)的速度前瞻算法研究[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2013（3）:16-20.

Study of a dynamic look-ahead algorithm for continuous trajectory interpolation

CHEN Ri-dong，LI Di，WANG Shi-yong，ZHANG Sun-ya
（School of Mechanical and Automotive Engineering，South China University of Technology，Guangzhou 510640，China）

Due to frequently speed changing，poor precision and low efficiency are still defects of processing continuous trajectory in a traditional way.To figure out what mentioned above，a dynamic look-ahead algorithm for processing continuous trajectory is proposed in this paper.For continuous trajectory smooth transition，the highest permitted speed at the joint should be figured out，which must depend on geometric features of continuous trajectory，processing error and required acceleration and deceleration.The dynamic look-ahead algorithm was implemented with the multi-core and Independent motion controller，which is based on ARM，DSP and FPGA processers.At last itwas verified that processing continuous trajectory with this algorithm was more than 20%faster than processing continuous trajectory in a traditional way.

continuous trajectory；dynamic look-ahead algorithm；smooth transition；motion controller

TN805

A

1674－6236（2017）07-0152-04

2016-05-31稿件編號(hào)：201605313

國(guó)家科技支撐計(jì)劃項(xiàng)目資助 （2015BAF20B01）；廣東省科技計(jì)劃項(xiàng)目（2009B010900016；2012A090100012；2013B010134010；2014B090921003；2014A050503009）；廣州市科技計(jì)劃項(xiàng)目資助（201508030007；201604010064）

陳日東（1991—），男，廣東河源人，碩士。研究方向：高性能嵌入式控制系統(tǒng)。