# Some Discussions about the Error Functions on SO(3) and SE(3) for the Guidance of a UAV Using the Screw Algebra Theory

Authors: Yi Zhu; Xin Chen; Chuntao Li
Journal: Advances in Mathematical Physics
 (2017)

---

## Abstract

In this paper a new error function designed on 3-dimensional special Euclidean group SE(3) is proposed for the guidance of a UAV (Unmanned Aerial Vehicle). In the beginning, a detailed 6-DOF (Degree of Freedom) aircraft model is formulated including 12 nonlinear differential equations. Secondly the definitions of the adjoint representations are presented to establish the relationships of the Lie groups SO(3) and SE(3) and their Lie algebras so(3) and se(3). After that the general situation of the differential equations with matrices belonging to SO(3) and SE(3) is presented. According to these equations the features of the error function on SO(3) are discussed. Then an error function on SE(3) is devised which creates a new way of error functions constructing. In the simulation a trajectory tracking example is given with a target trajectory being a curve of elliptic cylinder helix. The result shows that a better tracking performance is obtained with the new devised error function.


---

## Body

## 1. Introduction

The way of computing the tracking errors plays an important role in the guidance process of a UAV. For the problem of either a 2D tracking in a plane or a 3D tracking in the physical space, many valuable researches have been made about the guidance methods of “trajectory tracking” and “path following” [1].To solve the tracking problems, different researchers hold different opinions. The early methods somewhat originate from the target tracking of missiles such as proportional navigation, way point, and vector field method [2–4]. Then the body-mass point model is usually used so that the direct relationship between the position deviation and speed (or acceleration) can be concerned. Sometimes the influence of the attitude angles and the angular velocity of body frame is also taken into consideration [5]. On the contrary, the features of the inner loops of the aircraft system are often clearly figured out for lager aircrafts [6, 7]. For a 2D tracking issue there are some novel navigation methods and guidance strategies emerging in the light of geometrical intuition and physical interpretation [8–10]. Also, for the curves of 3D trajectories, the constraint of time parameter can be transformed into an arc length parameter by theory of differential geometry [11, 12].Actually, when a 6-DOF model of an aircraft is concerned, there are at least three basic coordinate frames included which are the inertial frame, the aircraft-body frame, and the airspeed frame. So the coordinate transformations between these different coordinate frames are directly related to the accuracy of the tracking errors computing, that is, where the error functions on SO(3) are used. For examples, in some literatures the guidance strategy is implemented based on a mixed structure of the attitude loops and guidance loops with controllers of the forces and moments [13, 14]. Another instance is the moving frame guidance method. This guidance method changes the ordinary error functions from the inertial frame to a moving frame by orthogonal matrices which belong to SO(3) [15].Many researches have been made about the formulation of a moving frame of a given trajectory, as recently in [16–19] and previously in [20, 21]. However, the designing of the error functions of a moving frame is a difficulty because there is interdisciplinary knowledge involved such as the Lie group theory. Some literatures indicate that the analyses about the Lie group can be simplified by the screw algebra theory [22]. Theses analyses are important particularly in the tracking process of aircrafts [23, 24]. So in this paper some discussions have been made to provide clear relationships between Lie groups SO(3) and SE(3) and their Lie algebras so(3) and se(3). Then some features of the error functions on SO(3) are proved before a new designed error function on SE(3) is proposed. Thus a new way of error functions constructing is presented. The effects of the different error functions are tested in the simulation with a 6-DOF UAV model.


## 2. Preliminary

### 2.1. UAV Model

A flight control system is a bit more complicated than ordinary control systems. The analytic expressions of 6-DOF motion of an aircraft, that is, the 12 nonlinear differential equations, are formulated as follows:(I)
Force equations:(1)u˙=vr-wq-gsin⁡θ+Fxmc,v˙=-ur+wp+gcos⁡θsin⁡ϕ+Fymc,w˙=uq-vp+gcos⁡θcos⁡ϕ+Fzmc.(II)
Kinematic equations:(2)ϕ˙=p+qsin⁡ϕ+rcos⁡ϕtan⁡θ,θ˙=qcos⁡ϕ-rsin⁡ϕ,ψ˙=qsin⁡ϕ+rcos⁡ϕcos⁡θ.(III)
Moment equations:(3)p˙=c1r+c2pq+c3L-+c4N,q˙=c5pr-c6p2-r2+c7M,r˙=c8p-c2rq+c4L-+c9N,where c1=(Iy-Iz)Iz-Ixz2/IxIz-Ixz2, c2=((Ix-Iy+Iz)Ixz)/IxIz-Ixz2, c3=Iz/IxIz-Ixz2, c4=Ixz/IxIz-Ixz2, c5=(Iz-Ix)/Iy, c6=Ixz/Iy, c7=1/Iy, c8=Ix(Ix-Iy)+Ixz2/IxIz-Ixz2, and c9=Ix/IxIz-Ixz2.(IV)
Navigation equations:(4)x˙g=ucos⁡θcos⁡ψ+vsin⁡ϕsin⁡θcos⁡ψ-cos⁡ϕsin⁡ψ+wsin⁡ϕsin⁡ψ+cos⁡ϕsin⁡θcos⁡ψ,y˙g=ucos⁡θsin⁡ψ+vsin⁡ϕsin⁡θsin⁡ψ+cos⁡ϕcos⁡ψ+w-sin⁡ϕcos⁡ψ+cos⁡ϕsin⁡θsin⁡ψ,h˙g=usin⁡θ-vsin⁡ϕcos⁡θ-wcos⁡ϕcos⁡θor(5)x˙g=Vcos⁡γcos⁡φ,y˙g=Vcos⁡γsin⁡φ,h˙g=Vsin⁡γ,where mc is the mass of the aircraft, Fi(i=x,y,z) represents the force of axes of aircraft-body coordinate frame, respectively, L-, M, and N are moments of body frame, u, v, and w are speed components of body frame, θ, ψ, and ϕ represent pitch angle, yaw angle, and bank angle, respectively, p, q, and r are angular velocity from body frame to inertial frame resolved in body frame, xg, yg, and hg represent the position of the aircraft in inertial frame, Ix, Iy, and Iz are rotary inertias of axes of body frame, V is true airspeed, and γ, φ are flight-path angles between the first/second axis of wind coordinate frame and inertial frame, respectively.In practice, we may not necessarily chooseu, v, and w as the state variables of an aircraft model concerning different requirements and we usually choose the true air speed V and aerodynamic angles, such as angle of attack α and sideslip angle β, instead. According to the rotation matrix RW2B from wind frame to body frame, along with the relationship Xbody=RW2BXwind, one has the equation(6)uvw=RW2BV00=Vcos⁡αcos⁡βVsin⁡βVsin⁡αcos⁡β.With (1)~(4) the 12 differential equations are obtained; however it is not adequate to establish a complete nonlinear model of a UAV. More additional parts are needed. Figure 1 shows the inner structure of the UAV dynamic model.Figure 1
The inner structure of the UAV model.Hereδe, δa, and δr are the angular deviations of the elevator, ailerons, and rudder, δT is the opening degree of throttle, and Mach represents Mach number. As shown in Figure 1, the actuators are δe, δa, δr, and δT rather than forces and moments. Then, it is viable to construct a simulation model of the given aircraft. By (1)~(4) a nonlinear state-space system can be obtained with state variables defined by XT=u,v,w,ϕ,θ,ψ,p,q,r,xg,yg,h and control input vector as UT=δe,δα,δr,δT.


