Subsumption architecture applied to flight control using composite rotations

Subsumption Architecture Applied to Flight Control using Composite Rotations

E. Oland, T. S. Andersen, R. Kristiansen

UiT - The Arctic University of Norway, Faculty of Engineering Science and Technology, Norway

Abstract

In this paper the subsumption theory is applied to flight control through composite rotations where multiple tasks can be defined as simple rotations. The tasks can then be arranged as a hierarchy, where the primary task is always fully pursued, and conflicting lower level tasks are removed by the primary rotation. The concept is applied to a group ofuavs that move through an urban terrain while avoiding collisions with the ground, the buildings and otheruavs as they track a desired waypoint.

Key words: Guidance, navigation and control of vehicles, autonomous systems, application of nonlinear analysis and design.

1 Introduction

To facilitate the design of a truly autonomous agent, the controller must enable the agent to perform multi- ple tasks in a complex dynamic environment. The lay- ered approach to behavioral control is presented in [3], where multiple tasks are arranged in a hierarchy and where a higher level task subsumes (or suppresses) lower level tasks. This ensures that the primary task is always fully pursued, while the lower level tasks are fully pur- sued as the higher level tasks are completed. This lay- ered structure enables the complex problem of behav- ioral control to be defined as several simple tasks that are combined using subsumption. By adding an addi- tion layer (or task), the competence of the agent is fur- ther augmented, thus increasing the apparent level of intelligence. The subsumption architecture has later re- sulted in the Null-Space-based Behavioral (nsb) control method as presented in [1]. It adheres to the principle of layered control and define each task as a desired ve- locity vector. The tasks (or velocity vectors) are then arranged in a hierarchy, where conflicting lower level ve- locity components are projected onto the nullspace, thus removing their contribution from the resulting reference velocity vector. For fixed-wing Unmanned Aerial Vehi- cles (uavs), the use of a reference velocity vector is not directly applicable since a fixed-winguavis underactu-

⋆ This paper was not presented at any IFAC meeting. Cor- responding author E. Oland. Tel. +47-97725709.

Email addresses: espen.oland@uit.no(E. Oland), tom.s.andersen@uit.no(T. S. Andersen),

raymond.kristiansen@uit.no(R. Kristiansen).

ated with only translational control in one direction. In- stead of defining the tasks as velocity vectors, it is pos- sible to design them directly as rotations that are multi- plied together, producing a desired orientation that en- sures that the primary task is always fully pursued.

The main idea and contribution of this paper is that a rotation between two frames can be decomposed into a composite rotation where each rotation can be treated as a task according to the subsumption architecture. Con- sider the composite quaternion rotation

q_D,A=q_D,C⊗q_C,B⊗q_B,A (1) which performs a rotation from frame A to frame D through the intermediate framesB andC. By defining each quaternion relative to the previous quaternion, the last quaternion will always define the direction that a vector rotated byq_D,Ais pointing in, independently of the previous rotations. This fact can be used to arrange the tasks into a hierarchy through composite rotations.

As the tasks are completed, their quaternion is set to the identity quaternion such that their contribution to the resulting quaternion is suppressed, enabling a fusion of tasks that facilitates behavioral control. Each task is in this paper designed by mapping position vectors de- scribing the task (e.g. error between current and desired position) fromR³ toR×S³, providing the quaternion that describes the direction that the velocity vector must be pointed in to solve the given task.

This paper applies the subsumption architecture to per- form collision (and ground) avoidance while tracking a

desired waypoint. The problem of collision avoidance has received much attention the last decades. The most well known approach is the use of potential fields as pre- sented in [5]. Even though the potential field approach is intuitively simple and appealing, it suffers from several problems such as local minima, inability to pass through narrow gaps, and the possibility of becoming trapped [6]. These facts motivated further research and resulted in the navigation function [10] which addresses the local minima problem, and has later has been extended for nonholonomic vehicles in [13] and applied for nonholo- nomic aircraft-like vehicles in [11]. The problems that were pointed out in [6] is applicable to many of the lo- cal collision avoidance methods. As such, they deserve special attention. In this paper, a detection box is ap- plied for obstacle detection, similarly to the vector his- togram as proposed in [2], and enables an agent to pass through narrow gaps. Furthermore, the subsumption ar- chitecture facilitates an arbitrary number of layers, and as such, even though it is not implemented in this pa- per, it is possible to add an addition layer that serves as a global navigation function that defines no-fly-zones wherever an agent can become trapped, i.e. the frame- work allows for an avoidance of local minima.

