Module 3 Slides.pdf

School of Electrical and Computer Engineering

Control of Mobile Robots

Dr. Magnus Egerstedt Professor School of Electrical and Computer Engineering

Module 3 Linear Systems

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

•  So far we have seen –  Controls Basics –  Mobile Robots

•  Need to be a bit more systematic in our discussion •  Rich class of models:

–  General enough –  Simple enough –  Expressive enough –  Relevant enough

Lecture 3.1 – A Simple Robot

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

LINEAR SYSTEMS!!

•  Given a point mass on a line whose acceleration is directly controlled:

•  Want to write this on a compact/general form

“Controlling” a Point Mass


u

pp = u

x1 = px2 = p ⇒ x1 = x2

x2 = u

•  Store these variables in a single, 2-dimensional “state” variable

On State Space Form


x1 = x2

x2 = ux =

�x1

x2

�

x =

�x1

x2

�=

�x2

u

�=

�0 10 0

� �x1

x2

�+

�01

�u

y = p = x1 =�1 0

�x

x = Ax+Buy = Cx

•  Or, even more generally

On State Space Form


x =

�x1

x2

�=

�x2

u

�=

�0 10 0

� �x1

x2

�+

�01

�u

y = p = x1 =�1 0

�x

A =

�0 10 0

�, B =

�01

�, C =

�1 0

�

A 2D Point-Mass


p = (px, py)

u = (ux, uy)px = ux

py = uy

x1 = pxx2 = pxx3 = pyx4 = pyu1 = ux

u2 = uy

y1 = pxy2 = py

A =

0 1 0 00 0 0 00 0 0 10 0 0 0

B =

0 01 00 00 1

C =

�1 0 0 00 0 1 0

�

x = Ax+Buy = Cx

LTI Systems


x = Ax+Buy = Cx

•  This is a so-called LTI (Linear Time-Invariant) System on State-Space form!

x ∈ �n

u ∈ �m

y ∈ �p⇒

A : n× nB : n×mC : p× n

n× 1 (n× n)(n× 1)n× 1

(n×m)(m× 1)x = A x + B u

n× 1

y = C x(p× n)(n× 1)p× 1

p× 1

•  The general LTI model is –  state –  output –  input

•  Main Question: How should the input be selected? •  But First: How can such systems be understood? And where

do they come from?

Lecture 3.2 – State-Space Models


x = Ax+Buy = Cx

xyu

u y

•  Recall our old friend, the car model

•  If we care about/can measure the velocity:

•  If we care about/can measure the position we have the same general equation with different matrices:

Example 1: The Car Model


v =c

mu− γv

x = Ax+Buy = Cx,

A = −γ, B =c

m, C = 1

A =

�0 10 −γ

�, B =

�0

c/m

�, C =

�1 0

�

•  Newton’s 2nd tells us that

•  For small angles we get

Example 2: Pendulum


Not linear!

θ � 1 ⇒ sin θ ≈ θ

x = Ax+Bu, y = Cx

θ = −g

�sin(θ) + cu

A =

�0 1

−g/� 0

�, B =

�0c

�, C =

�1 0

�

�θ

•  Consider two “robots” on a line

•  The Rendezvous Problem: Have them meet at the same location

•  Idea: Have them aim towards each other:

Example 3: Two Simple “Robots”


x1

x1 = u1 x2 = u2

x2

x =

�1 00 1

�u

(A = 0)

�u1 = x2 − x1

u2 = x1 − x2x =

�−1 11 −1

�x

A closed-loop feedback law!

Rendezvous


Example 4: Unicycle Robot


Not linear! φ

(x, y)

x = v cosφy = v sinφφ = ω

≈ 1

≈ φ

x = vy = vφφ = ω

Still not linear!

•  We need to be more systematic/clever when it comes to generating LTI models from nonlinear systems!

Lecture 3.3 – Linearizations


“Classifying systems as linear and non-linear is like classifying objects in the Universe as bananas and non-bananas.” -unknown

•  Given a non-linear model

•  We want to find a “local”, linear model around an operating point

•  The new equations of motion become

Linearizations


x = f(x, u), y = h(x)

(xo, uo) → (x = xo + δx, u = uo + δu)

δx = x− xo = x = f(xo + δx, uo + δu)

Linearizations


= f(xo, uo) +∂f

∂x(xo, uo)δx+

∂f

∂u(xo, uo)δu+H.O.T

δx = f(xo + δx, uo + δu)

Assumptions:

f(xo, uo) = 0

A B

h(xo) = 0

C

