CONTROL OF THE NONHOLONOMIC INTEGRATOR

Transcription

1 June 6, 25 CONTROL OF THE NONHOLONOMIC INTEGRATOR R. N. Banavar (Work done with V. Sankaranarayanan) Systems & Control Engg. Indian Institute of Technology, Bombay Mumbai -INDIA.

2 Outline Nonholonomic systems - a brief introduction Nonholonomic integrator (NI) (Kinematic model of a wheeled mobile robot) Four stabilizing control laws The Extended Nonholonomic Double Integrator (ENDI) - Dynamic model of a wheeled mobile robot A Sliding mode controller Other problems 1

3 Classification of constraints in mechanical systems Holonomic constraints - restrict the allowable configurations of the system Nonholonomic constraints - do not restrict the allowable configurations of the system but restrict instantaneous velocities/accelerations Velocity level constraints - parking of a car, wheeled mobile robots, rolling contacts in robotic applications Acceleration level constraints - fuel slosh in spacecrafts/launch vehicles, underwater vehicles, underactuated mechanisms (on purpose or loss of actuator) systems - serial link manipulators 2

4 A coin rolling on a horizontal plane y φ θ x Figure 1: Vertical coin on a plane 3

5 The Differential geometric view Rewriting the constraints in terms of annihilator codistributions [ ] ẋ sin(θ) cos(θ) ẏ = θ Permissible motions of the coin are such that the vector field is annihilated by the codistribution [ ] Ω= sin(θ) cos(θ) Can Ω be expressed as the gradient of a function λ :(IR 1 IR 1 S 1 ) IR 1 as [ ] Ω= dλ 4

6 The NI or Brockett Integrator Nonholonomic integrator Third order driftless system Equilibria - the whole of IR 3 x IR 3 ẋ 1 = u 1 ẋ 2 = u 2 ẋ 3 = x 1 u 2 x 2 u 1 Not linearly controllable at any equilibrium point No continuous feedback control law that can globally stabilize the system Solution: Time varying feedback OR discontinuous feedback Mimics the kinematic model of a mobile robot with a nonholonomic constraint 5

7 Past work on the NI Motion planning - Given x() = x and x f and a time interval [,t f ], find a control history u( ) (from an admissible class of functions) such that x(t f )=x f. Stabilization - Compute a feedback control law that stabilizes the system in a region (set) near the origin Exhaustive survey - A. M. Bloch - Nonholonomic Mechanics and Control -Springer, 23 6

8 A wheeled mobile robot on a horizontal plane Y τ 1 θ (x,y) τ 2 X Figure 2: Schematic of a mobile robot 7

9 Wheeled mobile robot Notation M = Mass of the vehicle I = Inertia of the vehicle F = 1 R (τ 1 + τ 2 ) τ = L R (τ 1 τ 2 ) L = Distance between the center of mass and the wheel R = Radius of the rear wheel τ 1 = Left wheel motor torque τ 2 = Right wheel motor torque Nonholonomic constraint - no lateral (sideways) motion ẋ sin θ ẏ cos θ = 8

10 Kinematics and Dynamics Generalized coordinates - position of the center of mass (x, y) and orientation θ Constraints of motion ẋ sin θ ẏ cos θ = No lateral motion Equations of motion θ = ω ẋ = v cos θ ẏ = v sin θ Control inputs (at the kinematic level) - drive v and steer ω Dynamics - (control inputs are forces and torques) M v = F I ω = τ 9

11 Transforming the variables Step 1 - State transformation from (θ, x, y) to(z 1,z 2,z 3 ) z 1 z 2 z 3 = θ = x cos θ + y sin θ = x sin θ y cos θ Step 2 - State transformation from (z 1,z 2,z 3 )to(η 1,η 2,η 3 ) η 1 η 2 = z1 = z2 η 3 = 2z3 + z 1 z 2 1

12 The new representation Define modified control inputs as u 1 = ω u2 = z3 ω + v The system in the new states and inputs is η 1 u 1 η 2 = u 2 η 3 η 1 u 2 η 2 u 1 11

