On Path Generation, Path Following On Path Generation, Path - - PowerPoint PPT Presentation

On Path Generation, Path Following On Path Generation, Path Following and Time Coordination for and Time Coordination for Small Small UAVs UAVs

• I. Kaminer
• I. Kaminer

Department of Mechanical and Department of Mechanical and Astronautical Astronautical Engineering, Engineering, Naval Postgraduate School Naval Postgraduate School Monterey, CA Monterey, CA Joint work with Joint work with NPS: Dobrokhodov, Yakimenko, Jones NPS: Dobrokhodov, Yakimenko, Jones IST: Pascoal, Ghabcheloo, Silvestre IST: Pascoal, Ghabcheloo, Silvestre VPI: Hovakimyan, Chu, Patel VPI: Hovakimyan, Chu, Patel

INTRODUCTION INTRODUCTION

UAV Wing span, m Wing type Max takeoff weight, kg Endurance, hour Payload, kg

FOG-R 3.2 high wing 41 1 Tern 3.5 high wing 59 4 Rascal 2.8 high wing 8 3 Telemaster 2.5 high wing 8 3 MAV 0.23 flapping-wing 0.015 0.3

Introduction

Mobile UAV Operations Testbed Runway at Camp Roberts Flight Test Architecture

Time Coordinated Control of Multiple UAVs

Wind drection

Segment 1: Glideslope capture (from any initial condition) Segment 2: Stabilized glideslope tracking

Top of glideslope

Bring each UAV from formation to the top of the glideslope at a given point in time

UAV 1 UAV n

• Net or wire
• 3000
• 2000
• 1000

1000 2000 3000 4000 5000 500 1000 1500 2000 2500 3000 3500 4000 x(m) y(m) UAV 1 UAV 2 UAV 3

• Time Coordinated Applications for Multiple UAVs (small)
• Sequential Autoland
• Coordinated Strike
• Coordinated Road Following
• Coordinate on the arrival of the leader subject to deconfliction and network

constraints

Outline Outline

• Generation of feasible trajectories using direct methods

Generation of feasible trajectories using direct methods

• Path following for a single UAV

Path following for a single UAV

• Coordination of multiple of

Coordination of multiple of UAVs UAVs

• Simulation results

Simulation results

• Flight Test results

Flight Test results

• Conclusions.

Conclusions.

• Assume polynomial trajectories defined using virtual length – decouples time and space

domains

Impact of changing total path length Impact of changing total path length and initial jerk

Generation of Feasible Trajectories Generation of Feasible Trajectories

Generation of Feasible Trajectories Generation of Feasible Trajectories

( )

d dt τ λ τ =

( ) ( )

2 2 2 1 2 3

( ) ( ) ( ) ( ) ( )

c

v x x x p τ λ τ τ τ τ λ τ τ ′ ′ ′ ′ = + + =

( ) ( ) ( ) ( )

2

( ) ( )

c c

a p p τ τ λ τ τ λ τ λ τ ′′ ′ ′ = +

,

• Let
• Then
• Boundary conditions
• given
• let
• then

( ) ( )

( ) ( )

0 , 0 , ,

c c c f c f

p p p t p t & && & &&

( ) ( ) ( )

( ) ( ) ( ) ( )

0 , (0) and ,

f f f f

v a v t a λ λ λ τ λ τ τ ′ ′ = = = =

( ) ( )

( )

( )

2

(0) (0) /

c c c

p p p λ λ ′′ ′ ′ = − &&

( ) ( )

( )

( )

2

( ) ( ) /

c f c f c f f f

p p t p t τ λ τ λ τ ′′ ′ ′ = − &&

Generation of Feasible Trajectories Generation of Feasible Trajectories

( )

min max

( )

c

v p v λ τ τ ′ ≤ ≤

( ) ( ) ( )

2 max

( ) ( )

c c

p p a τ λ τ τ λ τ λ τ ′′ ′ ′ + ≤ 0,

f

τ τ   ∀ ∈  

,

• Feasible trajectory for single UAV
• Multiple UAVs
• 1st UAV arrives in

