🐍 | Code | 🔍 | Worked Example |
📈 | Graph | 🧩 | Exercise |
🏷️ | Definition | 💻 | Numerical Method |
💎 | Theorem | 🧮 | Analytical Method |
📝 | Remark | 🧠 | Theory |
ℹ️ | Information | 🗝️ | Hint |
⚠️ | Warning | 🔓 | Solution |
from numpy import *
import matplotlib; matplotlib.use("nbAgg")
%matplotlib notebook
from matplotlib.pyplot import *
The system $\dot{x} = f(x,u)$ is controllable if
for any $t_0 \in \mathbb{R}$, $x_0 \in \mathbb{R}^n$ and $x_f \in \mathbb{R}^n$,
there are $t_f > t_0$ and $u: [t_0, t_f] \to \mathbb{R}^m$ such that
the solution $x(t)$ such that $x(t_0)=x_0$ satisfies
$$ x(t_f) = x_f. $$Let $x_r$ be a reference trajectory of the system state: $$ x_r(t), \, t\in[t_0, t_f]. $$
It is admissible if there is a function $u_r(t)$, $t \in [t_0, t_f]$ such that the solution $x$ of the IVP $$ \dot{x} = f(x, u_r), \; x(t_0) = x_r(t_0) $$ is the function $x_r$.
The position $d$ (in meters) of a car of mass $m$ (in kg) on a straight road is governed by
where $u$ the force (in Newtons) generated by its motor.
The car is initially at the origin of a road and motionless.
We would like to cross the end of the road (location $d_f > 0$) at time $t_f > 0$ and speed $v_f$.
Numerical values:
$m=1500 \, \mbox{kg}$,
$t_f=10 \, \mbox{s}$, $d_f=100 \, \mbox{m}$ and $v_f=100 \, \mbox{km/h}$.
We search for a reference trajectory for the state
such that:
$d_r(0)=0$, $\dot{d}_r(0) = 0$,
$d_r(t_f) = x_f$, $\dot{d}_r(t_f) = v_f$.
We check that this reference trajectory is admissible, i.e. that we can find a control $u_r(t)$ such that the solution of the IVP is $x(t) = x_r(t)$ when $x(0) = x_r(t)$.
Here, if $d_r$ is smooth and if we apply the control $u(t) = m\ddot{d}_r(t)$,
$$ (d-d_r)(0) = 0, \; \frac{d}{dt} (d-d_r)(0)= 0. $$
Thus, $d(t) = d_r(t)$ – and thus $\dot{d}(t) = \dot{d}_r(t)$ – for every $t\geq 0$.
We can find $d_r$ as a third-order polynomial in $t$
with
(equivalently, with $u(t)$ as an affine function of $t$).
m = 1500.0
xf = 100.0
vf = 100.0 * 1000 / 3600 # m/s
tf = 10.0
alpha = vf/tf**2 - 2*xf/tf**3
beta = 3*xf/tf**2 - vf/tf
def x(t):
return alpha * t**3 + beta * t**2
def d2_x(t):
return 6 * alpha * t + 2 * beta
def u(t):
return m * d2_x(t)
y0 = [0.0, 0.0]
def fun(t, y):
x, d_x = y
d2_x = u(t) / m
return [d_x, d2_x]
result = solve_ivp(
fun, [0.0, tf], y0, dense_output=True
)
figure()
t = linspace(0, tf, 1000)
xt = result["sol"](t)[0]
plot(t, xt)
grid(True); xlabel("$t$"); title("$d(t)$")
figure()
vt = result["sol"](t)[1]
plot(t, 3.6 * vt)
grid(True); xlabel("$t$")
title(r"$\dot{d}(t)$ km/h")
Let $\dot{x} = A x + B u$ with $x\in \mathbb{R}^2$, $u \in \mathbb{R}$, $$ A = \left[ \begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array} \right], \; B = \left[ \begin{array}{cc} 0 \\ 1 \end{array} \right]. $$
Find a smooth reference trajectory $x_r(t)$, $t\in[0, 1]$ which is not admissible.
The first line of the vector equation $\dot{x} = A x + Bu$ is
Any trajectory $x_r$ that does not satisfy this equation is not admissible; for example
Consider the pendulum with dynamics:
Find a smooth reference trajectory that leads the pendulum from the bottom configuration
to the top configuration
Show that the reference trajectory is admissible and compute the corresponding input $u_r(t)$.
Simulate the result and visualize the solution.
What should theoretically happen at $t=t_f$ if $u(t)=0$ is applied when $t \geq t_f$? What does happen in reality ? Why ? How can we mitigate this issue?
Numerical Values:
Since the initial and final conditions form a set of 4 equations, we search for an admissible trajectory defined by a 3rd-order polynomial
with unknown coefficients $a, b, c, d$.
The initial conditions $\theta_r(0)=0$ and $\dot{\theta}_r(0)=0$ are equivalent to
The final condition $\theta_r(t_f) = \pi$ is equivalent to
and $\dot{\theta}_r(t_f) = 0$ to
The latter equation can be transformed into
and the former becomes
and thus $b = \frac{3\pi}{t_f^2}.$
Finally, the following trajectory $(\theta_r(t),\dot{\theta}_t(t))$ is smooth and meets the required initial and final conditions:
By construction our reference trajectory meets the initial condition. There is a unique input that makes the solution follows this reference; its is defined by
where
m = 1.0
l = 1.0
b = 0.1
g = 9.81
t0, tf = 0.0, 10.0
def theta_r(t):
s = t/tf
return -2*pi*s**3 + 3*pi*s**2
def dtheta_r(t):
s = t/tf
return -6*pi/tf*s**2 + 6*pi/tf*s
def d2theta_r(t):
s = t/tf
return -12*pi/(tf*tf)*s + 6*pi/(tf*tf)
def u_r(t):
return (m*l*l*d2theta_r(t) +
b*dtheta_r(t) +
m*g*l*sin(theta_r(t)))
def fun(t, theta_dtheta):
theta, dtheta = theta_dtheta
d2theta = ((-b*dtheta - m*g*l*sin(theta) + u_r(t))
/ (m * l * l))
return dtheta, d2theta
t_span = [t0, tf]
y0 = [0.0, 0.0]
r = solve_ivp(fun, t_span, y0, dense_output=True)
t = linspace(t0, tf, 1000)
solt = r["sol"](t)
thetat, dthetat = solt
figure()
plot(t, theta_r(t), "k--", label=r"$\theta_r(t)$")
plot(t, thetat, label=r"$\theta(t)$")
yticks([0, 0.5*pi, pi], ["$0$", r"$\pi/2$", r"$\pi$"])
title(r"Simulation of $\theta(t)$")
grid(True); legend()
Theoretically, we should have $\theta(t_f) = \pi$ and $\dot{\theta}(t_f)=0$, but the simulation is quite far from that.
Since the top equilibrium is unstable, small (numerical) errors in the state may cause large deviations of the trajectory.
We may reduce the simulation error thresholds to alleviate this problem.
rhp = solve_ivp(fun, t_span, y0,
dense_output=True,
rtol=1e-6, atol=1e-9)
t = linspace(t0, tf, 1000)
solt_hp = rhp["sol"](t)
thetat_hp, dthetat_hp = solt_hp
figure()
plot(t, theta_r(t), "k--", label=r"$\theta_r(t)$")
plot(t, thetat, label=r"$\theta(t)$ (standard)")
plot(t, thetat_hp, label=r"$\theta(t)$ (low error)")
yticks([0, 0.5*pi, pi], ["$0$", r"$\pi/2$", r"$\pi$"])
title(r"Simulation of $\theta(t)$")
grid(True); legend()
A system $\dot{x} = Ax + Bu$ is controllable iff
from the origin $x_0 = 0$ at $t_0=0$,
we can reach any state $x_f \in \mathbb{R}^n$.
The system $\dot{x} = Ax+Bu$ ($x\in \mathbb{R}^n$) is controllable iff:
$[B, \dots, A^{n-1}B]$ is the controllability matrix.
def KCM(A, B):
n = shape(A)[0]
mp = matrix_power
cs = column_stack
return cs([mp(A, k) @ B for k in range(n)])
n = 3
A = zeros((n, n))
for i in range(0, n-1):
A[i,i+1] = 1.0
B = zeros((n, 1))
B[n-1, 0] = 1.0
C = KCM(A, B)
C_expected = [[0, 0, 1], [0, 1, 0], [1, 0, 0]]
assert_almost_equal(C, C_expected)
assert matrix_rank(C) == n
This implementation of KCM
is not optimized: fewer computations
are achievable using the identity:
Rank computations are subject to (catastrophic) numerical errors; sensitivity analysis or symbolic computations may alleviate the problem.
Consider $\dot{x} = A x + Bu$ with
$x \in \mathbb{R}^n$,
$u \in\mathbb{R}^m$ and,
$\mathrm{rank} \, B = n$.
Show that $m \geq n$.
Is the system controllable ?
Given $x_0$, $x_f$ and $t_f > 0$, show that any smooth trajectory that leads from $x_0$ at time $t_0$ to $x_f$ at time $t_f$ is admissible.
The shape of $B$ is $n \times m$, thus
By assumption $\mbox{rank} \, B = n$, thus $n \leq m$.
The system is controllable since
Since $\mbox{rank} \, B = n$, matrix $B$ contains a $n \times n$ invertible matrix $R$. Let’s assume for the sake of simplicity that $R$ is made of the first $n$ columns of $B$ and let $$ S: = \left[ \begin{array}{c} R^{-1} \\ 0 \end{array} \right] \in \mathbb{R}^{m \times n}. $$
Then by construction $B \times S = I \in \mathbb{R}^{n\times n}$.
If we define $u$ as a function of some auxiliary control $v \in \mathbb{R}^n$ by $$ u(t) = S v(t), $$ then $$ \dot{x} = A x + v. $$
To join $x_0$ at $t_0$ and $x_f$ at $t_f$, we can apply the control $$ v(t) := -A x_r(t) + \dot{x}_r(t) $$ where $$ x_r(t) := x_0 + \frac{t-t_0}{t_f-t_0} (x_f - x_0). $$
Indeed, that leads to $$ \dot{x}(t) = A x(t) - A x_r(t) + \dot{x}_r(t), \; x(t_0) = x_r(t_0) $$ or if we denote $e := x - x_r$, $$ \dot{e}(t) = Ae(t), \; e(t_0) =0 $$ and thus yields $x(t) = x_r(t)$ for any $t\geq t_0$.
Show that the system is controllable
We have
Thus,
The controllability matrix has full rank and the system is controllable.
$d T_1/dt = u + (T_2 - T_1)$
$d T_2/dt = (T_1 - T_2) + (T_3 - T_2)$
$d T_3/dt = (T_2 - T_3) + (T_4 - T_3)$
$d T_4/dt = (T_3 - T_4)$
Show that the system is controllable.
Assume now that the four cells are organized as a square.
Is the system still controllable?
Why?
How could you solve this problem?
We have
The controllability matrix
is full rank, thus the system is controllable.
If the system is organized as a square
The controllability matrix is
It is not full rank since the second and the third rows are equal. Thus the system is not controllable.
The lack of controllability is due to symmetry.
If the initial temperature in cell 2 and 3 are equal, since they play symmetric role in the system, their temperature will be equal for any subsequent time.
Hence, it will be impossible to reach a state with different temperature in cell 2 and 3, no matter what the input is.
To break this symmetry and restore controllability, we may for example try to add a second independent heat source sink in cell 2.
This leads to
and the controllability matrix
which has a full-rank.