Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 1

Chapter 10

Path Following

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 2

path planner

path manager

path following

autopilot

unmanned aircraft

waypoints

on-board sensors

position error

tracking error

status

destination, obstacles

servo commands state estimator

wind

path definition

airspeed, altitude, heading,

commands

map

x(t)

Control Architecture

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 3

Path Following •  For small UAVs, a major issue is wind

–  Always present to some degree –  Usually significant with respect to commanded

airspeed •  Wind makes traditional trajectory tracking

approaches difficult, if not infeasible –  Have to know the wind precisely at every instant to

determine desired airspeed •  Better approach: path following •  Rather than “follow this trajectory”, we control

UAV to “stay on this path”

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 4

Path Types

•  We will focus on two types of paths to follow: – Straight lines between two points in 3-D

•  Inclination of path within climb capabilities of UAV – Circular orbits or arcs in the horizontal plane

•  Paths for common applications can be built up from these path primitives – Methods for following other types of paths

found in literature

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 5

rp

ep = p� r

�q

epy

epx

north

east

Straight Line Path Description

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 6

rp

ep = p� r

�q

epy

epx

north

east

Lateral Tracking Problem

Path error:

ep

=

0

@epx

epy

epz

1

A 4= RP

i

�pi � ri

�

where the transformation from inertial frame

to path frame is

RPi

4=

0

@cos�

q

sin�q

0

� sin�q

cos�q

0

0 0 1

1

A

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 7

rp

ep = p� r

�q

epy

epx

north

east

Lateral Tracking Problem Relative error dynamics in path frame:

✓epx

epy

◆=

✓cos�

q

sin�q

� sin�q

cos�q

◆✓Vg

cos�Vg

sin�

◆

= Vg

✓cos(�� �

q

)

sin(�� �q

)

◆

Regulate the cross-track error epy

to zero by com-

manding the course angle:

epy

= Vg

sin(�� �q

)

� = b�

(�c � �) + b�

(�c � �)

Select �c

so that epy

! 0

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 8

Longitudinal Tracking Problem

ep = p� r

pq2n + q2

e

�qd

�rd

hd � (�rd)

hd

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 9

pq2n + q2

e

�qd

Longitudinal Tracking Problem

�rd

hd � (�rd)

hd

By similar triangles

(hd + rd)ps2n + s2e

=

�qdpq2n + q2e

Desired altitude based on current location

hd(r,p,q) = �rd �p

s2n + s2e

qdp

q2n + q2e

!

Select hcso that h ! hd(r,p,q)

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 10

Longitudinal Guidance Strategy Use altitude state machine from Ch. 6.

Closed-loop altitude dynamics:

h

hc=

bhs+ bhs2 + bhs+ bh

.

Altitude error:

eh4= h� hd(r,p,q) = h� hc

Error dynamics:

ehhc

= 1� h

hc

=

s2

s2 + bhs+ bh

Applying FVT:

eh,ss = lim

s!0s

s2

s2 + bhs+ bhhc

= 0, for hc=

H0

s,H0

s2

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 11

-100 -80 -60 -40 -20 0 20 40 60 80 1000

20

40

60

80

100

120

140

160

180

200

epy

�1

Lateral Tracking - Vector Field Concept

Desired course based on cross-track error:

�d(epy) = ��1 2

⇡tan

�1(kpathepy)

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 12

-100 -80 -60 -40 -20 0 20 40 60 80 100

kpath = 0.02

kpath = 0.05

kpath = 0.1

kpath = 0.2

kpath = 0.5

kpath = 1

Vector Field Tuning

kpath is a positive constant that affects the rate of transition of the desired course

•  kpath large à short, abrupt transition

•  kpath small à long, gradual transition

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 13

Lyapunov’s 2nd Method

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 14

Lateral Tracking Stability Analysis

Define the Lyapunov function W (epy) =12e

2py

Assume that course controller works and � = �q + �d(epy)

Since

˙W = epy epy

= �Vaepy sin

✓�1 2

⇡tan

�1(kpathepy)

◆

< 0

for epy 6= 0, then epy ! 0 asymptotically

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 15

Smallest Angle Turn Logic September 15, 2011 Time: 09:50am chapter10.tex

184 Chapter 10

Algorithm 3 Straight-line Following: [hc, χ c] = followStraightLine(r, q, p, χ)

Input: Path definition r = (rn, re, rd)⊤ and q = (qn, qe, qd)⊤, MAVposition p= (pn, pe, pd)⊤, course χ , gains χ∞, kpath, sample rate Ts.

1: Compute commanded altitude using equation (10.5).2: χq ← atan2(qe, qn)3: while χq − χ < −π do4: χq ← χq + 2π

5: endwhile6: while χq − χ > π do7: χq ← χq − 2π

8: endwhile9: epy ← − sin χq(pn − rn) + cos χq(pe − re)

