IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 1

An Offline-Sampling SMPC Framework with Application to Automated Space Maneuvers

Martina Mammarella1, Matthias Lorenzen2, Elisa Capello3 Member, IEEE, Hyeongjun Park4 Member, IEEE, Fabrizio Dabbene5, Senior Member, IEEE, Giorgio Guglieri1, Marcello Romano4, Senior Member, IEEE, and Frank Allgower¨ 2, Member, IEEE

Abstract—In this paper, a sampling-based Stochastic Model multi-variable systems, explicitly taking into account state and Predictive Control algorithm is proposed for discrete-time linear equipment constraints, see for instance the recent survey [1]. systems subject to both parametric uncertainties and additive Early publications on the topic already emphasized that, disturbances. One of the main drivers for the development of the proposed control strategy is the need of reliable and moving horizon schemes like MPC might incur significant robust guidance and control strategies for automated rendezvous performance degradation in the presence of uncertainty [2]. and proximity operations between spacecraft. To this end, the Furthermore, ignoring modeling errors and disturbances can proposed control algorithm is validated on a floating spacecraft lead to constraint violation in closed loop and the online experimental testbed, proving that this solution is effectively optimization being infeasible. To cope with this disadvantage, implementable in real-time. Parametric uncertainties due to the mass variations during operations, linearization errors, and dis- in the last years Robust MPC has received a great deal of atten- turbances due to external space environment are simultaneously tion and, at least for linear systems, it can nowadays be consid- considered. ered well-understood and having achieved a mature state [3]. The approach enables to suitably tighten the constraints Yet, the inherent conservativeness of robust approaches, has to guarantee robust recursive feasibility when bounds on the led to an increased interest in Stochastic Model Predictive uncertain variables are provided. Moreover, the offline sampling approach in the control design phase shifts all the intensive Control (SMPC) for processes where a stochastic model can computations to the offline phase, thus greatly reducing the online be formulated to represent the uncertainty and disturbance [4]. computational cost, which usually constitutes the main limit for Indeed, a probabilistic model allows to optimize the average the adoption of Stochastic Model Predictive Control schemes, performance or appropriate risk measures and the introduction especially for low-cost on-board hardware. Numerical simulations of so-called chance constraints, which seem more appropriate and experiments show that the approach provides probabilistic guarantees on the success of the mission, even in rather uncertain in some applications. Furthermore, chance constraints lead to and noise situations, while improving the spacecraft performance an increased region of attraction and enlarge the set of states in terms of fuel consumption. for which MPC provides a valid control law [5]. Index Terms— Stochastic Model Predictive Control; Chance On the other hand, the classical criticism of MPC schemes, Constraints; Sampling-based Approach; Receding Horizon Con- especially in their robust/stochastic instantiations, is their slow- trol; Real-time Implementability; Automated Rendezvous be- ness. This has limited their application to problems involving tween Spacecraft. slow dynamics, where the sample time is measured in tens of seconds or minutes. In particular, due to the increased computational load, SMPC has mainly been applied for slow I.INTRODUCTION systems, as e.g. water networks [6] or chemical processes [7]. N the last decades, model predictive control (MPC) has This widely recognized shortcoming is mainly due to the arXiv:1803.01652v2 [cs.SY] 8 Mar 2018 I become one of the most successful advanced control tech- computational effort required in the on-line solution of the en- niques for industrial processes, thanks to its ability to handle suing optimization problem, and to the difficulty of embedding a real-time solver for MPC implementation. When the number 1M. Mammarella and G. Guglieri are with the Depart- of variables and/or prediction horizon increase and the system ment of Mechanical and Aerospace Engineering, Politecnico to be controlled is characterized by fast dynamics, a practical di Torino, Torino, Italy, (martina.mammarella, [email protected]) solution proposed in the literature is to evaluate offline the 2M. Lorenzen and F. Allgower¨ are with the Insti- control law, and then the control action is implemented online tute for Systems Theory and Automatic Control, Univer- as a lookup table [8]. However, this solution renders the sity of Stuttgart, Germany, (matthias.lorenzen, [email protected]) controller less apt to deal win an appropriate way with model 3 E. Capello is with the Department of Mechanical and Aerospace uncertainties and external disturbances. Moreover, the com- Engineering and with the CNR–IEIIT, Politecnico di Torino, Torino, Italy, putational effort still grows very rapidly with the increase in [email protected] horizon, state and input dimensions. Hence, a quite substantial 4H. Park is with the Department of Mechanical and Aerospace Engineering, New Mexico State University, Las Cruces, NM, USA, computational capability and large memory requirements are ([email protected]) mandatory, especially for systems with fast dynamics, such as 5 F. Dabbene is with the CNR–IEIIT, Politecnico di Torino, Torino, Italy, UAV, aircraft, and spacecraft. [email protected] 6M. Romano is with the Department of Mechanical and Aerospace, Naval In space applications, the available and adopted processors Postgraduate School, Monterey, CA, USA, ([email protected]) provide limited computational power on board of current and IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 2 near-future spacecraft. This constrains the level of space- methods proposed in [16], [17], [18], in which the uncertainty craft autonomy because even relatively simple autonomous is propagated recurring to a finite number of scenarios to be operations require complex computations to be performed considered at each step. However, these approaches may be in near real time. In this framework, the requirement of still rather demanding for real-time implementations, since real-time implementability for new Guidance Navigation and different samples need to be drawn at each step. Recently, Control (GNC) algorithms gains the highest priority. The this drawback was overcome by the introduction of offline implementation of classical MPC on low-cost hardware, such sampling strategies, that allow to reduce the computational as microcontrollers, is already quite demanding. effort made online by means of a pre-processing of data made The contribution of the paper is twofold. From a theoret- offline. In particular, in [9] this approach was developed for ical viewpoint, the paper integrates and extends the previous problems involving additive disturbances, acting on a nominal works of the authors [5], [9], proposing offline sample-based system. In [5], parametric uncertainty are instead considered strategies for addressing in a computationally tractable manner in a noise-free setting. Clearly, both these approaches are Stochastic Model Predictive Control (SMPC). In particular, as somehow limited for real-world applications, such as those detailed Section I-A, the paper develops for the first time a encountered in spacecraft control. complete and integrated framework, able to cope simultane- This paper solves the nontrivial problem of extending the ously with additive random noise and parametric stochastic previous result into a comprehensive framework, able to tackle uncertainty. situations in which both additive disturbances and parametric From an application viewpoint, the paper demonstrates real- uncertainties are simultaneously present. The main contribu- time implementability of the proposed scheme, addressing a tion of this paper to the theory of SMPC is the introduction very important control problem arising in aerospace applica- of a nonconservative SMPC scheme that is computationally tions, the Autonomous Rendezvous and Docking (ARVD) ma- tractable and guarantees recursive feasibility. As in [9], the neuver among spacecraft. Indeed, as discussed in Section I-B, computational load is reduced by generating scenarios offline the ability to carry over proximity operations in a completely and keeping only selected, necessary samples for the online autonomous manner represents one of the main challenges optimization. The algorithm guarantees robust satisfaction of of modern spacecraft missions. These require the capability the input constraints and bounds on the confidence that the of dealing in an efficient way with external disturbances chance constraints are satisfied can be chosen by the designer. due to the space environment, and with uncertainties. These Due to the additive disturbance, the state does not converge uncertainties are not only due to unmodeled dynamics or lin- to the origin. Instead, an asymptotic performance bound is earization effects, but also to the necessity of designing control provided. The presented theory is attractive for real-world techniques able to be implemented on vehicles produced in applications, since the design can be based on real data gath- good quantities, which will be the trend in future missions. ered from experiments or high fidelity simulations. Moreover, The SMPC scheme is shown to be able to cope with all thanks to the offline sampling approach, this SMPC scheme these requirements, providing sufficiently high guarantees in can be applied to significantly fast dynamics, as those relative terms of safety and constraints satisfaction, and at the same to space platforms during the final phase of the automated time being sufficiently fast to be implemented in a real-time rendezvous and mating maneuver. framework (this latter issue is discussed in Section III-C). In the next section, we highlight the contributions of the B. ARVD Problem (Problem Setup) present work to the SMPC theory, while the next section describes in detail the application example considered, high- The advancement of robotics and autonomous systems lighting how it can benefit from the performance guarantees will be central to the transition of space missions from provided by the introduced control framework. current ground-in-the-loop (geocentric) architectures to self- sustainable, independent systems, mainly to support human activities beyond Low Earth Orbits (LEO). Indeed, the Com- A. A Novel Stochastic Model Predictive Control Framework mittee on NASA Technology Roadmaps has highlighted as The main problem encountered in the design of SMPC algo- “Robotics, Tele-Robotics, and Autonomous Systems” shall rithms is the derivation of computationally tractable methods be regarded as high-priority technology area in broadening to propagate the uncertainty for evaluating the cost function access to space and expanding human presence in the Solar and the chance constraints. Both problems involve multivari- System [19]. Among them, ARVD represents the cornerstone ate integrals, whose evaluation requires the development of technology, since all the scenarios that space agencies have suitable techniques. An exact evaluation is in general only defined for the future exploration program have one thing possible for linear systems with additive Gaussian disturbance, in common: each mission architecture heavily relies on the where the constraints can be reformulated as second-order ability to rendezvous and mate multiple elements in space cone constraints [10], or for finitely supported disturbances as autonomously. In order to meet the exploration enterprise in [11]. Approximate solutions include a particle approach [12] goals of affordability, safety and sustainability, the critical or polynomial chaos expansion [13]. Among the different capabilities of rendezvous, capture and in-space assembly must methods, randomized algorithms [14], and in particular the become routine and autonomous, increasing their reliability scenario approach [15], represent the most promising ones. [20]. The complexity of the ARVD mainly results from the The first approaches in this direction can be found in the multitude of safety and operational constraints which must IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 3 be fulfilled. These constraints are defined with respect to the their performance. rendezvous approach phase considered. In terms of safety, The space community is working on new regulations to the close range rendezvous phase is the most critical, since define the safety constraints for ARVD maneuvers. These the space systems involved are relatively close together and rules should be defined with respect to collision avoidance the trajectory of the chaser, by definition, leads toward the requirements driven by the mission as well as by the stake- target, so that any deviation from the planned trajectory holders, introducing a maximum level of constraints violation can potentially lead to a collision. Therefor, the main focus probability allowance. Furthermore, until now, rendezvous of this paper is on the final approach between the chaser maneuvers have involved the International Space Station (ISS) vehicle with the target one, considering the typical minus and the rigid safety requirements are mainly due to the V-bar approach. First, for sensing purposes (see [21]), it presence of a crew on board, which requires the highest level is required that the chaser vehicle remains inside a Line- of failure tolerance. On the other hand, in the future, numerous Of-Sight (LOS) from the docking point, constraint usually missions will involve ARVD maneuvers between unmanned defined in terms of an approaching corridor, as represented systems and the safety requirements could be relaxed, always in Figure1, which can be modeled as a polytope (without in compliance with the protection of the investment made in any generality loss, a rectangular parallelepiped can be used). the space systems involved. Moreover, soft docking constraints can be enforced, reducing Several methodologies have been proposed in the literature the approach velocity in line with distance to the target, as for the ARVD, which have shown robustness with respect to well as limiting the maximum approach velocity. When using known and unknown uncertainty and disturbance affecting the thrusters for spacecraft trajectory control, not only there are system during the final phase of the rendezvous maneuver. constraints on the maximum force that can be applied at The reader is referred to [23] for a recent survey. In any given instant, i.e. saturation of the actuators, but there is particular, we want to recall the approach proposed in [24], also the physical constraint of a thrust “dead-zone” between where a robust MPC is adopted to solve the problem of the thruster being fully off, and delivering its minimum non- spacecraft rendezvous, using the Hill-Clohessy-Wiltshire zero thrust, often referred to as the “Minimum Impulse Bit” (HCW) model and including additive disturbances and LOS (MIB), and the total number of firings available. Indeed, constraints. Furthermore, it has been proved that a robust constraints on the maximum deliverable ∆v are placed on approach implies higher fuel consumption with respect to each element of the input vector. Last but not least, another classical methods where disturbances are neglected (see [25]). constraint can be imposed on the fuel consumption or on However, still a probability of constraints violation needs to the amount of fuel dedicated to the maneuver. A second be considered. In this work, a stochastic approach is proposed in order to relax the safety trajectory constraints reducing the conservativeness with respect to a robust approach, as well as fuel consumption, optimizing the average performance and allowing an affordable level of constraints violation.

