Path planning with PH G2 splines in R2

Transcription

1 Path panning with PH G2 spines in R2 Laurent Gajny, Richard Béarée, Eric Nyiri, Oivier Gibaru To cite this version: Laurent Gajny, Richard Béarée, Eric Nyiri, Oivier Gibaru. Path panning with PH G2 spines in R2. IEEE 1st Internationa Conference on Systems and Computer Science, Aug 2012, Lie, France <ha > HAL Id: ha Submitted on 17 Jan 2013 HAL is a muti-discipinary open access archive for the deposit and dissemination of scientific research documents, whether they are pubished or not. The documents may come from teaching and research institutions in France or abroad, or from pubic or private research centers. L archive ouverte puridiscipinaire HAL, est destinée au dépôt et à a diffusion de documents scientifiques de niveau recherche, pubiés ou non, émanant des étabissements d enseignement et de recherche français ou étrangers, des aboratoires pubics ou privés.

2 Path panning with PH G 2 spines in R 2 Laurent Gajny, Richard Béarée, Éric Nyiri, Oivier Gibaru Arts et Métiers ParisTech, LSIS - UMR CNRS 7296, 8 Bouevard Louis XIV, Lie Cedex, France INRIA Lie-Nord-Europe, 40 avenue Haey Vieneuve d Ascq, France Abstract In this artice, we justify the use of parametric panar Pythagorean Hodograph spine curves in path panning. The eegant properties of such spines enabe us to design an efficient interpoator agorithm, more precise than the cassica Tayor interpoators and faster than an interpoator based on arc ength computations. Index Terms Path panning, Pythagorean-hodograph, Spines. I. INTRODUCTION Designing a good geometric support from given points is a fundamenta issue in path panning for industria machines Machine-too, manipuator, robot... Traditionnay, the methods used consist in minimizing the L p norm of the second derivative of poynomia sines which interpoate the given points, for p N See for exampe [1] for the genera method and [9] for its appication. In this artice, we are interested in a subcass of poynomia spines caed Pythagorean Hodograph spines. Pythagorean Hodograph curves PH curves, introduced in 1990 in [2] are characterized by the specia property that the derivative of their arc ength is a poynomia or rationa function of the curve parameter. This property enabes to compute exacty usefu quantities such as curvature, bending energy or arc ength. For simpe poynomia curve, we can not, in genera, cacuate these quantities and so, we appy some quadrature rues. By a good choice of the quadrature rue, we can reach a satisfying precision but it may be highy time consuming. It is not difficut to design a geometric support which interpoates data points using PH curves. We can appy for exampe L 1 C 1 method from [1] which gives some derivative vaues at each data point and reconstruct a soution as PH quintic spine by C 1 Hermite interpoation See [3]. But C 1 continuity is not sufficient for path panning of mechanica system if trajectory foowing performances are of interest. Indeed, curvature discontinuities, which are not physicay foowed, excite the structure and can significanty deteriorate the accuracy. Thus, we use the G 2 interpoation scheme introduced by Jakič et a. in [7]. By this method, we can ony treat, for the moment, convex data but it uses ow degree spine cubic. The existence of soution is guaranteed and the iterative method to construct the soution converges rapidy. Some artices See for exampe [4], [5], [11] dea with the use of PH curves in CNC. In these artices, authors ony treat the case of a singe PH curve and designed an interpoator based on arc ength computation in order to foow the parametric geometric support. We wi compare the performances in running time and accuracy of such an interpoator for both PH and cassica poynomia curve cases. Then, We wi extend this procedure on a PH spine. This paper is organized as foows. Section II gives some basic information about Pythagorean Hodograph cubic curves. In section III, we present briefy the interpoation scheme that we wi use in our experiment. Section IV is dedicated to the PH interpoator, its appication and a comparison with other interpoators. Some concusions wi be drawn in the ast section. II. PYTHAGOREAN HODOGRAPH CUBIC CURVES A poynomia Pythagorean hodograph curve rτ = xτ, yτ, τ [0, 1] is defined by the property that its hodograph r τ = x τ, y τ satisfies the Pythagorean condition : x 2 τ + y 2 τ = σ 2 τ, 1 for some poynomia στ. This remarkabe property enabes to compute exacty some usefu quantities ike arc ength, curvature or bending energy. To define curves satisfying 1, we use two poynomias, uτ and vτ such that : x τ = u 2 τ v 2 τ, y τ = 2uτvτ, στ = u 2 τ + v 2 τ. For PH cubics, we sha choose uτ and vτ inear functions expressed in the Bernstein basis : uτ = u 0 B 1 0τ + u 1 B 1 1τ, vτ = v 0 B 1 0τ + v 1 B 1 1τ. In order to define reguar curves, we may assume u 1 u v 1 v and u 0 v 1 u 1 v 0 0. By integrating the hodograph, we obtain the formuation of Bézier contro points of a PH cubic curve. Q 1 = Q u2 0 v 2 0, 2u 0 v 0, Q 2 = Q u 0u 1 v 0 v 1, u 0 v 1 + u 1 v 0, Q 3 = Q u2 1 v 2 1, 2u 1 v

