Module 4 Slides

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?
School of Electrical and Computer Engineering
Lecture 4.1 Stabilizing the Point Mass

Given a linear system
x = Ax + Bu, y = Cx
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
Magnus Egerstedt, Control of Mobile Robots, Georgia Ins<tute of Technology
4.1.1
Back to the Point Mass

u
p
0 1
x = Ax + Bu, A =
, B=
0 0
State Feedback: u = Kx = k1 k2 x
0
1
How pick the control gains?

Idea: Pick them in order to make the closed-loop system have
the desired eigenvalues (poles)
4.1.2
Computing Eigenvalues
Given a matrix M, its eigenvalues satisfy the characteristic
equation
M () = det(I M ) = 0
m1 m2
M=
m3 m4
1 0
m1
I M =
M =
0 1
m3
m2
m4
4.1.3
equation
M () = det(I M ) = 0
m1
m
2
det(I M ) =
m3
m4
= ( m1 )( m4 ) m2 m3
= 2 (m1 + m4 ) + m1 m4 m2 m3 = 0
4.1.3
equation
M () = det(I M ) = 0
M () = 2 (m1 + m2 ) + m1 m4 m2 m3 = 0
m1 + m4
(m1 + m4 )2
=
m1 m4 + m2 m3
2
4
But this is really annoying is there an easier way?
4.1.3
Almost Computing Eigenvalues

The fundamental theorem of algebra: The roots in a
polynomial are completely determined by the coefficients
M () = 2 (m1 + m2 ) + m1 m4 m2 m3 = 0
Lets stop here!
4.1.4
Back to the Point-Mass, Again
u = Kx x = (A BK)x
0
A BK =
0
ABK () =
1
0
k1
0
1
1
+ k2
k1
k2
0
k1
1
k2
= 2 + k2 + k1
4.1.5
Desired Eigenvalues
Lets pick the eigenvalues that we would like the closed-loop
system to have
1 , . . . , n
If these were indeed the eigenvalues, then the characteristic
polynomial would be
n
() = ( 1 )( 2 ) ( n ) =
i=1
( i )
For the robot, lets pick both eigenvalues at -1
() = ( + 1)( + 1) = 2 + 2 + 1
Now we just line up the coefficients!
4.1.6
Lining Up the Coefficients

ABK = 2 + k2 + k1
() = ( + 1)( + 1) = 2 + 2 + 1
1 :
0 :
k2 = 2
k1 = 1
K=
1 2
x = Ax + Bu
u
x
u = Kx
4.1.7
Lecture 4.2 Pole Placement

Pick the control gains such that the eigenvalues (poles) of the
closed loop system match the desired eigenvalues
ABK () = n + an1 n1 + . . . + a1 + a0
n
() =
( i ) = n + bn1 n1 + . . . + b1 + b0
i=1
n1 :
..
.
0 :
an1 (K) = bn1

a0 (K) = b0
4.2.1
Questions
Is this always possible? No!
How should we pick the eigenvalues? Mix of art and science
Do we have to compute large determinants? No!
In MATLAB:
>> P=[lambda1,lambda2,lambda3,];!
>> K=place(A,B,P);!
4.2.2
Example
x =
u = Kx =
k1
k2
ABK () =
2 0
1 1
x+
1
1
2 k1
x A BK =
1 k1
2 + k1
k2
1 + k1
1 + k2
k2
1 k2
= 2 + (3 + k1 + k2 ) + 2 k1 k2
4.2.3
Example
ABK () = 2 + (3 + k1 + k2 ) + 2 k1 k2
() = ( + 1)2 = 2 + 2 + 1
1 :
3 + k1 + k2 = 2 k1 + k2 = 5
???
0 : 2 k 1 k 2 = 1 k 1 + k 2 = 1
Whats 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
4.2.4
How to Pick Eigenvalues?

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:
(complex-conjugate pairs)
Im(i ) = 0 j : Im(j ) = Im(i ), Re(j ) = Re(i )

Re(i ) < 0, i (stability)
Im(i ) = 0 oscillations
Rate of convergence
4.2.5
Rate of Convergence
x = (A BK)x, Re() < 0, eig(A BK)
The smallest eigenvalue dominates the convergence rate