The remainder of this paper is organized as follows. SectionII introduces the finite horizon receding optimal Fig. 1. Line-Of-Sight constraint defined in terms of infeasible/feasible region control problem, starting with a suitable reformulation of the considering a minus V-bar approach [22]. constraints through an offline uncertainty sampling approach. Thereafter, the SMPC scheme algorithm is resumed, and important challenge for close-range ARVD is represented by its main theoretical properties are summarized and proved. the need to handle uncertainty. Thruster firings, aerodynamic In Section III, the experimental testbed used to validate drag in Low Earth Orbit (LEO), solar radiation pressure, and the real-time implementability of the proposed scheme is camera measurements can introduce uncertainties in relative described and its dynamical model is derived, including state knowledge and control accuracy. As the spacecraft nears the identification and modeling of uncertainty and additive its target, these uncertainties can induce violations in any of the disturbance and presenting the main issues linked to real-time aforementioned mission constraints. Hence, one should embed implementability and principal solvers investigated. The in ARVD algorithms the capability to handle any expected simulation and experimental results are discussed in Section uncertainty directly, i.e. incorporating strategies to handle all IV and the algorithm performances are discussed with respect known unknowns. The key for ARVD GNC strategies is to computational effort and fuel consumption. Finally, Section relying on solution techniques that can be made efficient V provides some conclusions and directions for future works. for real-time implementation. Indeed, in order to meet the GNC challenges of next-generation space missions, onboard Notation: The notation employed is standard. Uppercase letters algorithms will need to meet the following specifications: (i) are used for matrices and lower case for vectors. [A]j and real-time implementability; (ii) optimality; (iii) verifiability. [a]j denote the j-th row and entry of the matrix A and vector Therefor, new GNC algorithms need to be implemented and a, respectively. Positive (semi)definite matrices A are denoted 2 T executed on real-time processors, in a compatible amount A 0 (A  0) and kxkA = x Ax. The set N>0 denotes the of time, providing a feasible and (approximately) optimal positive integers and N≥0 = {0} ∪ N>0, similarly R>0, R≥0 solution, verifying the design metrics identified to describe for positive real numbers. The notation Pk {A} = P {A|xk} IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 4

. denotes the conditional probability of an event A given for all q ∈ Q, with Acl(qk) = A(qk) + B(qk)K, and with n×n m×m the realization of xk, similarly Ek {A} = E {A|xk} for Q ∈ R , Q 0, R ∈ R , R 0. the expected value. We use x for the (measured) state k Following a dual-mode prediction scheme, also adopted at time k and x for the state predicted l steps ahead at `|k in [4] to define the predicted control sequence for nominal, time k. The sequence of length T of vectors v , . . . , v 0|k T |k robust and also stochastic MPC, we consider the design of a is denoted by v . A ⊕ B = {a + b| a ∈ A, b ∈ B}, T |k parametrized feedback policy of the form A B = {a ∈ A| a + b ∈ A, ∀b ∈ B} denote the Minkowski sum and the Pontryagin set difference, respectively. u`|k = Kx`|k + v`|k, (4)

