【Optimal Control (CMU 16-745)】Lecture 6 Deterministic Optimal Control Introduction

Review:

• Constrained optimization
• Augmented Lagrangian
• Merit functions/line search

Lecture 6 Deterministic Optimal Control Introduction

Overview

• Control history
• Deterministic optimal control
• Pontryagin’s minimum principle
• Linear-quadratic regulator (LQR)

1. Control History

(1) Brachistochrone Problem

The first trajectory optimization problem.
$$\underset{x(t)}{\min }T={\int }_{P_0}^{P_f}\frac{1}{v}\mathrm{d}s={\int }_{x_0}^{x_f}\frac{\sqrt{1+(\mathrm{d}y/\mathrm{d}x{)}^2}}{\sqrt{2gy}}\mathrm{d}x$$

(2) Calculus of Variations

$$\underset{x(t)}{\min }J\left(x(t)\right)={\int }_{t_0}^{t_f}L\left(t,x(t),\dot{x}(t)\right)\mathrm{d}t$$

Lots of Applications:

• Statics(Catenary)
• Finite-Element Methods (Weak formulation)
• Optics(Fermat’s Principle)
• General Relativity (Einstein-Hilbert Action)
• Classical Mechanics(Hamilton’s Principle)
• Quantum Mechanics(Feynman Path Integral)

(3) Feedback Systems

• “Classical Control” (1910-1960)
• “Modern Control” (Post-1960)
• Adaptive Control and RL (1950s-Present)
• Robust Control (1980s-Present)
• Model Predictive Control (1970s-Present)
• Robotic Manipulators (1970s-1980s)
• Legged Robots (1980s-Present)

(4) Challenges for the Future?

• General theory for dealing with contact
• Bridging the gap between model-based control and RL
• Making RL more data efficient by incorporating prior knowledge
• Safety guarantees for uncertain nonlinear systems
• Dealing with other (possibly adversarial) agents

2. Deterministic Optimal Control

(1) Continuous-time formulation

$$\begin{array}{rl}& \underset{\mathbf{x}(t),\mathbf{u}(t)}{\min }J\left(\mathbf{x}(t),\mathbf{u}(t)\right)={\int }_{t_0}^{t_f}\ell \left(\mathbf{x}(t),\mathbf{u}(t)\right)\mathrm{d}t+{\ell }_F\left(\mathbf{x}(t_f)\right)\\ & \text{s.t.}\dot{\mathbf{x}}(t)=f\left(\mathbf{x}(t),\mathbf{u}(t)\right),\text{(possibly other constraints)}\end{array}$$

• $\mathbf{x}(t)$: state trajectory
• $\mathbf{u}(t)$: control trajectory
• $J\left(\mathbf{x}(t),\mathbf{u}(t)\right)$: cost function
• $\ell \left(t,\mathbf{x}(t),\mathbf{u}(t)\right)$: stage cost
• ${\ell }_F\left(\mathbf{x}(t_f)\right)$: terminal cost
• $f\left(\mathbf{x}(t),\mathbf{u}(t)\right)$: dynamics constraints

This is an infinite-dimensional optimization problem in the following sense:

$\mathbf{u}(t)={\lim }_{N\to \infty }{\mathbf{u}}_{1:N}$

• Solutions are open-loop trajectories.
• There are a handful of problems with analytic solutions but not many.
• We will focus on the discrete-time setting which leads to tractable algorithms.

(2) Discrete-time formulation

$$\begin{array}{rl}& \underset{{\mathbf{x}}_{1:N},{\mathbf{u}}_{1:N-1}}{\min }J\left({\mathbf{x}}_{1:N},{\mathbf{u}}_{1:N-1}\right)=\sum _{k=1}^{N-1}\ell \left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+{\ell }_F\left({\mathbf{x}}_N\right)\\ & \text{s.t.}{\mathbf{x}}_{k+1}=f\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\\ & {\mathbf{u}}_{\min }\le {\mathbf{u}}_k\le {\mathbf{u}}_{\max }\\ & \mathbf{c}({\mathbf{x}}_k)\le 0\end{array}$$

• ${\mathbf{u}}_{\min }\le {\mathbf{u}}_k\le {\mathbf{u}}_{\max }$: : torque limits

• $\mathbf{c}({\mathbf{x}}_k)\le 0$: obstacle/safety constraints

• This is a finite-dimensional optimization problem.

• Samples ${\mathbf{x}}_k$, ${\mathbf{u}}_k$ are called knot points.

• Continuous -> discrete-time using integration (e.g. Runge-Kutta)

• Discrete-time -> continuous using interpolation (e.g. cubic splines)

3. Pontryagin’s Minimum Principle

• It is also called “maximum principle” if you maximize a reward.
• First-order necessary conditions for deterministic optimal control problems.
• In discrete-time, just special case of KKT conditions.

Given
$$\begin{array}{rl}& \underset{{\mathbf{x}}_{1:N},{\mathbf{u}}_{1:N-1}}{\min }\sum _{k=1}^{N-1}\ell \left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+{\ell }_F\left({\mathbf{x}}_N\right)\\ & \text{s.t.}{\mathbf{x}}_{k+1}=f\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\end{array}$$

