Vous êtes sur la page 1sur 65

Control of

Mobile Robots
Module 3
Dr. Magnus Egerstedt
Professor
School of Electrical and
Linear Systems
Computer Engineering

How make mobile robots move in effective, safe,


predictable, and collaborative ways using modern
control theory?

School of Electrical and Computer Engineering


Lecture 3.1 A Simple Robot
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 LINEAR SYSTEMS!!
Simple enough
Expressive enough
Relevant enough

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


3.1.1
Controlling a Point Mass
Given a point mass on a line whose acceleration is directly
controlled:
u
p = u
p
Want to write this on a compact/general form

x1 = p x1 = x2
x2 = p
x2 = u

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


3.1.2
On State Space Form

x1 = x2 x1
x=
x2 = u x2

Store these variables in a single, 2-dimensional state


variable

x1 x2 0 1 x1 0
x = = = + u
x2 u 0 0 x2 1


y = p = x1 = 1 0 x

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


3.1.3
On State Space Form

x1 x2 0 1 x1 0
x = = = + u
x2 u 0 0 x2 1

y = p = x1 = 1 0 x
Or, even more generally

0 1 0
A= , B= , C= 1 0
0 0 1

x = Ax + Bu
y = Cx

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


3.1.4
A 2D Point-Mass
px = ux
py = uy u = (ux , uy )

p = (px , py ) 0 1 0 0
0 0 0 0 x = Ax + Bu
x 1 = px A=


y = Cx
0 0 0 1
x2 = px 0 0
0 0
x 3 = py 0 0
x4 = py 1 0
B=



u1 = ux 0 0
u2 = uy 0 1
y 1 = px 1 0 0 0
C=
y 2 = py 0 0 1 0
Magnus Egerstedt, Control of Mobile Robots, Georgia Ins<tute of Technology
3.1.5
LTI Systems
x = Ax + Bu
y = Cx
This is a so-called LTI (Linear Time-Invariant) System on
State-Space form!

x n A: nn
u m B : nm

y p C : pn
x = A x + B u
n 1 (n n)(n 1) (n m)(m 1)
n1 n1
y = C x
p1 (p n)(n 1)
p1

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


3.1.6
Lecture 3.2 State-Space Models
The general LTI model is
x state
y output
u input

u x = Ax + Bu y
y = Cx

Main Question: How should the input be selected?


But First: How can such systems be understood? And where
do they come from?

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


3.2.1
Example 1: The Car Model
Recall our old friend, the car model
c
v = u v
m
If we care about/can measure the velocity:
x = Ax + Bu c
A = , B = , C = 1
y = Cx, m
If we care about/can measure the position we have the same
general equation with different matrices:

0 1 0
A= , B= , C= 1 0
0 c/m

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


3.2.2
Example 2: Pendulum
Newtons 2nd tells us that
Not linear!
g
= sin() + cu

1 sin
For small angles we get x = Ax + Bu, y = Cx

0 1 0
A= , B= , C= 1 0
g/ 0 c

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


3.2.3
Example 3: Two Simple Robots
Consider two robots on a line

x1 = u1 x2 = u2 1 0
x = u
0 1
x1 x2
(A = 0)

The Rendezvous Problem: Have them meet at the same


location
Idea: Have them aim towards each other:

u1 = x2 x1 1 1
u2 = x1 x2 x = x
1 1
A closed-loop feedback law!

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


3.2.4
Rendezvous

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


3.2.5
Example 4: Unicycle Robot

1
x = v cos
y = v sin Not linear!

=
(x, y)
We need to be more systematic/
clever when it comes to generating
x = v
LTI models from nonlinear
y = v Still not linear! systems!
=

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


3.2.6
Lecture 3.3 Linearizations

Classifying systems as linear and non-linear is like


classifying objects in the Universe as bananas and
non-bananas. -unknown

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


3.3.1
Linearizations
Given a non-linear model
x = f (x, u), y = h(x)

We want to find a local, linear model around an operating