13 Technique 1 - Exponential Stability in a Dense Set Definition 1.1 If the system is exponentially stable in an open dense set O, and the control law is well defined and bounded along the closed loop trajectories, then the closed system is said to be almost exponentially stable (AES). The open dense set in the system under consideration is O = {x IR 3 x 1 }. Proposition 1.1 The NI is AES with the control law ( ) u = k 1 x 1 x k 3 3 x 1 k 1 x 2 x 1 where <k 1 <k 3. 12

14 Proof The closed loop dynamics can be written in the form ẋ = Ax + g(t) where k 1 A = k 1 k 3 ; g(t) = k 3x 3 (t) x 1 (t) Since g(t) k 3 x 3 () x 1 () e(k 1 k 3 )t and the eigen values of A are negative-real, the system is exponentially stable. If x 1 () =, then an open loop control can be used to steer the system to any non zero value of x 1 and then the control law can be applied. 13

15 Technique 2 - Bounded Control Law In Technique 1, if the initial condition is close to the surface x 1 =, the magnitude of u 2 is very large. Define α = x 3 x 1 and a set and three other sets as M 1 = {x IR 3 : α c} M 2 = {x IR 3 : x/ M 1 and x 2 + c 1} M 3 = {x IR 3 : x/ M 1 and x 2 + c < 1} M 4 = {x IR 3 : x 1 = x 3 =} 14

16 Technique 2 Proposition 1.2 The NI is exponentially stable in the set M 1 containing the origin under the following control law ( ) k 1 x 1 x k 3 if x M 1 3 x 1 k 1 x ( ) 2 S k 1/3 s x 2 +c u = ( ) d ( k 1 x 1 k 1 x 2 if x M 2 if x M 3 ) if x M 4 where S(x) = x 3 cx 1 and <k 1 <k 3, <k s and d is any non zero constant. 15

17 Proof of Technique 2 Proof: Case 2 If x M 2, then the closed loop system becomes S 1/3 ẋ 1 = k s The dynamics of S becomes ẋ 2 = x 2 + c ẋ 3 = x 2 k s S 1/3 Ṡ = k s S 1/3 x 2 + c This is called a power rate reaching law and it is finite-time stable. So there exists a finite-time T 1 such that S(t) = t [T 1, ). So all the trajectories will reach the boundary of the set M 1 in some finite-time after which case 1 follows. 16

18 Finite time stabilization to the surface S(x) = The dynamics of the surface is given by Ṡ = k s S 1/3 Consider a Lyapunov candidate function and its rate of change Hence S = is attractive globally. V = S 2 /2 V = k s S 4/3 < For finite time convergence we show V + kv α ([Haimo] condition for finite-time stability) where α (, 1) and k> 17

19 For α =2/3 and k<2 2 3k s we have V + kv 2 3 = S 4/3 (k s k ) < Further Ṡ = which implies that S = is positively invariant 18

20 Proof Case 3 If x M 3, then the dynamics of x 2 is ẋ 2 = d So there exists a finite-time T 2 such that x 2 (T 2 )+c = 1. Then Case 2 follows. Case 4 If x M 4, then closed loop system becomes ẋ 1 = ẋ 2 = k 1 x 2 ẋ 3 = and we have exponential stability 19

21 Techniques 1 and 2 Methods 1 and 2 are related though the AES property of 1 does not hold in 2. Method 2 however guarantees boundedness of the control law in a local domain around the origin. 2

22 Technique 3 - Sliding Mode Control The next two techniques rest on the variable structure (sliding mode) philosophy. The methodologies rest on bringing the system to a surface (defined by the dependent state in the dynamics) in finite time and then exponentially making the other two states reach the origin. Proposition 1.3 The NI is exponentially stable in the set M = {x IR 3 : x 3 =} with the following control law k 1 x 1 u = x k 1/3 if x x 1 k 1 x 2 where k 1,k 3 >. 21

