Lecture_traj Tracking Maneuver Regulation

LectureTrajectory Tracking and Maneuver Regulation

Giuseppe Notarstefano

Advanced Control Techniques

(ACT 2015) Lecture Traj. Track. and Man. Reg. Lecce, 2015 1 / 27

Outline

Trajectory tracking control

Task definition: trajectory trackingExact input-output linearization of the decoupled quadrotor modelTrajectory tracking via exact linearizationTrajectory tracking for the slightly coupled quadrotor model

From trajectory tracking to maneuver regulation

Definition of maneuverTask definition: maneuver regulationManeuver regulation of feedback linearizable systems

Maneuver regulation of non-feedback-linearizable systems:trajectory generation and transverse linearization


Task definition: trajectory tracking

System dynamics

x = f (x , u)

y = h(x)

where y Rp is a performance output of the system.

Goal: Given a desired output curve t 7 yd(t), t [0,), find a feedbackcontrol such that the closed-loop trajectory (x(), u()) satisfies

1 limt h(x(t)) yd(t) = 02 x() is bounded.


Quadrotor planar model

Decoupled model

x = uf sin z = uf cos g = u

States: x , z , x , z , ,

Inputs: uf =T1+T2M , u =

T2T12dT J

Outputs: x , z (center of mass position)



Coupled model also known as PVTOL

x = uf sin + u cos z = uf cos + u sin g = u



T2T12dT J


uf

u

u

Z

X



Coupled model also known as PVTOL




T2T12dT J


uf

u

u

Z

X

Remark: PVTOL model captures other aerial vehicle dynamics as, e.g.,vectored thrust aircraft.


Decoupled model: exact input-output linearization

Only the first input appears in the second derivative of both outputs:[xz

]=

[ sin 0cos 0

] [ufu

]+

[0g]

Not well defined vector relative degree!

Dynamic extension: use a dynamic compensator for uf

w1 = w2

w2 = uDE

uf = w1

with uf = w1 and uf = w2 additional states for the whole system.



We can differentiate the outputs two more times and get[x (4)

z(4)

]=

[uf

2 sin 2uf cos uf 2 cos 2uf sin

]+

[ sin uf cos cos uf sin

] [uDEu

]




z(4)

]=

[uf


]+


] [uDEu

]Input matrix invertible for uf > 0 (physical constraint).




z(4)

]=

[uf


]+


] [uDEu


Linearizing feedback law[uDEu

]=

[ sin cos cos uf sin uf

]([uf


]+

[uxuz

])




z(4)

]=

[uf


]+


] [uDEu


R()

1

s

1

s

1

s

1

s

1

s

1

s

uf

uf

u

x

z


Trajectory tracking controller

Feedback linearized system [x (4)

z(4)

]=

[uxuz

]Define the tracking error

x = x xd(t)...

z(3) = z(3) z(3)d (t)and design a static feedback law for this linear time-invariant system.




z(4)

]=

[uxuz


x = x xd(t)...


Remark: the desired output curve must be at least C4.




z(4)

]=

[uxuz


x = x xd(t)...


Remark: to perform output tracking we just need to design a desiredoutput time-law according to purely geometric objectives.




z(4)

]=

[uxuz


x = x xd(t)...


Remark: system trajectories can be easily generated by knowing theoutput and its derivatives: dynamic inversion through differential flatness.


Exact input-output linearization: a different perspective

Given the desired outputs xd(t) and zd(t), and their derivatives, a systemtrajectory can by obtained by

ufd(t) =((zd(t) g)2 + x2d (t)

) 12

d(t) = atan2( xd(t), zd(t) + g))

ud(t) = d(t)

z + g

x




ufd(t) =((zd(t) g)2 + x2d (t)

) 12


ud(t) = d(t)

z + g

x

R()

1

s

1

s

1

s

1

s

1

s

1

s

uf

uf

u

x

z




ufd(t) =((zd(t) g)2 + x2d (t)

) 12


ud(t) = d(t)

z + g

x

