[PPT] - Nonlinear trajectory tracking control based on adaptive backstepping PowerPoint Presentation

SLIDE 1

Introduction Trajectory tracking controller Simulation results Conclusions

DSOR

dynamical systems and

cean robotics LAB

Nonlinear trajectory tracking control based on adaptive backstepping

David Cabecinhas

Dynamic Stochastic Filtering, Prediction, and Smoothing Instituto Superior T´ ecnico

July 7th, 2010

David Cabecinhas Trajectory tracking control of a quadrotor 1/19

SLIDE 2

Introduction Trajectory tracking controller Simulation results Conclusions

DSOR

dynamical systems and

cean robotics LAB

Outline

1

Introduction Desired trajectory tracking controller Quadrotor model

2

Trajectory tracking controller Problem statement Controller design

3

Simulation results

4

Conclusions

David Cabecinhas Trajectory tracking control of a quadrotor 2/19

SLIDE 3

Introduction Trajectory tracking controller Simulation results Conclusions Desired trajectory tracking controller Quadrotor model

DSOR

dynamical systems and

cean robotics LAB

Trajectory tracking for quadrotors

Quadrotor – underactuated vehicle, nonlinear dynamics Desired controller properties Guarantee asymptotic stability of the tracking error at the origin with actuation bounded in tracking error in the presence of constant force disturbances Design methodology Dynamic model handled in its natural space SE(3) = R3 × SO(3) Nonlinear Lyapunov-based techniques

Adaptive backstepping

Dynamic augmentation of the actuation

David Cabecinhas Trajectory tracking control of a quadrotor 3/19

SLIDE 4

Introduction Trajectory tracking controller Simulation results Conclusions Desired trajectory tracking controller Quadrotor model

DSOR

dynamical systems and

cean robotics LAB

Trajectory tracking for quadrotors

Quadrotor – underactuated vehicle, nonlinear dynamics Desired controller properties Guarantee asymptotic stability of the tracking error at the origin with actuation bounded in tracking error in the presence of constant force disturbances Design methodology Dynamic model handled in its natural space SE(3) = R3 × SO(3) Nonlinear Lyapunov-based techniques

Adaptive backstepping

Dynamic augmentation of the actuation

David Cabecinhas Trajectory tracking control of a quadrotor 3/19

SLIDE 5

Introduction Trajectory tracking controller Simulation results Conclusions Desired trajectory tracking controller Quadrotor model

DSOR

dynamical systems and

cean robotics LAB

Quadrotor model – Actuation

2 pairs of counter-rotating rotors (F1, F3) and (F2, F4) Input transformation T = 4

i=1Fi

n =   α1(F1 − F3) α2(F2 − F4) α3(F1 − F2 + F3 − F4)   Underactuated vehicle

One-directional thrust T Full torque control n = [n1 n2 n3]T 1 force + 3 torques vs. 6 DoF

F1 F2 F3 F4 T n1

3

n n2

David Cabecinhas Trajectory tracking control of a quadrotor 4/19

SLIDE 6

Introduction Trajectory tracking controller Simulation results Conclusions Desired trajectory tracking controller Quadrotor model

DSOR

dynamical systems and

cean robotics LAB

Quadrotor Model – Equations of motion

Kinematics ˙ p = Rv ˙ R = −S(ω)R Dynamics ˙ v = −S(ω)v + 1

mf

˙ ω = −J−1S(ω)Jω + J−1n External force includes thrust and gravitational contributions f = −Tu3 + mgRTu3 + RTb, u3 = [0 0 1]T External force disturbance: b ∈ R3 Using input transformation n = Jτ + S(ω)Jω, Euler equations are reduced to integrator form ˙ ω = τ = [τ1 τ2 τ3]T

David Cabecinhas Trajectory tracking control of a quadrotor 5/19

SLIDE 7

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Trajectory tracking – Sketch of problem

Premise Vehicle is underactuated – cannot describe arbitrary trajectories in SE(3) Solution Enforce position tracking only (define desired position pd(t)) Orientation automatically constrained to direct T (desired rotation matrix Rd = [r1d r2d r3d] satisfies r3dTd = mgu3 + b − m¨ pd) pd(t) =    

r1 fg fg T T r3 r1 r3

pd(t) =   x(t)  

David Cabecinhas Trajectory tracking control of a quadrotor 6/19

SLIDE 8

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Trajectory tracking – Problem statement

