Minimum-time constrained velocity planning

Transcription

1 7th Meiterranean Conference on Control & Automation Makeonia Palace, Thessaloniki, Greece June 4-6, 9 Minimum-time constraine velocity planning Gabriele Lini, Luca Consolini, Aurelio Piazzi Università i Parma Dipartimento i Ingegneria ell Informazione Viale G.P. Usberti 8A 43 Parma, Italy gabriele.lini@gmail.com, luca.consolini@polirone.mn.it, aurelio.piazzi@unipr.it Abstract This paper proposes a metho for minimum-time velocity planning with velocity, acceleration jerk constraints generic initial final bounary conitions for the velocity the acceleration. This minimum-time planning problem is relevant in the context of robotic autonomous navigation, where the iterative steering supervisor perioically replans the future mobile robot motion starting from current position, velocity acceleration conitions. The problem is face through iscretization its solution is base on a sequence of linear programming feasibility checks, epening on motion constraints bounary conitions. I. INTRODUCTION The problem of motion planning for autonomous guie vehicles is a well known stuie issue in robotics, see for example the recent books [] []. This paper proposes a technique for minimum-time velocity planning, consiering given jerk, acceleration velocity constraints. This minimum-time planning problem is cast in the context of the so-calle path-velocity ecomposition [3] using the iterative steering navigation technique [4], [5]. In this scenery, it is important to plan a smooth velocity profile with arbitrary velocity acceleration bounary conitions. The relevant velocity planning problem is the synthesis of a velocity C -function that permits in minimum-time, with boune velocity, acceleration jerk, to interpolate given initial final conitions. The minimum-time transition is obtaine by iscretizing the continuous-time moel formulating an equivalent iscrete-time optimization problem solve by means of linear programming techniques. More precisely, bounary conitions problem constraints are expresse by linear inequalities on a column vector u, representing the input signal (i.e the jerk) at sampling times. Hence, the minimumtime planning problem is reformulate as easibility test for a linear programming problem, the minimum number of steps require to complete the given transiction can be foun through a simple bisection algorithm. The use of linear programming techniques for solving minimum-time problems for linear iscrete-time systems subject to boune inputs ates back to Zaeh [6]. Subsequently, many contributions have appeare focusing on Corresponing author. This work has been supporte by PRIN scientific research funs of the Italian Ministry of University Research. various improvements. For example aster algorithm is propose in [7]. For what concerns time-optimal control for continuous-time systems, a relate result, uner ifferent hypotheses, is presente in [8]. The paper is organize as follows. Section II states the minimum-time constraine planning problem introuces a sufficient conition for the existence of a time-optimal solution. The system iscretization the linear programming problem are presente in Section III. Section IV escribes with etails the bisection algorithm. Section V VI report ew velocity planning examples final remarks respectively. II. THE PROBLEM AND A SUFFICIENT CONDITION The problem face in this paper is the minimum-time planning of a smooth velocity profile v(t) PC ([, t f ]), where t f represents the travelling minimum-time along a given path whose length is equal to s f, respecting given velocity, acceleration, jerk constraints. The following efinition will be use along this paper. Definition : A function f : R R, t f(t) has a PC continuity, or piecewise C -continuity, we write f(t) PC if ) f(t) C (R), ) f(t) C (R {t, t,... }), 3) lim t t i f(t), lim t t + i f(t), i =,,... where {t, t,... } is a set of iscontinuity instants. The pose problem can be summarize as follows such that tf min t f () v PC v(ξ)ξ = s f () v() = v, v(t f ) = (3) v() = a, v(t f ) = (4) v(t) v M t [, t f ], (5) v(t) a M t [, t f ], (6) v(t) t [, t f ], (7) where s f >, v M, a M, > v,, a, R are given bounary conitions. For the special case of /9/$5. 9 IEEE 748 Authorize license use limite to: Universita egli Stui i Parma. Downloae on November, 9 at 8:6 from IEEE Xplore. Restrictions apply.

