Direct Transcription Using Single-Point Collocation

Eric Kerrigan and Jad Wehbeh

January 29, 2024

1 Continuous-time dynamic optimization problem

Consider the continuous-time dynamic optimization problem in Bolza form (i.e., a dynamic optimization
problem with both Mayer and Lagrange terms)
Z tf
min VM (x(t0 ), x(tf ), θ, t0 , tf ) + l(x(t), u(t), θ, t)dt
x(·),u(·), | {z } t0
θ,t0 ,tf Mayer term | {z }
!
arrival cost if no x(tf ) Lagrange / running cost term
terminal cost if no x(t0 )
subject to

Differential equations { f (ẋ(t), x(t), u(t), θ, t) = 0 ∀t ∈ [t0 , tf ] a.e.

Path Constraints { g(ẋ(t), x(t), u̇(t), u(t), θ, t) ≤ 0 ∀t ∈ [t0 , tf ] a.e.
Boundary Constraints 
  ψE (x (t0 ) , x(tf ), θ, t0 , tf ) = 0
Terminal / final if no x(t0 ) (tf ≥ t0 included if necessary)
ψI (x (t0 ) , x (tf ) , θ, t0 , tf ) ≤ 0
Initial if no x(tf )

Here, t0 is the initial or start time, tf is the final or end time, and tf − t0 is refereed to as the horizon length.
Additionally,

x : R → Rnx continuous state trajectory, piecewise differentiable

u : R → Rnu (discontinuous) free input trajectory, piecewise differentiable
θ ∈ Rnθ vector of (constant) parameters.

f, g, ψE , and ψI are vector-valued functions.

Note that a.e. is defined to mean ‘almost everywhere’.

Problem properties:
• Infinite-dimensional
• Solution non-unique

• Solution discontinuous

Figure 1: Example state and control input

trajectories
1
2 Trajectory parameterization
Approximate trajectories with any suitable parameterization.

Mesh nodes:

M := {t0 , t1 , . . . , tN } ∀i ∈ {1, 2, . . . , N − 1} :
t0 < t1 < . . . < tN = tf ti = t0 + αi (tf − t0 ) =⇒ hi = (αi+1 − αi )(tf − t0 )
hi := ti+1 − ti , h := max hi 0 < α1 < . . . < αN −1 < 1, αi given
i

Uniform mesh ⇐⇒ ti+1 − ti = h ∀i

i 1
αi := ⇐⇒ αi+1 − αi = , ∀i
N N
i
=⇒ ti = t0 + (tf − t0 ), ∀i
N 
tf − t0
⇐⇒ ti = t0 + i , ∀i
N
⇐⇒ ti = t0 + ih, ∀i
tf − t0 tf − t0
and h = ⇐⇒ N =
N h

Consider the simplest possible parameterizations:

• Piecewise-constant (p.w.c.) parameterization (for input):

ũ(t) := qi ∀t ∈ [ti , ti+1 )
p.w.c ≡ zero-order-hold (ZOH)

Figure 2: Piercewise-constant input parameterization

q := (q0 , q1 , . . . , qN −1 ) ∈ RN nu
qi ∈ Rnu , i = 0, 1, . . . , N − 1

• Piecewise affine parameterization (for state) (continuous for state):

si+1 − si
x̃(t) := si + (t − ti ), ∀t ∈ [ti , ti+1 ], i = 0, 1, . . . , N − 1
ti+1 − ti
˙ dx̃ si+1 − si
=⇒ x̃(t) = (t) = , ∀t ∈ (ti , ti+1 ), i = 0, 1, . . . , N − 1
dt ti+1 − ti

2
 Figure 3: Piecewise affine state parameterization

s := (s0 , s1 , . . . , sN ) ∈ RN nx
si ∈ Rnx , i = 0, 1, . . . , N − 1, N

3 Discretizing differential constraints using collocation methods

Original: f (ẋ(t), x(t), u(t), θ, t) = 0, ∀t ∈ [t0 , tf ] a.e. (uncountable set)

˙
Collocation: f (x̃(t), x̃(t), ũ(t), θ, t) = 0, ∀t ∈ Tf ⊂ [t0 , tf ] (∗) (finite set)

Given ũ p.w.c. and x̃ p.w.a, popular choices are:

1. Explicit Euler: Tf = {t+ + + + +

0 , t1 , . . . , ti , . . . , tN −1 }, ti = limτ ↘ti τ
 
−si
(∗) ⇐⇒ f sti+1 i+1 −ti
, si , q i , θ, t i = 0, ∀i ∈ {0, 1, . . . , N − 1}
 
If f (ẋ(t), x(t), u(t), θ, t) = 0 ⇐⇒ ẋ(t) = fe (x(t), u(t), θ, t)
then (∗) ⇐⇒ si+1 = si + (ti+1 − ti )fe (si , qi , θ, ti ), i = 0, 1, . . . , N − 1
Legendre-Gauss-Radau method of first order

2. Implicit Euler: Tf = {t− − − − −

1 , t2 , . . . , ti+1 , . . . , tN }, ti = limτ ↗ti τ
 
i+1 −si
(∗) ⇐⇒ f sti+1 −ti , si+1 , qi , θ, ti+1 = 0, ∀i ∈ {0, 1, . . . , N − 1}
 
If f (ẋ(t), x(t), u(t), θ, t) = 0 ⇐⇒ ẋ(t) = fe (x(t), u(t), θ, t)
then (∗) ⇐⇒ si+1 = si + (ti+1 − ti )fe (si+1 , qi , θ, ti+1 ), i = 0, 1, . . . , N − 1
Legendre-Gauss-Radau method of first order
n o
ti +ti+1 tN −1 +tN
3. Midpoint Rule: Tf = t0 +t 2
1 t1 +t2
, 2 , . . . , 2 , . . . , 2
 
si+1 −si si +si+1 ti +ti+1
(∗) ⇐⇒ f ti+1 −ti , 2 , qi , θ, 2 = 0, ∀i ∈ {0, 1, . . . , N − 1}
Recommended over explicit / implicit Euler because more accurate
Legendre-Gauss-Radau method of second order

3
4 Discretizing path constraints

Original: g(ẋ(t), x(t), u̇(t), u(t), θ, t) ≤ 0, ∀t ∈ [t0 , tf ]

˙
Finite Set: g(x̃(t), ˙
x̃(t), ũ(t), ũ(t), θ, t) ≤ ϵg (t, Tg ) , ∀t ∈ Tg ⊂ [t0 , tf ] (∗∗)

where the vector-valued function ϵg (t, Tg ) ≤ 0 is given / fixed.

Often, Tg = Tf and ϵg = 0, but this is not necessarily the best choice.

−
e.g. if x̃ p.w.a, ũ p.w.c., then could choose Tg = {t+ + +
0 , t1 , . . . , tN −1 , tN }, ϵg = 0
  
g si+1 −si , si , 0, qi , θ, ti ≤ 0 ∀i ∈ {0, 1, . . . , N − 1}
t −t
 i+1 i
(∗∗) ⇐⇒ 
g sN −sN −1 , sN , 0, qN −1 , θ, tN ≤ 0
tN −tN −1

5 Boundary constraints

ψE (x (t0 ) , x(tf ), θ, t0 , tf ) = 0
ψI (x (t0 ) , x (tf ) , θ, t0 , tf ) ≤ 0
⇓
 
ψE (x̃ (t0 ) , x̃(tf ), θ, t0 , tf ) = 0 ψE (s0 , sN , θ, t0 , tf ) = 0
⇐⇒
ψI (x̃ (t0 ) , x̃ (tf ) , θ, t0 , tf ) ≤ 0 x̃ p.w.a. ψI (s0 , sN , θ, t0 , tf ) ≤ 0

In many optimal control problems we are given an estimate / measurement of the state at x̂ ∈ Rnx at
the current time t0 . In this case, the constraint

x(t0 ) = x̂ =⇒ s0 = x̂
x̃ p.w.a.

is included in the definition of ψE .

6 Cost function

Original:
Z tf
VM (x(t0 ), x(tf ), θ, t0 , tf ) + l(x(t), u(t), θ, t)dt ≡ J
t0

Approximate with numerical integration (quadrature). E.g. can use trapezoidal rule:

N
X −1
=⇒ J ≈ VM (x̃(t0 ), x̃(tf ), θ, t0 , tf ) + Li (x̃(ti ), x̃(ti+1 ), ũ(t+
i ), θ, ti , ti+1 )dt
i=0

4
 Figure 4: Cost function approximation using trapezoidal rule

Area under ith trapezoid:

" #
−
l(x̃(ti ), ũ(t+
i ), θ, ti ) + l(x̃(ti+1 ), ũ(ti+1 ), θ, ti+1 )
Li = (ti+1 − ti )
2

E.g. if x̃ is p.w.a and ũ is p.w.c.

ti+1 − ti
=⇒ Li (si , si+1 , qi , θ, ti , ti+1 ) = (l(si , qi , θ, ti ) + l(si+1 , qi , θ, ti+1 ))
2
VM (x̃(t0 ), x̃(tf ), θ, t0 , tf ) = VM (s0 , sN , θ, t0 , tf )

7 Discretized dynamics optimization problem

x̃ : p.w.a
ũ : p.w.c
Quadrature : trapezoidal
Collocation : mid-point
Path constraints : mesh points

N
X −1
=⇒ s0 1
min
, s , ..., s
VM (s0 , sN , θ, t0 , tf ) + Li (si , si+1 , qi , θ, ti , ti+1 ) =: F (ω)
N,
q0 , q1 , ..., qN −1 , i=0
θ, t0 , tf

subject to

 
si+1 − si si + si+1 ti + ti+1
f , , qi , θ, = 0, ∀i ∈ {0, 1, . . . , N − 1}
ti+1 − ti 2 2
 
si+1 − si
g , si , 0, qi , θ, ti ≤ 0 ∀i ∈ {0, 1, . . . , N − 1}
ti+1 − ti
 
si+1 − si
g , sN , 0, qN −1 , θ, tN ≤ 0
ti+1 − ti
ψE (s0 , sN , θ, t0 , tf ) = 0
ψI (s0 , sN , θ, t0 , tf ) ≤ 0

5
Remember to substitute ti = t0 + αi (tf − t0 ) everywhere =⇒ ti+1 − ti = (αi+1 − αi )(tf − t0 )
 
t −t t −t
Uniform mesh =⇒ ti+1 − ti = f N 0 = h, ∀i and ti = t0 + i f N 0 , ∀i.

Formulate as a finite-dimensional optimization problem:

min F (ω)
ω

s.t.

G(ω) = 0
H(ω) ≤ 0

where

 
s0



 q0



 s1
F (ω) := as above 


 q1
  ..
ω := 
 
 .
sN −1 
 
qN −1 
 
 sN 
 
 t0 
tf

   
s1 −s0
g t1 −t0 , s0 , 0, q0 , θ, t0
   
..
 
s1 −s0 s0 +s1 t0 +t1
f t1 −t0 , 2 , q0 , θ, 2

 .


..
     
  si+1 −si
g ti+1 −ti , si , 0, qi , θ, ti
 

  . 

  
 
si+1 −si si +si+1 ti +ti+1 H(ω) :=  .
f ti+1 −ti , , q , θ, ..
  
2 i 2
G(ω) := 
   

.
   

..
  sN −sN −1
g tN −tN −1 , sN −1 , 0, qN −1 , θ, tN −1 
  
     
 sN −sN −1 sN −1 +sN tN −1 +tN 

s −sN −1

f tN −tN −1 , , q , θ,  g tN , s , 0, q , θ, t
 
2 N −1 2 N N −1 N
N −tN −1
 
ψE (s0 , sN , θ, t0 , tf ) ψI (s0 , sN , θ, t0 , tf )

Contributors
• Lester Shi
• Hua Tu