Im
min = argmin |Re()|
min
Re
But, the bigger the eigenvalues the bigger

the control gains/signals
4.2.6
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; !
4.2.7
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!
4.3.1
A Modest Example
Given a discrete-time system
xk+1 = Axk + Buk , x0 = 0

We would like to drive this system in n steps to a particular
target state x
Is this possible?
?
0
4.3.2
A Modest Example
?
0
x1 = Ax0 + Bu0 = Bu0

x
x2 = Ax1 + Bu1 = ABu0 + Bu1
x3 = Ax2 + Bu2 = A2 Bu0 + ABu1 + Bu2
..
.
xn = An1 Bu0 + . . . + Bun1
We want to solve
x =
AB
un1
..
An1 B .
u1
u0
4.3.3
A Modest Example
x
?
0
x =
AB
un1
..
An1 B .
u1
u0
(n mn)
This is possible for any target state if and only if
rank() = n
Turns out, this generalizes quite nicely
4.3.3
Controllability Theorem 1
x = Ax + Bu, x n
Definition: The system is completely controllable (CC) if it

is possible to go from any initial state to any final state.
AB
n1
controllability matrix
CC Theorem 1: The system is CC if and only if
rank() = n
rank(M)=# linearly independent rows or columns in M
4.3.4
x =
2 0
1 1
x+
Two Systems
1
1
pole-placement not possible
AB =
x =
2
2
0 1
0 0
0
1
1 2
1 2
AB =
1
0
0 1
1 0
= [B AB]
not CC!
pole-placement possible
n=2
rank() = 1
n=2
= [B AB]
CC!
rank() = 2
4.3.5
Controllability Theorem 2
u = Kx, x = (A BK)x
CC Theorem 2: Pole-placement to arbitrary

eigenvalues is possible if and only if the system is CC!
4.3.6
Point-to-Point vs. Trajectories

x =
0 1
0 0
0
1
x2
In MATLAB:
>> G=ctrb(A,B);!
>> rank(G)!
ans: 2!
x0
x1
4.3.7
Lecture 4.4 The Segway Robot

Lets unleash our newfound powers on a complicated robotic
system
4.4.1
Unicycle + Inverted Pendulum +
The Base:
x 1 = v cos
x 2 = v sin
=
Extra states:
The Pendulum:
,
Inputs:
L , R
v,
4.4.2
Unicycle + Inverted Pendulum +

T
x = [x1 x2 v ]
u = [L R ]T
x = f (x, u)
T
= [v cos v sin v ]
1
3(mw + mb )v mb d cos + mb d sin ( 2 + 2 ) = (L + R )
R
LINEARIZE!
1
2
2
+ mb d2 sin cos = L (L R )
((3L +
)m
+
m
d
sin
+
I
)
2
w
b
2R2
R
2
mb d cos v + (mb d2 I3 ) + mb d2 sin cos 2 + mb gd sin = L + R
4.4.3
Linearization
Linearize this mess around (x,u)=(0,0), and plug in the
physical parameters for the segway robot:
x = Ax + Bu
A=
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
2.16
0
0
0
72.49
0
0
0
0
0
1
0
B=
0
0
0
0
1.67
1.67
0
0
0.029
0.029
0
0
24.15 24.1514
4.4.4
Controllability?
>> rank(ctrb(A,B))!
ans =!
Not CC!
6!
We already know that the unicycle dynamics gets messed up

when linearized.
And if we can control v and , that should be enough, so
lets simply remove the unicycle part
4.4.5
A Smaller System
T , u = [L R ]T
x = [v ]
x = Ax + Bu
0
0
A=
0
0
0 2.16
0
0
0
0
0 72.49
0
0
1
0
1.67
1.67
0.029 0.029
B=
0
0
24.15 24.15
>> rank(ctrb(A,B))!
ans =!
CC!
4!
4.4.6
One Last Twist

We do not want to stabilize to (v,)=(0,0)
Let
x
= x [vd d 0 0]T = x
The dynamics become
x
= x = x = Ax + Bu = A(x ) + Bu + A
Luckily,
A = 0 x
= A
x + Bu
4.4.7
Pole-Placement
We have a CC system that we wish to stabilize
After some tweaking, the following eigenvalues seem to
behave well:
1 = 19, 2 = 7.5, 3 = 5, 4 = 0.3