Assumption Desired trajectory pd(t) is a class C4 function Control objective Design a control law for T and τ such that p → pd with the largest possible basin of attraction.

David Cabecinhas Trajectory tracking control of a quadrotor 7/19

SLIDE 9

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design

Position error e1 = p − pd Initial candidate Lyapunov function V1 = 1

2eT 1e1

Go through backstepping procedure until actuation is available ˙ V1 = eT

1 ˙

e1 ˙ V1 = −k1eT

1σ(e1) + k1eT 1(σ(e1) + 1 k1 ˙

e1) ˙ V1 = −W1(e1) + k1eT

1 (σ(e1) + 1

k1 (Rv − ˙ pd))

e2

¾(x) x

pmax ¡pmax

David Cabecinhas Trajectory tracking control of a quadrotor 8/19

SLIDE 10

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design

Position error e1 = p − pd Initial candidate Lyapunov function V1 = 1

2eT 1e1

Go through backstepping procedure until actuation is available ˙ V1 = eT

1 ˙

e1 ˙ V1 = −k1eT

1σ(e1) + k1eT 1(σ(e1) + 1 k1 ˙

e1) ˙ V1 = −W1(e1) + k1eT

1 (σ(e1) + 1

k1 (Rv − ˙ pd))

e2

¾(x) x

pmax ¡pmax

David Cabecinhas Trajectory tracking control of a quadrotor 8/19

SLIDE 11

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design

New Lyapunov function candidate V2 = V1 + 1

2eT 2e2

˙ V2 = −W2(e1, e2) + k1k2eT

2(e2 + 1 k2

1k2 ( 1

mRf − ¨

pd)) With full force control in f, we could obtain ˙ V2 = −W2(e1, e2) < 0 However f = −Tu3 + mgRTu3 + RTb and b is unknown! Define new error as e3 e2 +

1 k2

1k2 ( 1

m(−RTu3 + mgu3 +

b) − ¨ pd) ˙ V2 = −W2(e1, e2) + k1k2eT

2e3 + 1 mk1 eT 2

b and iterate backstepping process twice more

David Cabecinhas Trajectory tracking control of a quadrotor 9/19

SLIDE 12

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design

New Lyapunov function candidate V2 = V1 + 1

2eT 2e2

˙ V2 = −W2(e1, e2) + k1k2eT

2(e2 + 1 k2

1k2 ( 1

mRf − ¨

pd)) With full force control in f, we could obtain ˙ V2 = −W2(e1, e2) < 0 However f = −Tu3 + mgRTu3 + RTb and b is unknown! Define new error as e3 e2 +

1 k2

1k2 ( 1

m(−RTu3 + mgu3 +

b) − ¨ pd) ˙ V2 = −W2(e1, e2) + k1k2eT

2e3 + 1 mk1 eT 2

b and iterate backstepping process twice more

David Cabecinhas Trajectory tracking control of a quadrotor 9/19

SLIDE 13

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design

New Lyapunov function candidate V2 = V1 + 1

2eT 2e2

˙ V2 = −W2(e1, e2) + k1k2eT

2(e2 + 1 k2

1k2 ( 1

mRf − ¨

pd)) With full force control in f, we could obtain ˙ V2 = −W2(e1, e2) < 0 However f = −Tu3 + mgRTu3 + RTb and b is unknown! Define new error as e3 e2 +

1 k2

1k2 ( 1

m(−RTu3 + mgu3 +

b) − ¨ pd) ˙ V2 = −W2(e1, e2) + k1k2eT

2e3 + 1 mk1 eT 2

b and iterate backstepping process twice more

David Cabecinhas Trajectory tracking control of a quadrotor 9/19

SLIDE 14

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design – Adaptive backstepping

V4 = 1

2eTe

e = [eT

1 eT 2 eT 3 eT 4]T ∈ R12

˙ V4 = −W4(e) + eT

4

h(e, T, ˙

T, R, ω, p(4)

d ) + RM(T)¯

u +

1 k2

1k2

˙

b
+

1 k2

1k2 eT

3

˙

b + 1

k1

bT (e2 + e3 + e4)) ¯ u = ¨ T τ1 τ2 T , M(T) = c1

m

0 0 −T

0 T 1 0

, nonsingular if T = 0

Add estimation error to the Lyapunov function V4 = 1 2eTe + 1 2k1

bTΓ−1

b e = [eT

1 eT 2 eT 3 eT 4]T ∈ R12

˙ V4 = −W4(e) + eT

