Module4 PDF

Control of
Mobile Robots
Module 4
Dr. Magnus Egerstedt
Professor
School of Electrical and
Control Design
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 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 0
ẋ = Ax + Bu, A = , B=
0 0 1
� �
•  State Feedback: u = −Kx = − k1 k2 x
•  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 −m2
λI − M = λ −M =
0 1 −m3 λ − m4
4.1.3
equation
χM (λ) = det(λI − M ) = 0
� �
� λ − m1 −m �
det(λI − M ) = �� 2 �
−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
•  Let’s stop here!
4.1.4
Back to the Point-Mass, Again
u = −Kx ⇒ ẋ = (A − BK)x
� � � � � �
0 1 0 � � 0 1
A − BK = − k1 k2 =
0 0 1 −k1 −k2
� �
� λ −1
χA−BK (λ) = �� = λ2 + λk2 + k1
k1 λ + k2
4.1.5
Desired Eigenvalues
•  Let’s 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 )
i=1
•  For the robot, let’s pick both eigenvalues at -1
ϕ(λ) = (λ + 1)(λ + 1) = λ2 + 2λ + 1
•  Now we just line up the coefficients!
4.1.6
Lining Up the Coefficients
χA−BK = λ2 + k2 λ + k1
ϕ(λ) = (λ + 1)(λ + 1) = λ2 + 2λ + 1
λ1 : k2 = 2 � �
λ0 : 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
χA−BK (λ) = λn + an−1 λn−1 + . . . + a1 λ + a0

n
�
ϕ(λ) = (λ − λi ) = λn + bn−1 λn−1 + . . . + b1 λ + b0
i=1
λn−1 : an−1 (K) = bn−1

..
.
λ0 : 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
� � � �
2 0 1
ẋ = x+ u
1 1 1
� �
� � 2 − k1 −k2
u = −Kx = k1 k2 x ⇒ A − BK =
1 − k1 1 − k2
� �
� λ − 2 + k1 k2 �
χA−BK (λ) = ��
−1 + k1 λ − 1 + k2 �
= λ2 + λ(−3 + k1 + k2 ) + 2 − k1 − k2
4.2.3
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 − k 1 − k 2 = 1 ⇒ k 1 + k 2 = 1
•  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…
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(λ)|

Re
λmin
•  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? �
x
4.3.2
A Modest Example
x� x1 = Ax0 + Bu0 = Bu0
x2 = Ax1 + Bu1 = ABu0 + Bu1
?
x3 = Ax2 + Bu2 = A2 Bu0 + ABu1 + Bu2
..
.
0 xn = An−1 Bu0 + . . . + Bun−1
We want to solve  
un−1
� �  .. 
 
x = �
B AB ··· An−1 B  . 
 u1 
u0
4.3.3
A Modest Example
 
x� un−1
� �  .. 
?  
x = �
B AB · · · An−1 B  . 
 u1 
Γ (n × mn) u0
0
•  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.
� n−1
�
Γ= B AB ··· A B 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
Two Systems
� � � �
2 0 1
ẋ = x+ u n=2 Γ = [B AB]
1 1 1
pole-placement not possible not CC!
� � � �
2 1 2 rank(Γ) = 1
AB = Γ=
2 1 2
� � � �
0 1 0
ẋ = + u n=2 Γ = [B AB]
0 0 1
pole-placement possible CC!
� � � �
1 0 1 rank(Γ) = 2
AB = Γ=
0 1 0
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
� � � � In MATLAB:
0 1 0 >> G=ctrb(A,B);!
ẋ = + u
0 0 1 >> rank(G)!
ans: 2!
� x2
x x0
x1
4.3.7
Lecture 4.4 – The Segway Robot
•  Let’s unleash our newfound powers on a complicated robotic

system
4.4.1
Unicycle + Inverted Pendulum + …
The Base: The “Pendulum”:

