# FPGA implementation of higher degree polynomial ... ?· These discontinuities can produce undesirable…

Post on 04-Jun-2018

212 views

TRANSCRIPT

ARTICLE IN PRESS

0736-5845/$ - se

doi:10.1016/j.rc

CorrespondE-mail addr

Robotics and Computer-Integrated Manufacturing 25 (2009) 379392

www.elsevier.com/locate/rcim

FPGA implementation of higher degree polynomial acceleration profilesfor peak jerk reduction in servomotors

Roque Alfredo Osornio-Riosa,, Rene de Jesus Romero-Troncosob,Gilberto Herrera-Ruiza, Rodrigo Castaneda-Mirandaa

aFacultad de Ingeniera, Universidad Autonoma de Queretaro Cerro de las Campanas s/n, 76010 Queretaro, Qro., MexicobElectronics Department, FIMEE, Universidad de Guanajuato, Tampico 912, Col. Bellavista, 36720 Salamanca, Gto., Mexico

Received 14 December 2006; received in revised form 26 September 2007; accepted 29 January 2008

Abstract

Acceleration profile generation for jerk limitation is a major issue in automated industrial applications like computer numerical

control (CNC) machinery and robotics. The automation machinery dynamics should be kept as smooth as possible with suitable

controllers where trajectory precision ensures quality while smoothness decreases machinery stress. During the operation of

commercially available CNC and robotics controllers, small discontinuities on the dynamics are generated due to the controller position

profiler which is generally based on a trapezoidal velocity profile. These discontinuities can produce undesirable high-frequency

harmonics on the position reference which consequentially can excite the natural frequencies of the mechanical structure and

servomotors. Previous works have developed jerk limited trajectories with higher degree polynomial-based profiles, but lack one or both

of computer efficiency for on-line operation and low-cost hardware implementation. The present work shows a low cost,

computationally efficient, on-line hardware implementation of a high-degree polynomial-based profile generator with limited jerk

dynamics for CNC machines and robotics applications to improve the machining process. The novelty of the paper is the development of

a multiplier-free recursive algorithm for computationally efficient polynomial evaluation in profile generation and a low-cost

implementation of the digital structure in field programmable gate array (FPGA). Two experimental setups were prepared in order to test

the polynomial profile generator: the first one with the servomotor at no load and the second one for the servomotor driving a CNC

milling machine axis. From experimental results it is shown that higher degree polynomial profiles, compared to the standard trapezoidal

speed profile improve the system dynamics by reducing peak jerk in more than one order of magnitude while precision is maintained the

same and on-line operation is guaranteed.

r 2008 Elsevier Ltd. All rights reserved.

Keywords: CNC machinery; Robotics; Jerk; Polynomial profile generator; FPGA

1. Introduction

Nowadays, computer numerical control (CNC) ma-chines and robotics are widely spread in automatedindustrial applications, which improve end product quality,reduce production time and increase profits in the shortand long time, despite the high initial investment. In orderto guarantee an optimal relationship between productquality and production cost, the automation machinery

e front matter r 2008 Elsevier Ltd. All rights reserved.

im.2008.01.002

ing author. Tel./fax: +52 427 2741244.

ess: raor@uaq.mx (R.A. Osornio-Rios).

dynamics should be kept as smooth as possible withsuitable controllers where trajectory precision ensuresquality while smoothness decreases machinery stress.During the operation of commercially available CNC

and robotics controllers, small discontinuities on thedynamics are generated due to the controller positionprofile generator which is generally based on a trapezoidalvelocity profile. The discontinuities on the trajectorydynamics can produce undesirable high-frequency harmo-nics on the position reference, which consequentially canexcite the natural frequencies of the mechanical structureand servomotors. Such high-frequency harmonics can also

www.elsevier.com/locate/rcimdx.doi.org/10.1016/j.rcim.2008.01.002mailto:raor@uaq.mx

ARTICLE IN PRESSR.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392380

saturate the actuators and therefore, decrease the positiontrajectory precision that affects the effective contouringprocess. Linear and circular interpolation techniques havebeen proposed to minimize the discontinuities; however,these techniques have limited efficiency when the geometryof the machining process is complex, which decreaseproductivity in CNC machines and do not provide therequired smooth dynamics in robotics.

The parameter that measures the discontinuities anddynamics smoothness is known as jerk and it is defined asthe acceleration change rate. Sudden control changes inacceleration result in higher jerk levels which can stimulatethe mechanical systems into resonance; therefore, it isnecessary to provide position profiles sufficiently smooth inorder to reduce jerk levels, minimizing discontinuities andavoiding system resonance.

Hardware and software digital signal processing (DSP)techniques have been proposed to develop algorithms forjerk reduction in industrial controllers. By minimizing jerk,two immediate benefits are granted: machinery stress andvibration reduction on the dynamics and smoother move-ment which allows speed increasing while error is reduced.In consequence, overall performance can be improvedwhen jerk is limited.

According to Erkorkmaz and Altintas [1], trapezoidalspeed profiles generate high-frequency harmonics in theacceleration dynamics which yields poor jerk behavior.They also showed that higher degree polynomial-basedprofiles give a smoother dynamics, making the resultingtrajectory easier to track by the limited bandwidth of theservo controller.

Higher degree polynomial profiles are difficult togenerate due to the computational load demands bothhardware resources and processing time, which seriouslycompromise on-line implementation for controllers inpractice. Direct polynomial evaluation algorithms demandhigh computational efforts to be made at the processorsystem, highly increased by the polynomial degree.

