【Optimal Control (CMU 16-745)】Lecture 1 Intro and Dynamics Review

Lecture 1 Intro and Dynamics Review

1. Continuous-Time Dynamics

(1) Basic form (most general/generic) for smooth systems:

$$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})$$

$\mathbf{x}\in {\mathbb{R}}^n$ is the state, $\mathbf{u}\in {\mathbb{R}}^m$ is the control input, $\mathbf{f}:{\mathbb{R}}^n\times {\mathbb{R}}^m\to {\mathbb{R}}^n$ is the dynamics function.

The state can be written as $\mathbf{x}=\left[\begin{array}{c}\mathbf{q}\\ \mathbf{v}\end{array}\right]$, where $\mathbf{q}\in {\mathbb{R}}^n$ is called configuration (not always a vector), $\mathbf{v}\in {\mathbb{R}}^n$ is the velocity.

Attention: $\mathbf{v}$ is not always equal to $\dot{\mathbf{q}}$.

(2) Example: pendulum

Dynamics:
$$m{l}^2\ddot{\theta }+mg\sin \theta =\tau$$
where $\theta$ is the angle, $m$ is the mass, $l$ is the length, $g$ is the gravity, $\tau$ is the torque (and also the control input $\mathbf{u}$).

Define $\mathrm{x}=\left[\begin{array}{c}\theta \\ \dot{\theta }\end{array}\right]$, $\mathbf{q}=\theta$, $\mathbf{v}=\dot{\theta }$, then the dynamics can be written as
$$\dot{\mathbf{x}}=\left[\begin{array}{c}\dot{\theta }\\ \ddot{\theta }\end{array}\right]=\left[\begin{array}{c}\dot{\theta }\\ -\frac{g}{l}\sin \theta +\frac{1}{m{l}^2}\tau \end{array}\right]=\left[\begin{array}{c}\dot{\theta }\\ -\frac{g}{l}\sin \theta \end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m{l}^2}\end{array}\right]\mathbf{u}$$

$q\in S^1$(circle), not a vector space.

velocity $v\in \mathbb{R}$, which is a vector space.

state $x\in S^1\times \mathbb{R}$ (cyllinder).

configuration space of a double pendulum: torus.

2. Control-Affine Systems

(1) Basic form

$$\dot{\mathbf{x}}={\mathbf{f}}_0(\mathbf{x})+\mathbf{B}(\mathbf{x})\mathbf{u}$$

${\mathbf{f}}_0(\mathbf{x})$ is called “drift” and $\mathbf{B}(\mathbf{x})$ is called “input Jacobian”.

1. not linear, but affine in control input $\mathbf{u}$.
2. most systems can be written in this form.

(2) Example: pendulum

$$m{l}^2\ddot{\theta }+mg\sin \theta =\tau$$
The above equation can be written as
$${\mathbf{f}}_0(\mathbf{x})=\left[\begin{array}{c}\dot{\theta }\\ -\frac{g}{l}\sin \theta \end{array}\right],\mathbf{B}(\mathbf{x})=\left[\begin{array}{c}0\\ \frac{1}{m{l}^2}\end{array}\right]$$

3. Manipulator Dynamics

(1) Basic form

$$\mathbf{M}(\mathbf{q})\dot{\mathbf{v}}+\mathbf{C}(\mathbf{q},\mathbf{v})=\mathbf{B}(\mathbf{q})\mathbf{u}$$

$\mathbf{M}(\mathbf{q})$ is called “mass matrix”, $\mathbf{C}(\mathbf{q},\mathbf{v})$ is called “dynamic bias” (coriolis + gravity), $\mathbf{B}(\mathbf{q})$ is called “input Jacobian”.

The notion $\mathbf{B}(\mathbf{q})\mathbf{u}$ is overloaded, it a function of $\mathbf{q}$ instead of $\mathbf{x}$.

The right hand side includes the unconserative forces, so external forces $\mathbf{F}$ can be added to the right hand side.

(2) Velocity kinematics

$$\dot{\mathbf{q}}=\mathbf{G}(\mathbf{q},\mathbf{v})\mathbf{v}$$
(since $\mathbf{v}$ is not always equal to $\dot{\mathbf{q}}$)

Using this equation, we can rewrite the continuous-time dynamics as
$$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})=\left[\begin{array}{c}\mathbf{G}(\mathbf{q},\mathbf{v})\mathbf{v}\\ {\mathbf{M}}^{-1}(\mathbf{q})\left(\mathbf{B}(\mathbf{q})\mathbf{u}-\mathbf{C}(\mathbf{q},\mathbf{v})\right)\end{array}\right]$$

(3) Example: Pendulum

mass matrix term:
$$\mathbf{M}(\mathbf{q})=m{l}^2$$

