Dimitrov [2] MPC Dimitrov MPC MPC MPC [14] MPC [7] KKT MPC [14] Rao LDL MPC MCP [5] 2 MPC u k

Size: px

Start display at page:

Download "Dimitrov [2] MPC Dimitrov MPC MPC MPC [14] MPC [7] KKT MPC [14] Rao LDL MPC MCP [5] 2 MPC u k"

せぴあいまいだ
5 years ago
Views:

1 1 2 In model predictive control (MPC), an optimal control problem is solved at each time steps to determine control input. To realize on-line control of MPC, reducing computational time is requisite. In this paper, we apply a semismooth Newton method for MPC with simple bounds. The semismooth Newton method is one of iterative methods and is used to solve complementarity problems and KKT systems of optimization problems. The semismooth Newton method has an advantage over other QP solvers, such as interior point methods, active set methods and so on, that the initial point can be chosen arbitrarily, and this enables hot start. We show that the proposed method is globally convergent. We also show the condition guaranteeing the nonsingularity of the generalized Jacobian at a solution, which is closely related to the quadratic convergence of the algorithm. This is the first result to clarify the reason why constraints on state variables make MPC difficult from the algorithmic perspective. Furthermore, we apply the proposed method to the so-called soft constrained problems, in which the bound constraints for state variables are replaced by quadratic penalty function. Some numerical examples show that the proposed method is practically efficient. 1 (Model Predictive Control MPC) [2][11][14][15] Rao [11] MPC Wang [15] MPC LDL taji@nuem.nagoya-u.ac.jp 2

2 Dimitrov [2] MPC Dimitrov MPC MPC MPC [14] MPC [7] KKT MPC [14] Rao LDL MPC MCP [5] 2 MPC u k

3 1 k+n min 2 xt k+nq f x k+n + 2 (xt t Qx t +u T t Ru t ) t=k s.t. x t+1 = Ax t + Bu t (t = k,, k+n 1) x x t x + (t = k+1,, k+n) u u t u + (t = k,, k+n 1) k N x t R n u t R m t R Q Q f x +, x R n u +, u R m v t R n x t+1 = Ax t + Bu t g t, h t R n w t, z t R m KKT Ru t + B T v t + w t z t = 0 Ax t + Bu t x t+1 = 0 w t 0, u t + u + 0, wt T ( u t + u + ) = 0 z t 0, u t u 0, zt T (u t u ) = 0, (t = k, k + 1,, k + N 2) g t+1 0, x t+1 + x + 0, gt+1( x T t+1 + x + ) = 0 h t+1 0, x t+1 x 0, h T t+1(x t+1 x ) = 0, (t = k + 1,, k + N 1) Q t+1 x t+1 + A T v t+1 v t + g t+1 h t+1 = 0, (t = k + 1,, k + N 2) Q k+n x k+n v k+n 1 + g k+n h k+n = 0 (1a) (1b) (1c) (1d) (1e) (1f) (1g) (1h) (1c)-(1f) Fischer-Burmeister FB [6] φ(a, b) = a + b a 2 + b 2 (2) φ(a, b) = 0 a 0, b 0, ab = 0 q t = (u t, v t, w t, z t, g t+1, h t+1, x t+1 )

4 KKT (1) F (q) = 0 q = (q T k,, qt k+n 1 )T F = (F T k,, F T k+n 1 )T F F t (q t ), (t = k, k + 1,, k + N 1) Ru t +B T v t +w t z t Ax t +Bu t x t+1 φ(w t, u t + u + ) F t (q t )= φ(z t, u t u ) φ(g t+1, x t+1 + x + ) φ(h t+1, x t+1 x ) Q t x t+1 +A T v t+1 v t +g t+1 h t+1 t = k+n 1 (3) Q f x k+n v k+n 1 +g k+n h k+n φ FB FB φ(a, b) (a, b) = (0, 0) (3) 3 2 G : R n R n Rademacher G G D G B [1][3] 3.1 G : R n R n B G(x) x B { } B G(x) = V R n n V = lim G (x i ) x i x,x i D G B G(x) Clarke G(x) G x B G(x) G(x) G (x) FB (2) φ(a, b) = (d(a, b), e(a, b)) T d e {( ) } ( ) 1 ξ d(a, b) 1 η ξ2 + η 2 1 = ( ) e(a, b) 1 a a 2 +b 2 1 b a 2 +b 2 if (a, b) = (0, 0) if (a, b) (0, 0)

