1248 MOTE ET AL.

consequence of this, fuel efficiency has become one of the primary constraint-softening approach with the addition of an explicit model performance characteristics for spacecraft. The desire to reduce costs of the collision dynamics formulated in the constraints. In addition to has motivated the development of two alternate navigation modal- remaining feasible in the presence of an inevitable collision, this allows ities. The first is zero-g climbing [13], where the vehicle uses grasp- the vehicle to plan around the collision, all while minimizing a penalty ing contact in the surrounding environment to traverse between function that captures the estimated damage cost. This additional safety locations. A proposed faster and simpler alternative is the hopping measure may offer a particularly useful tool for platforms proposing modality [14,15]. In this case, the vehicle uses a robotic arm to propel autonomous operation in the presence of humans. itself between some fixed handrails. Although this strategy is attrac- The remainder of the paper is outlined as follows: In Sec. II, we tive in the sense that it is completely propellantless, it is also more review the mathematical preliminaries required to develop the main restrictive than the propulsive free-flying strategy in the sense that it results. In Sec. III.A we introduce the assumptions on the spacecraft, requires the precise coordination of a robotic arm and requires hand- and develop basic dynamical constraints. Experimental collision data rails to be present over the operational region. are obtained and used to derive a realistic collision model for the This paper presents a new approach to mobility for assistive space- spacecraft in Sec. III.B. Section IVuses the motion model and vehicle craft: supplementation of propulsive free-flying with planned colli- constraints to specify an optimal strategy for moving between states. In sional contact (bouncing). To the best of our knowledge, this approach Sec. V.Aan experimental case study is described, and the performance to spacecraft mobility has not been explored in the open literature. We of the collision-inclusive algorithm is compared with that of the show that this offers a strategy that is both less restrictive than the collision-free case. It is shown that the proposed method is capable propellant-free approaches, and often more efficient (with respect to a of significantly reducing a chosen objective function. Finally, Sec. V.B given cost function) than its collision-free counterpart. In contrast to explores potential safety applications with a simulated scenario. hopping, where a robotic arm interacts with the environment to provide the energy needed to change the momentum of the spacecraft, bounc- ing achieves similar maneuvers passively though impulsive contact. II. Mixed Integer Programming for Control For example, a spacecraft needing to redirect itself inside a corridor of Hybrid Systems may do so swiftly with a single well-planned collision, rather than Mixed integer programming (MIP) denotes an optimization prob- executing the series of maneuvers needed for coordinated hopping. lem that is composed of both real and integer decision variables. This Because the interaction is passive, bouncing poses very little require- type of problem provides a very general framework for capturing ments on the vehicle or the surrounding environment itself. Hence the many types of practical control objectives. Specifically, the inclusion main challenge stems from the task of developing an effective motion of integer variables allows for the expression of discrete decisions. planning strategy to leverage this capability. Focusing specifically on This makes it naturally well suited to optimizing the actions over the case of small, assistive intravehicular spacecraft, we assume that the systems governed by interdependent dynamic modes, logical state- vehicle operates in the proximity of fixed surfaces with which it may ments, and operational constraints [32]. For our purposes, this is collide, and that both the vehicle and the surface are able to withstand leveraged to optimize trajectories for a spacecraft experiencing low-speed impact. unique dynamic modes encountered during collision and free flight. There is a rich body of work related to impulsive contact in robotics, By modeling this hybrid behavior, integer variables encode the spanning applications such as running [16]; jumping [17]; batting [18]; choice of whether or not to collide. air hockey [19]; and car following [20]. In addition, the problem We consider programs where the objective function J z† is opti- appears in the aerospace context, within landing [21], docking [22], mized over piecewise affine (PWA) constraints, fitting the form grasping [23], and bouncing on planetary bodies [24]. Looking spe- below. cifically at the case of vehicle collisions, there has been foundational work in analyzing the stability and robustness of a colliding vehicle min J z† z [25], designing vehicles that are tolerant to collisions [26], and even s:t: Dczc ‡ Dbzb ≤ g; Aczc ‡ Abzb ˆ h (1) extracting localization information from instances of impact [27]. z ∈ Rnc ; z ∈ f0; 1gnb ; z ˆ‰z ; z Š ∈ Rn Collisions can further be harnessed as a practical means of improving c b c b the effectiveness of trajectories. Through dissipation of energy or m×n m×n m p×n where Dc ∈ R c , Db ∈ R b , g ∈ R , Ac ∈ R c , redirection of momentum, colliding agents are endowed with greater p×n p Ab ∈ R b , and h ∈ R . Although the problem is not convex in maneuverability. One can observe many examples of this phenomenon general, one can in principle compute globally optimal solutions in competitive situations, for example, swimming and parkour, and in whenever J is convex by solving a finite number of convex subpro- nature (e.g., animals pushing off of [28] or jumping between objects). blems [33]. Formulations with linear, quadratic, or second-order cone However, the use of planned impulsive contact explicitly for perfor- objectives are commonly applied in a variety of practical applications mance gains has only recently been considered in the context of robot [12,34–36]. Although in theory these problems are difficult to solve Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 trajectory planning. In [29], the authors use a mixed integer linear [37], solutions can readily be found with good average case perfor- programming (MILP) formulation to derive a time-optimal trajectory mance using off-the-shelf optimization software (e.g., CPLEX [38], incorporating planned collisions for a point mass. In this paper, we use Gurobi [39], MOSEK [40]). these initial results to develop a collision-inclusive, optimal trajectory- MIP allows for the representation of hybrid systems by associating planning formulation for in-plane motion of a free-flying spacecraft. integer variables with the current mode of the system. Specifically, Note that because the overall set of trajectories allowing collisions integer variables (also known as event variables) allow for the direct encompasses all collision-free trajectories as well, the optimal perfor- expression of first-order logic over the constraints. These variables mance with respect to any objective function must either remain the may be assigned a unique value based on the location of the state same or improve when compared with the case where collisions are vector, and in turn be used to relax a different set of constraints over always avoided. the continuous variables. To demonstrate this, let us consider the In addition to performance benefits, collisions may be used to following case, where an inequality condition cT z < d is used to improve the safety of a vehicle in the presence of observed changes activate distinct equality constraints: in the surrounding environment. Intuitively, in situations where colli- sions cannot be avoided, a safest plan of action incorporating the aT z ˆ b if cT z < d collision may be found. Looking specifically at the case of online 0 0 T T (2) model predictive control (MPC), hard collision-avoidance constraints ( a1 z ˆ b1 if c z ≥ d may render the problem infeasible when collisions are unavoidable. n This problem can be addressed by either resorting to a backup con- with z, ai, c ∈ R , bi, d ∈ R, i ˆ 0, 1. The main tools at our disposal troller when the MPC is not feasible [30] or softening the constraints for representing hybrid systems as programs in the form of Eq. (1) (i.e., replacing constraints with penalties in the objective function) come from the lemmas below, which define relationships between such that feasibility is preserved [31]. We extend this prototypical implications and inequalities of real and binary decision variables. MOTE ET AL. 1249

Let ζ ∈ f0; 1g, z ∈ Z ⊂ Rn, and parameters a, c ∈ Rn and b, d ∈ R. ^ F B ˆ OB; i^B; j^B; kB†, with basis vectors i^B and j^B aligned with We then have the following results [41–44]. ^ ^ F R T the orientation of the thrusters, and kB ˆ k. The orientation of B Lemma 1 [32]: Given M ∈ such that maxz∈Z d − c z† < M, with respect to F I is θ, with positive increments in the angle θ the following are equivalent: corresponding to counterclockwise rotations of the spacecraft, as seen from above. This geometry is illustrated in Fig. 2a. The angular i†‰cT z < dŠ ⇒ ‰ζ ˆ 1Š ii† cT z ‡ Mζ ≥ d velocity of the vehicle is ω ˆ θ_ k^. The nominal—that is, collision- ** T free—spacecraft dynamics are then Proof: If maxz∈Z d − c z† < M holds, then the statement (ii) is true for all z ∈ Z when ζ ˆ 1.Givenζ ˆ 0, (ii) is true when cT z < d  holds, and is false otherwise. Thus, the truth values for (ii) are sx ˆ ux; sy ˆ uy; θ ˆ uθ (3) identical to the implication (i) for all assignments. □ R T where u and u are the translational accelerations due to applied Lemma 2 [32]: Given M ∈ such that maxz∈Z c z − d† < M, x y the following are equivalent: thrust, and uθ is the rotational acceleration from an applied moment, which is generated by changes in the reaction wheel speed from a i†‰ζ ˆ 1Š ⇒ ‰cT z < dŠ ii† cT z − M 1 − ζ† < d lower level controller. We assume that the thrust inputs are balanced, such that the applied moment comes entirely from the reaction wheel. T Proof: If maxz∈Z c z − d† < M holds, then (ii) is true for all For the configuration shown in Fig. 2a, the relationship between ζ ˆ 0. Given ζ ˆ 1, (ii) is equivalent to cT z < d. □ translational acceleration and the thrust output from the eight indi- The proofs for the above lemmas may also be made apparent via vidual thrusters is the form of a truth table (see Appendix C). Note that we can apply u 1 cos θ† −sin θ† 0 −11 0 01−10 these together to form an equivalency. Likewise, application to x ˆ u inequalities of opposing sense (in conjunction) extends the result to u m sin θ† cos θ† −100−110 0 1 T  y     the case of equality constraints. (4) Lemma 3 ([45] Sec. 16.4): Given M ∈ R such that max aT z − b† < M, the following are equivalent: 8 z∈Z where m is the mass of the spacecraft and uT ∈ ‰0;uT;maxŠ is the vector of individual thruster output forces. Here u represents the ⇒ T T T T;max i†‰ζ ˆ1Š ‰a zˆbŠ ii†‰a z−M 1−ζ†≤bŠ ∧ ‰a z‡M 1−ζ†≥bŠ maximum force output of a single thruster. T Note that the thruster arrangements on the spacecraft are such that Proof: If maxz∈Z a z − b† < M holds, then (ii) is trivially the maximum accelerations achievable in the i^, j^ directions are satisfied for ζ ˆ 0.Givenζ ˆ 1, (ii) is equivalent to aT z ˆ b. □ functions of the body orientation θ. We can simplify this with the Equivalence then follows from the truth table. use of a conservative inner approximation on the maximum accel- From here we can combine these results to yield a conjunction of eration from thrust umax θ†, which generates a condition that is mixed integer inequalities that is equivalent to Eq. (2) over some uniform (not dependent on orientation) in the inertial frame. specified range on z. Theorem 1 ([45] Sec. 16.4): Given M ∈ R sufficiently large such 2 2 2 T T ux ‡ uy ≤ umax;umax ˆ min umax 膆† (5) that max maxz∈Z jc z − dj†; maxz∈Z a z − b†† < M holds, then θ the system Eq. (2) is equivalent to With the present geometry, we have that umax ˆ 2∕m†uT;max. T T T ‰a0 z−Mζ ≤ b0Š ∧ ‰a1 z−M 1−ζ† ≤ b1Š ∧ ‰c z−Mζ < dŠ T T T B. Collision Model ‰a0 z‡Mζ ≥ b Š ∧ ‰a1 z‡M 1−ζ† ≥ b Š ∧ ‰c z‡M 1−ζ† ≥ dŠ 0 1 To develop a framework for optimizing trajectories that allow In practice, the parameter M should be chosen carefully. Whereas collisions, we must first develop a model for the collision effects values that are too low may not satisfy the above conditions, exces- on the spacecraft. Collisions are generally difficult to understand and sively large values will decrease computational efficiency and may model conceptually, as a first principles analysis requires the consid- introduce numerical error. For notational simplicity, the sequel uses eration of many interacting physical phenomena relating to the geo- the same parameter M in all instances of this method. Note that from a metric, material, and inertial properties of each body involved, many computational viewpoint it is often better to avoid strict inequalities of which are in themselves difficult to model accurately. Many in implementation. This may be accomplished by using a nonstrict approaches have been proposed to model general collision behavior inequality and adding a small number ε to the side with lesser value. over a wide range of scenarios [46,47]. However, because we use a specific pair of objects over a relatively limited range of conditions, Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 we are able to develop an algebraic collision model empirically, by III. Vehicle Description and Constraints directly considering the relationship between the velocities immedi- The system of interest consists of a single free-flying spacecraft in ately before and after the instant of contact with no thrust com- the presence of one or more flat surfaces. The spacecraft is subject to manded. Figure 3 shows the effects of 82 individual collisions for input constraints and is restricted to moving within a plane. The the free-flyer spacecraft and testbed shown in Fig. 1. Within the tested colliding surface is assumed to be fixed and orthogonal to this plane. range, the data suggest that the changes in rotational velocity (Δω), The collision model is developed empirically for the specific use case translational velocity normal to the wall (ΔvN), and tangent to the Δ of operation on the robot and testbed at Stanford’s Space Robotics wall ( vT ) all follow a linear relationship with the precollision − Facility (see Fig. 1). A detailed description of this environment and normal velocity (vN) and relative velocity of the point of contact − the experimental setup is provided in Sec. V.A.1. (vrel). Furthermore, we observe that, for this set of parameters, effects in the normal direction are uncoupled from the tangential and rota- A. Free Flight Dynamics tional effects, leading to the following model: The motion of the spacecraft is expressed in an inertial frame Δv 0 κ F ˆ O; i;^ j;^ k^† with right-handed orthogonal basis vectors i^, j^lying T T v− I Δv ˆ κ 0 N (6) in the plane of motion and k^ ˆ i^× j^. For the testbed shown in Fig. 1, 2 N 3 2 N 3 v− Δω 0 κ  rel  O ^ ^ ω the origin is taken as the lower left corner, i, j lie along the testbed 4 5 4 5 boundary, and k^ points upward. The position of the vehicle’s center O O ^ ^ **The modeling and trajectory planning phases will assume a disturbance- of mass B with respect to is s ˆ sxi ‡ syj and the translational free and deterministic spacecraft model. Noise, parametric uncertainty, and velocity is v ˆ vxi^‡ vyj^. We may also define a body frame errors from approximation are to be addressed through online regulation. 1250 MOTE ET AL.

Fig. 1 Free-flyer spacecraft and testbed with the avoidance region for the experiment in Sec. V.A outlined in adhesive tape.

a) b) Fig. 2 Geometry and conventions for a) free-flyer spacecraft and b) collision with a flat wall S.