23 Proof of Technique 3 If x 1 () then the closed loop system is ẋ 1 = k 1 x 1 ẋ 2 = k 3x 1/3 3 k 1 x 2 x 1 ẋ 3 = k 3 x 1/3 3 Since k 1,k 3 >, the dynamics of x 3 implies finite time stability while that of x 1 implies exponential stability. So there exists a finite time T 1 > such that x 3 (t) t T 1. The system on M is then governed by the set of equations ẋ 1 = k 1 x 1 ẋ 2 = k 1 x 2 ẋ 3 22

24 Technique 4 Define S(x) = x 2 1 3k 1 x 2/3 3 where k 1 > is a predefined constant, Proposition 1.4 The NI is exponentially stable in the set M = {x IR 3 : x 3 =} with the following time-varying control law ( ) k 1 x 1 x 1 x 1/3 s [t, ) if S(x(t)) > 3 k 1 x ( 2 ) x 1 u = if S(x(t)) x ( 2 ) k 1 x 1 if x 3 = k 1 x 2 23

25 Case 1 Assume S(x()) > We have Proof of Technique 4 The evolution of x 1 (t) is given by The evolution of x 3 is ẋ 1 = k 1 x 1 ẋ 2 = x 1 x 1/3 3 k 1 x 2 ẋ 3 = x 2 1x 1/3 3 x 1 (t) =x 1 ()e k 1t x 3 (t) =±[ x 1() 2 e 2k 1t S(x()) 3k 1 ] 3/2 t [,T 1 ) x 3 (t) = t T 1 24

26 Proof of Technique 4 The positive (negative) sign corresponds to the solution for an initial condition x 3 () > (x 3 () < ). The closed loop system after time T 1 is ẋ 1 = k 1 x 1 ẋ 2 = k 1 x 2 ẋ 3 and is exponentially stable. 25

27 Proof of Technique 4 Case 2 If x 1 () and S(x()), then we have ẋ 1 = x 1 ẋ 2 = x 2 ẋ 3 = The above dynamics leads to an increase in the value of S(x(t)) as time increases. Note that x 3 remains a constant and x 1 has an exponential growth. So there exists a finite-time such that S(x(t)) >. Then Case 1 follows. 26

28 The third case is straightforward. If x 1 () = then any open loop control can push x 1 to some non-zero value, then either case 1 or case 2 follows. 27

29 Techniques 3 and 4 Methods 3 and 4 bring in a variable structure (or sliding mode) feature; the latter is also time-varying. Both techniques use a power rate reaching law to enter a set in which the system is exponentially stable. The idea in Method 4 is to utilize the state x 1 as a gain for the convergence of x 3 to zero in finite-time. So we have the term x 1 x 1/3 3 instead of x1/3 3 x 1. But since this control law valid in a local domain, we have introduced a swtiching strategy to move from anywhere to this domain. All the four control laws are not defined for x 1 ()=. 28

30 Closest parallels - notions of sigma process, paper by Bloch and Drakunov 29

31 Dynamic model of the mobile robot θ ẋ ẏ v ω = ω v cos θ v sin θ + 1 M F + 1 I τ Fifth order system with drift Discontinuous controllers (velocity level) designed from the kinematic model cannot be applied to obtain controllers at the acceleration level for the dynamic model. 3

32 Transforming the variables Step 1 - State transformation z 1 θ z 2 x cos θ + y sin θ z 3 = x sin θ y cos θ z 4 ω v (x sin θ y cos θ)ω The dynamic model in the z variables z 5 ż 1 ż 2 ż 3 ż 4 ż 5 = z 4 z 5 z 2 z 4 τ I F M τ I z 3 z 2 z

33 Transforming the variables Alternate inputs τ u 1 = u 2 = z 2 I 4 z 2 τ I z 3 + F M Step 2 - State transformation from (z 1,z 2,z 3,z 4,z 5 )to (x 1,x 2,x 3,y 1,y 2 ) x 1 = z1 x 2 = z2 x 3 = 2z3 + z 1 z 2 y 1 = z4 y 2 = z5 The system equations are now in the form ẍ 1 = u 1 ẍ 2 = u 2 ẋ 3 = x 1 ẋ 2 x 2 ẋ 1 32