3 We have a sufficient and necessary geometric condition on the contro poygon, iustrated in Fig. 1, for a parametric cubic curve to be a PH cubic. The foowing theorem wi be very usefu in the spine interpoation scheme we wi use in section III. Theorem 1: Let rτ be a parametric panar cubic curve with Bézier contro points Q 0, Q 1, Q 2, Q 3. Let L i = Q i Q i+1, i {0, 1, 2} be the engths of the contro poygon edges and θ 1, θ 2 be the contro poygon anges at the interior vertices Q 1 and Q 2. Then rτ is a PH cubic if and ony if : L 1 = L 0 L 2, and, θ 1 = θ 2. 4 The proof of this theorem can be found in [6] section d 0, P 0, P 1, d 1 a convex G 1 Hermite data. P 0 and P 1 are two points in the pane, d 0 and d 1 the associated tangent vectors. We are ooking for λ 0, λ 1 > 0 such that : Q 0 = P 0, Q 1 = P 0 + λ 0 d 0, Q 2 = P 1 λ 1 d 1, Q 3 = P 1, are the contro points of a PH cubic. The authors determine a unique admissibe soution : λ 0 = Λ 0 d 0, P 0, d 1, λ 1 = Λ 1 d 0, P 0, d 1, under the assumption that the anges ϕ 0 = d 0, P 0 and ϕ 1 = d 1, P 0 satisfy the condition : 5 Fig. 1. A PH cubic curve and its contro poygon. ϕ 0 + ϕ 1 < 4 3 π. The goba agorithm of G 2 PH cubic spine determination can be written as foows : Inputs : Data points P 0, P 1,..., P n, end conditions d 0, d n, a toerance ε. Outputs : Tangent vectors at each data points and engths of the first and the third edges of each contro poygon. III. PH G 2 SPLINE INTERPOLATION The probem is to interpoate P 0, P 1,..., P n in R 2 by a G 2 PH cubic spine with end conditions d 0 and d n, respectivey the tangent vectors at P 0 and P n. We wi use L 1 C 1 interpoation, which have good shape-preserving properties, to compute d 0 and d n. We give the main resut of the artice of Jakič et a. [7]. Theorem 2: Let P 0, P 1,..., P n a set of convex data and d 0, d n the tangent vectors at P 0 and P n. The interpoation probem of finding a G 2 PH cubic spine passing through the P i with respect of d 0 and d n has an admissibe without oops or cusps soution if and ony if the anges ϕ 0 = d 1, P 1, ϕ = P 1, P, = 1, 2,..., n 1, ϕ n = P n 1, d n, satisfy ϕ i +ϕ i 1 < M with M = 4π/3 for i = 1, 2,..., n 1. If M = Kπ with, K = π arccos < 4/3, 3 then the soution is unique. The system of non-inear equations, directy obtained by expicitation of 4 on each segment is soved by an iterative method. To understand the agorithm, we may consider the G 1 Hermite interpoation probem with PH cubic. Let 1 λ = 1, 1,..., 1 R 2m, r = 0 2 Compute d [0] = P +1 P 1 P +1 P 1, = 1, 2,..., m 1. 3 Compute λ 2 = Λ 0 d [r], P, d [r] λ 2+1 = Λ 1 d [r] = 0, 1,..., m 1. 4 Compute d [r+1] = 1 λ 2 2, P, d [r] +1, +1, P λ 2+1 d [r] λ 2 1 P 1 λ 2 2 d [r] 1, = 1, 2,..., m 1. 5 D = d [r+1], = 0, 1,..., m. 6 If λ od λ new < ε and D od D new < ε, STOP ese r = r + 1 and go back to 3. We have appied this agorithm to interpoate the ogarithmic spira and a circe. We note in Fig. 2 that the resuting curves preserve we the shape of the data. A. Principe IV. PH INTERPOLATOR In this part, we wish that a mechanica system foows a geometric motion support with respect of a known feedrate. The geometric support is designed by interpoation of some checkpoints. We choose here a set of convex data that we interpoate by the previous procedure. Knowing the feedrate enabes us, by integration, to define a position aw See

