Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive...
Transcript of Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive...
![Page 1: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/1.jpg)
1
Sanjiban Choudhury
Iterative LQR & Model Predictive Control
TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle
Content from Drew Bagnell, Pieter Abeel
![Page 2: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/2.jpg)
Table of Controllers
2
Control Law Uses model Stability Guarantee
Minimize Cost
PID No No No
Pure Pursuit Circular arcs Yes - with assumptions No
Lyapunov Non-linear Yes No
LQR Linear Yes Quadratic
![Page 3: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/3.jpg)
Can we use LQR to swing up a pendulum?
3✓ = ⇡
✓ = 0
![Page 4: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/4.jpg)
Can we use LQR to swing up a pendulum?
3✓ = ⇡
✓ = 0
No! (Large angles imply
large linearization error)
![Page 5: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/5.jpg)
Can we use LQR to swing up a pendulum?
3✓ = ⇡
✓ = 0
No! (Large angles imply
large linearization error)
But we can track a reference swing up trajectory
(small linearization error)
![Page 6: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/6.jpg)
4
But, first we need to talk about time-varying systems
![Page 7: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/7.jpg)
Today’s objectives
5
1. LQR for time-varying systems
2. Trajectory following with iLQR
3. General nonlinear trajectory optimization with iLQR
4. Model predictive control (MPC)
![Page 8: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/8.jpg)
LQR for Time-Varying Dynamical Systems
6
xt+1 = Atxt +Btut
![Page 9: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/9.jpg)
LQR for Time-Varying Dynamical Systems
6
xt+1 = Atxt +Btut
c(xt, ut) = x
Tt Qtxt + u
Tt Rtut
![Page 10: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/10.jpg)
LQR for Time-Varying Dynamical Systems
6
xt+1 = Atxt +Btut
c(xt, ut) = x
Tt Qtxt + u
Tt Rtut
Straight forward to get LQR equations
Kt = �(Rt +BTt Vt+1Bt)
�1BTt Vt+1At
Vt = Qt +KTt RtKt + (At +BtKt)
TVt+1(At +BtKt)
![Page 11: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/11.jpg)
7
Why do we care about time-varying?
Ans: Linearization about a trajectory
![Page 12: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/12.jpg)
Trajectory tracking for stationary rolls?
8
![Page 13: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/13.jpg)
Trajectory tracking for stationary rolls?
8
![Page 14: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/14.jpg)
−10
1 −2
0
2
−3
−2
−1
0
1
2
3
Easting (m)Northing (m)
Dow
n (
m)
9
How do we get such behaviors?
−10
−5
0
5
−6−4
−20
24
6
−2
−1
0
1
2
Easting (m)Northing (m)
Dow
n (m
)
Nose-in funnel Stationary rolls
Figure 1: (Best viewed in color.) (a) Series of snapshots throughout an autonomous flip. (b) Series of snapshotsthroughout an autonomous roll. (c) Overlay of snapshots of the helicopter throughout a tail-in funnel. (d)Overlay of snapshots of the helicopter throughout a nose-in funnel. (See text for details.)
Figure 1: (Best viewed in color.) (a) Series of snapshots throughout an autonomous flip. (b) Series of snapshotsthroughout an autonomous roll. (c) Overlay of snapshots of the helicopter throughout a tail-in funnel. (d)Overlay of snapshots of the helicopter throughout a nose-in funnel. (See text for details.)
![Page 15: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/15.jpg)
Task: Minimize tracking error
10
minu0,u1,...,uT�1
T�1X
t=0
c(xt, ut)
subject to xt+1 = f(xt, ut) 8t
In this scenario, cost is simply a quadratic tracking cost
Why is this a hard optimization problem?
![Page 16: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/16.jpg)
11
Iterative LQR (iLQR)
i=0 i=10 i=100
Start by guessing a control sequence, Forward simulate dynamics, Linearize about trajectory, Solve for new control sequence
and repeat!
![Page 17: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/17.jpg)
Step 1: Get a reference trajectory
12
6
4
22
1
5 0
0
-1
-2
-2
Down (m)
0 -4
-6-5
Note: Simply executing open loop trajectory wont work!
x
ref0 , u
ref0 , x
ref1 , u
ref1 , . . . , x
refT�1, u
refT�1
![Page 18: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/18.jpg)
Step 2: Initialize your algorithm
13
Choose initial trajectory at iteration 0 to linearize about
x
0(t), u0(t) = {x00, u
00, x
01, u
01, . . . , x
0T�1, u
0T�1}
![Page 19: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/19.jpg)
Step 2: Initialize your algorithm
13
Choose initial trajectory at iteration 0 to linearize about
x
0(t), u0(t) = {x00, u
00, x
01, u
01, . . . , x
0T�1, u
0T�1}
It’s a good idea to choose the reference trajectory
![Page 20: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/20.jpg)
Step 2: Initialize your algorithm
13
Choose initial trajectory at iteration 0 to linearize about
x
0(t), u0(t) = {x00, u
00, x
01, u
01, . . . , x
0T�1, u
0T�1}
It’s a good idea to choose the reference trajectory
Initialization is very important! We will be perturbing this initial trajectory
![Page 21: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/21.jpg)
Step 3: Linearize your dynamics!
14
At a given iteration i, we are going to linearize about
x
i0, u
i0, x
i1, . . .
![Page 22: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/22.jpg)
Step 3: Linearize your dynamics!
14
At a given iteration i, we are going to linearize about
x
i0, u
i0, x
i1, . . .
Change of variable - we will track the delta perturbations
�xt = xt � x
it
�ut = ut � uit
![Page 23: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/23.jpg)
Step 3: Linearize your dynamics!
15
Reference Trajectory
Linearization Trajectory
x
it
![Page 24: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/24.jpg)
16
Step 3: Linearize your dynamics!�xt = xt � x
it �ut = ut � ui
t
![Page 25: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/25.jpg)
16
Step 3: Linearize your dynamics!�xt = xt � x
it �ut = ut � ui
t
�xt+1 = At�xt +Bt�ut + (f(xit, u
it)� x
it+1)
A
t
=@f
@x
�����x
it
Bt =@f
@u
�����uit
![Page 26: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/26.jpg)
16
Step 3: Linearize your dynamics!�xt = xt � x
it �ut = ut � ui
t
�xt+1 = At�xt +Bt�ut + (f(xit, u
it)� x
it+1)
A
t
=@f
@x
�����x
it
Bt =@f
@u
�����uit
This is an affine system, not linear
![Page 27: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/27.jpg)
17
Step 3: Linearize your dynamics!
Homogenous coordinate system
�xt
1
�
�xt+1
1
�=
At+1 f(xi
t, uit)� x
it+1
0 1
� �xt
1
�+
Bt+1
0
� �ut
0
�
Affine dynamics is now linear!
At Bt
�ut
![Page 28: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/28.jpg)
18
Step 4: Quadricize cost about trajectoryOur cost function is already quadratic, otherwise we would apply
Taylor expansion
c(xt, ut) = (xt � x
reft )TQ(xt � x
reft ) + (ut � u
reft )TR(ut � u
reft )
![Page 29: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/29.jpg)
18
Step 4: Quadricize cost about trajectoryOur cost function is already quadratic, otherwise we would apply
Taylor expansion
c(xt, ut) = (xt � x
reft )TQ(xt � x
reft ) + (ut � u
reft )TR(ut � u
reft )
=
�xt
1
�T Q Q(xi
t � x
reft )
(xit � x
reft )TQ (xi
t � x
reft )T (xi
t � x
reft )
� �xt
1
�
+
�ut
1
�T R R(ui
t � ureft )
(uit � uref
t )TR (uit � uref
t )T (uit � uref
t )
� �ut
1
�
Qt
Rt
![Page 30: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/30.jpg)
We have all the ingredients to call LQR!
19
Kt = �(Rt + BTt Vt+1Bt)
�1BTt Vt+1At
similarly calculate the value function …
![Page 31: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/31.jpg)
20
Step 5: Do a backward pass
KT�1, VT�1
KT�2, VT�2
KT�3, VT�3
K0, V0
Calculate controller gains for all time steps
![Page 32: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/32.jpg)
21
Step 6: Do a forward pass to get new trajectory
x
i+1t = f(xi+1
t , u
i+1t )
Compute control action
u
i+1t = u
it + Kt
x
i+1t � x
it
1
� Apply dynamics
![Page 33: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/33.jpg)
21
Step 6: Do a forward pass to get new trajectory
x
i+1t = f(xi+1
t , u
i+1t )
x
i+10 = x
i0
ui+10 = ui
0 + Kt
01
�
Compute control action
u
i+1t = u
it + Kt
x
i+1t � x
it
1
� Apply dynamics
![Page 34: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/34.jpg)
22
Step 6: Do a forward pass to get new trajectory
Compute control action
x
i+1t = f(xi+1
t , u
i+1t )
Apply dynamicsu
i+1t = u
it + Kt
x
i+1t � x
it
1
�
(xi+11 , u
i+11 )
(xi1, u
i1)
![Page 35: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/35.jpg)
23
Step 6: Do a forward pass to get new trajectory
Compute control action
x
i+1t = f(xi+1
t , u
i+1t )
Apply dynamicsu
i+1t = u
it + Kt
x
i+1t � x
it
1
�
(xi2, u
i2)
(xi+12 , u
i+12 )
![Page 36: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/36.jpg)
24
Step 6: Do a forward pass to get new trajectory
Compute control action
x
i+1t = f(xi+1
t , u
i+1t )
Apply dynamicsu
i+1t = u
it + Kt
x
i+1t � x
it
1
�
![Page 37: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/37.jpg)
25
Problem: Forward pass will go bonkersWhy?
![Page 38: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/38.jpg)
25
Problem: Forward pass will go bonkersWhy?
Linearization error gets bigger and bigger and bigger
![Page 39: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/39.jpg)
25
Problem: Forward pass will go bonkersWhy?
Linearization error gets bigger and bigger and bigger
Remedies: Change cost function to penalize deviation from linearization
![Page 40: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/40.jpg)
Questions
26
![Page 41: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/41.jpg)
Questions
26
1. Can we solve LQR for continuous time dynamics?
![Page 42: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/42.jpg)
Questions
26
1. Can we solve LQR for continuous time dynamics?
Yes! Refer to Continuous Algebraic Ricatti Equations (CARE)
![Page 43: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/43.jpg)
Questions
26
1. Can we solve LQR for continuous time dynamics?
Yes! Refer to Continuous Algebraic Ricatti Equations (CARE)
2. Can LQR handle arbitrary costs (not just tracking)?
![Page 44: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/44.jpg)
Questions
26
1. Can we solve LQR for continuous time dynamics?
Yes! Refer to Continuous Algebraic Ricatti Equations (CARE)
2. Can LQR handle arbitrary costs (not just tracking)?
Yes! Just quadricize the cost
![Page 45: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/45.jpg)
Questions
26
1. Can we solve LQR for continuous time dynamics?
Yes! Refer to Continuous Algebraic Ricatti Equations (CARE)
2. Can LQR handle arbitrary costs (not just tracking)?
Yes! Just quadricize the cost
3. What if I want to penalize control derivatives?
![Page 46: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/46.jpg)
Questions
26
1. Can we solve LQR for continuous time dynamics?
Yes! Refer to Continuous Algebraic Ricatti Equations (CARE)
2. Can LQR handle arbitrary costs (not just tracking)?
Yes! Just quadricize the cost
3. What if I want to penalize control derivatives?
No problem! Add control as part of state space
![Page 47: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/47.jpg)
Questions
26
1. Can we solve LQR for continuous time dynamics?
Yes! Refer to Continuous Algebraic Ricatti Equations (CARE)
2. Can LQR handle arbitrary costs (not just tracking)?
Yes! Just quadricize the cost
3. What if I want to penalize control derivatives?
No problem! Add control as part of state space
4. Can we handle noisy dynamics?
![Page 48: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/48.jpg)
Questions
26
1. Can we solve LQR for continuous time dynamics?
Yes! Refer to Continuous Algebraic Ricatti Equations (CARE)
2. Can LQR handle arbitrary costs (not just tracking)?
Yes! Just quadricize the cost
3. What if I want to penalize control derivatives?
No problem! Add control as part of state space
4. Can we handle noisy dynamics?
Yes! Gaussian noise does not change the answer
![Page 49: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/49.jpg)
Table of Controllers
27
Control Law Uses model Stability Guarantee
Minimize Cost
PID No No No
Pure Pursuit Circular arcs Yes - with assumptions No
Lyapunov Non-linear Yes No
LQR Linear Yes Quadratic
iLQR Non-linear Yes Yes
![Page 50: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/50.jpg)
28
iLQR is just one technique
It’s far from perfect - can’t deal with model errors / constraints …
Model Predictive Control (MPC)
iLQR
DDP
Shooting
CMAESTrajLib
MPPI
![Page 51: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/51.jpg)
Recap: Feedback control framework
29
Look at current state error and compute control actions
Reference
![Page 52: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/52.jpg)
Recap: Feedback control framework
29
Look at current state error and compute control actions
xt
x
reft
ut = ⇡(xt, xreft )
Reference
![Page 53: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/53.jpg)
Recap: Feedback control framework
29
Look at current state error and compute control actions
xt
x
reft
ut = ⇡(xt, xreft )
ut+1 = ⇡(xt+1, xreft+1)
x
reft+1
xt+1
Reference
![Page 54: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/54.jpg)
Recap: Feedback control framework
29
Look at current state error and compute control actions
Goal: To drive error to 0 … to optimally drive it to 0
xt
x
reft
ut = ⇡(xt, xreft )
ut+1 = ⇡(xt+1, xreft+1)
x
reft+1
xt+1
Reference
![Page 55: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/55.jpg)
Limitations of this framework
30
ut = ⇡(xt, xreft )
A fixed control law that looks at instantaneous feedback
Fixed Reference
Why is it so difficult to create a magic control law?
![Page 56: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/56.jpg)
Problem 1: What if we have constraints?
31
Simple scenario: Car tracking a straight line
![Page 57: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/57.jpg)
Problem 1: What if we have constraints?
31
Simple scenario: Car tracking a straight line
Small error, control within
steering constraints
![Page 58: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/58.jpg)
Problem 1: What if we have constraints?
31
Simple scenario: Car tracking a straight line
Small error, control within
steering constraints
Large error, control violates
steering constraints
![Page 59: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/59.jpg)
Problem 1: What if we have constraints?
31
Simple scenario: Car tracking a straight line
Small error, control within
steering constraints
Large error, control violates
steering constraints
We could “clamp control command” … but what are the implications?
![Page 60: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/60.jpg)
General problem: Complex models
32
xt+1 = f(xt, ut)
g(xt, ut) 0
Dynamics
Constraints
Such complex models imply we need to:
1. Predict the implications of control actions
2. Do corrections NOW that would affect the future
3. It may not be possible to find one law - might need to predict
![Page 61: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/61.jpg)
Example: Rough terrain mobility
33
![Page 62: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/62.jpg)
Example: Rough terrain mobility
33
![Page 63: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/63.jpg)
Problem 2: What if some errors are worse than others?
34
We need a cost function that penalizes states non-uniformly
![Page 64: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/64.jpg)
35
Key Idea:
Frame control as an optimization problem
![Page 65: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/65.jpg)
Model predictive control (MPC)
36
1. Plan a sequence of control actions
2. Predict the set of next states unto a horizon H
3. Evaluate the cost / constraint of the states and controls
4. Optimize the cost
![Page 66: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/66.jpg)
Model predictive control (MPC)
37
xk+1 = f(xk, uk+1)
minut+1,...ut+H
t+H�1X
k=t
J(xk, uk+1)
g(xk, uk+1) 0
(Predict next state with dynamics)
(Constraints)
(Cost)(plan till horizon H)
![Page 67: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/67.jpg)
Model predictive control (MPC)
38
xk+1 = f(xk, uk+1)
minut+1,...ut+H
t+H�1X
k=t
J(xk, uk+1)
g(xk, uk+1) 0
ut
ut+1
ut+2
ut+3ut+4
xt
xt+1
xt+2
xt+3
xt+4
Jt Jt+1
Jt+2Jt+3
Jt+4
![Page 68: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/68.jpg)
How are the controls executed?
39
Step 1: Solve optimization problem to a horizon
xt
xt+1
xt+2
xt+3
xt+4
![Page 69: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/69.jpg)
How are the controls executed?
40
xt xt+1
Step 2: Execute the first control
Step 1: Solve optimization problem to a horizon
![Page 70: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/70.jpg)
How are the controls executed?
41
xt xt+1
Step 3: Repeat!
xt+2
xt+3
xt+4
xt+5
Step 2: Execute the first control
Step 1: Solve optimization problem to a horizon
![Page 71: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/71.jpg)
MPC is a framework
42
1 1. Concepts 1.1 Main Idea
Model Predictive Control
P(s)%
Objectives Model Constraints
Plant Optimizer
Measurements
Output Input Reference
Objectives Model Constraints
Plan Do
Plan Do
Plan Do Time
Receding horizon strategy introduces feedback.
MPC Part I – Introduction F. Borrelli, C. Jones, M. Morari - Fall Semester 2014 (revised August 2014) 1-4
1 1. Concepts 1.2 Classical Control vs MPC
Table of Contents
1. Concepts1.1 Main Idea1.2 Classical Control vs MPC1.3 Mathematical Formulation
MPC Part I – Introduction F. Borrelli, C. Jones, M. Morari - Fall Semester 2014 (revised August 2014)
Step 3: Repeat!
Step 2: Execute the first control
Step 1: Solve optimization problem to a horizon
![Page 72: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/72.jpg)
Why do we need to replan?
43
What happens if the controls are planned once and executed?
![Page 73: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/73.jpg)
Why do we need to replan?
44
What happens if the controls are planned once and executed?
![Page 74: Iterative LQR & Model Predictive Control1 Sanjiban Choudhury Iterative LQR & Model Predictive Control TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle Content from Drew Bagnell, Pieter](https://reader030.fdocuments.us/reader030/viewer/2022040302/5e7ed385707cf3599e6c851d/html5/thumbnails/74.jpg)
Model predictive control (MPC)
45
xk+1 = f(xk, uk+1)
minut+1,...ut+H
t+H�1X
k=t
J(xk, uk+1)
g(xk, uk+1) 0
(Predict next state with dynamics)
(Constraints)
(Cost)(plan till horizon H)