y = h(xo + δx) = h(xo) +∂h

∂x(xo)δx+H.O.T

Taylor expansion

Linearizations


x = f(x, u)y = h(x)

f(xo, uo) = 0h(xo) = 0

δx = Aδx+Bδuy = Cδx

A = ∂f∂x (xo, uo)

B = ∂f∂u (xo, uo)

C = ∂h∂x (xo)

x = xo + δxu = uo + δu

Computing the Jacobians


x ∈ �n, u ∈ �m, y ∈ �p, f =

f1f2...fn

, h =

h1...

hm

∂f

∂x=

∂f1∂x1

∂f1∂x2

· · · ∂f1∂xn

∂f2∂x1

· · · ∂f2∂xn

.... . .

...∂fn∂x1

· · · ∂fn∂xn

n× n



x ∈ �n, u ∈ �m, y ∈ �p, f =

f1f2...fn

, h =

h1...

hm

∂f

∂x=

∂f1∂x1

∂f1∂x2

· · · ∂f1∂xn

∂f2∂x1

· · · ∂f2∂xn

.... . .

...∂fn∂x1

· · · ∂fn∂xn

∂f

∂u=

∂f1∂u1

· · · ∂f1∂um

∂f2∂u1

· · · ∂f2∂um

.... . .

...∂fn∂u1

· · · ∂fn∂um

n×m



x ∈ �n, u ∈ �m, y ∈ �p, f =

f1f2...fn

, h =

h1...

hm

∂f

∂x=

∂f1∂x1

∂f1∂x2

· · · ∂f1∂xn

∂f2∂x1

· · · ∂f2∂xn

.... . .

...∂fn∂x1

· · · ∂fn∂xn

∂f

∂u=

∂f1∂u1

· · · ∂f1∂um

∂f2∂u1

· · · ∂f2∂um

.... . .

...∂fn∂u1

· · · ∂fn∂um

∂h

∂x=

∂h1∂x1

· · · ∂h1∂xn

.... . .

...∂hp

∂x1· · · ∂hp

∂xn

p× n

Example: Inverted Pendulum


�

θ u

θ =g

�sin θ + u cos θ

x1 = θx2 = θy = x1

,f(x, u) =

�x2

g/� sin(x1) + u cos(x1)

�

h(x) = x1

(xo, uo) = (0, 0)

A =

�∂f1∂x1

∂f1∂x2

∂f2∂x1

∂f2∂x2

�

(0,0)

=

�0 1

g/� cos(x1) 0

�

(0,0)

=

�0 1g/� 0

�



�

θ u

θ =g


x1 = θx2 = θy = x1

,f(x, u) =

�x2


�

h(x) = x1

(xo, uo) = (0, 0)

B =

� ∂f1∂u∂f2∂u

�

(0,0)

=

�0

cos(x1)

�

(0,0)

=

�01

�



�

θ u

θ =g


x1 = θx2 = θy = x1

,f(x, u) =

�x2


�

h(x) = x1

(xo, uo) = (0, 0)

C =�

∂h∂x1

∂h∂x2

�(0,0)

=�1 0

�

Example: Unicycle


φ

(x, y)

x = v cosφy = v sinφφ = ω

x1 = x, x2 = y, x3 = φy1 = x1, y2 = x2, y3 = x3

u1 = v, u2 = ω(xo, uo) = (0, 0)

A = 0, B =

1 00 00 1

, C =

1 0 00 1 00 0 1

x2 = 0 ???

•  Sometimes the linearizations give reasonable models and sometimes they do not…

•  Despite the fact that they are only local approximations, they are remarkably useful (when they work…)

Punchlines


•  Let’s figure out how such systems behave. •  Start by ignoring the input term:

•  What is the solution to this system?

Lecture 3.4 – LTI Systems


x = Ax+Buy = Cx

u y

x = Axx(t0) = x0

•  If everything is scalar:

•  How do we know?

•  For higher-order systems, we just get a matrix version of this

Solving the ODE


x = ax, x(t0) = x0 ⇒ x(t) = ea(t−t0)x0

x(t0) = ea(t0−t0)x0 = e0x0 = x0

d

dtx(t) = aea(t−t0)x0 = ax

x = Ax, x(t0) = x0 ⇒ x(t) = eA(t−t0)x0

matrix exponential

initial conditions

dynamics

•  The definition is just like for scalar exponentials

•  Derivative:

Matrix Exponentials

3.4.3

eat =∞�

k=0

aktk

k!, eAt =

∞�

k=0

Aktk

k!

d

dt

