【Optimal Control (CMU 16-745)】Lecture 2 Dynamics Discretization and Stability

Review:

• Controlled dynamics (continuous-time)
• Manipulator dynamics
• Equilibrium
• Stability (local)

Lecture 2 Dynamics Discretization and Stability

Overview

• Continuous ODEs -> Discrete-time Simulations
• More on stability

1. Motivation

• In general, we cannot solve $\dot{\mathrm{x}}=\mathbf{f}(\mathrm{x})$ for $\mathrm{x}(t)$ analytically. We need to use numerical methods to solve it.
• We need to represent $\mathrm{x}(t)$ with discrete values on computers.
• Discrete-time models can capture some effects that continuous-time ODEs cannot.

2. Discrete-time Dynamics

(1) Explicit Form

$${\mathrm{x}}_{n+1}={\mathbf{f}}_d({\mathrm{x}}_n,{\mathbf{u}}_n)$$

${\mathbf{f}}_d$ is called discrete-time dynamics.

The simplest discretization:
$${\mathrm{x}}_{n+1}={\mathrm{x}}_n+h\mathbf{f}({\mathrm{x}}_n,{\mathbf{u}}_n),$$ where $h$ is the time step. The right hand side is ${\mathbf{f}}_d({\mathrm{x}}_n,{\mathbf{u}}_n)$. We call this Forward Euler Integration.

Example: Pendulum Simulation

Parameters: $m=1,l=1,g=9.81$

(a) time step $h=0.1$

The angle diverges due to the accumulation of error.

(b) time step $h=0.01$

Seems better, but still diverges.

The divergence is inevitable.
This is because the natural dynamics of this system is oscillation, and the linear approximation of the dynamics is not accurate.
(imagin the linear approximation of a sine function, it will always overshoot)

3. Stability of Discrete-time Systems

(1) Recall: In continuous-time, we can use eigenvalues to determine the stability of a system.
$$\mathrm{Re}\left[\mathrm{eig}\left(\frac{\partial {\mathbf{f}}_d}{\partial \mathrm{x}}\right)\right]<0\Rightarrow \text{stable}$$

(2) Derivation:
In discrete-time, dynamics is an iterated map:
$${\mathrm{x}}_N={\mathbf{f}}_d\left({\mathbf{f}}_d\left({\mathbf{f}}_d\left(\cdots {\mathbf{f}}_d\left({\mathrm{x}}_0\right)\right)\right)\right)$$

Linearize the dynamics (apply the chain rule):
$$\frac{\partial {\mathrm{x}}_N}{\partial {\mathrm{x}}_0}={\frac{\partial {\mathbf{f}}_d}{\partial \mathrm{x}}\frac{\partial {\mathbf{f}}_d}{\partial \mathrm{x}}\cdots \frac{\partial {\mathbf{f}}_d}{\partial \mathrm{x}}|}_{\mathrm{x}={\mathrm{x}}_0}=A_d^N$$(${\mathrm{x}}_0$ is an equilibrium point)

Assume we setup coordinate system such that ${\mathrm{x}}_0=0$ is an equilibrium point.

(3) Conclusion:
A discrete-time system is stable
$\iff$ ${\lim }_{n\to \infty }{A}_d^n{\mathrm{x}}_0=0,\forall {\mathrm{x}}_0$
$\iff$ ${\lim }_{n\to \infty }{A}_d^n=0$
$\iff$ $\left|\mathrm{eig}\left(A_d\right)\right|<1$ (all eigenvalues are inside the unit circle)

(4) Example: Forward Euler Integration of Pendulum
$${\mathrm{x}}_{k+1}={\mathrm{x}}_k+h\mathbf{f}({\mathrm{x}}_k)={\mathbf{f}}_d({\mathrm{x}}_k)$$

Compute the $A_d$:
$$\begin{array}{rl}{A}_d& =\frac{\partial {\mathbf{f}}_d}{\partial {\mathrm{x}}_k}\\ & =I+hA\\ & =I+h\left[\begin{array}{cc}0& 1\\ -\frac{g}{l\cos \theta }& 0\end{array}\right]\\ & =\left[\begin{array}{cc}1& h\\ -\frac{gh}{l\cos \theta }& 1\end{array}\right]\end{array}$$

Let $\theta =0$, we have:
$$A_d=\left[\begin{array}{cc}1& h\\ -\frac{gh}{l}& 1\end{array}\right]$$

