Planning with Attitude

IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED DEC, 2020 1

Brian E Jackson1, Kevin Tracy1, and Zachary Manchester1

Abstract—Planning trajectories for floating-base robotic sys- satellite formations [8, 1], projection operators [25], or more tems that experience large attitude changes is challenging due general theory for optimization on manifolds [33]. Nearly all to the nontrivial group structure of 3D rotations. This paper of these methods rely heavily on principles from differential introduces a powerful and accessible approach for optimization- based planning on the space of rotations using only standard geometry and Lie group theory; however, despite these works, linear algebra and vector calculus. We demonstrate the ef- many recent papers in the robotics community continue to fectiveness of the approach by adapting Newton’s method to naively apply standard methods for motion planning and solve the canonical Wahba’s problem, and modify the trajectory control with no regard for the group structure of rigid body optimization solver ALTRO to plan directly on the space of motion. unit quaternions, achieving superior convergence on problems involving significant changes in attitude. In this paper, we make a departure from previous approaches to geometric planning and control that rely heavily on ideas Index Terms—Motion and Path Planning, Optimization and and notation from differential geometry, and instead use only Optimal Control, Computational Geometry, Underactuated Robots, Motion Control, basic mathematical tools from linear algebra and vector calculus that should be familiar to most roboticists. In Sec. III we introduce an approach to quaternion differential calculus I.INTRODUCTION similar to [20, 35], but significantly simpler and more general, ANY robotic systems—including quadrotors, air- enabling straight-forward adaptation of existing algorithms to M planes, satellites, autonomous underwater vehicles, and systems with quaternion states. For concreteness, we then quadrupeds—can perform arbitrarily large three-dimensional apply our method to the canonical Wahba’s problem [31] in translations and rotations as part of their normal operation. Sec. IV, and demonstrate superior convergence to approaches While representing translations is straightforward and intuitive, that fail to properly account for the group structure. In Sec. effectively representing the nontrivial group structure of 3D V we extend these ideas to the problem of trajectory opti- rotations has been a topic of study for many decades. Although mization, and detail modifications to ALTRO, a state-of-the- we can intuitively deduce that rotations are three-dimensional, art constrained trajectory optimization solver, and demonstrate a globally non-singular three-parameter representation of the performance gains on several benchmark problems. With the space of rotations does not exist [28]. As a result, when param- modifications presented in this paper, ALTRO explicitly lever- eterizing rotations, we must either a) choose a three-parameter ages both the structure of the trajectory optimization problem representation and deal with singularities and discontinuities, as well as the group structure of 3D rotations, making it or b) choose a higher-dimensional representation and deal with uniquely well-suited to solving challenging problems with near constraints between the parameters. While simply representing real-time performance. attitude is nontrivial, generating and tracking motion plans for In summary, our contributions include: floating-base systems is an even more challenging problem. • A unified approach to quaternion differential calculus Early work on control problems involving the rotation group entirely based on standard linear algebra and vector dates back to the 1970s, with extensions of linear control calculus. theory to spheres [3] and SO(3) [2]. Effective attitude tracking • Derivation of a Newton-based algorithm for nonlinear controllers have been developed for satellites [34], quadrotors optimization directly on the space of unit quaternions. [7, 18, 16, 10, 32, 23], and a 3D inverted pendulum [5] using • Implementation of a fast and efficient solver for trajectory various methods for calculating three-parameter attitude errors. optimization problems with attitude dynamics and non- More recently, these ideas have been extended to trajectory linear constraints that correctly accounts for the group generation [36], sample-based motion planning [37, 14], and structure of 3D rotations. optimal control. Approaches to optimal control with attitude states include analytical methods applied to satellites [27], II.BACKGROUND discrete mechanics [13, 12, 15], a combination of sampling- We begin by defining some useful conventions and notation. based planning and constrained trajectory optimization for Attitude is defined as the rotation from the robot’s body frame to the world frame. We also define gradients to be row vectors, Manuscript received: October, 15, 2020; Revised January 2, 2021; Accepted n ∂f 1×n January 4, 2021. that is, for f(x): R → R, ∂x ∈ R . This paper was recommended for publication by Editor Nancy Amato upon evaluation of the Associate Editor and Reviewers’ comments. This work was supported by a NASA Early Career Faculty Award, NASA JPL, and NSF A. Unit Quaternions GFRP Grant No. DGE-1656518. 1 We leverage the fact that quaternions are linear operators All authors are with the Robotics Institute, Carnegie Mellon University, 4 Pittsburgh, PA, USA [email protected] and that the space of quaternions H is isomorphic to R to Digital Object Identifier (DOI): see top of this page. explicitly represent—following the Hamilton convention—a 2 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED DEC, 2020