∞�

k=0

Aktk

k!= 0 +

∞�

k=1

kAktk−1

k!= A

∞�

k=1

Ak−1tk−1

(k − 1)!= A

∞�

k=0

Aktk

k!

d

dteAt = AeAt

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

•  The matrix exponential plays such an important role that it has its own name: The State Transition Matrix

•  But what if we have the controlled system: •  Claim:

Solving the Controlled Equation


eA(t−t0) = Φ(t, t0)

x = Ax ⇒ x(t) = Φ(t, τ)x(τ)

x = Ax+Bu

x(t) = Φ(t, t0)x(t0) +

� t

t0

Φ(t, τ)Bu(τ)dτ

�ddtΦ(t, t0) = AΦ(t, t0)Φ(t, t) = I

•  Claim:



x(t) = Φ(t, t0)x(t0) +

� t

t0

Φ(t, τ)Bu(τ)dτ

x(t0) = Φ(t0, t0)x(t0) +

� t0

t0

Φ(t0, τ)Bu(τ)dτ

I0

x(t0) = x(t0)

•  Claim:



x(t) = Φ(t, t0)x(t0) +

� t

t0

Φ(t, τ)Bu(τ)dτ

d

dtx(t) = AΦ(t, t0)x(t0) +

d

dt

� t

t0

Φ(t, τ)Bu(τ)dτ

d

dt

� t

t0

f(t, τ)dτ = f(t, t) +

� t

t0

d

dtf(t, τ)dτ

Φ(t, t)Bu(t) +

� t

t0

AΦ(t, τ)Bu(τ)dτ

•  Claim:



x(t) = Φ(t, t0)x(t0) +

� t

t0

Φ(t, τ)Bu(τ)dτ

d

dtx(t) = AΦ(t, t0)x(t0) +

d

dt

� t

t0

Φ(t, τ)Bu(τ)dτ

d

dtx(t) = A

�Φ(t, t0)x(t0) +

� t

t0

Φ(t, τ)Bu(τ)dτ

�+Bu(t)

d

dtx = Ax+Bu

In Summary


x = Ax+Bu, y = Cx

y(t) = CΦ(t, t0)x(t0) + C

� t

t0

Φ(t, τ)Bu(τ)dτ

Φ(t, τ) = eA(t−τ)

•  First order of business is always trying to figure out if the system “blows up” or not

•  Recall the control design objectives: –  Stability –  Tracking –  Robustness –  Other objectives

Lecture 3.5 – Stability


•  It is useful to start with scalar systems to get some intuition about what is going on

Scalar Systems


x = ax ⇒ x(t) = eatx(0)

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50

50

100

150

a > 0 :


Scalar Systems


x = ax ⇒ x(t) = eatx(0)

a < 0 :

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1


Scalar Systems


x = ax ⇒ x(t) = eatx(0)

a = 0 :

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

•  Asymptotically Stable:

•  Unstable:

•  Critically Stable: in-between (doesn’t blow up but doesn’t go to zero either)

Three Cases


x = ax ⇒ x(t) = eatx(0)

x(t) → 0, ∀x(0)

∃x(0) : �x(t)� → ∞

a > 0 : unstablea < 0 : asymptotically stablea = 0 : critically stable

•  We cannot say that A>0, but we can do the next best thing – eigenvalues!

•  The eigenvalues tell us how the matrix A “acts” in different directions (eigenvectors)

•  In MATLAB:

From Scalars to Matrices?


x = Ax ⇒ x(t) = eAtx(0)

Av = λv

eigenvalue

eigenvector

>> eig(A)

∈ C

∈ �n

Example


λ1 = 1, λ2 = −1, v1 =

�10

�, v2 =

�01

�A =

�1 00 −1

�

•  Asymptotically Stable (if and only if):

•  Unstable (if):

•  Critically Stable (only if):

•  Critically Stable (if): one eigenvalue is 0 and the rest have negative real part OR two purely imaginary eigenvalues and the rest have negative real part

Stability


x = Ax ⇒ x(t) = eAtx(0)

Re(λ) < 0, ∀λ ∈ eig(A)

∃λ ∈ eig(A) : Re(λ) > 0

Re(λ) ≤ 0, ∀λ ∈ eig(A)

We will design for this!

A Tale of Two Pendula


A =

�0 1−1 0

�

A =

�0 11 0

�

λ1 = j, λ2 = −j

critically stable! oscillates!

unstable! λ1 = −1, λ2 = 1

•  Let’s use the stability results to solve the so-called Rendezvous Problem in swarm robotics

