Module 4 Slides

School of Electrical and Computer Engineering

Control of Mobile Robots

Dr. Magnus Egerstedt Professor School of Electrical and Computer Engineering

Module 4 Control Design

How make mobile robots move in effective, safe, predictable, and collaborative ways using modern control theory?

•  Given a linear system

•  The dilemma: We seem to need x but all we have is y •  Game plan:

–  Design u as if we had x –  Figure out x from y

•  Step 1: State feedback using “pole placement”

Lecture 4.1 – Stabilizing the Point Mass

Magnus Egerstedt, Control of Mobile Robots, Georgia Ins<tute of Technology 4.1.1

x = Ax+Bu, y = Cx

•  State Feedback: •  How pick the control gains? •  Idea: Pick them in order to make the closed-loop system have

the desired eigenvalues (poles)

Back to the Point Mass


u

p

x = Ax+Bu, A =

0 10 0

, B =

01

u = −Kx = −k1 k2

x

•  Given a matrix M, its eigenvalues satisfy the characteristic equation

Computing Eigenvalues


χM (λ) = det(λI −M) = 0

M =

m1 m2

m3 m4

λI −M = λ

1 00 1

−M =

λ−m1 −m2

−m3 λ−m4




χM (λ) = det(λI −M) = 0

det(λI −M) =

λ−m1 −m2

−m3 λ−m4

= (λ−m1)(λ−m4)−m2m3

= λ2 − (m1 +m4)λ+m1m4 −m2m3 = 0


•  But this is really annoying – is there an easier way?



χM (λ) = det(λI −M) = 0

λ =m1 +m4

2±

(m1 +m4)2

4−m1m4 +m2m3

χM (λ) = λ2 − (m1 +m2)λ+m1m4 −m2m3 = 0

•  The fundamental theorem of algebra: The roots in a polynomial are completely determined by the coefficients

•  Let’s stop here!

Almost Computing Eigenvalues


χM (λ) = λ2 − (m1 +m2)λ+m1m4 −m2m3 = 0

Back to the Point-Mass, Again


u = −Kx ⇒ x = (A−BK)x

A−BK =

0 10 0

−

01

k1 k2

=

0 1

−k1 −k2

χA−BK(λ) =

λ −1k1 λ+ k2

= λ2 + λk2 + k1

•  Let’s pick the eigenvalues that we would like the closed-loop system to have

•  If these were indeed the eigenvalues, then the characteristic polynomial would be

•  For the robot, let’s pick both eigenvalues at -1

•  Now we just line up the coefficients!

Desired Eigenvalues


λ1, . . . ,λn

ϕ(λ) = (λ− λ1)(λ− λ2) · · · (λ− λn) =n

i=1

(λ− λi)

ϕ(λ) = (λ+ 1)(λ+ 1) = λ2 + 2λ+ 1

Lining Up the Coefficients


ϕ(λ) = (λ+ 1)(λ+ 1) = λ2 + 2λ+ 1

χA−BK = λ2 + k2λ+ k1

λ1 : k2 = 2λ0 : k1 = 1 K =

1 2

x = Ax+Bu

u = −Kx

xu

•  Pick the control gains such that the eigenvalues (poles) of the closed loop system match the desired eigenvalues

Lecture 4.2 – Pole Placement


χA−BK(λ) = λn + an−1λn−1 + . . .+ a1λ+ a0

ϕ(λ) =n

i=1

(λ− λi) = λn + bn−1λn−1 + . . .+ b1λ+ b0

λn−1 : an−1(K) = bn−1...λ0 : a0(K) = b0

•  Is this always possible?

•  How should we pick the eigenvalues?

•  Do we have to compute large determinants?

Questions


No!

Mix of art and science

In MATLAB:

>> P=[lambda1,lambda2,lambda3,…];!>> K=place(A,B,P);!

No!

Example


u = −Kx =k1 k2

x ⇒ A−BK =

2− k1 −k21− k1 1− k2

x =

2 01 1

x+

11

u

χA−BK(λ) =

λ− 2 + k1 k2−1 + k1 λ− 1 + k2

= λ2 + λ(−3 + k1 + k2) + 2− k1 − k2

•  What’s at play here is a lack of “controllability”, i.e., the effect of the input is not sufficiently rich to influence the system enough

•  More on controllability later…

Example


χA−BK(λ) = λ2 + λ(−3 + k1 + k2) + 2− k1 − k2

ϕ(λ) = (λ+ 1)2 = λ2 + 2λ+ 1

λ1 : − 3 + k1 + k2 = 2 ⇒ k1 + k2 = 5