2 Modeling

The time derivative of a vector is given as ˙x =dx/dt, the Euclidean length is written as||x|| =√

x^⊤xand† denotes the Moore-Penrose pseudoinverse. Superscript denotes the reference frame of a vector wherebdenotes the body frame,ndenotes the North East Down (ned) frame, w denotes the wind frame, d denotes the de- sired frame, e denotes the position error frame, c de- notes the collision avoidance frame and hdenotes the ground avoidance frame. The rotation matrix is denoted R^B_A ∈ SO(3) = {R ∈ R^3×3 : R^⊤R = I,det(R) = 1}, which rotates a vector from frame A to frame B and where Idenotes the identity matrix. The angular velocity vector is denoted ω^C

A,B, which represents the angular velocity of frame B relative to frame A refer- enced in frameC. Angular velocities between different frames can be added together asω^C

A,D=ω^C

A,B+ω^C

B,D. The time derivative of the rotation matrix is found as R˙^C_A = R^C_AS(ω^A

C,A) where the cross product operator S(·) is such that for two arbitrarily vectorsv1,v2∈R³, S(v1)v2=v1×v2. The rotation matrix can be param- eterized using quaternions, where the quaternion that rotates from frame A to frame C is denoted q_C,A ∈ S³ = {q ∈ R⁴ : q^⊤q = 1} and can be written as q_C,A = h

ηC,A ǫ^⊤

C,A

i⊤

= h

cos(^ϑC,A₂ )k^⊤_C,Asin(^ϑC,A₂ )i⊤

whereηC,Ais the scalar part andǫ_C,A∈R³is the vector part enabling the the rotation matrix to be constructed asR^C_A=I+ 2ηC,AS(ǫ_C,A) + 2S²(ǫ_C,A). The quaternion performs a rotation of an angleϑC,Aaround the unit vec- tork_C,Aand the inverse quaternion is defined asq_A,C =

h

ηC,A −ǫ^⊤

C,A

i^⊤

. Composite rotations are found using the quaternion product as q_C,E = q_C,A⊗q_A,E which ensures that the resulting quaternion maintains the unit length property. The translational kinematics can be de- fined as (cf. [12]) ˙pⁿ=Rⁿ_bv^bandv^b_r=v^b−R^b_nwⁿwhere pⁿ is the position vector,v^b is the velocity vector rela- tive to the ground,v^b_r is the velocity vector relative to the surrounding air andwⁿ is the velocity of the wind.

The relative velocity vector (v^b_r) can be rotated to the wind frame asv^w_r =R^w_bv^b_r=h

Va 0 0i⊤

whereVais the airspeed. By pointing the airspeed in a desired direction and move with a desired airspeed, any control objec- tive can be fulfilled. Hence, a quaternion error function between the wind frame and the desired frame can be defined as (cf. [7]) eq± = h

1∓ηd,w ǫ_d,w

i ∈ S³_e, where S³_e={h

1∓ηd,w ǫ^⊤

d,w

i⊤

|q_d,w∈S³}, and a speed error can be defined as ˜V =Va−VdwhereVdis a desired air- speed. The control objective can now be formalized as that of making ( ˜V ,e_q±,ω^b

d,w)→(0,0,0) which can be fulfilled by using the results from [8]. This paper is con- cerned with the design of the desired quaternion, angu- lar velocity and acceleration (qn,d,ω^d

n,d,ω˙^d

n,d) using the subsumption architecture.

3 Main Results

This paper considers three tasks that can be arranged into a hierarchy and represented using quaternions as:

• Task 1: Ground avoidance,q_c,h

• Task 2: Collision avoidance,q_e,c

• Task 3: Waypoint tracking,q_n,e;

and can be combined into a quaternion and correspond- ing angular velocity as

q_n,h=q_n,e⊗q_e,c⊗q_c,h (2) ω^h

n,h=R^h_eω^e

n,e+R^h_cω^c

e,c+ω^h

c,h. (3)

The primary task is ground avoidance which shall en- sure that the uavnever collides with the ground. The secondary task is collision avoidance which shall ensure that the uav does not collide with buildings nor other uavs¹. The tertiary task is waypoint tracking, where theuavshall reach a desired waypoint. All tasks are de- fined using position vectors in thenedframe. As a task is completed, its quaternion is set equal to the identity quaternion, while its angular velocity is set to zero. This enables multiple tasks to be fused together producing a quaternion and angular velocity that facilitates behav- ioral control. With basis in Figure 1, let the position er-

