arxiv: v1 [math.oc] 25 Nov 2017

Transcription

1 Constraine Geometric Attitue Control on SO(3) Shankar Kulumani* an Taeyoung Lee November 8, 017 arxiv:17199v1 [math.oc] 5 Nov 017 Abstract This paper presents a new geometric aaptive control system with state inequality constraints for the attitue ynamics of a rigi boy. The control system is esigne such that the esire attitue is asymptotically stabilize, while the controlle attitue trajectory avois unesire regions efine by an inequality constraint. In aition, we evelop an aaptive upate law that enables attitue stabilization in the presence of unknown isturbances. The attitue ynamics an the propose control systems are evelope on the special orthogonal group such that singularities an ambiguities of other attitue parameterizations, such as Euler angles an quaternions are completely avoie. The effectiveness of the propose control system is emonstrate through numerical simulations an experimental results. 1 Introuction Rigi boy attitue control is an important problem for aerospace vehicles, groun an unerwater vehicles, as well as robotic systems [1, ]. One istinctive feature of the attitue ynamics of rigi boies is that it evolves on a nonlinear manifol. The three-imensional special orthogonal group, or SO(3), is the set of 3 3 orthogonal matrices whose eterminant is one. This configuration space is non-eucliean an yiels unique stability properties which are not observable on a linear space. For example, it is impossible to achieve global attitue stabilization using continuous time-invariant feeback [3]. Attitue control is typically stuie using a variety of attitue parameterizations, such as Euler angles or quaternions [4]. Attitue parameterizations fail to represent the nonlinear configuration space both globally an uniquely [5]. For example, minimal attitue representations, such as Euler angle sequences or moifie Roriguez parameters, suffer from singularities. These attitue representations are not suitable for large angular slews. In orer to avoi singularities, the esigner must carefully switch the chosen Euler angle sequence base on the operating region. Another option is to artificially limit the operating region of the rigi boy. This ensures the system operates in a region free from singularities but limits the performance capabilities an ability to perform arbitrarily large angular maneuvers. Quaternions o not have singularities but they ouble cover the special orthogonal group. As a result, any physical attitue is represente by a pair of antipoal quaternions on the three-sphere. An immeiate implication of this ambiguity is that close-loop stability properties erive using quaternions may not hol for the physical rigi boy evolving on the true configuration space, namely the special orthogonal group. During implementation, the esigner must carefully resolve this non-unique representation in quaternion base attitue control systems to avoi unesirable unwining behavior [3]. This behavior is characterize by situations where the rigi boy starts close to the esire attitue, yet the system unnecessarily rotates through a large angle in spite of a small initial error. Many physical rigi boy systems must perform large angular slews in the presence of state constraints. For example, autonomous spacecraft or aerial systems are typically equippe with sensitive optical payloas, such as infrare or interferometric sensors. These systems require retargeting while avoiing irect exposure to sunlight or other bright objects. In aition, many groun base attitue testing environments, such as air bearing platforms, must operate in the presence of physical obstructions. Determining a satisfactory attitue control maneuver in the presence of state constraints is a challenging task. The removal of constraine regions from the rotational configuration space results in a nonconvex region. The attitue control problem in the feasible configuration space has been extensively stuie [6, 7, 8]. However, the attitue control problem in the presence of constraints has receive much less attention.

2 Several approaches have been evelope to treat the attitue control problem in the presence of constraints. A conceptually straightforwar approach is use in [9] to etermine feasible attitue trajectories prior to implementation. The algorithm etermines an intermeiate point such that an unconstraine maneuver can be calculate for each subsegment. Typically, an optimal or easily implementable on-boar control scheme for attitue maneuvers is applie to maneuver the vehicle along these segments. In this manner, it is possible to solve the constraine attitue control problem by linking several intermeiary unconstraine maneuvers. While this metho is conceptually simple, it is ifficult to generalize for an arbitrary number of constraints. In aition, this approach is only applicable to problems where the selection of intermeiate points are computationally feasible. The approach in [10] involves the use of ranomize motion planning algorithms to solve the constraine attitue control problem. A graph is generate consisting of vertices from an initial attitue to a esire attitue. A ranom iterative search is conucte to etermine a path through a irecte graph such that a given cost, which parameterizes the path cost, is minimize. The ranom search approach can only stochastically guarantee attitue convergence as it can be shown that as the number of vertices in the graph grow, the probability of nonconvergence goes to zero. However, the computational eman grows as the size of the graph is increase an a new graph is require when constraints are moifie. As a result, ranom search approaches are ill-suite to on-boar implementation or in scenarios that require agile maneuvers. Moel preictive control for spacecraft attitue ynamics is another popular approach an has been stuie in [11, 1, 13]. These methos rely on linear or non-linear state ynamics to repeately solve a finite-time optimal control problem. Also known as receing horizon control, the optimal control formulation allows for a straight forwar metho to incorporate state an control constraints. Computing the optimal control strategy over a moving horizon allows for a form of feeback type control rather than the more typical open-loop optimal control solution. Due to the iterative nature of solving optimization problems, moel preictive control methos are computational expensive an frequently apply irect optimization methos to solve the necessary conitions for optimality. Therefore, these methos are complicate to implement an may not be suitable for real-time control applications. Artificial potential functions are commonly use to hanle kinematic constraints for a wie range of problems in robotics [14]. The goal is the esign of attractive an repulsive terms which rive the system towar or away from a certain obstacle, respectively. The attractive function is esigne to rive the system towars the esire state. Similarly, a repulsive function is constructe such that the system is irecte away from the constraints. The superposition of these functions allows one to apply stanar feeback control schemes for stabilization an tracking. More specifically, artificial potential functions have previously been applie to the spacecraft attitue control problem in [15, 16]. However, both of these approaches were evelope using attitue parameterizations, namely Euler angles an quaternions, an as such, they are limite by the singularities of minimal representations or the ambiguity of quaternions. This paper is focuse on eveloping an aaptive attitue control scheme in the presence of attitue inequality constraints on SO(3). We apply a potential function base approach evelope irectly on the nonlinear manifol SO(3). By characterizing the attitue both globally an uniquely on SO(3), our approach avois the issues of attitue parameterizations, such as kinematic singularities an ambiguities, an is geometrically exact. A configuration error function on SO(3) with a logarithmic barrier function is propose to avoi constraine regions. Instea of calculating a priori trajectories, as in the ranomize approaches, our approach results in a close-loop attitue control system. This makes it ieal for on-boar implementation on UAV or spacecraft systems. In aition, unlike previous approaches our control system can hanle an arbitrary number of constraine regions without moification. This approach results in a conceptually simple obstacle avoiance scheme which extens the previous work of artificial potential functions on Eucliean spaces to the special orthogonal group. Furthermore, we use this new configuration error function to esign an aaptive upate law to enable attitue convergence in the presence of uncertain isturbances. The stability of the propose control systems is verifie via mathematically rigorous Lyapunov analysis on SO(3). In short, the propose attitue control system in the presence of inequality constraints is geometrically exact, computationally efficient an able to hanle uncertain isturbances. The effectiveness of this approach is illustrate via numerical simulation an emonstrate via experimental results.