The present work shows a low cost, computationallyefficient, on-line hardware implementation of a high-degreepolynomial-based profile generator with limited jerkdynamics for CNC machines and robotics applications toimprove the machining process. The novelty of the paper isthe development of a multiplier-free recursive algorithm forcomputationally efficient polynomial evaluation in profilesgeneration, suited for any processing unit (DSP, micro-processor, etc.) and its special purpose digital structureoptimized for field programmable gate array (FPGA)which due to its reconfigurability allows a system on-a-chip(SOC) approach while its architecture freedom, from thedesigner point of view, improves the servo loop updatetime for conventional and high-speed machining with ahigh-resolution parallel processing structure. Considerablejerk reduction is obtained by the design of higher degreepolynomial profiles with overall dynamics smoothness (i.e.limited jerk polynomial profile that gives smooth accelera-tion, velocity and position profiles).

2. Background

Several research lines have been followed to improve thejerk limitation dynamics on CNC and robotics controllers.The reported algorithms deal with higher degree poly-nomial profile generators in software and hardware;however, all of them use the direct polynomial evaluationwhich is not computationally efficient because this techni-que requires floating-point operations to achieve the suitedprecision, besides being computational intensive, compro-mising on-line implementation. Personal computer (PC)-based software implementation of the direct polynomialevaluation, though achieves the required precision, lacksthe on-line restriction due to the computational intensivenature of the algorithm. On the other hand, for a generalpurpose microprocessor or DSP system, high resources arerequired for the hardware section in order to achieve on-line operation while keeping the error bounded, generallyrequiring expensive floating-point units or complicatedmultiple precision operations, making the algorithmunsuited for polynomials of 5 or higher degrees.Erkorkmaz and Altintas [1] developed an algorithm for

limited jerk trajectory generation which highlights thepolynomial approach advantages over the traditionaltrapezoidal profile approach. This work deals with a fifthdegree multiple polynomial generation and its implementa-tion into a TMS C32 commercially available DSP board,recognizing the computing resources demand for thesolution. Yih [2] presents the design of a third-order linearfilter for jerk limitation on CNC machines in order toimprove the servomotor position profile. This approachfilters the original trapezoidal profile for smoothing therelated discontinuities, but lacks the required precision dueto the filter side effects. The implementation is done into anADSP21060 commercially available DSP where the com-putation load is relaxed with the aid of three circularbuffers. Gasparetto and Zanotto [3] proposed a newmethodology for smooth trajectory planning in roboticsbased on a quadratic jerk profile for a fifth degreepolynomial spline. This work highlights the relevance onjerk limitation for robotics structure damage reductionalong with the avoidance of resonance frequencies thatincrease the error; no implementation is presented and onlysimulation results are shown. Another development forrobotics is presented by Yang et al. [4], where the positionprofile selection is stated as a priority in an adaptiveposition controller. The main goal for the trajectoryplanning is to smooth the displacement. The developedalgorithm is presented as software simulations underSimulink and Matlab. Heo et al. [5] presented a develop-ment on profile generation, related to the machiningprocess time for molds and dies. The errors in themachining process are reported as a function of theacceleration and deceleration profiles, implemented insoftware. Cheng et al. [6] proposed a code generatorfor a NURBS interpolator and states that a positionreference profile generator as a high-speed hardware unit is

ARTICLE IN PRESSR.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392 381

mandatory in order to achieve on-line CNC control. Thisdevelopment is implemented with a PC and a TMS320C32DSP working together. Earlier polynomial-based NURBStrajectory planning algorithms were developed by Zhangand Greenway [7], and were programmed under C++ in aPC where it became obvious that on-line implementationof robotics trajectories with this approach were compro-mised due to the computational load, increasing thesampling period. Marchenko et al. [8] worked on aNURBS interpolator for CNC machines with adaptivecontrol for a constant feed rate. The algorithm wasprogrammed under Visual C++ and executed in a400MHz Pentium II-based PC with a Delta Tau PMACcommercial controller board. In order to implementthe algorithm, the sampling period 4 103 s, which isfour times higher than the typical 1 103 s servo loopcontrol time. Cervera and Trevelyan [9] developed anoptimization evolving structure for NURBS trajectorygeneration where its implementation in a PC takes 14.62 s,in order to perform the interpolation. Macfarlane andCroft [10] presented an on-line method for obtainingsmooth, jerk-bounded trajectories; their method describedherein uses a concatenation of fifth-order polynomials toprovide a smooth trajectory between two-way points. Thealgorithm was implemented in Matlab. All the cited worksclaim for an improvement in computation load whichdisables on-line application or increases the resourcerequirements.

On the other hand, FPGA implementation of specialpurpose hardware signal processing units for CNCapplications has became the state-of-the-art approachwhere low-cost, SOC and high computation power isrequired. Jeon and Kim [11] developed an FPGA-basedtrapezoidal acceleration and deceleration profile generatorfor industrial robotics applications, but the digital structureis not optimal, and although polynomial profile generationis mentioned in the work, no actual implementation ispresented because the authors estimated that the hardwareresources will increase exponentially with the polynomialdegree, based on a direct polynomial evaluation algorithm.Jimeno et al. [12] worked on an FPGA-based tool pathcomputation applied to shoe last machining on CNClathes, this work presents the implementation of the surfacevirtual digitalization for the specific tool geometry and itsdisplacement. The research focuses on the computationefficiency as well as precision and speed achieved by theFPGA application. Chen and Lin [13] developed anFPGA-based ultrasonic servomotor drive, highlightingthe speed performance efficiency, compared with othertechnologies, along with the device flexibility and thecapacity for non-linear real time system implementation.Girau and Boumaza [14] presented an embedded architec-ture to solve the navigation problem in robotic based onFPGA that computes trajectories along a harmonicpotential. Their architecture proposed allows very largeprecisions and computer resolution. Software implementa-tion of the harmonic function computation on a micro-