2 zero bounary conitions (v = =, a = = ) a close form solution has been provie by [9]. Remark that in our context of iterative autonomous navigation, it is crucial to consier generic bounary conitions on initial final velocities accelerations. The minimum-time constraine planning problem can be easily recast into a minimum-time control problem with respect to a suitable state-space system. Inee consier the jerk v(t) as the control input u(t) of the cascae of three integrators as epicte in Figure. u(t) v(t) v(t) s(t) s s s Fig.. The system moel for velocity planning Introuce the state x(t) as the following column vector x(t) := x (t) x (t) s(t) v(t). (8) x 3 (t) v(t) Then, the system equations are given by where A = ẋ(t) = A x(t) + B u(t), (9) B =. () Constraints (5), (6) (7) will be consiere as two state constraints an input boun respectively. Hence, problem ()-(5) is equivalent to fining a time-optimal control u (t) that brings system (9) from the initial state x() = [ v a ] T to the final state x(t f ) = [s f ] T in minimum time t f, while satisfying the following constraints x (t) v M t [, t f], () x 3 (t) a M t [, t f ], () u (t) t [, t f ]. (3) The existence of the solution u (t) of the problem ()-(7) epens on the values of the initial state x, the final state x f, it also epens on the values of the constraints ()- (3). To guarantee the existence of the optimal control u (t), these values must respect four sufficient conitions as state in the following result. Proposition : The minimum-time optimal control u (t), solution of the problem ()-(7), from initial state x() = [ v a ] T to final state x(t f ) = [s f ] T exists if the following sufficient conitions are satisfie: v v M a a M ; if a > : v + a v M ; (4) if a < : if < : v + a v M ; (5) a f v M ; (6) if > : a f v M. (7) Proof: Consier the case a > ; if conition (4) on initial state x is true, it is possible to apply a control function u(t) = which brings the acceleration x 3 (t) to zero before the velocity x (t) excees its bounary value v M, as epicte in Figure. In fact, if u(t) = with t [, t c ] Fig.. x (t) v M v x (t) a M a a M u(t) t c t c t c v c v c t f t f Sufficient conitions on velocity/acceleration initial final values (where t c is the critical time where the acceleration became null) the following result is true x 3 (t) = a + = a + tc tc = a t c. t f u(ξ)ξ (8) ( )ξ But in t = t c we have x 3 (t c ) = so it is possible to obtain the critical time value as follows t c = a. (9) Integrating equation (8) in t c, the value of v c = x (t c ) is obtaine tc v c = v + x 3 (ξ)ξ = v + tc (a ξ)ξ = v + a t c t c. () t t t 749 Authorize license use limite to: Universita egli Stui i Parma. Downloae on November, 9 at 8:6 from IEEE Xplore. Restrictions apply.

3 Hence, by substituting relation (8) in () we have v c = v + a, () then, by virtue of conition (4) we know that v c v M the constraint (5) is satisfie. The same results are true for the final state value. Consier the case of < ; if conition (6) is verifie the velocity value x ( ) = v c oes not excee the bounary value v M. Hence, the final state x(t f ) can be achieve by applying the control function u(t) = with t [, t f ] as epicte in Figure. Now consier the time-interval [t c, ]. It is always possible to etermine a polynomial jerk function j(t) = a + a t + a t, with t [t c, ], such that the following bounary conitions are verifie: x 3 ( ) = x ( ) = v c x ( ) = s f s, () where s = t f x (ξ)ξ, while satisfying the constraints ()- (3). This is true because the transition timeinterval [t c, ] can be chosen arbitrary large. The sufficient conitions (5) (7) can be easily prove in the same way. The conitions introuce in Proposition are sufficient but not necessary. In fact, for a minimum time t f sufficiently small, the conitions (4)-(7) can be violate, while still satisfying the constraints ()-(3). III. AN APPROXIMATED SOLUTION USING DISCRETIZATION This section shows how to fin a numerically approximate solution of problem ()-(7) by iscretization of system (9). The technique that will be introuce, exploits the results presente by Consolini Piazzi in [], which shows that, given a continuous-time system, an approximate optimal control can be foun through the following proceure: ) fin the iscretize system with sampling perio T s ; ) fin the optimal input sequence u (k); 3) use for the continuous-time system the input function u(t) obtaine from the iscrete-time sequence with a zero-orer hol ( u(t) = u T s t ), (3) T s where T s R is the sampling perio x R x = max {z Z : z x} enotes the integer part of x. As shown in [], when T s the approximate solution converges to the optimal continuous-time solution. The optimal iscrete-time control sequence u (t) can be foun by means of linear programming. In fact, in the iscrete-time case, the constraints ()-(3) can be represente as linear inequalities the minimum number of steps neee for the requeste transition can be foun through a sequence of feasibility tests of a linear programming problem. The matrices of the equivalent iscrete-time system are the following ones: A = e A Ts = T s T s T s, (4) ( ) Ts B = f(a, T s ) B = e A τ τ B = 6 T 3 s T s T s, (5) where T s is the sampling perio. Then, the iscrete-time system is x(k + ) = A x(k) + B u(k), (6) whose solution is given by where k x(k) = A k x + A k j B u(j), (7) x(k) = j= x (k) x (k) x 3 (k) Define the control vector u R k f as follows u() u() u =. u(k f ) from (3) it follows that it must be. (8) (9) u M kf u u M kf (3) where kf enotes the k f -imensional vector whose components are all equal to. The velocity constraint for iscretetime system is given by v M x (k) v M, with k =,...,k f. (3) From equation (7), velocity sequence x (k) can be written as follows where x (k) = C x(k) k = C A k x + j= A k j B u(j) k = C A k x + C A k j B u(j), (3) j= C = [ ]. (33) By substituting (3) in (3), the following relation is obtaine k v M C A k x C A k j B u(j) v M C A k x, j= (34) 75 Authorize license use limite to: Universita egli Stui i Parma. Downloae on November, 9 at 8:6 from IEEE Xplore. Restrictions apply.

