Exponential stability of trajectory tracking control in the orientation space utilizing unit quaternions

Trajectory tracking in the orientation space utilizing unit quaternions yields non linear error dynamics as opposed to Cartesian position. In this work, we study trajectory tracking in the orientation space utilizing the most popular quaternion error representations and angular velocity errors. By selecting error functions carefully we show exponential convergence in a region of attraction containing large initial errors. We further show that under certain conditions frequently en-countered in practice, the formulation respecting the geometric characteristics of the quaternion manifold and its tangent space yields linear tracking dynamics allowing us to guarantee a desired tracking performance by gain selection without tuning. Simulation and experimental results are provided.


I. INTRODUCTION
In many applications it is necessary for a control system to follow a desired orientation trajectory. Such applications involve the control of robotic manipulators [1], aerial and space agents [2], [3] or underwater vehicles [4], [5]. In the Cartesian position case this problem can be solved by using appropriate feedback terms in the position and velocity, generating linear system error dynamics. This is not however the case in the orientation space. Orientation space representations lie in the SO(3) group, which is a three dimensional manifold. No minimal representations of SO(3) exist, free of singularities or discontinuities [6].
A very popular representation of orientation in robotics and control is the unit quaternion [7]. Unit quaternions offer efficiency in calculation and a strong algebraic and kinematics background [8]. Orientation representations using unit quaternions are free of singularities but are not unique by construct. In fact an orientation can be described by either a unit quaternion or its negative. The uniqueness comes by constraining the unit quaternion space. However, when used to form the error between the current and the desired orientation, this constraint can lead to undesired behaviours known as unwinding [9], resulting in non-optimal paths. To alleviate it, instead of constraining the current or desired quaternion space, the error angle, in the equivalent angle axis representation should be confined in the interval [−π, π).
As the inherent quaternion dynamics is not linear, control systems in the orientation space are not linear in general. Furthermore, the orientation error used in such controllers must be defined in R 3 in order to be compatible with the angular velocity and acceleration. There are three popular Authors  orientation error definitions in R 3 that have been proposed for quaternion feedback. Initially an error inspired by the angle axis representation is proposed in [10] and still used in recent works [11]. In [12] the utilization of the vector part of the quaternion error is proposed, and has been largely adopted by the robotics and control community [13]- [15]. In recent years, the usage of another quaternion error is gaining popularity [16]- [20] that takes into account the logarithmic mapping from the unit quaternion manifold to the tangent space of angular velocity vectors [21], [22].
Moreover, there exist two different angular velocity error formulations. Earlier works use the error formed from the difference of the desired and the current angular velocity [7], [12], [23]. We will show that this error selection leads to nonautonomous closed loop dynamics. The local convergence properties in this case combined with the vector part of the quaternion error is studied in [24]. More recent works [14], [25]- [28] utilize a different angular velocity error, exploiting the geometry of the quaternion manifold.
The majority of related works in the literature, include proofs of the asymptotic stability of error dynamics. Exponential stability to the best of our knowledge is only addressed in [27] for an attitude tracking control system utilizing rotation matrices. In this work, we will prove exponential stability of the error dynamics with constant control gains in all formulations. This is in contrast to the claim made in some previous works that some cases require time varying gains e.g. [12] for the orientation error proposed in [10] and [29] for the logarithmic error. In contrast to asymptotic stability, exponential stability offers information about the system's convergence rate resulting in more intuitive system design. Furthermore, we will show that under certain conditions frequently encountered in practice, the formulation respecting the geometric characteristics of the quaternion manifold and its tangent space yields linear tracking dynamics; hence transient specifications like convergence rate or avoiding overshoot can be satisfied by appropriate gain selection without tuning. This linear behavior offers predictability which can be vital in high precision tasks.