processor-based computer, Pentium 4, 2GHz required100 106 s per iteration, while in FPGA structurerequired around 193 109 s. These works are a fewexamples of FPGA applications for industrial CNC androbotics and its low-cost, flexibility and speed advantages.Previously cited works use several methodologies for

polynomial reference profile generation, and in some casesthe hardware implementation is presented as well, wherethe main identified problems are: computational load,execution time constraints and hardware requirements; nolow-cost computationally efficient solution is presented.This paper deals with two main goals: the development of amultiplier-free computationally efficient polynomial eva-luation algorithm and its digital structure to be implemen-ted into an FPGA, having in mind that low-cost on-lineoperation is mandatory for profile generation. Jerklimitation can be achieved with higher degree polynomials,while precision on position control is guaranteed, as shownby experimentation.

3. Hardware polynomial-based profile generator

The proposed profile generator is based in a multiplier-free polynomial evaluation algorithm which reconstructsthe polynomial by a parameterization process usingsuccessive discrete time integration. The algorithm can beeasily implemented into an FPGA, requiring simplestructures as adders and registers plus logic. Hardwarerequirements increase linearly with the polynomial degreewhile computation time is not sacrificed. Multiple precisionaccumulation is performed by the digital structure in orderto guarantee the desired reference accuracy. In order to testthe profile generator performance and to show theversatility of the FPGA implementation, several comple-mentary digital structures were developed and integrated inthe design. Hardware description language VHDL (veryhigh-speed integrated circuit hardware description lan-guage) is used as the design platform for the FPGAimplementation.

3.1. Parameterization algorithm for polynomial evaluation

The parameterization of a polynomial is the mathema-tical process that extracts the minimum number of finitequantities that are necessary to reconstruct the givenpolynomial from an algorithm. Let WK(k) be an n-degreediscrete polynomial on k, where the most widely knownparameterization algorithms for these polynomials arecoefficient parameterization (1) and zeroes parameteriza-tion (2), where ai are the characteristic coefficients and ziare the polynomial zeroes, being C the group constant.Both parameterization algorithms are equivalent andrequire (n+1) parameters to characterize the polynomial:

W k a0 a1k a2k2 ankn, (1)

W k Ck z1k z2 . . . k zn. (2)

ARTICLE IN PRESSR.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392382

From the computational point of view, both algorithmsrequire multiplications and additions to evaluate for aspecific value of k. Other parameterization algorithms areavailable for the polynomial evaluation that are bettersuited from the computational point of view such asrecursive algorithms based on equally spaced evaluationpoints, which is the case for a discrete-time polynomial-based profile, where the operations to be computed areadditions only.

The recursive approach theory can be checked inRef. [15] and it is outlined next. Let WK(k) be an n-degreediscrete polynomial on k for K samples to be evaluated.The recursive discrete differences Dj on WK(k) are definedby Eq. (3) where it must be noted that the (n+1)th andhigher degree differences are zero.

DW K k W k W k 1;D2W K k DW k DW k 1; DnW K k Dn1W K k Dn1W K k 1;Dn1W K k 0:

(3)

From Eq. (3) it can be deduced the recursive algorithmfor the reconstruction of the original polynomial WK(k) asstated in Eq. (4).

DnW K k DnW K k 1;Dn1W K k DnW K k Dn1W K k 1; DW K k D2W K k DW K k 1;W K k DW K k W K k 1:

(4)

In order to recursively reconstruct WK(k) from Eq. (4) atthe start point k 0, the value of the n differences must beknown at k 1 as parameters which are the only dataneeded to parameterize the multiplier free algorithm, beingthese parameters the differences: DnWK(1), Dn1WK(1),y, D1WK(1) and WK(1).

From the cited literature [1,313], it can be seen that theapproach followed by these works is the direct computa-tion of the polynomial. Direct polynomial evaluation, in itsbest implementation, requires n+1 multiplications andadditions in at least 64-bit floating-point numericalprecision for each computed value; therefore, the computa-tional load is compromised for on-line applications. On theother hand, recursive computation is based on discretedifferentiations and integrations which imply a multiplierfree approach. Nevertheless, there is a drawback inrecursive algorithms, which is that the error is cumulative

+Z-1JW (k)

+++ Z-1

Fig. 1. Integrator chain block diagram

and increases as the number of samples taken for a givenprofile, K, increases, requiring higher resolution at theadditions in order to keep the error bounded. Theadvantage of the direct computation is that the error isabsolute at each sample, but requires multiplications infloating-point numerical representation; on the other hand,recursive computation is multiplier free and requires fixed-point addition operations only, which are simple and fastto execute, but in order to keep the error bounded, theresolution must be extended up to 96-bit or up fixed-pointnumerical representation. From the implementation pointof view in digital systems, it is better to perform very high-resolution fixed-point additions with error boundingtechniques, than to implement high-resolution floating-point multiplications and additions. The extra cost of veryhigh-resolution fixed-point additions of the recursivealgorithms is compensated in execution time performanceand resource savings, compared with the time penalty andresource requirements for a high-resolution floating-pointunit, as shown by Osornio et al. [16].

