一種無直線逼近的高速高精度圓弧插補(bǔ)算法*

李偉光，曾順星，葉國強(qiáng)，余 漳

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

0 引言

在數(shù)控系統(tǒng)中，為了提高插補(bǔ)運(yùn)算的效率，由軟件實(shí)現(xiàn)粗插補(bǔ)、硬件實(shí)現(xiàn)精插補(bǔ)的兩級插補(bǔ)得到廣泛應(yīng)用。這種方式充分利用了軟件的柔性和硬件的快速性、可靠性。為實(shí)現(xiàn)曲線軌跡的運(yùn)動定位，在傳統(tǒng)的兩級插補(bǔ)中，粗插補(bǔ)通常采用直線逼近曲線，然后對直線進(jìn)行精插補(bǔ)。然而直線逼近帶來了決定插補(bǔ)軌跡輪廓精度的誤差，需要引入復(fù)雜的誤差控制機(jī)制;帶來了速度與精度的矛盾;帶來了大量離散化的微小直線段，造成數(shù)據(jù)存儲和通信的負(fù)擔(dān)及速度的波動，速度波動進(jìn)一步導(dǎo)致無法實(shí)現(xiàn)高速進(jìn)給。在曲線曲率變化大的情況下，這種現(xiàn)象尤為突出。

1 無直線逼近的圓弧插補(bǔ)算法原理

傳統(tǒng)的兩級插補(bǔ)算法由直線段組成的粗插補(bǔ)和微小直線段組成的精插補(bǔ)構(gòu)成。兩級插補(bǔ)的原理框圖如圖1 所示:

圖1 兩級插補(bǔ)原理框圖

第一級插補(bǔ)預(yù)先執(zhí)行，生成允許的最大進(jìn)給步長。第二級插補(bǔ)調(diào)整進(jìn)給步長來減小運(yùn)動波動［1-2］。例如:圓弧插補(bǔ)中，粗插補(bǔ)以較大的弦線、切線或割線逼近圓弧，然后用微小線段進(jìn)行直線精插補(bǔ)。

無直線逼近的高速高精度圓弧插補(bǔ)算法，粗插補(bǔ)不再以直線逼近曲線，直接以弧長為進(jìn)給量計(jì)算出下一插補(bǔ)點(diǎn)，然后進(jìn)行圓弧精插補(bǔ)，以避免直線逼近帶來的逼近誤差、精度與速度的矛盾和速度波動的問題。

下面以平面圓弧插補(bǔ)為例詳述無直線逼近的圓弧插補(bǔ)算法的原理。

1.1 圓弧粗插補(bǔ)

圓弧粗插補(bǔ)基于時(shí)間分割法，采用極坐標(biāo)系描述目標(biāo)曲線。以弧長為進(jìn)給量，以含方向的圓心角為增量，以其正負(fù)判斷進(jìn)給方向，計(jì)算出下一插補(bǔ)點(diǎn)的坐標(biāo)［3］。不失一般性，在X-Y平面上，目標(biāo)曲線圓弧方程為

目標(biāo)曲線始于點(diǎn)S(Xs，Ys)，終止于點(diǎn)E(Xe，Ye)，當(dāng)前點(diǎn)是Pi(Xi，Yi)，下一插補(bǔ)點(diǎn)是Pi+1(Xi+1，Yi+1)。如圖2 所示:

圖2 圓弧粗插補(bǔ)示意圖

設(shè)當(dāng)前的插補(bǔ)周期為Ts(ms)，進(jìn)給速度為F(mm/min)，以圓弧PiPi+1的弧長ΔL(μm)為進(jìn)給量可得:

圓弧Pi Pi+1對應(yīng)的圓心角θ(rad)為:

根據(jù)圓弧的參數(shù)方程有:

根據(jù)圖2 和式(4)，下一插補(bǔ)點(diǎn)的參數(shù)方程為:

式(5)描述了下一插補(bǔ)點(diǎn)的坐標(biāo)值該如何計(jì)算。通過式(3)算出θ，再帶入式(5)即可求出下一插補(bǔ)點(diǎn)。