3 Problem Formulation.1 Attitue Dynamics Consier the attitue ynamics of a rigi boy. We efine an inertial reference frame an a boy-fixe frame, whose origin is at the center of mass an aligne with the principle irections of the boy. The stanar orthonormal basis is enote by e i for i {1,, 3}. The configuration manifol of the attitue ynamics is the special orthogonal group: SO(3) = {R R 3 3 R T R = I, et[r] = 1}, where a rotation matrix R SO(3) represents the transformation of the representation of a vector from the boy-fixe frame to the inertial reference frame. The equations of motion are given by J Ω + Ω JΩ = u + W (R, Ω), (1) Ṙ = RˆΩ, () where J R 3 3 is the inertia matrix, an Ω R 3 is the angular velocity represente with respect to the boyfixe frame. The control moment is enote by u R 3, an it is expresse with respect to the boy-fixe frame. We assume that the external isturbance is expresse by W (R, Ω), where W (R, Ω) : SO(3) R 3 R 3 p is a known function of the attitue an the angular velocity. The isturbance is represente by R p an is an unknown, but fixe uncertain parameter. In aition, we assume that a boun on W (R, Ω) an is known an given by W B W, B, (3) for positive constants B W, B. This form of uncertainty enters the system ynamics through the input channel an as a result is referre to as a matche uncertainty. While this form of uncertainty is easier than the unmatche variety, many physically realizable isturbances may be moele in this manner. For example, orbital spacecraft are subject to gravity graient torques cause by the non-spherical istribution of mass of both the spacecraft an central gravitational boy. This form of isturbance may be represente as a boy fixe torque on the vehicle. In aition, for typical scenarios, where the spacecraft is significantly smaller than the orbital raius, the isturbance torque may be assume constant over short time intervals. Another terrestrial example is frequently encountere by unmanne aerial vehicles. Multiple actuator systems, such as quarotor aerial vehicles, may exhibit an uneven mass istribution uring loa transportation or must operate in the presence of turbulence. Treating these effects as isturbances is a popular metho to esign control systems. In (), the hat map : R 3 so(3) represents the transformation of a vector in R 3 to a 3 3 skew-symmetric matrix such that ˆxy = x y for any x, y R 3 [6]. More explicitly, 0 x 3 x ˆx = x 3 0 x 1, x x 1 0 for x = [x 1, x, x 3 ] T R 3. The inverse of the hat map is enote by the vee map : so(3) R 3. We use the notation ˆx an (x) interchangeably. In particular, we use the latter form when the expression for the argument of the hat map is complicate. Several properties of the hat map are summarize as ˆxy = x y = y x = ŷx, (4) x ŷz = y ẑx, ˆxŷz = (x z)y (x y)z, (5) x y = ˆxŷ ŷˆx = yx T xy T, (6) tr[aˆx] = 1 tr[ˆx(a A T ) ] = x T (A A T ), (7) ˆxA + A T ˆx = ({tr[a] I 3 3 A} x), (8) RˆxR T = (Rx), R(x y) = Rx Ry (9)

4 for any x, y, z R 3, A R 3 3 an R SO(3). Throughout this paper, the ot prouct of two vectors is enote by x y = x T y for any x, y R n an the maximum eigenvalue an the minimum eigenvalue of J are enote by λ M an λ m, respectively. The -norm of a matrix A is enote by A, an its Frobenius norm is enote by A A F = tr[a T A] rank(a) A.. State Inequality Constraint The two-sphere is the manifol of unit-vectors in R 3, i.e., S = {q R 3 q = 1}. We efine r S to be a unit vector from the mass center of the rigi boy along a certain irection an it is represente with respect to the boy-fixe frame. For example, r may represent the pointing irection of an on-boar optical sensor. We efine v S to be a unit vector from the mass center of the rigi boy towar an unesire pointing irection an represente in the inertial reference frame. For example, v may represent the inertial irection of a bright celestial object or the incoming irection of particles or other ebris. It is further assume that optical sensor has a strict non-exposure constraint with respect to the celestial object. We formulate this har constraint as r T R T v cos θ, (10) where we assume 0 θ 90 is the require minimum angular separation between r an R T v. The objective is to a etermine a control input u that stabilizes the system from an initial attitue R 0 to a esire attitue R while ensuring that (10) is always satisfie. 3 Attitue Control on SO(3) with Inequality Constraints The first step in esigning a control system on a nonlinear manifol Q is the selection of a proper configuration error function. This configuration error function, Ψ : Q Q R, is a smooth an proper positive efinite function that measures the error between the current configuration an a esire configuration. Once an appropriate configuration error function is chosen, one can then efine a configuration error vector an a velocity error vector in the tangent space T q Q through the erivatives of Ψ [6]. With the configuration error function an vectors, the remaining proceure is analogous to nonlinear control esign on Eucliean vector spaces. One chooses control inputs as functions of the state through a Lyapunov analysis on Q. To hanle the attitue inequality constraint, we propose a new attitue configuration error function. More explicitly, we exten the trace form use in [6, 17] for attitue control on SO(3) with the aition of a logarithmic barrier function. Base on the propose configuration error function, nonlinear geometric attitue controllers are constructe. A smooth control system is first evelope assuming that there is no isturbance, an then it is extene to inclue an aaptive upate law for stabilization in the presence of unknown isturbances. The propose attitue configuration error function an several properties are summarize as follows. Proposition 1 (Attitue Error Function). Define an attitue error function Ψ : SO(3) R, an attitue error vector e R R 3, an an angular velocity error vector e Ω R 3 as follows: with Ψ(R, R ) = A(R, R )B(R), (11) e R = e RA B(R) + A(R, R )e RB, (1) e Ω = Ω, (13) A(R, R ) = 1 tr[ G ( I R T R )], (14) B(R) = 1 1 ( cos θ r T α ln R T ) v. (15) 1 + cos θ e RA = 1 ( GR T R R T R G ), (16) ( R T v ) r e RB = α (r T R T v cos θ). (17)

