Decentralized Formation Control including Collision Avoidance

Transcription

1 Decentralized Formation Control including Collision Avoidance Nopthawat Kitudomrat Fujita Lab Dept. of Mechanical and Control Engineering Tokyo Institute of Technology FL Seminar 01/10/ / 24

2 Today s Topics Introduction Problem Setting Set Invariance Theory Reference Governor Conclusion Future work Reference 2 / 24

3 Introduction Decentralized Control Group of dynamically decoupled subsystem Perform a cooperative task such as flocking and formation control Recognize only neighbors Set Invariance Theory Steer a system to target without violating constraints Reference Governor Feasibility on stable and constrained system Need to solve optimization problem 3 / 24

4 Problem Setting (1) Consider N v decoupled dynamical system Every agent has the same model Discrete-time time-invariant state equation x i k+1 = Axi k + Bui k (1) y i k = Cxi k = [ I 0 ] x i k = pi k, (2) where k Z + := {0, 1, 2, }, i {1,, N v }, u i k Rnp is the control input, x i k = [pi k v i k xc ] R n (n = n p + n v + n c ) is the state vector composed of the position, velocity and state of controller. 4 / 24

5 Problem Setting (2) Assumption 1 a desired tracking performance is achieved in the absence of the constraints with C(I A) 1 B = I. Assumption 2 The state x i k of the system (1) can be measured exactly. 5 / 24