Compute the eigenvalues (take $h=0.1$):
$$\mathrm{eig}({A_d|}_{\theta =0})=1\pm 0.313i$$

The eigenvalues are not inside the unit circle, so the system is unstable.

Plot the relationship between $h$ and the eigenvalues:

We find that whatever $h$ is, the norm of the eigenvalues is always larger than 1. So the system is always unstable.

Intuition of the overshoot:

Tips

• Be careful when discretizing ODEs.
• Sanity check based on energy, momentum, behavior of the system.
• Don’t use Forward Euler Integration.

4. A better explicit integrator

• 4th order Runge-Kutta (RK4) method (industry standard)
• Intuition:
  • Euler fiits a line sequent over each time step.
  • RK4 fits a cubic polynomial over each time step $\Rightarrow$ much better accuracy.
• Pseudo code:
  $$\begin{array}{rl}{\mathrm{x}}_{k+1}& =f_{RK4}({\mathrm{x}}_k)\\ h_1& =f({\mathrm{x}}_k)\\ h_2& =f({\mathrm{x}}_k+h/2h_1)\\ h_3& =f({\mathrm{x}}_k+h/2h_2)\\ h_4& =f({\mathrm{x}}_k+h{h}_3)\\ {\mathrm{x}}_{k+1}& ={\mathrm{x}}_k+\frac{h}{6}(h_1+2h_2+2h_3+h_4)\end{array}$$

Looks more stable than Forward Euler Integration.

The eigenvalues plot:

It is worthwhile to take more computation time to get a more accurate result.Even sophisticated integrators have issues, so we always need to do sanity check.

5. Implicit Form (Backward Euler Integration)

(1) Its basic form can be written as:
$${\mathrm{f}}_d({\mathrm{x}}_{n+1},{\mathrm{x}}_n,{\mathbf{u}}_n)=0$$

The simplest discretization:
$${\mathrm{x}}_{n+1}={\mathrm{x}}_n+h\mathbf{f}({\mathrm{x}}_{n+1})$$

The term $\mathbf{f}({\mathrm{x}}_{n+1})$ evaluates the dynamics at the next time step (in the future). This is called Backward Euler Integration.

(2) How do we simulate (solve this equation)?
We rewrite the equation as:
$${\mathrm{f}}_d\left({\mathrm{x}}_{n+1},{\mathrm{x}}_n,{\mathbf{u}}_n\right)={\mathrm{x}}_n+h\mathbf{f}({\mathrm{x}}_{n+1})-{\mathrm{x}}_{n+1}=0$$
and solve its root for ${\mathrm{x}}_{n+1}$ (will be discussed in the next lecture).

(3) Example: Pendulum Simulation

• It seems that the energy is lossing, though it is stable.
• Discretization adds damping to the system.
• While unphysical, this effect allows simulators to take big steps and is often convenient.
  (Most of the robotics simulators have this issue. It is acceptable)

(4) Takeaway:

• Implicit methods are often “more stable” than explicit methods.
• For forward simulations, solving implicit methods is more expensive than explicit methods.
• In many “direct” trajectory optimization methods, implicit methods are not more expensive than explicit methods.

6. Discretizing Controls

So far we have discretized the state $\mathrm{x}$, but not the control $\mathbf{u}$.

(1) Simplest option - Zero-order hold:
$$\mathbf{u}\left(t\right)={\mathbf{u}}_k,\forall t\in [t_k,t_{k+1})$$

• It’s easy to implement.
• May require lots of knot points to accurately capture a continuous $\mathbf{u}(t)$.

(2) (Possibly) Better option - First-order hold:
$$\mathbf{u}\left(t\right)={\mathbf{u}}_k+\left(\frac{{\mathbf{u}}_{k+1}-{\mathbf{u}}_k}{h}\right)(t-t_k),\forall t\in [t_k,t_{k+1})$$

• Can approximate $\mathbf{u}(t)$ with fewer knot points.
• Not much extra work over zero-order hold.
• Super common (e.g. classic DIRCOL).

(3) Other options:

• We can keep playing this game with higher order polynomials.
• In many control applications, $\mathbf{u}(t)$ is not smooth (e.g. bang-bang control). Therefore, higher order polynomials are not good approximations.
• Zero-order hold and first-order hold are the most common in practice.