5 where α R is efine as a positive constant an the matrix G R 3 3 is efine as a iagonal matrix matrix for istinct, positive constants g 1, g, g 3 R. Then, the following properties hol (i) Ψ is positive efinite about R = R on SO(3). (ii) The variation of A(R) with respect to a variation of δr = Rˆη for η R 3 is given by D R A δr = η e RA, (18) where the notation D R A δr represents the irectional erivative of A with respect to R along δr. (iii) The variation of B(R) with respect to a variation of δr = Rˆη for η R 3 is given by (iv) An upper boun of e RA is given as: where the constant b 1 is given by b 1 = D R B δr = η e RB. (19) h1 h +h 3 e RA A(R) b 1, (0) for h 1 = min {g 1 + g, g + g 3, g 3 + g 1 }, { h = min (g 1 g ), (g g 3 ), (g 3 g 1 ) }, { h 3 = min (g 1 + g ), (g + g 3 ), (g 3 + g 1 ) }. (v) Ψ is a locally quaratic function, which means there exist constants 0 < n 1 n such that on the neighborhoo D of the esire attitue R for 0 < ψ < h 1 an 0 < β < cos θ. Proof. See Appenix.1 n 1 e R Ψ(R) n e R, (1) D = { R SO(3) Ψ < ψ < h 1, r T R T v < β < cos θ } () Equation (11) is compose of an attractive term, A(R) towar the esire attitue, an a repulsive term, B(R) away from the unesire irection R T v. In orer to visualize the attitue error function on SO(3), we utilize a spherical coorinate representation. Recall, that the spherical coorinate system represents the position of a point relative to an origin in terms of a raial istance, azimuth, an elevation. This coorinate system is commonly use to efine locations on the Earth in terms of a latitue an longitue. Similarly, the positions of celestial objects are efine on the celestial sphere in terms of right ascension an eclination. We apply this concept an parameterize the rotation matrix R SO(3) in terms of the spherical angles 180 λ 180 an 90 β 90. Using the elementary Euler rotations, the rotation matrix is now efine as R = exp(λê ) exp(βê 3 ). We iterate over the omains of λ an β in orer to rotate the boy-fixe vector r throughout the two-sphere S. Applying this metho, Fig. 1 allows us to visualize the error function on SO(3). The horizontal axes of Fig. 1 represent the omain of the spherical angles λ an β in egrees, while the vertical axes represent the unitless magnitue of the error functions efine in (11), (14) an (15). The attractive error function, given by (14), has been previously use for attitue control on SO(3). The potential well of A(R) is illustrate in Fig. 1a, where the esire attitue lies at the minimum of A(R). To incorporate the state inequality constraints we apply a logarithmic barrier term. Barrier functions are typically use in optimal control an motion planning applications. A visualization of the repulsive error function is presente in Fig. 1b which shows that as the bounary of the constraint is neare, or r T R T 1 v cos θ, the barrier term increases, B. We use the scale factor 1+cos θ to ensure that Ψ remains

6 (a) Attractive A(R) (b) Repulsive B(R) (c) Combine Ψ Figure 1: Visualization of Configuration Error Functions using spherical coorinate representation positive efinite. The logarithmic function is popular as it quickly ecays away from the constraint bounary. The positive constant α serves to shape the barrier function. As α is increase the impact of B(R) is reuce away from the constraint bounary. The superposition of the attractive an repulsive functions is shown in Fig. 1c. The control system is efine such that the attitue trajectory follows the negative graient of Ψ towar the minimum at R = R, while avoiing the constraine region. While (15) represents a single inequality constraint given as (10), it is reaily generalize to multiple constraints P of an arbitrary orientation. For example, the configuration error function can be formulate as Ψ = A[1 + i Ci ], where Ci has the form of Ci = B 1 for the i-th constraint. In this manner, one may enforce multiple state inequality constraints, an we later emonstrate this through numerical simulation. This is in contrast to many previous approaches which are computationally ifficult to exten to situations with multiple constraints. We present the ynamics of the configuration error function in Proposition, which are use in the subsequent evelopment of the nonlinear control system. Proposition (Error Dynamics). The attitue error ynamics for Ψ, er, eω satisfy (Ψ) = er eω, t (er ) = e RA B + era B + A erb + Ae RB, t (era ) = E(R, R )eω, t (erb ) = F (R)eΩ, t (A(R)) = era eω, t (B(R)) = erb eω, t (eω ) = J 1 ( Ω JΩ + u + W (R, Ω) ), t (3) (4) (5) (6) (7) (8) (9) where the matrices E(R, R ), F (R) R3 3 are given by 1 T tr R R G I RT R G, T 1 F (R) = v Rr I RT vrt + α (rt RT v cos θ) RT v Rrv T Rr. (rt RT v cos θ) E(R, R ) = (30) (31) Proof. See Appenix.

