A moving switching sequence approach for nonlinear model predictive control of switched systems with state‐dependent switches and state jumps

In this paper, we propose an approach for real‐time implementation of nonlinear model predictive control (NMPC) for switched systems with state‐dependent switches called the moving switching sequence approach. In this approach, the switching sequence on the horizon moves to the present time at each time as well as the optimal state trajectory and the optimal control input on the horizon. We assume that the switching sequence is basically invariant until the first predicted switching time reaches the current time or a new switch enters the horizon. This assumption is reasonable in NMPC for systems with state‐dependent switches and reduces computational cost significantly compared with the direct optimization of the switching sequence all over the horizon. We update the switching sequence by checking whether an additional switch occurs or not at the last interval of the present switching sequence and whether the actual switch occurs or not between the current time and the next sampling time. We propose an algorithm consisting of two parts: (1) the local optimization of the control input and switching instants by solving the two‐point boundary‐value problem for the whole horizon under a given switching sequence and (2) the detection of an additional switch and the reconstruction of the solution taking into account the additional switch. We demonstrate the effectiveness of the proposed method through numerical simulations of a compass‐like biped walking robot, which contains state‐dependent switches and state jumps.


Summary
In this paper, we propose an approach for real-time implementation of nonlinear model predictive control (NMPC) for switched systems with state-dependent switches called the moving switching sequence approach. In this approach, the switching sequence on the horizon moves to the present time at each time as well as the optimal state trajectory and the optimal control input on the horizon. We assume that the switching sequence is basically invariant until the first predicted switching time reaches the current time or a new switch enters the horizon. This assumption is reasonable in NMPC for systems with state-dependent switches and reduces computational cost significantly compared with the direct optimization of the switching sequence all over the horizon. We update the switching sequence by checking whether an additional switch occurs or not at the last interval of the present switching sequence and whether the actual switch occurs or not between the current time and the next sampling time. We propose an algorithm consisting of two parts: (1) the local optimization of the control input and switching instants by solving the two-point boundary-value problem for the whole horizon under a given switching sequence and (2) the detection of an additional switch and the reconstruction of the solution taking into account the additional switch. We demonstrate the effectiveness of the proposed method through numerical simulations of a compass-like biped walking robot, which contains state-dependent switches and state jumps. nally forced switches. For example, we can see state-dependent switches in contacts of mechanical systems with objects or environments [2][3][4][5] and logical switching controllers [6][7][8] and state-independent switches in gear shifts of automobiles, 9 electrical circuit systems, 10,11 and process systems. 12,13 In this paper, we focus on control of switched systems with state-dependent switches.
It is difficult to apply conventional control methods directly to switched systems with state-dependent switches, particularly when they contain nonlinear subsystems. Nonlinear model predictive control (NMPC) [14][15][16] is the only generic method that can control such systems in practice. In NMPC, an open-loop optimal control problem over a finite future is solved at each time on the basis of the current state and the system's model, and the actual control input to the system is given by the initial value of the optimal control input. Even if the system contains discrete events such as switches or state jumps, we can realize NMPC in consideration of future discrete events as long as we can solve the finite horizon nonlinear optimal control problem (NOCP) within a given sampling period. However, it is difficult to solve the finite horizon NOCP for the switched systems with state-dependent switches in a short computational time because the control input and sequence of the active subsystems complicatedly depend on each other. 17 In previous studies, optimal control of systems with state-independent switches and that of systems with state-dependent switches are often considered in the same framework, and many different optimization approaches have been studied. Mixed-integer programming (MIP) is a classical method for optimization problem with a kind of switches and numerical methods based on MIP are successfully applied to systems with long sampling period, eg, process systems. 18,19 Two-stage methods [20][21][22] define two independent optimal control problems (one for the sequence of active subsystems over the horizon, and the other for control input and switching instants) and solve them alternately by conventional numerical methods for optimization. The algebraic geometry method 23,24 applies cylindrical algebraic decomposition to the NOCP for polynomial switched systems. The embedded method 25,26 converts the optimal control problem for switched systems into a conventional continuous optimal control problem for a larger set of systems by introducing variables corresponding to mode activation. Although these methods successfully optimize the sequence of active subsystems and control input simultaneously, it is difficult to realize NMPC by using these methods when the switched system contains nonlinear subsystems whose state equations are complex, and the sampling period is short because these methods still take long computation time. These studies require seeking the switching sequence, sequence of active subsystems, all over the horizon of NOCP, which increases the computational time significantly compared with the optimal control problem for systems without switches. On the other hand, the minimum-principle for the switched or hybrid systems has been studied for a long time, 27,28 and efficient numerical methods combining it with conventional optimization algorithms, such as dynamic programming or gradient-descent methods under a fixed sequence of the active subsystems, have been developed. 17,[29][30][31][32][33][34][35] However, these methods increase the computational cost a lot compared with the optimal control problem or NMPC for a system without any switches. These methods have to find an optimal (or at least a feasible) sequence of active subsystems on the horizon. For this purpose, they have to solve the optimal control problems for all possible sequence of active subsystems, which obviously takes much more computational time than solving the optimal control problem for only one sequence of active subsystems.
In this paper, we propose an approach for real-time implementation of NMPC for switched systems with state-dependent switches called the moving switching sequence approach, in which the switching sequence on the horizon moves to the present time at each time as well as the optimal state trajectory and the optimal control input on the horizon. We assume that the switching sequence is basically invariant until the first predicted switching time reaches the current time or a new switch enters the horizon. This assumption is reasonable in NMPC for systems with state-dependent switches and reduces computational cost significantly compared with directly optimizing the switching sequence all over the horizon. In the proposed method, we update the switching sequence by checking whether an additional switch occurs or not at the last interval of the present switching sequence and whether the actual switch will occur or not between the current sampling time and the next sampling time. We check the former by computing the state trajectory on the horizon on the basis of the current solution and the current switching sequence. If we detect an additional switch on the state trajectory, we reconstruct the solution of NMPC in consideration of the additional switch. We check the latter by evaluating the switching instants after the solution is updated. If the instant of the first switch in the switching sequence after the solution is updated becomes less than the next sampling time, we predict that the actual switch occurs between the current sampling time and the next sampling time and then remove variables related to the switch from the solution. We propose an algorithm of the moving switching sequence approach consisting of two parts: (1) the local optimization of the control input and switching instants by solving the two-point boundary-value problem (TPBVP) for the whole horizon under a given switching sequence and (2) the detection of an additional switch and the reconstruction of the solution by solving a reduced optimal control problem defined only for additional variables with respect to an additional switch.
The former takes almost the same computational time as the NOCP for systems without switches, and the latter can be achieved at sufficiently small computational burden. Therefore, the computational time of the proposed method increases only slightly from that of NMPC for a system without switches.
In solving the TPBVP, we can apply conventional numerical methods for implementing NMPC in real time. [36][37][38][39][40][41][42] We adopt the continuation/generalized minimum residual (C/GMRES) method, 37 which achieves fast computation by tracking the solution at each time without approximation for state equations and cost functions as long as the sampling period is set sufficiently short. By utilizing the C/GMRES method for the TPBVP containing switches, we update the control input and switching instants simultaneously at each sampling time in a short computational time. In reconstructing the solution for an additional switch, we obtain variables with respect to the switch by solving the reduced optimal control problem only for variables with respect to the additional active subsystem using conventional numerical methods, eg, Newton's method. Note that there are studies using the C/GMRES method that implicitly make the same assumption about the invariance of switching sequence for NMPC for a class of switched systems with state-dependent switches. [43][44][45] For example, Yamakita et al extended the C/GMRES method to mechanical systems with collisions by handling the optimality conditions approximately. 43,44 However, that approximation generates errors from the optimal control, and an undesirable oscillation of the control input is observed in their simulations. They introduce an integrator to avoid that oscillation, which does not guarantee the optimality of the solution. To treat the optimality conditions more appropriately with the C/GMRES method, we proposed a penalty function method. 45 However, this method can make numerical computation unstable due to the penalty function. These approximations are introduced to avoid directly treating changes in the number of optimization variables caused by the addition of a new switch. In contrast, the moving switching sequence approach does not introduce such approximations because this method obtains the additional variables efficiently when the number of switches on the horizon changes.
The rest of this paper is composed as follows. In Section 2, we describe a class of switched systems with state-dependent switches. In Section 3, we derive the optimality conditions and define the TPBVP for that system. In Section 4, we describe the C/GMRES method for TPBVP formulated in Section 3, detection of an additional switch and reconstruction of the solution for the additional switch, and the algorithm of the moving switching sequence approach. In Section 5, we present a numerical simulation of a compass-like robot walking for two cases, ie, a nominal case without any disturbances and a case with impulse disturbances, using the proposed method and demonstrate the method's effectiveness. In Section 6, we conclude our paper.