4 with k =,...,k f. Set v c = v M f, then the inequality on velocity constraint (3) can be written as follows v c G H u v c G, (35) where G R k f H R k f k f are given by C x C A x G = C A x, (36). C A k f x C B O O. C A B..... O H = C A B O. (37) C A k f B C B The acceleration constraint for iscrete-time system (6) is given by a M x 3 (k) a M, with k =,...,k f. (38) Set a c = a M f then constraint (38) is written as C = [ ], (39) a c G H u a c G, (4) where G R k f H R k f k f are given by C x C A x G = C A x, (4). C A k f x C B O O. C A B..... O H = C A B O. (4) C A k f B C B The interpolation conition on final state can be written as follows x f = x(k f ) = x (k f ) x (k f ) = s f. (43) x 3 (k f ) From equation (7) we have k f x f = A k f x + A k f j B u(j), (44) j= then, by substituting equation (44) in (43) we obtain the final state interpolation conition as follows H eq u = x f A k f x, (45) where H eq R 3 k f is given by [ ] H eq = A k f B A k f B B. (46) In conclusion given a number of steps k f, there exists an input vector u for which the constraints on velocity, acceleration jerk, the final interpolation conition are satisfie if only if the following linear programming problem is feasible u M kf u u M kf v c G H u v c G a c G H u a c G H eq u = x f A k f x. (47) IV. THE BISECTION ALGORITHM The minimum number of steps kf the associate optimal iscrete-time control sequence u (k), with k =,...,kf, can be eterminate by means of a sequence of linear programming feasibility tests, efine by (47), through a simple bisection algorithm. The Minimum-Time Control algorithm (MTC) is summarize as follows: begin k f ; l ; while LPP o l k f k f k f en h k f ; while h l > o k f h+l ; if LPP then l k f ; else h k f ; en kf h; u (k) u; en In MTC algorithm LPP enotes a linear programming proceure that solves problem (47), which, if easible solution exists, returns the solution sequence u the number of steps k; if the problem is feasible it also returns a Boolean true value. The algorithm performances strongly epen on the use sampling time: by reucing T s, which means sampling the continuous-time system with an higher frequency, the imension of the resulting linear programming problem increases, thus causing an increment of the total computational time. 75 Authorize license use limite to: Universita egli Stui i Parma. Downloae on November, 9 at 8:6 from IEEE Xplore. Restrictions apply.