7 er1 er3 er Ψ arccos(r T R T vi) (a) Attitue error vector e R components (b) Configuration error Ψ (c) Angle to each constraint Figure : Attitue stabilization without aaptive upate law 3.1 Attitue Control without Disturbance We introuce a nonlinear geometric controller for the attitue stabilization of a rigi boy. We first assume that there is no isturbance, i.e., = 0, an present a nonlinear controller in Proposition 3. Proposition 3 (Attitue Control). Given a esire attitue comman (R, Ω = 0), which satisfies the constraint (10), an positive constants k R, k Ω R we efine a control input u R 3 as follows u = k R e R k Ω e Ω + Ω JΩ. (3) Then the zero equilibrium of the attitue error is asymptotically stable, an the inequality constraint is always satisfie. Proof. See Appenix.3 3. Aaptive Control We exten the results of the previous section with the aition of a fixe but unknown isturbance. This scenario is typical of many mechanical systems an represents unmoelle ynamics or external moments acting on the system. For example, Earth orbiting spacecraft typically experience a torque ue to a gravitational graient. Aerial vehicles will similarly experience external torques ue to air currents or turbulence. An aaptive control system is introuce to asymptotically stabilize the system to a esire attitue while ensuring that state constraints are satisfie. Proposition 4 (Boun on ė R ). Consier the neighborhoo D, given in Proposition 1, about the esire attitue, then the following statements hol: (i) Upper bouns of A(R) an B(R) are given by where the constant b is given by b = A < b e RA < c A, B < c B. (33) h1h4 h for 5(h 1 ψ) h 4 = min {g 1 + g, g + g 3, g 3 + g 1 }, { h 5 = min (g 1 + g ), (g + g 3 ), (g 3 + g 1 ) }.

8 (ii) Upper bouns of E(R, R ) an F (R) are given by F E 1 tr[g], (34) ( β + 1 ) (β cos θ) β ( β ) α (β cos θ) 4. (35) (iii) Upper bouns of the attitue error vectors e RA an e RB are given by ψ e RA, (36) b 1 e RB sin θ α (cos θ β). (37) These results are combine to yiel a maximum upper boun of the time erivative of the attitue error vector ė R as where H R is efine as Proof. See Appenix.4 ė R H e Ω, (38) H = B E + e RA e RB + A F. (39) Aaptive control is typically use in ynamical systems with varying or uncertain components. In Proposition 5, we present an aaptive attitue controller which hanles uncertain isturbances while satisfying the state inequality constraints. Proposition 5 (Aaptive Attitue Control). Given a esire attitue comman (R, Ω = 0) an positive constants k R, k Ω, k, c R, we efine a control input u R 3 an an aaptive upate law for the estimate uncertainty as follows: u = k R e R k Ω e Ω + Ω JΩ W, (40) = k W T (e Ω + ce R ). (41) If c is chosen such that 0 < c < min { λ m k R n 1 4k R k Ω λ, M kω + 4k Rλ M H }, (4) the zero equilibrium of the error vectors is stable in the sense of Lyapunov. Furthermore, e R, e Ω 0 as t, an is boune. Proof. See Appenix.5 Nonlinear aaptive controllers have been evelope for attitue stabilization in terms of moifie Roriguez parameters an quaternions, as well as attitue tracking in terms of Euler angles. The propose control system is evelope on SO(3) an avois the singularities of Euler angles an Roriguez parameters while incorporating state inequality constraints. In aition, the unwining an ouble coverage ambiguity of quaternions are also completely avoie. The control system hanles uncertain isturbances while avoiing constraine regions. Compare to the previous work on constraine attitue control, we present a geometrically exact control system without parameterizations. The controller is esigne on the true configuration manifol, SO(3), an is free from the issues associate with other attitue representations. In aition, we incorporate state inequality constraints for the first time on SO(3). The presente control system is compute in real-time an offers significant computational avantages over previous iterative methos. In aition, the rigorous mathematical proof guarantees stability. This is in contrast to many of the previous methos which offer no stability guarantees. The presente analysis offers provable bouns on the expecte motion, which are critical for harware implementation or mission critical applications.

9 4 Numerical Examples We emonstrate the performance of the propose control system via numerical simulation. The inertia tensor of a rigi boy is given as J = kg m The control system parameters are chosen as G = iag[0.9, 1.1, ], k R = 0.4, k Ω = 0.96, c =, k = 0.5, α = 15. The iagonal matrix G serves as a weighting matrix for the relative ifference between R an R. Using this term, the control esigner can moify the shape of the attractive error function, given in (14), an the resulting behavior of the close loop system. Similarly, the constant α is use to moify the shape of the repulsive error function, given in (15). In general, this term is erive from the system esign an the nature of the obstacles in the ynamic environment. For example, a system wishing to avoi pointing at a iffuse obstacle, such as incoming ebris, may chose an appropriate value of θ, base on the best available information, an a relatively low α to ensure aitional safety margin near the constraint bounary. Similarly, in an environment with several ensely space obstacles, such as that presente in Fig. 3, a much larger α woul enable more aggressive maneuvers which pass closer to the constraint bounary without violation. This woul increase the allowable region of operation in a highly constraine environment. The parameters k R, k Ω, c, k are control parameters use to moify the close-loop behavior of the system. It is straightforwar to chose k R, k Ω, k, using a linear analysis, to satisfy esire response criteria, such as settling time or percent overshoot [18]. A boy fixe sensor is efine as r = [1, 0, 0], while multiple inequality constraints are efine in Table 1. The simulation parameters are chosen to be similar to those foun in [15], however we increase the size of the constraint regions to create a more challenging scenario for the control system. The initial state is efine Table 1: Constraint Parameters Constraint Vector (v) Angle (θ) [0.174, 0.934, 34] T 40 [0, , ] T 40 [ 0.853, 0.436, 0.86] T 40 [ 0.1, 0.140, 0.983] T 0 as R 0 = exp(5 π 180ê3), Ω 0 = 0, with e 3 = [ ] T. The esire state is R = I, Ω = 0. We show simulation results for the system stabilizing about the esire attitue with an without the aaptive upate law from Proposition 5. We assume a fixe isturbance of = [ ] T N m, with the function W (R, Ω) = I. This form is equivalent to an integral control term which penalizes eviations from the esire configuration. The first term of (41) has the effect of increasing the proportional gain of the control system, since the time erivative of the attitue error vector, ė R, is linear with respect to the angular velocity error vector e Ω. Simulation results without the aaptive upate law are shown in Fig.. Fig. a shows each component of the attitue error vector, (1), over the simulation time span. Fig. b shows the magnitue of the combine error function, (11). Without the upate law, the system oes not achieve zero steay state error. Fig. b shows that the configuration error function oes not converge to zero an there exist steay state errors. In spite of the uncompensate isturbance, the system is able to avoi the constraine regions as shown in Fig. c. The angle to each of the constraints, which is measure in egrees an given by arccos(r T R T v i ), is always greater than the specifie angle, θ i, in Table 1. Fig. 3 shows the results with the aition of the aaptive upate law. Figs. 3a an 3b are equivalent to Figs. b an c with the exception of the aition of the aaptive upate law. The aition of the aaptive