34 The Extended Nonholonomic Double Integrator (ENDI) ẋ = f(x)+ 2 g i (x)u i i=1 where x =[x 1,x 2,x 3,y 1,y 2 ] T IR 5 is the state vector and u =[u 1,u 2 ] IR 2 is the control y 1 y f(x) = 2 x 1 y 2 x 2 y 1 ; g 1 (x) = ; g 2 (x) =

35 Features of the ENDI The equlibria of the ENDI system are of the form x e = {x IR 5 : y 1 = y 2 =} and satisfy the following properties 1. x e is not stabilized by any smooth feedback control laws. 2. The ENDI is locally strongly accessible from any x IR The ENDI is small time locally controllable (STLC) from any x e. 34

36 A Sliding Mode Approach Define S 1 (x) = kx 1 + y 1 and S 2 (x) = x 1 y 2 x 2 y 1 + k 3 x 3 where k 3 >k> Define the sets M 1 = {x IR 5 : S 1 (x) =} M 2 = {x M1 : S 2 (x) =} Proposition 1.5 The origin of the ENDI is exponentially stable in the set M 2 with the following control law u 2 = u 1 = ky 1 S 1/3 1 if (x 1,y 1 ) S 1/3 2 x 1 k 3 y 2 (k 3 k)kx 2 x M 1 (x ) x 2 y 2 otherwise 35

37 Proof The closed loop dynamics for (x 1,y 1 ) and x/ M 1 becomes ẋ 1 = y 1 ẋ 2 = y 2 ẋ 3 = x 1 y 2 x 2 y 1 ẏ 1 = ky 1 S 1 (x) 1/3 ẏ 2 = x 2 y 2 The dynamics of S 1 (x) is Ṡ 1 = S 1/3 1 and the surface S 1 (x) = is finite-time stable 36

38 So there exists a time T 1 such that the closed loop trajectory for any permissible initial condition reaches the set M 1 and stays there for all future time. The control law u 1 allows both x 1 and y 1 to converge to zero as time gets large. At the same time as the system is being driven towards M 1, the PD control law u 2 with unity gain makes states x 2 and y 2 converge to zero. 37

39 The dynamics on the set M 1 The closed loop system on M 1 is ẋ 1 = kx 1 ẋ 2 = y 2 ẋ 3 = x 1 y 2 x 2 y 1 ẏ 1 = ky 1 ẏ 2 = S1/3 2 k 3 y 2 (k 3 k)kx 2 x 1 The dynamics of S 2 is Ṡ 2 = S 1/3 2 So there exists a time T 2 T 1 such that the closed loop trajectories reach the set M 2 and stay there for all future time. 38

40 The dynamics on the set M 2 The closed loop system on M 2 ẋ 1 = kx 1 ẋ 2 = y 2 ẋ 3 = x 3 ẏ 1 = ky 1 ẏ 2 = k 3 y 2 (k 3 k)kx 2 All trajectories converge to the origin 39

41 Simulation Initial conditions are x() = 1.5m, y()=4m, θ() = 2.3rad, ω() = 1rad/sec, v() = 1m/sec. Controller parameters are k =.5,k 3 =1. Vehicle parameters are M =1Kg, I =2Kgm 2, L =6cm, R =3cm. 4

42 5 4 intial condition y in meters x in meters Figure 3: Stabilization to the origin 41

43 5 4 x y θ 3 x,y in meters, θ in rad time in sec Figure 4: Position and orientation of the vehicle 42

44 1.5 ω v 1 v in m/sec, ω in rad/sec time in sec Figure 5: Velocities of the vehicle 43

45 .15 τ 1.1 τ 2.5 τ 1,τ 2 in Nm time in sec Figure 6: Input to the motors 44

46 Thank You 45