4

h(e, T, ˙

T, R, ω, p(4)

d ) + RM(T)¯

u +

1 k2

1k2

˙

b
+

1 k2

1k2 eT

3

˙

b + 1

k1

bT −Γ−1 ˙

b + (e2 + e3 + e4)
David Cabecinhas

Trajectory tracking control of a quadrotor 10/19

SLIDE 15

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design – Adaptive backstepping

V4 = 1

2eTe

e = [eT

1 eT 2 eT 3 eT 4]T ∈ R12

˙ V4 = −W4(e) + eT

4

h(e, T, ˙

T, R, ω, p(4)

d ) + RM(T)¯

u +

1 k2

1k2

˙

b
+

1 k2

1k2 eT

3

˙

b + 1

k1

bT (e2 + e3 + e4)) ¯ u = ¨ T τ1 τ2 T , M(T) = c1

m

0 0 −T

0 T 1 0

, nonsingular if T = 0

Add estimation error to the Lyapunov function V4 = 1 2eTe + 1 2k1

bTΓ−1

b e = [eT

1 eT 2 eT 3 eT 4]T ∈ R12

˙ V4 = −W4(e) + eT

4

h(e, T, ˙

T, R, ω, p(4)

d ) + RM(T)¯

u +

1 k2

1k2

˙

b
+

1 k2

1k2 eT

3

˙

b + 1

k1

bT −Γ−1 ˙

b + (e2 + e3 + e4)
David Cabecinhas

Trajectory tracking control of a quadrotor 10/19

SLIDE 16

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design – Closing the loop

Feedback laws Estimator update law: ˙

b = Γ(e2 + e3 + e4)

Control law: ¯ u = −M(T)−1RT h(.) +

1 k2

1k2

˙

b
Combines torque actuation [τ1 τ2] with the 2nd order

derivative of the thrust ¨ T τ3 plays no part in providing trajectory tracking Closed-loop system ˙ e = −Kbe + [0 I3 I3 I3]T 1

k1

b ˙

b = −Γ[0 I3 I3 I3]e

David Cabecinhas Trajectory tracking control of a quadrotor 11/19

SLIDE 17

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design – Closing the loop

Feedback laws Estimator update law: ˙

b = Γ(e2 + e3 + e4)

Control law: ¯ u = −M(T)−1RT h(.) +

1 k2

1k2

˙

b
Combines torque actuation [τ1 τ2] with the 2nd order

derivative of the thrust ¨ T τ3 plays no part in providing trajectory tracking Closed-loop system ˙ e = −Kbe + [0 I3 I3 I3]T 1

k1

b ˙

b = −Γ[0 I3 I3 I3]e

David Cabecinhas Trajectory tracking control of a quadrotor 11/19

SLIDE 18

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design – Closing the loop

Feedback laws Estimator update law: ˙

b = Γ(e2 + e3 + e4)

Control law: ¯ u = −M(T)−1RT h(.) +

1 k2

1k2

˙

b
Combines torque actuation [τ1 τ2] with the 2nd order

derivative of the thrust ¨ T τ3 plays no part in providing trajectory tracking Closed-loop system ˙ e = −Kbe + [0 I3 I3 I3]T 1

k1

b ˙

b = −Γ[0 I3 I3 I3]e

David Cabecinhas Trajectory tracking control of a quadrotor 11/19

SLIDE 19

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design – Stability analysis

Small position error (e1 < pmax) Asymptotic stability of the error system origin is proven by Barbalat’s Lemma Large position error (e1 > pmax) Using an auxiliar Lyapunov function Vσ = 1 2

4

i=2

eT

i ei + 1

2

bTΓ−1

b it can be proven that there is a time T such that for all t > T e1(t) < pmax.

David Cabecinhas Trajectory tracking control of a quadrotor 12/19

SLIDE 20

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design – Stability analysis

Small position error (e1 < pmax) Asymptotic stability of the error system origin is proven by Barbalat’s Lemma Large position error (e1 > pmax) Using an auxiliar Lyapunov function Vσ = 1 2

4

i=2

eT

i ei + 1

2

bTΓ−1

b it can be proven that there is a time T such that for all t > T e1(t) < pmax.

David Cabecinhas Trajectory tracking control of a quadrotor 12/19

SLIDE 21

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design – Exploring the extra DoF