### 2.2. Adjoint Representations of Lie Algebras

Before the features of the error functions on SO(3) and SE(3) are discussed, the features of SO(3) and SE(3) themselves should be made clear. In the following part, some basic concepts of the screw algebra and Lie group theory are discussed in detail.According to the screw algebra theory of motions of the rigid body, the definitions and adjoint representations of the 3-dimensional special orthogonal groupSO3, the 3-dimensional special Euclidean group SE3, and the corresponding Lie algebras so3 and se3 are presented as follows.(I) The3×3 matrix adjoint representation of so3 is as follows.s o 3 is the Lie algebra of the 3-dimensional special orthogonal group SO3, the adjoint representation of which has a form of a skew-symmetric matrix:(7)ads=As=s∧=0-szsysz0-sx-sysx0.Since the Lie algebra so3 is represented as a 3×3 skew-symmetric matrix, one basis of the 3-dimensional vector space in the form of matrices is(8)ads1=s1∧=00000-1010,ads2=s2∧=001000-100,ads3=s3∧=0-10100000,where s1=1,0,0T, s2=0,1,0T, and s3=0,0,1T.Any elements belonging toso3 can be represented as a linear combination of this basis.(II) The standard and adjoint representation ofse3 is as follows.s e 3 is the Lie algebra of the 3-dimensional special Euclidean group SE3. SEn, also denoted E+n, is defined to describe rigid body motions including translations and rotations, which is based on an identity of a rigid body motion and a curve in the Euclidean group.The standard4×4 matrix representation of se3 is (9)E=s∧s00T0,where s∧∈so3 and s0∈R3. These elements belong to a space which is a subset of R4×4. The generators of this 6-dimensional vector space are(10)E1=000000-1001000000,E2=00100000-10000000,E3=0-100100000000000,E4=0001000000000000,E5=0000000100000000,E6=0000000000010000.Also there is a 6×6 matrix adjoint representation of se3 defined as(11)U=adS=s∧0s0∧s∧,where the operator adS⊂R6×6 is isomorphic to E and the generators are(12)adS1=s1∧00s1∧,adS2=s2∧00s2∧,adS3=s3∧00s3∧,adS4=00s1∧0,adS5=00s2∧0,adS6=00s3∧0.We can see that the Lie algebra so3≅R3 is a subspace of se3≅R6, where the symbol ≅ means isomorphic.(III) The exponential mapping is as follows.The exponential mapping establishes a connection betweenso3 and SO3, se3 and SE3 as well. According to the Rodrigues equation, when the rotation axis s and the revolute joint θ are given, the rotation matrix R can be obtained as (13)R=eθAs=I+sin⁡θAs+1-cos⁡θAs2,where s∈R3, As=s×∈so3, and R∈SO3.Formula (13) presents the exponential mapping from so3 to SO3, the proof of which can be found in literature [22].Similarly, the exponential mapping fromse3 to SE3 is defined as (14)H=eθE=eθAsθs00T0=Rd0T1=eθAsVs00T1,where E is the standard 4×4 matrix representation of se3, and(15)V=θI+1-cos⁡θAs+θ-sin⁡θAs2.(IV) The relationship between SO3 and SE3 is as follows.Special Euclidean groupSE3 is a closed subgroup of 3-dimensional affine group Aff3. SE3 can be represented as a semidirect product of the special orthogonal group SO3 and the translation group T3; that is,(16)SE3≅SO3∝T3.The geometric meaning of above semidirect product is a rotation motion acting on a translation.Furthermore, a6×6 finite displacement screw matrix is defined as(17)N=R0ARR=I0AIR00R=NtNr,where rotation matrix R∈SO3 and A is a skew-symmetric matrix of translation action. Then we can see that(18)det⁡N=det⁡R0ARR=det⁡Rdet⁡R=1.So the finite displacement screw matrix belongs to the special linear group SLn, which is a subgroup of the general linear group GLn. Also N is an element of the Lie group SE3.