u = K x
x
= (A BK)
x
>> P=[-19,-7.5,-5,-0.3];!
>> K=place(A,B,P);!
4.4.8
Now, Lets Do It!
=
v
curvature rather than pose
4.4.9
Lecture 4.5 Observers

We now know how to design rather effective controllers using
state feedback.
But what about y???
x = Ax + Bu
y = Cx
?
4.5.1

state feedback.
But what about y???
x = Ax + Bu
y = Cx
4.5.1

state feedback.
But what about y???
x = Ax + Bu
y = Cx
observer
4.5.1
The Predictor-Corrector
x = Ax
y = Cx
First idea: Make a copy of the system
x
= A
x
predictor
Second idea: Add a notion of how

wrong your estimate is to the model
x
= A
x + L(y C x
)
corrector
Luenberger observer
4.5.2
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 e = x x
Cx
e = x x
= Ax A
x L(y C x
)
e = A(x x
) LC(x x
) = (A LC)e
Just pick L such that the eigenvalues to A-LC have negative
real part!!
4.5.3
Pole-Placement, Again
e = (A LC)e
Want
Re(eig(A LC)) < 0

We already know how to do this - Pole-placement!
4.5.4
Example
x =
1
0
2
2
e = (A LC)e =
=
x, y =
1
0
2
2
1 L1
L2
L1
L2
2 + L1
2 + L2
4.5.5
Example
e =
1 L1
L2
2 + L1
2 + L2
+ 1 + L1
ALC () =
L2
2 L1
+ 2 L2
= 2 + (3 + L1 L2 ) + 2 + 2L1 + L2
() = ( + 1)2 = 2 + 2 + 1
L1 = 2/3
L2 = 1/3
4.5.6
Example
1
x
= A
x+
3
2
1
1.2
0.9
(y C x
)
0.8
0.8
0.7
0.6
0.6
0.4
0.5
0.2
0.4
0
0.3
0.2
0.2
0.4
0.6
0.1
4.5.7
The Block-Diagram
x = Ax
y = Cx
x
= A
x + L(y C x
)
4.5.8
Does This Always Work?

No!
Next Lecture: We need to redo what we did for control design
(controllability) for observer design (observability)
4.5.9
Lecture 4.6 Observability

Need to redo what we did for control design to understand
when we can recover the state from the output
x = Ax
y = Cx
x
= A
x + L(y C x
)
4.6.1
A Modest Example (Again)

Given a discrete time system without inputs
xk+1 = Axk
yk = Cxk
Can we recover the initial condition by collecting n output
values?
y0 = Cx0
y1 = Cx1 = CAx0
..
.
yn1 = CAn1 x0
4.6.2
The Observability Matrix
y0
y1
y2
..
.
yn1
C
CA
CA2
..
.
CAn1
x0
The initial condition can be recovered from the

outputs when the so-called observability matrix
has full rank.
4.6.3
Observability Theorem 1
x = Ax, x n
y = Cx
Definition: The system is completely observable (CO) if it
is possible to recover the initial state from the output.
C
CA
..
.
CAn1
observability matrix
CO Theorem 1: The system is

CO if and only if
rank() = n
rank(M)=# linearly independent rows or columns in M
4.6.4
Observability Theorem 2
x
= A
x + L(y C x
), e = (A LC)e
CO Theorem 2: Pole-placement to arbitrary
eigenvalues is possible if and only if the system is CO!
4.6.5
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!
4.6.6
Lecture 4.7 The Separation Principle

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
4.7.1
A Game Plan
x = Ax + Bu
y = Cx
Assume CC and CO
Step 1: Design the state feedback controller as if we had x

(which we dont)
u = Kx x = (A BK)x
u = K x
what we design for
what we actually have
Step 2: Estimate x using an observer (that now also

contains u)
x
= A
x + Bu + L(y C x
)
e = (A LC)e
4.7.2
Does This Work?

Want both x and e to be stabilized
Analyze their joint dynamics:
x = Ax BK x
= Ax BK(x e) = (A BK)x + BKe
e=xx
e = (A LC)e
Or, when viewed together
x
e
A BK
0
BK
A LC
The strategy works if and only if