0.45 0.15 2.5

0.4 2 0.1 0.35 1.5 1 0.3 0.05 0.5 0.25 0 0 0.2 -0.5 0.15 -0.05 -1 0.1 -1.5 -0.1 0.05 -2

-0.3 -0.2 -0.1 0 -0.5 -0.25 0 0.25 0.5 -0.5 -0.25 0 0.25 0.5

Fig. 3 Observed data from 82 collisions, with linear interpolations taken with respect to the least-squares error. Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 where where the time until collision is

− − ω− vrel ≜ vT ‡ R (7) δ − s− † Δ − N t ˆ − (9) vN κT ; κN; κω†ˆ −0.29; −1.43; −5.0†, and R is the radius of the spacecraft, measured from the outer rim to the rotation center. The coefficients are obtained via a least-squares regression on the Note that the term Δt− introduces a nonlinearity in the tangential and model error. rotational update laws. Making the approximation that collision occurs If we assume that the collision occurs instantaneously, the positions midway through the interval Δt− ˆ 0.5Δt allows us to obtain a linear after the collision can be obtained by integrating the equations of motion form of these equations. The bounds on error from this assumption can with precollision velocities until the point of contact, and postimpact be calculated from the maximum difference between the exact and velocities afterward. Let Δt ≜ Δt− ‡ Δt‡ be the period between the approximated equations, which yields state measurements, and δ be the effective location of the wall along the orthogonal axis (inflated by R,asseeninFig.2b).Thentheexperimental κ κ e ≤ T jv− jΔt; e ˆ 0;e≤ ω jv− jΔt (10) model of Eq. (6) yields the following position update equations: T 2 rel N θ 2 rel

− − − − − ΔsT ˆ 1 ‡ κT †Δtv ‡ κT RΔtω − κT v ‡ Rω †Δt T T where eT , eN, eθ are the errors in the tangential, normal, and angular Δ Δ − − directions, respectively. Note that errors vanish both as jv− j decreases sN ˆ 1 ‡ κN† tvN ‡ κN sN − δ† rel and for finer resolutions Δt. The collision geometry of the free-flyer Δ Δ − Δ − − − Δ − θ ˆ 1 ‡ κωR† tω ‡ κω tvT − κω vT ‡ Rω † t (8) system is illustrated in Fig. 2b. MOTE ET AL. 1251