II. UNIT QUATERNIONS
A unit quaternion Q = [η T ] T is given by a scalar part η and a three dimensional vector part referred to as the real and imaginary part respectively; unit quaternions expressed by 4-dimensional vectors belong to a sphere of dimension 3 i.e. Q ∈ S 3 . Unit quaternions are the minimal singularity free representation of the orientation of a rigid body expressed by the rotation matrix R ∈ SO(3). The unit quaternion space S 3 is mapped to SO(3) by the mapping: where I 3 is the identity matrix of dimension 3 and S(x) the skew symmetric matrix constructed by a vector x: Unit quaternions are related to the equivalent axis n and rotation angle θ in the following way: The product of two unit quaternions is also a unit quaternion and is given by: The inverse of a unit quaternion coincides with its conjugate Q = η − T T . The unit quaternion space S 3 is a three dimensional Riemannian manifold. At each point on this manifold corresponds a tangent space of angular velocity vectors. The manifold S 3 forms a Lie group endowed with the quaternion multiplication (3) [30]. An efficient way to transition between the manifold and its tangent space is the utilization of distance preserving mappings that take into account their geometric characteristics. These are the exponential and logarithmic mappings between a unit quaternion Q ∈ S 3 and a tangent vector x ∈ R 3 stemming from the Lie Algebra of S 3 and are defined as follows: The quaternion logarithm can also be written as a function of the axis and angle through (2):

A. Unit Quaternion Kinematics
The derivative of a unit Quaternion is associated with the angular velocity expressed in the inertia frame by: with It is easy to establish some properties for matrix J Q (Q) (8): Hence the columns of matrix J Q (Q) form an orthogonal base of the tangent space of S 3 at Q. Let Q ∈ S 3 denote the error between a desired quaternion Q d and the current quaternion Q along the geodesic connecting the two points expressed in the inertia frame: To ensure that the quaternion error represents the minimal rotation we constrain its scalar part η ≥ 0 resulting through (2) to error angles in [−π, π), since η = cos θ 2 . Differentiating Q * Q d in (10) using (7), (8) yields the derivatives of the quaternion error's scalar and vector parts: Equations (11) and (12) can also be written as: and in compact form: where R is derived from Q through (1). Notice the similarity of (15) with (7). Hence ω − Rω d represents the angular velocity on the tangent space of S 3 at Q.

III. TRACKING CONTROL VARIANTS
The control objective is the tracking of a desired orientation trajectory Q d , ω d ,ω d . Consider systems that can be put in the form:ω = u (16) whereω is the system's angular acceleration and u is the control input. Equation (16) expresses the second order kinematics of a rigid body rotational motion. The respective rigid body dynamics (e.g. a robot's end-effector) can further take this form via an inner inverse dynamics control loop.
In the literature, two types of control structures have been proposed. They differ with respect to the angular velocity error they utilize. These are: The first is the trivial case, employing the Euclidean difference between the current and desired angular velocity [7], [12], [23]. The second angular velocity error represents the angular velocity at Q as discussed in the end of Section II, which takes into account the geometry of S 3 [14], [25]- [28]. Matrix R in (18) operates as a transport map which allows velocities belonging to different tangent spaces to be compared [31]. In contrast, the angular velocities ω and ω d in (17) only belong in the same tangent space for small errors. The respective control inputs are given by: where d, κ > 0 are positive damping and stiffness gains and e i is an expression of the orientation error in To construct a control system on the manifold of S 3 , one needs to define an appropriate orientation error function Ψ : S 3 × S 3 → R, which is positive definite and measures the error between two orientations [32]. The orientation and angular velocity errors emerge by differentiating Ψ. Related to the three choices of e i suggested in the literature we select the following orientation error functions Ψ i : • The vector part of Q [1], [12], [14], [15]: • Double the product of the scalar and the vector part of Q [10], [11]: • Double the quaternion logarithm (6) of Q [16]- [19]: For every orientation error, it is easy to prove that the derivative of the respective orientation error function yields: Notice that each orientation error is a product of the error rotation axis n with a scalar function of the error angle f i ( θ) that is depicted in Figure 1, along with function Ψ i .
The vector part of Q (21), proposed in [12] is the most popular to this day, as it facilitates Lyapunov stability analysis of control systems. However, it yields slower response than other selections [16]. The second error (23) is continuous when the error angle crosses ±π as opposed to the other choices but it is a non-monotonous function of the error angle as shown in Figure 1. Hence, its behavior deteriorates when angle errors are larger than π 2 [27]. Moreover e a = 0 implies = 0, (which in turn implies Q = Q d ) or η = 0; when η = 0 ( θ = −π), = 0 and hence Q does not track Q d . The logarithmic error (25) is gaining popularity in recent research works [16]- [19] as it offers a more mathematically sound approach utilizing the Lie Algebra of S 3 by employing the distance preserving logarithmic mapping.
Differentiating the orientation error using (11)- (14) we get: where matrix J i is given for each error selection in Appendix. System (16) with control inputs (19), (20) yield the following closed loop tracking systems: In the following sections, we express (30) and (31) in state space, find their equilibria, show the exponential convergence to the origin, determine the region of attraction and indicate conditions that render the tracking system linear and thus allow gain selection that satisfy predefined specifications.

