A Numerical Method for the Optimal Control of Switched Systems Humberto Gonzalez, Ram Vasudevan, Maryam Kamgarpour, S. Shankar Sastry, Ruzena Bajcsy, Claire Tomlin Abstract— Switched dynamical systems have shown great utility in modeling a variety of systems. Unfortunately, the determination of a numerical solution for the optimal control of such systems has proven difficult, since it demands optimal mode scheduling. Recently, we constructed an optimization algorithm to calculate a numerical solution to the problem subject to a running and final cost. In this paper, we modify our original approach in three ways to make our algorithm’s application more tenable. First, we transform our algorithm to allow it to begin at an infeasible point and still converge to a lower cost feasible point. Second, we incorporate multiple objectives into our cost function, which makes the development of an optimal control in the presence of multiple goals viable. Finally, we extend our approach to penalize the number of hybrid jumps. We also detail the utility of these extensions to our original approach by considering two examples. I. I NTRODUCTION A natural extension of classical dynamical systems are switched dynamical systems wherein the state of a system is governed by a finite number of differential equations. The control parameter for such systems has a discrete component, the sequence of modes, and two continuous components, the duration of each mode and the continuous input. Switched systems arise in numerous modeling applications [3], [8]. Stemming from Branicky et al.’s seminal work that estab- lished a necessary condition for the optimal trajectory of switched systems in terms of quasi-variational inequalities [2], there has been growing interest in the optimal control of such systems. However, Branicky provided only limited means for the computation of the required control. Several address just the continuous component of the optimal control of an unconstrained nonlinear switched sys- tem while keeping the sequence of modes fixed. Given a fixed mode schedule, Xu et al. develop a bi-level hi- erarchical optimization algorithm: at the higher level, a conventional optimal control algorithm finds the optimal continuous input assuming fixed mode duration and at the lower level, a conventional optimal control algorithm finds the optimal mode duration while keeping the continuous input fixed [12]. Axelsson et al. consider the special case of unconstrained nonlinear autonomous switched systems (i.e. systems wherein the control input is absent) and develop a similar bi-level hierarchical algorithm: at the higher level, H. Gonzalez, R. Vasudevan, M. Kamgarpour, S. S. Sastry, R. Bajcsy, and C. Tomlin are with the Department of Electrical Engineering and Com- puter Sciences, University of California at Berkeley, Berkeley, CA, 94720, {hgonzale,ramv,maryamka,sastry,bajcsy,tomlin}@eecs. berkeley.edu This work was supported by the Air Force Office of Scientific Research (AFOSR) under Agreement Number FA9550-06-1-0312 and PRET Grant 18796-S2 and the National Science Foundation (NSF) under grants 0703787 and 0724681. the algorithm updates the mode sequence by employing a single mode insertion technique, and at the lower level, the algorithm assumes a fixed mode sequence and minimizes the cost functional over the switching times [1], [5]. Recently, we generalized Axelsson’s approach by con- structing an optimal control algorithm for constrained non- linear switched dynamical systems [6]. We developed a bi- level hierarchical algorithm that divided the problem into two nonlinear constrained optimization problems. At the lower level, our algorithm assumed a fixed modal sequence and determined the optimal mode duration and optimal contin- uous input. At the higher level, our algorithm employed a single mode insertion technique to construct a new lower cost sequence. The result of our approach was an algorithm that provided a sufficient condition to guarantee the local optimality of the mode duration and continuous input while decreasing the overall cost via mode insertion. Though this was a powerful outcome given the generality of the problem under consideration, it suffered from three shortcomings which made its immediate application difficult. First, if our algorithm was initialized at an infeasible point it was unable to find a feasible lower cost trajectory. Unfortunately, initializing an optimization algorithm with a feasible point is nontrivial. Second, our algorithm did not incorporate multiple objectives into its cost function, which is useful for path planning type tasks. Finally, our algorithm did not penalize the number of hybrid jumps. In this paper, we design a new algorithm to address these three deficiencies and detail the utility of this modified approach on two examples. This paper is organized as follows: Section II provides the mathematical formulation of the problem under consid- eration, Section III describes the optimal control algorithm which is the primary result of this paper, Section IV de- tails the proof of convergence of our algorithm, Section V considers a numerical implementation of the optimal control scheme, Section VI presents numerical experiments, and Section VII concludes the paper. II. PROBLEM FORMULATION In this section, we present the mathematical formalism and define the problem considered in the remainder of the paper. We are interested in the control of systems whose continuous trajectory is governed by a set of vector fields f q : R n × R m → R n , where q belongs to Q = {1, 2,...,Q}. Any trajectory of such a system is encoded by a sequence of discrete modes, a corresponding sequence of times spent in each mode, and the continuous input over time. We also require a mapping between each of our objectives and an element of our sequence of discrete modes. To formalize the