SWITCHED SYSTEMS WITH STATE-DEPENDENT SWITCHES
We consider a switched system consisting of subsystems .
where x(t) ∈ ℝ n denotes the state vector, u(t) ∈ ℝ m denotes the control input vector, q ⊆ ℝ n denotes the domain of f q (x, u) for x, and M denotes a positive integer. We represent that subsystem q(t) is active or active subsystem is q(t) if the state is governed by . We also consider the switching sets Ψ q 1 ,q 2 ⊆ q 1 that represent the condition of state-dependent switches This means that, if subsystem q 1 is active and x ∈ q 1 reaches Ψ q 1 ,q 2 , that is, if x ∈ q 1 satisfies q 1 ,q 2 (x) = 0, then the active subsystem switches to q 2 . We also consider the state changes discontinuously, ie, a state jump occurs at the same time as the switch, as where x − ∈ Ψ q 1 ,q 2 denotes the state just before the state jump, and x + ∈ ℝ n denotes the state just after the state jump.
Note that, if x satisfies q,q (x) = 0 for some q ∈ Q, just the state jump x + = q,q (x − ) occurs without a switch of the active subsystems. We make the following assumptions about f q (x, u), q 1 ,q 2 (x), and q 1 ,q 2 (x).
Assumption 1. f q (x, u) are continuously differentiable for all x ∈ q , for all u ∈ ℝ m , and for all q ∈ Q, and q 1 ,q 2 (x) and q 1 ,q 2 (x) are also continuously differentiable for all x ∈ ℝ n and for all q 1 , q 2 ∈ Q.
Assumption 2. q 1 ,q 2 (x) is uniquely and explicitly determined for all x ∈ q 1 and for all q 1 , q 2 ∈ Q.
Note that Assumption 4 implies that the length of a time interval in which any subsystem is active is not zero, ie, state jumps do not occur in a row. That is, situations that are too complicated such as infinite loops of state jumps are not considered.