3.2. Polynomial-based profile generator digital structure

The digital structure for the polynomial evaluationalgorithm stated in Eq. (4) can be efficiently implementedas a consecutive discrete integrator chain as shown in Fig.1. For an n-degree polynomial, the parameterizationrequires n+1 integrators to reconstruct the desiredpolynomial profile reference. The advantage of the algo-rithm is that no multipliers are required and the integratorbank uses adders and registers optimally implemented indigital systems. Besides, as the algorithm is based inadditions, the on-line implementation requirements toachieve very fast servo loop update time is of no concern.The overall delay of the structure and the hardwareresources requirements for Fig. 1 follow a proportionalrelationship with the polynomial degree.The digital structure for the full implementation of the

block diagram in Fig. 1 can be seen in Fig. 2. The system isbased on five blocks: integrator bank, accumulatorregisters, reference read only memory (ROM), time baseand control sequencer. The integrator bank is the core ofthe polynomial profile generator and it is based on a 96-bitmultiple precision accumulator adder. Although thereference position has a 32-bit resolution which is typicalfor optical encoder-based servo loops, 64 additional bitswere used in order to keep the cumulative error bounded.The accumulator registers were used to store the jthdifferences for each integrator; n+1 96-bit registers were

. . .Wk

++ Z-1

N+1

for polynomial profile generation.

ARTICLE IN PRESSR.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392 383

required. The reference ROM block contains the n+1initial difference values DjW(0), required to start thepolynomial profile reconstruction by the accumulatorchain. The time base block generates the samplingfrequency for the system which conveys the servo loopupdate time; this signal is fully user programmable. Finally,the sequencer block is the finite state machine (FSM) thatcontrols and supervises the overall polynomial profilegeneration process. The process is started by the samplingpulse generated at the time base which makes the sequencercalculate the corresponding reference.

The detail structure of the integrator bank blockdiagram is shown in Fig. 3 and has seven operating

ROM REFERENCE

TIME BASE

SEQUENCER

REGISTERACCUMULATOR

INTEGRATORBANK

OUTPUTWK(k)

Fig. 2. Polynomial profile generator block diagram.

+++

CARRYREGISTER

MUX

MUX OPERANDSELECTOR

ADD

SE L

CoJ w

Fig. 3. Integrator bank digita

modules: 32-bit adder, two multiplexors, carry register,accumulator registers, 96-bit registers and FSM. Becausethe reconstruction algorithm is multiplier-free, the compu-tational load is not critical and the integrator chain can beperformed sequentially by using a single 32-bit adder that,with the aid of a carry register can perform the multipleprecision accumulation up to 96 bits. Partial results arestored in the accumulator registers and when the fullcomputation is ready, it is transferred to the 96-bit registerwhich contains the corresponding difference. The multi-plexors are required to provide the adder with the operand32-bit section to be processed by the adder. An FSM isused for overall control.

3.3. Complementary digital structures for testing

The profile generator was fully tested as a self-containedunit giving the expected results, however, in order to testthe system under operating conditions, it is necessary tocomplete a full digital control loop with additionalstructures such as: interface, adder, PID, digital to analogconverter (DAC) driver, position counter and timer; plusan external servoamplifier and servomotor with encoder.Fig. 4 shows the block diagram of the complete servo loopsystem. Notice that the complementary digital structureswere implemented into the same FPGA as the profilegenerator, which shows the obvious advantages of FPGAover other technologies to provide a SOC solution. Themost important complementary structure is the digital PIDcontroller which is based on a 36-bit multiplieraccumu-lator core. The adder block is the error computation unitwhich subtracts the actual encoder position from thereference provided by the polynomial profile generator.

FSM

ER

ACUMULATORREGISTERS 96 BIT

REGISTERS

J-1

LDC

w

l structure block diagram.

ARTICLE IN PRESS

AmplifierPID DACDRIVERADDER

POSITIONCOUNTER

TIMER

PROFILEGENERATOR

ENCODER

COMPLEMENTARYSTRUCTURES

FPGA DESIGN

SERIALINTERFACE

PC

SERVOMOTOR

Fig. 4. Complete servo loop system block diagram.

0 1 2 3 4 50

1

2

3

4

Pos

ition

(Cou

nts)

0 1 2 3 4 50

2

4

6

8

10

Spe

ed (C

ount

s/s)

20

15

10

5

0

5

10

15

20

Acc

eler

atio

n (C

ount

s/s2

)

20

15

10

5

0

5

10

15

20

Jerk

(Cou

nts/

s3)

x104

x103 x106

x104

Time (s) Time (s)0.50.5

3.5

2.5

1.5

0.5

5.5 5.5

0 1 2 3 4 5 0 1 2 3 4 5Time (s) Time (s)

0.50.5 5.5 5.5

Fig. 5. Trapezoidal speed profile dynamics for 4 104 count end reference in 5 s: (a) position, (b) speed, (c) acceleration, and (d) jerk.

R.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392384

The DAC driver is an FSM that controls the interfacebetween the digital system and the DAC circuit whichprovides the analog signal to the servoamplifier andservomotor. The timer provides the servo loop update