λ0 : 2− k1 − k2 = 1 ⇒ k1 + k2 = 1

???

•  Assuming that we have sufficient control authority to do pole placement, how should the desired eigenvalues be selected?

•  No clear-cut answer •  Some things to keep in mind:

How to Pick Eigenvalues?


Re(λi) < 0, ∀λi

Im(λi) = 0 ⇒(stability)

oscillations

(complex-conjugate pairs)

Rate of convergence

Im(λi) = 0 ⇒ ∃λj : Im(λj) = −Im(λi), Re(λj) = Re(λi)

•  The “smallest” eigenvalue dominates the convergence rate

•  But, the bigger the eigenvalues the bigger the control gains/signals

Rate of Convergence


x = (A−BK)x, Re(λ) < 0, ∀λ ∈ eig(A−BK)

λmin = argminλ|Re(λ)|!min

Re

Im

In MATLAB


% System matrices!A=[2,0;1,-1]; B=[1;1];!

% Let's pick our favorite poles!P=[-0.5+1i,-0.5-1i];!

% Pole placement!K=place(A,B,P);!

% Compute the solution!x=[1;1]; t=0; tf=5; dt=0.01; !

while (t<tf);! x=x+dt.*(A-B*K)*x;! t=t+dt;!end; !

Lecture 4.3 – Controllability


•  When can we place the eigenvalues however we want using state feedback?

•  When is the B matrix (the actuator configuration) rich enough so that we can make the system do whatever we want it to do?

•  The answer revolves around the concept of controllability!

•  Given a discrete-time system

•  We would like to drive this system in n steps to a particular target state

•  Is this possible?

A Modest Example


xk+1 = Axk +Buk, x0 = 0

x

?

0

x

A Modest Example


?

0

x x1 = Ax0 +Bu0 = Bu0

x2 = Ax1 +Bu1 = ABu0 +Bu1

x3 = Ax2 +Bu2 = A2Bu0 +ABu1 +Bu2...

We want to solve

x =B AB · · · An−1B

un−1...u1

u0

xn = An−1Bu0 + . . .+Bun−1

A Modest Example


?

0

x

x =B AB · · · An−1B

un−1...u1

u0

Γ

•  This is possible for any target state if and only if

•  Turns out, this generalizes quite nicely…

rank(Γ) = n

(n×mn)

Controllability – Theorem 1


x = Ax+Bu, x ∈ n

Γ =B AB · · · An−1B

Definition: The system is completely controllable (CC) if it is possible to go from any initial state to any final state.

CC Theorem 1: The system is CC if and only if rank(Γ) = n

controllability matrix

rank(M)=# linearly independent rows or columns in M

Two Systems


x =

2 01 1

x+

11

u

x =

0 10 0

+

01

u

n = 2

n = 2

Γ = [B AB]

Γ = [B AB]

AB =

22

AB =

10

Γ =

1 21 2

Γ =

0 11 0

rank(Γ) = 1

rank(Γ) = 2

pole-placement not possible

pole-placement possible

not CC!

CC!

Controllability – Theorem 2


CC Theorem 2: Pole-placement to arbitrary eigenvalues is possible if and only if the system is CC!

u = −Kx, x = (A−BK)x

Point-to-Point vs. Trajectories


x =

0 10 0

+

01

u

In MATLAB: >> G=ctrb(A,B);!>> rank(G)! ans: 2!

x0x

x1

x2

•  Let’s unleash our newfound powers on a complicated robotic system

Lecture 4.4 – The Segway Robot


Unicycle + Inverted Pendulum + …


The Base: x1 = v cosψx2 = v sinψψ = ω

φ, φThe “Pendulum”:

Inputs: τL, τR

Extra states: v, ω

Unicycle + Inverted Pendulum + …


x = [x1 x2 v ψ ψ φ φ]T

u = [τL τR]T

x = f(x, u)

= [v cosψ v sinψ v ψ ψ φ φ]T

3(mw +mb)v −mbd cosφφ+mbd sinφ(φ2 + ψ2) = − 1

R(τL + τR)

((3L2 +1

2R2)mw +mbd

2 sin2 φ+ I2)ψ +mbd2 sinφ cosφψφ =

L

R(τL − τR)

mbd cosφv + (−mbd2 − I3)φ+mbd

2 sinφ cosφφ2 +mbgd sinφ = τL + τR

LINEARIZE!

•  Linearize this mess around (x,u)=(0,0), and plug in the physical parameters for the segway robot:

Linearization