1 Note that this paper only proves collision avoidance for static obstacles. In the simulation, eachuavis encompassed with a protective sphere to account for this.

pⁿ

eⁿ

pⁿ_wp

yⁿ xⁿ y^w x^w

x^e y^e x^c

y^c

pⁿ_o cⁿ

Fig. 1. Position vectors relative to thenedframe. In the case of waypoint tracking, the objective is to align the wind frame with the position error frame which will make the position error go towards zero. In the case of collision avoidance, the objective is to align the wind frame with the collision avoid- ance frame, resulting in a circular motion around obstacles.

By combining the two quaternions, it is possible to move to a desired waypoint without collision.

ror frame be defined through the relation e^e:=h

||eⁿ|| 0 0 i⊤

=R^e_neⁿ=R^e_n(pⁿ_wp−pⁿ) (4) where the objective is to makee^e →0and switch to a new waypoint before the origin is reached. To facilitate waypoint tracking, consider the two assumptions:

Assumption 1 Va ≥βv>0.

Assumption 2 wⁿ =0.

Assumption 1 states that theuavhas a positive speed above stall speed which is a requirement for flight, some- thing that can be enforced through a speed controller.

Assumption 2 simplifies the waypoint problem and will later be removed by the quaternion (q_h,d) that compen- sates for the wind.

Lemma 1 Let Assumptions 1 and 2,||eⁿ(t⁰)||> δ >0 hold, and let a set be defined asH(δ,∆) :={x∈R³|δ≤

||eⁿ|| ≤∆}whereδ >0represents an inner radius and

∆> δ >0an outer radius. Given the position error as in (4) and by tracking the quaternion and angular velocity

q_n,e=h cos_ϑ

n,e

2

k^⊤_n,esin_ϑ

n,e

2

i⊤

(5) ϑn,e= cos⁻¹

e^e·eⁿ

||eⁿ||²

k_n,e= e^e×eⁿ

||e^e×eⁿ|| (6) ω^e

n,e=S^†(e^e)R^e_bv^b (7)

then the setH(δ,∆)is uniformly asymptotically stable.

Proof 1 The proof is given in Appendix A.

To facilitate collision avoidance, consider Figure 1. By moving perpendicular to the line of sight vector to the closest obstacle (cⁿ), collisions can be avoided. By choos- ing the sign of the collision avoidance frame, the uav

can move either to the left or the right around an ob- stacle. This paper applies a detection box that is fixed to the wind frame of the uav and contains two sec- tors. If an obstacle is detected in the left sector, the uav moves to the right, and left otherwise. The colli- sion avoidance frame be defined through the relation c^c = h

0 ±||c^e|| 0i⊤

= R^c_eR^e_ncⁿ where R^c_e is designed using quaternions. Note that the vectorcⁿis first rotated to the position error frame before designing the collision avoidance frame. This dependence is what enables the application of subsumption theory through composite rotations. For ground avoidance, the line of sight vector to the ground is first rotated to the collision avoidance frame before designing the ground avoidance frame.

Lemma 2 Let Assumptions 1 and 2 hold. Given the po- sition error between the closest obstacle (pⁿ_o) and theuav asc^e=R^e_ncⁿ =R^e_n(pⁿ_o −pⁿ), and by following the at- titude and angular velocity

q_e,c=h cos_ϑ

e,c

2

k^⊤_e,csin_ϑ

e,c

2

i⊤

(8) ω^c

e,c=S^†(c^c)(R^c_eS(ω^e

n,e)R^e_ncⁿ−R^c_nc˙ⁿ) (9) ϑe,c= cos⁻¹

c^c·c^e

||c^e||²

k_e,c= c^c×c^e

||c^c×c^e|| (10) c^c=h

0 ±||c^e|| 0i⊤

=R^c_eR^e_ncⁿ=R^c_ec^e, (11) then||c^c|| ≥βc>0∀t≥t⁰.

Proof 2 The proof is given in Appendix B.

Lemma 3 Let Assumptions 1 and 2 hold. Given the po- sition error between the closest point on the ground (pⁿ_g) and the uavash^c =R^c_eR^e_nhⁿ =R^c_n(pⁿ_g −pⁿ), and by following the attitude and angular velocity