TWO-POINT BOUNDARY-VALUE PROBLEMS
In NMPC, the optimality conditions, which are the necessary conditions of the optimal control, are formulated into a TPBVP and solved numerically to obtain the optimal control input at each time. The optimal control problem for switched systems with state-dependent switches and state jumps is defined as follows: find u(t ′ ) (t ≤ t ′ ≤ t + T) minimizing the cost function subject to (1)-(3), where q (x) ∈ ℝ denotes the terminal cost for subsystem q and L q (x) ∈ ℝ denotes the stage cost for subsystem q. To derive the optimality conditions, we introduce a sequence of subsystems for t ∈ [t, t+T], a switching sequence defined as where q k ∈ Q for all k = 1, … , m, and introduce switching instants t k ∈ ℝ for k = 1, … , m − 1 representing the instants of the switch from subsystem q k to subsystem q k+1 , where 0 ≤ m < ∞, t ≤ t 1 < · · · < t m−1 ≤ t + T, and q 1 , … , q m ∈ Q. Note that t i ≠ t i+1 because of Assumption 4. This denotes that subsystem q 1 is active at [t, t 1 ), subsystem q k is active at (t k−1 , t k ) for k = 2, … , m − 1, and subsystem q m is active at (t m−1 , t + T]. Figure 1 illustrates the switching sequence and a state trajectory on the horizon. We make the following assumption about the solution of the NOCP for systems with state-dependent switches.
Assumption 5. The optimal control input u and switching sequence of a finite length exist.
Suppose that the switching sequence on the horizon t ′ ∈ [t, t + T] is given as = (q 1 , q 2 , … , q m ), where q 1 , … , q m ∈ Q and the instants of the switch from q k to q k+1 are given as t k for all k = 1, … , m − 1. The state trajectory is then given as

FIGURE 1
The switching sequence = (q 1 , q 2 , … , q m ) and a state trajectory on the horizon The optimal control problem is then defined as finding the optimal control input u(t ′ ) (t ≤ t ′ ≤ t + T) minimizing the cost function subject to (6)- (10).
For numerical computation, we discretize the optimal control problem. We divide the horizon t ′ ∈ [t, t + T] into N steps, define the time step Δ ∶= T∕N, and introduce i k as an integer satisfying Figure 2 shows an example of the discretization over the horizon, in which i k The optimal control problem is then given as follows: find the optimal control input sequence

FIGURE 2
An example of the discretization of x and u over the horizon subject to x To derive the optimality conditions, we define the augmented cost function, 46 which is obtained by adjointing equality constraints (14)- (20) to the cost function (12). We introduce the Lagrange multipliers for the state equations in (14), (15), and (18) We also introduce the Lagrange multipliers for the switching conditions q 1 ,q 2 The augmented cost functionJ is then given as We further introduce the Hamiltonian of subsystem q ∈ Q We then obtain the optimality conditions by calculus of variations 46 * The unknown quantities of the optimal control problem are (29). Thus, we define variables to be determined as satisfying where for k = 1, … , m − 1. Note that, if = (q), ie, if only one subsystem q is active over the whole horizon, the optimal control problem is just given as the conventional one for subsystem q.