IV. Problem Formulation B. Representing Dynamics in the Presence of a Single Collision Surface This section formulates the problem of generating optimal trajecto- For notational simplicity, we assume for this case that the basis F S ^ ries for a spacecraft in the presence of 1) an obstacle avoidance region vectors of I are oriented with the wall so that j points away from ^ ^ ^× ^ A,composedofNP convex polygons Pk,withk ∈ f1;:::;NPg, the wall, i is tangent, and k ˆ i j remains pointed upward (see and 2) surfaces S, S (representing half-planes and convex polygons, Fig. 2b). The discrete time equations of motion are given by respectively) with which collisions are permissible. For both and S, S , we will use the convention that the interior of the walls is denoted Axi ‡ Bui if ζi‡1 ˆ 0 xi‡1 − xi ˆ i ˆ 1;:::;τ − 1 (15) R3 A x ‡ b if ζ ˆ 1; by the union of sublevel surfaces of some defined planes in ,and  c i c i‡1 the exterior is the complement of the interior. It is shown that the combined dynamics of the spacecraft, saturation constraints, and where A and B represent the nominal dynamics: obstacle avoidance conditions are all amenable to approximation 0 Δ Δ 2 with piecewise affine constraints. In light of this, we choose to pose 3 I3 t 0.5I3 t A ˆ 0 0 ; B ˆ (16) the trajectory optimization problem as an MIP. We consider the 3 3 I3Δt discrete time approximations of the models developed in previous     sections over a horizon of i ˆ 1; :::;τ. The state of the vehicle at the and Ac and bc represent the collision dynamics: T ith time step is defined as xi ˆ‰sx;i;sy;i; θi;vx;i;vy;i; ωiŠ, and control T vector as u ˆ‰ux;i;uy;i;uθ;iŠ. For completeness, we expound upon i 000 1 ‡ 0.5κT †Δt 0 0.5κT RΔt some basic control and obstacle avoidance constraint formulations Δ found in [34,43,44]. 20 κN 00 1 ‡ κN† t 0 3

60 0 0 0.5κωΔt 0 1 ‡ 0.5κωR†Δt7 6 7 A. Obstacle Avoidance and Saturation Constraints Ac ˆ 6 7; 6000 κT 0 κT R 7 The saturation constraint Eq. (5) can be represented by approxi- 6 7 6 7 mating the Euclidean norm with an NU sided polygon: 6000 0 κN 0 7 6 7 6 7 6000 κω 0 κωR 7 2πn 2πn 6 7 u sin ‡u cos ≤u ;nˆ1;:::;N ;iˆ1;:::;τ 4 5 x;i N y;i N max U 0  U   U  (11) 2−κNδ3 6 0 7 Whereas the approximation improves with the number of sides bc ˆ 6 7 (17) 6 0 7 NU, the added constraints may increase the amount of time required 6 7 6 7 to calculate the solution. The aggregate obstacle avoidance region A 6 0 7 P 6 7 can be constructed from a set of NP convex polygons k: 6 7 6 0 7 6 7 N 4 5 R2 P A ≜ z ∈ j ∨ z ∈ Pk ; The period between steps i and i − 1 is Δt, which is assumed to kˆ1   remain constant over all iterations. P R2 T 0 where; k ≜ fz ∈ jck;qz < dk;q;qˆ 1;:::;NQ;kg (12) Let the wall be located at a distance δ from the origin of the inertial 0 frame F I, and define δ ≜ δ ‡ R, where R is the radius of the R2 R spacecraft, measured from the rotation center to the point of contact. where ck;q ∈ and dk;q ∈ specify the qth side of the kth polygon, Then we can define the wall by the set S ≜ fz ∈ R2jz < δg. The which has NQ;k sides. We can construct the avoidance constraint 2 s ∈= A by defining event variables ψ ∈ f0; 1g such that cT s < occurrence of a collision can be associated with an event variable k;q;i k;q i ζ ∈ f0; 1g. This triggers the switch between the nominal and colli- d ⇒ ψ ˆ 1, and ensuring that the position of the vehicle lies in i k;q k;q;i sion dynamics; it is activated (equal to one) on an iteration i if the the positive end (exterior) of at least one of half-spaces defining the nominal dynamics predict that the vehicle will enter S on that walls of each polygon. This is accomplished with the following iteration, that is, constraints: Δ Δ 2 sy;i ‡ vy;i t ‡ uy;i0.5 t < δ ⇔ ζi‡1 ˆ 1;iˆ 1;:::;τ − 1 Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 N N Q;k Q;k T (18) ∧ ck;qsi ‡ Mψ k;q;i ≥ dk;q ∧ ψ k;q;i ≤ NQ;k − 1; qˆ1 qˆ1 X Using Lemmas 1 and 2, we can express Eq. (18) with the equiv- k ˆ 1;:::;NP;iˆ 1;:::;τ (13) alent set of constraints:

Note that each conjunct is an application of Lemma 1, and the Δ Δ 2 sy;i ‡ vy;i t ‡ uy;i0.5 t ‡ Mζi‡1 ≥ δ summations enforce the condition that there is at least one side q in i ˆ 1;:::;τ − 1 Δ Δ 2 T ( sy;i ‡ vy;i t ‡ uy;i0.5 t − M 1 − ζi‡1† < δ; each polygon such that ck;qsi ≥ dk;q. Example: Rectangular Boundary Let us consider the simplified (19) A P R2 min max case of a rectangle ˆ 1 ˆfz ∈ jz1 ∈ z1 ;z1 †; min max z2 ∈ z2 ;z2 †g. The equivalent MIP constraints for the condition and from Theorem 1, we see that Eq. (15) is equivalent to s ∈= A are

xi‡1 −xi −Axi −Bui ‡MI6ζi‡1 ≥0 min max min −sx ‡Mψ 1 ≥−z1 ∧ sx ‡Mψ 2 ≥ z1 ∧−sy ‡Mψ 1 ≥−z2 xi‡1 −xi −Axi −Bui −MI6ζi‡1 ≤0 8 0 iˆ1;:::;τ−1 (20) 4 >xi‡1 −xi −Acxi −bc ‡MI6 1−ζi‡1†≥ max <> 0 ∧ sy ‡Mψ 2 ≥ z2 ∧ ψ q ≤ 3: (14) xi‡1 −xi −Acxi −bc −MI6 1−ζi‡1†≤ ; qˆ1 X > :Section B of the Appendix provides an example implementation of ⋄ this method for the simple case of an idealized bouncing ball. 1252 MOTE ET AL.

Note that collisions may have the undesirable effect of imparting T T aj s~i ‡ Mγi;j ≥ bj ∧ aj s~i − M 1 − γi;j† < bj; an external moment onto the spacecraft. Although this may be useful for translating stored angular momentum into lateral momentum in i ˆ 2;:::;τ;jˆ 1;:::;NS (25) safety critical scenarios, it could also lead to saturation of the reaction T wheels over time. As such, it may be desirable to minimize momen- which fixes γi;j ˆ 1 exactly when the constraint aj s~i < bj is satisfied ⇔ T tum transfer by constraining the relative velocity of the contact point (i.e., γi;j ˆ 1 aj s~i < bj). We can use constraints of the same form ⇒ to zero at the time of collision: ζi ˆ 1 vrel;i ˆ 0. Equivalently, to indicate the position of the vehicle with respect to the half-spaces from Lemma 3, defined by (αj, βj):

v ‡Rω −M 1−ζ †≤0 ∧ v ‡Rω ‡M 1−ζ †≥0;iˆ2;: : : ; τ T αT x;i i i x;i i i αj si ‡ Mλi;j ≥ βj ∧ j si − M 1 − λi;j† < βj

(21) i ˆ 2;:::;τ;jˆ 1;:::;NS (26)

⇔ αT Note that meeting this condition preserves the initial tangential and which expresses λi;j ˆ 1 j s~i < βj for the appropriate values of angular velocity over the collision. i, j. The equivalencies in Eq. (24) can then be enforced by the constraints C. Representing Dynamics in the Presence of Polygonal Collision

Surfaces NS Ξ At the expense of introducing some complexity, we can generalize γi;p ‡ λi−1;j − λi−1;σ j† − γi−1;j ‡ M 1 − i;j† ≥ NS ‡ 1 collision surfaces S from half-planes to convex polygons: pˆ1 X NS S R2 T Ξ ≜ fz ∈ jaj z < bj;jˆ 1;:::;NS g (22) ∧ γi;p ‡ λi−1;j − λi−1;σ j† − γi−1;j − M i;j ≤ NS (27) pˆ1 X where NS is the number of sides in the polygon and the indices j label the walls in a counterclockwise order. In contrast to the previous case, which are applied for i ˆ 2;:::;τ, j ˆ 1;:::;NS . The dynamics for this system are then the basis vectors i^, j^ are not restricted to a particular orientation. We will assume that the vehicle collides into the jth wall of S on iteration NS Ξ i if 1) the position at time step i − 1 is closest to the boundary of the Axi‡Bui if jˆ1 i‡1;j ≤0 xi‡1 −xi ˆ iˆ1;:::;τ−1 (28) jth wall, and 2) the nominal update equation predicts that the vehicle 8 j j Ξ Acxi‡bc if i‡1P;j ˆ1;jˆ1;:::;NS ; will enter the interior of S on iteration i. It is convenient to represent < the second condition as s~ ≜ A x ‡ B u ∈ S , where A , B j j i s i−1 s i−1 s s where Ac, :bc are the collision dynamics for the jth wall. To are the first two rows of A, B, corresponding to the position update represent these dynamics, let us first define a local frame for the jth under the nominal dynamics. Likewise we can represent the first wall F j ˆ O; t^ ; a^ ; k^† with t^ , a^ pointing tangent and normal to condition as s ∈ C , where C is the region exterior to the polygon, w j j j j i−1 j j ^ ^ × closest to wall j. This can be defined as wall j, and kj ˆ tj a^j upward. Each local frame is a rotation of ^ ^ R3×3 F I ˆ O; i;^ j;^ k† about k by an angle ϕj. Let L3 ϕj† ∈ be the C R2 αT αT T j j ≜ fz ∈ j j z < βj; σ j†z ≥ βσ j†; aj z ≥ bjg (23) rotation matrix converting vectors in F I to vectors in F w. Then we can express the collision dynamics for each wall in F I by rotating the position and velocity vectors to and from this local frame: where σ j† is the jth element of σ ≜ NS ; 1; 2;:::;NS − 1†, and αj, βj define the half-space bisecting wall j and the next wall in the T T j j bj counterclockwise rotation, such that αj z < βj is satisfied for points A ˆ Λ A ΛT b ˆ Λ 0; −κ ; 0; 0; 0; 0 c j c j c j N ka k closer to the jth edge. An example configuration is shown in Fig. 4.  j 2  Our goal is to define event variables Ξ ∈ f0; 1g to indicate the i;j L3 ϕj† 03 occurrence of collisions. Specifically, we want Ξ ˆ 1 when a where; Λ ≜ (29) i;j j 0 collision occurs with the jth wall on iteration i, " 3 L3 ϕj† #