Control law for trajectory tracking leaves τ3 = ˙ ω3 free. 1st approach Apply τ3 = −k3ω3 to stabilize angular velocity ω3 at zero. 2nd approach Enforce zero sideslip angle ⇔ vy = [0 1 0]v → 0 1 Consider Rd = [r1d r2d r3d] ∈ SO(3) Thrust direction r3d already prescribed by trajectory tracking requirements. 2 Define r2d so that r2 → r2d ⇔ rT

2r2d → 1 ⇒ vy → 0

David Cabecinhas Trajectory tracking control of a quadrotor 13/19

SLIDE 22

Introduction Trajectory tracking controller Simulation results Conclusions Problem statement Controller design

DSOR

dynamical systems and

cean robotics LAB

Controller design – Exploring the extra DoF

2nd approach 3 Apply PD-like control law τ3 = −l2(ω3 − ω3d + l1rT

2dr1) + ˙

ω3d − l1 d

dt(rT 2dr1)

r2 r2d r1d rT

2dr1

r1

−l2l1rT

2dr1 opposes growth

in angular distance between r2 and r2d

David Cabecinhas Trajectory tracking control of a quadrotor 14/19

SLIDE 23

Introduction Trajectory tracking controller Simulation results Conclusions

DSOR

dynamical systems and

cean robotics LAB

Simulation results

−10 −5 5 10 15 20 25 30 −20 −15 −10 −5 x (m) y (m) Desired trajectory Vehicle trajectory − Saturation active Vehicle trajectory − Saturation inactive

Track an eight-shaped trajectory pd(t) =   5 cos(0.2t) − 5 2.5 sin(0.4t) −10   Saturation active for initial position Constant lateral wind disturbance of 4.76 N 5 % uncertainty in gravitational acceleration

David Cabecinhas Trajectory tracking control of a quadrotor 15/19

SLIDE 24

Introduction Trajectory tracking controller Simulation results Conclusions

DSOR

dynamical systems and

cean robotics LAB

Simulation results

Linear velocity

5 10 15 20 25 30 −4 −2 2 4 6 Linear Velocity (m/s) Time (s) vx vy vz

Bounded velocity rapidly acquired Convergence to desired velocity when saturation is no longer active vy converges to zero Force disturbance estimate

5 10 15 20 25 30 −2 −1 1 2 3 4 Force disturbance estimation (N/Kg) Time (s) ˆ bx ˆ by ˆ bz

b converges to b

bz captures uncertainty in g bx, by captures wind disturbance

David Cabecinhas Trajectory tracking control of a quadrotor 16/19

SLIDE 25

Introduction Trajectory tracking controller Simulation results Conclusions

DSOR

dynamical systems and

cean robotics LAB

Simulation results

Errors

2 4 −20 20 40 e1 2 4 −5 5 e2 2 4 −5 5 e3 2 4 −5 5 e4 Time (s) 5 10 15 20 25 30 −10 10 20 e11 e12 e13 5 10 15 20 25 30 −1 −0.5 0.5 e21 e22 e23 5 10 15 20 25 30 −1 −0.5 0.5 e31 e32 e33 5 10 15 20 25 30 −0.2 0.2 Time (s) e41 e42 e43

Actuation

2 4 40 45 50 55 60 Force (N) Time (s) 2 4 −10 −5 5 10 Torque (Nm) Time (s) 5 10 15 20 25 30 48 49 50 51 52 53 Time (s) 5 10 15 20 25 30 −1 −0.5 0.5 1 Time (s) T n1 n2 n3

David Cabecinhas Trajectory tracking control of a quadrotor 17/19

SLIDE 26

Introduction Trajectory tracking controller Simulation results Conclusions

DSOR

dynamical systems and

cean robotics LAB

Conclusions

Trajectory tracking control of quadrotors Solution for underactuated vehicles based on dynamic augmentation of the actuation Resorts to

Nonlinear systems theory Lyapunov-based stability analysis Adaptive backstepping techniques

Properties

Asymptotic stability in the presence of constant force disturbances Bounded influence of the position error in force and torque actuation

David Cabecinhas Trajectory tracking control of a quadrotor 18/19

SLIDE 27

Introduction Trajectory tracking controller Simulation results Conclusions

DSOR

dynamical systems and

cean robotics LAB

Nonlinear trajectory tracking control based on adaptive backstepping

David Cabecinhas

Dynamic Stochastic Filtering, Prediction, and Smoothing Instituto Superior T´ ecnico

July 7th, 2010

David Cabecinhas Trajectory tracking control of a quadrotor 19/19