5 Consiering the computational complexity, Karmarkar has shown in [] that a linear programming problem can be solve by means of an interior-point algorithm with running time proportional to n 3.5, where n is the number of inequalities. In our case this woul means that each feasibility test woul require a time proportional to n 3.5 s, where n s is the total number of samples. The complexity of the bisection search, with respect to the minimum number of samples, is given by O(log n s ), therefore the total complexity of the propose algorithm is given by O(n 3.5 s log n s ). For more etails on the algorithm complexity see [] a(t) u * (t) V. EXAMPLES Example : consier the following interpolation conitions constraints: initial state final state x := x f := problem constraints s v a s f v M =, 65 [m/s] a M =.5 [m/s ] =.5 [m/s 3 ] The jerk, acceleration, velocity space profiles, obtaine by means of the MTC algorithm, are epicte in Figure 3. Example : consier the following problem: initial state final state x := x f := problem constraints s v a s f, 5 v M =, 5 [m/s] a M =.6 [m/s ] =.5 [m/s 3 ] The jerk, acceleration, velocity space profiles, obtaine in this case, are epicte in Figure 4. Example 3: the problem ata are given by: initial state final state x := x f := s v a s f.5, 67, 5, v(t) s(t) Fig. 3. Optimal solutions for Example problem constraints v M = [m/s] a M =.5 [m/s ] =.5 [m/s 3 ] Figure 5 shows optimal solution obtaine by means of the MTC algorithm. VI. CONCLUSION This paper has propose a metho for velocity planning consiering velocity, acceleration jerk constraints. A sufficient conition on initial final states was introuce. The minimum-time planning problem is solve by iscretization of the continuous-time system, formulating an equivalent iscrete-time optimization problem solve by linear programming techniques. The MTC algorithm, which etermines an approximation of the minimum-time solution, is well suite to be aopte into a supervisory architecture for iterative steering navigation. REFERENCES [] S. M. LaValle, Planning Algorithms. Cambrige, U.K.: Cambrige University Press, 6. [] B. Siciliano O. Khatib, Es., Springer Hbook of Robotics. Springer, 8. [3] K. Kant S. Zucker, Towar efficient trajectory planning: the pathvelocity ecomposition, Int. J. of Robotics Research, vol. 5, no. 3, pp. 7 89, 986. Authorize license use limite to: Universita egli Stui i Parma. Downloae on November, 9 at 8:6 from IEEE Xplore. Restrictions apply.

6 .8.6 a(t) u * (t) v(t) s(t).8 a(t) u * (t) Fig. 4. Optimal solutions for Example.5 v(t) s(t) [4] P. Lucibello G. Oriolo, Robust stabilization via iterative state steering with an application to chaine-form systems, Automatica, vol. 37, no., pp. 7 79, January. [5] C. Guarino Lo Bianco, A. Piazzi, M. Romano, Smooth motion generation for unicycle mobile robots via ynamic path inversion, IEEE Trans. on Robotics, vol., no. 5, pp , Oct. 4. [6] L. A. Zaeh, On optimal control linear programming, IRE Transactions on Automatic Control, vol. 7, no. 4, pp , 96. [7] G. Bashein, A simplex algorithm for on-line computation of optimal controls, IEEE Transactions on Automatic Control, vol. 6, no. 5, pp , 97. [8] S.-J. Kim, D.-S. Choi, I.-J. Ha, A comparison principle for stateconstraine ifferential inequalities its application to time-optimal control, IEEE Transactions on Automatic Control, vol. 5, no. 7, pp , July 5. [9] R. D. Peters, Ieal lift kinematics: Complete equations for plotting optimum motion, Proceeings of ELEVCON95 (The International Association of Elevator Engineers), 995, republishe by Elevator Worl, April 996 by Elevatori, May/June 996. [] L. Consolini A. Piazzi, Generalize bang-bang control for feeforwar constraine regulation, Proceeings of the 45th IEEE Conference on Decision Control, pp , 6. [] N. Karmakar, A new polynomial-time algorithm for linear programming, Report. AT&T Bell Laboratories, 984. [] L. Consolini, O. Gerelli, C. Guarino Lo Bianco, A. Piazzi, Flexible joints control: A minimum-time fee-forwar technique, Mechatronics, Elsevier, vol. 9, pp , Oct Fig. 5. Optimal solutions for Example Authorize license use limite to: Universita egli Stui i Parma. Downloae on November, 9 at 8:6 from IEEE Xplore. Restrictions apply.