where for a given x0|k = xk, the sequence of correction terms .  II.SMPC DESIGN UNDER UNCERTAINTY AND RANDOM vk = v`|k T −1 is determined by the SMPC algorithm as `∈N0 NOISE the minimizer of the expected finite-horizon cost We consider the following discrete-time system subject to (T −1 ) X T T T both random noise and stochastic uncertainty JT (xk, vk) = E (x`|kQx`|k + u`|kRu`|k) + xT |kP xT |k , l=0 xk+1 = A(qk)xk + B(qk)uk + Bw(qk)wk, (1) (5) n m subject to constraints (2). with state xk ∈ R , control input uk ∈ R , additive mw nq disturbance wk ∈ R , and parametric uncertainty qk ∈ R . We first report the principal assumptions made in the paper A. Offline Uncertainty Sampling for SMPC regarding system (1). For the following analysis, we first explicitly solve equa- (w ) The disturbance sequence k k∈N≥0 is assumed to be a tion (1) with prestabilizing input (4) for the predicted realization of a stochastic process (Wk)k∈ . N≥0 states x1|k, . . . , xT |k and predicted inputs u0|k, . . . , uT −1|k. In particular, simple algebraic manipulations show that it 0 Assumption 1 (Bounded Random Disturbance). The distur- is possible to derive suitable transfer matrices Φ`|k(qk), v w bances Wk, for k = 0, 1, 2..., are independent and identically Φ`|k(qk), Φ`|k(qk) and Γ` (the reader is referred to Ap- distributed (iid), zero-mean random variables with support W, pendixA for details), such that which is a bounded and convex set. 0 v w x`|k(qk, wk) = Φ`|k(qk)xk + Φ`|k(qk)vk + Φ`|k(qk)wk(6a) We assume that the system matrices A(qk), B(qk) and 0 v u`|k(qk, wk) = KΦ`|k(qk)xk + (KΦ`|k(qk) + Γ`)vk Bw(qk), of appropriate dimensions, are (possibly nonlinear) w +KΦ`|k(qk)wk, (6b) functions of the uncertainty qk. The uncertainty vector .  qk belongs to a given set Q and satisfies the following where wk = w`|k T −1 . In the previous equations, we `∈N0 assumption. highlight that both predicted states and inputs are function of the uncertainty qk and the noise sequence wk. Given the Assumption 2 (Stochastic Uncertainty). The parameters solution (6), the expected value of the finite-horizon cost (5) nq can be evaluated offline, leading to a quadratic cost function qk ∈ R , for k ∈ N, are realizations of i.i.d. multivari- of the form ate real valued random variables Qk. Moreover, let G =   {(A(qk),B(qk),Bw(qk))} , a polytopic outer approxima- xk . qk∈Q ¯  j j j J (x , v ) = [xT vT 1T ]S˜ v tion G = co A ,B ,Bw Nc ⊇ G exists and is known. T k k k k mw  k  (7) j∈N1 1mw The system is subject to px individual chance-constraints on the state and m hard constraints on the input in the deterministic variables xk and vk. The evaluation of S˜ p requires the computation of an expected value, which can be [H ] x ≤ [h ] ≥ 1 −  , ∀ ` ∈ , α ∈ x P x α `|k x α α N≥0 N1 explicitly evaluated or sufficiently exact approximated taking (2a) random samples of qk and wk (see again AppendixA for Huu`|k ≤ hu, ∀ ` ∈ N>0, (2b) details).

px×n px m×m m with Hx ∈ R , hx ∈ R , Hu ∈ R , hx ∈ R , and α ∈ (0, 1). Note that the probability P in (2) denotes the We now follow the same approach proposed in [9], and joint probability with respect to qk and wk. Then, as typical observe that an inner approximation for the chance constraint x in stabilizing MPC, we assume that a suitable terminal set XT (2a) can be derived in the form of linear constraints on k, and an asymptotically stabilizing control gain for (1) exist. vk and wk, utilizing a sampling-based approach. In particular, px for each probabilistic state constraint α ∈ N1 , and for each Assumption 3 (Terminal set). There exists a terminal set T −1 time step ` ∈ N0 , let us define the corresponding chance- XT = {xk |HT xk ≤ hT }, which is robustly forward invariant constrained set as follows for (1) under the (given) control law uk = Kxk. Given any P,α   xk ∈ XT , the state and input constraints (2) are satisfied and X` = P [Hx]αx`|k(qk, wk) ≤ [hx]α ≥ 1 − α . (8) there exists P ∈ n×n such that R In the above definition, we use the apex P as in [9] to indicate T T Q + K RK + E[Acl(qk) PAcl(qk)] − P  0 (3) that the set has probabilistic nature. Then, exploiting results IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 5