為避免硬件計(jì)算精度限制帶來的累積誤差，確保插補(bǔ)點(diǎn)始終落在圓弧上，算法采用如下計(jì)算規(guī)則來計(jì)算(Xi+1，Yi+1)。首先由Xi+1= Xicosθ- Yisinθ 計(jì)算出Xi+1，再由(6)計(jì)算出Yi+1:

式(6)中的Yi+1的正負(fù)可在算法中根據(jù)θ 的值判斷。

各軸的位移量為:

1.2 圓弧精插補(bǔ)

在圓弧精插補(bǔ)階段，基于數(shù)字積分法，根據(jù)圓弧粗插補(bǔ)運(yùn)算得到當(dāng)前插補(bǔ)點(diǎn)坐標(biāo)、下一插補(bǔ)點(diǎn)坐標(biāo)和順逆圓等信息，為各坐標(biāo)軸分配脈沖，最終生成圓弧軌跡。

數(shù)字積分法的基本思想，就是把插補(bǔ)看作是插補(bǔ)點(diǎn)在各坐標(biāo)軸分量分別累積的過程。坐標(biāo)值應(yīng)該每經(jīng)過一個(gè)時(shí)間間隔，累積一個(gè)單位位移。每個(gè)坐標(biāo)軸分別分配一個(gè)由余數(shù)寄存器和被積函數(shù)寄存器組成的積分器。每經(jīng)過一個(gè)時(shí)間間隔，余數(shù)寄存器的值就與被積函數(shù)寄存器的值累加，當(dāng)余數(shù)寄存器溢出就發(fā)出一個(gè)進(jìn)給脈沖，否則，繼續(xù)累加，并把余數(shù)保存在余數(shù)寄存器。

傳統(tǒng)的數(shù)字積分法的主要缺點(diǎn)是累加過程的時(shí)鐘頻率固定。確定寄存器位數(shù)后，都要經(jīng)過固定次數(shù)的累加運(yùn)算才能完成插補(bǔ)運(yùn)算，無論目標(biāo)軌跡長短。所以進(jìn)給時(shí)間不可改變。即目標(biāo)軌跡越長，插補(bǔ)速度越快，反之亦然。傳統(tǒng)數(shù)字積分法的另一缺點(diǎn)是造成各坐標(biāo)軸在小線段銜接處進(jìn)給速度的突變［1］。

通過速度規(guī)劃算法生成速度變化平穩(wěn)的軌跡，具體過程如下:①獲得每個(gè)粗插補(bǔ)周期的進(jìn)給率。②對精插補(bǔ)模塊的時(shí)鐘頻率進(jìn)行分頻，即改變在數(shù)字積分法中的時(shí)鐘頻率。經(jīng)過如上所述的處理，雖然目標(biāo)曲線長度不一樣時(shí)積分器的累加次數(shù)固定不變，但累加的快慢改變，累加運(yùn)算的時(shí)間改變。累加運(yùn)算的時(shí)間決定了進(jìn)給速度，即通過變頻實(shí)現(xiàn)了數(shù)字積分法的速度控制。

設(shè)脈沖當(dāng)量為δ(mm)，則進(jìn)給速度F轉(zhuǎn)換成進(jìn)給脈沖的頻率為f(Hz):

設(shè)硬件精插補(bǔ)模塊的時(shí)鐘源的頻率為f_origin(MHz)，則其分頻系數(shù)divider_origin與進(jìn)給脈沖的頻率f(Hz)的關(guān)系如式(9):

通過過象限處理實(shí)現(xiàn)任意弧長(包括微小圓弧、近似整圓或整圓)、任意象限的圓弧精插補(bǔ)。過象限處理的基本思路，就是以(R，0)、(0，R)、(- R，0)和(0，-R)為分界點(diǎn)，把圓弧劃分為四個(gè)象限，然后根據(jù)動點(diǎn)的坐標(biāo)判斷其所屬象限，在不同象限根據(jù)不同的進(jìn)給規(guī)律進(jìn)行插補(bǔ)。把分界點(diǎn)看作跨象限圓弧跨象限前的臨時(shí)終點(diǎn)。根據(jù)半徑R和坐標(biāo)值的大小關(guān)系判斷動點(diǎn)是否即將過象限，以分界點(diǎn)為臨時(shí)終點(diǎn)控制各軸積分器啟停。