## 2.1. UAV Model

A flight control system is a bit more complicated than ordinary control systems. The analytic expressions of 6-DOF motion of an aircraft, that is, the 12 nonlinear differential equations, are formulated as follows:(I)
Force equations:(1)u˙=vr-wq-gsin⁡θ+Fxmc,v˙=-ur+wp+gcos⁡θsin⁡ϕ+Fymc,w˙=uq-vp+gcos⁡θcos⁡ϕ+Fzmc.(II)
Kinematic equations:(2)ϕ˙=p+qsin⁡ϕ+rcos⁡ϕtan⁡θ,θ˙=qcos⁡ϕ-rsin⁡ϕ,ψ˙=qsin⁡ϕ+rcos⁡ϕcos⁡θ.(III)
Moment equations:(3)p˙=c1r+c2pq+c3L-+c4N,q˙=c5pr-c6p2-r2+c7M,r˙=c8p-c2rq+c4L-+c9N,where c1=(Iy-Iz)Iz-Ixz2/IxIz-Ixz2, c2=((Ix-Iy+Iz)Ixz)/IxIz-Ixz2, c3=Iz/IxIz-Ixz2, c4=Ixz/IxIz-Ixz2, c5=(Iz-Ix)/Iy, c6=Ixz/Iy, c7=1/Iy, c8=Ix(Ix-Iy)+Ixz2/IxIz-Ixz2, and c9=Ix/IxIz-Ixz2.(IV)
Navigation equations:(4)x˙g=ucos⁡θcos⁡ψ+vsin⁡ϕsin⁡θcos⁡ψ-cos⁡ϕsin⁡ψ+wsin⁡ϕsin⁡ψ+cos⁡ϕsin⁡θcos⁡ψ,y˙g=ucos⁡θsin⁡ψ+vsin⁡ϕsin⁡θsin⁡ψ+cos⁡ϕcos⁡ψ+w-sin⁡ϕcos⁡ψ+cos⁡ϕsin⁡θsin⁡ψ,h˙g=usin⁡θ-vsin⁡ϕcos⁡θ-wcos⁡ϕcos⁡θor(5)x˙g=Vcos⁡γcos⁡φ,y˙g=Vcos⁡γsin⁡φ,h˙g=Vsin⁡γ,where mc is the mass of the aircraft, Fi(i=x,y,z) represents the force of axes of aircraft-body coordinate frame, respectively, L-, M, and N are moments of body frame, u, v, and w are speed components of body frame, θ, ψ, and ϕ represent pitch angle, yaw angle, and bank angle, respectively, p, q, and r are angular velocity from body frame to inertial frame resolved in body frame, xg, yg, and hg represent the position of the aircraft in inertial frame, Ix, Iy, and Iz are rotary inertias of axes of body frame, V is true airspeed, and γ, φ are flight-path angles between the first/second axis of wind coordinate frame and inertial frame, respectively.In practice, we may not necessarily chooseu, v, and w as the state variables of an aircraft model concerning different requirements and we usually choose the true air speed V and aerodynamic angles, such as angle of attack α and sideslip angle β, instead. According to the rotation matrix RW2B from wind frame to body frame, along with the relationship Xbody=RW2BXwind, one has the equation(6)uvw=RW2BV00=Vcos⁡αcos⁡βVsin⁡βVsin⁡αcos⁡β.With (1)~(4) the 12 differential equations are obtained; however it is not adequate to establish a complete nonlinear model of a UAV. More additional parts are needed. Figure 1 shows the inner structure of the UAV dynamic model.Figure 1
The inner structure of the UAV model.Hereδe, δa, and δr are the angular deviations of the elevator, ailerons, and rudder, δT is the opening degree of throttle, and Mach represents Mach number. As shown in Figure 1, the actuators are δe, δa, δr, and δT rather than forces and moments. Then, it is viable to construct a simulation model of the given aircraft. By (1)~(4) a nonlinear state-space system can be obtained with state variables defined by XT=u,v,w,ϕ,θ,ψ,p,q,r,xg,yg,h and control input vector as UT=δe,δα,δr,δT.


## 2.2. Adjoint Representations of Lie Algebras