We can form the Lagrangian
$$\mathcal{L}=\sum _{k=1}^{N-1}\left[\ell \left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+{\lambda }_{k+1}^{\top }\left(f\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)-{\mathbf{x}}_{k+1}\right)\right]+{\ell }_F\left({\mathbf{x}}_N\right)$$

This result is usually stated in terms of the Hamiltonian:
$$\mathcal{H}(\mathbf{x},\mathbf{u},\lambda )=\ell (\mathbf{x},\mathbf{u})+{\lambda }^{\top }f(\mathbf{x},\mathbf{u})$$

Plug it into $\mathcal{L}$, we have
$$\mathcal{L}=\mathcal{H}({\mathbf{x}}_1,{\mathbf{u}}_1,{\lambda }_2)+\left[\sum _{k=2}^{N-1}\mathcal{H}({\mathbf{x}}_k,{\mathbf{u}}_k,{\lambda }_{k+1})-{\lambda }_k^{\top }{\mathbf{x}}_k\right]+{\ell }_F({\mathbf{x}}_N)-{\lambda }_N^{\top }{\mathbf{x}}_N$$

Take derivatives w.r.t. $\mathbf{x}$ and $\lambda$, we have
$$\frac{\partial \mathcal{L}}{\partial {\lambda }_k}=\frac{\partial \mathcal{H}}{\partial {\lambda }_k}-{\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{u}}_{k-1})-{\mathbf{x}}_k=0$$ (namely the dynamics constraints)

$$\frac{\partial \mathcal{L}}{\partial {\mathbf{x}}_k}=\frac{\partial \mathcal{H}}{\partial {\mathbf{x}}_k}-{\lambda }_k=\frac{\partial \ell }{\partial {\mathbf{x}}_k}+{\lambda }_{k+1}^{\top }\frac{\partial f}{\partial {\mathbf{x}}_k}-{\lambda }_k^{\top }=0$$ (for $k=1,\cdots ,N-1$)

For the $N$-th state, we have
$$\frac{\partial \mathcal{L}}{\partial {\mathbf{x}}_N}=\frac{\partial {\ell }_F}{\partial {\mathbf{x}}_N}-{\lambda }_N^{\top }=0$$

For $\mathbf{u}$, we write the min explicitly to handle torque limits:
$$\begin{array}{rl}& u_k=\underset{\tilde{\mathbf{u}}}{\mathrm{argmin}}\mathcal{H}({\mathbf{x}}_k,\tilde{\mathbf{u}},{\lambda }_{k+1})\\ & \text{s.t.}\tilde{\mathbf{u}}\in \mathcal{U}(\text{shorthand for "in feasible set"})\end{array}$$

In summary:
$$\begin{array}{rl}& {\mathbf{x}}_{k+1}={\nabla }_{\lambda }\mathcal{H}({\mathbf{x}}_k,{\mathbf{u}}_k,{\lambda }_{k+1})=f({\mathbf{x}}_k,{\mathbf{u}}_k)\\ & {\lambda }_k={\nabla }_{\mathbf{x}}\mathcal{H}({\mathbf{x}}_k,{\mathbf{u}}_k,{\lambda }_{k+1})={\nabla }_{\mathbf{x}}\ell ({\mathbf{x}}_k,{\mathbf{u}}_k)+{\left(\frac{\partial f}{\partial {\mathbf{x}}_k}\right)}^{\top }{\lambda }_{k+1}\\ & {\mathbf{u}}_k=\underset{\tilde{\mathbf{u}}}{\mathrm{argmin}}\mathcal{H}({\mathbf{x}}_k,\tilde{\mathbf{u}},{\lambda }_{k+1})\\ & \text{s.t.}\tilde{\mathbf{u}}\in \mathcal{U}\\ & {\lambda }_N=\frac{\partial {\ell }_F}{\partial {\mathbf{x}}_N}\end{array}$$

This is where the shooting method comes from.
The dynamics are integrated forward in time using the control ${\mathbf{u}}_k$ and the adjoint variables ${\lambda }_k$ are integrated backward.

Continuous-time version:
$$\begin{array}{rl}& \dot{\mathbf{x}}(t)={\nabla }_{\lambda }\mathcal{H}(\mathbf{x}(t),\mathbf{u}(t),\lambda (t))=f(\mathbf{x}(t),\mathbf{u}(t))\\ & -\dot{\lambda }(t)={\nabla }_{\mathbf{x}}\mathcal{H}(\mathbf{x}(t),\mathbf{u}(t),\lambda (t))={\nabla }_{\mathbf{x}}\ell (\mathbf{x}(t),\mathbf{u}(t))+{\left(\frac{\partial f}{\partial \mathbf{x}}\right)}^{\top }\lambda (t)\\ & \mathbf{u}(t)=\underset{\tilde{\mathbf{u}}}{\mathrm{argmin}}\mathcal{H}(\mathbf{x}(t),\tilde{\mathbf{u}},\lambda (t))\\ & \text{s.t.}\tilde{\mathbf{u}}\in \mathcal{U}\\ & \lambda (t_f)=\frac{\partial {\ell }_F}{\partial \mathbf{x}(t_f)}\end{array}$$

Some Notes:

• Historically many algorithms were based on integrating the continuous ODEs, forward/backward to do gradient descent on $\mathbf{u}(t)$.
• These methods are called indirect methods or shooting methods.
• In continous-time $\lambda (t)$ is called the co-state trajectory.
• These methods have largely fallen out of favor as computers have improved.