Hermite Simpson collocation is a type of direct method used to solve optimal control problems. The state and control variables are approximated as piecewise polynomials which leads to a sparse optimization problem.

The mains steps in Hermite Simpson collocation are summarized as follows.

  • time is divided into $N$ intervals
  • Simpson quadrature is used to evaluate the definite integrals
  • the derivative of the state vector ($x$) is approximated as a piecewise-quadratic polynomial
  • the states are piecewise cubic polynomials
  • the control inputs ($u$) can be parameterized as piecewise constant or linear polynomial
  • the integral in the objective function will use the composite form of the Simpson quadrature.
  • the path constrains which may arise from the optimal control problem must be enforced at the collocation point in addition to the interval boundaries.
  • results in a sparse optimization

Let the dynamics of the system be given in state space form as $\dot{x}=f(x,u)$. On the $i^{th}$ interval $t \in [t_i, t_{i+1}]$, integrate the dynamics.

$\int_{t_i}^{t_{i+1}}\dot{x}dt=\int_{t_i}^{t_{i+1}}f~\mathrm{d}t$

$x_{i+1}-x_i=\int_{t_i}^{t_{i+1}}f~\mathrm{d}t$

Approximating the integral using Hermite Simpson integration yields,

$\int_{t_i}^{t_{i+1}}fdt=\dfrac{h}{6}\left(f_i+4f_{i+\frac{1}{2}}+f_{i+1}\right)$

where $f_i=f(x_i,u_i)$, $f_{i+1}=f(x_{i+1},u_{i+1})$, $h=\frac{t_{N+1}-t_{1}}{N}=t_{i+1}-t_i$ and $i+\frac{1}{2}$ is the midpoint of the interval.

This leads to the discrete form of state space equation,

$x_{i+1}-x_i=\dfrac{h}{6}\left(f_i+4f_{i+\frac{1}{2}}+f_{i+1}\right)$

Note that the value of the state derivative at the midpoint is still a unknown. The terms at the boundaries are the optimization variables. The state and control at midpoint can be included as additional variables in the optimization problem along with with additional constraints or by considering the polynomial approximations, the value of the state, state derivative and control can be computed at midpoint too.

On the $i^{th}$ interval let the state be represented as

$x(\tau) \approx a\tau^3+b\tau^2+c\tau+d$

Note that for $\tau \in [0,h]$, the jacobian for this transformation is identity. The state and its derivative is evaluated at the boundaries on each interval to determine the coefficients of the cubic polynomial.

$x(0)=a0^3+b0^2+c0+d$
$x(h)=ah^3+bh^2+ch+d$
$\dot{x}(0)=3a0^2+2b0+c$
$\dot{x}(h)=3ah^2+2bh+c$

From the above equations, $c=\dot{x}(0)$ and $d=x(0)$.

\[\begin{bmatrix} h^3 &h^2\\ 3h^2 &2h \end{bmatrix} \begin{bmatrix} a\\ b \end{bmatrix} =\begin{bmatrix} x(h)-\dot{x}(0)h-x(0)\\ \dot{x}(h)-\dot{x}(0) \end{bmatrix}= \begin{bmatrix} x_{i+1}-f_ih-x_i\\ f_{i+1}-f_{i} \end{bmatrix}\] \[\begin{bmatrix} a\\ b \end{bmatrix}=\dfrac{-1}{h^4} \begin{bmatrix} 2h &-h^2\\ -3h^2 &h^3 \end{bmatrix} \begin{bmatrix} x_{i+1}-f_ih-x_i\\ f_{i+1}-f_{i} \end{bmatrix}\]

The polynomial is a function which is dependent solely on the values at the boundaries. This allows to calculate the value of the state and its derivative at all other points. At the midpoint, the state can be expressed as

\[\begin{aligned} x(\frac{h}{2}) &=x_{i+\frac{1}{2}}\\ &=a\dfrac{h^3}{8}+b\dfrac{h^2}{4}+c\dfrac{h}{2}+d\\ &= \begin{bmatrix} \dfrac{h^3}{8}&\dfrac{h^2}{4} \end{bmatrix} \begin{bmatrix} a\\ b \end{bmatrix} + f_i\dfrac{h}{2}+x_i\\ &= \begin{bmatrix} \dfrac{h^3}{8}&\dfrac{h^2}{4} \end{bmatrix} \dfrac{-1}{h^4} \begin{bmatrix} 2h &-h^2\\ -3h^2 &h^3 \end{bmatrix} \begin{bmatrix} x_{i+1}-f_ih-x_i\\ f_{i+1}-f_{i} \end{bmatrix} + f_i\dfrac{h}{2}+x_i\\ &= \dfrac{-h^4}{h^4} \begin{bmatrix} (\dfrac{1}{4}-\dfrac{3}{4})&(\dfrac{-1}{8}+\dfrac{1}{4}) \end{bmatrix} \begin{bmatrix} x_{i+1}-f_ih-x_i\\ f_{i+1}-f_{i} \end{bmatrix} + f_i\dfrac{h}{2}+x_i\\ &= \begin{bmatrix} \dfrac{1}{2}&\dfrac{h}{8} \end{bmatrix} \begin{bmatrix} x_{i+1}-f_ih-x_i\\ f_{i+1}-f_{i} \end{bmatrix} + f_i\dfrac{h}{2}+x_i\\ &= \dfrac{x_{i+1}}{2}-\dfrac{f_ih}{2}-\dfrac{x_i}{2}+ \dfrac{f_{i+1}h}{8}-\dfrac{f_{i}h}{8} + f_i\dfrac{h}{2}+x_i\\ &= \dfrac{1}{2}(x_{i+1}+x_i)+\dfrac{h}{8}(f_{i+1}-f_i) \end{aligned}\]

This along with value of control input at midpoint can be substituted in the collocation equation. The control at $i+\frac{1}{2}$ is either constant or linear. Appropriate interpolation rules are used to compute its value.

The equation corresponding to $x_{i+\frac{1}{2}}$ can be substituted in the quadrature equations, in which case it is referred to as compressed form. Alternatively, the state at midpoint can be included in the optimization problem as additional decision variables. To prevent few degrees of freedom issue that may arise in the optimization solver, additional equality constraints must be added. In this case it is referred to as uncompressed form.