Before the features of the error functions on SO(3) and SE(3) are discussed, the features of SO(3) and SE(3) themselves should be made clear. In the following part, some basic concepts of the screw algebra and Lie group theory are discussed in detail.According to the screw algebra theory of motions of the rigid body, the definitions and adjoint representations of the 3-dimensional special orthogonal groupSO3, the 3-dimensional special Euclidean group SE3, and the corresponding Lie algebras so3 and se3 are presented as follows.(I) The3×3 matrix adjoint representation of so3 is as follows.s o 3 is the Lie algebra of the 3-dimensional special orthogonal group SO3, the adjoint representation of which has a form of a skew-symmetric matrix:(7)ads=As=s∧=0-szsysz0-sx-sysx0.Since the Lie algebra so3 is represented as a 3×3 skew-symmetric matrix, one basis of the 3-dimensional vector space in the form of matrices is(8)ads1=s1∧=00000-1010,ads2=s2∧=001000-100,ads3=s3∧=0-10100000,where s1=1,0,0T, s2=0,1,0T, and s3=0,0,1T.Any elements belonging toso3 can be represented as a linear combination of this basis.(II) The standard and adjoint representation ofse3 is as follows.s e 3 is the Lie algebra of the 3-dimensional special Euclidean group SE3. SEn, also denoted E+n, is defined to describe rigid body motions including translations and rotations, which is based on an identity of a rigid body motion and a curve in the Euclidean group.The standard4×4 matrix representation of se3 is (9)E=s∧s00T0,where s∧∈so3 and s0∈R3. These elements belong to a space which is a subset of R4×4. The generators of this 6-dimensional vector space are(10)E1=000000-1001000000,E2=00100000-10000000,E3=0-100100000000000,E4=0001000000000000,E5=0000000100000000,E6=0000000000010000.Also there is a 6×6 matrix adjoint representation of se3 defined as(11)U=adS=s∧0s0∧s∧,where the operator adS⊂R6×6 is isomorphic to E and the generators are(12)adS1=s1∧00s1∧,adS2=s2∧00s2∧,adS3=s3∧00s3∧,adS4=00s1∧0,adS5=00s2∧0,adS6=00s3∧0.We can see that the Lie algebra so3≅R3 is a subspace of se3≅R6, where the symbol ≅ means isomorphic.(III) The exponential mapping is as follows.The exponential mapping establishes a connection betweenso3 and SO3, se3 and SE3 as well. According to the Rodrigues equation, when the rotation axis s and the revolute joint θ are given, the rotation matrix R can be obtained as (13)R=eθAs=I+sin⁡θAs+1-cos⁡θAs2,where s∈R3, As=s×∈so3, and R∈SO3.Formula (13) presents the exponential mapping from so3 to SO3, the proof of which can be found in literature [22].Similarly, the exponential mapping fromse3 to SE3 is defined as (14)H=eθE=eθAsθs00T0=Rd0T1=eθAsVs00T1,where E is the standard 4×4 matrix representation of se3, and(15)V=θI+1-cos⁡θAs+θ-sin⁡θAs2.(IV) The relationship between SO3 and SE3 is as follows.Special Euclidean groupSE3 is a closed subgroup of 3-dimensional affine group Aff3. SE3 can be represented as a semidirect product of the special orthogonal group SO3 and the translation group T3; that is,(16)SE3≅SO3∝T3.The geometric meaning of above semidirect product is a rotation motion acting on a translation.Furthermore, a6×6 finite displacement screw matrix is defined as(17)N=R0ARR=I0AIR00R=NtNr,where rotation matrix R∈SO3 and A is a skew-symmetric matrix of translation action. Then we can see that(18)det⁡N=det⁡R0ARR=det⁡Rdet⁡R=1.So the finite displacement screw matrix belongs to the special linear group SLn, which is a subgroup of the general linear group GLn. Also N is an element of the Lie group SE3.


## 3. Error Functions Defined on SO(3) and SE(3)

### 3.1. General Situation

In the beginning of this section an example is introduced to show the features of equations with matrices belonging toSE3. The following equations are given:(19)R˙=RΩ∧,P˙=RV,where R∈SO3 and P∈R3. Introduce the matrices P, G defined by(20)P=RP01×31,G=Ω∧V01×31,where P∈SE3, G∈se3 are both 4×4 matrices. The following equation holds:(21)P˙=P·G.Also, there is 6×6 matrix representation of elements of SE3:(22)G~=Ω∧0V∧Ω∧.These two adjoint representations of 4×4 and 6×6 matrices, that is, G and G~, are isomorphic to each other. se3, the Lie algebra of SE3, is isomorphic to SE3 as well. It is convenient to choose different forms we need in different situations. However, it is not difficult to verify that the 6×6 matrix representations of SE3 do not satisfy (21).This is different from the previous error functions which are defined onSO3, which is the subgroup of SE3, such as(23)ΦR,Rd=12trI-RdTR.In this paper a trial has been made to define an error function straightforwardly on SE3, so that the error function will include both the information of the rotation matrix and the position vectors or the speed vectors. The new error function on SE3 is defined by(24)ΨR,Rd,P,Pd=12trP-PdTP.Actually, (24) has a close relationship with ΦR,Rd. Since(25)P-PdTP=RT-RdT03×1PT-PdT1RP01×31=RT-RdTRRT-RdTPPT-PdTRPT-PdTP,thus(26)ΨR,Rd,P,Pd=12trP-PdTP=12trI-RdTR00P2-PdTP=12trI-RdTR+12P2-PdTP.Hence one can see that with an initial position P0 and a trajectory Pd, as long as a negative feedback of position signals is guaranteed, the position error ΔP is certain to be a decreasing function when it tends to the steady state. So the error function P2-PdTP is bounded. Let supP2-PdTP=D; then the domain of attraction of Ψ is regarded as a linear manifold of Φ. That means some features about the error function Φ defined on SO3 will still be helpful.


### 3.2. Error Function on SO(3)