q_c,h=h cos_ϑ

c,h

2

k^⊤_c,hsin_ϑ

c,h

2

i^⊤

(12) ω^h

c,h=S^†(h^h)(R^h_cS(ω^c

e,c)R^c_nhⁿ+R^h_eS(ω^e

n,e)R^e_nhⁿ

−R^h_bh˙ⁿ) (13)

ϑc,h= cos⁻¹

h^h·h^c

||h^c||²

k_c,h= h^h×h^c

||h^h×h^c|| (14) h^h=h

0 0 ||h^c||i⊤

=R^h_cR^c_eR^e_nhⁿ =R^h_ch^c, (15) then||h^h|| ≥βh>0∀t≥t⁰.

Proof 3 The proof follows the same lines as Lemma 2 and is therefore omitted.

Remark 1 The lower bounds βc andβh are functions of speed, detection range, actuator constraints and the rotational control law. Hence, the detection range must be sufficiently long to account for this to ensure collision- free trajectories.

Remark 2 By setting the z component of cⁿ and the xandy components ofhⁿ to zero before calculating the quaternions, the quaternionq_c,hbecomes constrained to the vertical plane and only subsumes the flight path angle.

This implies that the vehicle may always avoid simulta- neously the ground (controlling the flight path usingq_c,h) and an obstacle defined as a vertical cylinder (by control- ling the heading angle usingq_e,c), or to simultaneously track a ground target while avoiding the ground.

Theorem 1 Let Assumptions 1 and 2 hold. By follow- ing the attitude and angular velocity (2)-(3) where the terms are given in lemmas 1, 2 and 3, then||c^c|| ≥βc >

0,||h^h|| ≥βh>0∀t≥t⁰ and||e^e|| → H(δ,∆).

Proof 4 The proof is given in Appendix C.

To remove Assumption 2, let a wind compensation quaternion be designed as

q_h,d=h cos_ϑ

h,d

2

k^⊤_h,dsin_ϑ

h,d

2

i⊤

(16) ϑh,d= cos⁻¹

v^b·v^b_r

||v^b|| ||v^b_r||

k_h,d= v^b×v_r^b

||v^b×v_r^b|| (17) which compensates for the wind and where the angular velocity and acceleration are found using linear filters.

The tasks from the subsumption architecture defined through (2) and (3) together with wind compensation produce the desired quaternion and angular velocity as q_n,d=q_n,e⊗q_e,c⊗q_c,h⊗q_h,d (18) ω^d

n,d=R^d_eω^e

n,e+R^d_cω^c

e,c+R^d_hω^h

c,h+ω^d

h,d (19) which also shows the extendability of the proposed ap- proach. The addition of a new task is simply done by ex- panding the quaternion product, and where the resulting angular velocity is found by adding an additional term.

Remark 3 The proofs of this paper are only valid for one static obstacle, and in practice there are many situations that invalidate the presented approach, e.g. the waypoint is too close to the ground, multiple obstacles form a trap for theuav, a moving obstacle has higher speed than the uav, etc.

4 Simulation

Simulations have been performed with basis in the non- linear dynamics and controllers from [8]. Seven uavs shall move through an urban terrain while avoiding col- lisions with the buildings, the ground and otheruavs.

The main wind vector is set to wⁿ = h

10 0 0i⊤

and wind gusts are simulated using the Dryden wind turbu- lence model with the default settings as in Simulink. The uavs shall keep 40 meters above the ground and shall

0 0

0 100 200

200 500

500 1000

1000 1500

1500 2000

2000 -500

-200 -100

300

400 400 600 800

xⁿ

yn

uav-1 uav-2 uav-3 uav-4 uav-5 uav-6 uav-7

Fig. 2. Sevenuavs move through an urban terrain. The build- ings are represented by rectangles, the desired waypoint as a circle and the final positions as triangles.

0 00 00

10 10

10 10

10

20 20

20 20

20

30 30

30

30 30

30

50

50 50

50 50

50

Time (s) Time (s) Time (s)

uav-1 uav-1 uav-1

uav-2 uav-2 uav-2

uav-3 uav-3 uav-3

uav-4 uav-4 uav-4

uav-5 uav-5 uav-5

uav-6 uav-6 uav-6

uav-7 uav-7 uav-7

40

40 40

40 40

40

60

60 60 60