6 Problem Setting (3) The interconnection of the above systems is described by G k = {V, A k } { 1, if (i, j) Ak A k (i, j) = 0, otherwise A k = {(i, j) y i k yj k d s } (3a) (3b) (3c) Agent tries to avoid collision when y i k yj k d Note 1 d is collision avoidance range d s is connection range d d s 6 / 24

7 Notations x k = (x 1 k xnv k ) x i (t; x i 0, ri ), t Z + is the evolution of the state from the initial state x i 0 which ri is a step reference. x(t; x 0, r) = (x 1 (t; x 1 0, r1 ),, x Nv (t; x Nv 0, rnv )) r = [r 1 r Nv ] R = {r r i r j d + ɛ}, ɛ > 0. is a reference set. 7 / 24

8 Maximal output admissible set Definition 1 The maximal output admissible set S x is defined by S i x = {(x i 0, r i ) x i (t; x i 0, r i ) X, r i U, t} (4) S x = {(x 0, r) r R, (x i 0, r i ) S i x, i} (5) 8 / 24

9 Collision Free set and Collision set Definition 2 The collision free set Y is defined by Definition 3 Y = {x (x i, x j ) Y i,j i, j (i j)} (6) Y i,j = {(x i, x j ) y i y j > d} (7) The collision set Ỹ is defined by Ỹ = {x i and j(i j) s.t. (x i, x j ) Ỹi,j } (8) Ỹ i,j = {(x i, x j ) X y i y j d} (9) 9 / 24

10 Safe Region Definition 4 The safe region S is defined by S = {(x 0, r) S x (x i 0, x j 0, ri, r j ) S i,j S i,j = {(x i 0, x j 0, ri, r j ) (x i (t; x i 0, r i ), x j (t; x j 0, rj )) Y i,j t Z + }. i, j(i j)}, 10 / 24

11 Predictive Collision Set Definition 5 The predictive collision set is defined by Õ i,j 0 = {(x i, x j, r i, r j ) (x i, x j ) Ỹi,j, (x i, r i ) S i x, (x j, r j ) S j x} Õ i,j k+1 = {(xi, x j, r i, r j ) (x i, x j, r i, r j ) Õi,j k, (x i, r i ) Sx, i (x j, r j ) Sx} j Õ i,j = k=0 Õ i,j k (10) 11 / 24

12 Safe Region Calculation Procedure The algorithm for calculating the safe region S is described as: 1 Start with k = 0, and compute the initial predictive collision set Õi,j 0. 2 Find Õi,j k+1 which satisfies the constraint Ax + Br Õi,j k. 3 Check if Õi,j k+1 Õi,j k Yes : Go to step 4. No : Increase a time step k by 1 and go to step 1. 4 Compute the safe region S by using the following equations Õ i,j = p {0,...,k} S i,j = S x \ Õi,j Õ i,j p 12 / 24

13 Decision Making Algorithm The algorithm can be described in details as follow: 1 Compute the adjacency matrix A k according to (3c) 2 Select the agent and consider its link structure if there are any neighbors or not : Yes : Check the existence of a neighbor which does not connect to other neighbors. Yes : The agent tries to stop and wait for a next operation. (condition 1) No : Calculate the direction of movement by sum of the vectors from neighbors to the considered agent and rotate it 90 degrees. Then, we compute the input according to the algorithm shown in the next section. (condition 2) No : Apply the controller that is described in the next section. (condition 3) 3 If the agent and all of its neighbors stop, then, at the next sampling time, the agent sends the information of direction of movement to all the neighbors and moves. 13 / 24

14 Examples 1 STOP All Stop 14 / 24

15 Reference governor (RG) Properties of RG Deal with tracking control problem Modify input to satisfy the state constraints Consist of optimization problem r k i RG u k i Stable Closed-loop system Controller Plant y k i x k i 15 / 24

16 Input Computation We introduce a constant Kk i [0, 1] such that u i k = Ki k ri k + (1 Ki k )ûi k, where Ki k is computed at each time step and û i k is an input calculated from decision making algorithm. Find the optimized value K which satisfies the collision avoidance (x i k, xj k, ui k (Ki k ), ui,j k (Kj k )) Si,j j s.t. (i, j) A k. (11) where u i k (K) = Kri k + (1 K)ûi k, u i,j k (K) = Kˆri,j k + (1 K)ûj k, and ˆri,j k from the agent i. r j k is the estimate value of 16 / 24

17 Input Computation (2) The input rk i and ûi k algorithm. can be acquired by using the decision making Condition r i k û i k 1 p i k p i k 2 direction (magnitude??) p i k 3 terminal point u i k 1 17 / 24

18 Input Computation (3) The agent does not know the next state behavior (Move, Stop) of the neighbor so both cases are considered (x i k, ui k (Ki k )) Si x (12a) (x j k, ui,j k (Kj k )) Sj x j s.t. (i, j) A k (12b) (Ax i k + Bui k (Ki k ), CAxi k + CBui k (Ki k )) Si x (12c) (Ax j k + Bui,j k (Kj k ), CAxj k + CBui,j k (Kj k )) Sj x j s.t. (i, j) A k (12d) (x i k, xj k, ui k (Ki k ), ûj k ) Si,j j s.t. (i, j) A k (x i k, xj k, ûi k, ui,j k (Kj k )) Si,j j s.t. (i, j) A k (12e) (12f) 18 / 24

19 Input Computation (4) The optimization problem can be formulated as follow : Kk i + max K i k,kj k (i,j) A k K j k s.t. : Kk i, Kj k [0, 1], constraints(11) and (12) In condition 3, we solve the following problem. max K i k,kj k s.t. K i k : Kk i, Kj k [0, 1], constraints(11) and (12a), (12c) 19 / 24

20 Conclusion We have proposed an algorithm to calculate a reference input for the stable closed-loop system. Set region which satisfies collision avoidance constraints is realized by set invariance theorem. We applied the reference governor controller to achieve the tracking performance without violating constraints. 20 / 24

21 Future Works Consider the minimum range of sensor (d s ) Calculate the magnitude of input r in condition 2 to guarantee stability. Simplify the calculation of K i k, Kj k 21 / 24

22 References S.V. Rakovic and D.Q. Mayne, Robust model predictive control for obstacle avoidance: discrete time case, Proceedings of the International Workshop on Assessment and Future Directions of Nonlinear Model Predictive Control NMPC05, Freudenstadt - Lauterbad, Germany, August 2005, pp S.V. Rakovic and D.Q. Mayne, Robust Time Optimal Obstacle Avoidance Problem for Constrained Discrete Time Systems, Proceedings of the 44th IEEE Conference on Decision and Control, and the European Control Conference, December 2005, pp F. Blanchini, Set invariance in control, Automatica, Vol. 35, No. 11, November 1999, pp / 24

23 References (2) E.C. Kerrigan, Robust Constraint Satisfaction: Invariant Sets and Predictive Control, Ph.D. dissertation, Control Group, Department of Engineering, University of Cambridge, England, November K. Hirata and M. Fujita, Set of Admissible Reference Signals and Control of Systems with State and Control Constraints, Proceedings of the 38th IEEE Conference on Decision and Control, Phoenix, Arizona, USA, December 1999, pp A. Bemporad, A. Casavola and E. Mosca, A predictive reference governor for constrained control systems, Computers in Industry, Vol.36, Issue 1-2, April 1998, pp / 24

24 Appendix Pic 24 / 24