4 T T quaternion q ∈ H as a standard vector q ∈ R := [qs qv ] 3 T where qs ∈ R and qv ∈ R are referred to as the scalar q and vector parts of the quaternion, respectively. The space of δq 3 unit quaternions, S = {q : kqk2 = 1}, is a double-cover of the rotation group SO(3), since q and −q represent the same rotation [21]. Quaternion multiplication is deﬁned as

q2 ⊗ q1 = L(q2)q1 = R(q1)q2 (1) O where L(q) and R(q) are orthonormal matrices defined as T qs −qv L(q) := × (2) qv qsI + [qv] T q −q n−1 s v Fig. 1. When linearizing about a point q on an sphere S in n-dimensional R(q) := × , (3) n−1 qv qsI − [qv] space, the tangent space T is a plane living in R , illustrated here with 3 n = 3. Therefore, when linearizing about a unit quaternion q ∈ S , the space × 3 and [x] is the skew-symmetric matrix operator of differential rotations lives in R .   0 −x3 x2 × [x] := x3 0 −x1 . (4)   III.QUATERNION DIFFERENTIAL CALCULUS −x2 x1 0 We now present a simple but powerful method for taking The inverse of a unit quaternion q−1, giving the opposite derivatives of functions involving quaternions based on the rotation, is equal to its conjugate q∗, which is simply the same notation and linear algebraic operations outlined in Sec. II-A. quaternion with a negated vector part: Derivatives consider the effect an infinitesimal perturbation 1 q∗ = T q := q. (5) to the input has on an infinitesimal perturbation to the output. −I3 For vector spaces, the composition of the perturbation with The following identities, which are easily derived from (2)– the nominal value is simple addition and the infinitesimal (5), are extremely useful: perturbation lives in the same space as the original vector. For unit quaternions, however, neither of these are true; L(T q) = L(q)T = L(q)−1 (6) instead, they compose according to (1), and infinitesimal unit T −1 R(T q) = R(q) = R(q) . (7) quaternions are (to first order) confined to a 3-dimensional We will sometimes find it helpful to create a quaternion plane tangent to S3 (see Fig. 1). with zero scalar part from a vector r ∈ R3. We denote this The fact that differential unit quaternions are three- operation as, dimensional should make intuitive sense: Rotations are inher- 0 ently three-dimensional and differential rotations should live rˆ = Hr ≡ r. (8) 3 I3 in the same space as angular velocities, i.e. R . Unit quaternions rotate a vector through the operation rˆ0 = There are many possible three-parameter representations q ⊗ rˆ ⊗ q∗. This can be equivalently expressed using matrix for small rotations in the literature. Many authors use the multiplication as exponential map [2, 36, 15, 25, 26, 6, 33], while others have 0 T T used the Cayley map (also known as Rodrigues parameters) r = H L(q)R(q) Hr = A(q)r, (9) [13, 12], Modified Rodrigues Parameters (MRPs) [29], or where A(q) is the rotation matrix in terms of the elements of the vector part of the quaternion [7]. We choose Rodrigues the quaternion [11]. parameters [21] because they are computationally efficient and do not inherit the sign ambiguity associated with unit B. Rigid Body Dynamics quaternions. The mapping between a vector of Rodrigues For clarity, we will restrict our attention to rigid bodies parameters φ ∈ R3 and a unit quaternion q is known as the moving freely in 3D space. That is, we consider systems with Cayley map: dynamics of the following form: 1 1     q = ϕ(φ) = . (11) r v q 2 φ q 1 q ⊗ ωˆ = 1 L(q)Hω 1 + kφk x =   , x˙ =  2 2  (10) v  1 WF (x, u)     m  We will also make use of the inverse Cayley map: ω J −1(Bτ(x, u) − ω × Jω) −1 qv where x and u are the state and control vectors, r ∈ 3 is the φ = ϕ (q) = . (12) R qs position, q ∈ S3 is the attitude, v ∈ R3 is the linear velocity, and ω ∈ R3 is the angular velocity. m ∈ R is the mass, A. Jacobian of Vector-Valued Functions J ∈ R3×3 is the inertia matrix, WF (x, u) ∈ R3 are the forces in the world frame, and Bτ(x, u) are the moments in the body When taking derivatives with respect to quaternions, we frame. must take into account both the composition rule and the JACKSON et al.: PLANNING WITH ATTITUDE 3