70 80

Altitude(m)Distance(m)Distance(m)

Fig. 3. The top plot shows the shortest distance to the clos- estuav, the middle plot shows the shortest distance to the closest obstacle, and the bottom plot shows the altitudes.

keep 5 meters away from the closest obstacle while flying withVd= 50 m/s. Figure 2 shows the 2D plot of the sim- ulation, while Figure 3 shows the relative distances. No collisions have occurred and alluavs reached the way- point. The altitude of the second waypoint is 1000 m.

5 Conclusion

This paper has presented a solution to the problem of maneuvering in a complex dynamical environment us-

ing the subsumption architecture. Multiple tasks can be defined as simple rotations that are multiplied together producing a desired quaternion. The architecture en- sures that the primary task will always be fully pursued while lower level tasks are fully pursued as the higher level tasks are completed.

A Waypoint Tracking

The following proof first designs the position error frame by taking basis ine^e=R^e_neⁿ, and then the stability of the setH(δ,∆) is shown by looking at the error function

e^e_δ =h

||eⁿ||δ 0 0i^⊤

=R^e_neⁿ (A.1) where||eⁿ||δ = infx∈H||x−pⁿ||which considers all er- rors except a small ball around the origin of eⁿ = 0. Consider the error function (4) where the rotation ma- trixR^e_n can be constructed with quaternions using (5).

The angular velocity can be found by differentiating (4) resulting in

˙

e^e=S(e^e)ω^e

n,e−R^e_bv^b (A.2) where −S(ω^e

n,e)e^e = S(e^e)ω^e

n,e, ˙pⁿ_wp = 0 and ˙pⁿ = Rⁿ_bv^b have been used. The pseudoinverse of the skew symmetric matrix can be expressed as

S^†(e^e) =









0 0 0

0 0 −_||e¹ⁿ||

0 _||e¹n|| 0









, (A.3)

which filters out any components along the x^e axis, such thatS^†(e^e) ˙e^e =0. This can be exploited to solve (A.2) for the angular velocity as in (7). Notice that as

||eⁿ|| →0 the angular velocity||ω^e

n,e|| → ∞, such that a switching algorithm must be applied to avoid this. More precisely, as long as||eⁿ|| ≥ δ > 0, the angular veloc- ity will remain bounded. To that end, the setH(δ,∆) is defined as a shell around the origin eⁿ = 0, such that the uav will never reach the interior of the shell which ensures that||eⁿ|| ≥ δ > 0 making the angular velocity (7) upper bounded as||ω^e

n,e|| ≤ ¹_δβVmax where βVmax is an upper bound of the velocity vector². Equa- tion (A.1) can be differentiated as ˙e^e_δ =−S(ω^e

n,e)e^e_δ + R^e_n( ˙pⁿ_wp−p˙ⁿ) where by using Assumption 2 it follows thatv^b =v^b_r =R^b_wh

Va 0 0i⊤

withVa ≥βv using As- sumption 1. The waypoint is a fixed point, such that

˙

pⁿ_wp = 0, enabling the error kinematics to be written

2 It can be shown that ˙Va = −D+γ(·) where γ(·) is a bounded function andD is a positive drag force, such that

||v^w_r||=Va≤βV_max. Withwⁿ=0,||v^b||=||v^w_r|| ≤βV_max.

as ˙e^e_δ =−S(ω^e

n,e)e^e_δ−R^e_wv^w_r where the rotation matrix R^e_w=I+ 2ηe,wS(ǫ_e,w) + 2S²(ǫ_e,w) can be inserted as

˙

e^e_δ =−S(ω^e

n,e)e^e_δ−v_r^w

−(2ηe,wS(ǫ_e,w) + 2S²(ǫ_e,w))v^w_r (A.4) where the termv^w_r provides damping to the system. Us- ing Assumption 2, it follows thatq_n,d=q_n,e,i.e. there is no quaternion for wind compensation. Thus, by making the error (eq±,ω^b

d,w)→(0,0), thenq_e,w →h

1 0 0 0i⊤

such that the last term in (A.4) will disappear. The sys- tem can now be written on cascaded form as

˙

x1=f¹(t,x1) +g(t,x)x (A.5)

˙

x2=f2(t,x2) (A.6) wherex=h

x^⊤1 x^⊤2

i^⊤

,x1:=e^e_δ,x2=h