4.4.4 Magnus Egerstedt, Control of Mobile Robots, Georgia Ins<tute of Technology

x = Ax+Bu

A =

0 0 1 0 0 0 00 0 0 0 0 0 00 0 0 0 0 2.16 00 0 0 0 1 0 00 0 0 0 0 0 00 0 0 0 0 0 10 0 0 0 0 72.49 0

B =

0 00 0

−1.67 −1.670 0

0.029 −0.0290 0

−24.15 −24.1514

Controllability?


>> rank(ctrb(A,B))!

ans =!

6!

Not CC!

•  We already know that the unicycle dynamics gets messed up when linearized.

•  And if we can control v and ω, that should be “enough”, so let’s simply remove the unicycle part

A Smaller System


x = [v ω φ φ]T , u = [τL τR]T

x = Ax+Bu

A =

0 0 2.16 00 0 0 00 0 0 10 0 72.49 0

B =

−1.67 −1.670.029 −0.0290 0

−24.15 −24.15

>> rank(ctrb(A,B))!

ans =!

4!

CC!

•  We do not want to stabilize to (v,ω)=(0,0) •  Let

•  The dynamics become

•  Luckily,

One Last Twist


x = x− [vd ωd 0 0]T = x− δ

˙x = x− δ = x = Ax+Bu = A(x− δ) +Bu+Aδ

Aδ = 0 ⇒ ˙x = Ax+Bu

•  We have a CC system that we wish to stabilize •  After some tweaking, the following eigenvalues seem to

behave well:

Pole-Placement


λ1 = −19, λ2 = −7.5, λ3 = −5, λ4 = −0.3

u = −Kx ⇒ ˙x = (A−BK)x

>> P=[-19,-7.5,-5,-0.3];!

>> K=place(A,B,P);!

Now, Let’s Do It!


κ =ω

vcurvature rather than pose

v ↑

v ↓

ω ↓ ω ↑

•  We now know how to design rather effective controllers using state feedback.

•  But what about y???

Lecture 4.5 – Observers


x = Ax+Buy = Cx

u y

?





x = Ax+Buy = Cx

u y

x





x = Ax+Buy = Cx

u y

xobserver

The Predictor-Corrector


x = Axy = Cx

First idea: Make a copy of the system ˙x = Ax predictor

Second idea: Add a notion of how wrong your estimate is to the model

˙x = Ax+ L(y − Cx)corrector

“Luenberger” observer

Picking the Observer Gain


•  What we want to stabilize (drive to zero) is the estimation error, i.e., the difference between the actual state and the estimated state

•  Just pick L such that the eigenvalues to A-LC have negative real part!!

e = x− x

e = x− ˙x = Ax−Ax− L(y − Cx)Cx

e = A(x− x)− LC(x− x) = (A− LC)e

Pole-Placement, Again


•  Want

•  We already know how to do this - Pole-placement!

e = (A− LC)e

Re(eig(A− LC)) < 0

Example


x =

−1 20 −2

x, y =

1 −1

x

e = (A− LC)e =

−1 20 −2

−

L1

L2

1 −1

e

=

−1− L1 2 + L1

−L2 −2 + L2

e

Example


e =

−1− L1 2 + L1

−L2 −2 + L2

e

χA−LC(λ) =

λ+ 1 + L1 −2− L1

L2 λ+ 2− L2

= λ2 + λ(3 + L1 − L2) + 2 + 2L1 + L2

ϕ(λ) = (λ+ 1)2 = λ2 + 2λ+ 1

L1 = −2/3L2 = 1/3

Example


˙x = Ax+1

3

−21

(y − Cx)

0 1 2 3 4 5 6 7 80.6

0.4

0.2

0

0.2

0.4

0.6

0.8

1

1.2

0 1 2 3 4 5 6 7 80

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

The Block-Diagram


x = Axy = Cx

y

x ˙x = Ax+ L(y − Cx)

Does This Always Work?


•  No!

•  Next Lecture: We need to redo what we did for control design (controllability) for observer design (observability)

•  Need to redo what we did for control design to understand when we can recover the state from the output

Lecture 4.6 – Observability


x = Axy = Cx

y

x ˙x = Ax+ L(y − Cx)

•  Given a discrete time system without inputs

•  Can we recover the initial condition by collecting n output values?

A Modest Example (Again)


xk+1 = Axk

yk = Cxk

y0 = Cx0

y1 = Cx1 = CAx0

yn−1 = CAn−1x0

...

•  The initial condition can be recovered from the outputs when the so-called observability matrix has full rank.

The Observability Matrix


y0y1y2...