ẋ1 = v cos ψ φ, φ̇
ẋ2 = v sin ψ
Inputs: τL , τR
ψ̇ = ω
Extra states: v, ω
4.4.2
Unicycle + Inverted Pendulum + …
x = [x1 x2 v ψ ψ̇ φ φ̇]T
u = [τL τR ]T
ẋ = f (x, u)
= [v cos ψ v sin ψ v̇ ψ̇ ψ̈ φ̇ φ̈]T
1
3(mw + mb )v̇ − mb d cos φφ̈ + mb d sin φ(φ̇2 + ψ̇ 2 ) = − (τL + τR )
R
12 2 2
LINEARIZE!
2 L
((3L + )m w + mb d sin φ + I 2 ) ψ̈ + mb d sin φ cos φ ψ̇ φ̇ = (τL − τR )
2R2 R
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
   
0 0 1 0 0 0 0 0 0
 0 0 0 0 0 0 0   0 0 
   
 0 0 0 0 0 2.16 0   −1.67 −1.67 
   
A= 0 0 0 0 1 0 0  B= 0 0 
   
 0 0 0 0 0 0 0   0.029 −0.029 
   
 0 0 0 0 0 0 1   0 0 
0 0 0 0 0 72.49 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
let’s simply remove the unicycle part
4.4.5
A Smaller System
x = [v ω φ φ̇]T , u = [τL τR ]T
ẋ = Ax + Bu
   
0 0 2.16 0 −1.67 −1.67
 0 0 0 0   0.029 −0.029 
A=
 0
 B= 
0 0 1   0 0 
0 0 72.49 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̃˙ = Ax̃ + 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, Let’s Do It!
ω
κ=
v
curvature rather than pose
v↑
ω↓ ω↑
v↓
4.4.9
Lecture 4.5 – Observers
•  We now know how to design rather effective controllers using

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

state feedback.
u ẋ = Ax + Bu y
y = Cx
4.5.1

state feedback.
u ẋ = Ax + Bu y
y = Cx
x̂
observer
4.5.1
The Predictor-Corrector
ẋ = Ax
y = 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 − 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 − Ax̂ − 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
� �
−1 2 � �
ẋ = x, y = 1 −1 x
0 −2
��
−1 2 L1 � �
ė = (A − LC)e = − 1 −1 e
0 −2 L2
� �
−1 − L1 2 + L1
= e
−L2 −2 + L2
4.5.5
Example
� �
−1 − L1 2 + L1
ė = e
−L2 −2 + L2
� �
� λ + 1 + L1 −2 − L1 ��
χA−LC (λ) = ��
L2 λ + 2 − L2 �
= λ2 + λ(3 + L1 − L2 ) + 2 + 2L1 + L2
ϕ(λ) = (λ + 1)2 = λ2 + 2λ + 1
�
L1 = −2/3
L2 = 1/3
4.5.6
Example
� �
˙x̂ = Ax̂ + 1 −2
(y − C x̂)
3 1
1.2 1
1 0.9
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.1
ï0.6 0
0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8
4.5.7
The Block-Diagram
ẋ = Ax y
y = Cx
x̂
x̂˙ = Ax̂ + 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
y = Cx
x̂
x̂˙ = Ax̂ + 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
..
.
yn−1 = CAn−1 x0
4.6.2
The Observability Matrix
   
y0 C
 y1   CA 
   
 y2   CA2 
 =  x0
 ..   .. 
 .   . 
yn−1 CAn−1
Ω
•  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 observability matrix
 CA 
  CO Theorem 1: The system is
Ω= .. 
 .  CO if and only if
CAn−1 rank(Ω) = n
rank(M)=# linearly independent rows or columns in M
4.6.4
Observability – Theorem 2
x̂˙ = Ax̂ + 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
Assume CC and CO
y = Cx
Step 1: Design the state feedback controller as if we had x
(which we don’t)
u = −Kx ⇒ ẋ = (A − BK)x what we design for
u = −K x̂ what we actually have