Ξ  i;j ˆ 1 ⇔ s~i ∈ S ∧ si−1 ∈ Cj;iˆ 2;:::;τ;jˆ 1;:::;NS (24) The dynamics in Eq. (28) are then represented by the following Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 MIP constraints: These indicator variables may then be used to activate the collision dynamics for the wall involved in the collision. To indicate the NS x −x −Ax −Bu −M Ξ ≤ 0 position with respect to the walls (a , b ) defining the polygon S , i‡1 i i i i‡1;j j j jˆ1 ! we introduce constraints: X NS ∧ xi‡1 −xi −Axi −Bui ‡M Ξi‡1;j ≥ 0 jˆ1 ! X j j xi‡1 −xi −Acxi −bc −MΞi‡1;j ≤ 0 j j Ξ ∧ xi‡1 −xi −Acxi −bc ‡M i‡1;j ≥ 0;jˆ 1;:::;NS (30)

which are applied at i ˆ 1;:::;τ − 1.

D. Example Objective Functions In practice, the appropriate choice of an objective function depends on the specific needs of the mission. The proposed method- ology does not assume a particular form for the objective. However, because vehicle efficiency is commonly of critical importance to real- Fig. 4 Example geometry for triangular collision polygon S . world missions, we find it useful to review here two common MOTE ET AL. 1253

approximations for penalizing actuation using quadratic and linear Table 1 Free-flyer spacecraft parameters functions. A simple option is to use the power limiting cost Parameter Value Unit function [48,49], Average mass, m 18.08 kg τ 2 2 Radius, R 0.157 m J1 ˆ ux;i ‡ uy;i† (31) Max individual thruster output, uT;max 0.20 N iˆ1 2 X Body inertia about spin axis, Ib 0.184 kg ⋅ m 2 which forms a Mixed Integer Quadratic Program (MIQP). With the Reaction wheel inertia, Iw 0.029 kg ⋅ m introduction of additional constraints, it is also possible to use a PWA Max acceleration of reaction wheel 0.628 rad∕s2 approximation of the Euclidean norm of commanded translational Reaction wheel speed range 60–340 RPM acceleration [34]. The resulting cost function is linear:

τ NJ 2πn 2πn J2 ˆ Gi; s:t: ∧ Gi ≥ux;i sin ‡uy;i cos ;iˆ1;:::;τ nˆ1 introduced in Eq. (31). The performances of the vehicle are compared iˆ1 NJ NJ X     both in terms of this approximation, and a fuel cost measured through (32) pulse width modulation (PWM) signals sent to the thruster. Assum- ing constant mass flow rate through the thrusters, the latter cost is The constraints here approximate the second-order cone con- directly proportional to fuel consumption. The relative velocity straints Gi ≥ k‰ux;i;uy;iŠk2;iˆ 1;:::;τ with an NJ-sided polygon. of the contact point is constrained to zero in order to minimize angular momentum transfer with the wall [Eq. (21)]. A small penalty V. Applications and Case Studies on angular velocity is also included to reduce unnecessary spin of the spacecraft, which, due to the limited update rate of the thrust The potential benefits of the proposed approach are demonstrated controller—approximately 2 Hz—may diminish the accuracy of through two case studies. First, the problem formulation developed in acceleration commands. Sec. IVis validated on hardware. The effectiveness of the approach in The trajectory is generated with Gurobi optimization software improving upon a chosen objective function is studied through a using the formulation in Sec. IV with the parameters listed above. comparison between collision-inclusive and collision-free trajecto- The thrust saturation constraint Eq. (5) uses NU ˆ 20 sides in the ries. Results are tabulated for both ideal and experimental cases. approximation. To ensure that regulation is possible in the presence Next, a simulated example provides a qualitative demonstration of of disturbances, the MIP limits the maximum acceleration (umax)to how the collision-inclusive planner may be applied in safety-critical 90% of its theoretical value. The ideal state is tracked using a linear applications. quadratic regulator (LQR) as the ancillary control law. The net control at time t ∈ R is A. Experimental Performance Comparison   1. Description of Hardware and Testbed u t†ˆu t†‡Klqr x t† − x t†† (33) Experiments were conducted for this work in the Stanford Space 3×6 Robotics Facility on the free-flyer spacecraft robot testbed. A set of where Klqr ˆ‰2.86I3; 14.43I3Š ∈ R is the LQR gain matrix, and robots is designed to hover frictionlessly on air bearings, thus emu- u ∈ R3, x ∈ R6 are the ideal control and state at time t, taken from lating microgravity dynamics in the plane of a table. Though previous a polynomial interpolation of the control and state solutions returned generations of the free-flyer robot used in this experiment operated on from the MIP. The input u is then mapped to PWM signals on the compressed air [50], the current iteration of the free-flyer operates on thrusters u ˆ u ∕u ∈ ‰0; 1Š8. This mapping is derived by ’ pwm T T;max CO2, owing to CO2 s ability to be stored in liquid form at room taking the pseudo-inverse of the mapping from individual thruster temperature at only 1000 psi, resulting in a much higher fuel density forces to forces in the body frame. The resulting mixing equation is than can be achieved at comparable pressures with compressed air. balanced in the body frame, ensuring that no moment is produced The robots are also equipped with actuators commonly used in from the thrusters. An inner PID loop regulates the speed of the spacecraft, namely, a reaction wheel for attitude control and eight reaction wheel, which is used to achieve the desired moment. cold-gas thrusters primarily for translational control. Because of high Experiments are conducted for this scenario with the time horizons capacity of the CO2 tanks, the robots can perform aggressive thrust fixed to 45 and 60 s. The experimental update rate of the controller maneuvers for over an hour and can hover without thrust for over 10 h varies slightly from the fixed 0.5 s period assumed in the planning continuously. phase. As a consequence of this, the 45 s experiments are both The robots use an Odroid XU4 for its primary onboard computa- completed in 82 steps, and the collision-free and collision-inclusive

Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 tion, as well as an mbed Microcontroller for low-level control of 60 s experiments are completed in 108 and 109 steps, respectively. various subsystems. Additionally, the free-flyer software stack is The trajectories taken are shown in Fig. 5, and the efficiency mea- implemented in ROS and is connected to an off-board hub computer, sures are plotted against time in Fig. 6. A video comparison of two where more heavy computation can be run as needed for planning and experiments may be found in [51]. Note that despite having a control. The ROS stack also gives access to real-time data from a significant effect on the total cost, the difference in time allocated motion-capture system, giving position and velocity information at to reach the goal has virtually no effect on the shape of the planned 120 Hz. The granite table used for experiments is 9 0 × 12 0—approx- × — path. Table 2 shows the total costs for each experiment, along with the imately 2.74 m 3.66 m allowing ample room for complex plan- corresponding ideal values, and the resulting PWM costs. It is ning scenarios. Further parameters for the free-flyer robot can be apparent that the collision-inclusive approach is capable of demon- found in Table 1, where average mass is reported due to variations in strating significant improvements in overall efficiency for a given the state of the tanks. time horizon. In particular, we see reductions in the J1 cost of 44.3 and 22.9% for the 45 and 60 s experiments, respectively (compared 2. Performance Comparison with 47.8 and 41.2% for the ideal case), and reductions of 31.7 and The spacecraft and testbed described in the above section are 23.8% in the PWM cost for the 45 and 60 s experiments, respectively. considered with S taken as the lower wall of the testbed and the The main boost in efficiency occurs midway through the trajectory. origin of F I at the lower left corner, as shown in Figs. 1 and 5. As the collision-free vehicle requires increased thrust to reduce its We now compare the performance of vehicles navigating from rest velocity and redirect its momentum, the collision-inclusive approach T at initial position s1 ˆ‰0.41 m; 2.29 mŠ to rest at final position allows the spacecraft to minimize its thrust at this point, gaining the T sτ ˆ‰3.15 m; 2.29 mŠ , while remaining in the boundary of the required momentum transfer directly from an impulsive force at the R2 testbed, and avoiding a central rectangular region P ˆfz ∈ jz1 wall. There appears to be some tradeoff when using this approach ∈ ‰1.45; 2.12Š;z2 ∈ ‰0.57; 2.74Šg. We minimize the cost J1 in that a spike in thrust is seen to occur directly after collision. 1254 MOTE ET AL.