THE MOVING SWITCHING SEQUENCE APPROACH
The moving switching sequence approach considers the recession of the switching sequence on the horizon as well as the optimal state trajectory and the optimal control input on the horizon. To justify this approach, we make the following assumption.
Assumption 6. Let = (q 1 , … , q m ) be the switching sequence on the horizon at the sampling time t. The switching sequence at the next sampling time t + Δt is given as one of the following: = (q 1 , … , q m ), = (q 1 , … , q m , q m+1 ) for some q m+1 ∈ Q, = (q 2 , … , q m ), or = (q 2 , … , q m , q m+1 ). This assumption limits the change in the switching sequence to those only by the exit of the first switch of the switching sequence because of an actual switch occurrence or the addition of a new switch to the end of the switching sequence. This is a reasonable assumption for systems with state-dependent switches as long as the actual state changes continuously. If the state changes continuously, ie, the actual state changes slightly at each time in a sufficiently short sampling period, the optimal state trajectory also changes slightly from that of the previous sampling time. This assumption will then hold as long as the distance between each switching condition is sufficiently far.
Under this assumption, the switching sequence = (q 1 , … , q m ) moves to the present time at each time without changing from the previous time to the current time and an additional switch may occur at t ′ ∈ (t m , t + T], where the last subsystem of is active. We update the switching sequence by checking whether an additional switch occurs at t ′ ∈ (t m , t + T] at each time and whether the actual switch occurs or not between the current sampling time and the next sampling time. We check the former by computing the state trajectory on the horizon on the basis of the current solution and the current switching sequence. If we detect an additional switch on the state trajectory, we reconstruct the solution taking the switch into account. We check the latter after the solution is updated by evaluating the updated switching instants. If the instant of the first switch of the switching sequence after the solution is updated is less than the next sampling time, we predict the actual switch between the current sampling time and the next sampling time and then remove variables related to the switch from the solution. We propose an algorithm of the moving switching sequence approach composed of two main parts: (1) the optimization of the control input and switching instants under the switching sequence obtained at the previous sampling time by solving the TPBVP formulated in Section 3 and (2) the reconstruction of the solution by solving the reduced optimal control problem for the additional switch. For (1), we utilize the C/GMRES method, and for (2), we define and solve the optimal control problem just for variables with respect to the additional switch and additional active subsystem and reconstruct the solution.
Possible changes of the switching sequence from t to t + Δt under Assumption 6. A, The switching sequence dows not change; B, An additional switch from subsystem q m to subsystem q m+1 is detected in the last interval of the switching sequence; C, An actual switching from subsystem q 1 to subsystem q 2 occurs between t and t + t; D, An actual switch from subsystem q 1 to subsystem q 2 occurs between t and t + t, and an additional switch from subsystem q m to subsystem q m+1 is detected in the last interval of the switching sequence In Section 4.1, we briefly introduce the C/GMRES method for the TPBVP and the method to update the solution in the C/GMRES method in accordance with the switches. In Section 4.2, we describe the detection of an additional switch and optimization of variables related to an additional switch. In Section 4.3, we summarize the algorithm of the moving switching sequence algorithm.

The C/GMRES method
To realize NMPC for systems with state-dependent switches, we have to compute the unknown quantities to be determined, U(t), which is defined by (36) and satisfies (37), at each sampling time within a given sampling period. The C/GMRES method 37 is a fast algorithm of NMPC composed of the continuation method 47 and the GMRES method. 48 In the C/GMRES method, we do not compute U(t) directly but update the solution by integrating . U(t) at each time, eg, as where Δt is the sampling period, and the nonlinear

equation F(U(t), x(t), t) = 0 for U(t) is transformed into a linear equation for
.

U(t) by the continuation method as
where > 0 is a stabilization parameter. We solve this equation by using the forward-difference GMRES method (FD-GMRES method), 48 which solves linear equations fast. In the FD-GMRES method, forward-difference approximation is used to approximate the products of Jacobians and some W ∈ ℝ n ′ , w ∈ ℝ n , ∈ ℝ as where h > 0 is a time increment. By utilizing the FD-GMRES method for the TPBVP formulated in Section 3, we obtain not only the time derivatives of the optimal control input sequence but also the time derivatives of the optimal switching instants and the corresponding Lagrange multipliers with respect to the switching conditions. Integration of . (39) is based on the continuity of U(t) between t and t + Δt, and we have to modify the numerical integration method when there are switches and state jumps because that continuity does not hold when there are switches or state jumps. Let = (q 1 , … , q m ) be the switching sequence for the NOCP at time t, let t k (t) be the optimal switching instants from subsystem q k to subsystem q k+1 at time t for k = 1, … , m − 1, and let i k (t) be an integer satisfying i k (t)Δ ≤ t k (t) − t < (i k (t) + 1)Δ . Note that subsystem q k is active at t ′ ∈ (t k (t), t k+1 (t)) in the NOCP at time t, and then,

