Real-Time Embedded Convex Optimization. Stephen Boyd joint work with
Michael Grant, Jacob Mattingley, Yang Wang. Electrical Engineering Department
...
Real-Time Embedded Convex Optimization
Stephen Boyd joint work with Michael Grant, Jacob Mattingley, Yang Wang Electrical Engineering Department, Stanford University
ISMP 2009
Outline
• Real-time embedded convex optimization • Examples • Parser/solvers for convex optimization • Code generation for real-time embedded convex optimization
ISMP 2009
1
Embedded optimization
• embed solvers in real-time applications • i.e., solve an optimization problem at each time step • used now for applications with hour/minute time-scales – process control – supply chain and revenue ‘management’ – trading
ISMP 2009
2
What’s new embedded optimization at millisecond/microsecond time-scales millions problem size −→
traditional problems →
thousands
← our focus tens microseconds
seconds
hours days
solve time −→ ISMP 2009
3
Applications • real-time resource allocation – update allocation as objective, resource availabilities change • signal processing – estimate signal by solving optimization problem over sliding window – replace least-squares estimates with robust (Huber, ℓ1) versions – re-design (adapt) coefficients as signal/system model changes • control – closed-loop control via rolling horizon optimization – real-time trajectory planning • all of these done now, on long (minutes or more) time scales but could be done on millisecond/microsecond time scales ISMP 2009
4
Outline
• Real-time embedded convex optimization • Examples • Parser/solvers for convex optimization • Code generation for real-time embedded convex optimization
ISMP 2009
5
Grasp force optimization • choose K grasping forces on object to – resist external wrench (force and torque) – respect friction cone constraints – minimize maximum grasp force • convex problem (second-order cone program or SOCP): maxi kf (i)k2 P (i) (i) ext Q f = f subject to i P (i) (i) (i) ext p × (Q f ) = τ i 1/2 (i) (i)2 (i)2 µifz ≥ fx + fy
minimize
max contact force force equillibrium torque equillibrium friction cone constraints
variables f (i) ∈ R3, i = 1, . . . , K (contact forces) ISMP 2009
6
Example
ISMP 2009
7
Grasp force optimization solve times
• example with K = 5 fingers (grasp points) • reduces to SOCP with 15 vars, 6 eqs, 5 3-dim SOCs • custom code solve time: 50µs (SDPT3: 100ms)
ISMP 2009
8
Robust Kalman filtering • estimate state of a linear dynamical system driven by IID noise • sensor measurements have occasional outliers (failures, jamming, . . . ) • model:
xt+1 = Axt + wt,
yt = Cxt + vt + zt
– wt ∼ N (0, W ), vt ∼ N (0, V ) – zt is sparse; represents outliers, failures, . . . • (steady-state) Kalman filter (for case zt = 0): – time update: x ˆt+1|t = Aˆ xt|t – measurement update: x ˆt|t = x ˆt|t−1 + L(yt − C x ˆt|t−1 ) • we’ll replace measurement update with robust version to handle outliers ISMP 2009
9
Measurement update via optimization • standard KF: x ˆt|t is solution of quadratic problem minimize v T V −1v + (x − x ˆt|t−1)T Σ−1 (x − x ˆt|t−1 ) subject to yt = Cx + v with variables x, v (simple analytic solution) • robust KF: choose x ˆt|t as solution of convex problem minimize v T V −1v + (x − x ˆt|t−1 )T Σ−1 (x − x ˆt|t−1) + λkzk1 subject to yt = Cx + v + z with variables x, v, z (requires solving a QP) ISMP 2009
10
Example
• 50 states, 15 measurements • with prob. 5%, measurement components replaced with (yt)i = (vt)i • so, get a flawed measurement (i.e., zt 6= 0) every other step (or so)
ISMP 2009
11
State estimation error kx − x ˆt|tk2 for KF (red); robust KF (blue); KF with z = 0 (gray)
0.5
ISMP 2009
12
Robust Kalman filter solve time
• robust KF requires solution of QP with 95 vars, 15 eqs, 30 ineqs • automatically generated code solves QP in 120 µs (SDPT3: 120 ms) • standard Kalman filter update requires 10 µs
ISMP 2009
13
Linearizing pre-equalizer • linear dynamical system with input saturation
∗h
y
• we’ll design pre-equalizer to compensate for saturation effects
u
ISMP 2009
equalizer
v
∗h
y
14
Linearizing pre-equalizer
∗h u
e equalizer v
∗h
• goal: minimize error e (say, in mean-square sense) • pre-equalizer has T sample look-ahead capability ISMP 2009
15
• system:
xt+1 = Axt + B sat(vt),
• (linear) reference system:
yt = Cxt
ref xref = Ax t+1 t + But,
ytref = Cxref t
• et = Cxref t − Cxt • state error x ˜t = xref t − xt satisfies x ˜t+1 = A˜ xt + B(ut − vt),
et = C x ˜t
• to choose vt, solve QP Pt+T
2 minimize ˜Tt+T +1P x ˜t+T +1 τ =t eτ + x subject to x ˜τ +1 = A˜ xτ + B(uτ − vτ ), eτ = C x ˜τ , |vτ | ≤ 1, τ = t, . . . , t + T
τ = t, . . . , t + T
P gives final cost; obvious choice is output Grammian ISMP 2009
16
Example
• state dimension n = 3; h decays in around 35 samples • pre-equalizer look-ahead T = 15 samples • input u random, saturates (|ut| > 1) 20% of time
ISMP 2009
17
Outputs desired (black), no compensation (red), equalized (blue) 2.5
0
−2.5
ISMP 2009
18
Errors no compensation (red), with equalization (blue) 0.75
0
−0.75
ISMP 2009
19
Inputs no compensation (red), with equalization (blue) 2.5
0
−2.5
ISMP 2009
20
Linearizing pre-equalizer solve time
• pre-equalizer problem reduces to QP with 96 vars, 63 eqs, 48 ineqs • automatically generated code solves QP in 600µs (SDPT3: 310ms)
ISMP 2009
21
Constrained linear quadratic stochastic control • linear dynamical system:
xt+1 = Axt + But + wt
– xt ∈ Rn is state; ut ∈ U ⊂ Rm is control input – wt is IID zero mean disturbance • ut = φ(xt), where φ : Rn → U is (state feedback) policy • objective: minimize average expected stage cost (Q ≥ 0, R > 0) T −1 1 X T T E xt Qxt + ut Rut J = lim T →∞ T t=0
• constrained LQ stochastic control problem: choose φ to minimize J ISMP 2009
22
Constrained linear quadratic stochastic control • optimal policy has form φ(z) = argmin{v T Rv + E V (Az + Bv + wt)} v∈U
where V is Bellman function – but V is hard to find/describe except when U = Rm (in which case V is quadratic) • many heuristic methods give suboptimal policies, e.g. – projected linear control – control-Lyapunov policy – model predictive control, certainty-equivalent planning ISMP 2009
23
Control-Lyapunov policy • also called approximate dynamic programming, horizon-1 model predictive control • CLF policy is φclf (z) = argmin{v T Rv + E Vclf (Az + Bv + wt)} v∈U
where Vclf : Rn → R is the control-Lyapunov function • evaluating ut = φclf (xt) requires solving an optimization problem at each step • many tractable methods can be used to find a good Vclf • often works really well ISMP 2009
24
Quadratic control-Lyapunov policy • assume – polyhedral constraint set: U = {v | F v ≤ g}, g ∈ Rk – quadratic control-Lyapunov function: Vclf (z) = z T P z • evaluating ut = φclf (xt) reduces to solving QP minimize v T Rv + (Az + Bv)T P (Az + Bv) subject to F v ≤ g
ISMP 2009
25
Control-Lyapunov policy evaluation times • tclf : time to evaluate φclf (z) • tlin: linear policy φlin(z) = Kz • tkf : Kalman filter update • (SDPT3 times around 1000× larger)
ISMP 2009
n
m
k
tclf (µs)
tlin (µs)
tkf (µs)
15
5
10
35
1
1
50
15
30
85
3
9
100
10
20
67
4
40
1000
30
60
298
130
8300
26
Outline
• Real-time embedded convex optimization • Examples • Parser/solvers for convex optimization • Code generation for real-time embedded convex optimization
ISMP 2009
27
Parser/solvers for convex optimization • specify convex problem in natural form – declare optimization variables – form convex objective and constraints using a specific set of atoms and calculus rules (disciplined convex programming) • problem is convex-by-construction • easy to parse, automatically transform to standard form, solve, and transform back • implemented using object-oriented methods and/or compiler-compilers • huge gain in productivity (rapid prototyping, teaching, research ideas) ISMP 2009
28
Example: cvx • parser/solver written in Matlab • convex problem, with variable x ∈ Rn; A, b, λ, F , g constants minimize
kAx − bk2 + λkxk1
subject to F x ≤ g • cvx specification: cvx begin variable x(n) % declare vector variable minimize (norm(A*x-b,2) + lambda*norm(x,1)) subject to F*x