point
(xo , uo ) (x = xo + x, u = uo + u)

The new equations of motion become


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

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


3.3.2
Linearizations
x = f (xo + x, uo + u)
Taylor
expansion
f f
= f (xo , uo ) + (xo , uo )x + (xo , uo )u + H.O.T
x u
A B
h
y = h(xo + x) = h(xo ) + (xo )x + H.O.T
x
Assumptions: C
f (xo , uo ) = 0
h(xo ) = 0

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


3.3.3
Linearizations


x = Ax + Bu
x = f (x, u)
y = Cx

x = xo + x

y = h(x)

u = uo + u


A = f
x (xo , uo )

f (xo , uo ) = 0



h(xo ) = 0
B = f

u (xo , uo )




C = h
x (xo )

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


3.3.4
Computing the Jacobians

f1
h1
f2 ..
x n , u m , y p , f = .. , h = .
.
hm
fn
f1 f1 f1

x1 x2 xn
f2 f2
f x1 xn
= .. .. ..
x . . .


fn fn
x1 xn
nn
Magnus Egerstedt, Control of Mobile Robots, Georgia Ins<tute of Technology
3.3.5
Computing the Jacobians

f1
h1
f2 ..
x n , u m , y p , f = .. , h = .
.
f1 f1
f1

hm
f


x1
f2
x1
x2

xn
f2
xn

fn
= .. .. ..
x . . .


fn
x1 fn
xn f1 f1

u1 um
f2 f2
f u1 um
= .. .. ..
u . . .


fn fn
u1 um
nm
Magnus Egerstedt, Control of Mobile Robots, Georgia Ins<tute of Technology
3.3.5
Computing the Jacobians

f1
h1
f2 ..
x n , u m , y p , f = .. , h = .
.
f1 f1
f1

hm
f


x1
f2
x1
x2

xn
f2
xn

fn
= .. .. ..
x . . .


fn fn

x1
f1 f1
x
n



u1
f2
um
f2 h1 h1
f
u

=
u1
..

..
um
..


x1 xn
. . .
fn
fn h .. .. ..
u1 um
= . . .
x hp hp
x1 xn
pn

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


3.3.5
Example: Inverted Pendulum
g
= sin + u cos


x1 = x2
f (x, u) =
u x2 = , g/ sin(x1 ) + u cos(x1 )
y = x1 h(x) = x1

(xo , uo ) = (0, 0)

f1 f1
x1 x2 0 1 0 1
A= f2 f2 = =
x1 x2
g/ cos(x1 ) 0 (0,0)
g/ 0
(0,0)

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


3.3.6
Example: Inverted Pendulum
g
= sin + u cos


x1 = x2
f (x, u) =
u x2 = , g/ sin(x1 ) + u cos(x1 )
y = x1 h(x) = x1

(xo , uo ) = (0, 0)
f1

u 0 0
B= f2 = =
u
cos(x1 ) (0,0)
1
(0,0)

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


3.3.6
Example: Inverted Pendulum
g
= sin + u cos


x1 = x2
f (x, u) =
u x2 = , g/ sin(x1 ) + u cos(x1 )
y = x1 h(x) = x1

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

C= x1 x2 (0,0)
= 1 0

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


3.3.6
Example: Unicycle

x = v cos x1 = x, x2 = y, x3 =

y = v sin y1 = x1 , y2 = x2 , y3 = x3
=
u1 = v, u2 =

(xo , uo ) = (0, 0)
(x, y)

1 0 1 0 0
A = 0, B = 0 0 , C = 0 1 0
0 1 0 0 1

x2 = 0 ???

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


3.3.7
Punchlines
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)

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


3.3.8
Lecture 3.4 LTI Systems

u x = Ax + Bu y
y = Cx

Lets figure out how such systems behave.


Start by ignoring the input term:
x = Ax
x(t0 ) = x0

What is the solution to this system?

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