2.5 2

2 0

1.5 -2

1

0.5 2

1 0 3 0123 0 2 0 1

Fig. 5 Comparison of paths taken in experimental scenarios.

2 1.5

1.5 1 1

0.5 0.5

0 5 10 15 20 25 30 35 40 45 0 102030405060

10 -4 10 -4 6 8

6 4 4 2 2

0 0 0 5 10 15 20 25 30 35 40 45 0 102030405060

Fig. 6 Costs vs time for collision-free (red) and collision-inclusive (blue) experiments, and corresponding ideal values (transparent) for the J1 cost.

This might be attributed to a number of factors that could potentially event, and the effects of the collision are considered in the constraints, lead to increased model error on the collision iteration. For example, then the planner can not only remain feasible, but also direct the sensitivity to modeling the precise location of the wall (δ) or vehicle vehicle toward an optimal mitigating action. In this section we turn to radius (R); to precisely matching the commanded tangential and a simulated scenario to demonstrate the potential of the collision- angular velocities at the time of collision; or from the zero thrust inclusive planner to bring about enhanced safety in this sense. approximation made in the update equations. Consider the spacecraft and parameters as described in previous

Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 sections, now given the task of traversing across a larger, more B. Application to Safety-Critical Systems cluttered environment, consisting of a number of walls whose loca- If collision avoidance is posed as a hard constraint in the problem tions are known to it via an internal map. The vehicle also performs online sensing, which it may use to detect unmapped objects in the formulation, then online MPC becomes vulnerable to being rendered environment. Collision with the walls is known to cause minor infeasible in situations where collisions can no longer be avoided. damage to the robot, whereas collision with a newly detected object This situation might arise from a number of factors, including model is considered more damaging, as neither the type of object nor error, external perturbations, or movement of objects in the environ- consequences of hitting it are known in advance. We now compare ment. The problem is exacerbated by the tendency of optimal trajec- the two strategies for this case in the environment shown in Fig. 7. tories to lie near the boundary of the infeasible region (see, e.g., Here the spacecraft (with parameters from previous sections) is given Fig. 5). On the other hand, if the constraint to avoid collisions is the goal of reaching the point in the top right corner while avoiding replaced by a term in the cost function capturing the damage from this obstacles. The vehicle starts on the green trajectory shown in Fig. 7; however, the vehicle eventually detects the presence of a new obstacle Table 2 Experiment cost values (red box) obstructing the original path and is not able to stop in time to prevent collision. If collision avoidance with the obstacles is posed as Experimental Ideal J1 cost, Experimental a hard constraint, then the MPC is rendered infeasible. Without an Specification J cost, m2∕s4 m2∕s4 PWM cost, s 1 update, the vehicle may simply continue on its original course and hit Collision-free, 45 s 0.01822 0.01619 65.04 the object at high velocity. A more thoughtful implementation might Collision-inclusive, 45 s 0.01014 0.00845 44.42 include a backup controller that brings the vehicle to rest as quickly as Collision-free, 60 s 0.00633 0.00611 43.76 Collision-inclusive, 60 s 0.00488 0.00359 33.33 possible; however, even this backup strategy will result in inevitable and uncontrolled collisions with both the wall and obstacle [43]. MOTE ET AL. 1255

0

-0.5

-1

-1.5

-2

3 2 1 4 0 2 0

Fig. 7 Original (green) and updated (red) paths taken to evade observed obstacle (left) [52]; composite trajectory (right).