, ith in

• therefore must guarantee
• ( )

( )

1 1 min max min max

1 1 1 1 1

, ,

f f

c c f f

p d p d T t t v v

τ τ

τ τ τ τ   ′ ′   = =        

∫ ∫

i

T

( )

( )

( )

2 2 , 1,...,

min , , [0, ] [0, ]

j k c j c k j k fj fk j k n j k

p p E τ τ τ τ τ τ

= ≠

− ≥ ∀ ∈ ×

, , 1, ,

i j

T T i j n i j ∩ ≠ ∅ ∀ = ≠

Generation of Feasible Trajectories Generation of Feasible Trajectories

,

• Optimization Problem
• where, for example the cost

( ) ( )

3 3 3

( )

f f

t i f D ci f D i ci

J c c v t dt c c p d

τ

ρ ρλ τ τ τ ′ = =

∫ ∫

represents the total fuel spent

[ ]

( )

( )

( )

min min max max max

, 1, 1, 2 2 , 1,..., 1 1 1

min (4) 1, min , , [0, ] [0, ] , , 2,...

fi

i i i n i n j k c j c k j k fj fk j k n j k f fi f f fi

w J subject to for each i n and p p E t t t t t i n

τ

τ τ τ τ τ τ

= = = ≠

   ∈    − ≥ ∀ ∈ ×    ≤ ≤ ≤ =  

∑

Generation of Feasible Trajectories Generation of Feasible Trajectories

• 2D and 3D view

,

• 3000
• 2000
• 1000

1000 2000 3000 4000 5000 500 1000 1500 2000 2500 3000 3500 4000 x(m) y(m) UAV 1 UAV 2 UAV 3

• 2000

2000 4000 6000 1000 2000 3000 4000 5000 200 400 600 800 X(m) Y(m) Z(m) UAV 1 UAV 2 UAV 3

1000 2000 3000 4000 5000 6000 16 18 20 UAV1 speed (m/sec) 1000 2000 3000 4000 5000 6000 7000 8000 10 15 20 25 UAV2 speed (m/sec) 2000 4000 6000 8000 10000 12000 10 15 20 25 path length (m) UAV3 speed (m/sec)

UAV 1 UAV 2 UAV 3

1 T 2 T 3 T 1 2 3 T T T ∩ ∩

• feasible velocity profiles
• arrival intervals

• UAV

– Hardware

• Airframe; Sig Rascal 110

– 2.8 meter span, 8 kg, 26 cc gas engine – 2-3 hour endurance – 15-30 m/s velocity

• Payload:

– PTZ color camera with AF and 10:1 zoom – PC104 with WLAN Mesh Networks card – PC104 for gimbal control and AP interface – PELCO network video server

Network Control Flight Test Architecture

Onboard PC104

UAV Path Following UAV Path Following

• Problem: follow polynomial trajectories defined using virtual pathlength

– time independent – must use UAV attitude – leaves velocity as a degree of freedom for time coordination

• Inner/Outer Loop Solution

Onboard A/P (Inner loop) Path following (Outer loop)

Pitch rate Yaw rate commands

Trajectory Generation User Laptop

polynomial path Boundary condiitons

L1 adaptive controller

Path

q

Q

{I} : Inertial Frame

P

{F} : Serret-Frenet Frame

s1 y1 Virtual Target

UAV Path Following UAV Path Following

Key idea: use virtual target to determine desired location on the path

Path Q

{I} : Inertial Frame

P

Control the evolution of the virtual target : added degree of freedom

UAV Path Following UAV Path Following

UAV Path Following (Outer Loop) UAV Path Following (Outer Loop)

3D Case: 3D Case: Kinematic Kinematic equations equations Kinematic Kinematic Error equations Error equations

1 1 1 1 1 1 1

(1 ) cos cos ( ) cos sin sin

e e e e e e e e

s y v y s z v G z y v u u

θ ψ

τ κ θ ψ τ κ ς θ ψ ςτ θ θ ψ  = − − +  = − − +   = = − −   =   =  & & & & & & & &