P,α from statistical learning theory [26], an estimate of X` may this end, for given probabilistic level β ∈ (0, 0.14) for each x p x (i` ) u u ˜ be constructed extracting N` iid samples q from Qk, and β ∈ N1 , we draw N` ≥ N(n + `m, β, δ) random samples x N x (i` ) x ` sampled input constraint set w , with i` ∈ N1 , and building the corresponding sampled and construct the state constraint set n u o S,β iu N` U` = xk, vk | [Hu]βu`|k(q ) ≤ hu, iu ∈ N1 n x N x o S,α (i` ) x ` = xk, vk | [Hx]αx`|k(q ) ≤ [hx]α, i` ∈ 1 , X` N T −1 u for ` ∈ N0 , thus obtaining the N` linear constraints for ` ∈ T −1. The apex S is used to indicate that the set is N0 H u (q(iu), w(iu)) ≤ h , the outcome of a sampling process. u `|k u In particular it was shown in [9] that, for given probabilistic which, from (6b), rewrites as the following linear constraint levels δ ∈ (0, 1) and α ∈ (0, 0.14), if we define in xk, vk x  4.1 21.64 8e  ˜ x ˜ u k ˜ ˜ Hu Hu ≤ hu. (11) N(d, α, δ) = ln + 4.39d log2 , vk α δ α ˜ x ˜ u x ˜ S,α where Hu and Hu are defined analogously to (10), and involve then the choice N ≥ N(p+`m, α, δ) guarantees that ⊆ . ` X` N u = PT −1 N u samples. Finally, for each γ ∈ n,  ∈ P,α with probability greater than 1−δ. Hence, we obtain that `=1 ` N1 γ X` (0, 0.14), the terminal constraints can also be approximated x ∈ S,α is guaranteed with high probability whenever x `|k X` `|k by drawing N ≥ N˜(n + T m,  , δ) random samples and satisfies the following set of linear constraints T γ constructing the sets x (ix) (ix) x N ` ` ` n o Hxx`|k(q , w ) ≤ hx, for i` ∈ N1 . S,γ iT NT XT = xk, vk | [HT ]γ xT |k(q ) ≤ hT , iT ∈ N1 Note that, from (6a), the above equations rewrite as the NT following linear constraint in xk, vk for iT ∈ N1 , which lead to   (iT ) (iT ) x HT x (q , w ) ≤ hT . H˜ x H˜ u k ≤ h˜ (9) T |k x x v x k that through (6a), where we defined    ˜ x ˜ u xk ˜  0 (1) v (1)  HT HT ≤ hT (12) HxΦ0|k(q ) HxΦ0|k(q ) vk . .  . .  ˜ x ˜ u T  . .  where HT and HT involve N samples.  0 (N x) v (N x)   H Φ (q 0 ) H Φ (q 0 )  The linear constraints (9), (11), (12), possibly after con-  x 0|k x 0|k  x u  . .  straint reduction, can be summarized in the following linear [H˜ H˜ ] =  . .  , x x  . .  constraint set 0 (1) v (1)  HxΦ (q ) HxΦ (q )   T−1|k T−1|k   H˜ x H˜ u h˜   . .   x x   x   . .  ˜ x ˜ u xk ˜  . .  D = xk, vk | HT HT  ≤ hT  x x vk 0 (NT−1) v (NT−1)  ˜ x ˜ u ˜  HxΦT−1|k(q )HxΦT−1|k(q ) Hu Hu hu     (10a) xk ˜ = xk, vk | H˜ ≤ h . (13)  w (1) (1)  vk hx −HxΦ0|k(q )wk Moreover, again similar to [9], a first step constraint is added  .   .  to (13), defined starting from the set  x x  w (N ) (N0 )  h −H Φ (q 0 )   x x 0|k wk    n    xk n+m ∃v1|k, ··· , vT −1|k ∈ R , ˜  .  CT = ∈ R (14) hx =  .  . (10b) v0|k s.t. (xk, vk) ∈ D    h −H Φw (q(1))w(1)   x x T−1|k k  which defines the set of feasible states and first inputs of the    .  scenario program with given fixed samples. Therefore, we can  .  ∞ x x define CT,x = {xk |H∞xk ≤ h∞} as the (maximal) robust w (N ) (NT−1 ) h −H Φ (q ` )w x x T−1|k k control invariant set for the system (1) with (xk, uk) ∈ CT . Note that the total number of samples to be drawn to construct Finally, in order to ensure robust recursive feasibility, a con- x . PT−1 x straint on the first input is added to (13) and the additional the sampled constraint sets (9) is equal to N = `=0 N` , and thus the total number of linear inequalities will be pN x. constraint set is given by On the other hand, these sets can be be computed offline. We  DR = xk, vk | H∞Acl(qk)xk + H∞B(qk)v0|k ≤ note also that, due to the sampling procedure, these linear h − H B (q )w (15) constraints are in general highly redundant. To cope with this ∞ ∞ w k 0|k issue, suitable algorithms for redundant constraints removal with A(qk),B(qk),Bw(qk) from Assumption 1 and Acl(qk) = may be applied and the sets can be further simplified. The A(qk) + B(qk)K. The final set of linear constraints to be reader is referred to [9] for a thorough discussion on this issue. employed in online implementation is thus given by the In a similar way, the hard input constraints can be approx- intersection of the sets D and DR, defined in (13) and (15) imated by introducing a suitable sampled approximation. To respectively. IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 6

B. SMPC Algorithm Based on Offline Sampling instrumental in proving that the control input returned by the The complete sampling-based SMPC algorithm we propose algorithm guarantees satisfaction of the chance-constraints on is split into two parts: (i) an offline step, which comprises the state and hard constraints on the input defined in (2). This the sample generation and the computation of the ensuing is formally stated next. sets, and (ii) a repeated online optimization. While the first ∞ Proposition 2 (Constraint Satisfaction). If x0 ∈ CT,x, then step may be rather costly, the online implementation has only the closed-loop system under the OS-SMPC control law, for involves the solution of quadratic programs, which may be all k ≥ 1, satisfies each probabilistic state constraint (2a) with carried out in a very efficient way. A detailed description confidence (1−δ), and the hard input constraint (2b) robustly. of the Offline Sampling-Based Stochastic Model Predictive Control (OS-SMPC) scheme is reported next. Proof Since the OS-SMPC algorithm is robustly recursively feasible (Proposition1), hard input constraint satisfaction is OS-SMPC scheme guaranteed, because of Huu0|k ≤ hu, which does not rely on sampling. On the other hand, for all α = 1, . . . , p, we S,j OFFLINE STEP. Before running the online control algorithm: have D ⊆ X1 . Hence, by Proposition1, for all feasible 1) Compute the finite-horizon cost matrix S˜ in (7); (xk, vk) ∈ D, we can ensure with confidence (1 − δ) that the 2) Draw a sufficiently large number of samples to deter- chance constraint (2a) is satisfied.  S,α S,β S,γ mine the sampled constraints X` , U` , and XT , defined respectively in (9), (11), (12), Finally, we analyze the convergence properties of the pro- posed scheme. To this end, we first remark that, since additive 3) Possibly remove redundant constraints and get D in (13) disturbances affect the system at every time instant, we cannot 4) Determine the first step constraint set D in (15). R expect the closed-loop system to be asymptotically stable at the origin. ONLINE IMPLEMENTATION. At each time step k: However, we can show that, under persistent noise excita- 1) Measure the current state xk; tion, the closed-loop state does remains bounded, under the 2) Determine the minimizer of the quadratic cost (7) subject following technical assumption. to the pre-computed linear constraints D and DR   Assumption 4 (Bounded Optimal Value Function). Let xk ∗ ˜ VT (xk) be the optimal value function of the quadratic program vk = arg min [xk vk 1mw ]S  vk  n×n vk (2), and let P`,Pu ∈ R ,P` 0,Pu 0 be such that 1mw T T ∞ xk P`xk ≤ VT (xk) − c ≤ xk Puxk holds for all xk ∈ CT,x, s.t. (xk, vk) ∈ D ∩ DR; where c is a constant term related to the presence of additive 3) Apply the control input disturbance. ∗ Assumption4 guarantees that the increase in cost, in cases uk = Kxk + v0|k, when the candidate solution does not remain feasible, is ∗ where v0|k is the first control action of the optimal limited. We are now in the position to state the main result of ∗ sequence vk. this section, whose proof is reported in AppendixB.

In the next section, we prove several important properties Proposition 3 (Asymptotic Bound). Let f = [0, 1) be the of the proposed OS-SMPC scheme. maximum probability that the previously planned trajectory is not feasible. Then, there exists a constant C = C(f ) such that C. Theoretical Guarantees of OS-SMPC t 1 X 2 lim kxkk2 ≤ C. (16) First, we show how the introduction of the first step con- t→∞ t k=0 straint DR allows to prove recursive feasibility of the OS- SMPC scheme. The results of this section guarantee that the proposed OS- SMPC scheme enjoys important theoretical properties. These, Proposition 1 (Recursive Feasibility). Let (x ) = V k combined with the efficiency of the scheme, which confines v ∈ mT | (x , v ) ∈ ∩ . If v ∈ (x ), then, for k R k k D DR k V k all costly computations in an offline step, and the generality every realization q and x = A (q )x + B(q )v + k k+1 cl k k k 0|k of the considered setup, addressing both additive noise and B (q )w , the OS-SMPC guarantees w k 0|k parametric uncertainty, render the scheme suitable for efficient V(xk+1) 6= ∅. real-time applications. In the next section, we show how the scheme can be applied to control the last stage of a ARVD Proof The proof follows similar lines to the one provided mission. in [9], and is briefly sketched here: From (xk, vk) ∈ DR ∞ it follows xk+1 ∈ CT,x robustly. Then, by construction, ∞ III.PROXIMITY OPERATIONS MODEL SETUP CT,x ⊂ {x | V(x) 6= ∅}.  The objective of the following section is to investigate the The previous proposition, besides showing how the OS- applicability of. the OS-SMPC to achieve autonomous docking SMPC algorithm guarantees recursive feasibility, it is also in ARVD mission. Goal of the control, in the docking stage, is IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 7 to guide an active vehicle, the chaser, towards a passive one, of the nozzle inlet pressure, which changes depending on the the target, along a specific trajectory, while satisfying security number of thrusters that are being fired simultaneously as well constraints. as the actual pressure in the tank. The onboard computational

A. The NPS-POSEYDIN Simulator The performance proposed MPC controller was experimen- tally evaluated at the Naval Postgraduate School (NPS) Prox- imity Operation with Spacecraft: Experimental hardware-In- the-loop DYNamic simulator (POSEIDYN), an experimental testbed developed to provide a representative system-level platform upon which to develop, experimentally test, and partially validate GNC algorithms.

Fig. 3. NPS-POSEIDYN FSSs: the chaser on the left and the target on the right.

capabilities of the FSS are provided by a PC-104 form- factor onboard computer, based on an Intel Atom 1.6 GHz 32-bit processor, with 2 GB of RAM and an 8 GB solid- state drive. Despite the onboard computer not being space- grade, its computational capabilities may be regarded to be of the same order of magnitude of state-of-the-art space-grade computers. A serial interface is used to communicate with an onboard fiber-optic gyroscope which provides angular velocity measurements at a 100 Hz rate. Hence, the NPS-POSEIDYN Fig. 2. NPS-POSEIDYN testbed with the Vicon motion capture cameras, FSSs, and granite monolith in the Spacecraft Robotics Laboratory at the Naval setup is able to provide full-state estimate. Two 95 Wh lithium- Postgraduate School. The target FSS is on the right and the chaser FSS is on ion batteries and a battery management module regulate the the left. For obvious reasons, the applicability of the testbed, as a high-fidelity electrical power to the FSS. Whereas, a Wi-Fi module provides dynamic simulator, is limited to short lived close proximity operations, with respect to the planar motion only. the FSS with wireless communication capabilities with other FSS and the ground station. Furthermore, once the location As shown in Figure2, the NPS-POSEIDYN consists of of the FSS is determined by the Vicon system, the ground four main elements: (i) a 15 ton, 4-by-4 meter polished station computer streams the telemetry data to the FSS using granite monolith, with a planar accuracy of ±0.0127 mm the Wi-Fi link. The main FSS physical properties are resumed and a horizontal leveling accuracy at least 0.01 deg; (ii) in TableI, in terms of mass, geometry, and moment of inertia two Floating Spacecraft Simulators (FSS), representing real (MOI). Additional details are provided in [28]. spacecraft, which use three 25 mm air bearings to float on top of the granite table in a quasi-frictionless and low TABLE I residual acceleration dynamic environment; (iii) a commercial SUMMARY OF RELEVANT FSSPHYSICAL PROPERTIES. motion capture system, produced by British Vicon Motion Systems Ltd [27], composed by ten overhead cameras, which Parameter Value accurately determines the position of objects carrying passive Dry Mass [kg] 9.465 ± 0.001 markers (i.e. the FSS); (iv) a ground station computer. Wet Mass [kg] 9.882 ± 0.001 Dimensions [m] 0.27 × 0.27 × 0.52 2 The FSS are custom-designed vehicles that emulate orbital MOI [kg·m ] 0.2527 spacecraft moving in close proximity of another vehicle or object (see Figure3). The air bearings use compressed air, In order to simplify the algorithm development and subse- delivered by an onboard tank, to lift the FSS approximately quent implementation on the FSS, a development simulator 5 µm, creating an air film between the vehicle and the granite and a FSS software template were created using a common surface that eliminates their direct contact. To propel the custom library. The simulator uses simulated sensors and FSS, the vehicles are equipped with eight cold-gas thrusters, actuators and also simulates the plant (i.e. the FSS) response, mounted in couple to each corner of the upper part of the while the FSS software allows to develop the algorithms in vehicle, each one providing a maximum thrust of 0.15 N. a simulation environment and, when ready, easily generate This value fluctuates considerably, as the thrust is a function the FSS onboard software to test them. The multi-rate GNC IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 8

software running atop the RT-Linux OS is developed uti- with x in the direction of the motion iθ, z in the radial direction lizing MATLAB/Simulink environment. Once developed, the ir and iy = iθ × ir normal to the orbital plane, the scalar Simulink models are autocoded to C and compiled. form of the well-known CW Equation can be obtained. Hence, the parametric uncertainty introduced in the model are of the 2 2 2 B. Model of the Planar Experimental Testbed same order of O(ρ /r1) and O(ρ ). When external forces are acting on the system, in this case due to the correction actions To design the control architecture, we started by deriving actuated by the thrusters (Fx,Fy,Fz) of the AOCS subsystem, a continuous-time description of the Chaser dynamics, taking we have into account parametric uncertainties and additive noise, ob- taining an uncertain state-space equation of the form 2 d x dz Fx 2 − 2ω = , x˙ = A(q)x + B(q)u + Bww, (17) dt dt mCV 2 d y 2 Fy in which w is the vector of additive disturbance and q is + ω y = , (21) dt2 m the vector of parametric uncertainty, defined according to CV d2z dx F Assumption1 and Assumption2, respectively. In our setup, 2 z 2 + 2ω − 3ω z = . the additive noise term, which is modeled as a random dt dt mCV and bounded model (truncated Gaussian), is related to the Considering only the in-plane motion, here defined by the external environment, in which the experimental tests will be x-z plane, and neglecting the terms (−2ωz˙), (+2ωx˙ − 3ω2z), performed. The uncertainties in the state-space model are due we get double integrators for the translational dynamics to several sources: (i) discrepancies between the mathematical F F model and the actual dynamics of the physical system in x¨ = x z¨ = z . (22) operation, as linearization effects and neglected high-order mCV mCV dynamics; (ii) parametric physical uncertainties, such as mass Furthermore, a double integrator is also considered for the ¨ ¨ and MOI variation due to fuel consumption, characterized by rotational dynamics as θ = τ/Iz, where θ is the angular a uniform distribution. acceleration, τ is the control torque and Iz denotes the MOI In particular, we describe next how linearization introduces about the vertical axis of the chaser FSS. Then, starting important uncertainty sources in the state-space model. The from the definition of the FSS dynamic model, and defining T linearized relative dynamics of the chaser with respect to the the state vector as x = x, y, x˙, y˙ and the contol vector target vehicle during the final approach of the rendezvous  T u = Fx,Fy , a continuous-time linearized model of the maneuver, modeled as two double integrators, has been de- form (17). Then, after discretization, we obtained the following rived by Clohessy and Wiltshire in [29], starting from the discrete-time representation of the FSS uncertain dynamics as nonlinear equations for the restricted three-body problem and considering for the both the spacecraft a reference circular xk+1 = A(qk)xk + B(qk)uk + Bwwk (23) orbit around a master body. Considering the two spacecraft 4 2 masses infinitesimal with respect to the mass of the main where xk ∈ R is the state vector at time k, uk ∈ R is the control input, and w ∈ 4 and q = [q , q , q , q ] ∈ 4 are body (reference planet), we define ρ = ρiρ and r1 = r1 iξ k R k 1 2 3 4 R as the position vectors of the chaser and the target spacecraft the vectors of the additive disturbance and the parametric un- certainty, respectively. In particular, the uncertainty vector q respectively, where iρ and iξ represent the direction main k body-chaser and main body-target, respectively. Then, letting takes into account the linearization errors previously discussed, and the parametric uncertainty due to the mass variation. The r = r iξ the vectorial sum of the two positions, r = ρ + r1, the equations of motion of the chaser spacecraft can be rewritten corresponding continuous uncertain state and control matrices as are   d2ρ dρ ω2r3 q1 0 1 0  0  + 2ω × + ω × [ω × (ρ + )] = − 1 , 0 q 0 1 2×2 2 r1 3 r (18)  1  1 dt dt r A(q) = ,B(q) =  m + q4 0  .  0 2q2 0 0 1 0 + q4 where ω is the orbital angular rate. Note that this differential 0 3q3 −2q2 0 m equation presents nonlinearities due to the term 1/r3. In [29], (24) using a Taylor Series expansion, a linear equation was obtained All the described uncertainty sources were taken into ac- 3 2 2 r1 count in.constructing the linearized state and control matrices by ignoring the high order terms O(ρ /r1), as r3 = 1 − ρ ρ2 defined in (24),. In particular, the parametric uncertainties 3 iξ · iρ r + O( 2 ). That is, Eq. (18) reduces to the linearized 1 r1 q1, q2, q3 take into account linearization effects and are differential equation for the motion of the chaser relative to described as iid random variables with uniform distribu- the target spacecraft as −5 −4 tion: q1 ∼ U[5 × 10 , 5 × 10 ], q2 ∼ U[0.001, 0.0014], 2 −6 −6 d ρ dρ 2 2 2 q3 ∼ U[10 , 1.44 × 10 ], while q4 refers to uncertainty + 2ω × = −ω ζiζ + 3ω ξ(iξ + O(ρ )). (19) −4 dt2 dt in the mass, and is expressed as q4 ∼ U[−0.0091, 10 ]. Furthermore, the system is affected by persistent bounded Ignoring the O(ρ2) and expressing the position vector in a disturbances w ∈ 4, described as a truncated Gaussian with more convenient way as R zero mean value and unitary covariance, bounded in the set .  4 −3 ρ ≡ r = x iθ + z ir − y iy, ir1 = ir ω = −ω iy, (20) W = w ∈ R | kwk∞ ≤ 5 · 10 . IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 9

Focus of this experimental campaign was to investigate the performance of the OS-SMPC algorithm in the control of the translational dynamics of the chaser during the last part of the rendezvous maneuver. Attitude control of the FSS was achieved through a Tube-based Robust MPC (TRMPC) approach, already experimentally validated in [25]. The re- quirement of (deterministic) robust control for the attitude was driven by the physical characteristic of the docking mechanisms, located on both the FSS. Indeed, docking is ensured by an attractive force generated by the magnets on the docking interfaces, which requires a fine alignment of the two vehicles. The TRMPC was hence adopted to align and maintain the FSS pointing at the desired attitude, with respect to the target one. Goal of the translational control is to drive the chaser to the docking position, where the target is located, while guar- anteeing the satisfaction of the typical position and velocity constraints applied to the proximity maneuver. It is important to precise that the x-y coordinate system of the testbed ”coin- x z cides” with the - orbital plane of (22). In particular, the tra- Fig. 4. NPS-POSEIDYN testbed with the cone constraints. The chaser initial jectories should lie in a desired approach cone (see Figure4), condition has to be chosen within the feasible region (light green) whereas i.e. LOS-like constraint, whose polytope vertices are defined the target spacecraft can be located within the feasible terminal region (dark χ = (0, 0), χ = (4, 2.25), χ = (2.25, 4) green). φ defines the cone half-angle, whereas θ represents the chaser FSS as follows: 1 2 3 . The attitude with respect to the testbed reference system. target is located in the suitable terminal region, determined ac- cording to Assumption3. From the state constraint polytopes, linear inequality constraints can be derived. Additionally, the approaching and terminal velocities are bounded according to been performed to find the optimal one able to deal with a soft docking constraints. These constraints on the state are very high number of constraints and compliant with online expressed in terms of chance constraints of the form (2a). implementation and low computational power hardware. Sev- Moreover, the thrusters actuators of the chaser are limited eral solvers have been tested to evaluate their computational by a saturation constraint, according to the maximum thrust capabilities and limitations with respect to embedded imple- available for each cold thruster equipped on the FSS. This is mentation. Moreover, since hardware GNC software running an hard input constraints of the form (2b), that is on the FSS is developed in a MATLAB/Simulink environment, the selection criteria for the solver analyzed was driven by the  2 uk ∈ U = u ∈ R | kuk∞ ≤ 0.3 , compatibility with this environment and available MATLAB interface. The tested solvers were: (i) IBM ILOG CPLEX Op- since at most two thrusters can be fired contemporary in the timizer [30], (ii) Mosek [31], (iii) Gurobi Optimizer [32], (iv) same direction. MATLAB quadprog (v) fastmpc [33], (vi) quadwright,[34]. (i) IBM ILOG CPLEX Optimizer [30], a decision C. Real-time Implementability optimization software developed by IBM which provides In this section, we discuss implementation issues related flexible, high-performance mathematical programming solvers to real-time applicability of the proposed scheme, showing also for quadratic programming problems; (ii) Mosek [31], a how it is indeed possible to envisage the application of an tool for solving mathematical optimization problems such as OS-SMPC in an embedded implementation. As previously convex quadratic problems based on a powerful state-of-the-art discussed, this is due to the offline uncertainty approach, which interior-point optimizer; (iii) Gurobi Optimizer [32], a state- significantly lowers the online computational effort. On the of-the-art solver for mathematical programming, designed other hand, it should be remarked that the computational cost from the ground up to exploit modern architectures and multi- of the proposed OS-SMPC approach is negatively affected core processors, using the most advanced implementations by the possibly high number of constraints involved in the of the latest algorithms, including a quadratic programming optimization problem definition. For this reason, a meticulous solver; (iv) quadprog, the interior-point-convex algorithm an analysis of the solver to be implemented in the embedded provided by the MATLAB Optimization Toolbox to solve microcontroller is still mandatory. quadratic programming problem; (v) fastmpc exploits the To this regard, it should be pointed out that the OS-SMPC structure of the quadratic programming that arise in MPC, proposed in this work was never implemented for real-time obtaining an innovative online optimization tool, based on an applications, and more generally the validation in realistic interior-point method, able to evaluate the control action about simulation environments of scenario programs as well as 100 times than a method that uses a generic optimizer, as sampling-based SMPC approaches [9], [5] is rather limited. presented in [33]; (vi) quadwright, a quadratic programming For this reason, a deep analysis of the available solvers has solver developed by J. Currie at al., presented in [34], able IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 10

−3 to speed up the computational capabilities for embedded α = β = γ = 0.05 and δ = 10 (they should be applications. satisfied with probability of 95% and confidence 99.999%). x u T The ensuing number of samples Ntot = N + N + N is IBM CPLEX and Gurobi are commercial softwares that equal to 32, 370. Then, MPC cost weight matrices were set to provide quite easy MATLAB interfaces, enabling the user Q = diag 104, 104, 108, 108 and R = diag 106, 106 , and access to higher performing state-of-the-art solvers. However, the prediction horizon to T = 10. An appropriately robustly both optimizers are not hardware-driven even if they provide stabilizing feedback gain matrix K was designed offline using embedding methods, and they showed bad memory leaks when classical robust tools. calling the solver many times. Mosek is a tool for solving The main sample times set for the FSS model are reported mathematical optimization problems, and in particular, convex in TableII. The initialization settings introduced here have quadratic problems. The software provides replacements for been adopted both for simulations and experiments, to be some MATLAB functions, including quadprog, and showed a as conservative as possible and obtain comparable results. rather high computational time when facing the large number In particular, the sample time for OS-SMPC has been set of constraints involved in our setup. The MATLAB quad- in compliance with the real-time implementability for the prog gives the possibility to choose between two different experimental validation. approaches: (i) an interior-point-convex method; and (ii) an active-set method. The first algorithm handles only convex problems whereas the second one, identified as trust-region- TABLE II reflective algorithm, is able to manage problems with only MODELINITIALIZATIONSETTINGS. bounds, or only linear equality constraints, but not both. In Parameter Sample Time [s] both cases, MATLAB quadprog showed slower performance Sensors, Actuators, Telemetry 0.01 than Mosek, and moreover it cannot be C-compiled. For what Navigation, G&C 0.02 concerns the fastmpc solver, it has been developed to speed TRMPC, SMPC 5 up MPC computational time and it has been proved to be able to compute in approximately 5ms the control actions for a problem with 12 states, 3 inputs, 30 as prediction horizon and Samples of the uncertainty and of the noise sequence were about 1300 constraints. However, even if the number of states extracted offline and the constraint sets (9), (11), and (12) and inputs was lower for our problem, as well the prediction were derived offline, leading to a total of 956,752 linear horizon is smaller, the much higher number of constraints inequality constraints. Then, an iterative reduction procedure resulted in a degeneration of its performance. was applied leading to a final reduced constraint set of the Our final choice fell on the quadratic programming solver form (13), composed by only 10,125 constraints. Once the first quadwright. This very fast solver, developed with a focus on step constraint (15) has been obtained and intersected with efficient memory use, ease of implementation, and high speed (13). This completed the offline part of the OS-SMPC scheme. convergence, is based on the optimization algorithm proposed in [35]. This approach has been specifically developed to The OS-SMPC algorithm was first validated by MATLAB handle the core problem in MPC, namely control of a linear simulations, and subsequently applied to the NPS-POSEIDYN process with quadratic objectives subject to general linear system. It should be remarked that preliminary simulation inequality constraints. In particular, the algorithm does not results were presented in [36] in which 100 trajectories, each exploit sparsity and it has been refined by pre-factorizing one for a feasible random initial condition (IC), were simu- where possible, using the Cholesky Decomposition factoriza- lated. In this paper, considering the NPS-POSEIDYN setup tion when required, and heuristic for warm start, as reported and the diagonal symmetry of both the granite monolith and in [34]. the cone constraint, the ICs for the OS-SMPC simulated and As described in [28], a real-time operating system (OS) experimental validation were set only in one half of the plane. represents the core of the FSS software architecture and the Thus, three case studies corresponding to three relevant ICs desired real-time requirement is ensured by the adoption of a were chosen due to their peculiarities: (i) the first IC represents Ubuntu 10.04, 32-bit server-edition OS and its Linux kernel the diagonal case, in which the chaser FSS is farthest from the 2.6.33. The multirate GNC software runs atop of this and the cone boundaries (case A); (ii) the second IC is the most critical Simulink model is autocoded into C, compiled and sent from IC, since the FSS is very close to cone constraint (case B); the ground station to the FSS via Wi-Fi, loading the software (iii) the last case represents the halfway condition (case C). on the FSS on-board computer. Each case study was simulated and subsequently experimen- tally reproduced several times, to validate the behavior of the controller. The results obtained are represented in Figure5, IV. SIMULATION AND EXPERIMENTAL RESULTS which depict 20 repetitions for each IC, both for simula- In this section, we present both simulation and experimental tions and experiments. Comparing the simulation trajectories results related to the application of the OS-SMPC scheme (Figure 5(a)) with the experimental ones (Figure 5(b)), we to control the uncertain FSS system dynamics in the last- observed a rather good adherence of the results. In particular, part of the ARVD phase. To this end, we first set the in all experiments the chaser FSS is always driven from the probabilistic parameters of the state chance constraints as IC to the terminal region, where the target FSS is located. IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 11

(a) Simulation Results (a) Simulation Results

(b) Experimental Results

Fig. 5. Simulation and experimental results for 3 different ICs, considering (b) Experimental Results 20 repetitions for each one. Fig. 6. Zoom on the terminal region of both simulation and experimental results. A zoom-in of the terminal region, both for simulations and experiments, is reported in Figure6. We notice a relevant difference between Figure 6(a) and Figure 6(b) with respect to the stopping condition. In the simulations, the chaser stops was not introduced in simulation. when the relative distance with respect to the virtual target Center of Mass (CoM) is lower than a certain threshold (0.18 Once the OS-SMPC scheme has been validated for the real- m). On the other hand, in the experimental setup, the target time implementability point-of-view, the results were analyzed is a real FSS, which is equipped with a female magnetic also with respect to the following performance parameters: docking mechanism. Similarly, the the chaser FSS has a male interface. Hence, the end of the docking phase between the 1) Time-to-dock ttd, defining as the time required to the spacecraft is due to the magnetic force generated between the chaser FSS to reach and dock the target one, starting two magnets. The effects of this force are evident in Figure from the initial condition; 6(b), where trajectories are not funneled as in Figure 6(a) but 2) Control effort fc, an estimate of the fuel consumption they are distributed around the target docking interface. This required for the maneuver, which represents also the discrepancy is mainly due to the fact that the magnetic force efficiency of the control approach from an application IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 12