10 Ψ arccos(r T R T vi) (a) Configuration error Ψ (b) Angle to each constraint (c) Disturbance estimate components () Attitue trajectory Figure 3: Attitue stabilization with aaptive upate law upate law allows the system to converge to the esire attitue in the presence of constraints. The path of the boy fixe sensor in the inertial frame, namely Rr, is illustrate in Fig. 3 by the blue trajectory. The renering of the spacecraft is presente in the esire, or final, orientation of the simulation. The inequality constraints from Table 1 are epicte as re cones, where the cone half angle is θ. The control system is able to asymptotically converge to the esire attitue. Fig. 3b shows that the angle, arccos(r T R T v i ) an measure in egrees, between the boy fixe sensor an each constraint is satisfie for the entire maneuver. In aition, the estimate of the isturbance converges to the true value as shown in Fig. 3c. Both control system are able to automatically avoi the constraine regions. In aition, these results show that it is straightforwar to incorporate an arbitrary amount of large constraints. In spite of this challenging configuration space, the propose control system offers a simple metho of avoiing constraine regions. These close-loop feeback results are compute in real time an offer a significant avantage over typical open-loop planning methos. These results show that the propose geometric aaptive approach is critical to attitue stabilization in the presence of state constraints an isturbances. 4.1 Attitue Parameterizations Attitue parameterizations, such as Euler angles an Quaternions, are frequently use in the aerospace an astroynamics communities [19]. For example, Euler angle sequences are frequently use to escribe the transformation between a variety of reference frames use to escribe the position an orientation of the orbit of Earth satellites [19]. In aition, quaternions were use uring the operation of Skylab an the NASA Space Shuttle [1]. However, the choice of attitue parameterization plays a critical role in control esign an the resulting motion of the system. Euler angle sequences are a minimum, three-parameter set of angles which escribe the transformation

11 between two reference frames. Using Euler angles, we can represent any general rotation as a sequence of three intermeiate rotations [4]. By convention, there are 4 possible Euler angle sequences for any given rotation. In aition, Euler angles are a minimum representation, as only three angles, an the associate sequence, are require to escribe the three angular egrees of freeom of the rigi boy. However, there is great ambiguity in the representation of the attitue as there are many equivalent Euler angle sequences for a given attitue of the system. Therefore, great care must be taken in the control system esign to ensure that a consistent sequence is use. Furthermore, it has been shown that no minimal attitue representation can escribe orientations both globally an without singularities [1, 3]. These singularities can cause significant ifficulties uring control esign an harware implementation. To emonstrate the effect of the kinematic singularities inherent with Euler angles we will represent the attitue of the boy fixe reference frame, b i, with respect to the inertial frame, e i, in terms of the Euler angle sequence. More explicitly, this correspons to the rotation sequence θ 1 b 3, θ b 1, θ 3 b 3. The rotation matrix, R(θ 1, θ, θ 3 ), corresponing to this sequence is s 1c s 3 + c 3 c 1 s 1 c c 3 s 3 c 1 s 1 s c 1 c s 3 + c 3 s 1 c 1 c c 3 s 3 s 1 c 1 s, (43) s s 3 s c 3 c where s i, c i represent sin θ i, cos θ i for i = {1,, 3}. Using this representation, the kinematic ifferential equations for the associate Euler angles are given as θ 1 (Ω 1 s 3 + Ω c 3 )/s θ = Ω 1 c 3 Ω s 3. (44) θ 3 (Ω 1 s 3 + ω c 3 ) c /s + Ω 3 From (44), it is immeiately clear that a singularity exists when sin θ = 0 or equivalently, θ = 0, ±π. In the vicinity of the singularity, the angular velocities of the Euler angles will ten to approach ± an the angular velocities will experience instantaneous sign changes. Furthermore, all Euler angle sequences will exhibit a similar singularity at either θ = 0, ±π or θ = ± π, ± 3π. Therefore simply switching the sequence oes not alleviate the issue, but rather only moves the singularity. As a result, Euler angles are not appropriate for systems which experience large angular rotations, such as those emonstrate in Fig. 3, or control systems which rely on the angular velocities θ i. 4. Time-varying Disturbance The form of the uncertainty, given in (1), is commonly use in the aaptive control literature [17, 0]. A wie variety of realistic isturbances, such as gravitational graients or malfunctioning thrusters for spacecraft scenarios, are accurately represente via this moel. In aition, it is possible to represent the uncertainty of a time-varying inertia matrix as an equivalent external isturbance. For example, Euler s law gives the relationship for the rate of change of angular momentum as M ext = Ḣ = JΩ + J Ω. Using this, we can see that an instantaneous change in J is proportional to an external moment. Finally, it has been shown that this aaptive control formulation is able to hanle time-varying isturbances uner some mil assumptions [0]. We emonstrate the ability to hanle an uncertain time-varying isturbance via numerical example. The system is ientical to the one presente in Section 4, however we moify the external isturbance. The external isturbance is the superposition of constant an time-varying terms as = 0. sin 9t 0. + cos 9t N m. 0. (sin 9t + cos 9t) 1 We efine a constraint in the inertial frame as v = [ 1, 1, 0] T with θ = 1. The initial state is efine as R(0) = exp( π ê3), while the esire state is R = I. The goal is to rotate the vehicle about the e 3 axis while avoiing the obstacle an compensating for the time-varying isturbance.