( )

1

cos sin cos sin . sin cos cos tan cos cos cos

e e e e e e e e e

u q u r

θ ψ

φ φ γ ψ ςτ φ φ γ τ ς θ ψ κ θ θ

−

−               = −           − +               & &

where I F W

, γ ψ , ,

e e e

φ θ ψ , ,

c c c

φ θ ψ

Coordinate systems

torsion curviture τ κ − −

1

cos cos cos sin sin 1 cos x v y v z v q r γ ψ γ ψ γ γ ψ γ

−

=   = −   =          =               & & & & &

UAV Path Following UAV Path Following (Outer Loop)

(Outer Loop)

Kinematic Kinematic Control Law Control Law

Let where then

( )

( )

2 2 2 2 2 1 1 1 1 2

1 1 1 ( ) . 2 2 2

e e

V s y z c c

θ ψ

θ δ ψ δ = + + + − + −

1 1 1 1 1 1

sin and sin

d d

z y z y

θ ψ

δ θ δ ψ ε ε

− −

    = =         + +    

( )

( )

1 1 2 1 1 3 2 1

cos cos sin sin sin sin cos

e e e e e e e e e

K s v u K c z v u K c y v

θ θ θ θ θ ψ ψ ψ ψ ψ

τ θ ψ θ δ θ δ δ θ δ ψ δ ψ δ θ δ ψ δ = + − = − − + + − − = − − − + − & & &

V V λ ≤ − &

• 25
• 20
• 15
• 10
• 5

5 10 15 20 25

• π/2

π/2

ρ δ(ρ)

Shaping function

Choose Choose K K1

1, K

, K2

2, K

, K3

3 to provide time

to provide time-

• scale separation between

scale separation between inner and outer loops inner and outer loops

Onboard PC104

UAV Path Following: Inner Loop UAV Path Following: Inner Loop

Onboard A/P (Inner loop) Path following (Outer loop)

Pitch rate Yaw rate commands

Trajectory Generation User Laptop

polynomial path Boundary condiitons

L1 adaptive controller

L1 Adaptive Output Feedback Controller for Systems of Unknown Dimension System dynamics: Assumptions:

• Control objective:

Courtesy of Naira Hovakimyan, VPI, Blacksburg, VA

Reference System and Stability

Consider the following reference system Small-gain theorem One possible choice Restricts the class of systems

System dynamics: Control law: Computable lower bound for the adaptation rate: Output predictor: Adaptive law:

L1 Adaptive Output Feedback Controller

( )

( ) ( ) ( ) ( ) u s C s r s s σ = − )