To choose the tracking error vectorseR and eΩ reasonably, let(27)Ψ-=12trI-RdTR+D.By finding the derivative of (27) we have(28)Ψ-˙=Φ˙=-12trRdTR˙=-12trRdTRΩ∧.Before further discussion, the following properties of 3-order skew-symmetric matrices are presented:(29)ItrAx∧=12trx∧A-AT,(30)II12trx∧A-AT=-xTA-AT∨,(31)IIIx·y∧z=y·z∧x,property  of  the  vector  mixed  product,(32)IVx∧y=x×y=-y×x=-y∧x,(33)Vx∧y∧z=x×y×z=y·x·z-z·x·yproperty  of  the  vector  triple  product,(34)VIx∧A+ATx∧=trAI3×3-Ax∧,(35)VIIRx∧RT=Rx∧,(36)VIIIR˙-R˙dRdTR=RΩ-RTRdΩd∧.Some proofs of (29)~(36) are clearly given [23, 24]; hence only the proofs of (29), (30), and (36) are given here. First of all, Theorem 1 is presented.Theorem 1.
LetA, B be n×m and m×n matrices, respectively; then the following equation holds:(37)trAB=trBA.Proof.
Let(38)A=a11⋯a1ma21⋯a2m⋮⋱⋮an1⋯anm=aij,i=1,2,…,n,j=1,2,…,m,B=b11⋯b1nb21⋯b2n⋮⋱⋮bm1⋯bmn=bij,i=1,2,…,m,j=1,2,…,n,and thus
A B = c i j is a square matrix of n order, where cij=∑k=1maikbkj,i,j=1,2,…,n.
B A = d i j is a square matrix of m order, where dij=∑k=1nbikakj,i,j=1,2,…,m:(39)trAB=∑i=1ncii=∑i=1n∑k=1maikbki,trBA=∑i=1mdii=∑i=1m∑k=1nbikaki=∑k=1n∑i=1makibik=∑i=1n∑k=1maikbki.So trAB=trBA, proof finished.In addition, by the definition of the trace of a matrix, for any square matrixA, obviously we have(40)trA=trAT.Then the proof of (29) is presented as follows.Proof of (29).
By (37), (40), and the property of skew-symmetric matrix that(41)-x∧T=x∧,we have(42)trAx∧=12trx∧A+trAx∧=12trx∧A+trx∧TAT=12trx∧A+tr-x∧AT=12trx∧A-x∧AT=12trx∧A-AT.Proof finished.Proof of (30).
For a three-order square matrixA, it is easy to see that A-AT is a skew-symmetric matrix. Denoting(43)x=x1,x2,x3T,A-AT∨=z=z1,z2,z3T,then(44)12trx∧A-AT=12tr0-x3x2x30-x1-x2x100-z3z2z30-z1-z2z10=12-x2z2-x3z3+-x1z1-x3z3+-x1z1-x2z2=-x1z1+x2z2+x3z3=-xTA-AT∨.Proof finished.By the definition of the inner product, one has that(45)-xTA-AT∨=-A-AT∨·x.By (29), (30), and (45), the following equation holds:(46)trAx∧=-A-AT∨·x.With regard to (46), (28) can be rewritten as(47)Ψ-˙=-12trRdTRΩ∧=12RdTR-RTRd∨Ω,where RdTR-RTRd∈so3 is a skew-symmetric matrix, ∨ is the inverse mapping of the hat mapping ∧. Thus, the tracking error function of attitude can be defined as(48)eR=12RdTR-RTRd∨.Then the proof of (36) is presented as follows.Proof of (36).
According to the rule of finding the derivatives of the rotation matrices with respect to time, we have that(49)R˙=RΩ∧,R˙d=RdΩd∧.According to (35)(50)R˙-R˙dRdTR=RΩ∧-RdΩd∧RdTR=RΩ∧RT-RdΩd∧RdTR=RΩ∧-RdΩd∧R=RΩ-RdΩd∧R=RΩ-RTRdΩd∧R=RΩ-RTRdΩd∧RTR=RΩ-RTRdΩd∧.Proof finished.So, we can choose(51)eΩ=Ω-RTRdΩd,as the tracking error function of angular velocity vector. Actually, eΩ is the angular velocity of the rotation matrix RdTR, which is represented in the body frame, because of the following formulation:(52)dRdTRdt=RdTReΩ.The proof of (52) is given as follows.Proof of (52).
(53) d R d T R d t = d R d T d t R + R d T d R d t = - Ω d ∧ R d T R + R d T R Ω ∧ = R d T R Ω ∧ - R d T R R T R d Ω d ∧ R d T R = R d T R Ω ∧ - R T R d Ω d ∧ = R d T R Ω - R T R d Ω d ∧ = R d T R e Ω .Proof finished.In getting the above conclusion the following equations are used:(54)R˙d=RdΩd∧,dRdTdt=R˙dT=RdΩd∧T=Ωd∧TRdT=-Ωd∧RdT.


### 3.3. Error Function on SE(3)