Step 2: Estimate x using an observer (that now also
contains u)
x̂˙ = Ax̂ + 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 = x − x̂
ė = (A − LC)e
•  Or, when viewed together
� � � ��
ẋ A − BK BK x
=
ė 0 A − LC e
•  The strategy works if and only if
this is an asymptotically stable system!
4.7.3
The Separation Principle
� � � ��
ẋ A − BK BK x
=
ė 0 A − LC e
M
•  This is an (upper) triangular block-matrix. Its eigenvalues are
given by the eigenvalues of the diagonal blocks!
χM (λ) = χA−BK (λ)χA−LC (λ)
•  If we haven’t 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
y = Cx
u = −K x̂
x̂
x̂˙ = Ax̂ + 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 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!
4.8.2
Eigenvalue Selection
Im(λ)
Re(λ)
observer
controller
4.8.3
Humanoid Robot
torque constant
1
θ̈ = (Ki − bθ̇)
J
current friction coefficient
moment of inertia
� � � �
1 0 0
ẋ = x+ u
−b/J 0 K/J
Aldebaran Nao � �
y= 1 0 x 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̂˙ = Aê + 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̂˙ = Ax̂ + Bu + L(y − cx̂)
� ∞
•  LQ Optimal Control: min (xT Qx + uT Ru)dt
0
K = R−1 B T P
0 = AT P + P A + Q − P BR−1 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̂˙ = Ax̂ + Bu + L(y − cx̂)
ẋ = Ax + Bu + DI vI
•  The Kalman Filter: 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

Module4 PDF

Hochgeladen von

Dokumentinformationen

Originalbeschreibung:

Originaltitel

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

Module4 PDF

Hochgeladen von

Copyright:

Verfügbare Formate

Control of

How make mobile robots move in effective, safe,

School of Electrical and Computer Engineering

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

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

• Let’s stop here!

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

λn−1 : an−1 (K) = bn−1

• How should we pick the eigenvalues? Mix of art and science

• Do we have to compute large determinants? No!

• What’s at play here is a lack of “controllability”, i.e., the effect

• Assuming that we have sufficient control authority to do pole

λmin = argminλ |Re(λ)|

• But, the bigger the eigenvalues the bigger

% Let's pick our favorite poles!

% Compute the solution!

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

• Turns out, this generalizes quite nicely…

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

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

CC Theorem 2: Pole-placement to arbitrary

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

The Base: The “Pendulum”:

mb d cos φv̇ + (−mb d2 − I3 )φ̈ + mb d2 sin φ cos φφ̇2 + mb gd sin φ = τL + τR

ans =! Not CC!

• We already know that the unicycle dynamics gets messed up

• The dynamics become

• We now know how to design rather effective controllers using

• We now know how to design rather effective controllers using

• We now know how to design rather effective controllers using

Second idea: Add a notion of how

• What we want to stabilize (drive to zero) is the estimation

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

• Next Lecture: We need to redo what we did for control design

• Need to redo what we did for control design to understand

• Given a discrete time system without inputs

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

x̂˙ = Ax̂ + L(y − C x̂), ė = (A − LC)e

CO Theorem 2: Pole-placement to arbitrary

• Now, how do we put everything together?

u = −K x̂ what we actually have

• This remarkable fact is known as the Separation Principle!

“In theory, theory and practice are the

Das könnte Ihnen auch gefallen

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

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

•  Let’s stop here!

•  How should we pick the eigenvalues? Mix of art and science

•  Do we have to compute large determinants? No!

•  What’s at play here is a lack of “controllability”, i.e., the effect

•  Assuming that we have sufficient control authority to do pole

•  But, the bigger the eigenvalues the bigger

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

•  Turns out, this generalizes quite nicely…

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

•  We already know that the unicycle dynamics gets messed up

•  The dynamics become

•  We now know how to design rather effective controllers using

•  We now know how to design rather effective controllers using

•  We now know how to design rather effective controllers using

•  What we want to stabilize (drive to zero) is the estimation

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

•  Next Lecture: We need to redo what we did for control design

•  Need to redo what we did for control design to understand

•  Given a discrete time system without inputs

•  Now, how do we put everything together?

•  This remarkable fact is known as the Separation Principle!