( )

) ( ˆ ) ( ) ( ) ( ˆ s s u s M s y σ + = System

y y ˆ r σ ˆ y ~ +−

σ ˆ − r kg ) (s C

Control Law

u

Output Predictor Adaptive Law ( )

ˆ ˆ Proj , y c σ σ = Γ & %

Control Architecture

Main Result

Performance bounds:

Main result:

Discussion on Class of Systems

Stability of Let Stabilization of via a PI Open-loop system with PI in the small-gain theorem Closed-loop system with PI We claim: Indeed:

PI

Rohrs’ Example: Unmodeled Dynamics

) ( 229 30 229 ) (

2

s u s s a s k s y

p

+ + + =

System with unmodeled dynamics:

Unmodeled dynamics plant dynamics

1 , 2 = = a k p

Nominal values

• f plant parameters:

Reference system dynamics:

) ( 3 3 ) ( s r s s ym + =

Control signal: ) ( ) ( ) ( ) ( ) ( t r t k t y t k t u

r y

+ = Adaptive laws from Rohrs’ simulations:

5 . 1 , 14 . 1 ) ( )), ( ) ( )( ( ) ( 1 , 65 . ) ( )), ( ) ( )( ( ) (

* *

= = − = − = − = − =

r r m r r y y m k y

k k t y t y t r t k k k t y t y t y t k γ γ & &

5 10 15 20

• 4
• 3
• 2
• 1

1 2 3 4 Time (sec) Output r(t)=0.3+1.85*sin(16.1*t)

Instability in the presence of unmodeled dynamics

5 10 15 20

• 15
• 10
• 5

5 10 15 Time (sec) Parameters r(t)=0.3+1.85*sin(16.1*t) Kr Ky

Parameter drift System output

) 1 . 16 sin( 85 . 1 3 . ) ( t t r + =

Changing the reference input

5 10 15 20

• 3
• 2
• 1

1 2 3 4 Time (sec) Output r(t)=0.3+2*sin(8*t) 5 10 15 20

• 10
• 5

5 Time (sec) Parameters r(t)=0.3+2*sin(8*t) Kr Ky

) 8 sin( 2 3 . ) ( t t r + =

• 140
• 120
• 100
• 80
• 60
• 40
• 20

System: P0 Frequency (rad/sec): 16.1 Magnitude (dB): -24.2 System: P Frequency (rad/sec): 16.1 Magnitude (dB): -30.6 Magnitude (dB) 10

• 2

10

• 1

10 10

1

10

2

10

3

• 270
• 225
• 180
• 135
• 90
• 45

System: P0 Frequency (rad/sec): 16.1 Phase (deg): -86.4 Phase (deg) System: P Frequency (rad/sec): 16.1 Phase (deg): -180 Bode Plot (Rohr's Ex) Frequency (rad/sec) P0=1/(s+1) P=P0*229/(s

2+30s+229)

) 1 . 16 sin( 85 . 1 3 . ) ( t t r + =

• 140
• 120
• 100
• 80
• 60
• 40
• 20

System: P0 Frequency (rad/sec): 8 Magnitude (dB): -18.1 Magnitude (dB) System: P Frequency (rad/sec): 8 Magnitude (dB): -20.3 10

• 2

10

• 1

10 10

1

10

2

10

3

• 270
• 225
• 180
• 135
• 90
• 45

System: P0 Frequency (rad/sec): 8 Phase (deg): -82.8 Phase (deg) System: P Frequency (rad/sec): 8.01 Phase (deg): -139 Bode Plot (Rohr's Ex) Frequency (rad/sec) P0=1/(s+1) P=P0*229/(s

2+30s+229)

) 8 sin( 2 3 . ) ( t t r + =

Rohrs’ Explanation for Instability: The Feedforward Transfer Function

At the frequency 16.1rad the phase reaches -180 degrees in the presence of unmodeled dynamics, reverses the sign of high-frequency gain, the loop gain grows to infinity

instability

At the frequency 8rad the phase reaches -139 degrees in the presence of unmodeled dynamics, no sign reversal for the high-frequency gain the loop gain remains bounded

bursting

5 10 15 20

• 0.1

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Time (sec) Output With Projection operator(3) r(t)=0.3+1.85*sin(16.1*t)

Adding projection to Rohrs’ simulations to keep the parameters bounded

5 10 15 20

• 3
• 2
• 1

1 2 3 Time (sec) Parameters With Projection operator(3) r(t)=0.3+1.85*sin(16.1*t) Kr Ky

) 1 . 16 sin( 85 . 1 3 . ) ( t t r + =

3 3 3 3 ≤ ≤ − ≤ ≤ −

r y

k k

5 10 15 20 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Time (sec) Output With Projection operator(1.5) r(t)=0.3+1.85*sin(16.1*t)

5 . 1 5 . 1 5 . 1 5 . 1 ≤ ≤ − ≤ ≤ −

r y

k k

5 10 15 20

• 2.5
• 2
• 1.5
• 1
• 0.5

0.5 1 1.5 2 2.5 Time (sec) Parameters With Projection operator(1.5) r(t)=0.3+1.85*sin(16.1*t) Kr Ky

Improved knowledge of uncertainty can reduce the amplitude of oscillations

Rohrs’ example with L1 adaptive controller 91 . 1 88 . || )) ( 1 )( ( || 100 3 1 , 1 ) (

1

≈ < ≈ − = = = =

m L r k

T L s C s G k s s D γ γ

Predictor model System Adaptive Law

y y ˆ r u

r y k

k ˆ , ˆ + − χ

r x k u k

y r

3 ˆ − + ) / 1 ( ) ( s s D =

u

) ( 3 ) ( t t u χ − =

Control Law

Adaptive laws:

14 . 1 ) ( )), ( ) ( ˆ )( ( ) ( 65 . ) ( )), ( ) ( ˆ )( ( ) ( = − − = − = − − =

r r r y k y

k t y t y t r t k k t y t y t y t k γ γ & &

Design elements

Rohrs’ example simulated with L1 adaptive controller

5 10 15 20 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 Time (sec) Output L1 r(t)=0.3+1.85*sin(16.1*t)

) 1 . 16 sin( 85 . 1 3 . ) ( t t r + =

5 10 15 20

• 1
• 0.5

0.5 1 1.5 2 2.5 3 3.5 4 Time (sec) Parameters L1 r(t)=0.3+1.85*sin(16.1*t) Kr Ky 5 10 15 20 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Time (sec) Output L1 r(t)=0.3+2*sin(8*t) 5 10 15 20

• 2
• 1

1 2 3 4 5 Time (sec) Parameters L1 r(t)=0.3+2*sin(8*t) Kr Ky

) 8 sin( 2 3 . ) ( t t r + =

Difference in Feedforward TFs

MRAC

m

y

) ~ , ( ˆ y u Kr 1 2 + s 3 1 + s ) ~ , ( ˆ y y K y

y ~

229 30 229

2

+ + s s

r(t) P(s) y(s)

• u

Pref(s) + + + Pum(s) MRAC cannot alter the phase in the feedforward loop

L1

) ~ , ( ˆ y u K k s k

r

+ − 1 2 + s 3 1 + s ) ~ , ( ˆ y u K y

y ~

229 30 229

2

+ + s s

r(t) P(s) y(s)

• u

Ppr (s) + +

+

Pum(s) kg

) ~ , ( ˆ y u Kr ) ~ , ( ˆ y u K y

+ +

The phase shift in the feedforward loop is the mechanism that prevents instability

Feedforward TFs in MRAC and in L1 MRAC L1

Adaptation on both a) loop gain and b) phase Adaptation only on loop gain No adaptation on phase