e^⊤_q± (ω^b

d,w)^⊤i^⊤ , f¹(t,x1) :=−S(ω^e

n,e)e^e_δ−v^w_r, g(t,x) := h

0 2ηe,wS(v^w_r) + 2S(ǫ_e,w)S(v^w_r) 0 i, and where f²(t,x2) represents the rotational dynamics in closed loop with a rotational controller. Consider first the unforced system ˙x1=f¹(t,x1) written as

˙

x1=−S(ω^e_n,e)x¹−v_r^w. (A.7) Let a Lyapunov function candidate be defined asV :=

1

2x^⊤₁x1which through differentiation and inserting (A.7) becomes ˙V = −x^⊤1v^w_r since x^⊤1S(ω^e

n,e)x¹ = 0. Note that bothx1=h

||eⁿ||δ 0 0i⊤

andv^w_r =h

Va 0 0i⊤

are positive with only components on the x-axis, such that V˙ =−||x1||βv whereβv represents the lower bound of the airspeed. This means that asx1(t)→0, the trajec- tories will converge to the setH(δ,∆). Hence, it follows by using Theorem 4.9 in [4] that the setH(δ,∆) is uni- formly asymptotically stable whenx2=0.

To prove that the cascade is uniformly asymptotically stable, three assumptions must be fulfilled in order to invoke the results on cascaded theory by [9]. Since the set H(δ,∆) of the unforced system ˙x1 = f1(t,x1) is uniformly asymptotically stable, it follows from converse theorems (e.g. [4]) that there exists a suit- able V(t,x1), and as such the first assumption in [9] is fulfilled. The second assumption requires that the interconnection term g(t,x) has a linear growth bound. The interconnection term can be bounded as

||g(t,x)|| ≤ h

0 2||ηe,wS(v^w_r)||+ 2||S(ǫ_e,w)S(v^w_r)|| 0 i, where |ηe,w| ≤ 1, ||ǫ_e,w|| ≤ 1, ||v^w_r|| ≤ βVmax, and consequently the second assumption is fulfilled. The third assumption states that the trajectories of sys- tem (A.6) must converge sufficiently fast to the origin through an integrability constraint. The sliding sur- face controller in [8] in closed loop with the rotational

dynamics results in uniformly exponential stability of the equilibrium x2 = 0. This means that ||x2|| → 0 exponentially fast, thus fulfilling the third assumption.

Hence, it follows by invoking Theorem 1 in [9] that the setC:={x1∈R³,x2∈S³_e×R³|δ≤ ||x1|| ≤∆,x2=0} of the cascaded system (A.5)-(A.6) is uniformly asymp- totically stable.

B Collision Avoidance

The quaternion can be found by studying (11) resulting in (8). Equation (11) can be differentiated as

˙

c^c=−S(ω^c

e,c)c^c−R^c_eS(ω^e

n,e)R^e_ncⁿ+R^c_nc˙ⁿ (B.1) whereS(ω^c

e,c)c^c =−S(c^c)ω^c

e,cwith

S(c^c) =









0 0 ∓||c^e||

0 0 0

±||c^e|| 0 0









(B.2)

which filters out anyy^ccomponents such thatS^†(c^c) ˙c^c= 0. Solving (B.1) for the angular velocity results in (9).

With ˙pⁿ_o =0equation (B.1) can be rewritten as

˙

c^c=−S(ω^c

e,c)c^c−R^c_eS(ω^e

n,e)R^e_ncⁿ−R^c_wv^w_r (B.3) where ˙pⁿ = Rⁿ_wv^w_r follows from Assumption 2.

Let the rotation matrix be written as R^c_w = I + 2ηc,wS(ǫ_c,w) + 2S²(ǫ_c,w), which can be inserted into (B.3) as ˙c^c = −S(ω^c

e,c)c^c −R^c_eS(ω^e

n,e)R^e_ncⁿ −(I+ 2ηc,wS(ǫ_c,w) + 2S²(ǫ_c,w))v^w_r.Let the collision avoidance quaternion be the primary task, such thatq_n,d=q_n,c, then by making (e_q±,ω^b

d,w) → (0,0) through rota- tional control, it follows that q_c,w → h

1 0 0 0i⊤

. The system can now be written on cascaded form similarly as in (A.5)-(A.6), but where x1 := c^c, f¹(t,x1) := −S(ω^c

e,c)c^c − R^c_eS(ω^e

n,e)R^e_ncⁿ − v^w_r and g(t,x) := h

