UAV Obstacle Avoidance using Differential Geometry Concepts

Proceedings of the 18th World Congress The International Federation of Automatic Control Milano (Italy) August 28 - September 2, 2011

UAV Obstacle Avoidance using Differential Geometry Concepts Brian A. White ∗ Hyo-Sang Shin ∗ Antonios Tsourdos ∗ ∗

Autonomous Systems Group,Department of Informatics and System Engineering, Cranfield University, Shrivenham, SN6 8LA UK

Abstract: This paper investigates the application of differential geometry to UAV collision avoidance based on differential geometry. The Differential Geometry approach has been useful in determining a guidance algorithm to produce a constant curvature evasion manoeuvre to avoid collision with an Aircraft flying in a straight line. Simulation results presented in the paper demonstrated the effectiveness of the proposed UAV collision avoidance algorithms. Keywords: differential geometry, collision avoidance, UAV guidance. 1. INTRODUCTION

2. DIFFERENTIAL GEOMETRY KINEMATICS Consider the scenario shown in figure 1, where a UAV is following a prescribed path and an aerial vehicle is crossing the path with the possibility of interception. The UAV

UAV missions for civil and military objectives have grown in the recent years and the missions diversify into vehicle tracking surveillance, enemy tracking, patrol near borders, sea monitoring, weather and environmental observation, and cargo delivery Kuchar and Yang (2000).In order to achieve various UAV missions, it is necessary to design a reliable and intelligent UAV system which can be integrated in civilian airspace. Especially, a reliable and sophisticated collision avoidance system, has become one of the most important design parameters for UAV system, because a collision accident causes serious casualties and losses of aerial vehiclesAsmat et al. (2006). These algorithms should not only assure conflict-free trajectory planning, but also provide basic autonomous navigation capabilities. Differential geometric techniques are a promising approach to the path planning problem and have the potential to overcome limitations of the Proportional Navigation (PN) guidance algorithm Mujumdar and Padhi (2009). The original PN algorithm was conceived for a target moving with a constant speed along a straight line. As one generalises to scenarios with target manoeuvres, this leads naturally to freeing the intercept geometry of the rectilinear framework. The differential geometry of spatial curves offers an attractive setting for this task. In particular, curvature is an important parameter quantifying by how much the trajectory deviates from the straight line. Apart from this geometric meaning, it also has a clear and useful kinematic interpretation in terms of lateral acceleration. This dual, spatiotemporal character of curvature allows for natural coordination of trajectories. This paper we present an algorithm to enable a UAV to automatically avoid collision with an Aircraft. The Differential Geometry approach has been useful in determining a guidance algorithm to produce a constant curvature evasion manoeuvre to avoid collision with an Aircraft flying in a straight line. 978-3-902661-93-7/11/$20.00 © 2011 IFAC

ta na

qas qa

tu

Aircraft Sightline r ts

X ra

Y

qs

qu qus

ru

UAV

ns nu

Fig. 1. Guidance Geometry senses the Aircraft and establishes a sight line between it and the Aircraft. Assuming the velocity of the Aircraft is known (usually using a motion estimator), the motion geometry can be defined. Several axes sets can be defined. A tangent and normal basis vector set ( ts ns ) defines the sightline, with ( tu nu ) defining the UAV and ( ta na ) defining the Aircraft. From figure 1, the sightline range vector is given by: r = ra − ru (1) If the assumption is made that both the UAV and Aircraft velocity is constant and the sightline vector is written in terms of the basis vector set ( ts ns ), then the differential of equation (1) yields: rt ˙ s + rθ˙s ns = Va ta − Vu tu

(2)

This equation represents the components of the Aircraft velocity relative to the UAV. Components of the relative velocity along and normal to the sightline are given by projection onto the basis vectors ts and ns . Hence:

6325

r˙ = Va t′s .ta − Vu t′s .tu rθ˙s = Va n′ .ta − Vu n′ .tu s

s

(3) (4)

10.3182/20110828-6-IT-1002.02344