為了對坐標(biāo)軸上的點(diǎn)進(jìn)行統(tǒng)一處理，算法對“象限”進(jìn)行重新定義。非坐標(biāo)軸上的點(diǎn)，象限的定義不變;坐標(biāo)軸上的點(diǎn)，象限的定義為下一插補(bǔ)點(diǎn)所屬象限。算法對坐標(biāo)點(diǎn)所屬象限定義如表1 所示:

表1 坐標(biāo)點(diǎn)所屬象限

對于圓弧精插補(bǔ)，X軸和Y軸的被積函數(shù)寄存器分別存儲Y軸和X軸的動點(diǎn)坐標(biāo)值的絕對值。只要在插補(bǔ)過程中計(jì)算出動點(diǎn)的坐標(biāo)值，即可根據(jù)表1 判斷動點(diǎn)所屬象限，然后根據(jù)表2 的規(guī)律得出進(jìn)給方向，并在動點(diǎn)坐標(biāo)變化時(shí)，修正被積函數(shù)寄存器的值。

在圓弧精插補(bǔ)過程中，X軸坐標(biāo)值累積過程中的溢出脈沖被當(dāng)作Y軸的進(jìn)給脈沖，反之亦然。基于數(shù)字積分法的圓弧精插補(bǔ)，在X軸和Y軸不是同時(shí)到達(dá)終點(diǎn)，所以兩軸都要進(jìn)行終點(diǎn)判別，只有兩軸都到達(dá)終點(diǎn)才認(rèn)為到達(dá)終點(diǎn)。判別條件如式(10)所示:

式中JX和JY分別是X坐標(biāo)和Y坐標(biāo)進(jìn)給脈沖總數(shù)。

圓弧精插補(bǔ)的流程如圖3 所示。其中JVX 和JVY分別代表X軸和Y軸被積函數(shù)寄存器，JRX 和JRY 分別代表X軸和Y軸余數(shù)寄存器。

圖3 圓弧精插補(bǔ)流程圖

2 算法的MATLAB 仿真

不失一般性，設(shè)整圓的起點(diǎn)坐標(biāo)S和終點(diǎn)坐標(biāo)E是同一個(gè)點(diǎn)。在X-Y平面的坐標(biāo)為(1.200，1.600)，單位為mm。脈沖當(dāng)量為1 μm，粗插補(bǔ)的插補(bǔ)周期Ts為1ms。

采用內(nèi)接弦線逼近圓弧，即弦線逼近的兩級圓弧插補(bǔ)算法(傳統(tǒng)算法)時(shí)，若令er為圓弧粗插補(bǔ)允許的最大徑向誤差，R為圓弧半徑，則進(jìn)給步長應(yīng)滿足:

若er為10 μm，則ΔL≤400μm，且由式(2)可知F≤24000(mm/min)。若最大徑向誤差er減小到2 μm，則F≤10733(mm/min)。

進(jìn)給速度F分別為 24000(mm/min) 和10733(mm/min)時(shí)，在MATLAB 中仿真的傳統(tǒng)算法和本文的無直線逼近的兩級圓弧插補(bǔ)算法(簡稱本文算法)的插補(bǔ)軌跡如圖4 所示，進(jìn)給速度F為24000(mm/min)時(shí)插補(bǔ)軌跡的局部放大圖如圖5 所示，傳統(tǒng)算法和本文算法的圓弧插補(bǔ)的徑向誤差曲線如圖6 所示。

圖4 不同進(jìn)給速度的圓弧插補(bǔ)軌跡

圖5 圓弧插補(bǔ)軌跡局部放大圖

圖6 不同進(jìn)給速度的圓弧插補(bǔ)誤差曲線

據(jù)圖5 可知本文算法的粗插補(bǔ)的插補(bǔ)點(diǎn)比傳統(tǒng)算法更加精確，插補(bǔ)軌跡更加接近給定圓弧軌跡;傳統(tǒng)算法在直線段銜接處有速度波動，而本文算法沒有。

據(jù)圖6 可知，本文算法在進(jìn)給速度改變時(shí)，徑向誤差不變;傳統(tǒng)算法存在精度與效率的矛盾。這是因?yàn)楸疚脑O(shè)計(jì)的兩級算法粗插補(bǔ)理論誤差較小，插補(bǔ)精度僅由精插補(bǔ)的精度，即脈沖當(dāng)量決定。所以對于本文算法可以提高進(jìn)給速度或延長粗插補(bǔ)周期，而不影響插補(bǔ)精度。