IV. STABILITY ANALYSIS
Define the state space vectors for i = { , a, l}, j = {G, E}: In state space form the system is written as: where Notice that due to the time dependant term S(ω d ), system (30), with angular velocity error ω E is non-autonomous.

Proposition 1.
In the region θ ∈ [−π, π) the origin ξ j i = 0 is a unique equilibrium of (33) for i = { , a, l}, j = {G, E} Proof. Setting˙ ω j = 0, the second equation of (33) yields ω j = − κ d e i . For i = setting˙ = 0 implies˙ η = 0 owing to the unit quaternion norm constraint. Substituting the above in (11) or (13) implies − κ d T = 0 and in turn ξ j = 0. For i = l, setting˙ e l = 0, the first equation of (33), using J l e l = e l from the Appendix, yields e l = 0 for j = G and − κ d I 3 + S(ω d ) e l = 0 for j = E. Notice that matrix − κ d I 3 + S(ω d ) is full rank, thus the origin is the only equilibrium of (33) for i = l. For i = a, as before we get J a e a = 0 for j = G; substituting J a from (55) and e a from (23) we get after some calculations J a e a = (2 η 2 − 1)e a = 0. For j = E, we find respectively − κ d (2 η 2 − 1)I 3 + S(ω d ) e a = 0. Notice that the expression in the parenthesis is zero for the value of η = 1 . Substituting this result in (11) or (13) yields˙ η = κ √ 2d T = 0 and hence the solution cannot stay at this value. Thus, in the above identities e a = 0 is the only solution. Notice that e a = 0 does not guarantee tracking as discussed in Section III for η = 0. It is however possible to show that this is not a stable state. Theorem 1. The origin ξ j i = 0 of (33) for i = { , a, l}, j = {G, E} is exponentially stable and an estimate of its the region of attraction is: Proof. We will initially prove that (34), (35) is an invariant set. The inequality (34) holds since θ ∈ (−π, π). Consider the function: and Ψ i (t) < 1 2 π 2 for i = l ∀t > 0 which means that the orientation errors are well defined.
Define the candidate Lyapunov function: Notice that: where and where L 11 = κ, U 11 = 2κ for i = , L 11 = U 11 = κ 2 η 2 for i = a and L 11 = U 11 = κ 2 for i = l. Notice that (34) implies η > 0. Matrices L i , U i are positive definite when c < √ 2κ, for i = and c < √ κ, for i = {a, l}. Differentiating (37) for j = G, using (31), (27) yields: Differentiating (37) for j = E using (30), (27) yields: where B ≥ ω d is the maximum norm value of the desired angular velocity and M j i are given by: 43) Using (60), matrices (43) are positive definite when c < 4κd 2κpi+q 2 j , with p = 1, p a = 2, p l = π and q G = d, q E = d + B. Since there exists a value for c that satisfies this constraint, the origin is exponentially stable [33], with: Remark 1. We will now show that η = 0 is not a stable state for i = a. Consider the set: We have already shown that for function (36)Ẇ ≤ 0. Notice that W (ξ S ) = 2κ, since = 1 in this case. Assume a point ξ σ emerging from a small disturbance η = σ. Then, W (ξ σ ) = 2κ(1 − σ 2 ) < W (ξ S ). Thus if ξ S is perturbed, ξ j a will never return to ξ S , rendering it unstable.

