Introduction to Dynamic Path Inversion

Transcription

1 Dipartimento di ingegneria dell Informazione Università di Parma Dottorato di Ricerca in Tecnologie dell Informazione a.a. 2005/2006 Introduction to Dynamic Path Aurelio PIAZZI DII, Università di Parma 25 January 2006

2 Outline Introduction The problem and differential flatness A selection of solved problems Geometric continuity of Cartesian paths 2

3 Introduction In the previuos lesson we have posed and solved (for linear and scalar systems) the stable dynamic input-output signal inversion problem ut () Σ yt () For multivariable systems the signal inversion problem is: p Given a desired bounded y ( t) find a bounded ( ) d p u d t such that ( d ( ), d ( )) u t y t B. 3

4 Introduction For multivariable systems the inversion problem can also be posed as a stable dynamic input-output path inversion problem The idea is to consider the output signal y(t) a function (curve) parameterization of a path Γ in the output space R p. For a given time interval [0, t 1 ] Γ = y([0, t 1 ]). y 3 Γ y 2 y 1 4

5 The problem and differential flatness Dynamic Input-Output Path Problem: Given a path p Γ and a traveling time t 1 > 0 find initial conditions and input ut () for which the system output y() t safisfies ([0, ]) y t = Γ 1 This problem is quite general and especially relevant for the motion control of nonholonomic wheleed vehicles. 5

6 The problem and differential flatness The path inversion problem has a strong connection with differential flatness (Fliess et al. 1993). A system with m (scalar) inputs is said to be (differentially) flat if there exist m outputs y F1,..., y Fm for which the system variables (the states and the inputs) can be algebraically expressed as functions of the y Fi s and their derivatives (till a finite order). The vector y = ( y,, y ) is called the flat outp ut. F F1 Fm 6

7 F F1 Fm The problem and differential flatness Consider the nonlinear system Σ in state-space form: x = f( x, u) m n p u, x, y y = g( x, u) Σ is differentially flat if there exists a vector-valued function h( ) for which defining T y = [ y,, y ], y = ( β ) 1 βm, 1,, 1,,,, h x Du D u Du D u F m m there exist functions A( ) and B( ) satisfying: x= A y D y y D y α1 αm (,,,,,, ), F1 F1 Fm Fm u = B y D y y D y (,, α + αm,,,, + ). F1 F1 Fm Fm 7

8 The problem and differential flatness The dynamic path inversion problem is (relatively) easy to solve when the system is differentially flat and the actual output is flat (y = y F ). Conceptual solution: Given the path Γ choose a velocity planning on it ( ) to find the trajectory yt ( ) for which y[0, t] =Γ. Then, determine the initial conditions x(0) and the input signal ut ( ) by applying the functions A( ) and B( ). 1 Proving that y is flat may be not trivial 8

9 A selection of solved problems Solved path inversion problems: 1) car-like vehicle (Nelson 1989, Rouchon et al. 1993, Reuter 1998, ARGO Project: Guarino, Piazzi, Bertozzi, Broggi, Fascioli, 1999, 2002 ) x = vcosθ y = vsinθ v θ = tanδ l y µ l ± x 9

10 A selection of solved problems Consolini, Piazzi, Tosques 2001, 2003 l Γ δ Q ν τ z w d P θ 10

11 A selection of solved problems 2) Unicycle mobile robot (solution with smooth velocities, Guarino, Piazzi, Romano 2004 TR) x = vcosθ y = vsinθ θ = ω y θ x 11

12 A selection of solved problems 3) Wheeled omnidirectional robot (Guarino, Piazzi, Romano 2002): an holonomous model. y 2 1 θ 3 x 12

13 A selection of solved problems 4) General n-trailer system (Rouchon et al. 1993, Altafini 2002, ) 5) VTOL model (Consolini, Tosques 2004 CDC): a nonminimumphase system. 6) Chaplygin-like nonholonomic systems (Tosques, Consolini 2003 ECC). 7). The general dynamic path inversion is an open research problem 13

14 A selection of solved problems The solution to the path inversion problem is an input signal that can be used as a feedforward control. In preview of a practical application how to complement this feedforward with a feedback action? 1. Path-error feedback correction (classic approach to path following: various schemes can be devised) 2. Iterative steering (Lucibello, Oriolo 1996 CDC, Automatica 2001). Originally it was proposed as a novel approach to stabilization of nonlinear systems. 3. Path-error feedback correction plus Iterative steering. 14

15 A selection of solved problems Iterative steering concept applied to the path following problem idial desired path replanned path actual path Iterative steering requires a supervisor architecture 15

16 Geometric continuity of Cartesian paths Relevant issues for the path inversion problem Apart differential flatness other issues are: Nonholonomy Minimum-phase/Nonminimum-phase Geometric continuity of paths 16

17 Geometric continuity of Cartesian paths A curve on the Cartesian plane can be described by the map p(u), u [u 0, u 1 ] : p : [ u, u ] 2 α (u) u p( u) = β (u) y u 1 The path associated to the curve p( u) is the image of [ u, u ] according to p( u) : p ( u u ) [, ]. u 0 x 17

18 Geometric continuity of Cartesian paths def. A curve p( u), u [ u, u ] is regular if ( ) 1. p ( ) P [ u, u ] 2. p ( ) 0 u [ u, u ] ( ) P [ u, u ] is the set of piecewise-continuous functions over the domain [ u, u ]. A regular curve has a well-defined pu ( ) unit tangent vector τ ( u) pu ( ) 18