U(t) taking discontinuity of the control input into account The integration
, and u * i k+1 (t) (t) are the optimal control input on the interval where subsystem q k is active. Figure 4 illustrates the update of the control input taking the discontinuity of the control input into account. Black circles represent u * i (t) and u * i (t + Δt) for i = i k (t), … , i k+1 (t) + 1, and white circles represent u * (t + k (t)), u * (t + k+1 (t)), u * (t + k (t + Δt)), and u * (t + k+1 (t + Δt)). We then have to determine the control input for subsystem q k at time t + Δt on the basis of control inputs and their time derivatives for subsystem q k at time t, ie, we have to determine u * (t + k (t + Δt)), u * i k (t+Δt)+1 (t + Δt), … , u * i k+1 (t+Δt)−1 (t + Δt), and u * i k+1 (t+Δt) (t + Δt) on the basis of u * (t + k (t)), u * i k (t)+1 (t), … , u * i k+1 (t) (t), However, when i k (t) ≠ i k (t + Δt), it is not suitable to determine u * i (t + Δt) by (39) for i such that i k (t + Δt) < i ≤ i k (t) or i k (t) < i ≤ i k (t + Δt) because (39) does not consider the difference between the active subsystem at iΔ at the sampling time t and that of iΔ at the sampling time t + Δt. For example, in Figure 4, ie, when i k (t) = i k (t + Δt) + 1 and i k+1 (t) = i k+1 (t + Δt) − 1, it is not suitable to determine u * i k (t) (t + Δt) by (39) because control input u * i k (t) (t) and its time derivative . u * i k (t) (t) are for subsystem q k−1 whereas u * i k (t+Δt) (t + Δt) is for subsystem q k . Additionally, it is also not suitable to determine u * i k+1 (t) (t + Δt) by (39) because control input u * i k+1 (t)+1 (t) and its time derivative . u * i k+1 (t)+1 (t) are for q k+1 whereas u * i k+1 (t+Δt)+1 (t + Δt) is for q k . The update (39) when i k (t) ≠ i k (t + Δt) is modified as follows. We first compute u * (t + k (t + Δt)) for k = 1, … , m − 1 by and the switching instants t k (t + Δt) for k = 1, … , m − 1 by Next, we compute u * i (t + Δt) for i satisfying i k (t) < i ≤ i k+1 (t) and i k (t + Δt) < i ≤ i k+1 (t + Δt) by (39), ie, by Thin arrows in Figure 4 represent updates by (42) and (44). We update u * i (t + Δt) for i such that i k (t + Δt) < i ≤ i k (t) or i k (t) < i ≤ i k (t + Δt) using the control input for q k at the sampling time t + Δt and its time derivative. We regard u * i (t + Δt) for i such that i k (t + Δt) < i ≤ i k (t) as the control input obtained by integrating Additionally, we regard u * i (t + Δt) for i such that i k (t) < i ≤ i k (t + Δt) as the control input obtained by integrating Thick arrows in Figure 4 represent the integration by (45) and (46). Note that the Lagrange multipliers for the switching conditions * q 1 ,q 2 (t + Δt), … , * q m−1 ,q m (t + Δt) are just updated by (39), ie, as * q k ,q k+1 (t + Δt) = * q k ,q k+1 (t) +

U(t). Prediction of the actual switch
Suppose that the switching sequence is given as = (q 1 , q 2 , … , q m ). If the switching instant t 1 (t + Δt) in U(t + Δt) after the update of U(t) by the C/GMRES method satisfies t 1 (t + Δt) < t + Δt, we predict that the switch from subsystem q 1 to q 2 occurs on the actual system between t and t + Δt. We then remove q 1 from and U q 1 ,q 2 (t + Δt) from U(t + Δt). Note that u * 0 (t + Δt) is already updated for subsystem q 2 by (45) in this case because i 1 (t + Δt) < 0 ≤ i 1 (t) holds from the fact that t 1 (t + Δt) < t + Δt.