4 a b Fig. 2. Circe. Interpoation by G 2 PH cubic spine. a Logarithmic spira b Fig. 3. A simpe feedrate top and its associated position aw bottom. Fig. 3 which means that we know, at time t 0, the distance the system must have run through. The main probem is to determine the geometric parameter which corresponds to the time parameter. In practice, we discretize the time interva [0, T ] with a constant 1 samping period t. We obtain the ist 0 = t 0 < t 1 < < t m = T. For each i {0, 1,..., m}, we have to find the associated geometric parameter of the curve τ i by soving the foowing equation : t i = sτ i, where and s are respectivey the arc ength functions of the position aw and the spine. B. Arc ength computation for a PH curve Let rτ be a PH cubic defined by the coefficients u 0, u 1, v 0, v 1 as in 2. Arc ength at parameter τ [0, 1] may be expressed in the form : where and : sτ = 3 n s k 1 τ 3 k τ k, 6 k k=0 k 1 s 0 = 0, s k = σ j, k = 1,..., 3 j=0 σ 0 = u v 2 0, σ 1 = u 0 u 1 + v 0 v 1, σ 2 = u v We can choose a variabe stepsize. It wi not change the performances of the method. We have in particuar : s1 = σ 0 + σ 1 + σ So contrary to ordinary poynomia curve, we have a cosed form for the arc ength at parameter τ. C. Agorithm We now describe the agorithm we wi use further both in case of a simpe poynomia spine and a PH spine. Inputs : Data points and associated parameters, the interpoating spine defined by some coefficients, the position aw, a time ist. Output : A ist of geometric parameters. 1 Compute k = t k, k = 0, 1,... m. 2 Compute S j, j = 0, 1,..., n the arc engths of the PH cubic spine at each ν j. 3 τ 0 = ν 1 ; r 0 = 1. 4 Compute r k, k = 1, 2,..., n the number of the spine piece where τ k must be. 5 If r k = r k 1, sove : τk ν rk k k 1 = s rk τ k 1 τ k ν rk +1. ν rk +1 ν rk s rk τk 1 ν rk ν rk +1 ν rk,

5 6 If r k r k 1, sove : τk ν rk k S rk = s rk, ν rk +1 ν rk ν rk τ k ν rk +1. Here, s j. is the arc ength function of the j th piece of the spine. The equations to sove in case of PH cubic spine are aso cubics. So, we can sove them exacty by Cardano method. In practice, for a given precision ε = 10 13, we use the Newton-Raphson agorithm which eads to the soution with machine precision in two or three iterations. For poynomia spine, using this agorithm eads to use a quadrature formua. We wi see that in appications, it is more time consuming. V. NUMERICAL EXPERIMENTS A. Traveing a singe PH curve Let us give a PH curve and a feedrate as shown in Fig. 4. This feedrate has been generated with the imited jerk strategy described in [10]. We wi appy the agorithm presented in section IV-C for this curve using the Pythagorean property 1 and in a second experiment, seeing it as a cassica poynomia curve and using a quadrature rue. In both case, we choose a samping period of 10 3 s. Fig. 5. Semiogarithmic errors for position and speed in the singe PH case. Fig. 4. A singe PH curve to trave Top, eft. The desired feedrate Top, right and jerk Bottom. An error anaysis for the PH case is shown in Fig. 5. Fina position error is about m and speed error gobay osciates between and m.s 1. It occurs very sma osciations on the feedrate. We now consider that we do not have a PH curve but ony a genera poynomia one. We obtain simiar accuracy as shown in Fig. 6. The major difference is the running time. For this exampe, treating about 1500 points of discretization takes near to 85 seconds in the poynomia case and ony 6 in the PH case. In order to prove this observation on running time, we aunch severa experiments with different samping periods. The resuts are summarized in Tabe I and Fig. 7. We notice a quasi-inearity property with approximated constant Fig. 6. Semiogarithmic errors for position and speed in the singe poynomia case. in PH case and in poynomia case. So, for the given precision ε, it takes approximativey 14.4 more time to run the interpoator with poynomia spines than PH spines. In poynomia case, running time wi be too high when we wi have to treat an entire spine and not just a singe part. That seems to be the reason that such an interpoator is not used and