Remark: [xd(t) zd(t) xd(t) zd(t) d(t) d(t)], [ufd(t) ud(t)] is atrajectory, i.e. it satisfies the dynamics!

Remark: this is related to the vector-thrust control strategy!


Quadrotor full model

Use Roll, Pitch and Yaw (or Euler angles) parametrization for the rotationmatrix: (Roll), (Pitch), (Yaw)

States: x , y , z , , , , x , y , z , , ,

Inputs: fz R (total thrust), R3 (torques)Outputs: x , y , z ,

The (decoupled) full model is also exactly input-output linearizable(i.e., feedback linearizable w.r.t. the chosen outputs)

Use dynamic extension on the total thrust fz

Mistler et al. Exact linearization and noninteracting control of a 4 rotors helicopter via

dynamic feedback. IEEE Int. Workshop on Human Interactive Communication, 2001.


Trajectory tracking: coupled model

Coupled PVTOL model



Trajectory tracking: coupled model

Coupled PVTOL model


R()

1

s

1

s

1

s

1

s

1

s

1

s

uf

uf

u

x

z


Remarks on the coupled PVTOL

Good news Coupled PVTOL feedback linearizable w.r.t. different output(Huygens Center of oscillation of the pendulum)

(Martin, Devasia and Paden, Aut. 1996)





Bad news Flat output (HC) not nice to design desired output trajectories.





Bad news Flat output (HC) not nice to design desired output trajectories.

Can we still apply the same feedback lawpretending it is decoupled?


Coupled PVTOL model is non-minimum phase

Both the inputs appear in the second derivative of both outputs:[xz

]=

[ sin cos cos sin

] [ufu

]+

[0g]

Vector relative degree is [2 2]

We can input-output linearize the system using the feedback control[ufu

]=

[ sin cos cos

sin

]([0g

]+

[uxuz

])




]=

[ sin cos cos sin

] [ufu

]+

[0g]


x = ux

z = uz

=(uz + g)

sin +

ux

cos




]=

[ sin cos cos sin

] [ufu

]+

[0g]


x = ux

z = uz

=(uz + g)

sin +

ux

cos

(x, z)

Z

X




]=

[ sin cos cos sin

] [ufu

]+

[0g]


x = ux

z = uz

=(uz + g)

sin +

ux

cos

(x, z)

Z

X

Remark: The I/O linearized PVTOL is a driven inverted pendulum.




]=

[ sin cos cos sin

] [ufu

]+

[0g]


x = ux

z = uz

=(uz + g)

sin +

ux

cos

(x, z)

Z

X

Unstable zero dynamics

=g

sin

The system is non-minimum phase!


Coupled model: approximate input-output linearization

Choose as control inputs[ufu

]=

[1 tan 0 1

] [ufu

]




]=

[1 tan 0 1

] [ufu

]The output dynamics become[

xz

]=

[ sin cos cos 0

] [ufu

]+

[0g]




]=

[1 tan 0 1

] [ufu


xz

]=

[ sin cos cos 0

] [ufu

]+

[0g]

Result: for sufficiently small (and bounded away from pi2 ) thefeedback is still stabilizing for reasonable trajectories.

Hauser et al. Nonlinear control of slightly non-minimum phase systems: application to

V/STOL aircraft. Automatica 1992.




]=

[1 tan 0 1

] [ufu


xz

]=

[ sin cos cos 0

] [ufu

]+

[0g]

General result: feedback linearization can be used for slightlynon-minimum phase systems to track not too aggressive trajectories.

Hauser et al. Nonlinear control of slightly non-minimum phase systems: application to

V/STOL aircraft. Automatica 1992.


Arc-length parametrization of PVTOL trajectories

Given: t 7 (xd(t), zd(t)) output trajectory of the PVTOL (time law).




Let sd(t) be the arc-length of the desired output curve at time t

sd(t) =

t0

xd()2 + zd()2d Z

X

s = sd(t)

Assume t 7 sd(t) be strictly increasing, and let s 7 td(s) be its inverse.




Let sd(t) be the arc-length of the desired output curve at time t