Detection of an additional switch and reconstruction of the solution
Suppose that the switching sequence is given as = (q 1 , q 2 , … , q m ). In solving the TPBVP, the state trajectory on the horizon is computed on the basis of x(t), , and U(t), and we then check whether an additional switch occurs or not for t m−1 < t ′ ≤ t + T. In the discretized problem, the state trajectory for t m−1 < t ′ ≤ t + T is given as , which is computed by (19) and (20), and we check whether an additional switch occurs or not between t m−1 and (i m−1 + 1)Δ + t and between iΔ + t and (i + 1)Δ + t for i = i m−1 + 1, … , N − 1 by evaluating q m ,q k (x) for all q k ∈ Q. For example, when l q m ,q k = 1 for all q k ∈ Q, we regard that a switch from subsystem q m to q k occurs when the sign of q m ,q k (x * (t + m (t))) and that of q m ,q k (x * i m (t)) are different or when that of q m ,q k (x * i (t)) and that of q m ,q k (x * i+1 (t)) are different for some When we detect an additional switch in computing the state trajectory, we reconstruct the solution U(t) taking the additional switch and the additional active subsystem into account. Suppose that we detect a switch from q m to q m+1 between i m Δ + t and (i m + 1)Δ + t where i m−1 < i m < N. Note that, if the switch is detected between t m−1 and (i m−1 + 1)Δ + t, we can formulate the NOCP for reconstructing the solution just by replacing x * i m (t), u * i m (t), and i m Δ + t in the following formulation with x * ( + m−1 (t)), u * ( + m−1 (t)), and t m−1 , respectively. When we detect a switch from q m to q m+1 between i m Δ + t and (i m + 1)Δ + t, then t m (t), * q m ,q m+1 (t), and u * (t + m (t)) are appended to U(t). In addition, we have to reoptimize u * i m +1 (t), … , u * N−1 (t) for the additional active subsystem q m+1 because u * i m +1 (t), … , u * N−1 (t) are the optimal control input sequence for subsystem q m before detecting the additional switch. To reduce the computational cost of the reconstruction of the solution, we define an optimal control problem only for the interval after the additional switch to determine t m (t), * q m ,q m+1 (t), u * (t + m (t)), and u * i m +1 (t), … , u * N−1 (t) numerically. Although this optimal control problem only considers the optimality of the interval where q m+1 is active, we resolve the entire optimal control problem in consideration of the whole horizon after solving this optimal control problem. We define the reduced optimal control problem for these variables as follows: find the optimal control input u * (t + m (t)), u * i m (t), … , u * N−1 (t) minimizing the cost function defined for t ′ ∈ (i m Δ + t, t + T], subject to the state trajectory for t ′ ∈ (t + i m Δ , t + T], The optimality conditions for this optimal control problem are derived as * The unknown quantities in this optimal control problem are given as , t m , and * q m ,q m+1 (t). Note that u * i m (t) and x * i m (t) are given as the boundary value. For a given u * (t + m (t)), u * i m (t), … , u * N−1 (t), t m (t), and * q m ,q m+1 (t), we can compute x * (t − m (t)), x * (t + m (t)), … , x * N (t) from (49) and (51)-(53) and * N (t), … , * (t + m (t)) from (54)-(57). Therefore, variables to be determined for this optimal control problem are given as and U q m+1 (t) has to satisfy We solve this problem numerically using the conventional methods, eg, Newton's method. For computation using Newton's method, we make the following assumption for F q m+1 (U q m+1 (t)) and U q m+1 (t).
Note that, if Assumption 7 does not hold, we add a minimum number of the control inputs u to U q m+1 (t) so that the Jacobian of F q m+1 (U q m+1 (t)) for U q m+1 (t) becomes nonsingular and redefine the optimal control problem.
Algorithm 1 shows the reconstruction of the solution for an additional switch. First, if we detect a switch between i m and i m + 1, where i m < N − 1, we set t m (t) between i m Δ + t and (i m + 1)Δ + t. For example, it is reasonable to approximate t m (t) by linear interpolation as Next, we substitute initial guess values for u * (t + m (t)), u * i m (t), … , u * N−1 (t), and * q m ,q m+1 (t). After that, we iterate Newton's method for (62) and obtain U q m+1 (t). We terminate the iteration when the error in the optimality condition ||F q m+1 (U q m+1 (t))|| is sufficiently small or when the number of iteration is sufficiently large. We introducer ∈ ℝ for the criterion of the former and i max ∈ ℕ for that of the latter. After the iteration, we replace u * i m +1 (t), … , to U(t), and append q m+1 to .

The algorithm of the moving switching sequence approach
The length of the horizon We set the length of the horizon as a smooth increasing function such that T(0) = 0 and T(t) → T f (t → ∞), eg, as where T ∈ ℝ and ∈ ℝ are positive constant values, to initialize U(0) in sufficiently short computational time and to modify the solution U(t) when Assumption 6 does not hold.