3.4.1
Solving the ODE
If everything is scalar:
x = ax, x(t0 ) = x0 x(t) = ea(tt0 ) x0
How do we know?
x(t0 ) = ea(t0 t0 ) x0 = e0 x0 = x0 initial conditions
d
x(t) = aea(tt0 ) x0 = ax dynamics
dt
For higher-order systems, we just get a matrix version of this

x = Ax, x(t0 ) = x0 x(t) = eA(tt0 ) x0

matrix exponential

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


3.4.2
Matrix Exponentials
The definition is just like for scalar exponentials



a k tk Ak t k
eat = , eAt =
k! k!
k=0 k=0

Derivative:

d Ak t k kAk tk1 Ak1 tk1 Ak t k
=0+ =A =A
dt k! k! (k 1)! k!
k=0 k=1 k=1 k=0

d At
e = AeAt
dt

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


3.4.3
Solving the Controlled Equation
The matrix exponential plays such an important role that it has
its own name: The State Transition Matrix
eA(tt0 ) = (t, t0 )
d
x = Ax x(t) = (t, )x( ) dt (t, t0 ) = A(t, t0 )
(t, t) = I

But what if we have the controlled system: x = Ax + Bu


Claim: t
x(t) = (t, t0 )x(t0 ) + (t, )Bu( )d
t0

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


3.4.4
Solving the Controlled Equation
Claim: t
x(t) = (t, t0 )x(t0 ) + (t, )Bu( )d
t0
t0
x(t0 ) = (t0 , t0 )x(t0 ) + (t0 , )Bu( )d
t0
I
0
x(t0 ) = x(t0 )

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


3.4.5
Solving the Controlled Equation
Claim: t
x(t) = (t, t0 )x(t0 ) + (t, )Bu( )d
t0
t
d d
x(t) = A(t, t0 )x(t0 ) + (t, )Bu( )d
dt dt t0

t t
d d
f (t, )d = f (t, t) + f (t, )d
dt t0 t0 dt
t
(t, t)Bu(t) + A(t, )Bu( )d
t0

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


3.4.6
Solving the Controlled Equation
Claim: t
x(t) = (t, t0 )x(t0 ) + (t, )Bu( )d
t0

d d t
x(t) = A(t, t0 )x(t0 ) + (t, )Bu( )d
dt dt t0
t
d
x(t) = A (t, t0 )x(t0 ) + (t, )Bu( )d + Bu(t)
dt t0

d
x = Ax + Bu
dt

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


3.4.6
In Summary
x = Ax + Bu, y = Cx

t
y(t) = C(t, t0 )x(t0 ) + C (t, )Bu( )d
t0

(t, ) = eA(t )

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


3.4.7
Lecture 3.5 Stability

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

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


3.5.1
Scalar Systems

It is useful to start with scalar systems to get some intuition


about what is going on
x = ax x(t) = eat x(0)
150

a>0:
100

50

0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

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


3.5.2
Scalar Systems

It is useful to start with scalar systems to get some intuition


about what is going on
x = ax x(t) = eat x(0)
1

a<0: 0.9

0.8

0.7

0.6

0.5

0.4

0.3

0.2

0.1

0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

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


3.5.2
Scalar Systems

It is useful to start with scalar systems to get some intuition


about what is going on
x = ax x(t) = eat x(0)
2

a=0: 1.8

1.6

1.4

1.2

0.8

0.6

0.4

0.2

0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

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


3.5.2
Three Cases
Asymptotically Stable: x(t) 0, x(0)

Unstable: x(0) : x(t)

Critically Stable: in-between (doesnt blow up but doesnt go


to zero either)

x = ax x(t) = eat x(0)



a > 0 : unstable
a < 0 : asymptotically stable

a = 0 : critically stable

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


3.5.3
From Scalars to Matrices?
x = Ax x(t) = eAt x(0)
We cannot say that A>0, but we can do the next best
thing eigenvalues! eigenvalue C

Av = v eigenvector n

The eigenvalues tell us how the matrix A acts in


different directions (eigenvectors)
In MATLAB:
>> eig(A)

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