As an alternative strategy, we may incorporate the presence of the a learning approach may be used to approximate the policy of the various objects in the program through penalties in a multi-objective collision-inclusive backup controller. Once this has been achieved, cost function. The total cost J will be taken as the sum of some nominal the backup strategy may be validated in an experimental scenario. As cost Jnom (e.g., fuel consumption) and a damage cost Jdam.Herewewill an alternate safety application, the enhanced maneuverability of the take the damage cost to be a weighted sum of the speeds at the time of collision-inclusive MPC may be leveraged in a controlled set invari- impact, defined for the jth wall on the ith iteration as follows: ance framework such as the one presented in [53,54]. If the proposed approach is to be applied in orbit, and over longer a impact Ξ j timescales, then one may consider replacing the double-integrator vi;j ≜− i;j‰vx;i;vy;iŠ (34) kajk2 model with Clohessy–Wiltshire–Hill (CWH) dynamics [12,55]. Because this model is linear, the nominal dynamics may be incorpo- Ξ where i;j ˆ 1 indicates collision with wall defined by aj on iteration rated directly into the form presented in Eqs. (15) and (28). In this case, i. Note that impact speed is a quadratic function of the problem one would either need to modify the collision update equations to variables. Letting σ1 be the set of indices for the mapped walls (blue) account for the new dynamics, or for the errors introduced if the and σ2 be the indices for the unmapped walls (red). Then we can perturbation is neglected between the time steps surrounding the express the total cost as J ˆ Jnom ‡ Jdam with collision. A general extension of the planner to the six-degree-of- freedom case may potentially be challenging. However, it is important τ τ to note that for planar (2-D) and spatial (3-D) operations, the coupling impact impact Jdam ≜ K1vi;j ‡ K2vi;j (35) terms between the attitude and translational dynamics disappear during iˆ2 j∈σ1 iˆ2 j∈σ2 collision if the relative velocity at the point of contact goes to zero. X X X X Hence, one could in principle use this approach to plan trajectories for R where K1, K2 ∈ weight the collision penalties for each type of the translational states, and delegate the task of rotating the spacecraft object. For this situation, we specify that K1 is much less than K2, to match the relative velocity of the wall to a separate, lower-level directing to vehicle to avoid the unknown object as much as possible. controller. Similarly, one might simplify the spatial case formulation in The red path in Fig. 7 shows the new trajectory that is calculated using a way that allows some of the translation–attitude coupling to be this cost function once the red box is first detected—that is, accounted leveraged by constraining the robot to orient its rotation axis to be for in the motion planning. Here the spacecraft is able to leverage the orthogonal to the direction of relative velocity on collision iterations. collision dynamics with the blue box to avoid collision with the unknown obstacle altogether. In addition to simply applying thrust to push itself away from the object, the vehicle increases its angular speed VII. Conclusions before the collision and uses stored angular momentum to push itself This paper both introduces and validates the idea of optimizing away on impact. Additional simulation parameters are listed in Sec. A spacecraft trajectories comprising planned collisions. The main theo- Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 of the Appendix. A video of the simulation may be found at [52]. retical contribution consists of formulating the collision-inclusive trajectory planner as a mixed integer programming problem. Ex- VI. Limitations of the Proposed Approach periments comparing the efficiency of collision-free and collision- inclusive trajectories provide a proof-of-concept demonstration of and Future Work the approach’s capability to bring about practical performance The formulation may be further developed through consideration enhancements. Moreover, a simulated case study shows the potential of more complex environments or vehicle geometries, either of which for application of the method as an online safety measure. Though may require the construction of a more complex collision model. The modeling of the collision in the constraints and penalizing a metric of results of the trajectory planning may be sensitive to the accuracy of damage in the cost, the vehicle is able to find novel solutions to the collision model and other parameters. New regulation strategies mitigate scenarios where collisions are inevitable. Tradeoffs appear may be explored to reduce the loss in efficiency in the time samples to be present in the added complexity of the problem and its apparent surrounding the collision, and to make the strategy more robust to sensitivity to model error over iterations surrounding the collisions. parametric uncertainty. The basic framework for optimizing around Future work will focus on generalizing the approach to the spatial collisions may be adapted to applications such as docking or landing, case, developing more robust regulation strategies, and extending the or alternate vehicle platforms such as quad-copters. A key focus of practicality of online safety applications. future research will be to expand on the applications toward safety though the creation and implementation of damage-minimizing backup controllers. Though the proposed framework is capable of Appendix A: Parameters Used in Simulated generating the necessary trajectories, the current implementation of Safety Scenario T the MPC may not be able to generate the aggressive paths quickly The vehicle navigates from the initial state x1 ˆ enough to be practical for online use. To make this more tractable, ‰0.30 m;0.40 m;0.00 rad;0.15 m∕s;0.15 m∕s;0.00 rad∕sŠ, to the final 1256 MOTE ET AL.

Table A1 Location of boxes in safety scenario A x if s ‡ v Δt − 0.5gΔt2 < 0 x − x ˆ c i y;i y;i i‡1 i Ax ‡ b if s ‡ v Δt − 0.5gΔt2 ≥ 0 xmin,m ymin,m xmax,m ymax, m Index  i y;i y;i 1.20 1.40 2.00 2.80 1 2.80 0.50 3.80 1.50 2 where 2.60 2.05 3.50 2.90 3 Δ Δ 3.23 1.65 3.43 1.85 4 κN 1 ‡ κN† t 0 t A ˆ ; A ˆ ; c 0 κ 00  N    2 T −0.5gΔt sy;i state xτ ˆ‰4.60 m;3.40 m;0.00 rad;0.0 m∕s;0.00 m∕s;0.00 rad∕sŠ. b ˆ ; x ˆ −gΔt i v The time resolution of the simulation is 0.06 s. The collision penalty    y;i  weights are K1;K2†ˆ 10; 10e6†. The vehicle is initially com- manded to reach its goal location in 25 s, and the time horizon is and Δt is the time step. Note that, because this model is uncontrolled, appended by 10 s until a feasible solution is found. The collision and the objective function plays no role here. The set of constraints used vehicle parameters are taken as those found for the real vehicle. The to model these dynamics is given as follows. locations of the boxes are listed in Table A1. Indices 1–3 correspond First the condition that collision is predicted to occur on the next to the boxes that were previously mapped out by the object, and box 4 iteration can be associated with the event variable ζ ∈ f0; 1g, leaving Δ Δ 2 is the box that is discovered by the vehicle. This obstacle is observed us with ζi ˆ 1 ⇔ sN;i ‡ vN;i t − 0.5g t < 0. This is transformed at t ˆ 12.37 s from the start of the simulation. to the constraints

Δ Δ 2 Δ Δ 2 sy;i ‡ vy;i t ‡ Mζi‡1 − g0.5 t ≥ 0 ‰sy;i ‡ vy;i t − g0.5 t ≥ 0Š ⇒ ‰ζi‡1 ˆ 1Š† Δ Δ 2 Δ Δ 2 sy;i−1 ‡ vy;i−1 t − M 1 − ζi† − g0.5 t < 0 ‰sy;i ‡ vy;i t − g0.5 t < 0Š‰ζi‡1⇐1Š†

with i ˆ 1;:::;τ − 1. Next, the nominal position and velocity dynamics constraints are

2 2 sy;i‡1 − sy;i − Δtvy;i ‡ Mζi‡1 ≥ 0.5gΔt ‰Δsy;i‡1 ≥ Δtvy;i − 0.5gΔt Š ⇒ ‰ζi‡1 ˆ 0Š† 2 2 sy;i‡1 − sy;i − Δtvy;i − Mζi‡1 ≤ 0.5gΔt ‰Δsy;i‡1 ≤ Δtvy;i − 0.5gΔt Š⇐‰ζi‡1 ˆ 0Š† Δ Δ Δ vy;i‡1 − vy;i ‡ Mζi‡1 ≥−g t ‰ vy;i‡1 ≥−g tŠ ⇒ ‰ζi‡1 ˆ 0Š† Δ Δ Δ vy;i‡1 − vy;i − Mζi‡1 ≤−g t ‰ vy;i‡1 ≤−g tŠ⇐‰ζi‡1 ˆ 0Š† Δ Δ Δ sy;i‡1 − 1 ‡ κN†sy;i − 1 ‡ κN† tvy;i − Mζi‡1 ≥−M ‰ sy;i‡1 ≥ κNsy;i ‡ 1 ‡ κN† tvy;iŠ ⇒ ‰ζi‡1 ˆ 1Š† Δ Δ Δ sy;i‡1 − 1 ‡ κN†sy;i − 1 ‡ κN† tvy;i ‡ Mζi‡1 ≤ M ‰ sy;i‡1 ≤ κNsy;i ‡ 1 ‡ κN† tvy;iŠ⇐‰ζi‡1 ˆ 1Š† Δ vy;i‡1 − 1 ‡ κN†vy;i − Mζi‡1 ≥−M ‰ vy;i‡1 ≥ κNvy;iŠ ⇒ ‰ζi‡1 ˆ 1Š† Δ vy;i‡1 − 1 ‡ κN†vy;i ‡ Mζi‡1 ≤ M ‰ vy;i‡1 ≤ κNvy;iŠ⇐‰ζi‡1 ˆ 1Š†

Appendix B: Example: 1-D Modeling of Bouncing which are applied at i ˆ 1;:::;τ − 1. The system is simulated with Particle with MIP an MIP using Δt ˆ 0.01, and τ ˆ 150. The solution is plotted in Fig. B1. Points found with the collision update equation are shown in Here we consider the simple case of a free-floating particle Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 red, and points found with the nominal update equation are shown being released from rest at s ˆ 1m and pulled down at gra- y in blue. vitational acceleration g ˆ 9.8 m∕s2 until it experiences a collision with the ground at sy ˆ 0. The particle responds to the collision Δ ‡ − according to the update law vy ˆ κNvy , where κN ˆ −1.55 and Δ ‡ − vy , vy indicate the change in velocity, and velocity immediately Appendix C: Truth Tables for Lemmas 1 and 2 before collision, respectively. We can model the motion of the As shown in Table C1, Lemmas 1 and 2 may be conveniently particle with expressed in the form of a truth table. The constant M is assumed to

4 1 2

0.5 0

-2 0 -4 00.511.50 0.5 1 1.5