12 Fig. 5 emonstrates the ability for the aaptive controller, which is presente in Proposition 5, to hanle time-varying isturbances. Fig. 5a shows the non-imensional value of the configuration error function an emonstrates that the aaptive controller is able to stabilize the system to the esire attitue configuration. In aition, Fig. 5b shows that the constraint is never violate as the angle between the boy-fixe sensor r an the constraint v is greater than 1 over the entire attitue maneuver. We can see in Fig. 5c that that estimate for each of the components accurately tracks the true isturbance after approximately 5 s. 5 Experiment on Hexrotor UAV Figure 4: Attitue control testbe A hexrotor unmanne aerial vehicle (UAV), as seen in Fig. 4, has been evelope at the Flight Dynamics an Controls Laboratory (FDCL) at the George Washington University [1]. The UAV is compose of three pairs of counter-rotating propellers. Typical UAVs are compose of four or more co-planar propellers. As a result, these systems are uneractuate an unable to impart a force along every egree of freeom. For example, quarotor UAVs are unable to translate laterally without first conucting a rotation. Conversely, the propeller pairs of the hexrotor are angle relative to one another to allow for a fully actuate rigi boy. This allows the hexrotor to impart a force in any irection an a moment about any axis. Attitue Ψ 0.5 arccos(r T R T vi) (a) Configuration error Ψ (b) Angle to constraint (c) Disturbance estimate components Figure 5: Time-varying external isturbance simulation information is measure by a combination of both on an off boar sensor systems. The VectorNav VN-100 is a rugge, miniature high-performance inertial measurement unit which provies high frequency angular velocity measurements. A Vicon motion capture system is installe within the test environment an use to provie high accuracy attitue measurements. A series of reflective markers are place on the hexrotor an their relative position is capture by a series of infrare optical cameras. Assuming a fixe rigi boy, the Vicon system is able to erive the attitue of the hexrotor an transmit this ata to the processor onboar the hexrotor. The control input is compute on-boar, using the full state measurement, an implemente at approximately 100 Hz. In orer to constrain the motion, allowing us to test only the attitue ynamics, we

13 er er 0.5 Ψ 0.5 u1 u er3 1 0 u (a) Attitue error vector e R components (b) Configuration error Ψ 50 (c) Control input u components arccos(r T R T vi) () Attitue Trajectory (e) Angle to constraint Figure 6: Constraine Attitue stabilization experiment attach the hexrotor to a spherical joint. Since, the center of rotation is below the center of gravity of the hexrotor there is a estabilizing gravitational moment. The resulting attitue ynamics are similar to an inverte penulum moel. We augment the control input in (40) with an aitional term to negate the effect of the gravitational moment. A sensor pointing irection is efine in the boy-fixe frame of the hexrotor as r = [1, 0, 0] T. We efine an obstacle in the inertial frame as v = [ 1 1,, 0] T with θ = 1. An initial state is efine as R(0) = exp( π ê3), while the esire state is R = I. This results in the UAV performing a 90 yaw rotation about the vertical axis of the spherical joint an the constraine region is on the shortest path connecting R 0 an R. The attitue control system is ientical to the one presente in Proposition 5 with the exception of a gravity moment term, M g = r cg mgr T e 3 which represents the gravitational moment ue to the istance r cg between the center of mass an the center of rotation. In aition, the following parameters were also moifie: k R = 0.4, k Ω = 0.7, c = 0.1, α = 8 an k = 5 to account for the ifferences in the harware moel of the hexrotor. The experimental results are shown in Fig. 6. Fig. 6a shows the behavior of each of the components of the attitue error vector, efine by (1), over the experiment time span. Fig. 6b shows the time history of the attitue error function, efine by (11). Fig. 6c shows the magnitue of each component of the control input in N m, which is compute from (40). Finally, Fig. 6e shows the angle between the boy-fixe sensor an the obstacle in egrees. In orer to maneuver the system close to the constraine zone we utilize several intermeiary set points on either sie of the obstacle. From the initial attitue the hexrotor rotates to the first set point, pauses, an then continues aroun the obstacle to the secon set point before continuing towar the esire attitue. As a result this creates the steppe behavior of the configuration error history as shown in Fig. 6b. The hexrotor avois the constraine region illustrate by the circular cone in Fig. 6, by rotating aroun the bounary of the constraint. Fig. 6e shows the angle, arccos r T R T v, between the boy mounte sensor an the inertially fixe sensor. The experiment emonstrates that the minimum angular separation is 14 which

14 satisfies the constraint of θ = 1. This further valiates that the propose control system exhibits the esire performance in the experimental setting as well. A vieo clip is available at 6 Conclusions We have evelope a geometric aaptive control system which incorporates state inequality constraints on SO(3). The presente control system is evelope irectly on SO(3) an it avois singularities an ambiguities that are inherent to attitue parameterizations. The attitue configuration error is augmente with a barrier function to avoi the constraine region, an an aaptive control law is propose to cancel the effects of uncertainties. We show the stability of the propose control system through a rigorous mathematical analysis. In aition, we have emonstrate the control system via numerical simulation an harware experiments on a hexrotor UAV. A novel feature of this control is that it is compute autonomously on-boar the UAV. This is in contrast to many state constraine attitue control systems which require an a priori attitue trajectory to be calculate. The presente metho is simple, efficient an ieal for harware implementation on embee systems..1 Proof of Proposition 1 To prove (i), we note that (14) is a positive efinite function about R = R [6]. The constraint angle is assume 0 θ 90 such that 0 cos θ. The term r T R T v represents the cosine of the angle between the boy fixe vector r an the inertial vector v. It follows that 0 cos θ rt R T v 1 + cos θ for all R SO(3). As a result, its negative logarithm is always positive an from (15), 1 < B. The error function Ψ = AB is compose of two positive terms an is therefore also positive efinite. Next, we consier (ii). The infinitesimal variation of a rotation matrix is efine as δr = ɛ R exp ɛˆη = Rˆη. ɛ=0 Using this, the variation of (14) is taken with respect to R as D R A δr = η 1 ( GR T R R T R G ), where we use (7) to achieve the simplifie form. A straightforwar application of the chain an prouct rules of ifferentiation allows us to show (iii) as D R B δr = η 1, ( R T v ) r α (cos θ r T R T v), where the scalar triple prouct (5) was use. We show (v) by computing the hessian of Ψ, namely Hess (Ψ), using the chain rule as Hess (Ψ) (δr, δr) = (D R (D R A δr) δr)b + (D R A δr) (D R B δr) + (D R B δr) (D R A δr) + (D R (D R B δr) δr)a. The first orer erivative of A(R) an B(R) are given by (16) an (17). The hessian of A(R) is compute as D R (D R A δr) δr = η 1 ( GR T δr δr T R G ) = η 1 = η 1 = η 1 ( GR T Rˆη + ˆηR T R G ) [ ({tr [ R T R G ] I R T R G } η ) ] ( tr [ R T R G ] I R T R G ) η,