0 2ηc,wS(v^w_r) + 2S(ǫ_c,w)S(v^w_r)0 i. Consider the unforced system ˙x1 = f1(t,x1) which can be written in original coordinates as ˙c^c =

−S(ω^c

e,c)c^c−R^c_eS(ω^e

n,e)R^e_ncⁿ−v^w_r. A Lyapunov func- tion candidate can be defined asV = ¹2(c^c)^⊤c^c which through differentiation results in ˙V = 0. This means that as long as the collision avoidance quaternion is perfectly tracked, the relative distance between the ob- stacle and the uav will neither increase nor decrease, such that the uav will move in a circular trajectory around the obstacle. The rest of the proof follows as in Appendix A. Even though Theorem 1 in [9] consid- ers thestability of cascades, similar arguments can be applied to prove collision avoidance. As x2 → 0 and V˙ →0, it follows that the distance to the obstacle goes

to a constant,βc>0. Hence, it follows that the position error between the obstacle and theuav becomes lower bounded as||c^c|| ≥βc>0, and collision is avoided.

C Proof of Theorem 1

When the primary task is active, it follows from Lemma 3 that ||h^c|| ≥ βh > 0 ∀t ≥ t⁰. As the primary task is completed, it follows that q_c,h = h

1 0 0 0i⊤

reducing (2) toq_n,h=q_n,c=q_n,e⊗q_e,cwhere the secondary task becomes fully active. Using Lemma 2, the secondary task ensures that||c^c|| ≥βc >0 ∀t ≥t0. As the secondary task is completed, (2) is reduced toq_n,h =q_n,e which by using Lemma 1 ensures that||e^e|| → H(δ,∆).

References

[1] G. Antonelli, F. Arrichiello, and S. Chiaverini. The null-space-based behavioral control for mobile robots.

In Proceedings of IEEE International Symposium on Computational Intelligence in Robotics and Automation, Espoo, Finland, 2005.

[2] J. Borenstein and Y. Koren. The vector field histogram - fast obstacle avoidance for mobile robots.IEEE Transactions on Robotics and Automation, Vol. 7, No. 3:pp. 278–288, 1991.

[3] R. A. Brooks. A robust layered control system for a mobile robot. IEEE Journal of Robotics and Automation, Vol. RA- 2, No. 1:pp. 14–23, 1986.

[4] H. K. Khalil. Nonlinear systems. 3rd ed., Prentice Hall, ISBN 0-13-067389-7, 2002.

[5] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research, Vol 5. No. 1:pp. 90–98, 1986.

[6] Y. Koren and J. Borenstein. Potential field methods and their inherent limitations for mobile robot navigation.

In Proceedings of the IEEE Conference on Robotics and Automation, Sacramento, CA, USA, 1991.

[7] R. Kristiansen, A. Lor´ıa, A. Chaillet, and P. J. Nicklasson.

Spacecraft relative rotation tracking without angular velocity measurements.Automatica, Vol. 45, No. 3:pp. 750–756, 2009.

[8] E. Oland, R. Schlanbusch, and R. Kristiansen.

Underactuated waypoint tracking of a fixed-wing UAV. In Proceedings of the 2nd RED-UAS, Compiegne, France, 2013.

[9] E. Panteley and A. Lor´ıa. On global uniform asymptotic stability of nonlinear time-varying systems in cascade.

Systems & Control Letters, Vol. 33, No. 2:pp. 131–138, 1998.

[10] E. Rimon and D. E. Koditschek. Exact robot navigation using artificial potential functions.Transactions on Robotics and Automation, Vol. 8, No. 5:pp. 501 – 518, 1992.

[11] G. Roussos, D. V. Diamarogonas, and K. J. Kyriakopoulos.

3D navigation and collision avoidance for nonholonomic aircraft-like vehicles. Intl. Journal of Adaptive Control and Signal Processing, Vol 24, No. 10:pp. 900–920, 2010.

[12] B. L. Stevens and F. L. Lewis. Aircraft control and simulation. 2nd ed., Wiley, ISBN 978-0-471-37145-8, 2003.

[13] H. G. Tanner, S. G. Loizou, and K. J. Kyriakopoulos.

Nonholonomic navigation and control of cooperating mobile manipulators. IEEE Transactions on Robotics and Automation, Vol. 19. No. 1:pp. 53–64, 2003.