As mentioned above, the error functionΨ is defined as (24) by the 4×4 adjoint matrix representation of elements on SE(3). The benefit of this adjoint representation rests with the simplicity of defining the semidirect product. However, the form of 6×6 matrix representation is adopted here for the convenience of calculation. For a 6×6 matrix N∈SE(3), according to the principle of Chasles motion decomposition, one has(55)N=R0ARR=NlNc=I0lAsIR0re∧RR,where R∈SO3, A∈so3, and(56)A=Ae+Al=Ae+lAs=re∧+lAs,l being the Frobenius norm of matrices. By the definition of Frobenius norm, for a matrix A∈Rm×n(57)AF≜∑i=1m∑j=1naij21/2=trATA1/2.For a three-order skew-symmetric matrix Ax=x∧=0-x3x2x30-x1-x2x10 which is obtained by a hat mapping, its Frobenius norm is(58)AxF=2x12+x22+x321/2.Sometimes when it is necessary to change the pitch parameter of a screw and the variable h is added, Nh=I0hII, then(59)N~=NhN=I0hIIR0ARR=R0A~RR,where A~=h+A. See (59) for details and it is can be seen that N~∉SE(3) because A~∉so3. Before further discussion of N~, another theorem is presented.Theorem 2 (the Laplace’s expansion theorem).
Ifk rows (or k columns) of n-order determinant D are selected, where 1≤k≤n-1, the sum of products of all the k-order subdeterminants of elements of the k rows (or k columns) and the corresponding algebraic cofactors equals the value of the determinant D.Detailed discussions of Laplace’s expansion theorem can easily be found in teaching materials of matrix theory or linear algebra, so the proof is omitted here. According to Theorem2, for a block lower (upper) triangular matrix(60)A=Bm×m0∗Cn×n,or (61)A=Bm×m∗0Cn×n,in all the subdeterminants of the first m rows of det⁡A, only one of them is nonzero. Thus, by expansion of the first m rows, the following deduction is obtained:(62)det⁡A=det⁡Bdet⁡C.So det⁡N~=R0A~RR=det⁡Rdet⁡R=1, and we can see that N~∈SL3 (the special linear group), which is a subgroup of general linear group GL3.Similar to (23) and (24), a new error function is defined by(63)Ξ1=trNdTN-NTNd,where, with regard to (59), Nd=RH2I0A~HRH2IRH2I∈SL(3), A~H=AH+hI=ωH∧+hI, ωH is the Darboux vector of the frame H and ωH∧∈so3, N=RB2I0ABRB2IRB2I∈SE(3), and AB=ωDarboux∣Bishop∧, substituted into (63), and we get(64)Ξ1=trRH2ITRH2ITA~HT0RH2ITRB2I0ABRB2IRB2I-RB2ITRB2ITABT0RB2ITRH2I0A~HRH2IRH2I=trRH2ITRB2I+RH2ITA~HTABRB2IRH2ITA~HTRB2IRH2ITABRB2IRH2ITRB2I-RB2ITRH2I+RB2ITABTA~HRH2IRB2ITABTRH2IRB2ITA~HRH2IRB2ITRH2I=trRH2ITRB2I+RH2ITA~HTABRB2I-RB2ITRH2I+RB2ITABTA~HRH2I+RH2ITRB2I-RB2ITRH2I=tr2RH2ITRB2I-RB2ITRH2I+RH2ITA~HTABRB2I-RB2ITABTA~HRH2I.Let M1=RH2ITRB2I-RB2ITRH2I and M2=RH2ITA~HTABRB2I-RB2ITABTA~HRH2I; then we have Ξ1=tr2M1+M2. Obviously, M1,M2 are both skew-symmetric matrices belonging to so3. Since elements of so3 have closure property with additive operation, thus 2M1+M2∈so3. Another vector ωHB=ωH-ωB is defined where ωB=AB∨=ωDarboux∣Bishop and another error function is defined by(65)Ξ2=12trωHB∧2M1+M2.By (30) (66)Ξ2=-ωHBT2M1+M2∨.


## 3.1. General Situation

In the beginning of this section an example is introduced to show the features of equations with matrices belonging toSE3. The following equations are given:(19)R˙=RΩ∧,P˙=RV,where R∈SO3 and P∈R3. Introduce the matrices P, G defined by(20)P=RP01×31,G=Ω∧V01×31,where P∈SE3, G∈se3 are both 4×4 matrices. The following equation holds:(21)P˙=P·G.Also, there is 6×6 matrix representation of elements of SE3:(22)G~=Ω∧0V∧Ω∧.These two adjoint representations of 4×4 and 6×6 matrices, that is, G and G~, are isomorphic to each other. se3, the Lie algebra of SE3, is isomorphic to SE3 as well. It is convenient to choose different forms we need in different situations. However, it is not difficult to verify that the 6×6 matrix representations of SE3 do not satisfy (21).This is different from the previous error functions which are defined onSO3, which is the subgroup of SE3, such as(23)ΦR,Rd=12trI-RdTR.In this paper a trial has been made to define an error function straightforwardly on SE3, so that the error function will include both the information of the rotation matrix and the position vectors or the speed vectors. The new error function on SE3 is defined by(24)ΨR,Rd,P,Pd=12trP-PdTP.Actually, (24) has a close relationship with ΦR,Rd. Since(25)P-PdTP=RT-RdT03×1PT-PdT1RP01×31=RT-RdTRRT-RdTPPT-PdTRPT-PdTP,thus(26)ΨR,Rd,P,Pd=12trP-PdTP=12trI-RdTR00P2-PdTP=12trI-RdTR+12P2-PdTP.Hence one can see that with an initial position P0 and a trajectory Pd, as long as a negative feedback of position signals is guaranteed, the position error ΔP is certain to be a decreasing function when it tends to the steady state. So the error function P2-PdTP is bounded. Let supP2-PdTP=D; then the domain of attraction of Ψ is regarded as a linear manifold of Φ. That means some features about the error function Φ defined on SO3 will still be helpful.


## 3.2. Error Function on SO(3)