10: Compute commanded course angle using equation (10.8).11: return hc, χ c

Algorithm 4 Circular Orbit Following: [hc, χ c] = followOrbit(c, ρ, λ, p, χ)

Input: Orbit center c = (cn, ce, cd)⊤, radius ρ, and direction λ, MAVposition p = (pn, pe, pd)⊤, course χ , gains korbit, sample rate Ts.

1: hc ← −cd2: d ←

!(pn − cn)2 + (pe − ce)2

3: ϕ ← atan2(pe − ce, pn − cn)4: while ϕ − χ < −π do5: ϕ ← ϕ + 2π

6: endwhile7: while ϕ − χ > π do8: ϕ ← ϕ − 2π

9: endwhile10: Compute commanded course angle using equation (10.13).11: return hc, χ c

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 16

Porbit

(c, ⇥,�)

(cn, ce)

(pn, pe)

'

�o

⇢

d

Orbit Following Orbit definition:

Porbit

(c, ⇢,�) =n

r 2 R3 : r = c+ �⇢�

cos', sin' 0�>

,' 2 [0, 2⇡)o

Center: c 2 R3

Radius: ⇢ 2 RDirection: � = 1 (CW) or � = �1 (CCW)

In polar coordinates, the position of MAV given by:

d: radial distance from orbit center

': phase angle of relative position

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 17

Porbit

(c, ⇥,�)

(cn, ce)

(pn, pe)

'

�o

⇢

d

Orbit Following Easiest to analyze in polar coordinates.

Using ✓pnpe

◆=

✓Vg cos�Vg sin�

◆

and converting to polar coordinates gives

✓˙d

d'

◆=

✓cos' sin'� sin' cos'

◆✓pnpe

◆

=

✓cos' sin'� sin' cos'

◆✓Vg cos�Vg sin�

◆

=

✓Vg cos(�� ')Vg sin(�� ')

◆

The result is

˙d = Vg cos(�� ')

' =

Vg

dsin(�� ')

� = �b��+ b�(�c � �)

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 18

Porbit

(c, ⇥,�)

(cn, ce)

(pn, pe)

'

�o

⇢

d

Orbit Following

Define

�o

= '+ �⇡

2

When d � ⇢ ! �d

⇡ �o

+ �⇡

2

.

When d = ⇢ ! �d

= �o

Therefore, let the desired course angle be

�d

(d�⇢,�) = �o

+� tan

�1

✓korbit

✓d� ⇢

⇢

◆◆

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 19

Orbit Tracking Stability Analysis Define the Lyapunov function W =

1

2

(d� ⇢)2

Assume that course controller works and � = �d(d� ⇢,�)

Since

˙W = (d� ⇢) ˙d

= (d� ⇢)(Vg cos(�� '))

= �Vg(d� ⇢) sin

✓tan

�1

✓korbit

✓d� ⇢

⇢

◆◆◆

< 0

for d� ⇢ 6= 0, then d� ⇢ ! 0 asymptotically

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 10, Slide 20

Porbit

(c, ⇥,�)

(cn, ce)

(pn, pe)

'

�o

⇢

d

Orbit Following

September 15, 2011 Time: 09:50am chapter10.tex

184 Chapter 10

Algorithm 3 Straight-line Following: [hc, χ c] = followStraightLine(r, q, p, χ)

Input: Path definition r = (rn, re, rd)⊤ and q = (qn, qe, qd)⊤, MAVposition p= (pn, pe, pd)⊤, course χ , gains χ∞, kpath, sample rate Ts.

1: Compute commanded altitude using equation (10.5).2: χq ← atan2(qe, qn)3: while χq − χ < −π do4: χq ← χq + 2π

5: endwhile6: while χq − χ > π do7: χq ← χq − 2π

8: endwhile9: epy ← − sin χq(pn − rn) + cos χq(pe − re)

10: Compute commanded course angle using equation (10.8).11: return hc, χ c

Algorithm 4 Circular Orbit Following: [hc, χ c] = followOrbit(c, ρ, λ, p, χ)

Input: Orbit center c = (cn, ce, cd)⊤, radius ρ, and direction λ, MAVposition p = (pn, pe, pd)⊤, course χ , gains korbit, sample rate Ts.

1: hc ← −cd2: d ←

!(pn − cn)2 + (pe − ce)2

3: ϕ ← atan2(pe − ce, pn − cn)4: while ϕ − χ < −π do5: ϕ ← ϕ + 2π

6: endwhile7: while ϕ − χ > π do8: ϕ ← ϕ − 2π

9: endwhile10: Compute commanded course angle using equation (10.13).11: return hc, χ c

The commanded course is

�c(t) = '+ �

⇡

2

+ tan

�1

✓korbit

✓d� ⇢

⇢

◆◆�

The orbit angle must be wrapped:

' = atan2(pe � ce, pn � cn) + 2⇡m