19 Geometric continuity of Cartesian paths The arc length function is f :[ u, u ] [0, s ] s u f ( u) s = p ( ξ ) dξ f( u ) is the total curve length 1 Hence, there exists the inverse f : u u 0 ( ) Given a regular curve p( u), f( ) C [ u, u ] and it is bijective. 1 f s1 u0 u1 :[0, ] [, ] 1 s u = f s ( ) 19

20 Geometric continuity of Cartesian paths Attached to every point of a regular curve p(u) there is the orthonormal moving frame {τ(u), ν(u) } congruent to the axes of the {x, y }-plane. y ν τ ( ) If p ( ) P [ u, u ] then the curvature of p( u) is well-defined according to the Frenet formula osculating circle x dτ ( u) = kc ( u) ν ( u), kc ( u) ds 1 is the radius of the osculating circle k ( u) c 20

21 Geometric continuity of Cartesian paths The curvature function is kc :[ u0, u1], u kc( u) α( u) β( u) α( u) β( u) kc ( u) = 3/2 1 ( 2 2 α ( u) + β ( u) ) The curvature as a function of the arc length is κ :[0, s ], s κ( s) c ( 1 ()) κ ( s) = k f s u kc( u) = κ( f( u) ) = κ ( ξ) dξ p u0 21

22 1 ( G -curves) Geometric continuity of Cartesian paths def. A curve p( u) has first order geometric continuity and we say p( u) is a G 1 -curve if 1. p( u) is regular; 2. the unit tangent vector is continuous alon 2 ( G -curves) 1 1. ( u) is a G -curve; ( u u ) 2. p ( ) P [, ] ; 2 -curve if ( ) 0 g the curve: τ ( C ) [ u0, u1] def. A curve p( u) has second order geometric continuity and we say p( u) is a G p 0 3. the curvature is continuous along the curve: k ( ) C [ u, u ] ( s ) 0 or κ ( ) C [0, 1]. c ( ) 22

23 G 1 2 Geometric continuity of Cartesian paths - and G -curves were introduced in computer graphics by Barsky and Beatty (1983). ( ) 0 0 A curve p( u) C [ u0, u1] can be defined as a G -curve. Generalization to G k -curves (Piazzi, Romano, Guarino 2003 ECC) k ( G k ) def. -curves; 2 A curve p( u) has k-th order geometric continuity and we say p( u) is a G p k 1 1. ( u) is a G -curve; ( ) k 2. D p( ) P [ u, u ] ; k -curve if 3. the ( k 2)-nd order derivative with respect to the arc length of the ( ) k 2 0 curvature is continuous along the curve: D κ ( ) C [0, s1 ]. 23

24 def. Geometric continuity of Cartesian paths k ( G -paths; k 0) A set of points of a Cartesian plane is a -path, i.e., a path with k k-th order geometric continuity, if there exists a G -curve whose image is the given path. 2 k Formally: Γ is a G -path if there exists G k ( ) k a G -curve p( u), u [ u, u ] such that p [ u, u ] =Γ. 24

25 References M. Fliess, J. Levine, Ph. Martin, P. Rouchon, Flatness and defect of nonlinear systems: introductory theory and examples, Int. J. Control, Vol. 61, No. 6, pp , P. Rouchon, M. Fliess, J. Levine, Ph. Martin,, Flatness, motion planning and trailer systems, Proc. Conf. Decision and Control, pp , P. Lucibello, G. Oriolo, Stabilization via iterative state steering with application to chained-form systems, Proc. Decision and Control, Vol. 3, pp , P. Lucibello, G. Oriolo, Robust stabilization via iterative state steering with an application to chained-form systems, Automatica, Vol. 37, pp , W.L. Nelson, Continuous Steering-Function Control of robot carts, Transactions on Industrial Electronics, Vol. 36, No. 3, pp , J. Reuter, Mobile robot trajectories with continuously diffirentiable curvature: an optimal control approach, Proc.Int. Conf. Intelligent Robots and Systems, Victoria B.C. (Canada), October C. Altafini, Following a Path of Varying Curvature as an Output Regulation Problem, Transactions on Automatic Control, Vol. 47, No. 9, pp , September A. Broggi, M. Bertozzi, A. Fascioli, C. Guarino Lo Bianco, and A. Piazzi, The ARGO autonomous vehicle s vision and control systems, Int. J. of Intelligent Control and Systems, Vol. 3, No. 4, pp , A. Piazzi, C. Guarino Lo Bianco, M. Bertozzi, A. Fascioli, and A. Broggi, Quintic G^2-splines for the iterative steering of vision-based autonomous vehicles, IEEE Transactions on Intelligent Transportation Systems, Vol. 3, No. 1, pp , March L. Consolini, A. Piazzi, M. Tosques, Path following of car-like vehicles using dynamic inversion, Int. J. Control, Vol. 76, No. 17, pp , November C. Guarino Lo Bianco, A. Piazzi, M. Romano, Smooth motion generation for unicycle mobile robots via dynamic path inversion, IEEE Transactions on Robotics, Vol. 20, No. 5, pp , October C. Guarino Lo Bianco, A. Piazzi, M. Romano, Smooth control of a wheeled omnidirectional robot, Proc.IFAC 2004 Intelligent Autonomous Vehicles Conference, Lisboa, Portogal, 5-7 July L. Consolini, M. Tosques, A controlled invariance problem for the VTOL aircraft with bounded internal dynamics, Proc. Conf. Decision Control, December M. Tosques, L. Consolini, A path-following problem for a class of non-linear uncertain system, Proc. European Control Conf., September B.A. Barsky, J.C. Beatty, Local control of bias and tension in beta-spline, Computer Graphics, Vol. 17, No. 3, pp , A. Piazzi, M. Romano, C. Guarino Lo Bianco, G3- splines for the path planning of wheeled mobile robots, Proc. European Control Conf., September