V. TRACKING SYSTEM LINEARITY CONDITIONS AND GAIN SELECTION
We will now show that controller (20) utilizing the angular velocity error ω G (18) with the logarithmic orientation error e l (25) yields linear tracking error dynamics with certain initial conditions. The latter is typical in many applications. We will further show that for small error angles the system remains linear, regardless of the initial conditions, even in the presence of disturbances. This property is useful considering a tracking system at steady state under modelling errors and measurement noise. In these cases, one can select control gain values for κ, d that satisfy specifications about the speed of response and its damping ratio (e.g being critically damped) or even satisfy a desired compliant behaviour under external forces. Proof. Using (15), (9) and (2) we get: Notice that (46) has three orthogonal components, since˙ n is the derivative of the unit vector n and is orthogonal to it by construct, and S( n)˙ n represents the cross product between n and˙ n. Differentiating (25) yields: Notice from (46) and (47) that when˙ n = 0, the geometric angular velocity error is equal to the derivative of the logarithmic orientation error. Define the projection matrices P = n n T , P ⊥ = I 3 − n n T . Projecting (46) yields: Differentiating (48) and (49) using (31), with i = l and (46) yields: Let the initial orientation error be e l (0)) = θ(0) n(0) and the initial angular velocity error ω G (0) = 0. Given that θ(0) = 0, the initial condition ω G (0) = 0 implies from (46) that˙ n(0) = 0,˙ θ(0) = 0. Hence from (50) and (51) we deduce that d dt (P ⊥ ω G ) remains zero and ω G evolves along the direction of n(0) implying˙ n = 0.
From the above analysis, we deduce that in both cases, the system is linear asė l = ω G =˙ θ n.
Remark 2. System (31) with orientation errors e (21) or e a (23) yields an angular velocity error ω G along n(0) following the same analysis performed in the above proof. However the resulting dynamics is not linear as the orientation error derivatives areė =˙ θ 2 cos θ 2 n+sin θ 2˙ n anḋ e a =˙ θ cos θ n + sin θ˙ n respectively.
We will now consider system (31) with a disturbance input Consider small disturbances which can occur by unmodelled environment dynamics. Such disturbances induce small errors to the system, disrupting the linear behavior of the above analysis. However, for small error angles θ < π 6 the approximations sin θ ≈ θ and cos θ ≈ 1 hold, thus (46) can be written as: ω G =˙ θ n + θ˙ n =ė l (53) making the system again linear.

Remark 3.
Considering the other two error selections, (53) also holds for the angle axis orientation error (23). Therefore, near the system's steady state the behavior of the two orientation errors is similar. However, as we discussed in Section III this error selection's performance deteriorates for large error angles.

VI. SIMULATION RESULTS
We will demonstrate the performance of controllers (19), (20) using all three types of orientation errors (21), (23), (25). For the numerical integration of the quaternion error we have used the exponential mapping (4) in order to preserve its unit norm: Q(t + ∆t) = exp 1 2 ω G (t + ∆t)∆t * Q(t) We present the results of two sets of simulations, using two different desired trajectories shown in Figure 2. Their main difference is that the second trajectory has a non zero initial angular velocity. For the first trajectory the system was initialized with θ 0 = 3π 4 , and zero angular velocity error, while for the second with θ 0 = 0 and angular velocity errors with initial norm ω G = ω E = 20.9rad/s. Therefore the two systems would cover the two cases of linearity of Proposition 2. The control gains were set κ = 30.25 and d = 11, for the linear system to be critically damped with settling time t s ≈ 1s. Figures 3 and 4 show responses of the error angle and the norm of the angular velocity error respectively for both systems, where (30) is depicted with dotted lines and (31) is depicted with solid lines and for all orientation error selections; the left subplot includes responses for the case of zero initial angular velocity while the right subplot those with a non-zero angular velocity. In Figure 3 we call "geometric", error angle responses from (31) and "Euclidean" those from (30). In Figure 4 we show the response of ω E and ω G . The subplot within this figure is a closer to the x-axis view. Responses converge exponentially to the origin as expected. For both trajectories, e l in system (31) responds with the expected speed as it is related to a linear tracking system according to Proposition 2. In all cases, it is evident that system (31) converges faster than (30), owing to its autonomous nature. Indeed, as shown in (43), the system's convergence rate is dependant on the bound of the norm of the desired angular velocity, rendering slower than (31). Notice that in both figures, due to the nonautonomous nature of system (30) the sinusoidal nature of the input is also present in the error system response.