nonlinear mapping between the space of unit quaternions and IV. MODIFYING NEWTON’S METHOD our chosen three-parameter error representation. Newton’s method uses derivative information about a func- 3 Let φ ∈ R be a differential rotation applied to a function tion to iteratively approximate its roots. For unconstrained sys- 3 p with quaternion inputs y = h(q): S → R , such that tems, this method is highly effective, and can exhibit quadratic convergence rates. For constrained systems, the updates can y + δy = h(L(q)ϕ(φ)) ≈ h(q) + ∇h(q)φ. (13) be projected back onto the feasible set at each iteration, but Note that we chose to represent φ in the body frame, consistent without the same convergence guarantees. with the standard definition of angular velocity, and therefore In this section, we will leverage the quaternion calculus it is applied to q through right (rather than left) multiplication. results introduced in the previous section to modify Newton’s p×3 method so that it implicitly accounts for the quaternion unit- We can calculate the Jacobian ∇h(q) ∈ R by differentiat- ing (13) with respect to φ, evaluated at φ = 0: norm constraint. Unlike the projection approach, this modified form of Newton’s method retains the fast convergence rates as- T ∂h ∂h ∂h −qv sociated with the unconstrained method. We will demonstrate ∇h(q) = L(q)H := G(q) = × (14) ∂q ∂q ∂q qsI3 + [qv] this behavior on Wahba’s Problem, a least-squares attitude estimation problem [31, 21]. where G(q) ∈ R4×3 is the attitude Jacobian, which essen- tially becomes a “conversion factor” allowing us to apply A. Methodology results from standard vector calculus to the space of unit W quaternions. This form is particularly useful in practice since Given a set of known vectors in the world frame, wi, and B ∂h/∂q ∈ p×4 can be obtained using finite differences measurements of these vectors in the body frame, vi, we seek R W B or automatic differentiation. As an aside, although we have the rotation from the body to the world frame A(q) that used Rodrigues parameters, G(q) is actually the same (up solves the following optimization problem, to a constant scalar factor) for any choice of three-parameter minimize W (q) attitude representation. q 3 subject to q ∈ S , B. Hessian of Scalar-Valued Functions where Wahba’s loss function W (q) is, If the output of h is a scalar (p = 1), then we can find its X W B 2 2 W (q) = wi − A(q) vi 2 = kr(q)k2, (19) Hessian by taking the Jacobian of (14) with respect to φ using i the product rule, again evaluated at φ = 0: and r(q) is the residual vector.

2 The Jacobian of r(q) can be found using (14): 2 T ∂ h ∂h ∇ h(q) = G(q) G(q) + I3 q, (15) ∂r ∂q2 ∂q ∇r(q) = G(q) (20) ∂q where the second term comes from the second derivative of ! T T X B ϕ(φ). Similar to G(q), this expression is the same (up to = −2H R(q) R( vˆi) G(q). (21) a constant scalar factor) for any choice of three-parameter i attitude representation. Given a guess solution, qk, The standard Gauss-Newton method can then be used to compute a three-parameter update, C. Jacobian of Quaternion-Valued Functions φk via the Moore-Penrose psuedoinverse:

T −1 T We now consider the case of a function that maps unit φk = (∇r ∇r) ∇r r(qk). (22) quaternions to unit quaternions, q0 = f(q): S3 → S3. Here we must also consider the non-trivial effect of a differential The update is then applied using the composition for the group: rotation applied to the output, i.e.: qk+1 = qk ⊗ ϕ(φk). (23) L(q0)ϕ(φ0) = f(L(q)ϕ(φ)). (16) This “multiplicative” Gauss-Newton method is summarized in Algorithm 1. Solving (16) for φ0 we ﬁnd, Algorithm 1 Multiplicative Gauss-Newton Method φ0 = ϕ−1 L(q0)T f(L(q)ϕ(φ)) ≈ ∇f(q) φ. (17) 1: k = 0 Finally, the desired Jacobian is obtained by taking the deriva- 2: while kφk > tolerance do ∂r(qk) tive of (17) with respect to φ: 3: ∇r = ∂q G(qk) . Compute Jacobian T −1 T 4: φ = −(∇r ∇r) ∇r r(qk) . Compute update step T 0 T ∂f 0 T ∂f ∇f(q) = H L(q ) L(q)H = G(q ) G(q). (18) 5: qk+1 = L(qk)ϕ(φ) . Apply update step ∂q ∂q 6: k = k + 1 Once again, (18) holds (up to a constant) for any three- 7: end while parameter attitude representation. 4 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED DEC, 2020

Sec: III, we adapt the algorithm to optimize directly on the 101 error state δx ∈ R12:   −2 rk − r¯k 10 −1 −1 ϕ (q¯k ⊗ qk) δxk =   . (25)  vk − v¯k  10−5 ωk − ω¯k naive modiﬁed We begin by linearizing the dynamics about the reference angle error (degrees) 10−8 state and input trajectories, x¯ and u¯, using (18). The linearized 1 2 3 4 5 error dynamics become, iterations δxk+1 = Akδxk + Bkδuk, (26) Fig. 2. Convergence comparison for Wahba’s problem. The error is the angle between the current solution and the globally optimal solution computed using where a singular-value decomposition. The thick line is the average result of 100 ∂f A = E(¯x )T | E(¯x ), trials with randomized orientations and measurements. The thin lines are the k k+1 ∂x x¯k,u¯k k maximum and minimum over all 100 trials. By modfying Newton’s method (27) with the methods of section III, quadratic convergence rates are achieved, T ∂f Bk = E(¯xk+1) |x¯ ,u¯ , while a na¨ıve approach stalls after only a few iterations. ∂u k k