Initialization of U(0)
We have to compute the initial solution U(0) by iterative method such as Newton's method instead of the C/GMRES method because we do not know the optimal U(t) before t = 0. We also have to compute U(0) in a short computational time unless we know x(0) and compute U(0) in advance. If we set T(t) such that T(0) = 0, we can compute the initial solution U(0) in sufficiently short computational time even by iterative method such as Newton's method: from the initial state x(0) and active subsystem q at t = 0, we obtain u(0) ∈ ℝ m by solving where 0 ∈ ℝ n is obtained by and we set u * i (0) = u(0) for all i = 0, … , N − 1. Note that U(t) obtained by this initialization satisfies F(U(t), x(t), t) = 0. Note also that, if we set the sampling period sufficiently short, T(t) and T(t + Δt) are almost same and we can update U(t) just by integrating .

Shrinkage of the horizon when Assumption 6 does not hold
When we detect an additional switch before the end of the switching sequence in computing the state on the horizon, ie, when Assumption 6 does not hold, we can still use the C/GMRES method by shrinking the horizon and then increasing the length of the horizon smoothly. Suppose that = (q 1 , q 2 , … , q m ) is the switching sequence at time t and an additional switch is detected at the middle of the switching sequence, eg, an additional switch from q k to q j (1 ≤ j < m, q j ≠ q k+1 ) is detected between i j and i j + 1 (i k + 1 ≤ i j < i k+1 ). We then shrink the length of the horizon T(t) by setting t 0 > 0 so that T(t) satisfies We also modify u * , which correspond to u * (t), u * (t+Δ 2 ), … , u * (t+(N−1)Δ 2 ), by linear interpolation from u * 0 (t), … , u * N−1 (t) before the modification, which correspond to u * (t), u * (t+Δ 1 ), … , u * (t+i j Δ 1 ). Figure 5 shows an example of the linear interpolation of u * 0 (t), … , u * N−1 (t) when we shrink the horizon. After interpolating u * 0 (t), … , u * N−1 (t), we remove U q k ,q k+1 (t), … , U q m−1 ,q m (t) from U(t) and remove q k+1 , … , q m from . Once the solution U(t) for the shrunk horizon is obtained, we can continue to update the solution by the C/GMRES method with the length of the horizon T(t) increased smoothly toward T f .

Entire algorithm
Algorithm 2 shows the entire algorithm of the moving switching sequence approach. Note that, in Algorithm 2, we treat q 1 as the first element of and q m as the last element of . This algorithm basically updates U(t) using the C/GMRES method introduced in Section 4.1. If we predict that t 1 (t + Δt) satisfies t 1 (t + Δt) < t + Δt, where q 1 is the first element of the switching sequence , we regard that a switch occurs between the current sampling time and the next sampling time and we remove variables related to the switch from U(t) and remove q 1 from . While computing x * 0 (t), … , x * N (t) in solving the TPBVP, we monitor whether a new switch occurs or not at t ′ ∈ (t m−1 , t + T], where q m is the last element of . If we detect a switch from q m to q m+1 for some q m+1 ∈ Q at t ′ ∈ (t m−1 , t + T], we obtain the additional variables related to the switch numerically and reconstruct U(t) by Algorithm 1 and append q m+1 to . If we detect the violation of Assumption 6, we shrink the horizon and modify U(t) and .

NUMERICAL SIMULATIONS OF COMPASS-LIKE WALKING
This section describes an application of the proposed method to the simplest biped walking robot model, called a compass-like model. 2 Compass-like walking involves state-dependent switches and state jumps, and we successfully control it by using the moving switching sequence approach. Figure 6 shows the model of the compass-like robot, and Table 1 lists its physical parameters. The compass-like robot is composed of two legs and one joint, and we assume that the physical properties of both legs are the same. Let 1 be q T ] T be the state vector. The dynamics of compass-like walking contains three phases: (1) leg 1 is standing on the ground and leg 2 is swinging, (2) leg 2 is standing on the ground and leg 1 is swinging, and (3) legs 1 and 2 are standing on the ground. To simplify the problem, we make the following assumption about the impact between the swinging leg and the ground. 3 Assumption 8. The impact between the swinging leg and the ground results in no rebound and no slipping, and the supporting leg lifts from the ground at the time of the impact.