3.5.4
Example

1 0
A=
0 1
1 0
1 = 1, 2 = 1, v1 = , v2 =
0 1

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


3.5.5
Stability
x = Ax x(t) = eAt x(0)
Asymptotically Stable (if and only if):
Re() < 0, eig(A) We will design
for this!
Unstable (if):
eig(A) : Re() > 0
Critically Stable (only if):
Re() 0, eig(A)
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

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


3.5.6
A Tale of Two Pendula

0 1
A= 1 = j, 2 = j
1 0
critically stable!
oscillates!


0 1 1 = 1, 2 = 1
A=
1 0
unstable!

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


3.5.7
Lecture 3.6 Swarm Robotics

Lets 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)

This is what agent i can measure

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

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


3.6.1
The Two-Robot Case
We have already seen the two-robot case (scalar does not
matter)
x1 = u1 x2 = u2

x1 x2
If the agents simply aim towards each other:

u1 = x2 x1 1 1
x = x
u2 = x1 x2 1 1

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


3.6.2
The Two-Robot Case

1 1
A= 1 = 0, 2 = 2
1 1
critically stable

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
null(A) = {x : Ax = 0}
For this particular A, the null-space is


null(A) = {x : x = , }

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


3.6.3
The Two-Robot Case

We have that
x1 , x2 (x1 x2 ) 0
Rendezvous is achieved!
If there are more than two agents, they should probably aim
towards the centroid of their neighbors (or something similar)

xi = (xj xi )
jNi

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


3.6.4
The Multi-Robot Case

xi = (xj xi ) x = Lx
jNi
Fact: If the underlying graph is
x1 connected then the graph Laplacian L
..
x= . has one zero eigenvalues and the rest
xN are positive
Critically Stable!


..
null(L) = {x : x = . }

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


3.6.5
The Multi-Robot Case

xi = (xj xi ) x = Lx
jNi

xi , i (xi xj ) 0, i, j

Rendezvous is achieved!

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


3.6.6
Rendezvous

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


3.6.7
Beyond Rendezvous

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


3.6.8
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)

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


3.6.9
Lecture 3.7 Output Feedback

So now we know that the first order of business is to stabilize


the system such that all the eigenvalues have negative real part

u x = Ax + Bu y
y = Cx

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


3.7.1
Back to the Worlds Simplest Robot
u
p = u
p

0 1 0
x = x+ u
0 0 1

y= 1 0 x Idea: Move towards the origin!

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

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


3.7.2
Or, In General
u = Ky = KCx

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

Pick, if possible, K such that

Re() < 0 eig(A BKC)

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


3.7.3
Back to the Robot

0 1 0
x = 1 1 0 x
0 0 1

0 1
x = x
1 0

eig(A BKC) = j

Critically stable!

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


3.7.4
Back to the Robot

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


3.7.5
Whats The Problem?

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)

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


3.7.6
Lecture 3.8 State Feedback

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

u x = Ax + Bu y
y = Cx

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


3.8.1
Closing the Loop

x = Ax + Bu
u = Kx closed-loop dynamics

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

Pick, if possible, K such that the closed-loop system


is stabilized, i.e.,
Re(eig(A BK)) < 0
Module 4!

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


3.8.2
Back to the Robot
u , x 2 , K : 1 2

K = k1 k2

0 1 0
x = k1 k2 x
0 0 1

0 1
x = x
k1 k2

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


3.8.3
Picking the Gains

In the next module, we will pick gains in a systematic manner,


but for now, lets try
k1 = k2 = 1

0 1
A BK =
1 1
eig(A BK) = 0.5 0.866j
Asymptotically stable!
Damped oscillations

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


3.8.4
Attempt 1

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


3.8.5
Another Attempt

k1 = 0.1, k2 = 1

0 1
A BK =
0.1 1
eig(A BK) = 0.1127, 0.8873
Asymptotically stable!
No oscillations

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


3.8.6
Attempt 2

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


3.8.7
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.

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


3.8.8