time signal for the controller. The position counter is a32-bit relative to absolute counter that contains theactual position of the servomotor. A serial interface to aPC is also included in the systems in order to provide

ARTICLE IN PRESSR.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392 385

communication for configuration parameters for the unitsand to monitor the actual position, which is compared withthe profile generator reference.

4. Polynomial profile cases of study

Six cases of study were developed in order to verify theprofile generator efficiency under real system conditions.The first five experiments are polynomial profiles that wereimplemented in the FPGA with no load applied to theservomotor. The polynomial profiles for these experimentsare: trapezoidal speed, biquadratic acceleration, cubicacceleration, quintic acceleration and piecewise cubicsequence in jerk; all of them for three different end referenceof: 1 104 counts in 1 s, 2 104 counts in 2 s and 4 104counts in 5 s. For the case of study number 6, four profileswere implemented: trapezoidal speed, biquadratic accelera-tion, cubic acceleration and quintic acceleration for an endreference of 4 104 counts in 5 s with the servomotor actingover a high-speed CNC milling machine feed axis.

4.1. Case of study 1: trapezoidal speed profile

The first case of study is a standard trapezoidal speedprofile, used in most commercially available controllers,

0 1 2 3 4 50

0.5

1

1.5

2

25

3

3.5

4

Pos

ition

(Cou

nts)

10

5

0

5

10

Acc

eler

atio

n (C

ount

s/s2

)

x104

x103

Time (s)0.5 5.5

0 1 2 3 4 5Time (s)

0.5 5.5

Fig. 6. Biquadratic acceleration profile dynamics for 4 104 count end r

used as reference for jerk reduction on higher degreepolynomials. Fig. 5 shows the trapezoidal speed profiledynamics for an end position reference of 4 104 counts in1 s. Fig. 5a shows the position profile, Fig. 5b is thetrapezoidal speed profile, Fig. 5c contains the accelerationprofile and the jerk profile can be seen in Fig. 5d. It isevident from Fig. 5 that, although the position trajectorylooks smooth, the acceleration profile has discontinuities,which are clearly reflected in the jerk profile as high-energyimpulses, which stress the mechanical system.

4.2. Case of study 2: biquadratic acceleration profile

A biquadratic polynomial for acceleration profile is usedas case of study 2. In this case, the biquadratic profile isformed with two quadratic polynomials for accelerationand deceleration in a piecewise sense. The dynamics of thisprofile candidate are shown in Fig. 6. As it can be seen inFig. 6, for this higher degree polynomial profile thedynamics are smoother than the trapezoidal speed profileof Fig. 5. No discontinuities are present in the acceleration,Fig. 6c, while speed and position follow a smoothtrajectory. Peak jerk dynamics is reduced in several ordersof magnitude, compared with the trapezoidal speed profileand no high-energy impulses are present.

0

1

1.5

2

Spe

ed (C

ount

s/s)

2

1

0

1

2

Jerk

(Cou

nts/

s3)

x104

x104

0.5

0.5

1.5

0.5

0.5

1.5

0 1 2 3 4 5Time (s)

0.5 5.5

0 1 2 3 4 5Time (s)

0.5 5.5

eference in 5 s: (a) position, (b) speed, (c) acceleration, and (d) jerk.

ARTICLE IN PRESS

0.5 0 1 2 3 4 50

0.5

1

1.5

2

2.5

3

3.5

4P

ositi

on (C

ount

s)

0.5

0

1

1.5

Spe

ed (C

ount

s/s)

10

5

0

5

10

Acc

eler

atio

n (C

ount

s/s2

)

1

0.5

0

0.5

1

1.5

2

Jerk

(Cou

nts/

s3)

x104 x104

x103 x104

0.5

Time (s)5.5 0.5 0 1 2 3 4 5

Time (s)5.5

0.5 0 1 2 3 4 5Time (s)

5.5 0.5 0 1 2 3 4 5Time (s)

5.5

Fig. 7. Cubic acceleration profile dynamics for 4 104 count end reference in 5 s: (a) position, (b) speed, (c) acceleration, and (d) jerk.

R.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392386

4.3. Case of study 3: cubic acceleration profile

Case of study 3 is a single-trace cubic polynomial foracceleration. The dynamics of this candidate profile areshown in Fig. 7. From the trajectories of the cubic profiledynamics, it can be seen that position, speed andacceleration are smooth without discontinuities and thejerk is also limited with no high-energy impulses.

4.4. Case of study 4: quintic acceleration profile

A higher degree polynomial candidate is proposed forcase of study 4 which is a quintic polynomial inacceleration. Fig. 8 shows the dynamics of the proposedprofile where, for this case, the four profiles: position,speed, acceleration and jerk are smooth and continuous.Peak jerk, Fig. 8d, is slightly increased from the biqua-dratic and cubic cases, but it is lower than the jerk in thetrapezoidal speed profile.

4.5. Case of study 5: piecewise cubic jerk profile

A piecewise cubic jerk profile is developed for case ofstudy 5. This polynomial candidate is presented to showthe possibilities of fully arbitrary piecewise profiles with

different acceleration and deceleration. The dynamics ofthis profile are shown in Fig. 9. Despite the smoothness ofthe trajectories for all the parameters, piecewise polyno-mials can provide control over acceleration and decelera-tion paths without discontinuities.

4.6. Case of study 6: profiles implemented in a high-speed

CNC milling machine axis

Case of study number 6 is the polynomial profilesdeveloped in cases of studies 15 for an end reference of4 104 counts in 5 s which are implemented for a high-speed CNC milling machine axis control. The followingsection shows the results.