3 無直線逼近的圓弧插補(bǔ)算法的實(shí)現(xiàn)

采用如圖7 所示的基于DSP(數(shù)字信號處理器)+FPGA(現(xiàn)場可編程門陣列)的運(yùn)動控制器實(shí)現(xiàn)本文設(shè)計(jì)的圓弧插補(bǔ)算法。該運(yùn)動控制器架構(gòu)如圖7 所示，實(shí)物圖如圖8 所示。高速高精度的本文算法可在該運(yùn)動控制器架構(gòu)上實(shí)現(xiàn):首先在DSP 上執(zhí)行圓弧粗插補(bǔ);然后將粗插補(bǔ)結(jié)果通過DSP 的XINTF(外部接口)接口將粗插補(bǔ)結(jié)果發(fā)送到FPGA;最后在FPGA 上執(zhí)行圓弧精插補(bǔ)，并發(fā)送脈沖到伺服驅(qū)動器以驅(qū)動電機(jī)［5-7］。

表2 進(jìn)給方向及坐標(biāo)絕對值變化規(guī)律

3.1 基于DSP 的軟件粗插補(bǔ)

上述運(yùn)動控制器中的DSP 采用32 位浮點(diǎn)數(shù)處理器—TMS320F28335，它專用于控制領(lǐng)域。在運(yùn)動控制器中DSP 用于執(zhí)行基于時(shí)間分割法的軟件粗插補(bǔ)。

圖7 基于DSP+FPGA 的運(yùn)動控制器

圖8 雙軸運(yùn)動控制實(shí)驗(yàn)平臺

TMS320F28335 有著出色的三角函數(shù)運(yùn)算能力，首先它有可進(jìn)行三角函數(shù)運(yùn)算的IQmath 運(yùn)算庫，其次它可通過查表法完成三角函數(shù)運(yùn)算。這些都保證了插補(bǔ)運(yùn)算的精度和實(shí)時(shí)性。為減少運(yùn)算消耗，在編程中可先計(jì)算sinθ(或cosθ)，然后通過式(12)計(jì)算cosθ(或sinθ)

式(12)中的sinθ、cosθ 的正負(fù)可在程序中根據(jù)θ 的值判斷。

圓弧粗插補(bǔ)通過以C 語言或匯編語言在集成開發(fā)環(huán)境CCSv3.3 中編寫的DSP 程序的執(zhí)行而實(shí)現(xiàn)。DSP 程序執(zhí)行過程如下:

(1)在DSP 中將待插補(bǔ)平面圓弧通過坐標(biāo)平移統(tǒng)一成圓心位于坐標(biāo)原點(diǎn)的形式;

(2)根據(jù)平移后的圓弧起點(diǎn)坐標(biāo)、終點(diǎn)坐標(biāo)和進(jìn)給方向等信息進(jìn)行采用時(shí)間分割法，以弧長為進(jìn)給量進(jìn)行圓弧粗插補(bǔ);

(3)圓弧粗插補(bǔ)的結(jié)果數(shù)據(jù)通過XINTF 接口把發(fā)送到FPGA。

3.2 基于FPGA 的硬件精插補(bǔ)

圓弧精插補(bǔ)通過以Vreilog HDL 語言在集成開發(fā)環(huán)境QuartusII 中編寫的FPGA 程序的執(zhí)行而實(shí)現(xiàn)。FPGA 根據(jù)圓弧粗插補(bǔ)結(jié)果采用數(shù)字積分法進(jìn)行圓弧精插補(bǔ)，并把圓弧精插補(bǔ)的結(jié)果以脈沖的形式發(fā)送到伺服驅(qū)動器用于驅(qū)動電機(jī)，并通過XINTF 接口反饋狀態(tài)信息到DSP，然后請求下一次圓弧精插補(bǔ)的數(shù)據(jù)。

采用Verilog HDL 語言實(shí)現(xiàn)基于數(shù)字積分法的硬件精插補(bǔ)模塊分頻。如前所述，分頻模塊式硬件精插補(bǔ)模塊的子模塊，它實(shí)現(xiàn)了數(shù)字積分發(fā)的運(yùn)算速度控制。即分頻模塊使得所提出的算法實(shí)現(xiàn)了高速運(yùn)動控制。FPGA 程序中的分頻模塊的部分源代碼如下:

“clkin”代表FPGA 上來自晶振的初始時(shí)鐘輸入，且等于式(9)中的f_origin;divider_origin 等于式(9)中的divider_origin;“clkout”代表經(jīng)過分頻后的時(shí)鐘輸出;“rst”表示FPGA 復(fù)位，“en”表示精插補(bǔ)使能。

分頻模塊程序流程如圖9 所示:

圖9 分頻模塊程序流程

按照上述思路實(shí)現(xiàn)的本文算法可以控制運(yùn)動平臺插補(bǔ)出精確的目標(biāo)曲線(平面圓弧)軌跡。

4 結(jié)論

在傳統(tǒng)算法的粗插補(bǔ)階段，逼近誤差導(dǎo)致了運(yùn)動速度與精度的矛盾。為實(shí)現(xiàn)高速高精度的運(yùn)動控制，需要縮短插補(bǔ)周期，卻又不能無限制的縮短。因?yàn)椴逖a(bǔ)周期的大小由運(yùn)動控制器的硬件性能限制。

在本文算法的粗插補(bǔ)階段，以目標(biāo)曲線的最大允許弧長為進(jìn)給量，避免了線性逼近誤差。所以本文算法的插補(bǔ)精度主要由精插補(bǔ)階段決定。在滿足插補(bǔ)精度的前提下，可增大進(jìn)給率、延長插補(bǔ)周期，所以可實(shí)現(xiàn)高速高精度的運(yùn)動控制。通過二維運(yùn)動平臺上的圓弧插補(bǔ)驗(yàn)證了本文算法的實(shí)用性。

無直線逼近的圓弧插補(bǔ)算法適合于復(fù)雜曲線、大曲率圓弧的高速高精度運(yùn)動控制;無直線逼近的思路可以推廣到更復(fù)雜的曲線插補(bǔ)。

［1］Jinho Park，Sungho Nam and Minyang Yang，Development of a realtime trajectory generator for NURBS interpolation based on the two stage interpolation method，Int J Adv Manuf Technol，26(2005):359 -365.

［2］X. Zhiming，C. Jincheng and F. Zhengjin，Performance Evaluation of a Real-Time Interpolation Algorithm for NURBS Curves，Int J Adv Manuf Technol，20(2002):270 -276.

［3］劉宜，叢爽，錢煒，等.一種基于時(shí)間分割法和數(shù)字積分法混合實(shí)現(xiàn)的空間直線插補(bǔ)方法［J］.機(jī)械設(shè)計(jì)，2008(25)12:27 -30.

［4］羅良玲，劉旭波. 基于時(shí)間分割法的圓柱螺旋線直接插補(bǔ)算法［J］.南昌大學(xué)學(xué)報(bào)，2001(23)4:57 -59.

［5］J. B. Huang，Z. W. Xie，H. Liu，K. Sun，Y. C. Liu and Z. N. Jiang，DSP/FPGA-based Controller Architecture for Flexible Joint Robot with Enhanced Impedance Performance，J Intell Robot Syst，53(2008):247 -261.

［6］Bin Yao，Mohammed Al-Majed and Masayoshi Tomizuka，High-Performance Robust Motion Control of Machine Tools:An Adaptive Robust Control Approach and Comparative Experiments，IEEE/ASME Trans. Mechatron，2 (1997):63 -76.

［7］HO Seong Lee and Masayoshi Tomizuka，Robust Motion Controller Design for High-accuracy Positioning Systems，IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS，43(1996):48 -55.

［8］ TI Corporation， TMS320F28335， TMS320F28334，TMS320F28332， TMS320F28235， TMS320F28234，TMS320F28232 Digital Signal Controllers (DSCs)，The technology document of TI Company，2007.

［9］Hua Qiu，Kai Cheng and Yan Li，Optimal circular arc interpolation for NC tool path generation in curve contour manufacturing，Computer-Aided Design，29(1997):751 -760.

［10］Michael D. Ciletti ，Advanced Digital Design with the Verilog(TM)HDL，Prentice Hall，Upper Saddle River，NJ;2002.