To choose the tracking error vectorseR and eΩ reasonably, let(27)Ψ-=12trI-RdTR+D.By finding the derivative of (27) we have(28)Ψ-˙=Φ˙=-12trRdTR˙=-12trRdTRΩ∧.Before further discussion, the following properties of 3-order skew-symmetric matrices are presented:(29)ItrAx∧=12trx∧A-AT,(30)II12trx∧A-AT=-xTA-AT∨,(31)IIIx·y∧z=y·z∧x,property  of  the  vector  mixed  product,(32)IVx∧y=x×y=-y×x=-y∧x,(33)Vx∧y∧z=x×y×z=y·x·z-z·x·yproperty  of  the  vector  triple  product,(34)VIx∧A+ATx∧=trAI3×3-Ax∧,(35)VIIRx∧RT=Rx∧,(36)VIIIR˙-R˙dRdTR=RΩ-RTRdΩd∧.Some proofs of (29)~(36) are clearly given [23, 24]; hence only the proofs of (29), (30), and (36) are given here. First of all, Theorem 1 is presented.Theorem 1.
LetA, B be n×m and m×n matrices, respectively; then the following equation holds:(37)trAB=trBA.Proof.
Let(38)A=a11⋯a1ma21⋯a2m⋮⋱⋮an1⋯anm=aij,i=1,2,…,n,j=1,2,…,m,B=b11⋯b1nb21⋯b2n⋮⋱⋮bm1⋯bmn=bij,i=1,2,…,m,j=1,2,…,n,and thus
A B = c i j is a square matrix of n order, where cij=∑k=1maikbkj,i,j=1,2,…,n.
B A = d i j is a square matrix of m order, where dij=∑k=1nbikakj,i,j=1,2,…,m:(39)trAB=∑i=1ncii=∑i=1n∑k=1maikbki,trBA=∑i=1mdii=∑i=1m∑k=1nbikaki=∑k=1n∑i=1makibik=∑i=1n∑k=1maikbki.So trAB=trBA, proof finished.In addition, by the definition of the trace of a matrix, for any square matrixA, obviously we have(40)trA=trAT.Then the proof of (29) is presented as follows.Proof of (29).
By (37), (40), and the property of skew-symmetric matrix that(41)-x∧T=x∧,we have(42)trAx∧=12trx∧A+trAx∧=12trx∧A+trx∧TAT=12trx∧A+tr-x∧AT=12trx∧A-x∧AT=12trx∧A-AT.Proof finished.Proof of (30).
For a three-order square matrixA, it is easy to see that A-AT is a skew-symmetric matrix. Denoting(43)x=x1,x2,x3T,A-AT∨=z=z1,z2,z3T,then(44)12trx∧A-AT=12tr0-x3x2x30-x1-x2x100-z3z2z30-z1-z2z10=12-x2z2-x3z3+-x1z1-x3z3+-x1z1-x2z2=-x1z1+x2z2+x3z3=-xTA-AT∨.Proof finished.By the definition of the inner product, one has that(45)-xTA-AT∨=-A-AT∨·x.By (29), (30), and (45), the following equation holds:(46)trAx∧=-A-AT∨·x.With regard to (46), (28) can be rewritten as(47)Ψ-˙=-12trRdTRΩ∧=12RdTR-RTRd∨Ω,where RdTR-RTRd∈so3 is a skew-symmetric matrix, ∨ is the inverse mapping of the hat mapping ∧. Thus, the tracking error function of attitude can be defined as(48)eR=12RdTR-RTRd∨.Then the proof of (36) is presented as follows.Proof of (36).
According to the rule of finding the derivatives of the rotation matrices with respect to time, we have that(49)R˙=RΩ∧,R˙d=RdΩd∧.According to (35)(50)R˙-R˙dRdTR=RΩ∧-RdΩd∧RdTR=RΩ∧RT-RdΩd∧RdTR=RΩ∧-RdΩd∧R=RΩ-RdΩd∧R=RΩ-RTRdΩd∧R=RΩ-RTRdΩd∧RTR=RΩ-RTRdΩd∧.Proof finished.So, we can choose(51)eΩ=Ω-RTRdΩd,as the tracking error function of angular velocity vector. Actually, eΩ is the angular velocity of the rotation matrix RdTR, which is represented in the body frame, because of the following formulation:(52)dRdTRdt=RdTReΩ.The proof of (52) is given as follows.Proof of (52).
(53) d R d T R d t = d R d T d t R + R d T d R d t = - Ω d ∧ R d T R + R d T R Ω ∧ = R d T R Ω ∧ - R d T R R T R d Ω d ∧ R d T R = R d T R Ω ∧ - R T R d Ω d ∧ = R d T R Ω - R T R d Ω d ∧ = R d T R e Ω .Proof finished.In getting the above conclusion the following equations are used:(54)R˙d=RdΩd∧,dRdTdt=R˙dT=RdΩd∧T=Ωd∧TRdT=-Ωd∧RdT.


## 3.3. Error Function on SE(3)

As mentioned above, the error functionΨ is defined as (24) by the 4×4 adjoint matrix representation of elements on SE(3). The benefit of this adjoint representation rests with the simplicity of defining the semidirect product. However, the form of 6×6 matrix representation is adopted here for the convenience of calculation. For a 6×6 matrix N∈SE(3), according to the principle of Chasles motion decomposition, one has(55)N=R0ARR=NlNc=I0lAsIR0re∧RR,where R∈SO3, A∈so3, and(56)A=Ae+Al=Ae+lAs=re∧+lAs,l being the Frobenius norm of matrices. By the definition of Frobenius norm, for a matrix A∈Rm×n(57)AF≜∑i=1m∑j=1naij21/2=trATA1/2.For a three-order skew-symmetric matrix Ax=x∧=0-x3x2x30-x1-x2x10 which is obtained by a hat mapping, its Frobenius norm is(58)AxF=2x12+x22+x321/2.Sometimes when it is necessary to change the pitch parameter of a screw and the variable h is added, Nh=I0hII, then(59)N~=NhN=I0hIIR0ARR=R0A~RR,where A~=h+A. See (59) for details and it is can be seen that N~∉SE(3) because A~∉so3. Before further discussion of N~, another theorem is presented.Theorem 2 (the Laplace’s expansion theorem).
Ifk rows (or k columns) of n-order determinant D are selected, where 1≤k≤n-1, the sum of products of all the k-order subdeterminants of elements of the k rows (or k columns) and the corresponding algebraic cofactors equals the value of the determinant D.Detailed discussions of Laplace’s expansion theorem can easily be found in teaching materials of matrix theory or linear algebra, so the proof is omitted here. According to Theorem2, for a block lower (upper) triangular matrix(60)A=Bm×m0∗Cn×n,or (61)A=Bm×m∗0Cn×n,in all the subdeterminants of the first m rows of det⁡A, only one of them is nonzero. Thus, by expansion of the first m rows, the following deduction is obtained:(62)det⁡A=det⁡Bdet⁡C.So det⁡N~=R0A~RR=det⁡Rdet⁡R=1, and we can see that N~∈SL3 (the special linear group), which is a subgroup of general linear group GL3.Similar to (23) and (24), a new error function is defined by(63)Ξ1=trNdTN-NTNd,where, with regard to (59), Nd=RH2I0A~HRH2IRH2I∈SL(3), A~H=AH+hI=ωH∧+hI, ωH is the Darboux vector of the frame H and ωH∧∈so3, N=RB2I0ABRB2IRB2I∈SE(3), and AB=ωDarboux∣Bishop∧, substituted into (63), and we get(64)Ξ1=trRH2ITRH2ITA~HT0RH2ITRB2I0ABRB2IRB2I-RB2ITRB2ITABT0RB2ITRH2I0A~HRH2IRH2I=trRH2ITRB2I+RH2ITA~HTABRB2IRH2ITA~HTRB2IRH2ITABRB2IRH2ITRB2I-RB2ITRH2I+RB2ITABTA~HRH2IRB2ITABTRH2IRB2ITA~HRH2IRB2ITRH2I=trRH2ITRB2I+RH2ITA~HTABRB2I-RB2ITRH2I+RB2ITABTA~HRH2I+RH2ITRB2I-RB2ITRH2I=tr2RH2ITRB2I-RB2ITRH2I+RH2ITA~HTABRB2I-RB2ITABTA~HRH2I.Let M1=RH2ITRB2I-RB2ITRH2I and M2=RH2ITA~HTABRB2I-RB2ITABTA~HRH2I; then we have Ξ1=tr2M1+M2. Obviously, M1,M2 are both skew-symmetric matrices belonging to so3. Since elements of so3 have closure property with additive operation, thus 2M1+M2∈so3. Another vector ωHB=ωH-ωB is defined where ωB=AB∨=ωDarboux∣Bishop and another error function is defined by(65)Ξ2=12trωHB∧2M1+M2.By (30) (66)Ξ2=-ωHBT2M1+M2∨.