this is an asymptotically stable system!
x
e
4.7.3
The Separation Principle
x
e
A BK
0
BK
A LC
x
e
This is an (upper) triangular block-matrix. Its eigenvalues are

given by the eigenvalues of the diagonal blocks!
M () = ABK ()ALC ()
If we havent messed up in the design, we have
Re(eig(A BK)) < 0, Re(eig(A LC)) < 0
Everything works!!
4.7.4
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!
4.7.5
The Final Block-Diagram

u
x = Ax + Bu
y = Cx
u = K x
x
= A
x + Bu + L(y C x
)
4.7.6
Lecture 4.8 Practical Considerations

The separation principle means that we can decouple control
and observer design (in theory)
In theory, theory and practice are the

same. In practice they are not. -Yogi Berra
4.8.1
Eigenvalue Selection
Typically, the observer should be faster than the controller,
since the controller wont 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!
4.8.2
Eigenvalue Selection
Im()
Re()
observer
controller
4.8.3
Humanoid Robot
torque constant
= (Ki b)
J
current
moment of inertia
Aldebaran Nao
0
0
1
x =
b/J
y= 1 0 x
friction coefficient
x+
0
K/J
CC & CO!!
4.8.4
Reference Tracking
x = Ax + Bu
d
e=
y = Cx
d
e = Ax + Bu = Ae + A
+ Bu = Ae + Bu
0
d
y = Cx = Ce + C
= Ce + d
0
u = K e
e = A
e + Bu + L(y C e d )
4.8.5
Beyond Pole Placement

The design structure is quite general (does not need to be
based on pole placement)
u = K x
x
= A
x + Bu + L(y c
x)
LQ Optimal Control: min
K = R1 B T P
(xT Qx + uT Ru)dt
0 = AT P + P A + Q P BR1 B T P
4.8.6
Beyond Pole Placement

The design structure is quite general (does not need to be
based on pole placement)
u = K x
x
= A
x + Bu + L(y c
x)
The Kalman Filter:
x = Ax + Bu + DI vI
y = Cx + DO vO
min lim E(x(t) x

(t))2
t
T 1
L = P C T (DO DO
)
T 1
0 = AT P + P A + DI DIT P C T (DO DO
) CP
4.8.7
Next Module
But, the real world (especially for robots) is more complex
than a simple LTI system
HYBRID SYSTEMS!
4.8.8

Module 4 Slides

Hochgeladen von

Dokumentinformationen

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

Module 4 Slides

Hochgeladen von

Copyright:

Verfügbare Formate

Control of

How make mobile robots move in effective, safe,

School of Electrical and Computer Engineering

Lecture 4.1 Stabilizing the Point Mass

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

Back to the Point Mass

How pick the control gains?

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

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

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

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

Almost Computing Eigenvalues

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

Back to the Point-Mass, Again

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

For the robot, lets pick both eigenvalues at -1

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

Lining Up the Coefficients

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

Lecture 4.2 Pole Placement

an1 (K) = bn1

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

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

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

Whats at play here is a lack of controllability, i.e., the effect

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

How to Pick Eigenvalues?

Im(i ) = 0 j : Im(j ) = Im(i ), Re(j ) = Re(i )

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

The smallest eigenvalue dominates the convergence rate

min = argmin |Re()|

But, the bigger the eigenvalues the bigger

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

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

Lecture 4.3 Controllability

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

xk+1 = Axk + Buk , x0 = 0

x1 = Ax0 + Bu0 = Bu0

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

This is possible for any target state if and only if

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

Definition: The system is completely controllable (CC) if it

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

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

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

pole-placement not possible

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

CC Theorem 2: Pole-placement to arbitrary

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

Point-to-Point vs. Trajectories

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

Lecture 4.4 The Segway Robot

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

Unicycle + Inverted Pendulum +

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

Unicycle + Inverted Pendulum +

mb d cos v + (mb d2 I3 ) + mb d2 sin cos 2 + mb gd sin = L + R

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

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

We already know that the unicycle dynamics gets messed up

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

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

One Last Twist

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