coriolis term:
$$\mathbf{C}(\mathbf{q},\mathbf{v})=gl\sin \theta$$

input Jacobian term:
$$\mathbf{B}(\mathbf{q})=I,$$

velocity kinematics matrix:
$$\mathbf{G}=I$$

All mechanimal systems can be written in this form because this function is a rewriting version of Euler-Lagrange equation.

Euler-Lagrange equation:
$$L=\frac{1}{2}{\mathbf{v}}^T\mathbf{M}(\mathbf{q})\mathbf{v}-U(\mathbf{q}),$$ where $U(\mathbf{q})$ is the potential energy, $\frac{1}{2}{\mathbf{v}}^T\mathbf{M}(\mathbf{q})\mathbf{v}$ is the kinetic energy.

4. Linear Systems

(1) Basic form

$$\dot{\mathbf{x}}=\mathbf{A}(t)\mathbf{x}+\mathbf{B}(t)\mathbf{u}$$

1. If $\mathbf{A}(t)=\mathbf{A}$ and $\mathbf{B}(t)=\mathbf{B}$, then the system is called “time-invariant”.
   Otherwise, the system is called “time-varying”.
2. Super important in control theory.
3. Approximation of nonlinear systems.

   if $\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})$, then $\mathbf{A}(t)=\frac{\partial \mathbf{f}}{\partial \mathbf{x}}(\mathbf{x},\mathbf{u})$ and $\mathbf{B}(t)=\frac{\partial \mathbf{f}}{\partial \mathbf{u}}(\mathbf{x},\mathbf{u})$.

5. Equilibrium

(1) Definition

It is a state ${\mathbf{x}}^{\ast }$ such that $\mathbf{f}({\mathbf{x}}^{\ast },{\mathbf{u}}^{\ast })=0$.
Algebraically, it is a solution to $\mathbf{f}(\mathbf{x},\mathbf{u})=0$.

(2) Example: Pendulum

Dynamics:
$$\dot{\mathbf{x}}=\left[\begin{array}{c}\dot{\theta }\\ -\frac{g}{l}\sin \theta \end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m{l}^2}\end{array}\right]\tau$$

Let $\dot{\mathbf{x}}=0$, then $\tau =0$, $\dot{\theta }=0$, and $\theta =0$ or $\pi$.

It means the configuration manifold is compact.

6. First Control Problem

• Can we move the equilibrium using control input $\mathbf{u}$? - Yes
• Consider letting the pendulum stay at $\theta =\pi /2$.
  We can write
  

Solve the equation, we can get
$$\mathbf{u}=mgl$$

In general, we can make the system stay at any equilibrium by choosing the right control input $\mathbf{u}$.(Not always true, but true for some systems)

7. Stability of Equilibrium

(1) Definition

When will we stay “near” an equilibrium point under perturbation?

Take a 1D system as an example ($\mathbf{x}\in \mathbb{R}$)

There are three equilibrium points.
(1) The right one and the left one are unstable equilibrium points.
(2) The origin is a stable equilibrium point.

From the above example, we can know:
If $\partial \mathbf{f}/\partial \mathbf{x}<0$, then the equilibrium point is stable.
If $\partial \mathbf{f}/\partial \mathbf{x}>0$, then the equilibrium point is unstable.

For higher dimensional systems, the conclusion also holds.
$\partial \mathbf{f}/\partial \mathbf{x}$ is called “Jacobian”.
We need to check the real part of the eigenvalues of the Jacobian.

(2) Example: Pendulum

$$\mathrm{f}(\mathbf{x},\mathbf{u})=\left[\begin{array}{c}\dot{\theta }\\ -\frac{g}{l}\sin \theta \end{array}\right]$$

The Jacobian is
$$\frac{\partial \mathbf{f}}{\partial \mathbf{x}}=\left[\begin{array}{cc}0& 1\\ -\frac{g}{l}\cos \theta & 0\end{array}\right]$$

(1) When $\theta =\pi$, then the Jacobian is
$$\frac{\partial \mathbf{f}}{\partial \mathbf{x}}=\left[\begin{array}{cc}0& 1\\ \frac{g}{l}& 0\end{array}\right]$$

The eigenvalues are $\pm \sqrt{\frac{g}{l}}$. One is positive and the other is negative, so the equilibrium point is unstable.

(2) When $\theta =0$, then the Jacobian is
$$\frac{\partial \mathbf{f}}{\partial \mathbf{x}}=\left[\begin{array}{cc}0& 1\\ -\frac{g}{l}& 0\end{array}\right]$$

The eigenvalues are $\pm i\sqrt{\frac{g}{l}}$. Both are imaginary, which means the motion is undamped oscillation. It is called marginally stable. (Remember that the analysis is based on linearization)

We can add damping using controller (e.g. $\mathbf{u}=-k\dot{\theta }$). Then the system will be stable.