15 where we use the scalar triple prouct rule, from (5), to arrive at the final form. The hessian of B(R) is compute as [ ( δr T v ) r D R (D R B δr) δr =η α (r T R T v cos θ) ( R T v ) ( r r T δr T v ) ] α (r T R T v cos θ). The term ( δr T v ) r is simplifie as ( δr T v ) r = ˆrδR T v = ˆr (Rˆη) T v = ˆr (ˆηR T ) v = ˆr ( R T v ) η, where we utilize the hat map property from (4). Similarly, the term r T δr T v is simplifie as r T δr T v = r T ( ˆηR T ) v = r T ( R T v ) η. The hessian of B(R) then becomes [ ˆr ( R T v ) D R (D R B δr) δr =η α (r T R T v cos θ) ( R T v ) ( rr T R T v ) ] α (r T R T v cos θ) η. Using these terms, we evaluate Hess Ψ at the esire attitue R = R as follows. Since A = 0 an D R A = 0 at R = R, Hess (Ψ) (δr, δr) R=R = η 1 B (tr[g] I G) η, which is positive efinite since B > 1 an g i > g i. The omain D is an open neighborhoo of the esire attitue R, an it exclues the unesire equilibrium points of A(R) an the infeasible regions efine by the constraints r T R T v i. Therefore, the only critical point of the error function Ψ in the omain D correspons to the esire attitue R = R with e R = 0 an Ψ = 0. Therefore, in D the configuration error function is quaratic an the bouns in (v) are vali accoring to [6, Proposition 6.30]. The proof of (iv) is available in [17].. Proof of Proposition From the kinematics (), an noting that Ṙ = 0 the time erivative of R T R is given as Applying this to the time erivative of (14) gives ( R T t R ) = R T Rê Ω. t (A) = 1 tr[ GR T Rê Ω ].