sd(t) =

t0

xd()2 + zd()2d

(xd(s), zd(s))

(x0d(s), z0d(s))

Z

X

s = sd(t)

sd(t)

Assume t 7 sd(t) be strictly increasing, and let s 7 td(s) be its inverse.Arc-length parametrized output trajectory

Path: spatial image of the desired output trajectory

s 7 (xd(s), zd(s)) := (xd(td(s)), zd(td(s)))Arc-length time-law: speed the path is travelled with.

t 7 sd(t)


Output maneuvers for planar vehicles

Output maneuver

Path: geometric 2D curve

s 7 (xd(s), zd(s))

Speed profile: desired speed at each path point

s 7 Vd(s)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

Vd(s)



Output maneuver


s 7 (xd(s), zd(s))


s 7 Vd(s)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

Vd(s)

For a given output trajectory, Vd(s) = sd(td(s)), so that Vd(sd(t)) = sd(t)



Output maneuver


s 7 (xd(s), zd(s))


s 7 Vd(s)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

Vd(s)


Remark: for a given path, different output maneuvers can be obtained bydeciding different speed profiles.



Output maneuver


s 7 (xd(s), zd(s))


s 7 Vd(s)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

Vd(s)


Remark: for a given output maneuver, different output trajectories can beobtained depending on the initial time at a given s.


System trajectories and maneuvers

Given: t 7 (xd(t), ud(t)), (desired) system trajectory, such thatxd(t) = f (xd(t), ud(t)), t 0yd(t) = h(xd(t))



Given: t 7 (xd(t), ud(t)), (desired) system trajectory, such thatxd(t) = f (xd(t), ud(t)), t 0yd(t) = h(xd(t))

Define

sd(t) =

t0||yd()||2d

with t 7 sd(t) strictly increasing, and let s 7 td(s) be its inverse.



Given: t 7 (xd(t), ud(t)), (desired) system trajectory, such thatxd(t) = f (xd(t), ud(t)), t 0

Output maneuver

Path: s 7 yd(s) := yd(td(s))Speed profile: s 7 Vd(s) := sd(td(s))




Desired maneuver

State-path: arc-length parametrized state-input curve

s 7 (xd(s), ud(s)) := (xd(td(s)), ud(td(s)))

Speed profile: desired speed for each point on the state-path

s 7 Vd(s) := sd(td(s))




Desired maneuver





Using the chain rule for derivation, it can be shown that

d

dsxd(s) =

1

Vd(s)f (xd(s), ud(s)).




Desired maneuver





Remark Vd(s) related to the velocity components of the state maneuverxd(s). Thus, a maneuver is uniquely defined by s 7 (xd(s), ud(s)).


Task definition: maneuver regulation

Definition

Given a desired output maneuver s 7 yd(s) and s 7 Vd(s), s [0,),find a feedback control such that the closed-loop trajectoryt 7 (x(t), u(t)) satisfies, for some s(t) = pi(x(t)),

1 limt ||h(x(t)) yd(s(t))|| = 02 limt ||s(t) Vd(s(t))|| = 0.3 x() is bounded.


Task definition: maneuver regulation

Definition

Given a desired output maneuver s 7 yd(s) and s 7 Vd(s), s [0,),find a feedback control such that the closed-loop trajectoryt 7 (x(t), u(t)) satisfies, for some s(t) = pi(x(t)),

1 limt ||h(x(t)) yd(s(t))|| = 02 limt ||s(t) Vd(s(t))|| = 0.3 x() is bounded.

Remark Path following: just satisfy condition 1 (and 3)


Maneuver regulation for feedback linearizable systems

For a feedback linearizable system

x = f (x , u)

there exist x = (x) and u = (x , u) such that

x = Ax + Bu.

Let s 7 (xd(s), ud(s)) be the desired maneuver for the linear system.




x = f (x , u)


x = Ax + Bu.


Decoupled PVTOL

1 feedback linearizable by using the performance output

2 given yd() (desired output curve), a desired trajectory/maneuver canbe computed




