Control Design via. Feedback. Linearization. Harry G. Kwatny. Department of
Mechanical Engineering & Mechanics. Drexel University ...
Control Design via Feedback Linearization Harry G. Kwatny Department of Mechanical Engineering & Mechanics Drexel University
Outline
Definitions & Notation
Input-output feedback linearization
Lie Derivative, Exponential Map,Lie Bracket SISO, MIMO Relative degree & zero dynamics
Exact (state) feedback linearization Dynamic extension Regulation of feedback linearizable systems
Differential Map dφ dt
F dφ dt
F(p) p
an arbitray curve φ ( t ) on M passing through point p
φ (t )
φ (t ) = F (φ (t ))
maps into φ ( t ) on N passing through point F ( p ) R
Given the map F : M → N , the differential map is the induced mapping F* : TM p → TN F ( p ) that takes tangent vectors into tangent vectors.
Analysis using Local Coordinates F dφ dt
dφ dt p U
φ (t )
φ (t )
F(p)
N
ϕN
M
ϕM
dx dt
dx dt V Rm
W
x (t )
x (t )
Rn F
Lie Derivative Definition: Let v(x) denote a vector field on an mdimensional manifold M and F(x) a mapping from M to an n-dimensional manifold N, both in local coordinates. Then the Lie derivative of order 0,…,k is k −1 ∂ L v , Lkv ( F ) = L0v ( F ) F= v ∂x
Example: F ( x) = Bx, B ∈ R m×n , v ( x ) = Ax, A ∈ R m×m = L0v ( F ) Bx, = L1v ( F )
∂Bx = Ax BAx ∂x
∂BAx ∂BA2 x 2 3 = = L (F ) = Ax BA x, = Lv ( F ) Ax BA3 x ∂x ∂x 2 v
Flow Definition: Let v be a smooth vector field on M and denote the parameterized maximal integral curve through p∈M by Ψ(t,p) and Ψ(0,p)=p. Ψ(t,p) is called the flow generated by v. Properties of flows: • satisfies ode
d Ψ ( t , p ) =Ψ v ( ( t , p ) ) , Ψ ( 0, p ) = p dt
(
)
Ψ ( t1 + t2 , p ) • semigroup property Ψ t2 , Ψ ( t1 , p ) =
Exponential Map We will adopt the notation
e p := Ψ ( t , p ) tv
The motivation for this is that the flow satisfies the three basic properties ordinarily associated with exponentiation – from properties of Ψ(t,p).
e0⋅v p = p
(
boundary condition
)
d tv e p = v e tv p dt ( t1 + t2 ) v t2 v t1 v e p=e e p
differential equation semi-group property
Series Expansion Along Trajectory Suppose x ( t ) = satisfies x v= ( x ) , x ( 0 ) x0 . Let F : R m → R p . 2 2 d d 1 F ( x (t )) = F ( x0 ) + F ( x ( t ) ) t + 2 F ( x ( t ) ) t + 2 dt dt t =0 t =0
∂F d 1 = F= x t v x L ( ) v ( F ( x )) ( )) ( ∂x dt ∂ ∂F d2 2 = F x t v x v x L ( ) ( ) v ( F ( x )) ( )) ( 2 ∂x ∂x dt 1 2 1 F ( x (t )) = F ( x0 ) + Lv ( F ( x0 ) ) t + Lv ( F ( x0 ) ) t 2 + 2
Series Representation of Exp Map For F a scalar or vector, we can derive the Taylor expansion of F(x(t)) about t=0 ∞
k t F etv x = ∑ Lkv ( F ( x ) ) k =0 k !
(
)
Choose F(x)=x (in words, and identity map), to obtain ∞
k t etv x = ∑ Lkv ( x ) k =0 k !
Example: scalar linear fields dx v ( x) = 1 ⇒ = 1 ⇒ Ψ (t , x) = x + t dt 1 2 2 0 1 Ψ (t , x) = x t Lv ( x ) + tLv ( x ) + t Lv ( x ) + =+ 2 dx v ( x ) = x ⇒ = x ⇒ Ψ (t , x) = et x dt ∞ ∞ tk k tk Ψ (t , x)= ∑ Lv ( x )= ∑ x= et x k! = k 0= k 0 k!
Example: general linear field v ( x ) = Ax L ( x ) == Ax 1 v
= L ( x ) L= ( Ax ) A x 2 v
1 v
2
= L ( x ) L= ( A x) A x 1 v
k v
k −1
k
k ∞ t k t k tv = = = e x ∑ Lv ( x ) ∑ A x e At x k! k 0= = k 0 k! ∞
k
Example: Affine Field v ( x= ) Ax + b L1v ( x= ) Ax + b
Lkv ( x ) = L1v ( Ak −1 x + Ak − 2 b ) = Ak x + Ak −1b k k k ∞ ∞ t t t tv k k k −1 At At −1 e x= L x = A x + A b = e x + e A b ( ) ∑ ∑ ∑ v k! = k 0 k! = k 0 k! k 0= ∞
Examples, Cont’d − x 3 ⇔ x = − x3 v ( x) = Exact solution: x (t ) =
x0 1 + 2 x0 2t
=(1 − x02t + 128 x04t 2 − 208 x06t 3 + 358 x08t 4 − 638 x010t 5 + ) x0
via exponential map: x ( t ) =e
− x3
∂ ∂x
x0 =(1 − x02t + 128 x04t 2 − 208 x06t 3 + 358 x08t 4 − 638 x010t 5 + ) x0
Lie Bracket Definition: If v,w are vector fields on M, then their Lie bracket [v,w] is the unique vector field defined in local coordinates by the formula
Property:
∂w ∂v [v, w] = v− w ∂x ∂x dw ( Ψ ( t , x ) ) dt
= [ v, w] x t =0
The rate of change of w along the flow of v
Lie Bracket Interpretation Let us consider the Lie bracket as a commutator of flows. Beginning at point x in M follow the flow generated by v for an infinitesimal time which we take as ε for convenience. This takes us to point
y = exp( ε v ) x Then follow w for the same length of time, then -v, then -w. This brings us to a point ψ given by
ψ (ε , x ) = e
− εw
e
− εv
e
εw
e
εv
x
Lie Bracket Interpretation Continued u
z -v -w
w y
ψ (ε , x )
[v,w] v x
(
)
d ψ 0 + , x = [ v, w] x dε
Iterated Lie Bracket We recursively define higher order Lie Brackets:
ad ( w) = w 0 v
k −1 ad = v, ad v ( w) k v
Setup x f ( x) + G ( x)u = y = h( x ) x ∈ Rn , u ∈ Rm , y ∈ R p f ( x), G ( x) = [ g1 ( x) g m ( x) ] , h( x) smooth
I-O Linearization: SISO case x f ( x) + g ( x)u = y = h( x ) ∂h y = x L f h( x) + Lg h( x)u = ∂x if Lg h( x) ≠ 0 stop, else y L2f h( x) + Lg L f h( x)u = if Lg Lkf−1h( x) ≠ 0 stop, else continue until y ( r ) Lrf h( x) + Lg Lrf−1h( x)u with Lg Lrf−1h( x) ≠ 0 = u set=
1 r −1 f
Lg L
( h( x )
)
v − Lrf h( x) + v ⇒ y ( r ) =
Relative Degree Definition: Let U be a neighborhood of x0 and suppose there is a finite integer r such that Lg Lkf h( x) = 0, ∀x ∈ U , k = 0, , r − 2, Lg Lrf−1h( x) ≠ 0
Then r is the relative degree. If the sequence does not terminate in finite steps, the system relative degree is
r= ∞
State Transformation •
r finite ⇒ r ≤ n
i −1 = zi ( x) : L= define: 1, , r f h( x ), i
•
zi ( x), i = 1, , r are independent functions
note: , z2 y , , zr = y ( r −1) = z1 y= • complete the transformation by adding functions define : ξi= ( x), i 1, , n − r independent of z ( x) ⇓ x (ξ , z )
Normal Form ξ = F (ξ , z , u ) = F (ξ , z ) special choice of ξ z = Az + b α ( x(ξ , z ) ) + ρ ( x(ξ , z ) ) u y = cz 0 1 0 0 0 0 1 0 , b = = A = , c [1 0 0] 1 0 0 1 0 0 α ( x) = Lrf h( x), ρ ( x) = Lg Lrf−1h( x)
Feedback Linearization & Zero Dynamics − ρ −1 ( x) {α ( x) + v} u= ⇓
ξ = F (ξ , z ) internal dynamics z Az + bv = I-O linearized dynamics y = cz
ξ = F (ξ , 0) zero dynamics
Chain of Integrators
Example 1
Design an I/O linearizing control system with y → 0 x1 = x2 + x13 + u x2 = −u y = x1 y = x1 = x2 + x13 + u ⇒r= 1, α ( x ) =x2 + x13 , β ( x ) = 1, u =− x2 − x13 + v z1 ( x ) = x1 we need one more function to complete the transformation
choose ξ1 ( x ) = x2 z1 = v
I/O linearized dynamics
ξ1 =−u =x2 + x13 − v ⇒ ξ1 =ξ1 + z13 − v ξ1 = ξ1
zero dynamics ( unstable!)
internal dynamics
Example 1, Cont’d What can be done in the case of unstable zero dynamics? The conventional way is to try and redefine the output function. Two ways of approaching this are: • Try to find a new h ( x ) such that the zero dynamics are stable. • Try to find a new h ( x ) such that the system is full state linearizable, i.e., r = n. y= h ( x= ) x1 + x2
y =x2 + x13 , y =3 x12 ( x2 + x13 ) + ( 3 x12 − 1) u ⇒ r= 2, α= 3 x12 ( x2 + x13 ) , β= problem if x1 = ± 1 3
( 3x
2 1
− 1)
Example x1 x1 x2 − x13 0 x x 2 2 + x 2 3 1 + = u, x3 − x3 1 2 x 0 x x + 4 1 2 z1 z 2 = z3 z4
x4 2 x 1 + x2 x1 1 2 2 − x2 + 2 x3 + x3
(
x1 x ⇔ 2 = x3 x4
)
y= x4
z3 2 z z − 2 3 −1 − 1 + z − z 2 − z 2 3 4 z1
Example, Cont’d Normal form: z2 0 2 4 2 4 z z z z + − 3 2 3 3 −2 1 + z2 − z32 − 2 z4 u + z3 z2 − 2 z32 0 −1 − z − z / 2 + z 2 − 1 + z − z 2 + 2 z − 2 z 0 2 3 3 2 3 4 4 Zero dynamics z1 z 2 z3 z4
(
)
3 2 z − z3 3 z = 2 2 4 −1 − z3 / 2 + z3 − 1 − z3 + 2 z4 − 2 z4
I-O Linearization: MIMO case
{
assume p = m
}
= ri inf k Lg j Lkf−1hi ( x) ≠ 0 for at least one j ri = α i ( x) L= 1, , m f hi ( x ), i ri = ρij L= L 1, , m g j f hi ( x ), i , j
Definition: if det { ρ ( x)} ≠ 0, r = [ r1 rm ] is the vector relative degree.
MIMO State Transformation state transformation , x → (ξ , z ), ξ ∈ R n − r , z ∈ R r , r =r1 + + rm y1 h1 z 1 r −1 r − 1 1 1 L f (h1 ) y1 = z ( x) = := hm y m z m − 1 − 1 r r L fm (hm ) y mm
ξ = F (ξ , z , u )
z = Az + E α ( x(ξ , z ) ) + ρ ( x(ξ , z ) ) u y = Cz
I-O Linearization Summary v
ρ ( x) −1
u
x = f ( x ) + G ( x )u
y
y = h( x )
-
x
α ( x)
Kz ( x )
ξ = F (ξ , z , v )
v
z = Az + Ev y = Cz
y
Computational Tools Function Name VectorRelativeOrder DecouplingMatrix IOLinearize NormalCoordinates LocalZeroDynamics
computes the relative degree vector computes the decoupling matrix, ρ (x) computes the linearizing control, u = ρ −1 (x){− α (x) + v} computes the partial state transformation, z(x) computes the local form of the zero dynamics F( ξ ,0) , near x0
Example: Simple Vehicle Assumptions: m = 0,= s 0 2
κ 2 2 − + a b ( ) ωθ + ( a − b ) vy − avxδ J v zz x δ 1 − m1ωθ v y − κ ( aωθ + v y ) m1 vx 1 ωθ vx − κ ( ( a − b ) ωθ + 2v y − vxδ ) m1vx κ − − ( a 2 + b 2 ) ωθ + ( a − b ) v y − avxδ J zz vx ωδ
(
ωθ v x d vy dt ωδ δ
(
)
)
− 1 J zz 0 + 0 1 + 1 I zz J zz 0
0 1 m1 T 0 F 0 0
Example, Cont’d Objective: Steer vehicle along a circular path of constant radius R and constant speed Vd . Equivalently, specify constant angular velocity ωd = Vd R and define two outputs y1 = vx2 + v y2 − Vd2 y= ωθ − ωd 2
Example, Cont’d
Example: Dynamic Extension x1 cos x 3 0 u1 x 2 = sin x 3 0 u 2 x 3 0 1 y1 x1 y = x 2 2 x1 u1 cos x3 0 x u sin x 0 3 1 define: u1 = v ⇒ 2 = + x3 0 0 u 0 1 1
cos x3 ρ= sin x3
0 0 v 1 u2 0
0 singular! 0
cos x3 −u1 sin x3 ρ= sin x u cos x 3 1 3 nonsingular!
Dynamic Extension Algorithm x = f ( x ) + ∑ i =1 gi ( x ) ui , m
y= h ( x)
1) Compute ρ ( x ) . If rankρ = m Stop! 2) If rankρ= s < m, perform elementary column operations to make the first s columns independent and the last m − s columns zero. ρ1 ( x ) = ρ ( x ) E ( x ) , E ( x ) square, nonsingular. 3) Suppose there are q columns each having two or more elements that are not identically zero. If q = 0, Stop! The process fails. 4) If q ≠ 0, define the column index set α = {i1 ,, iq } and let α denote its complement in {1, , m} . Put and integrator in series with the q corresponding controls to obtain a new augmented system f ( x ) + ∑ i∈α gi ( x ) ui ∑ i∈α gi ( x ) ui + v 0 α 5) Go to step 1 and repeat the process with the new augmented system.
x = uα
Example: Wheeled Robot x = vx cos θ y = vx sin θ θ = ω Mvx = F J ω = T
body fixed frame yb
vx
xb
θ
space frame y (x,y)
x
Wheeled Robot, 2 x = vx cos θ y = vx sin θ θ = ω y1 =− vx cos θ vx sin θ ω = −vxω sin θ + cos θ F vx F ⇒ = y2 = vx sin θ + vx cos θ ω = vxω cos θ + sin θ F ω = T y1 = x y2 = y −vxω sin θ cos θ 0 α = , ρ = sin θ 0 vxω cos θ ⇒ q= 1, α=
{1}
F ⇒ add integrator in front of u= 1
Wheeled Robot, 3 The extended system is: θ x d y = dt vx ω F
ω 0 v cos θ 0 x vx sin θ 0 + F 0 0 0 0 1
0 0 0 u 0 T 1 0
Wheeled Robot, 4 And, the normal form: z1 z 2 d z3 = dt z4 z5 z6
0 0 0 0 0 0
1 0 0 0 0 0
0 1 0 0 0 0
0 0 0 0 0 0
0 0 0 1 0 0
0 z1 0 0 z2 0 0 z3 1 + 0 z4 0 1 z5 0 0 z6 0
0 0 0 [α + ρ u ] 0 0 1
− zω sin θ + ω ( −vxω cos θ − z sin θ ) cos θ = ρ , sin θ + − + ω θ ω ω θ θ z v z cos sin cos ( ) x
α =
−vx sin θ vx cos θ
Wheeled Robot, 5
Feedback linearizable
Regulation of Feedback Linearizable Systems - 1 w disturbance e performance variables
Plant
Controller
y measurements
u control
x f ( x, w ) + G ( x ) u state = w = ϕ ( w ) disturbance e = he ( x, w ) y = hy ( x , w )
Assumptions: equilibrium point at the origin, i.e.
hy ( 0, 0 ) = 0 f= error ( 0, 0 ) , 0 h= z ( 0, 0 ) , 0 measurement disturbance dynamics are neutrally stable
Goals: 1. stabilize the plant (internal stability) 2. regulate the performance variables: lim e ( t ) = 0 t →∞
Regulation of Feedback Linearizable Systems - 2 Apply standard reduction to augmented system
( x, w ) → ( ξ , z ) ξ = F (ξ , w, z , u ) z = A0 z + E0 α ( x, w ) + ρ ( x, w ) u e = C0 z
( x, w ) {−α ( x, w ) + v} take v Kz ( x, w ) so that Re λ ( A0 + E0 K ) < 0 0 ⇒ limt →∞ e ( t ) = 0 ⇒ limt →∞ z ( t ) =
choose: u= ρ
−1
Regulation of Feedback Linearizable Systems - 3 w = ϕ ( w )
w
v
= u x f ( x, w) + G ( x)u ρ ( x, w) e = h( x, w) −1
-
e
x
α ( x, w)
Kz ( x, w)
ξ = F (ξ , w, z , v) v
= z A0 z + E0 v e = C0 z
e
Example: SISO Linear Systems x = Ax + Ew + bu w = Zw = z cx + fw z = cAx += cEw + fZw + cb u
z
2
A E x f ] + cAb u 0 Z w
[c
z (r )
[c
A E x f ] + cb u 0 Z w
[c
First u appearance
r
A E x r −1 + f ] cA bu 0 Z w
1 = ⇒ ρ cAr −= b, α
[c
r
A E x f ] w Z 0
expand matrix
Example, Continued x cE ( Ar −1 + Ar − 2 Z + + AZ r − 2 + Z r −1 ) w
= α cAr
ρ = cAr −1b 1 = − cAr u r −1 cA b z1 = [ c
cE ( A
r −1
+A
r −2
Z + + AZ
r −2
+Z
x f ] w
x z2 = [ cA cE + fZ ] w x zr = cAr −1 cE ( Ar − 2 + Ar −3 Z + Z r − 2 ) + fZ r −1 w
r −1
x ) w + v
new z coordinates
Tracking: A Special Case x =+ f ( x ) G ( x ) u,
y= h ( x)
, y Fw w Zw = = e = y − y = h ( x) − y
= e h ( x) − y
∂h e= x − y= L f h( x) − y + Lg h( x)u ∂x if Lg h( x) ≠ 0 stop, else e L2f h( x) − y + Lg L f h( x)u =
= e L f h ( x ) − y = e( r −1) Lrf−1h ( x ) − y ( r −1)
k −1 f
if Lg L h( x) ≠ 0 stop, else continue until ) e( r= Lrf h( x) − y ( r ) + Lg Lrf−1h( x)u with Lg Lrf−1h( x) ≠ 0
set u =
1 Lg Lrf−1
− L h( x ) + y ( h( x ) r f
(r )
v + v ) ⇒ e( r ) =
note that: y (k ) = F Z k w
Tracking Regulator: General Structure y r (t )
w = Zw y = Fw
z (t ) −
y (t ) y t ( ) z (t ) = r −1 y ( t )
v
ρ ( x) −1
K
-
α ( x)
z ( x)
u
x = f ( x ) + G ( x )u
y
y = h( x ) x
= z Az + Bv y = Cz
Example: Wheeled Robot Position Control x = vx cos θ y = vx sin θ θ = ω vx = F ω = T w1 = 0 w 2 = w3 w 3 = 0 y1= x − w1 y2= y − w2
Note: 0 = y1 w= 1 , y1 0 = y2 w= w= 2 , y2 3 , y2 Recall: = r1 3,= r2 3
Fall 09 Project t xc ( t ) = 50 cos 2π τ t yc ( t ) = 50sin 4π τ
Trajectory Actual blue Command red
y
40 20 40
20
20 20 40
40
x
Fall 09 Project
Trajectory Actual blue Command red
y
coord
40 20 40 2020 40
20
40
Trajectory Error X blue Y red
x
0.1 0.2 0.3 0.4 0.5
10 20 30 40 50
Trajectory Actual blue Command red
40
x
0.00005 0.00010 0.00015 0.00020
y
coord 2. 1011 1. 1011
20
40
x
1. 1011 2. 1011 3. 1011
t
100 150
10
20
30
40
50
t
10
0.2 0.4
20
1
40
60
80 100
t
0.05 0.10 0.15
Control Force
50
t
20
40
60
80 100
t
T
0.4 0.2 0.2 0.4
40
Control Torque
F
t
30
Control Torque
2
200
20
T 0.15 0.10 0.05
1
50
0.4 0.2
Control Force
Trajectory Error X blue Y red
40 20
Control Torque
T
F 2
20 40 60 80 100
Trajectory Actual blue Command red
40 2020 40
2 4
coord
40 20 20
t
Trajectory Error X blue Y red
y
40 2020 40
Control Force
F 8 6 4 2
50
100
150
200
t
0.03 0.02 0.01 0.01 0.02 0.03
50
100
150
200
t
