2015 IEEE 54th Annual Conference on Decision and Control (CDC) December 15-18, 2015. Osaka, Japan Baumgarte Stabilisation over the SO(3) Rotation Group for Control Sebastien Gros, Marion Zanon, Moritz Diehl Abstract— Representations of the SO(3) rotation group are quaternion and the Direction Cosine Matrix (DCM). For a crucial for airborne and aerospace applications. Euler angles complete survey, we refer to [2]. is a popular representation in many applications, but yield Quaternions are an extension of complex numbers to models having singular dynamics. This issue is addressed via non-singular representations, operating in dimensions higher a four-dimensional space, which allows for describing the than 3. Unit quaternions and the Direction Cosine Matrix are SO(3) rotation group. They can be construed as four- the best known non-singular representations, and favoured in dimensional vectors q R4. The SO(3) rotation group is challenging aeronautic and aerospace applications. All non- 2 4 then covered by q Q := q R q>q = 1 with: singular representations yield invariants in the model dynamics, 2 2 j i.e. a set of nonlinear algebraic conditions that must be fulfilled R(q) = E(q)G(q)> SO(3); by the model initial conditions, and that remain fulfilled over 2 2 3 q1 q0 q3 q2 time. However, due to numerical integration errors, these condi- − − tions tend to become violated when using standard integrators, G(q) = 4 q2 q3 q0 q1 5; − − making the model inconsistent with the physical reality. This q3 q2 q1 q0 issue poses some challenges when non-singular representations 2 − − 3 q1 q0 q3 q2 are deployed in optimal control. In this paper, we propose a − − simple technique to address the issue for classical integration E(q) = 4 q2 q3 q0 q1 5: − − schemes, establish formally its properties, and illustrate it on q3 q2 q1 q0 the optimal control of a satellite. − − Keywords SO(3) rotation group, optimal control, Direction A significant advantage of quaternions over Euler angles Cosine Matrix, quaternions. is that the aforementioned singularity does not occur, such that aircraft or satellite models based on the quaternion I. INTRODUCTION representation do not suffer from the gimbal lock. Systems having a 3 degrees-of-freedoms (DOF) orienta- A sometimes prefered higher-dimensional representation tion are common in the aerospace and aeronautic industries, of the SO(3) rotation group is using the three orthonormal e.g. aircraft and satellite control requires the description of vectors describing the three axis of the object reference frame their 3DOF orientation. The orientation of a body in space given in the fixed reference frame. The 9 numbers providing is described via the group of all rotations about the origin these three orthonormal vectors, if properly organised, yield in the three-dimensional Euclidian space, labeled the SO(3) directly the rotation matrix R SO(3) between the object rotation group [1]. and the fixed reference frame. The2 latter is then labeled the Because they are simple to manipulate in the context Direction Cosine Matrix (DCM). Similarly to the quaternion of basic control theory, Euler angles are often preferred to representation where q is normed to 1, a valid DCM must carry out that description. However, Euler angles suffer from satisfy the orthonormality condition a major defect due to the algebra underlying the tangent R>R I = 0: (1) space to the SO(3) rotation group. Namely, for any choice − ( ) of rotation sequence, there are always elements in SO 3 The DCM representation is easy to handle, and the non- for which the associated tangent space is not spanned via linearity of the model dynamics associated to rotations are variations of the corresponding Euler angles. typically milder than when using Euler-based or quaternion- This difficulty is often referred to as the gimbal lock and based representations. This makes the DCM approach partic- implies that aircraft and satellite models based on Euler ularly attractive in the context of direct optimal control [3], angles are singular for some orientations. In order to avoid [4]. The DCM representation has been used for airborne that problem, higher-dimensional representations need to be applications in e.g. [5], [6], [7], [8], [9], [10]. used. The two most commonly used higher-dimensional While providing non-singular models, the quaternion and ( ) representations of the SO 3 rotation group are the unit DCM-based representation of the SO(3) rotation group S. Gros is with the Department of Signals and Systems, Chalmers present some challenges in control. Long simulations of the University of Technology, Horsalsv¨ agen¨ 11, SE-412 96 Goteborg,¨ Swe- model based on quaternions or on the DCM over a long den, [email protected] . M. Zanon and M. Diehl are with time horizon do not preserve the norm or orthonormality the Systems Control and Optimization Laboratory, Department of Mi- conditions q q = 1 and R R I = 0 respectively, because crosystems Engineering (IMTEK), University of Freiburg, Georges- > > − Koehler-Allee 102, 79110 Freiburg, Germany. M.Diehl is additionally of accumulated numerical error. That is, over time these with the Department of Mathematics of the University of Freiburg. representations of the SO(3) rotation group become invalid. mario.zanon,moritz.diehl @imtek.uni-freiburg.de f * This research was supported byg the EU via ERC-HIGHWIND (259 In the context of direct optimal control, when a long control 166), FP7-ITN-TEMPO (607 957), and H2020-ITN-AWESCO (642 682). horizon is used, this can yield inconsistent or inaccurate 978-1-4799-7886-1/15/$31.00 ©2015 IEEE 620 solutions. Some classes of integrators offer solutions to that such that the quaternion norm is globally driven to 1. For vec- 4 issue [11], [12], [13]. However, these solutions require the tors q R such that q>q = 1, the quaternion dynamics (3) implementation of specific codes which are typically not reduce2 to (2). We additionally observe that the proposed available in optimal control toolboxes. Additionally, to the correction is orthogonal to the quaternion natural dynamics, 1 1 1 best author’s knowledge, the proposed methods all involve i.e.q ˙natural = G(q) w andq ˙correction = rq q q − 1 2 > 2 > − additional computations when compared to standard integra- are orthogonal. Indeed, tion methods. Finally, the computational burden of perform- 1 r − ing the Algorithmic Differentiation [14] of these integrators q˙natural;q˙correction = w>G(q)q q>q 1 = 0 (4) is not established yet, and may be more demanding than for h i 4 − classical integrators. We next provide a similar development for the more intricate In contrast, we propose a trivial (Baumgarte-like [15]) case of the DCM representation. modification of the dynamics associated to both the quater- nions and the DCM in order to address the issue of numerical B. DCM representation drift while using standard integration schemes, such that the In this Section, we present the modification of the dynam- formulation of optimal control problems is straightforward ics associated to the DCM so as to tackle the issues raised and no special care needs to be taken. In this paper, we in the introduction. The dynamics of the DCM are given by: extend the framework developed in [3], propose a less R˙ = RW; (5) computationally expensive DCM stabilisation, a quaternion 3 3 stabilisation, and analyse formally the effect of these modi- where W R × is a skew symmetric matrix stemming from fications. We illustrate their effect on the optimal control of the angular2 velocity w. In principle, dynamics (5) preserve a satellite. the orthonormality condition (1). Indeed, for R>R = I the II.S TABILISATION OF NON-SINGULAR SO(3) following holds: REPRESENTATIONS d R>R I = W>R>R + R>RW = W> + W = 0 (6) In this Section, we present the correction that allows dt − for stabilise the orthonormality conditions arising in the However, because of numerical integration error, a long quaternion and the DCM representation. We start with the simulation of (5) can yield large violations of (1). Let us case of the quaternions, which is straightforward. first provide a Proposition that establishes the behaviour of the orthonormality condition (1) under the dynamics (5) for A. Quaternion representation 3 3 any arbitrary matrix R R × . The quaternion dynamics are given by 2 3 3 Proposition 1: For any matrix R R × , subject to the 1 dynamics (5), the following holds: 2 q˙ = G(q)>w; (2) 2 d 1 2 3 where w R is the angular velocity in the reference frame R>R I = 0: 2 dt 2 − F attached to the rotating body. For any vector q R4, it can be verified that G(q)q = 0 holds, such that the2 quaternion Here the notation : F is used for the Froebinuis norm. norm is an invariant of the dynamics (2), in the sense that Proof: Propositionk k 1 follows from the properties of the it is preserved over time. In particular, if a unit quaternion trace operator, and can be established as follows: vector is provided as initial condition, the unit norm is d 1 2 conserved over the whole system trajectory. Due to numerical R>R I = dt 2 − F integration error, however, when processed numerically the d 1 > dynamics (2) yield quaternion vectors that are not unitary. = Tr R>R I R>R I To tackle that issue, one can introduce a simple correction dt 2 − − in (2), so as to ensure that the quaternion norm is not actually > = Tr R˙>R + R>R˙ R>R I preserved over time, but such that it actually exponentially − converges to 1. In the following, we will refer to this > = Tr R>RW WR>R R>R I correction as a stabilisation of the invariant.