x = f (x , u)


x = Ax + Bu.



u = ud(t) + K (x xd(t)).



Given s 7 (xd(s), ud(s)), let

pi(x) = arg minsR||x xd(s)||2P ,

with ||x xd(s)||2P = (x xd(s))TP(x xd(s)).






Assumption pi

Given P > 0, c > 0 such that pi is well defined on

c = {x Rn | ||x xd(s)||2P < c}






Assumption pi

Given P > 0, c > 0 such that pi is well defined on

c = {x Rn | ||x xd(s)||2P < c}

Remark: to satisfy Assumption pi, it must hold dds xd(s) bounded away

from zero and d2

ds2xd(s) bounded.






Theorem

Let s 7 (xd(s), ud(s)) be a desired maneuver satisfying Assumption pi.Let K be designed so that (A + BK ) is Hurwitz, and let P > 0 such that,(A + BK )TP + P(A + BK ) = Q < 0. Then

u = ud(pi(x)) + K (x xd(pi(x)))

provides exponentially stable maneuver regulation.






Theorem

Let s 7 (xd(s), ud(s)) be a desired maneuver satisfying Assumption pi.Let K be designed so that (A + BK ) is Hurwitz, and let P > 0 such that,(A + BK )TP + P(A + BK ) = Q < 0. Then

u = ud(pi(x)) + K (x xd(pi(x)))

provides exponentially stable maneuver regulation.

Hauser, Hindman. Maneuver regulation from trajectory tracking: feedback linearizable

systems. NOLCOS 1995.


Maneuver regulation for non-minimum phase systems

Problem:

1 the system cannot be feedback linearized

2 the (unobservable) zero dynamics is unstable

Thus, we cannot simply apply the maneuver regulation controller on thelinear subsystem.



Problem:




Assumption: given desired maneuver, i.e., s 7 (xd(s), ud(s)) such thatd

dsxd(s) =

1

Vd(s)f (xd(s), ud(s))

yd(s) = h(xd(s))



Problem:




Assumption: given desired maneuver, i.e., s 7 (xd(s), ud(s)) such thatd

dsxd(s) =

1

Vd(s)f (xd(s), ud(s))

yd(s) = h(xd(s))

IMPORTANT: we assume there is a tool to compute a desired trajectoryt 7 (xd(t), ud(t)) given a desired output t 7 yd(t) (tomorrows talk!)


Longitudinal and transverse coordinates

Given: desired maneuver s 7 (xd(s), ud(s))

Suppose y = h(x) = (x1, x2) R2.




Suppose y = h(x) = (x1, x2) R2. Define new coordinates as follows

s = pi(x) = arg min||h(x) yd()||

w1 = (h(x))

with such that w1 = 0 for y = yd(s)






w1 = (h(x))

w2 = x3 x3d(s)...

wn1 = xn xnd(s)






w1 = (h(x))

w2 = x3 x3d(s)...

wn1 = xn xnd(s)

Remark States on the maneuver, x = xd(s), are mapped to wi = 0.






w1 = (h(x))

w2 = x3 x3d(s)...

wn1 = xn xnd(s)

Remark s longitudinal coordinate, wi transverse coordinates.


Longitudinal and transverse coordinates for the PVTOL

State-space PVTOL dynamics in original coordinates

x = vx

z = vz

vx = uf sin + u cos vz = uf cos + u sin g =

= u




x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

[xz

]=

[xd(s)zd(s)

]+

[cos d(s) sin d(s)sin d(s) cos d(s)

] [0w1

]




x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

w2 = vx vxd(s)...

w5 = d(s)


Dynamics of the PVTOL in the new coordinates


x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X




x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

[xz

]=

[xd(s)zd(s)

]+


] [0w1

]




x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

Differentiating with respect to time[xz

]=

[x d(s)z d(s)

]s +

[ cos d(s) sin d(s)

]d(s)sw1 +

[ sin d(s)cos d(s)

]w1




x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

[vxvz

]=

[cos d(s)sin d(s)

](1 d(s)w1)s +