5 [8][10] 3.2 G : R n R n V G(x + h) h 0 V h G (x; h) = o( h ) x semismooth V h G (x; h) = O( h 2 ) G strongly semismooth G (x; h) x k x k+1 = x k V 1 k G(x k ) V k B G(k k ) V k G(x k ) FB (φ(a, b)) 2 [4] 3.1 F (q) FB θ(q) = 1 F 2 (q) 2 V F (q) θ(q) = V T F (q) [7] θ(q) step 0 q 0 ρ > 0 p > 2 σ (0, 1 ) k = 0 2 step 1 q k step 2 V k F (q k ) d k V k d k = F (q k ) (4) (4) θ(q k ) T d k ρ d k p d k = θ(q k )

6 step 3 i θ ( q k + dk 2 i ) θ(q k ) + σ 2 i θ(qk ) T d k q k+1 = q k + 2 i d k k k + 1 step 1 W w Wt u, Zt z, Zt u,g g t, G x t,ht h, Ht x ( ( ( ( W w i W u i Zi z Zi u G g i G x i Hi h Hi x ) = φ(w i, u i + u + ) = (i, i) ( d(w i, u i + u + ) e(w i, u i + u + ) ) ( ) = φ(z i, u i u ) = d(z i, u i u ) e(z i, u i u ) ) ( ) = φ(g i, x i + x + ) = d(g i, x i + x + ) e(g i, x i + x + ) ) ( ) = φ(h i, x i x ) = d(h i, x i x ) e(h i, x i x ) ) t, I 0 0 A I 0 A d :=. 0 A I A I B d := diag{b, B,, B}, Q }{{} d := diag{q,, Q, Q }{{} f }, R d := diag{r, R,, R} }{{} N N 1 N x 1 u 1 Ax 0 x 2 X :=., U := u 2., X 0 0 :=. 0 x N u N 1 min X T Q d X + U T R d U s.t. A d X + B d U = X 0 X X X +, U U U +

7 FB KKT F 1 Q d X + A T d V + G H F 2 R d U + Bd T V + W Z F 3 X 0 A d X B d U F (q) = F 4 = Φ(X + X, G) = 0 (5) F 5 Φ(X X, H) F 6 Φ(U + U, W ) Φ(U U, Z) F 7 Φ(X + X, G) φ(x + x t, g t ) Φ(X X, H) Φ(U + U, W ) Φ(U U, Z) F Ṽ Q d 0 A T d I I R d B T d 0 0 I I A d B d Ṽ = G x 0 0 G g H x H h W u W w 0 0 Z u Z z G x G x t Gg, H x, H h, W u, W w, Z u, Z z (5) F (q) = 0 F F V F (q) Ṽ F (q) 3.1 Q Q f R {q k } q θ(q) F (q) = 0 : θ(q ) = 0 [3] Th θ(q ) = 0 F (q ) = 0 θ(q) Ṽ F (q) θ(q) = Ṽ T F (q) (5) θ(q ) = 0 (6) Q d F 1 A T d F 3 + G x F 4 + H x F 5 = 0 R d F 2 B T d F 3 + W u F 6 + Z u F 7 = 0 A d F 1 + B d F 2 = 0 (7a) (7b) (7c)

8 F 1 + G g F 4 = 0 F 1 + H h F 5 = 0 F 2 + W w F 6 = 0 F 2 + Z z F 7 = 0 (7d) (7e) (7f) (7g) FB 0 d(a, b), e(a, b) 2 d(a, b) = 0 e(a, b) = 0 φ(a, b) = 0 (7d) i (G g ) i = d(g i, x i + x + ) = 0 (F 4 ) i = φ(g i, x i + x + ) = 0 (7e) i (H h ) i (F 5 ) i = d(h i, x i x )φ(h i, x i x ) = 0 (F 5 ) i = 0 (7f) (7g) G g H h W w Z z (7c) (7d) (7e) (7f) (7g) F1 T (7a) + F2 T (7a) = F1 T (Q d F 1 A T d F 3 + G x F 4 + H x F 5 ) + F2 T (R d F 2 Bd T F 3 + W u F 6 + Z u F 7 ) ( = F1 T Qd G x (G g ) 1 + H x (H h ) 1) ( F 1 + F2 T Rd W u (W w ) 1 + Z u (Z z ) 1) F 2 = 0 (8) G g W w (8) F 1 = 0 F 2 = 0 (7d) (7e) (7f) (7g) F 4 = F 5 = F 6 = F 7 = 0 (7a) A T d F 3 = 0 A d F 3 = q F (q) = 0 F q BD V B F (q ) 3.1 B F (q) F (q) V F G g = I G x = 0

9 W w = I W u = 0 (6) Ṽ Q d 0 A T d I 0 0 R d Bd T 0 I V = A d B d H x 0 0 H h 0 0 Z u 0 0 Z z α = {i H h i = 0}, ᾱ = {i H h i > 0}, β = {i Z z i = 0}, β = {i Z z i > 0} H h i = 0 Z z i = 0 x i = x u i = u V Q d 0 A T d I α Iᾱ R d Bd T 0 0 I β I β A d B d Hα x Hᾱ x Hᾱ h Zβ u Z ū Z z β β H h ᾱ Zz β Schur complement Q d 0 A T d I α 0 0 Rd Bd T 0 I β A d B d Hα x Zβ u ( Q d ) ii = Q ii + (H h i ) 1 H x i, i ᾱ ( R d ) ii = R ii + (Z z i ) 1 Z u i, i β [ ] Qd 0 0 Rd [ A T d J α 0 Bd T 0 J β ] (9) R

10 3.1 Q Q f [A T d J α ] Ṽ X < X < X + α = φ A d 3.3 Q Q f R X < X < X + F (q) Ṽ 3.1 MPC 4 1 [9][13] l 2-2 min k+n xt k+nq f x k+n + 2 (xt t Qx t +u T t Ru t ) + ρc(x) t=k s.t. x t+1 = Ax t + Bu t (t = k,, k+n 1) u u t u + (t = k,, k+n 1) C(x) x x x + C(x) = 1 2 {max(0, x t x + ) 2 + max(0, x x t ) 2 } k+n 1 t=k

11 ρ 0 ρ = 0 ρ 1 C(x) min X T Q d X + U T R d U + ρc(x) s.t. A d X + B d U = X 0, U U U + KKT Q d X + ρ C(X) + A T d V R d U + Bd T V + W Z ˆF (q) = X 0 A d X B d U = 0 Φ(U + U, W ) Φ(U U, Z) C(X) i C i (X) = max(0, x i x + ) max(0, x x i ) ˆF Q d + 2 C(X) 0 A T d R d Bd T I I A d B d W u 0 W w 0 0 Z u 0 0 Z z 2 C(X) (i, i) 2 C i (X) 1 (x i < x x + < x i ) 2 C i = [0, 1] (x i = x x + ) 0 (x < x i < x + ) (10) (11) C (10) α = φ 4.1 Q Q f R l 2-2

12 5 Intel Core2Quad Q GHz 3.21GB MatlabR2012a ρ = 10 8, p = 2.1, σ = 10 3 θ(q) < 10 7 LDL (4) LDL [15] (4) 9(m + 2n) 3 N Rao (4) Rao [11] (15n 3 + 7mn 2 + 3m 2 n + 9m 3 )N LDL MCP MCP [5] (4) LDL (IP) (AS) Matlab quadprog MCP MCP FB (2) x x x + [5] ψ(x, y) = φ(x x, φ(x + x, y)) (12) FB ψ x x x + ψ(x, y) = 0 (x x )y 0 (x x + )y 0 (12) (3) 3 4 φ(w t, u t + u + ) = 0, φ(z t, u t u ) = 0 ψ(u t, z t ) = 0

13 x x 2,u 2 1,u 1 mass1 mass2 1: Two carts positioning system ψ FB LDL, Rao, MCP k q (k t) k + 1 q0 (k+1 t) q(k+1 1) 0 q(k+1 2) 0. q 0 (k+1 N 1) q 0 (k+1 N) = q(k 2) q(k 3). q (k N) q (k N) (13) 1 2 [12] ẋ = A c x + B c u A c = , B c =

14 0.02 Q R [ Q= , R= Q f ] = Q x 0 = [ 0.1, 0.25, 0, 0] T x f = [0, 0, 0, 0] T u u x x 2 x x 4 (14) 1 N #iter. total max 1 CPU time total ave. 1 max MPC N = MCP, LDL, Rao MCP LDL, Rao, MCP (4) 2(a) 2(b) 2(c) N = 40 LDL 2(a) 2(b) 2(c) 2(d) N = 40 LDL 2(a), 2(c) 2 (100 ) 1

15 1: Numerical results for two cars system N #iter. CPU time [s] total max total ave. max LDL Rao MCP IP AS

16 (13) 4 6 Matlab quadprog (13) (a) Control inputs (b) Position (c) Velocity (d) The number of iterations (N = 40) 2: The reseuts for two cars system x + = 0.05 x = 0.05 N = 40 LDL 2 ρ x 4 3(a) Hard (14) 2(c) 3(a) N = 40 ρ = 1 3(b)

17 2: Numerical results of two cars sysytem (soft constraits) iteration time[s] N ρ total max total ave. max (a) Trajectoris of x 4 (b) The number of iterations 3: The results of two cars system (soft constraints)

4 ẋ = A c x + B c u 0 0 1 0 0 0 0 1 A c = 2(K 0 f +K r) 2(K f +K r) 2(l f K f +l rk r) m mv mv B c = 0 2(l f K f l r K r ) 2(l f K f l r K r ) 2(l2 f K f +lrk 2 r) I IV IV 0 0 0 0 2K f m 2l f K f I

18 4 ẋ = A c x + B c u A c = 2(K 0 f +K r) 2(K f +K r) 2(l f K f +l rk r) m mv mv B c = 0 2(l f K f l r K r ) 2(l f K f l r K r ) 2(l2 f K f +lrk 2 r) I IV IV K f m 2l f K f I 2K r m 2l rk r I δ f δ r x = [y, θ, ẏ, θ] T y θ [ Q = , Q f = , R = N = (a) 5(b) 5(c) ρ = 0, 1, 10 y ±0.1 ρ = 0 ρ = 1 ρ = 10 ρ = 10 4 ρ ] 4: Vehicle Model

19 n o i t i s o p step (a) ρ = n o i t i s o p step (b) ρ = n o i t i s o p step (c) ρ = 10 5: The results of a tracking problem

20 3: Physical parameters of a vehicle model ( ) m 1400(kg) I 2500(Nm) ( ) l f 1.02(m) ( ) l r 1.58(m) ( ) K f 40000(N) ( ) K r 40000(N) V 10(km/h) 4: Numerical results for a tracking problem ρ max time total time ave. time max iter. total iter [1] F.H. Clarke, Optimization and Nonsmooth Analysis, SIAM, Philadelphia, PA, 1990 [2] D. Dimitrov, A. Sherikov and P. Wieber: A sparse model predictive control formulation for walking generation, Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2292/2299 (2011) [3] F. Facchinei and J.S. Pang: Finite-Dimensional Variational Inequality and Complementarity Problems I and II, Springer-Verlag, New York, 2003 [4] F. Facchinei and J. Soares: A new merit function for nonlinear complementarity problems and a related algorithm, SIAM Journal on Optimization, 7, 225/247 (1997) [5] M.C. Ferris, C. Kanzow and T.S. Munson: Feasible descent algorithms for mixed complementarity problems, Mathematical Programming, 86, 475/497 (1999)

21 [6] A. Fischer: A special Newton-type optimization, Optimization, 24, 269/284 (1992) [7] T. De Luca, F. Facchinei and C. Kanzow: A semismooth equation approach to the solution of nonlinear complementarity problems, Mathematical Programming, 75, 407/439 (1996) [8] R. Mifflin: Semismooth and semiconvex functions in constrained optimization, SIAM Journal on Control and Optimization, 15, 957/972 (1977) [9] N.M.C. Oliveira and L.T. Biegler: Constraint Handling and Stability Properties of Model-Predictive Control, AIChE Journal, 40(7), 1138/1155 (1994) [10] L. Qi and J. Sun: A nonsmooth version of Newton s method, Mathematical Programming 58, 353/367 (1993) [11] C. Rao, S. Wright and J. Rawlings: Application of interior-point methods to model predictive control, JOTA, 99(3), 723/757 (1998) [12] M-, 40(9) 906/914 (2004) [13] P.O.M. Scokaert and J.B. Rawlings: Feasibility Issues in Linear Model Predictive Control, AIChE Journal, 45(8), 1649/1659 (1999) [14] /506 (2005) [15] Y. Wang and S. Boyd: Fast model predicitve control using online optimization, IEEE Transaction on Control Systems Technology, 18(2), 267/278 (2010)

1 [1, 2, 3, 4, 5, 8, 9, 10, 12, 15] The Boston Public Schools system, BPS (Deferred Acceptance system, DA) (Top Trading Cycles system, TTC) cf. [13] [

Vol.2, No.x, April 2015, pp.xx-xx ISSN xxxx-xxxx 2015 4 30 2015 5 25 253-8550 1100 Tel 0467-53-2111( ) Fax 0467-54-3734 http://www.bunkyo.ac.jp/faculty/business/ 1 [1, 2, 3, 4, 5, 8, 9, 10, 12, 15] The