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

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

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

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

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

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

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

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

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

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

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

表1TMS320C6748任務屬性

2 動態(tài)前瞻控制算法的實現(xiàn)

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

2.1 銜接速度的計算

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

1）運動指令中的目標加工速度F，F(xiàn)不允許超過機床各軸所允許的最大速度，一旦超過按照機床最大的允許加工速度處理。

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

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

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

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

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

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

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

圖2 前瞻示意圖

2.2 動態(tài)前瞻算法具體實現(xiàn)

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

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

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

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

圖3 動態(tài)前瞻具體實現(xiàn)

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

2.3 銜接處的插補處理

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

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

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

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

3 實例驗證

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

圖4 直線間過渡

圖5 實驗平臺

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

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

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

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

4 結(jié) 論

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

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

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

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

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

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

[6]張力.NURBS曲線插補的速度前瞻控制算法的研究設計[D].沈陽：中國科學院研究生院（沈陽計算技術研究所），2014.

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

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

[9]董靖川，王太勇，王自靜，等.通用型前瞻速度規(guī)劃算法[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].機床與液壓，2013（3）:89-92.

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

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

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

[15]馮強，胡毅，于東，等.基于多核嵌入式數(shù)控系統(tǒng)的速度前瞻算法研究[J].組合機床與自動化加工技術，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稿件編號：201605313

國家科技支撐計劃項目資助 （2015BAF20B01）；廣東省科技計劃項目（2009B010900016；2012A090100012；2013B010134010；2014B090921003；2014A050503009）；廣州市科技計劃項目資助（201508030007；201604010064）

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