16 Applying (7) into this shows (7). Next, the time erivative of the repulsive error function is given by ) rt T (ˆΩR v t (B) = α (r T R T v cos θ). Using the scalar triple prouct, given by (5), one can reuce this to (8). The time erivative of the attractive attitue error vector, e RA, is given by t (e R A ) = 1 (êω R T R G + (R T R G) T ) ê Ω. Using the hat map property given in (8) this is further reuce to (5) an (30). We take the time erivative of the repulsive attitue error vector, e RB, as with a R an b R given by t (e R B ) = aωv T Rr ar T vω T r + br T ˆvRr, a = [ α ( r T R T v cos θ )] 1, b = r T ˆΩR T v α (r T R T v cos θ). Using the scalar triple prouct from (5) as r Ω ( R T v ) = ( R T v ) r Ω gives (6) an (31). We show the time erivative of the configuration error function as t (Ψ) = AB + AḂ. A straightforwar substitution of (14), (15), (7) an (8) into this an applying (1) shows (3). We show (9) by rearranging (1) as.3 Proof of Proposition 3 Consier the following Lyapunov function: t e Ω = Ω = J 1 (u Ω JΩ + W (R, Ω) ). V = 1 e Ω Je Ω + k R Ψ(R, R ). From (i) of Proposition 1, V 0. Using (3) an (9) with = 0, the time erivative of V is given by V = k Ω e Ω. Since V is positive efinite an V is negative semi-efinite, the zero equilibrium point e R, e Ω is stable in the sense of Lyapunov. This also implies lim t e Ω = 0 an e R is uniformly boune, as the Lyapunov function is non-increasing. From (5) an (6), lim t ė R = 0. One can show that ë R is boune. From Barbalat s Lemma, it follows lim t ė R = 0 [, Lemma 8.]. Therefore, the equilibrium is asymptotically stable. Furthermore, since V 0 the Lyapunov function is uniformly boune which implies Ψ(R(t)) V(t) V(0). In aition, the logarithmic term in (15) ensures Ψ(R) as r T R T v cos θ. Therefore, the inequality constraint is always satisfie given that the esire equilibrium lies in the feasible set.

17 .4 Proof of Proposition 4 Consier the open neighborhoo D of R = R efine in Proposition 1. The proof of the upper boun of A(R) is given in [17]. The selecte omain ensures that the configuration error function is boune Ψ < ψ. This implies that that both A(R) an B(R) are boune by constants c A c B < ψ < h 1. Furthermore, since B > 1 this ensures that c A, c B < ψ an shows (33). Next, we show (34) an (35) using the Frobenius norm. The Frobenius norm E F is given in [17] as E F = tr[e T E] = 1 tr[g ] + tr[r T R G]. Applying Rorigues formula an the Matlab symbolic toolbox, this is simplifie to E F 1 ( tr [ G ] + tr[g] ) 1 4 tr[g], which shows (34), since E E F. To show (35), we apply the Frobenius norm F F : where the terms a, b, an c are given by A straightforwar computation of a T a shows that 1 [ [ F F = tr a T α (r T R T v cos θ) a ] tr [ a T b ] +tr [ a T c ] + tr [ b T b ] tr [ b T c ] + tr [ c T c ]]. a = r T RrI, b = R T vr T, c = RT ˆvRrv T Rˆr r T R T v cos θ. tr [ a T a ] = ( v T Rr ) tr[i] 3β, where we use the fact that v T Rr = r T R T v < β from our given omain. Similarly, one can show that tr [ a T b ] is equivalent to tr [ a T b ] = v T Rrtr [ R T vr T ] = ( v T Rr ) β, where we use the fact that tr [ xy T ] = x T y. The prouct tr [ a T c ] is given by tr [ a T c ] = v T Rr [ (R r T R T v cos θ tr T v ) ( rv T R ) ] ˆr, where we use the hat map property (9). One can show that tr[a T c] 0 over the range 1 v T Rr cos θ. Next, tr [ b T b ] is equivalent to since r, v S. Finally, tr [ c T c ] is reuce to tr [ b T b ] = tr [ rv T RR T vr T ] = 1, tr [ c T c ] = tr [ˆrR T vr T [ I + R T vv T R ] rv T Rˆr ], where we use the fact that ˆx = x I + xx T. Expaning an collecting like terms gives tr [ c T c ] = 1 ( v T Rr ) + ( v T Rr ) 4 (r T R T v cos θ). Using the given omain r T R T v β gives the upper boun (35). The boun on e RA is given in (0) while e RB arises from the efinition of the cross prouct a b = a b sin θ. Finally, we can fin the upper boun (4) as ė R ( B E + e RA e RB + A F ) e Ω. Using (33) to (37) one can efine H in terms of known values.

18 .5 Proof of Proposition 5 Consier the Lyapunov function V given by V = 1 e Ω Je Ω + k R Ψ + cje Ω e R + 1 k e e, over the omain D, efine in Proposition 1. In this set, the properties of Propositions 1 an are satisfie. From Proposition 1, the configuration error function is locally quaratic an it is boune in D by (1). Using this, the Lyapunov function V is boune by z T W 1 z V z T W z, where e =, z = [ e R, e Ω, e ] T R 3 an the matrices W 1, W R 3 3 are given by k R n 1 1 cλ M 0 W 1 = 1 cλ 1 M λ m 0, k 1 k R n cλ M 0 W = 1 cλ M 1 λ M k The time erivative of V with the control input efine by (40) is given as V = k Ω e T Ωe Ω + (e Ω + ce R ) T W e k R ce T Re R k Ω ce T Re Ω + cje T Ωė R 1 k e T, where we use ė =. The terms linearly epenent on e are combine with (41) to yiel ( e T W T (e Ω + ce R ) 1 ) = 0. k Using Proposition 4, an upper boun on V is written as V ζ T Mζ, where ζ = [ e R, e Ω ] R, an the matrix M R is [ k kr c Ωc ] M = k Ωc. k Ω cλ M H If c is chosen such that (4) is satisfie then the matrices W 1, W an M are positive efinite. This implies that V is positive efinite an ecrescent, an V is negative semiefinite in the omain D. As such, the zero equilibrium is stable in the sense of Lyapunov, an all of the error variables are boune. Furthermore, lim t ζ = 0 accoring to the LaSalle-Yoshizawa theorem []. References [1] P. Hughes, Spacecraft Attitue Dynamics. Dover Publications, 004. [] J. R. Wertz, Spacecraft Attitue Determination an Control. Springer, 1978, vol. 73. [3] S. P. Bhat an D. S. Bernstein, A topological obstruction to continuous global stabilization of rotational motion an the unwining phenomenon, Systems & Control Letters, 000. [4] M. D. Shuster, A survey of attitue representations, Journal of the Astronautical Sciences, vol. 41, no. 8, pp , Oct

19 [5] N. Chaturvei, A. K. Sanyal, N. H. McClamroch, et al., Rigi-boy attitue control, Control Systems, IEEE, vol. 31, no. 3, pp , 011. [6] F. Bullo an A. D. Lewis, Geometric Control of Mechanical Systems, ser. Texts in Applie Mathematics. New York-Heielberg-Berlin: Springer Verlag, 004, vol. 49. [7] C. Mayhew an A. Teel, Synergistic potential functions for hybri control of rigi-boy attitue, in Proceeings of the American Control Conference, 011, pp [8] T. Lee, Global exponential attitue tracking controls on SO(3), IEEE Transactions on Automatic Control, vol. 60, no. 10, pp , 015. [9] H. B. Hablani, Attitue commans avoiing bright objects an maintaining communication with groun station, Journal of Guiance, Control, an Dynamics, vol., no. 6, pp , 015/09/ [Online]. Available: [10] E. Frazzoli, M. Dahleh, E. Feron, an R. Kornfel, A ranomize attitue slew planning algorithm for autonomous spacecraft, in AIAA Guiance, Navigation, an Control Conference an Exhibit, Montreal, Canaa, 001. [11] A. Guiggiani, I. Kolmanovsky, P. Patrinos, an A. Bempora, Fixe-point constraine moel preictive control of spacecraft attitue, arxiv:141479, 014. [Online]. Available: [1] U. Kalabic, R. Gupta, S. Di Cairano, A. Bloch, an I. Kolmanovsky, Constraine spacecraft attitue control on SO(3) using fast nonlinear moel preictive control using reference governors an nonlinear moel preictive control, in American Control Conference (ACC), 014, June 014, pp [13] R. Gupta, U. Kalabic, S. Di Cairano, A. Bloch, an I. Kolmanovsky, Constraine spacecraft attitue control on SO(3) using fast nonlinear moel preictive control, in American Control Conference (ACC), 015, July 015, pp [14] E. Rimon an D. E. Koitschek, Exact robot navigation using artificial potential functions, Robotics an Automation, IEEE Transactions on, vol. 8, no. 5, pp , 199. [15] U. Lee an M. Mesbahi, Spacecraft Reorientation in Presence of Attitue Constraints via Logarithmic Barrier Potentials, in Proceeings of the American Control Conference, 011, pp [16] C. R. McInnes, Large angle slew maneuvers with autonomous sun vector avoiance, Journal of Guiance, Control, an Dynamics, vol. 17, no. 4, pp , 015/07/ [Online]. Available: [17] T. Lee, Robust aaptive tracking on SO(3) with an application to the attitue ynamics of a quarotor UAV, IEEE Transactions on Control Systems Technology, vol. 1, no. 5, pp , September 013. [18] N. S. Nise, Control Systems Engineering, 4th e. John Wiley & Sons, 004. [19] D. A. Vallao, Funamentals of Astroynamics an Applications, 3r e. Microcosm Press, 007. [0] P. A. Ioannou an J. Sun, Robust Aaptive Control. Courier Corporation, 01. [1] E. Kaufman, K. Calwell, D. Lee, an T. Lee, Design an evelopment of a free-floating hexrotor UAV for 6-of maneuvers, in Proceeings of the IEEE Aerospace Conference, 014. [] H. K. Khalil, Nonlinear Systems, 3r e. Prentice Hall New Jersey, 00.