point-of-view. The control effort can be evaluated as TABLE III COMPARISON OF THE AVERAGE CONTROL EFFORT FOR THREE DIFFERENT t Xtd MPC APPROACHESADOPTEDTOCONTROLTHE FSS DURINGA fc = kukk1∆t, (25) RENDEZVOUSMANEUVERSONTHE NPS-POSEIDYN TESTBED:(I) k=0 LQMPC;(II)TRMPC;(III)OS-SMPC. where ∆t represents the system sample time. MPC approach Control Effort [Ns] Figure7 depicts the control effort for all 60 experiments as LQMPC 4.99 a function of the time-to-dock. As we can notice, in all three TRMPC 14.24 cases the maneuver lasted about 120 − 200s, with an average OS-SMPC 4.69 control effort between 4Ns and 5Ns.

spacecraft is proven and validated on an experimental testbed. Parametric uncertainties due to the mass variations during op- erations, linearization errors, and disturbances due to external space environment are simultaneously considered. The offline sampling approach in the control design phase is shown to reduce the computational cost, which usually constitutes the main limit for the adoption of SMPC schemes, especially for low-cost on-board hardware, and to provide a very effective control in terms of time-to-dock and fuel consumption. These characteristics are demonstrated both through simulations and by means of experimental results.

APPENDIX A QUADRATIC COST MATRIX DEFINITION Simple algebraic manipulations show that the terms in (6) can be written as follows