18th IFAC World Congress (IFAC'11) Milano (Italy) August 28 - September 2, 2011

where ′ represents the vector transpose. The UAV to Aircraft relative acceleration is given by differentiating equation (2),to give: (¨ rts +r˙ θ˙s ns )+(r˙ θ˙s ns +rθ¨s ns −rθ˙s2 ts ) = (Va t˙ a −Vu t˙ u ) (5)

Aircraft

va

vu vr

Sightline

The Serret-Frenet equations for the UAV and the Aircraft can be re-written in terms of a constant velocity trajectory in the form :

Fig. 3. Relative Velocity for Collision

t˙ i = κi Vi ni = θ˙i ni n˙ i = −κi Vi ti = −θ˙i ti , i = a, u

(6) (7)

where κi is the curvature of the trajectory and θ˙i is the instantaneous rotation rate of the Serret-Frenet frame about the bi-normal vector bi . The normal vector ni is a unit vector that defines the direction of the curvature of the trajectory (c.f. figure 1) and the bi-normal vector bi is orthonormal to ti and ni , forming a right-handed triplet (ti , ni , bi ). Hence (¨ r − rθ˙2 )ts + (rθ¨s + 2r˙ θ˙s )ns = V 2 κa na − V 2 κu nu (8) u

a

s

3. COLLISION KINEMATICS In order to develop the conditions for avoidance, the conditions for collision need to be understood. 3.1 Collision Kinematics for Non-manoeuvring UAV In order to determine wether the UAV and Aircraft will collide, kinematics need to be examined further. The geometry of a non-manoeuvring Aircraft with a direct, straight line UAV collision trajectory is shown in figure 2(a). The geometry of a non-manoeuvring Aircraft with a ta qa

sa

Aircraft r ra

na

tu ts

Sightline

qu Y

ru

ta/g

qas

nu

qus

tu

A

ns

UAV

(a) Guidance Geometry

ns

na r/(gSa)t s

ensures that the sightline will not change direction, but only change in length and so the geometry does not change over time. From the Intercept Triangle in figure 2(b), we have: su tu = rts + sa ta (10) Noting that: su Vu = = γ (11) sa Va gives:   1 r ts + ta (12) tu = γ sa

Equation (12) can be visualised as a vector addition, and is shown in figure 2(b). It is in a non-dimensional form and will thus represent the solution for all ranges between the UAV and the Aircraft. The ratio r/sa is fixed for the whole solution and thus as the range r decreases, so will the Aircraft arc length sa . Given the geometry of the Aircraft basis vector ta and the range basis vector ts , the direction of the UAV basis vector tu is fixed. In equation (12), the ratio r/sa can be obtained by the use of the cosine rule in figure 2(b). From figure 2(b), we have: 

r sa

2

+ 2 cos(θas )



r sa



− (γ 2 − 1) = 0

(13)

This quadratic in r/sa can be solved explicitly, to give: q r = − cos(θas ) ± γ 2 − sin2 (θas ) (14) sa Given that both r > 0 and sa > 0, the solution will exist for any γ if:

I

su

qas

X

UAV

nu U

γ > | sin(θas )|

(b) Matching Condition

Fig. 2. Collision Geometry direct, straight line intercepting UAV trajectory is shown in figure 2(b). Note that the Intercept Triangle AIU is invariant, in that as the UAV and Aircraft move along their respective straight line trajectories, the AIU triangle does not change shape, but shrinks. The UAV-to-sightline angle θus and the Aircraft-to-sightline angle θas remain constant over the whole engagement. A useful interpretation of this condition can be visualised if the relative velocity of the UAV with respect to the Aircraft is considered. This is shown in figure 3 for collision. where the relative velocity of UAV 1 with respect to UAV 2 is given by: v r = v u − va (9) The condition for avoidance is shown to be such that the relative velocity vector should lie along the sightline. This

(15)

The collision direction can be easily determined from figure 3. This is because, when the collision geometry meets the kinematic condition in equation (14) and hence the geometric condition in equation (12), the geometry is invariant and the relative velocity vr defined in equation (9) and shown in figure 3 defines the approach direction. 3.2 Collision Kinematics for Manoeuvring UAV Following the same approach for the case of the nonmanoeuvring UAV, the Avoidance Triangle is now defined by the Aircraft tangent vector ta and the UAV arc chord defined by the vector tLu , as shown in figure 4. Hence: Lu tLu = rts + sa ta (16) where the chord basis vector tLu can be obtained from the missile basis vector tu by a rotation through −θcu /2. The arc length is given by:

6326

18th IFAC World Congress (IFAC'11) Milano (Italy) August 28 - September 2, 2011

ta

I

su

qas A au qcu

tLu ts

1/bg ta

ta/bg

Lu r

nu

C

I

I

sa

tu

qLs

qas ns

qus U

tL

A

ns

na

nu

qas

tu

au U

(a) Guidance Geometry

qat

A

r/(bgSa)t s nu

qcu/2

qua

1/b tc b/b tt

tLu

tu ns

na r/(bgSa)t s

(b) Matching Condition

qLs U

Fig. 4. Geometry for Collision for a manoeuvring UAV su = θcu ru θcu = κu and the chord length Lu is given by: Lu = 2ru sin(θcu /2) sin(θcu /2) = su θcu /2 = βsu

nu

Fig. 5. Approach Geometry for Collision

(17)

(18)

Equation (16) can now be written in the form:    r 1 ts + ta (19) tLu = γβ sa The equivalent invariance exists for this geometry, as was the case for the non-manoeuvring UAV. Hence the matching condition for the manoeuvring UAV case can be visualised as a vector addition, as shown in figure 4(b). From this figure, the matching condition can be calculated by applying the cosine rule to the Intercept Triangle AIU, as before. Hence:  q r = − cos(θas ) + β 2 γ 2 − sin2 (θas ) (20) sa The solution (r/sa ) and the arc angle θcu are related by:   γκu r r = (21) sa θcu Although an analytic solution to equation (20) is not possible, an iterative solution can be calculated by using fixing the curvature κu . Hence, given θas , γ, r and κu , and using the arc angle θcu as the iteration variable, then the following iteration can be used:

sides, with the tangent tt defining the sightline angle at the point of impact completing the Terminal Triangle. Note that to use the common Aircraft tangent edge, the Terminal Triangle is scaled by β. Applying the co-sine rule to triangle AIT gives: 12 2 − cos(θua + θcu /2) b2 = 12 + (22) γ γ Then, using the sin rule, we have: sin(θua ) sin(θat ) = (23) b This gives the approach angle relative to the Aircraft velocity vector. 4. AVOIDANCE KINEMATICS The conditions for collision can now be modified by extending the kinematic geometry analysis to examine the conditions for the UAV to miss the Aircraft rather than collide with it. For the case where the UAV needs to avoid collision with other Aircraft that could be in the area of operation, it is required that the UAV follows a trajectory that guarantees a Closest Approach Distance (CAD) to the Aircraft. Hence consider the geometry for the UAV and the Aircraft on straight line trajectories with a required CAD of dm . The collision geometry can thus be modified by superimposing a circle of radius dm around the Aircraft, as shown in figure 6. dm Aircraft

pm

va vu dr vr r

Collision Condition (CC) Algorithm (1) (2) (3) (4) (5)

T

set θcu = 0 calculate β using equation (18). calculate r/sa using equation (20). calculate θcu (b) using equation (21). repeat from 2, until θcu converges.

UAV

Fig. 6. Relative Velocity for Closest Approach

Once a solutionis found, the approach angle can be determined by examining the conditions near collision. As the UAV manoeuvres, the Avoidance Triangle geometry will change. The matching condition in figure 4(b) can be modified to take account of the curvature of the UAV trajectory. This is shown in figure 5. The Collision Triangle is supplemented by the addition of triangle AIT. This is constructed with the Aircraft trajectory as the common triangle edge. The UAV velocity vector tangent tc at the point of impact is used to define one of the other triangle

4.1 Collision Avoidance Kinematics for Non-manoeuvring UAV For the case of a non-manoeuvring UAV, the velocity vectors of both the UAV and the Aircraft are fixed. If the relative velocity vector vr is aligned with the tangent to the circle, then the point of closest approach will be at pm with a CAD of dm . In order to determine the geometry of the engagement for avoidance, the avoidance geometry shown in figure 2 can thus be modified to that shown

6327

18th IFAC World Congress (IFAC'11) Milano (Italy) August 28 - September 2, 2011

or:

ps dm

sa

dm qas

sa

dm ps

va vu

dr

Aircraft r

qd

qas Aircraft dm qad

qud

UAV

r2 − d2m dm cos(θas ) ∓ sin(θas ) (28) r r Note that the geometry with respect to the sightline is now not fixed, but as the solution requires the relative velocity vector to lie along the tangent line from pu to pm , this line will not rotate. Taking that fact, together with the fact that the UAV and Aircraft velocity vectors are fixed, means that the triangle {pu , pm , ps } is fixed in shape and orientation and will shrink as the UAV and Aircraft approach each other. Hence the ratio dr /sa will have a fixed solution, as will the angle subtended between the Aircraft velocity vector and the tangent line θsa . Hence, we have a solution to equation (25) given by:   q dr = − cos(θad ) ± γ 2 − sin2 (θad ) (29) sa The direction of closest approach is obtained in a similar way to the Collision solution. The approach direction is given by the direction of the relative velocity. This is shown in figure 7. cos(θad ) =

su

sa

qad pm

pm pu

va

su

sa

vu r dr

qd

qud UAV

pu

(b) Anti-clockwise Solution

(a) Clockwise Solution

Fig. 7. Geometry for Closest Approach in figure 7. The figure shows that the original avoidance triangle has been modified to the Avoidance Triangle given by {pu , pm , ps }, for both a clockwise and anti-clockwise rotation from the sightline to the point of closest approach. This Avoidance Triangle will maintain its orientation and shape in a similar manner to the Impact Triangle. The matching condition for the clockwise and anti-clockwise solutions are shown in figure 8.

5. COLLISION AVOIDANCE KINEMATICS FOR MANOEUVRING UAV

I 1/g ta

If the UAV can manoeuvre by pulling constant lateral acceleration it will follow a constant curvature trajectory. This will change the avoidance geometry, as shown in figure 9 for the anti-clockwise case. The vector sum for

qad M

tu

qas dm/gsa td A

r/gsa ts

qd

I 1/g ta

dr/gsa tm

A dm/gsa td qad qud

tu r/gsa ts

M

dr/gsa tm

qd

U

p

qud

dm

U

sa

(a) Clockwise Solution

(b) Anti-clockwise Solution

dm qms

va pm dr

Fig. 8. Matching condition for Closest Approach

Aircraft

The vector sums for the UAV is now calculated with respect to the Avoidance Triangle MIU. Hence:   1 dr tu = td + ta (24) γ sa where the sightline basis set ( ts tn ) are replaced by the CAD basis set ( td td ). Applying the co-sine rule to this geometry now gives:    2 dr dr + 2 cos(θad ) − (γ 2 − 1) = 0 (25) sa sa where: p dr = r2 − d2m θad = θas ± θd (26) and where θd is either subtracted for an clockwise solution and added for an anti-clockwise solution. Now, we have: cos(θad ) = cos(θas ± θd ) = cos(θas ) cos(θd ) ∓ sin(θas ) sin(θd ) p r2 − d2m dr = cos(θd ) = r r dm sin(θd ) = r

qad

(27)

r

vu

sa

ps

su

Lu qud

qcu

tLu

qd UAV

p1

Fig. 9. Geometry for constant Curvature Miss Avoidance Triangle MIU is modified in the same manner for the manoeuvring collision case, to give:   1 dr td + ta (30) tLu = γβ sa  2   dr dr − (β 2 γ 2 − 1) = 0 (31) + 2 cos(θad ) sa sa This can be factored to give a solution in the form:   q dr = − cos(θad ) + β 2 γ 2 − sin2 (θad ) (32) sa Unlike the straight line avoidance case, the closest approach point needs to be established. This is because the relative velocity vector at CAD for the non-manoeuvring UAV case, as shown in figure 7, is normal to the CAD circle at point M, whereas the relative velocity vector for the manoeuvring UAV case is changing during the trajectory. Hence the CAD condition for the manoeuvring UAV case needs to establish that the relative velocity vector is normal to the CAD circle at point M. This can be achieved using the method for determining the approach direction

6328

18th IFAC World Congress (IFAC'11) Milano (Italy) August 28 - September 2, 2011

developed for the manoeuvring UAV. Hence the geometry at point M can be established using the Terminal Triangle. This is shown in figure 10. An iterative solution is required

A geometric interpretation of equation (34) is reproduced in figure 11. Figure 11 shows that as the geometry of the B A dr/(bgsa)tm Minimum

I 1/bg ta

qms

dm/bgsa td

qad M qm d /bgs r

A

qum

qad

qcu/2

tLu

ta/bg

qum

tLu a

tm

qd

r/bgsa ts

1/b tt

Maximum

qud

Fig. 11. Geometry Interpretation of Avoidance Solution

U

b/bgsa ts da/bgsa ta

engagement changes due to mismatched UAV and Aircraft tangent vector, the solution ˆtu will change and rotate around the circle. The figure also shows that the rotation of the solution vector ˆtu and the rotation of the sightline vector ts are related. In moving from solution A to solution B the solution angle θˆum increases. The Aircraft to miss angle θad also increases. Further, the figure shows that:

T

Fig. 10. Approach Geometry for Collision to change the point M around the CAD circle until the angle θm is a right angle, which will result in the correct conditions for the closest approach. Using the angle θms as the iteration variable, solutions to the MIU Avoidance Triangle for a fixed κu is computed. The same equations as in section 3.2 as used to determine the vector between U and M, then using the triangle TAM, the angle θm can be used to terminate the iteration. Hence: Avoidance Condition (AC) Algorithm (1) set θms = collision solution. (2) calculate the Terminal Triangle TIM using CC Algorithm. (3) calculate the internal angle θm to triangle TAM. (4) If θm = π/2, stop. (5) if θm > π/2, then increase θms , else decrease θms . (6) Repeat from step 2. where, using the co-sine rule on triangle TAM: (d2 − d2m − b2 ) cos(θm ) = a 2dm b

(33)

5.1 Guidance Algorithm The condition required for a miss of the Aircraft for a manoeuvring UAV is given by:    dr ˆtLu = 1 ts + ta (34) γβ sa The guidance algorithm can be determined by use of a simple Liapunov function V of the form: 1 (35) V = θǫ2 2 where: θǫ = θˆud − θud (36) is the angle between the desired UAV tangent vector ˆtu and the UAV tangent vector tu . with: V˙ = θ˙ǫ θǫ ˙ θ˙ǫ = θ˙ud − θˆud

(37)

˙ θˆad = λ(θum )θ˙um λ(θad ) > 0

(38)

Hence:

˙ θˆad θ˙um > 0 (39) Also from the figure, by noting vector lengths and that max(β) = 1, we have:     1 1 1+ ≤ λ(θum ) ≤ 1 − (40) γ γ ˙ To calculate θˆum , the rate of change of the desired UAV tangent chord vector ˆtLm is required. So:       ˆt˙ Lm = 1 d dr ts + dr θ˙s ns γβ dt sa sa    1 dr − 2 (41) ts + ta β˙ γβ sa Differentiating equation (32), gives:    d dr dr 2 dt sa sa   d r +2 sin(θad )θ˙ad sa   d dr −2 cos(θad ) dt sa ˙ =0 −2γ 2 ββ (42) The rate of change of the UAV angle θcu is given by: ! ˙r (dr /sa ) − dr d (dr /sa ) d dt θ˙ud = γκu (43) (dr /sa )2 The rate of change of β is given by:   d 2 sin(θcu /2) β˙ = dt θcu ˜ ˙ = β θcu   θcu cos(θcu /2) − 2 sin(θcu /2) ˜ β= 2 θcu

6329

(44)

18th IFAC World Congress (IFAC'11) Milano (Italy) August 28 - September 2, 2011

Also

distance is set at 25 m and the initial Aircraft range is 200m. The trajectory, range, UAV curvature and Liapunov function are shown in figure 12.

θ˙ad = θ˙d + θ˙t = θ˙d

(45)



dr sa



Miss Range 250

250

Substituting into equation (42), gives:      d dr dr − cos(θad ) sa dt sa −γ 2 β β˜θ˙cu   dr ˙ θd = 0 + sin(θad ) sa Bringing the equations together, gives: dr γκu d (dr /sa )2 dt

Trajectory 300

200 200

m

150

150

100 100

50 0

50 −50 −100 −150

−100

−50

0

50 m

100

150

200

250

0

0

1

2

3

4

5

6

7

5

6

7

(46) (a) Trajectory

d˙r γκu + θ˙cu = (dr /sa )

UAV κ

−3

12

(47)

(b) Range

x 10

−3

4

11 10

2.5 8 2 7 1.5 6 1

5

0.5

4 3

0

1

2

3

4

5

6

7

(c) UAV κ

where:

 [(dr /sa ) − cos(θad )] −γ 2 β β˜  dr γκu Au =  1 2 (dr /sa ) ! − sin(θad )(dr /sa ) 0 γκ u Bu = 0 (dr /sa )

3

9

Hence, if the ratio (dr /sa ) is known by iterating onto the solution of equation (32), then the rate of change of (dr /sa ) and θcu are given by the solution of:     d (dr /sa ) θ˙ = Bu ˙d (48) Au θ cu dr dt 

Liapunov Function

x 10

3.5

0

0

1

2

3

4

(d) Liapunov Function

Fig. 12. Simulation Parameters 7. CONCLUSIONS (49)

Hence:

    d (dr /sa ) θ˙d B = A−1 (50) u u θ cu d˙r dt Bringing terms together, equation (41) can be written in the form:    d dr        ˜  1 d d β dt sa  r r  ˆt˙ u = td + ta nd  td − ˙   θ γβ β sa sa cu θ˙d (51)  dr d ˙ where θd is the missline rate measurement, and dt

This paper has developed an algorithm to enable a UAV to automatically avoid collision with an Aircraft. The Differential Geometry approach has been useful in determining a guidance algorithm to produce a constant curvature evasion manoeuvre to avoid collision with an Aircraft flying in a straight line. The analysis has been developed for a 2D engagement, but the analysis will translate, with some modification, to a 3D engagement. Simulation has shown that the guidance algorithm performs as expected. The analysis did not include the UAV dynamics and this, together with the extension to 3D will be the subject for future study. REFERENCES

sa

and θ˙ad are obtained from equation (50). Given that: ˆt˙ Lu = θˆ˙u n ˆu (52) A guidance law of the form: ˙ κu = θˆu sign(θǫ ) − Kθe (53) is proposed, to give: dV (54) = −Kθǫ2 dt which is negative definite, as required. 6. SIMULATION

The UAV and Aircraft avoidance algorithm was simulated to verify the correct operation of the algorithm. The Aircraft is another UAV travelling at 30 m/sec across the path of the UAV which is travelling at 50 m/sec. The miss

J. Kuchar and L. Yang, Review of Conflict Detection and Resolution Modeling Methods IEEE Transactions on Intelligent Transportation Systems, Vol. 1, No. 4, December 2000, pp. 179.189. J. Asmat, B. Rhodes, B., Umansky, C. Villavicencio, A. Yunas, G. Donohue,and A. Lacher, UAS Safety: Unmanned Aerial Collision Avoidance System (UCAS)”, Proceedings of the 2006 Systems and Information Engineering Design Symposium Anusha Mujumdar and Radhakant Padhi Nonlinear Geometric and Differential Geometric Guidance of UAVs for Reactive Collision Avoidance Proceedings of the 2010 AIAA Guidance, Navigation, and Control Conference, AIAA-2010-8315

6330