yn−1

=

CCACA2

...CAn−1

x0

Ω

Observability – Theorem 1


Definition: The system is completely observable (CO) if it is possible to recover the initial state from the output.

CO Theorem 1: The system is CO if and only if

observability matrix

rank(M)=# linearly independent rows or columns in M

x = Ax, x ∈ n

y = Cx

Ω =

CCA...

CAn−1

rank(Ω) = n

Observability – Theorem 2


CO Theorem 2: Pole-placement to arbitrary eigenvalues is possible if and only if the system is CO!

˙x = Ax+ L(y − Cx), e = (A− LC)e

Putting It All Together


•  We are very close to being able to design the controllers as if we had x while all we really have is y (and hence an estimate of x).

•  Next lecture – Putting it all together!

•  We have lots of really good building blocks: –  Controllability –  Observability –  State feedback –  Observers –  Pole-placement

•  Now, how do we put everything together? •  Answer: The wonderful Separation Principle

Lecture 4.7 – The Separation Principle


A Game Plan


Step 1: Design the state feedback controller as if we had x (which we don’t)

x = Ax+Buy = Cx Assume CC and CO

u = −Kx ⇒ x = (A−BK)x

u = −Kx

what we design for

what we actually have

Step 2: Estimate x using an observer (that now also contains u)

˙x = Ax+Bu+ L(y − Cx)

⇒ e = (A− LC)e

Does This Work?


•  Want both x and e to be stabilized •  Analyze their joint dynamics:

•  Or, when viewed together

•  The strategy works if and only if this is an asymptotically stable system!

x = Ax−BKxe = x− x

= Ax−BK(x− e) = (A−BK)x+BKe

e = (A− LC)e

xe

=

A−BK BK

0 A− LC

xe

The Separation Principle


•  This is an (upper) triangular block-matrix. Its eigenvalues are given by the eigenvalues of the diagonal blocks!

•  If we haven’t messed up in the design, we have

•  Everything works!!

xe

=

A−BK BK

0 A− LC

xe

M

χM (λ) = χA−BK(λ)χA−LC(λ)

Re(eig(A−BK)) < 0, Re(eig(A− LC)) < 0

The Separation Principle


•  This remarkable fact is known as the Separation Principle! •  Means that we can design controllers as if we have x •  Can design observers independent of the control actions •  Control and observer designs can be separated!

The Final Block-Diagram


y

x

x = Ax+Buy = Cx

u

˙x = Ax+Bu+ L(y − Cx)

u = −Kx

•  The separation principle means that we can decouple control and observer design (in theory)

Lecture 4.8 – Practical Considerations


“In theory, theory and practice are the same. In practice they are not.” -Yogi Berra

•  Typically, the observer should be faster than the controller, since the controller won’t do anything useful until the state estimate is close.

•  Note: Large observer eigenvalues give large observer gains: –  Not a problem – The observer is a software construct!

Eigenvalue Selection


Eigenvalue Selection


controller

observer

Re(λ)

Im(λ)

Humanoid Robot


moment of inertia

torque constant

current friction coefficient

θ =1

J(Ki− bθ)

x =

0 10 −b/J

x+

0

K/J

u

y =1 0

x

Aldebaran Nao CC & CO!!

Reference Tracking


e =

θ − θd

θ

x = Ax+Bu

e = Ax+Bu = Ae+A

θd0

+Bu = Ae+Bu

y = Cx

y = Cx = Ce+ C

θd0

= Ce+ θd

u = −Ke

˙e = Ae+Bu+ L(y − Ce− θd)

•  The design structure is quite general (does not need to be based on pole placement)

•  LQ Optimal Control:

Beyond Pole Placement


u = −Kx ˙x = Ax+Bu+ L(y − cx)

K = R−1BTP

0 = ATP + PA+Q− PBR−1BTP

min

∞

0(xTQx+ uTRu)dt

•  The design structure is quite general (does not need to be based on pole placement)

•  The Kalman Filter:

Beyond Pole Placement


u = −Kx ˙x = Ax+Bu+ L(y − cx)

x = Ax+Bu+DIvIy = Cx+DOvO

min limt→∞

E(x(t)− x(t))2

L = PCT (DODT

O)−1

0 = ATP + PA+DIDT

I − PCT (DODT

O)−1CP

•  But, the real world (especially for robots) is more complex than a simple LTI system

•  HYBRID SYSTEMS!

Next Module


Date post:	11-Feb-2016
Category:	Documents
Upload:	yashar2500
View:	40 times
Download:	4 times

Module 4 Slides

Documents