Compass-like walking model
Under this assumption, the model of the compass-like walking robot is composed of the dynamics when one leg is standing on the ground and the other is swinging and state jumps by the collisions between the swinging leg and the ground. The state equation when leg 1 is standing and leg 2 is swinging is derived by Lagrange's method as where u ∈ ℝ is a control input torque, M 1 ∈ ℝ 2×2 is the inertia matrix, H 1 ∈ ℝ 2 consists of terms related to Coriolis force and terms about gravity force, and B 1 = [1 − 1] T . Additionally, the state equation when leg 2 is standing and leg 1 is swinging is derived as where M 2 ∈ ℝ 2×2 is the inertia matrix, H 2 ∈ ℝ 2 consists of terms related to Coriolis force and terms about gravity force, and B 2 = [−1 1] T . The equations of state jump are derived from the conservation law of angular momentum. 2 When leg 2 collides with the ground, the conservation law of angular momentum is written as where Q − 1,2 , Q + 1,2 ∈ ℝ 2×2 , and when leg 1 collides to the ground, the conservation law of angular momentum is written as where Q − 2,1 , Q + 2,1 ∈ ℝ 2×2 . Note that the posture of the robot is not changed by the collision because the collision is instantaneous from Assumption 8. The equations of the state jump are then given as and ] . (74) The collision occurs when the height of the tip of the swinging leg, h in Figure 6, becomes zero. That is, the switching conditions are given as 1,2 (x) ∶= l(cos 1 − cos 2 ) = 0, (75) and 2,1 (x) ∶= l(cos 2 − cos 1 ) = 0.
We introduce an assumption to abstract compass-like walking motion easily as a switched system. 3 Assumption 9. The swinging leg does not contact with the ground when the swinging leg is put forward from behind the supporting leg.
Under Assumption 9, we ignore the collision between leg 2 and the ground when 1 − 2 ≤ 0 and the collision between leg 1 and the ground when 2 − 1 ≤ 0. Figure 7 summarizes switches and state jumps in compass-like walking.

Controller design
To achieve a steady gait, we design the cost function with two aims: moving forward steadily and achieving a steady gait. To achieve the former aim, we make the angular velocity of the supporting leg close to the reference constant v ref . To achieve the latter aim, we add a 2 ( 1 + 2 ) 2 to the cost function, thereby preventing the tip of the swinging leg from being too high. In addition to these two aims, we add a penalty to the control input u. Consequently, we design the cost functions as and 1 (x) = 2 (x) = 0. The parameters of NMPC are listed in Table 2.

Simulation conditions
To evaluate the robustness of the proposed method, the numerical simulations are performed in two cases: the nominal case without disturbances and the case with impulsive disturbances. In the simulation with disturbances, we input instantaneous changes on the angular velocity as   Figures 8,9, and 10 show simulation results for the nominal case, those for the case with disturbances, and the enlarged time history of the predicted switching instant around the disturbances, respectively. Note that dashed lines in these Figures represent discontinuous change of variables. In Figures 8 and 9, switches and state jumps occur at points where . 1 and . 2 are discontinuous except for discontinuous points due to the disturbances, and the compass-like robot walked successfully in both cases. ||F|| in Figures 8 and 9 denotes the norm of the residual in (37), and t k in Figures 8, 9, and 10 are switching instants on the predicted horizon. Note that, when there are no switches on the predictive horizon, t k is plotted as t k = 0.

Simulation results
We can see that the predicted switching instants are mostly constant in the nominal case from Figure 8 and fluctuate in the case with disturbances from Figures 9 and 10. From Figure 9, instantaneous increases of ||F|| at the moments of the impulsive disturbances attenuate immediately, which suggests that the proposed method optimizes the switching instants and control input simultaneously. Note that, even if in the nominal case, the error increases at the sampling time when an additional switch is detected and when a switch is removed from , ie, when an actual switch occurs. This suggests that errors increase when the switching sequence changes from the previous sampling time. In addition to the points where the error peaks due to such reasons, we can see errors increase when there is a switch on the horizon. This occurs when i k (t) differs from i k (t − Δt). However, ||F|| is sufficiently small on the whole, and we can see that the proposed method can compute the optimal solution. We also found that each update takes around 0.14 [ms] and that we can implement control update within the sampling period of 1 [ms].

CONCLUSIONS
In this paper, we proposed a moving switching sequence approach for real-time computation of NMPC for switched systems with state-dependent switches. In the approach, the switching sequence on the horizon moves to the present time at each time as well as the optimal state trajectory and optimal control input on the horizon. We assume that the switching sequence is basically invariant on the horizon and update the switching sequence at each time by checking whether an additional switch occurs or not and whether the actual switch will occur or not. We propose an algorithm composed of the update of the solution by solving the TPBVP and reconstruction of the solution taking into account an additional switch. We utilize the C/GMRES method for the former problem and solve the reduced optimal control problem for the latter. We performed numerical simulations of compass-like walking for a nominal case without disturbances and a case with impulsive disturbances and found that the proposed method successfully computes the optimal solution in a short time even if there are disturbances.
For future work, we will analyze the necessary or sufficient conditions of the assumption about the invariance of the switching sequence. We will also seek methods to design the cost functions and constraints so that the assumption holds.

ACKNOWLEDGEMENT
This work was partly supported through the JSPS Kakenhi grant JP15H02257.