5. Experimental setup for cases of study

Two experimental setups were prepared in order to testthe polynomial profile generator: the first one withthe servomotor at no load for the cases of studies 15;the second one for the servomotor driving a CNCmilling machine axis for the case of study 6. Bothexperimental setups were implemented into a Spartan-3FPGA from Xilinx which has 200,000 logic gates, 216Kbrandom access memory (RAM), 12 hardwired 18-bit

ARTICLE IN PRESS

0.5 0 1 2 3 4 50

1

2

3

4

Pos

ition

(Cou

nts)

0

10

20

Spe

ed (C

ount

s/s)

1

0

0.5

1

1.5

20

15

10

5

0

5

10

15

Jerk

(Cou

nts/

s3)

Acc

eler

atio

n (C

ount

s/s2

)

x106

x104

Time (s)

x106

3.5

2.5

1.5

0.5

0.5

1.5

x104

5.5 0.5 0 1 2 3 4 5Time (s)

5.5

0.5 0 1 2 3 4 5Time (s)

5.5 0.5 0 1 2 3 4 5Time (s)

5.5

Fig. 8. Quintic acceleration profile dynamics for 4 104 count end reference in 5 s: (a) position, (b) speed, (c) acceleration, and (d) jerk.

R.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392 387

multipliers and up to 173 user defined input/outputpins [17]. The master clock works at 50MHz andthe servo loop update time (sampling period for thecontrol loop) was set to 1 103 s. The complementarydigital structures to the profile generator were alsoimplemented into the same FPGA. The servomotor is aMAXON DC 2266.85-73216-2000 with a Copley controlMOD 403 servoamplifier which is driven by a TLV5636DAC from Burr-Brown [18]. Fig. 10 shows the experi-mental setup for the first test: Spartan-3 developmentboard for FPGA, DAC, servoamplifier and servomotorwith encoder.

6. Results

The tests validated the polynomial profile generatorperformance and the dynamics obtained are shownin Tables 13. The parameters shown in these tablesare: end position, top speed, maximum acceleration,maximum deceleration and peak jerk for five polynomialprofiles. On the other hand, FPGA resources are reportedin Table 4.

Five cases of study are presented such as: singlepolynomial (cases 3 and 4), dual piecewise polynomial(case 2) and triple piecewise polynomial (case 5), where itcan be seen from Tables 13 that higher degree polynomial

profiles, compared to the standard trapezoidal speed profile(case of study 1 used as reference), improve the systemdynamics by reducing peak jerk in more than one order ofmagnitude. The sixth case of study tests the algorithmfunctionality in operating conditions with the servomotordriving a high-speed CNC milling machine axis as shownin Fig. 11. Precision under operating conditions for theunloaded servomotor is high as shown in Fig. 12 withan end point reference error of 4 counts from an endpoint reference of 4 104 counts for all cases, as measuredfrom the servomotor encoder. For the case of study 6with the servomotor driving the CNC milling machineaxis, the end point reference error is around 30 counts forcases 1, 4 and 5; and around 40 counts for cases 2 and 3,from an end point reference of 4 104 counts, as shown inFig. 13.

7. Conclusions

This work presents the development of a higher degreepolynomial profile generator for CNC and roboticsapplications, aimed to improve the machinery dynamicsby jerk reduction. Jerk limitation helps reducing theservomotor and machinery stress by a smooth anddiscontinuities free overall dynamics parameters: position,velocity, acceleration and jerk. An FPGA implementation

ARTICLE IN PRESS

0 1 2 3 4 50

1

2

3

4P

ositi

on (C

ount

s)

0 1 2 3 4 50.2

0

0.2

0.4

0.6

0.8

1

Spe

ed (C

ount

s/s)

4

3

2

1

0

1

2

Acc

eler

atio

n (C

ount

s/s2

)

30

20

10

0

10

20

30

40

Jerk

(Cou

nts/

s3)

x104 x104

x104 x104

0.5Time (s)

5.5 0.5Time (s)

5.5

0 1 2 3 4 5 0 1 2 3 4 50.5Time (s)

5.5 0.5Time (s)

5.5

Fig. 9. Piecewise cubic jerk profile dynamics for 4 104 count end reference in 5 s: (a) position, (b) speed, (c) acceleration, and (d) jerk.

Fig. 10. Experimental setup for cases of studies 15 showing: profile

generator, DAC, servoamplifier and servomotor.

R.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392388

of the profile generator is developed, along with comple-mentary digital structures, to show the benefits of using thistechnology for embedded systems.

Previous works reported higher degree polynomialprofile computations, nevertheless, those techniques arecomputationally intensive, which made them unsuitable foron-line implementation due to their hardware requirements

are high increasing the costs. The contribution of thepresent work is to present a costless implementation of ahigh efficient algorithm for polynomial evaluation which iseasily implemented into FPGA while preserving therequired precision of the overall process.From the digital implementation point of view, FPGA

technology has proven to be ideally suited for the task.SOC approach is natural in FPGA where the polynomialprofile generator, along with complementary structuressuch as: PID, DAC driver, timer, error adder, feedbackencoder counter and PC interface, are implementedtogether in parallel. The parallel architecture ensures thaton-line application could be achieved for the embeddedsystem developed. For the cases of study, a 1 103 sservo loop update time is used, which gives no problem tothe computation load of the FPGA and a 10 106 sservo loop update time can be produced with minimalchanges to the same structure (i.e. modify the timersettings via PC interface). Finally, the Spartan-3 familyFPGA used in the development is a very low costunit (under US $10.00) which is a self-contained SOCsystem for not only de profile generator but PID, DACdriver, etc.It is to be noted that the polynomial profiles of the cases