## 4. Simulations and Analysis

Figure2 shows an overview structure of the whole flight control system. It is can be seen that there are inner loops (attitude loops and trajectory loops) since the 6-DOF model of the UAV is used. However, the designing of the inner loops is independent of the designing of error functions. So the inner loop controllers are all chosen as ordinary ones.Figure 2
Overview structure of the devised flight control system.In the simulations, the employed UAV model originates from an improved and trial type of China’s “Sharp Sword” unmanned combat aerial vehicle. Its three-view drawing is shown in Figure3.Figure 3
Three-view drawing of the UAV used in the simulations.The main data of the UAV are shown in Table1. From Table 1 it is can be seen that the UAV has a big size of 2300 kg; thus the target trajectory is also a large curve.Table 1
Data of the UAV in simulations.
Aerodynamic configuration Flying-wing configuration Mass of UAV 2.3 × 1 0 3 kg Inertiaproperties I x = 2.6242 × 1 0 3 Wing span of UAV 8.76 m I y = 4.5183 × 1 0 3 Max thrust(single engine of two) 500 kgf I z = 8.9088 × 1 0 3 Max Mach 0.85 I x y = I z y = I x z = 0 Maximum flight altitude 15 kmThe target trajectory is chosen as an elliptic cylinder helix extending along the horizontal direction. The expression of the given trajectory with regard to a time parameter is defined as(67)rt=a1t+c1,a2cos⁡b1t+c2,a3sin⁡b2t+c3T,where a1,a2,a3,b1,b2,c1,c2,c3 are all the coefficients. The beginning of the tracking action is from the moment the UAV arrives in a small neighborhood of any point of the target trajectory. The initial speed vector of body frame is V0=200,0,0 and the attitude angles vector is ϕ,θ,ψ0=0,2,0, where the units are, respectively, “m/s” and “°.” According to the initial condition of the UAV, the values of the coefficients of (67) are finally chosen by(68)a1=200,a2=300,a3=-250,b1=0.1,b2=0.1,c1=0,c2=-300,c3=-3000.The curve of the target trajectory rt is shown as in Figure 4(a).Figure 4
(a) Curve of the target trajectory. (b) A comparison of the forward errors. (c) Curve of the pitch angle. (d) Force of the thrust.
(a) (b) (c) (d)For the similarity of longitudinal and lateral channels of the flight control system, here just take the tracking errors in axis-x for example. In Figure 4(b), the magnitude of tracking error is largest with the ordinary error function. When the error function on SO(3) is added, the tracking error decreases significantly. When the error function on SE(3) is added, the tracking error decreases more. It should be noted that in Figure 4(b) the condition “error function on SE(3)” means “error function on SE(3) is added” so the previous error functions are still being used.Figure4(c) shows the tracking curve of the pitch angle θ. The pitch angle is limited in the reasonable ranges with amplitude limits of 20° and 30°. The pitch angle ranges within ±8°. Some slight buffeting near the vertex of the curve of the pitch angle has something to do with the inner loop controllers. Figure 4(d) shows the curve of the single thrust T and there are two engines. In most literatures there is a supposition that the thrust of the aircraft is large enough. Actually a too large thrust means a difficulty in rapid reduction of the forward speed. The UAV mass is about 2300 kg and in simulation the maximum thrust weight ratio is about 0.32. The first 30 s of Figure 4(d) indicates a process of rapid convergence of the forward tracking error. The thrust is controlled directly by the opening degree of the throttle which is constrained in the closed interval [0,1].


## 5. Conclusions

According to the nonlinear model of a UAV, a 3D trajectory tracking method is devised. Efforts have been made to discuss the features about the error functions on SO(3) and SE(3). The tracking effect of the flight control system is tested in the numerical simulation. The result of the simulations shows a satisfactory tracking performance so that the error functions designed in this paper are feasible in the UAV tracking process. The designing of the new error function on SE(3) provides a new way of error functions constructing in solving the guidance problem of the UAV.



---

*Source: 1016530-2017-01-04.xml*