Fig. B1 Position and velocity of 1-D bouncing particle modeled with MIP. MOTE ET AL. 1257

Table C1 Truth table for Lemmas 1 and 2

cT z < d ζ ˆ 1 cT z ‡ Mζ ≥ d cT z − M 1 − ζ† < d cT z < d ⇒ ζ ˆ 1 ζ ˆ 1 ⇒ cT z < d TTT T T T TFF T F T FTT F T F FFT T T T

have a value satisfying the appropriate assumptions from these [13] Rehnmark, F., Bluethmann, W., Mehling, J., Ambrose, R. O., Diftler, lemmas over the variable domains. M., Chu, M., and Necessary, R., “Robonaut: The ‘Short List’ of Tech- nology Hurdles,” Computer, Vol. 38, No. 1, 2005, pp. 28–37. https://doi.org/10.1109/MC.2005.32 Acknowledgments [14] Virgili-Llop, J., and Romano, M., “ASTROBATICS: Demonstrating This research was supported in part by the National Science Propellantless Robotic Maneuvering Onboard the International Space Station,” Naval Postgraduate School, Tech. Rept., Monterey, CA, 2018, Foundation under CPS award 1931815, award 1544332, by an Early ’ http://hdl.handle.net/10945/58053. Stage Innovations grant from NASA s Space Technology Research [15] Alsup, K. P., “Robotic Spacecraft Hopping: Application And Analysis,” Grants Program, by the King Abdulaziz City for Science and Tech- Naval Postgraduate School, OMB 0704-0188, Monterey, CA, 2018. nology (KACST), and by King Abdullah University of Science and [16] Xi, W., and Remy, C. D., “Optimal Gaits and Motions for Legged Technology (KAUST) baseline fund. Robots,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2014), IEEE, New York, 2014, pp. 3259–3265. https://doi.org/10.1109/iros.2014.6943015 References [17] Giardina, F., and Iida, F., “Efficient and Stable Locomotion for Impulse- [1] Pedersen, L., Kortenkamp, D., Wettergreen, D., Nourbakhsh, I., and Actuated Robots Using Strictly Convex Foot Shapes,” IEEE Trans- Korsmeyer, D., “A Survey of Space Robotics,” 7th International Sym- actions on Robotics, Vol. 34, No. 3, 2018, pp. 674–685. posium on Artificial, Intelligence, Robotics, and Automation in Space, https://doi.org/10.1109/TRO.2018.2813359 Citeseer, 2003. [18] Serra, D., Satici, A. C., Ruggiero, F., Lippiello, V.,and Siciliano, B., “An [2] Fredrickson, S. E., Abbott, L. W., Duran, S., Jochim, J. D., Studak, J. W., Optimal Trajectory Planner for a Robotic Batting Task: The Table Tennis Wagenknecht, J. D., and Williams, N. M., “Mini AERCam: Develop- Example,” International Conference on Informatics in Control, Auto- ment of a Free-Flying Nanosatellite Inspection Robot,” Space Systems mation and Robotics (2), Vol. 105, Elsevier, July 2018, pp. 121–137. Technology and Operations, Vol. 5088, International Soc. for Optics https://doi.org/10.5220/0005982000900101 and Photonics, Society of Photo-Optical Instrumentation Engineers [19] Spong, M. W., “Impact Controllability of an Air Hockey Puck,” Systems (SPIE), 2003, pp. 97–112. and Control Letters, Vol. 42, No. 5, 2001, pp. 333–345. https://doi.org/10.1117/12.498108 https://doi.org/10.1016/S0167-6911(00)00105-5 [3] Williams, T., and Tanygin, S., “On-Orbit Engineering Tests of the [20] Xin, W., Hourdos, J., Michalopoulos, P.,and Davis, G., “The Less-Than- AERCam Sprint Robotic Camera Vehicle,” Spaceflight Mechanics, Perfect Driver: A Model of Collision-Inclusive Car-Following Behavior,” 1998, pp. 1001–1020. Transportation Research Record, Vol. 2088, No. 1, 2008, pp. 126–137. [4] Boyd, I., Buenconsejo, R., Piskorz, D., Lal, B., Crane, K., and https://doi.org/10.3141/2088-14 De La Rosa Blanco, E., “On-Orbit Manufacturing and Assembly of [21] Kiefer, J., Ward, M., and Costello, M., “Rotorcraft Hard Landing Spacecraft,” Inst. for Defense Analysis, IDA Paper P-8335, Alexandria, Mitigation Using Robotic Landing Gear,” Journal of Dynamic Systems, VA, 2017. Measurement, and Control, Vol. 138, No. 3, 2016, Paper 031003. [5] Bylard, A., MacPherson, R., Hockman, B., Cutkosky, M. R., and https://doi.org/10.1115/1.4032286 Pavone, M., “Robust Capture and Deorbit of Rocket Body Debris Using [22] Lee, S. H., Yi, B.-J., Kim, S. H., and Kwak, Y. K., “Analysis on Impact Controllable Dry Adhesion,” Aerospace Conference, IEEE, New York, Propagation of Docking Platform for Spacecraft,” IEEE International 2017, pp. 1–9. Conference on Robotics and Automation, Proceedings 2001 ICRA, https://doi.org/10.1109/aero.2017.7943844 Vol. 1, IEEE, New York, 2001, pp. 413–420. [6] Acquatella, P., “Development of Automation and Robotics in Space https://doi.org/10.1109/robot.2001.932586 Exploration,” Proceedings of the AIAA SPACE 2009 Conference and [23] Estrada, M. A., Hockman, B., Bylard, A., Hawkes, E. W., Cutkosky, Exposition, Citeseer, 2009, pp. 1–7. M. R., and Pavone, M., “Free-flyer Acquisition of Spinning Objects [7] Bualat, M., Barlow, J., Fong, T., Provencher, C., and Smith, T., with Gecko-Inspired Adhesives,” IEEE International Conference “Astrobee: Developing a Free-flying Robot for the International Space on Robotics and Automation (ICRA), IEEE, New York, 2016, Station,” AIAA SPACE 2015 Conference and Exposition, AIAA pp. 4907–4913. Paper 2015-4643, 2015. https://doi.org/10.1109/icra.2016.7487696 https://doi.org/10.2514/6.2015-4643 [24] Hockman, B. J., Frick, A., Reid, R. G., Nesnas, I. A., and Pavone, M., Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788 [8] Miller, D., Saenz-Otero, A., Wertz, J., Chen, A., Berkowski, G., Brodel, “Design, Control, and Experimentation of Internally-Actuated Rovers C., Carlson, S., Carpenter, D., Chen, S., and Cheng, S., et al., for the Exploration of Low-gravity Planetary Bodies,” Journal of Field “SPHERES: A Testbed for Long Duration Satellite Formation Flying Robotics, Vol. 34, No. 1, 2017, pp. 5–24. in Micro-Gravity Conditions,” Proceedings of the AAS/AIAA Space https://doi.org/10.1002/rob.2017.34.issue-1 Flight Mechanics Meeting, Vol. 105, Advances in the Astronautical [25] Posa, M., Tobenkin, M., and Tedrake, R., “Stability Analysis and Control Sciences, Clearwater, FL, Jan. 2000, pp. 167–179. of Rigid-Body Systems with Impacts and Friction,” IEEE Transactions on [9] Jewison, C., and Miller, D. W., “Probabilistic Trajectory Optimization Automatic Control, Vol. 61, No. 6, 2016, pp. 1423–1437. Under Uncertain Path Constraints for Close Proximity Operations,” https://doi.org/10.1109/TAC.2015.2459151 Journal of Guidance, Control, and Dynamics, Vol. 41, No. 9, 2018, [26] Mulgaonkar, Y., Makineni, A., Guerrero-Bonilla, L., and Kumar, V., pp. 1843–1858. “Robust Aerial Robot Swarms Without Collision Avoidance,” IEEE https://doi.org/10.2514/1.G003152 Robotics and Automation Letters, Vol. 3, No. 1, 2018, pp. 596–603. [10] Chamitoff, G. E., Saenz-Otero, A., Katz, J. G., Ulrich, S., Morrell, B. J., https://doi.org/10.1109/LRA.2017.2775699 and Gibbens, P. W., “Real-Time Maneuver Optimization of Space- [27] Mayya, S., Pierpaoli, P., Nair, G., and Egerstedt, M., “Localization in Based Robots in a Dynamic Environment: Theory and On-Orbit Experi- Densely Packed Swarms Using Interrobot Collisions as a Sensing Modal- ments,” Acta Astronautica, Vol. 142, Jan. 2018, pp. 170–183. ity,” IEEE Transactions on Robotics,Vol.35,No.1,2019,pp.21–34. https://doi.org/10.1016/j.actaastro.2017.10.001 https://doi.org/10.1109/TRO.2018.2872285 [11] Morrell, B., “Enhancing 3D Autonomous Navigation Through Obstacle [28] Connell, S., “Crazy Cat [Video],” 2010, https://www.youtube.com/ Fields: Homogeneous Localisation and Mapping, with Obstacle-Aware watch?v=7vak9Vi4e44. Trajectory Optimisation,” Univ. of Sydney, 2018. [29] Mote, M., Afman, J. P., and Feron, E., “Robotic Trajectory [12] LeValley, A. S., “A Mixed Integer Programming Framework for the Fuel Planning Through Collisional Interaction,” 2017 IEEE 56th Annual Optimal Guidance of Complex Spacecraft Rendezvous and Proximity Conference on Decision and Control (CDC), IEEE, New York, 2017, Operation Missions,” Air Force Inst. of Technology, AFIT-ENV-MS- pp. 1144–1149. 19-M-185, Wright-Patterson AFB, OH, 2019. https://doi.org/10.1109/cdc.2017.8263810 1258 MOTE ET AL.