12×13 and E(xk) ∈ R is the error-state Jacobian: B. Results   I3 Figure 2 compares the multiplicative Gauss-Newton method  G(q)  with a na¨ıve Newton’s method in which the quaternion is E(x) =  . (28)  I3  simply projected back onto the unit sphere at every iteration. I3 The na¨ıve method makes progress initially, but quickly stalls. By correctly handling the group structure of unit quaternions, By applying (14) and (15) to the nonlinear cost functions ` the multiplicative method is able to maintain the fast conver- and (18) to the nonlinear constraint functions gk and hk, we gence rates typical of Newton’s method. By comparing our can calculate the second-order expansion of the cost function: method with the global solution obtained from a singular- value decomposition [22], we see that our method recovers the 1 1 δ`(x, u) ≈ δxT ` δx + δuT ` δu + δT ` δu globally optimal solution within a small number of iterations. 2 xx 2 uu u ux T T T + `x δx + ù δu. (29) V. TRAJECTORY OPTIMIZATIONFOR RIGID BODIES With these results, we can apply standard Newton and quasi- Here we outline the modifications to the ALTRO solver Newton techniques along the lines of Section IV. We can [9] to solve trajectory optimization problems for rigid bodies, also calculate a second-order expansion of the “action-value which extends easily to arbitrary systems whose state is in function” Q(x, u) needed in DDP and LQR-based methods, Rn × S3. We consider trajectory optimization problems of the T form, Qxx = `xx + Ak Pk+1Ak (30) N−1 T X Quu = ùu + Bk Pk+1Bk (31) minimize `f (xN ) + `k(xk, uk) x1:N , u1:N−1 T k=1 Qux = ùx + Bk Pk+1Ak (32) (24) T subject to xk+1 = f(xk, uk), Qx = `x + Ak pk+1 (33) g (x , u ) ≤ 0, T k k k Qu = ù + Bk pk+1, (34) hk(xk, uk) = 0, from which we can calculate the quadratic expansion of the where x and u are the state and control vectors as described in 12×12 12 cost-to-go Pk ∈ R , pk ∈ R , and optimal linear Sec. II-B, f are the dynamics as defined in (10), ` is a general m×12 k feedback gains Kk ∈ R and feed-forward corrections nonlinear cost function at a single time step, N is the number m dk ∈ R by starting at the terminal state and performing a of time steps, and gk, hk are general nonlinear inequality and backward Riccati recursion as usual [17, 9]. equality constraints. During the “forward rollout” of these methods, the dynamics ALTRO combines techniques from both differential dy- are simulated forward in time using updated control inputs: namic programming (DDP) and direct transcription methods to achieve high performance on challenging constrained nonlin- uk =u ¯k − dk − Kkδxk. (35) ear trajectory optimization problems. Like most methods for nonlinear optimization, ALTRO iteratively approximates the where u¯k is the control value from the previous iteration, and nonlinear functions f, `, g, and h with their first or second- δx is computed using (25). For more details on the ALTRO order Taylor series expansions. Leveraging the methods from algorithm, we refer the reader to [9]. JACKSON et al.: PLANNING WITH ATTITUDE 5

A. Quaternion Cost Functions TABLE I TRAJECTORY OPTIMIZATION TIMING RESULTS (NAIVE/MODIFIED) In addition to the straight-forward modifications to the ALTRO algorithm itself, some care must be taken in designing Problem Iterations time (ms) cost functions that are well-suited to unit quaternions. We barrellroll 23 / 17 105.74 / 72.64 frequently minimize costs that penalize distance from a goal quadflip 31 / 25 505.59 / 433.59 1 T state, e.g. 2 (x − xg) Q(x − xg); however, na¨ıve substraction satellite 16 / 17 170.98 / 263.10 of unit quaternions does not respect their group structure, and often results in undesired behavior. Instead, we have found the following cost function, which penalizes the geodesic distance between two unit quaternions [14], to work well in practice: T Jgeo = (1 − qg q ). (36) Its gradient and Hessian are, T T Fig. 3. Barrel roll trajectory computed by ALTRO using a terminal cost to ∇Jgeo = sign(q q)q G(q) (37) d g encourage an upside-down attitude. 2 T T ∇ Jgeo = sign(qd q)I3qg q, (38) where sign denotes the signum function. This cost function issues with integration error and drift in the magnitude of is particularly useful for rotations since it eliminates the the quaternion, the following constraint function was used to ambiguity arising from the quaternion double-cover of SO(3). enforce a terminal orientation of q¯: VI.EXPERIMENTS qv T q − q¯v sign q¯ = 0 (41) In this section we present several trajectory optimization kqk kqk problems for systems that undergo large changes in attitude: The solver was initialized with level flight trim conditions. an airplane barrel roll, a quadrotor flip, and a satellite with The convergence of the different versions of ALTRO is com- flexible solar panels that must slew to a new orientation while pared in Fig. 4. As expected, the modified version achieves avoiding a keep-out zone. All problems are run using ALTRO, better convergence and faster solve times compared to the first without any of the modifications presented in the current na¨ıve version since the expansions being provided to the paper, analagous to the na¨ıve Newton’s method in section IV algorithm more accurately capture the relationship between the and labeled “naive”, and then using the modifications listed in attitude state and the goal and constraints. For this relatively Sec. V and the geodesic cost function described in Sec. V-A, simple problem, we gained a modest 31% improvement in labeled “modified”. All cost functions are of the following runtime, despite the additional matrix multiplications when form: calculating the cost and constraint expansions.