0 cl cl cl Φ`|k(qk) = A`−1|k(qk)A`−2|k(qk) ··· A0|k(qk), Fig. 7. Control effort with respect to the time-to-dock results for 60 T experiments.  cl cl  A`−1|k(qk) ··· A1|k(qk)B0|k(qk)  .  In order to assess the effectiveness of the proposed OS- Φv (q ) =  .  `|k k   , SMPC approach, the average control effort for all the ex-  B`−1|k(qk)  periments can be compared with the control effort obtained 0n×(T −`)m applying other two MPC approaches validated for the same maneuver and using the same testbed: a LQMPC and a  cl cl T A`−1|k(qk) ··· A1|k(qk)Bw0|k (qk) TRMPC. In particular, in [25], the performance of a robust  .  Φw (q ) =  .  MPC controller has been evaluated and compared with a `|k k   , B (q ) classical LQMPC scheme, both in simulations and on an  w`−1|k k  0 experimental setup. A Linear Matrix Inequalities (LMI) ap- n×(T −`)mw proach is applied to In Table III, the average control effort of the three MPC approaches are reported. We notice that Γ` = [0m×`m Im 0m×(T −`−1)m]. the robust MPC scheme represents the most fuel-consuming approach, with a fuel demand about three times higher than the Then, defining the matrix classical and stochastic MPC, which instead are characterized  0 v w  Φ0|k(qk)Φ0|k(qk)Φ0|k(qk) by comparable fuel consumption, in the order of 5Ns. The .  . . .  fact that OS-SMPC has much lower fuel consumption than ΦT (qk) =  . . .  , TRMPC is somehow surprising, but it can be explained by 0 v w ΦT |k(qk)ΦT |k(qk)ΦT |k(qk) the much lower conservatisms of the stochastic approach. and considering Q¯ = IT ⊗ Q, R¯ = IT ⊗ R, K¯ = IT ⊗ K, and Γ = [0 I 0 ] Q V. CONCLUSIONS defining mT ×n mT mT ×mwT , the two terms, E and R of the explicit cost matrix S˜ An offline sampling-based Stochastic Model Predictive E ˜ Control (OS-SMPC) algorithm is proposed for discrete-time S = E {(QE + RE)} , (26) linear systems subject to both parametric uncertainties and ad- can be written as: ditive disturbances, and its theoretical properties are assessed.  ¯  Real-time implementability of guidance and control strategies T T Q 0nT ×n QE = M ΦT (qk) ΦT (qk)M for automated rendezvous and proximity operations between 0n×nT P IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 13

T T RE = M [K¯ ΦT (qk) + Γ] R¯[K¯ ΦT (qk) + Γ]M Applying Cauchy-Schwarz Inequality first, and then Young Inequality, we have where the matrix M is  2 2 max kAcl(qk)xk + B(qk)vkkPu + kBw(qk)wkkPu   (A(qk),B(qk))∈G, w∈W In 0n×mT 0n×nT 1/2 T 1/2  2 + 2k(Pu (Acl(qk)xk + B(qk)vk)) (Pu Bw(qk)wk)k − kxkkP` M = 0mT ×n mT 0mT ×nT  I   0mwT ×n 0mwT ×mT wImwT ≤ max 2kAcl(qk)xk (A(qk),B(qk))∈G, w∈W 2 2  2 + B(qk)vkkPu + 2kBw(qk)wkkPu − kxkkP`

APPENDIX B  2 ≤ 2 max kAcl(qk)xk + B(qk)vkkPu PROOFTO ASYMPTOTIC BOUND (A(qk),B(qk))∈G

If the candidate solution does not remain feasible, the cost 2 2  + 2maxkBw(qk)wkkPu − kxkkP` . increase can be bounded through the matrices in Assump- w∈W ∗ tion4. Let VT (xk) = JT (xk, v ) be the optimal value of k Let λmin(qk) be a lower bound on the smallest eigenvalue (2) at time k and consider the optimal value function of the of online optimization program as stochastic Lyapunov function. U(q ) = Hence, if the candidate solution v˜ remains feasible, we have k

 2f T 1 2f T  Q − (A(qk) PuA(qk) − P`) − A(qk) PuB(qk) 1−f 2 1−f E {VT (xk+1) | xk, v˜k+1 feasible} − VT (xk)     , 2f T 2f T ≤ {J (x , v˜ ) | x } − V (x ) − B(qk) PuA(qk) R − B(qk) PuB(qk) E T k+1 k+1 k T k 1−f 1−f (T −1 ) (32) X 2 2 2 λ ≤ min ( min λ (U(q ))) ≤ E (kx˜`|k+1kQ + ku˜`|k+1kR) + kx˜T |k+1kP that is min qk∈Q i=1,...,n+m i k . Hence, l=0 applying the law of total probability (T −1 ) X  ∗ 2 ∗ 2 ∗ 2 E VT (xk+1) | xx, v˜T |k+1 − V (xk) − E (kx`|kkQ + ku`|kkR) + kxT |kkP  n o  l=0 ∗ 2 2 2 ≤ (1 − f ) E kBw(qk)wT |kkP − kxkkQ − kukkR n ∗ 2 ∗ ∗ 2 = E kxT |kk T + kAcl(qk)xT |k + Bw(qk)wT |kkP  Q+K RK−P +  2 kA (q )x + B(q )v k2 f max cl k k k k Pu ∗ 2 ∗ 2 o (A(qk),B(qk))∈G − kx0|kkQ − ku0|kkR  + 2 kB (q )w k2 − kx k2 n max w k k Pu k P` ∗ 2 ∗ 2 ∗ 2 w∈W ≤ E kxT |kkQ+KT RK−P + kAcl(qk)xT |kkP + kBw(qk)wT |kkP 2 n ∗ 2 o ∗ T ∗ ∗ 2 ∗ 2 o ≤ − (1 − f )λminkxkk2 + (1 − f )E kBw(qk)wT |kkP + 2(Acl(qk)xT |k) P (Bw(qk)wT |k) − kx0|kkQ − ku0|kkR . + 2 kB (q )w k2 f max w k k Pu w∈W According to the definition of Terminal Set (Assumption 3), 2 1  2 ≤ − kxkk2 + E kBwwkP we obtain λmin 2 n + f kB (q )w k2 ≤ −kx k2 + C. ∗ 2 ∗ 2 max w k k Pu k 2 kx k T T + kB (q )w k w∈ E T |k Q+K RK−P +Acl(qk) PAcl(qk) w k T |k P λmin(1 − f ) W ∗ 2 ∗ 2 o − kx0|kkQ − ku0|kkR n ∗ 2 ∗ 2 ∗ 2 o The final statement follows taking iterated expectations.  ≤ E kBw(qk)wT |kkP − kx0|kkQ − ku0|kkR

n ∗ 2 o 2 2 ≤ E kBw(qk)wT |kkP − kxkkQ − kukkR. REFERENCES [1] D. Mayne, “Model predictive control: Recent developments and future On the other hand, if the candidate solution is not feasible, promise,” Automatica, vol. 50, no. 12, pp. 2967–2986, 2014. we get [2] E. Dreyfus, “The art and theory of dynamic programming,” Tech. Rep., 1977. [3] J. B. Rawlings and D. Q. Mayne, Model predictive control: Theory and  Design. Nob Hill Pub., 2009. E VT (xk+1) | xk, v˜T |k+1 not feasible − VT (xk) [4] B. Kouvaritakis and M. Cannon, Model Predictive Control: Classical, ≤ kA(q )x + B(q )u + B (q )w k2 max k k k k w k k Pu Robust, and Stochastic. Springer, 2015. (A(qk),B(qk))∈G, w∈W [5] M. Lorenzen, F. Dabbene, R. Tempo, and F. Allgower,¨ “Constraint- − kx k2 tightening and stability in stochastic model predictive control,” IEEE k P`  Transactions on Automatic Control, vol. 62, no. 7, pp. 3165–3177, 2017. ≤ kA (q )x + B(q )v k2 [6] J. Grosso, C. Ocampo-Mart´ınez, V. Puig, and B. Joseph, “Chance- max cl k k k k Pu (A(qk),B(qk))∈G, w∈W constrained model predictive control for drinking water networks,” Journal of Process Control, vol. 24, no. 5, pp. 504–516, 2014. + kB (q )w k2 + 2k(P 1/2(A (q )x w k k Pu u cl k k [7] D. V. Hessem and O. Bosgra, “Stochastic closed-loop model predictive  control of continuous nonlinear chemical processes,” Journal of Process + B(q )v ))T (P 1/2B (q )w )k − kx k2 . k k u w k k k P` Control, vol. 16, no. 3, pp. 225–241, 2006. IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 14

[8] A. Bemporad, M. Morari, V. Dua, and E. Pistikopoulos, “The explicit [35] S. Wright, “Applying new optimization algorithms to model predictive linear quadratic regulator for constrained systems,” Automatica, vol. 38, control,” Tech. Rep., 1996. no. 1, pp. 3–20, 2002. [36] M. Mammarella, E. Capello, M. Lorenzen, F. Dabbene, and F. Allgower,¨ [9] M. Lorenzen, F. Dabbene, R. Tempo, and F. Allgower,¨ “Stochastic MPC “A general sampling-based SMPC approach to spacecraft proximity with offline uncertainty sampling,” Automatica, vol. 81, no. 1, pp. 176– operations,” in Proc. of Conference on Decision and Control, 2017. 183, 2017. [10] M. S. Lobo and S. Boyd, “Policies for simultaneous estimation and optimization,” in Proc. of American Control Conference, 1999. [11] D. Bernardini and A. Bemporad, “Stabilizing model predictive control of stochastic constrained linear systems,” IEEE Transactions on Automatic Control, vol. 57, no. 6, pp. 1468–1480, 2012. [12] L. Blackmore, M. Ono, A. Bektassov, and B. Williams, “A probabilistic particle-control approximation of chance-constrained stochastic predic- tive control,” IEEE Transactions on Robotics, vol. 26, no. 3, pp. 502– 517, 2010. [13] A. Mesbah, S. Streif, R. Findeisen, and R. Braatz, “Stochastic nonlinear model predictive control with probabilistic constraints,” in Proc. of American Control Conference, 2014. [14] R. Tempo, G. Calafiore, and F. Dabbene, Randomized Algorithms for Analysis and Control of Uncertain Systems: with Applications. Springer Science & Business Media, 2012. [15] G. C. Calafiore and M. C. Campi, “The scenario approach to robust control design,” IEEE Transactions on Automatic Control, vol. 51, no. 5, pp. 742–753, 2006. [16] G. Calafiore and L. Fagiano, “Robust model predictive control via sce- nario optimization,” IEEE Transactions on Automatic Control, vol. 58, no. 1, pp. 219–224, 2012. [17] ——, “Stochastic model predictive control of LPV systems via scenario optimization,” Automatica, vol. 49, no. 6, pp. 1861–1866, 2013. [18] G. Schildbach, L. Fagiano, C. Frei, and M. Morari, “The scenario approach for stochastic model predictive control with bounds on closed- loop constraint violations,” Automatica, vol. 50, no. 12, pp. 3009–3018, 2014. [19] National Research Council and others, NASA Space Technology Roadmaps and Priorities: Restoring NASA’s Technological Edge and Paving the Way for a New Era in Space. National Academies Press, 2012. [20] D. Zimpfer, P. Kachmar, and S. Tuohy, “Autonomous rendezvous, capture and in-space assembly: past, present and future,” in Proc. of 1st Space exploration conference: continuing the voyage of discovery, 2005. [21] L. Breger and J. How, “Safe trajectories for autonomous rendezvous of spacecraft,” Journal of Guidance, Control, and Dynamics, vol. 31, no. 5, pp. 1478–1489, 2008. [22] E. N. Hartley, “A tutorial on model predictive control for spacecraft rendezvous,” in Proc. of European Control Conference. IEEE, 2015, pp. 1355–1361. [23] Y. Luo, J. Zhang, and G. Tang, “Survey of orbital dynamics and control of space rendezvous,” Chinese Journal of Aeronautics, vol. 27, no. 1, pp. 1–11, 2014. [24] F. Gavilan, R. Vazquez, and E. F. Camacho, “Robust model predictive control for spacecraft rendezvous with online prediction of disturbance bound,” in Proc. of Aerospace Guidance, Navigation and Flight Control Systems AGNFCS’09, 2009. [25] M. Mammarella, E. Capello, H. Park, G. Guglieri, and M. Romano, “Spacecraft proximity operations via tube-based robust model predictive control with additive disturbances,” in Proc. of 68th International Astronautical Congress. International Astronautical Federation, 2017. [26] M. Vidyasagar, Learning and Generalisation: with Applications to Neural Networks. Springer Science & Business Media, 2013. [27] https://www.vicon.com/motion-capture/engineering. [28] R. Zappulla, J. Virgili-Llop, C. Zagaris, H. Park, and M. Romano, “Dy- namic air-bearing hardware-in-the-loop testbed to experimentally evalu- ate autonomous spacecraft proximity maneuvers,” Journal of Spacecraft and Rockets, vol. 54, no. 4, pp. 1–15, 2017. [29] W. Clohessy, “Terminal guidance system for satellite rendezvous,” Journal of the Aerospace Sciences, vol. 27, no. 9, pp. 653–658, 1960. [30] https://www-03.ibm.com/software/products/it/ibmilogcpleoptistud. [31] https://www.mosek.com/products/mosek/. [32] http://www.gurobi.com/products/gurobi-optimizer. [33] Y. Wang and S. Boyd, “Fast model predictive control using online opti- mization,” IEEE Transactions on Control Systems Technology, vol. 18, no. 2, pp. 267–278, 2010. [34] J. Currie, A. Prince-Pike, and D. Wilson, “Auto-code generation for fast embedded model predictive controllers,” in Proc. of International Conference Mechatronics and Machine Vision in Practice, 2012.