[30] Seto, D., Krogh, B. H., Sha, L., and Chutinan, A., “Dynamic Control [44] Richards, A., and How, J., “Mixed-Integer Programming for Control,” System Upgrade Using the Simplex Architecture,” IEEE Control Proceedings of the 2005 American Control Conference, IEEE, New Systems Magazine, Vol. 18, No. 4, 1998, pp. 72–80. York, 2005, pp. 2676–2683. https://doi.org/10.1109/37.710880 https://doi.org/10.1109/acc.2005.1470372 [31] Kerrigan, E. C., and Maciejowski, J. M., “Soft Constraints and Exact [45] Borrelli, F., Bemporad, A., and Morari, M., Predictive Control for Linear Penalty Functions in Model Predictive Control,” Proceedings of and Hybrid Systems, 1st ed., Cambridge Univ. Press, New York, 2017, UKACC International Conference, 2000. Chap. 2. [32] Bemporad, A., and Morari, M., “Control of Systems Integrating https://doi.org/10.1017/9781139061759 Logic, Dynamics, and Constraints,” Automatica, Vol. 35, No. 3, 1999, [46] Chatterjee, A., Rigid Body Collisions: Some General Considerations, pp. 407–427. New Collision Laws, and Some Experimental Data, Cornell Univ., https://doi.org/10.1016/S0005-1098(98)00178-2 Ithaca, NY, 1997, Chaps. 3–6. [33] Lubin, M., Zadik, I., and Vielma, J. P., Mixed-integer Convex Repre- [47] Cataldo, E., and Sampaio, R., “A Brief Review and a New Treatment for sentability, Lecture Notes in Computer Science, edited by F. Eisenbrand, Rigid Bodies Collision Models,” Journal of the Brazilian Society of and J. Koenemann, Integer Programming and Combinatorial Optimiza- Mechanical Sciences, Vol. 23, No. 1, 2001, pp. 63–78. tion, Vol. 10328, Springer, Cham, 2017, pp. 392–404. https://doi.org/10.1590/S0100-73862001000100006 https://doi.org/10.1007/978-3-319-59250-3_32 [48] Guelman, M., and Aleshin, M., “Optimal Bounded Low-Thrust Ren- [34] Richards, A., Schouwenaars, T., How, J. P., and Feron, E., “Spacecraft dezvous with Fixed Terminal-Approach Direction,” Journal of Guid- Trajectory Planning with Avoidance Constraints Using Mixed-Integer ance, Control, and Dynamics, Vol. 24, No. 2, 2001, pp. 378–385. Linear Programming,” Journal of Guidance, Control, and Dynamics, https://doi.org/10.2514/2.4722 Vol. 25, No. 4, 2002, pp. 755–764. [49] Pardis, C. J., and Carter, T. E., “Optimal Power-Limited Rendezvous https://doi.org/10.2514/2.4943 with Thrust Saturation,” Journal of Guidance, Control, and Dynamics, [35] Mellinger, D., Kushleyev, A., and Kumar, V., “Mixed-Integer Quadratic Vol. 18, No. 5, 1995, pp. 1145–1150. Program Trajectory Generation for Heterogeneous Quadrotor Teams,” https://doi.org/10.2514/3.21517 2012 IEEE International Conference on Robotics and Automation [50] Schneider, S. A., and Cannon, R. H., “Object Impedance Control (ICRA), IEEE, New York, 2012, pp. 477–483. for Cooperative Manipulation: Theory and Experimental Results,” https://doi.org/10.1109/icra.2012.6225009 IEEE Transactions on Robotics and Automation, Vol. 8, No. 3, 1992, [36] Lorenzetti, J., Chen, M., Landry, B., and Pavone, M., “Reach-Avoid pp. 383–394. Games via Mixed-integer Second-order Cone Programming,” 2018 https://doi.org/10.1109/70.143355 IEEE Conference on Decision and Control (CDC), IEEE, New York, [51] Mote, M., Egerstedt, M., Bylard, A., Feron, E., and Pavone, M., 2018, pp. 4409–4416. “Collision-Inclusive Trajectory Optimization for Spacecraft–Experimental https://doi.org/10.1109/cdc.2018.8619382 Comparison,” Jan. 2020. [37] Del Pia, A., Dey, S. S., and Molinaro, M., “Mixed-Integer Quadratic https://doi.org/10.6084/m9.figshare.11720640.v1 Programming Is in NP,” Mathematical Programming, Vol. 162, [52] Mote, M., Feron, E., Bylard, A., Egerstedt, M., and Pavone, M., “Online Nos. 1–2, 2017, pp. 225–240. Safety Through Utilization of Collisions,” Jan. 2020. https://doi.org/10.1007/s10107-016-1036-0 https://doi.org/10.6084/m9.figshare.11720664.v1 [38] Cplex, I. I., “V12. 1: User’s Manual for CPLEX,” International Business [53] Gurriet, T., Mote, M., Ames, A. D., and Feron, E., “An Online Approach Machines Corporation, Vol. 46, No. 53, 2009, p. 157. to Active Set Invariance,” 2018 IEEE Conference on Decision and [39] “Gurobi Optimizer Reference Manual, 2015,” Gurobi Optimization, Control (CDC), IEEE, New York, 2018, pp. 3592–3599. Inc., 2014, http://www.gurobi.com. https://doi.org/10.1109/cdc.2018.8619139 [40] Mosek, A., “The MOSEK Optimization Software,” Vol. 54, Nos. 1–2, [54] Gurriet, T., Mote, M., Singletary, A., Ames, A. D., and Feron, E., “A 2010, p. 5, http://www.mosek.com. Scalable Controlled Set Invariance Framework with Practical Safety [41] Hillier, F. S., Lieberman, G. J., Hillier, F., and Lieberman, G., Introduc- Guarantees,” 2019 IEEE Conference on Decision and Control (CDC), tion to Operations Research, McGraw–Hill, New York,2004, Chap. 12. IEEE, New York, 2019. [42] Morari, M., “Hybrid System Analysis and Control via Mixed Integer [55] Starek, J. A., Schmerling, E., Maher, G. D., Barbee, B. W., and Pavone, Optimization,” IFAC Proceedings Volumes, Vol.34, No. 25, 2001, pp. 1–12. M., “Fast, Safe, Propellant-Efficient Spacecraft Motion Planning Under https://doi.org/10.1016/S1474-6670(17)33796-5 Clohessy–Wiltshire–Hill Dynamics,” Journal of Guidance, Control, [43] Schouwenaars, T., “Safe Trajectory Planning of Autonomous Vehicles,” and Dynamics, Vol. 40, No. 2, 2017, pp. 418–438. Ph.D. Thesis, Massachusetts Inst. of Technology, 2006. https://doi.org/10.2514/1.G001913 Downloaded by STANFORD UNIVERSITY on September 21, 2020 | http://arc.aiaa.org DOI: 10.2514/1.G004788