1 T 1 T `naive(x, u, x,¯ Q, R) = (x − x¯) Q(x − x¯) + u Ru 2 2 B. Quadrotor Flip (39) A 360 degree flip trajectory for a quadrotor was optimized ¯ T `modified(x, u, x,¯ Q, R) = `naive(x, u, x,¯ Q, R) + w(1 ± q¯ q) with dynamics adapted from [24]. To encourage the flip, we (40) specified a “waypoint” cost function of the following form: ¯ ~ where x¯ is the reference state and Q = diag(Qr, 04,Qv,Qω), X X `(xk, uk, x,ˆ Q,ˆ R) + `(xk, uk, x¯k,Qw,R) (42) with Qr,Qv,Qω being the weights of Q for position, and linear and angular velocity, respectively. k∈N k∈W Timing results are summarized in Table I. All experiments where xˆ, Qˆ are the nominal state and state weight matrix, −5 are solved to a constraint satisfaction tolerance of 10 and Qw is the weight matrix for the waypoints, and W = discretized with a 4th order Runge-Kutta integrator. The results {20, 45, 51, 55, 75, 101}, N = {1 : 101}\W. Four intermedi- were run on a laptop computer with a 2.8 GHz i7-1165G7 ary “waypoints” were used to encourage the quadrotor to reach processor with 16 GB of RAM. Code for all experiments is angles of 90°, 180°, 270°, and 360° around an approximately available on GitHub1. circular arc. The last waypoint was used to encourage the quadrotor to move towards the final goal, and the first kept A. Airplane Barrel Roll it above the floor before starting the loop. The solver was A 180 degree barrel roll trajectory for a fixed-wing airplane provided a dynamically infeasible initial trajectory that linearly was optimized. The airplane’s dynamics model consists of the interpolates between the initial and final states, rotating the a simple rigid body as defined in Section II-B with forces and quadrotor around the x-axis a full 360°. torques due to lift and drag fit from wind tunnel data [19]. Figure 5 shows snapshots of the trajectory as generated The airplane was tasked to do a barrel roll by constraining using ALTRO. To compare the convergence properties of the the terminal state to upside-down (see Fig. 3). To mitigate two methods, the optimal state and control trajectories were perturbed with random Gaussian white noise with a mean of 1https://github.com/RoboticExplorationLab/PlanningWithAttitude 1 for position, linear velocity, and angular velocity, 0.1 for the 6 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED DEC, 2020

C. Satellite Attitude Keep-Out 100 A spacecraft with ﬂexible appendages was tasked to perform a 150 degree slew maneuver while ensuring that a body- mounted camera did not point within 40 degrees of a “keep- 10−5 naive out zone” around the sun vector. The spacecraft dynamics are modiﬁed presented in detail in [30], and are based on equation (10)

contraint satisfaction with the addition of six states to account for three flexible 0 5 10 15 20 25 modes. Control torques are generated by four reaction wheels. iterations A quadratic cost function penalizes error from the desired final attitude as well as displacement of the flexible modes. Fig. 4. Constraint satisfaction as a function of iteration when solving the We enforce the camera keep-out zone with the following barrel roll problem using ALTRO both with and without the modifications for optimizing unit quaternions. constraint,

W T W BB ◦ rsun A(q) rcam ≤ cos(40 ), (43)

B where rcam is the camera line-of-sight unit vector in the body W frame and rsun is the unit vector pointing to the sun in the world frame. The attitudes that satisfy this constraint comprise a non-convex set, with the constraint itself being nonlinear in q.

3 1 φ Fig. 5. Snapshots of the quadrotor ﬂip trajectory. The gree-colored quadrotors represent the state near t=0 s and the red-colored quadrotors represent the state near t=5.0 s 0 0

0 1 1 2 φ1 controls, and 145 degrees for the orientation (about a random φ2 axis). As shown in Fig. 6, the modified method converges more constraint surface reliably than the na¨ıve method. It is also worth noting that unconstrained this problem could not be solved using any three-parameter constrained attitude representation, since it passes through the singularities at 90◦, 180◦, and 360◦ associated with Euler angles, Rodrigues Fig. 7. Visualization of the flexible spacecraft slew with a keep-out zone. parameters, and Modified Rodrigues Parameters, respectively. Attitude is parameterized with a Rodrigues parameter to visualize the trajectory in three dimensions. The constraint surface represents attitudes where the camera line-of-sight is within 40◦ of the sun. The unconstrained solution violates this constraint, while the constrained solution is able to avoid the keep out zone. 97 ALTRO is able to converge to a locally optimal trajec- 95 tory for this problem without an initial guess (all controls were initialized to zero). The resulting attitude trajectory is 90 depicted in Fig. 7. Without enforcing the camera constraint, the trajectory passes through the keep-out zone. As noted

Percent Success 86 in Table I, the quaternion modifications did not result in 85 a significant improvement over the naive implementation of ALTRO for this problem, indicating that the computational Na¨ıve Modified benefits are problem dependent. We hypothesize that the more Fig. 6. Convergence comparison for quadrotor flip. Percent of 100 trials that dynamic behaviors in the other examples benefit more from successfully converged, where each trial is initialized with locally-optimal the quaternion modifications than the relatively slow-moving trajectories perturbed with significant Gaussian white noise. spacecraft. JACKSON et al.: PLANNING WITH ATTITUDE 7

VII.CONCLUSIONS [2] J. Baillieul. “Geometric Methods for Nonlinear Optimal We have presented a general, unified method for Control Problems”. en. In: Journal of Optimization optimization-based planning and control for rigid-body sys- Theory and Applications 25.4 (Aug. 1978), pp. 519– tems with arbitrary attitude using standard linear algebra 548. ISSN: 0022-3239, 1573-2878. DOI: 10 . 1007 / and vector calculus. The application of this methodology is BF00933518. URL: http://link.springer.com/10.1007/ straightforward and yields substantial improvements in the BF00933518 (visited on 01/07/2020). convergence of Newton and DDP-based methods, while also [3] R. W. Brockett. “Lie Theory and Control Systems offering improvements for nonlinear constrained trajectory Defined on Spheres”. en. In: SIAM Journal on Applied optimization for floating-base systems. Mathematics 25.2 (Sept. 1973), pp. 213–225. ISSN: Many state-of-the-art trajectory optimization methods, in- 0036-1399, 1095-712X. DOI: 10.1137/0125025. URL: cluding direct collocation and sequential convex programming, http://epubs.siam.org/doi/10.1137/0125025 (visited on rely on general-purpose optimization solvers whose internal 01/07/2020). numerical methods are not exposed to the user. Therefore, [4] Jan Brudigam¨ and Zachary Manchester. “Linear-Time these methods are unable to exploit the full structure of both Variational Integrators in Maximal Coordinates”. In: the trajectory optimization problem and the rotation group arXiv (2020). at a low level. In contrast, we are able to implement deep, [5] Nalin A. Chaturvedi, N. Harris McClamroch, and Den- native support for quaternions into the ALTRO solver, making nis S. Bernstein. “Asymptotic Smooth Stabilization of it possible to solve more challenging problems with higher the Inverted 3-D Pendulum”. In: IEEE Transactions on performance than other algorithms. Automatic Control 54.6 (June 2009), pp. 1204–1215. In future work, we plan to apply ALTRO to real-time model- ISSN: 2334-3303. DOI: 10.1109/TAC.2009.2019792. predictive control problems for aerial vehicles like quadrotors [6] Taosha Fan and Todd Murphey. “Online Feedback and airships that experience large attitude changes. The meth- Control for Input-Saturated Robotic Systems on Lie ods we have presented can also be leveraged to adapt other Groups”. en. In: Robotics: Science and Systems XII classes of gradient or Newton-based algorithms to exploit the (2016). DOI: 10 . 15607 / RSS . 2016 . XII . 027. arXiv: structure of 3D rotations. Future directions beyond trajectory 1709 . 00376. URL: http : / / arxiv. org / abs / 1709 . 00376 optimization may include simulation and planning methods (visited on 01/09/2020). that leverage maximal-coordinate formulations of multi-body [7] Emil Fresk and George Nikolakopoulos. “Full Quater- dynamics [4], system-identification for complex multi-body nion Based Attitude Control for a Quadrotor”. In: systems and fixed-wing aircraft in post-stall conditions, and 2013 European Control Conference (ECC). July 2013, state estimation for spacecraft with sparse measurements. pp. 3864–3869. DOI: 10.23919/ECC.2013.6669617. [8] I. Garcia and J.P. How. “Trajectory Optimization for Satellite Reconfiguration Maneuvers with Position and ACKNOWLEDGEMENTS Attitude Constraints”. In: Proceedings of the 2005, This work was supported by a NASA Early Career Faculty American Control Conference, 2005. June 2005, 889– Award (Grant Number 80NSSC18K1503). This research was 894 vol. 2. DOI: 10.1109/ACC.2005.1470072. carried out in part at the Jet Propulsion Laboratory, California [9] Taylor A Howell, Brian E Jackson, and Zachary Manch- Institute of Technology, under a contract with the National ester. “ALTRO: A fast solver for constrained trajectory Aeronautics and Space Administration and funded through optimization”. In: 2019 IEEE International Conference JPL’s Strategic University Research Partnerships (SURP) pro- on Intelligent Robots and Systems, IEEE. 2019. gram. This material is based upon work supported by the [10] Eric N. Johnson and Suresh K. Kannan. “Adaptive National Science Foundation Graduate Research Fellowship Trajectory Control for Autonomous Helicopters”. en. In: Program under Grant No. DGE-1656518. Any opinions, find- Journal of Guidance, Control, and Dynamics 28.3 (May ings, and conclusions or recommendations expressed in this 2005), pp. 524–538. ISSN: 0731-5090, 1533-3884. DOI: material are those of the author(s) and do not necessarily 10.2514/1.6271. URL: https://arc.aiaa.org/doi/10.2514/ reflect the views of the National Science Foundation. 1.6271 (visited on 01/07/2020). [11] Thomas R Kane, Peter W Likins, and David A Levin- REFERENCES son. Spacecraft Dynamics. McGraw Hill, 1983. [1] Georges S. Aoude, Jonathan P. How, and Ian M. Gar- [12] Marin Kobilarov. “Discrete Optimal Control on Lie cia. “Two-Stage Path Planning Approach for Solving Groups and Applications to Robotic Vehicles”. In: 2014 Multiple Spacecraft Reconfiguration Maneuvers”. en. IEEE International Conference on Robotics and Au- In: The Journal of the Astronautical Sciences 56.4 (Dec. tomation (ICRA). May 2014, pp. 5523–5529. DOI: 10. 2008), pp. 515–544. ISSN: 0021-9142. DOI: 10.1007/ 1109/ICRA.2014.6907671. BF03256564. URL: http://link.springer.com/10.1007/ [13] Marin B. Kobilarov and Jerrold E. Marsden. “Discrete BF03256564 (visited on 01/08/2020). Geometric Optimal Control on Lie Groups”. In: IEEE Transactions on Robotics 27.4 (Aug. 2011), pp. 641– 655. ISSN: 1941-0468. DOI: 10 . 1109 / TRO . 2011 . 2139130. 8 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED DEC, 2020

[14] J.J. Kuffner. “Effective Sampling and Distance Metrics Control 58.9 (Sept. 2013), pp. 2230–2245. ISSN: 2334- for 3D Rigid Body Path Planning”. en. In: IEEE Inter- 3303. DOI: 10.1109/TAC.2013.2258817. national Conference on Robotics and Automation, 2004. [26] Joan Sola.` “Quaternion Kinematics for the Error-State Proceedings. ICRA ’04. 2004. New Orleans, LA, USA: Kalman Filter”. en. In: arXiv:1711.02508 [cs] (Nov. IEEE, 2004, 3993–3998 Vol.4. ISBN: 978-0-7803-8232- 2017). arXiv: 1711 . 02508 [cs]. URL: http : / / arxiv. 9. DOI: 10 . 1109 / ROBOT. 2004 . 1308895. URL: http : org/abs/1711.02508 (visited on 09/03/2019). //ieeexplore.ieee.org/document/1308895/ (visited on [27] Karlheinz Spindler. “Optimal Control on Lie Groups 01/08/2020). with Applications to Attitude Control”. en. In: Math- [15] T. Lee, M. Leok, and N. H. McClamroch. “Optimal ematics of Control, Signals, and Systems 11.3 (Sept. Attitude Control of a Rigid Body Using Geometri- 1998), pp. 197–219. ISSN: 0932-4194, 1435-568X. DOI: cally Exact Computations on SO(3)”. en. In: Journal 10.1007/BF02741891. URL: http://link.springer.com/10. of Dynamical and Control Systems 14.4 (Oct. 2008), 1007/BF02741891 (visited on 01/07/2020). pp. 465–487. ISSN: 1079-2724, 1573-8698. DOI: 10 . [28] John Stuelpnagel. “On the parametrization of the three- 1007/s10883- 008- 9047- 7. URL: http://link.springer. dimensional rotation group”. In: SIAM review 6.4 com / 10 . 1007 / s10883 - 008 - 9047 - 7 (visited on (1964), pp. 422–430. 12/19/2019). [29] George Terzakis, Manolis Lourakis, and Djamel Ait- [16] Taeyoung Lee, Melvin Leok, and N Harris McClam- Boudaoud. “Modified Rodrigues Parameters: An Effi- roch. “Geometric tracking control of a quadrotor UAV cient Representation of Orientation in 3D Vision and on SE (3)”. In: 49th IEEE conference on decision and Graphics”. en. In: Journal of Mathematical Imaging and control (CDC). IEEE. 2010, pp. 5420–5425. Vision 60.3 (Mar. 2018), pp. 422–442. ISSN: 0924-9907, [17] Weiwei Li and Emanuel Todorov. “Iterative linear 1573-7683. (Visited on 12/19/2019). quadratic regulator design for nonlinear biological [30] Kevin Tracy and Zachary Manchester. “Model- movement systems.” In: ICINCO (1). 2004, pp. 222– Predictive Attitude Control for Flexible Spacecraft Dur- 229. ing Thruster Firings”. In: AAS/AIAA Astrodynamics [18] Hao Liu, Xiafu Wang, and Yisheng Zhong. Specialist Conference. Lake Tahoe, CA, Aug. 9, 2020. “Quaternion-Based Robust Attitude Control for [31] G. Wahba. “A Least Squares Estimate of Satellite Atti- Uncertain Robotic Quadrotors”. In: IEEE Transactions tude”. In: SIAM Review 7.3 (July 1, 1965), pp. 409–409. on Industrial Informatics 11.2 (Apr. 2015), ISSN: 0036-1445. DOI: 10.1137/1007077. URL: http: pp. 406–415. ISSN: 1941-0050. DOI: 10 . 1109 / //epubs.siam.org/doi/abs/10.1137/1007077. TII.2015.2397878. [32] Michael Watterson and Vijay Kumar. “Control of [19] Zachary Manchester and Scott Kuindersma. quadrotors using the HOPF fibration on SO (3)”. In: “Derivative-Free Trajectory Optimization with Robotics Research. Springer, 2020, pp. 199–215. Unscented Dynamic Programming”. In: IEEE [33] Michael Watterson et al. “Trajectory Optimization On Conference on Decision and Control (CDC). pubs- Manifolds with Applications to SO(3) and R3 × S2.” conference. Las Vegas, NV: IEEE, Dec. 2016. URL: In: Robotics: Science and Systems. 2018. https://rexlab.stanford.edu/papers/udp.pdf. [34] Bong Wie and Peter M Barba. “Quaternion feedback [20] D. P. Mandic, C. Jahanchahi, and C. Cheong Took. for spacecraft large angle maneuvers”. In: Journal of “A Quaternion Gradient Operator and Its Applications”. Guidance, Control, and Dynamics 8.3 (1985), pp. 360– In: IEEE Signal Processing Letters 18.1 (Jan. 2011), 365. pp. 47–50. ISSN: 1558-2361. DOI: 10.1109/LSP.2010. [35] Dongpo Xu, Yili Xia, and Danilo P. Mandic. “Optimiza- 2091126. tion in Quaternion Dynamic Systems: Gradient, Hes- [21] F Landis Markley and John L Crassidis. Fundamen- sian, and Learning Algorithms”. In: IEEE Transactions tals of spacecraft attitude determination and control. on Neural Networks and Learning Systems 27.2 (Feb. Vol. 33. Springer, 2014. 2016), pp. 249–261. ISSN: 2162-2388. DOI: 10.1109/ [22] F Landis Markley and Daniele Mortari. “How to esti- TNNLS.2015.2440473. mate attitude from vector observations”. In: (1999). [36] M. Zefran, V. Kumar, and C.B. Croke. “On the Gen- [23] Daniel Mellinger and Vijay Kumar. “Minimum snap eration of Smooth Three-Dimensional Rigid Body Mo- trajectory generation and control for quadrotors”. In: tions”. In: IEEE Transactions on Robotics and Automa- 2011 IEEE International Conference on Robotics and tion 14.4 (Aug. 1998), pp. 576–589. ISSN: 2374-958X. Automation. IEEE. 2011, pp. 2520–2525. DOI: 10.1109/70.704225. [24] Daniel Mellinger, Nathan Michael, and Vijay Kumar. [37] Milosˇ Zefran,ˇ Vijay Kumar, and Christopher Croke. “Trajectory generation and control for precise aggres- “Metrics and Connections for Rigid-Body Kinematics”. sive maneuvers with quadrotors”. In: The International en. In: The International Journal of Robotics Research Journal of Robotics Research 31.5 (2012), pp. 664–674. 18.2 (Feb. 1999), pp. 242–1. ISSN: 0278-3649. [25] Alessandro Saccon, John Hauser, and A. Pedro Aguiar. “Optimal Control on Lie Groups: The Projection Op- erator Approach”. In: IEEE Transactions on Automatic