•  The setup: –  Given a collection of mobile agents who can only measure the relative

displacement of their neighbors (no global coordinates)

–  Problem: Have all the agents meet at the same (unspecified) position

Lecture 3.6 – Swarm Robotics


This is what agent i can measure

•  We have already seen the two-robot case (scalar – does not matter…)

•  If the agents simply aim towards each other:

The Two-Robot Case


x1

x1 = u1 x2 = u2

x2

�u1 = x2 − x1

u2 = x1 − x2x =

�−1 11 −1

�x

•  Fact: If one eigenvalue is 0 and all others have negative real part, then the state will end up in the so-called null-space of A

•  For this particular A, the null-space is

The Two-Robot Case


A =

�−1 11 −1

�λ1 = 0, λ2 = −2

critically stable

null(A) = {x : Ax = 0}

null(A) = {x : x =

�αα

�, α ∈ �}

•  We have that

•  Rendezvous is achieved! •  If there are more than two agents, they should probably aim

towards the centroid of their neighbors (or something similar)

The Two-Robot Case


x1 → α, x2 → α ⇔ (x1 − x2) → 0

xi =�

j∈Ni

(xj − xi)

The Multi-Robot Case


xi =�

j∈Ni

(xj − xi)

x =

x1...

xN

x = −Lx

•  Fact: If the underlying graph is connected then the graph Laplacian L has one zero eigenvalues and the rest are positive

•  Critically Stable!

null(L) = {x : x =

α...α

}

The Multi-Robot Case


xi =�

j∈Ni

(xj − xi) x = −Lx

•  Rendezvous is achieved!

xi → α, ∀i ⇔ (xi − xj) → 0, ∀i, j

Rendezvous


Beyond Rendezvous


And Now For Real


•  2 Kheperas •  Same PID go-to-goal controller as before •  The proposed “high-level” controllers are used to pick a

intermediary goal-points •  Robots keep track of position using odometry •  Positions are communicated rather than sensed (later…)

•  So now we know that the first order of business is to stabilize the system such that all the eigenvalues have negative real part

Lecture 3.7 – Output Feedback


x = Ax+Buy = Cx

u y

?

•  Idea: Move towards the origin!

Back to the World’s Simplest Robot


u

pp = u

x =

�0 10 0

�x+

�01

�u

y =�1 0

�x

u > 0 if y < 0u < 0 if y > 0

u = −y

Or, In General


u = −Ky = −KCx

x = Ax+Bu = Ax−BKCx = (A−BKC)x

Re(λ) < 0 ∀λ ∈ eig(A−BKC)

•  Pick, if possible, K such that

Back to the Robot


x =

��0 10 0

�−�

01

�1�1 0

��x

x =

�0 1−1 0

�x

eig(A−BKC) = ±j

Critically stable!

Back to the Robot


•  The problem is that we do not take the velocity into account! •  We need to use the full state information in order to stabilize

this system! •  Problem 1 (How to do that?) •  Problem 2 (But we do not know x, we know y?) •  Next Module! (and some next lecture…)

What’s The Problem?


•  Need to stabilize the system such that all the eigenvalues have negative real part – Using state feedback!

Lecture 3.8 – State Feedback


x = Ax+Buy = Cx

u y

?

Closing the Loop


x = Ax+Bu

u = −Kx

x = Ax+Bu = Ax−BKx = (A−BK)x

•  Pick, if possible, K such that the closed-loop system is stabilized, i.e.,

•  Module 4! Re(eig(A−BK)) < 0

closed-loop dynamics

Back to the Robot


u ∈ �, x ∈ �2, K : 1× 2

K =�k1 k2

�

x =

��0 10 0

�−

�01

� �k1 k2

��x

x =

�0 1

−k1 −k2

�x

Picking the Gains


•  In the next module, we will pick gains in a systematic manner, but for now, let’s try

k1 = k2 = 1

A−BK =

�0 1−1 −1

�

eig(A−BK) = −0.5± 0.866j

Asymptotically stable! Damped oscillations

Attempt 1


Another Attempt


Asymptotically stable! No oscillations

k1 = 0.1, k2 = 1

A−BK =

�0 1

−0.1 −1

�

eig(A−BK) = −0.1127,−0.8873

Attempt 2


Eigenvalues Matter


•  It is clear that some eigenvalues are better than others. Some cause oscillations, some make the system respond too slowly, and so forth…

•  In the next module we will see how to select eigenvalues and how to pick control laws based on the output rather than the state.

Documents

Module 3 Slides.pdf