6 that interpoators based on Tayor expansions, ess accurate but ess time consuming, are prefered. Samping Number CPU Time, CPU Time, period of points PH poynomia 10 1 s s s s s s s s s s s s s s s 10 2 s s s s s s s s s s s s s s s 10 3 s s s s s s s s s s s s s s s 10 4 s s s TABLE I CPU TIMES OF THE INTERPOLATOR IN SINGLE PH AND SINGLE POLYNOMIAL CASES FOR SOME SAMPLING PERIODS. a Fig. 8. b a PH cubic spine to trave. b The desired feedrate. Once again, we present error graphs in Fig. 9. We ceary distinguish the pieces of the spine in these graphs. When we change of spine piece, the previous error is corrected by starting on the new piece by its extremity and not by the ast computed point. This produces imited jumps for the speed error which are not important. a b Fig. 7. CPU time in function of the number of discretization points, a in the PH case, b in the poynomia case. B. Traveing a PH spine We are now interested in foowing a rea PH G 2 spine with a desired feedrate as shown in Fig. 8. Again, this feedrate has been generated with the strategy described in [10]. Fig. 9. Semiogarithmic errors for position and speed in the PH spine case.

7 We compare the efficiency of the agorithm with an interpoator based on a first order Tayor expansion. The parameters τ i, i = 0, 1,..., m with m = 7959 in our exampe are given by the approximation : τ i+1 = τ i + V i dγτ dτ T s, 9 τ=τi where V i is the desired speed at time t i, Γ the spine obtained by the L 2 1C 2 method see [1] and T s the samping period. With this interpoator, we evidence in Fig. 10 an average position error of 0.3 mm and a fina position error of 5 microns. Error graphs presented in Fig. 11 show the accuracy defaut of the method. Fig. 11. Semiogarithmic errors for position and speed with the first order Tayor interpoator. Fig. 10. Goba resut and fina error position for the first order Tayor interpoator. CONCLUSION In this artice, we have justified the use of Pythagorean hodograph spines in path panning. The associated PH interpoator based on exact arc ength cacuation shows simiar accuracy than a simiar one for simpe poynomia spines but it is amost fifteen times faster. Moreover, it is much more precise than a Tayor interpoator. Thus, PH spines seems to be a good soution for rea time and accuracy issues. REFERENCES [1] Phiippe Auquiert, Oivier Gibaru, Éric Nyiri, Fast L k 1 Ck poynomia spine interpoation agorithm with shape-preserving properties. Computer Aided Geometric Design [2] Rida T. Farouki, T. Sakkais, Pythagorean Hodographs. IBM Journa of Research and Deveopment [3] Rida T. Farouki, Andrew Neff, Hermite interpoation by Pythagorean hodograph quintics. Mathematics of computation 64, n [4] Rida T. Farouki, Sagar Shah, Rea time CNC interpoators for Pythagorean-hodograph curves. Computer Aided Geometric Design [5] Rida T. Farouki, Yi-Feng Tsai, Exact Tayor series coefficient for variabe-feedrate CNC curve interpoators. Computer Aided Geometric Design [6] Rida T. Farouki, Pythagorean-hodograph curves - Agebra and geometry inseparabe. Springer-Verag [7] Gašper Jakič, Jernej Kozak, Marjeta Krajnc, Vito Vitrih & Emi Žagar, On interpoation by panar cubic G 2 Pythagorean-Hodograph spine curves. Mathematics of computation 79, n [8] D.S. Meek, D.J. Waton, G 2 curve design with a pair of Pythagorean Hodograph quintic spira segments. Computer Aided Geometric Design [9] Ade Oabi, Richard Béarée, Éric Nyiri, Oivier Gibaru Enhanced Trajectory Panning For Machining With Industria Six-Axis Robots.IEEE Internationa Conference on Industria Technoogy, ICIT [10] Ade Oabi, Richard Béarée, Oivier Gibaru, Mohamed Damak, Feedrate panning for machining with industria six-axis robots. Contro Engineering Practice, Vo. 18 Issue [11] Yi-Feng Tsai, Rida T. Farouki, Bryan Fedman, Performance anaysis of CNC interpoators for time-dependent feedrates aong PH curves. Computer Aided Geometric Design Future work wi concentrate on appication of this work for rea path and on designing a G 2 PH interpoation scheme for non convex data.