of study presented in this work were proposed to show the

ARTICLE IN PRESS

Table 1

Peak parameter dynamics for cases of studies 15 at an end position of 1 104 counts in 1 s

Profiles results (1 104 counts position hardware generation in 1 s)

Parameter Trapezoidal speed Biquadratic acceleration Cubic acceleration Quintic acceleration Piecewise cubic jerk

End position (counts) 1 104 1 104 1 104 1 104 1 104Top speed (count/s) 1.11 104 2 104 1.875 104 4.3 104 11.75 104Maximum acceleration (count/s2) 11.2 104 6 104 5.77 104 28.7 104 11 104Maximum deceleration (count/s2) 11.2 104 6 104 5.77 104 28.7 104 11 104Peak jerk (count/s3) 112 106 47.9 104 59.82 104 330 104 170 104

720 104

Table 2

Peak parameter dynamics for cases of studies 15 at an end position of 2 104 counts in 2 s

Profiles results (2 104 counts position hardware generation in 2 s)

Parameter Trapezoidal speed Biquadratic acceleration Cubic acceleration Quintic acceleration Piecewise cubic jerk

End position (counts) 2 104 2 104 2 104 2 104 2 104Top speed (count/s) 1.11 104 2 104 1.875 104 8.542 104 1.175 104Maximum acceleration (count/s2) 5.6 104 3 104 2.88 104 57.3 104 5.5 104Maximum deceleration (count/s2) 5.6 104 3 104 2.88 104 57.3 104 11 104Peak jerk (count/s3) 56 106 12 104 15 104 62.5 104 42.4 104

159.8 104

Table 3

Peak parameter dynamics for cases of studies 15 at an end position of 4 104 counts in 5 s

Profiles results (4 104 counts position hardware generation in 5 s)

Parameter Trapezoidal speed Biquadratic acceleration Cubic acceleration Quintic acceleration Piecewise cubic jerk

End position (counts) 4 104 4 104 4 104 4 104 4 104Top speed (count/s) 8.8 104 1.5 104 1.5 104 17.09 104 9.411 103Maximum acceleration (count/s2) 1.78 104 9.6 103 9.24 103 1.8 104 1.76 104Maximum deceleration (count/s2) 1.78 104 9.6 103 9.24 103 1.8 104 3.5 104Peak jerk (count/s3) 18 106 1.535 104 1.92 104 12.5 106 5.43 104

21.73 104

Table 4

FPGA implementation resources

Time and device results (polynomial profiles SPARTAN 3 FPGA 3s200ft256-4)

Parameter Trapezoidal Biquadratic Cubic Quintic Piecewise cubic

Polynomial profile generator only

Number of slices 797 of 1920 795 of 1920 1073 of 1920 1118 of 1920 1508 of 1920

Number of slice flip flops 556 of 3840 721 of 3840 844 of 3840 1069 of 3840 1126 of 3840

Number of four input LUTs 1438 of 3840 1363 of 3840 1846 of 3840 2002 of 3840 2529 of 3840

Overall average FPGA usage (%) 29 30 39 43 53

Polynomial profile generator and complementary structures

Number of slices 1241 of 1920 1271 of 1920 1340 of 1920 1632 of 1920 1803 of 1920

Number of slice flip flops 1186 of 3840 1356 of 3840 1441 of 3840 1634 of 3840 1540 of 3840

Number of four input LUTs 2184 of 3840 2227 of 3840 2384 of 3840 2533 of 3840 2730 of 3840

Overall average FPGA usage (%) 48 50 54 60 63.2

R.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392 389

ARTICLE IN PRESS

Fig. 11. Experimental setup for case of study 6 showing: PC for parameter

configuration, profile generator, DAC, servoamplifier, servomotor and

high speed CNC milling machine.

0 1 2 3 4 50

5

10

15

20

25

Time (s)

Pos

ition

Err

or (c

ount

s)

0 1 2 3 4 52

0

2

4

6

8

Time (s)

Pos

ition

Err

or (c

ount

s)

0 1 20

2

4

6

8

T

Pos

ition

Err

or (c

ount

s)

Fig. 12. Error between generated vs. measured reference position for cases of st

trapezoidal speed profile, (b) biquadratic acceleration profile, (c) cubic acceler

profile.

R.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392390

dynamics improvement by jerk reduction and real worldfunctionality in a closed servo loop system. From theresults it can be seen the prototype efficiency as anembedded system with end position error below 0.01%for the unloaded servomotor and below 0.1% for theservomotor driving the CNC milling machine axis. Thepresented polynomials cannot be considered optimal fromthe overall dynamics point of view, no matter how theyincrease smoothness and reduce jerk; therefore, furtherresearch on optimal higher degree polynomial profiles forjerk limitation is proposed.

Acknowledgements

This research was partially supported by CONACyTscholarship No. 177102.

0 1 2 3 4 501234567

Time (s)

Pos

ition

Err

or (c

ount

s)

0 1 2 3 4 50

2

4

6

8

Time (s)

Pos

ition

Err

or (c

ount

s)

3 4 5ime (s)