[ sin d(s)cos d(s)

]w1




x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

[vxvz

]=


] [(1 d(s)w1)s

w1

]




x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

[(1 d(s)w1)s

w1

]=

[cos d(s) sin d(s) sin d(s) cos d(s)

] [vxvz

]




x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

s =vx cos d(s) + vz sin d(s)

(1 d(s)w1)w1 = vx sin d(s) + vz cos d(s)




x = vx

z = vz


= u

w1

d(s)

(x, z)

(xd(s), zd(s))

(x0d(s), z0d(s))

sZ

X

w2 = vx v xd(s)s...

w5 = u d(s)s


Transverse form of the dynamics

Let u = ud(s) + uw , so that uw = 0 on the maneuver.




The dynamics in the (longitudinal and transverse) coordinates can bewritten as

s = Vd(s) + f1(s,w , uw )

w = A(s)w + B(s)uw + f2(s,w , uw )

with f1(s, 0, 0) = 0 and f2(s,w , uw ) higher order in (w , uw ).




The dynamics in the (longitudinal and transverse) coordinates can bewritten as

s = Vd(s) + f1(s,w , uw )

w = A(s)w + B(s)uw + f2(s,w , uw )

with f1(s, 0, 0) = 0 and f2(s,w , uw ) higher order in (w , uw ).

We can write the dynamics of the transverse coordinates using s asindependent variable instead of t

d

dsw = AT (s)w + BT (s)uw + fT (s, w , uw )

with AT (s) =1

Vd (s)A(s) and BT (s) =

1Vd (s)

B(s)


Maneuver regulation via transverse linearization

Transverse linearization

d

dsw = AT (s)w + BT (s)uw




d


State-feedback to exponentially stabilize the space-varying linearization

uw = K (s)w




d



uw = K (s)w

The feedback K () can be designed by solving an LQR problem

min1

2

S0

w(s)TQw(s) + uw (s)TRuw (s)ds +

1

2w(S)TPf w(S)

subj. tod

dsw = AT (s)w + BT (s)uw w(0) = w0




d



uw = K (s)w

Maneuver regulation controller

u = ud(s) + K (s)w(s)

= ud(pi(x)) + K (pi(x))W (x)

where x 7W (x) = w




d



uw = K (s)w

Maneuver regulation controller

u = ud(s) + K (s)w(s)

= ud(pi(x)) + K (pi(x))W (x)

IMPORTANT: nonlinear feedback!


Maneuver regulation of the full quadrotor model

0.50

0.51

1.50.5

1

1.5

4

3.5

3

2.5

2

1.5

n(s)

b(s)

s

w1D

Qw2

t(s)

Longitudinal coordinate

s := arg minRp p()2.

Transverse coordinates

w1 := n(s)T (p p(s)),w2 := b(s)T (p p(s)),

wi := xi+1 xi+1d(s), i = 3, ..., 11.Spedicato et al.. Aggressive maneuver regulation of a quadrotor UAV. International

Symposium on Robotics Research, 2013.


Some references

Hauser et al. Nonlinear control of slightly non-minimum phase systems:application to V/STOL aircraft. Automatica 1992.

Hauser, Hindman. Maneuver regulation from trajectory tracking: feedbacklinearizable systems. NOLCOS 1995.

Nielsen, Maggiore. Output Stabilization and Maneuver Regulation: Ageometric approach. Systems and Control Letters 2006.

Akhtar, Waslander, Nielsen. Path following for a quadrotor using dynamicextension and transverse feedback linearization. IEEE CDC 2012.

Saccon, Hauser, Beghi. A Virtual Rider for Motorcycles: ManeuverRegulation of a Multi-Body Vehicle Model. IEEE TCST, 2013.

Spedicato et al.. Aggressive maneuver regulation of a quadrotor UAV.International Symposium on Robotics Research, 2013.


Date post:	11-Sep-2015
Category:	Documents
Upload:	marco
View:	221 times
Download:	0 times

Lecture_traj Tracking Maneuver Regulation

Documents