IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED DEC, 2020 1 Planning with Attitude 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 cal- culus 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 2 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 2 H as a standard vector q 2 R := [qs qv ] 3 T where qs 2 R and qv 2 R are referred to as the scalar q and vector parts of the quaternion, respectively. The space of δq 3 unit quaternions, S = fq : kqk2 = 1g, is a double-cover of the rotation group SO(3), since q and −q represent the same rotation [21]. Quaternion multiplication is defined 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 2 S , the space × 3 and [x] is the skew-symmetric matrix operator of differential rotations lives in R . 2 3 0 −x3 x2 × [x] := x3 0 −x1 : (4) 4 5 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 2 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 φ 2 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 2 3 2 3 q = '(φ) = : (11) r v q 2 φ q 1 q ⊗ !^ = 1 L(q)H! 1 + kφk x = 6 7 ; x_ = 6 2 2 7 (10) 6v7 6 1 WF (x; u) 7 4 5 4 m 5 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 2 3 is the φ = ' (q) = : (12) R qs position, q 2 S3 is the attitude, v 2 R3 is the linear velocity, and ! 2 R3 is the angular velocity. m 2 R is the mass, A.