Augmentation of an existing autopilot by L1 controller

( ) u z y ξ ξ ζ = + + = & F G H y

y x g x f x ) ( ) ( + = &

x

u

p

G

e

G

p

G

u

Autopilot UAV y

Path Following Kinematics UAV with Autopilot

Cascaded system

Augmentation of an existing autopilot by L1 controller

Exponentially stable system

Path following Kinematics

Path following Control algorithm

e

G x

c

y

c

y

UAV

y

c

y

Autopilot Path following Kinematics Path following Control algorithm

e

G

p

G x

Poor performance

UAV y

c

y Autopilot Path following Kinematics Path following Control algorithm

e

G

p

G u x Adaptive Controller

1

L

L1 augmentation

Time Coordinated Control Time Coordinated Control

• Define then for all i implies simultaneous arrival
• For ith UAV normalized position along the path is defined by

i i fi

τ τ τ ′ =

( ) 1

i f

t τ ′ =

• Assumption: the underlying network is modeled by an undirected connected

graph with a graph Laplacian L

• Then

such that

• Define error vector
• Then (Agreement Problem)

1 1 , , /

cos cos : ,

i i e i e i i i i i fi fi

K s v v d d θ ψ τ τ τ + = = + → &

11 11

1 1 , 1 0, , .

T T T T n n n

U L U U U U I UL U L L n n       = = = =            

( ) ( ) ( )

1 1 1 11 11

, 0, , ( ) 1

n n n n

L and L R L U R rank U n

− × − × −

≥ ∃ ∈ > ∈ = −

/ T

U µ τ =

/ / / 1 2

.....

n

µ τ τ τ = ⇔ = = =

• Coordination Problem: drive to 0
• Solution: a P controller for the leader and PI controller for followers using

normalized path lengths of the neighbors

• The followers learn the required velocity
• The gains selected to guarantee fast convergence

Time Critical Coordination Time Critical Coordination

µ

1

,1 1 1 1 1

( ) , ( ) , ( ), 2,..., ,

i i

d f j j J f i fi i j Ii j J Ii i j j J

v v a v a c i n τ τ τ τ τ τ τ χ χ τ τ

∈ ∈ ∈

  ′ ′ = + −         ′ ′ = − +       ′ ′ = − =

∑ ∑ ∑

&

( ) 1

i f

t τ ′ = :

f

t <<

Normalized Path lengths

Coordinated Path Following Coordinated Path Following

• Path following: follow polynomial trajectories defined using virtual

path length

• Coordination: coordinate UAV positions along respective path using

velocity to guarantee appropriate time of arrival Onboard A/P + UAV (Inner loop) Path following (Outer loop)

Pitch rate Yaw rate commands

Path Generation Coordination Network info

Velocity command desired path

L1 adaptation

Simulation Results Simulation Results

Network Model Network Model 341 s 342.7s 340 s UAV 3 UAV 2 UAV 1 Times of Arrival Times of Arrival

• 3000
• 2000
• 1000

1000 2000 3000 4000 5000 500 1000 1500 2000 2500 3000 3500 4000 4500 x(m) y(m)

• 4000
• 2000

2000 4000 6000 1000 2000 3000 4000 5000 6000

• 200

200 400 600 800 x(m) y(m) z(m)

50 100 150 200 250 300 350 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 time(sec) normalized path lengths UAV 1 UAV 2 UAV 3

2D and 3D view 2D and 3D view Normalized path lengths Normalized path lengths

50 100 150 200 250 300 350 −20 −15 −10 −5 5 10 Time (sec) UAV 1 y−errors (m) 50 100 150 200 250 300 350 −20 20 40 60 80 100 Time (sec) UAV 1 z−errors (m) With L1 Without L1 50 100 150 200 250 300 350 −15 −10 −5 5 Time (sec) UAV 2 y−errors (m) 50 100 150 200 250 300 350 −150 −100 −50 50 Time (sec) UAV 2 z−errors (m) With L1 Without L1 50 100 150 200 250 300 350 −80 −60 −40 −20 20 40 Time (sec) UAV 3 y−errors (m) 50 100 150 200 250 300 350 −50 50 100 150 200 Time (sec) UAV 3 z−errors (m) With L1 Without L1

Simulation Results: Path Following Errors Simulation Results: Path Following Errors

UAV 1 UAV 2 UAV 3

Hardware-in-the-Loop Simulation

Flight Test Setup

C-130 SA Mesh node IP: 192.168.112.XXX Pelco Transmitter IP: 192.168.199.2 Mesh Network Rascal_Status Rascal_Control Pelco_Control Rascal_GPS Avionics Control Pelco_Video SA_Rascal_Post Rascal Control Gimbal Control PC104 IP: 192.168.199.3 Gimbal Control Gateway / SA IP: 192.168.199.1 Mesh: 192.168.112.6 Field SA Mesh node IP: 192.168.112.XXX SA Remote Server IP: 192.168.112.XXX UAV Segment

3dBi Omni 2dBi Omni + 1W Amp 2dBi Omni + 1W Amp High performance antenna

Flight Test Results: Path Following

• no adaptation – effect of gain changes

Flight Test Results: Path Following

• no adaptation

Conclusions

♦ Direct Methods to generate trajectories for time-critical multi-UAV missions ♦ Trajectories are parametrized using virtual arc (path) ♦ Time and space are decoupled – small number of optimization parameters ♦ A priori satisfaction of the boundary conditions ♦ Analytic representation - absence of wild trajectories ♦ Path Following by controlling attitude ♦ Fits naturally with proposed direct method ♦ Easy to implement inner-outer loop structure ♦ L1 adaptation guarantees reasonable performance of the complete system

♦ Coordination in time by controlling speed

♦ Satisfies underlying network constraints

♦ Future Work:

♦ Better network model ♦ Flight test adaptation ♦ Flight test of the complete time coordinated system