VII. EXPERIMENTAL RESULTS
We implemented the methods studied in the previous sections using a UR5e robotic manipulator operating with control frequency 500Hz. The planned trajectory was designed to resemble that of a polishing task of the interior of a spherical surface. The position of the wrist was kept constant; we can imagine this as the center of the sphere with the polishing tool having the length of the sphere's radius. We again conducted two experiments, one with zero initial angular velocity and one with non zero. In both cases the system was initialized with error angle θ = π 3 . The two trajectories are shown in Figure 5. We integrated system (33) producing a reference trajectory Q, ω. Then the joint reference trajectory was provided to the robot utilizing a Closed Loop Inverse Kinematics scheme [34]. In our experiments we set as requirement a compliant behavior, thus system parameters were set to κ = d = 4, making the linear system critically damped with settling time t s ≈ 3.
Experimental results are shown in Figures 6, 7. All error selections are shown to converge exponentially to zero. In the first trajectory, the system using the logarithmic orientation error and the geometric angular velocity error, shows linear response. The second trajectory only demonstrates the exponential convergence of all systems, as it does not obey the conditions of Proposition 2, since for e l (0) = 0 and  ω G (0) = 0. Again with e l convergence is faster followed closely by the response with the use of e a . This is because the initial angle error in the experiment is much less to the one used in the simulations ( π 3 vs 3π 4 ) and soon θ becomes small enough for the approximation sin θ ≈ θ to hold; the latter implies e l ≈ e a . Furthermore responses from the geometric and Euclidean system are similar, since the bound of the desired angular velocity norm is not as large as in the simulations to create notable differences.
We performed an additional experiment with regulation control, with a large initial error angle θ(0) = 3π 4 . Notice that in the case of regulation control, where ω d = 0∀t, the two angular velocity errors become identical. Therefore there is not difference between system (31) and (30). The results are shown in Figure 8. The logarithmic orientation error is shown to reach faster the desired orientation, with settling time again t s ≈ 3s, as expected since it exhibits linear behavior. Due to the larger initial error angle, the performance of the angle axis orientation error is shown to deteriorate from the previous experiment.
We tested the performance of the logarithmic orientation error external inputs with an admittance control setup, using (52). A polishing tool was mounted to the robot's end effector, being in contact with a mechanical part. The setup is shown in Figure 9. To execute the task, Trajectory 1 from   Experimental results are shown in Figures 10 and 11. In Figure 10 the contact torque norm is shown to induce small error angles, less than π 6 . Thus the approximations discussed in Section V hold and the system should remain linear. This claim is further demonstrated in Figure 11, where the geometric angular velocity error is shown to be equal with the derivative of the logarithmic orientation error.

VIII. CONCLUSIONS
In this work we studied trajectory tracking in the orientation space using the unit quaternion representation. We presented existing variants regarding the orientation error  and angular velocity error. We have associated each of these choices with the appropriate orientation error function and rigorously proved exponential error convergence in all cases.
We have further showed that the formulation respecting the geometric characteristics of the quaternion manifold and its tangent space yields linear tracking dynamics under conditions frequently encountered in practice. Thus transient specification can be satisfied by appropriate gain selection without tuning. Theoretical results are validated by simulations and experiments.

APPENDIX ORIENTATION ERROR JACOBIAN MATRICES
We will now provide the Jacobian matrices used in equations (28), (29). When using the vector part orientation error (21), we can write (14) and (12) as (28) and (29) with: For the angle axis orientation error we differentiate (23) substituting˙ η and˙ from (13) (14) for j = G and (11) (12) for j = E to get: For the logarithmic orientation error when differentiating (25) we need˙ θ and˙ n, which can be found by differentiating the axis and angle representations of (2): Thus, we get: It is easy to prove using (25) that J l e l = e l . In fact J l has an eigenvalue of one and hence e l is the associated eigenvector. Notice that this property applies only for the logarithmic orientation error. By evaluating the singular values of J i . it can be shown that the norm of J i is given by: where Notice that 1 ≤ h( θ) ≤ π 2 ∀ θ ∈ [−π, π) and J a ≤ 1.