udies 15 at 4 104 counts end position in 5 s, servomotor with no load: (a)ation profile, (d) quintic acceleration profile, and (e) piecewise cubic jerk

ARTICLE IN PRESS

0 1 2 3 4 50

10203040506070

Time (s)

Pos

ition

Err

or (c

ount

s)

0 1 2 3 4 520

0

20

40

60

80

Time (s)

0 1 2 3 4 5Time (s)

0 1 2 3 4 5Time (s)

0 1 2 3 4 5Time (s)

Pos

ition

Err

or (c

ount

s)0

10203040506070

Pos

ition

Err

or (c

ount

s)

0

10

20

30

40

50

Pos

ition

Err

or (c

ount

s)

0102030405060

Pos

ition

Err

or (c

ount

s)

Fig. 13. Error between generated vs. measured reference position comparison for case of study 6 at 4 104 counts end position in 5 s, servomotor driving ahigh speed CNC milling machine axis: (a) trapezoidal speed profile, (b) biquadratic acceleration profile, (c) cubic acceleration profile, (d) quintic

acceleration profile, and (e) piecewise cubic jerk profile.

R.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392 391

Appendix A. Supplementary materials

Supplementary data associated with this article canbe found in the on-line version at doi:10.1016/j.rcim.2008.01.002.

References

[1] Erkorkmaz K, Altintas Y. High speed CNC system design. Part I:

jerk limited trajectory generation and quintic spline interpolation. IJ

Machine Tools Manufact 2001;41:132345.

[2] Yih FC. Design and implementation of a linear jerk filter for a

computerized numerical controller. J Control Eng Practice 2005;13:

56776.

[3] Gasparetto A, Zanotto V. A new method for smooth trajectory

planning of robot manipulators. Mech Machine Theory 2007;42:

45571.

[4] Yang Z, Xi F, Wu B. A shape adaptive motion control system with

application to robotic polishing. Robot Comput-Integr Manufact

2005;21:35567.

[5] Heo EY, Kim DW, Kim BH, Chen FF. Estimation of NC machining

time using NC block distribution for sculptured surface machining.

Robot Comput-Integr Manufact 2006;22:43746.

[6] Cheng MY, Tsai MC, Kuo JC. Real-time NURBS command

generators for CNC servo controllers. IJ Machine Tools Manufact

2002;42:80113.

[7] Zhang QG, Greenway RB. Development and implementation of

NURBS curve motion interpolation. Robot Comput-Integr Manu-

fact 1998;4:2736.

[8] Marchenko T, Ko TJ, Lee SH, Kim HS. NURBS interpolator for

constant material removal rate in open NC machine tools. IJ

Machine Tools Manufact 2004;44:23745.

[9] Cervera E, Trevelyan J. Evolutionary structural optimisation based

on boundary representation of NURBS. Part I: 2D algorithms.

Comput Struct 2005;83:190216.

[10] Macfarlane S, Croft E. Jerk-bounded manipulator trajectory plan-

ning: design for a real time applications. IEEE Trans Robot Autom

2003;19:4252.

dx.doi.org/10.1016/j.rcim.2008.01.002dx.doi.org/10.1016/j.rcim.2008.01.002

ARTICLE IN PRESSR.A. Osornio-Rios et al. / Robotics and Computer-Integrated Manufacturing 25 (2009) 379392392

[11] Jeon WJ, Kim YK. FPGA based acceleration and deceleration circuit

for industrial robots and CNC machine tools. Mechatronics

2002;12:63542.

[12] Jimeno A, Sanchez JL, Mora H, Mora J, Garca JM. FPGA-based

tool path computation: an application for shoe last machining on

CNC lathes. Comput Ind 2006;57:10311.

[13] Chen JS, Lin IN. Toward the implementation of an ultrasonic motor

servo drive using FPGA. Mechatronics 2002;12:51124.

[14] Girau B, Boumaza A. Embedded harmonic control for dynamic

trajectory planning on FPGA. In: Proceedings of the IASTED

conference on artificial intelligence and applications. Innsbruck,

Austria; 2007. p. 124.

[15] Hamming RY. Numerical methods for scientists and engineers. 2nd

ed. New York: Dover Publications; 1986.

[16] Osornio-Rios RA, Romero-Troncoso RJ, Herrera-Ruiz G, Catane-

da-Miranda R. Computationally efficient parametric analysis of

discrete-time polynomial based accelerationdeceleration profile

generation for industrial robotics and CNC machinery. Mechatronics

2007;17:51123.

[17] Xilinx Corporation. Spartan-3 family FPGAs data sheet. Xilinx

Corporation; 2005.

[18] Burr-Brown Corporation. TLV5636 data sheet. Burr-Brown Cor-

poration, A division of Texas Instruments Inc.; 1998.

FPGA implementation of higher degree polynomial acceleration profiles for peak jerk reduction in servomotorsIntroductionBackgroundHardware polynomial-based profile generatorParameterization algorithm for polynomial evaluationPolynomial-based profile generator digital structureComplementary digital structures for testing

Polynomial profile cases of studyCase of study 1: trapezoidal speed profileCase of study 2: biquadratic acceleration profileCase of study 3: cubic acceleration profileCase of study 4: quintic acceleration profileCase of study 5: piecewise cubic jerk profileCase of study 6: profiles implemented in a high-speed CNC milling machine axis

Experimental setup for cases of studyResultsConclusionsAcknowledgementsSupplementary materialsReferences

Recommended