Robo Quest

Controllability

For a linear dynamical system $\dot x = Ax$ the system is stable or unstable depending on the eigenvectors of the matrix $A$. If we now add a control variable to the system the dynamics can be written as \ref{eq:dynsys}

\begin{equation} \label{eq:dynsys} \dot x = Ax + Bu \end{equation} where $x \in \mathbb{R}^n$, $u \in \mathbb{R}^q$ and $B \in \mathbb{R}^{n \times q}$

We can now shape the eigenvectors using the control input, even for systems which otherwise are inherently unstable(for example, an inverted pendulum).

If we now assume a fully observable system where the output $y = Cx, C = I, y \in \mathbb{R}^p$ then the optmial control law(if controllable) is given as \begin{equation} \label{eq:optcontrol} u = -Kx \end{equation}

If we substitute \ref{eq:optcontrol} in \ref{eq:dynsys} then the resulting dynamics are given as

\begin{align} \dot x &= Ax \hspace{0.1cm} – BKx \nonumber \\ \dot x &= (A \hspace{0.1cm} – BK)x \end{align}

The eigenvectors of $(A-BK)$ govern the stability of the system and are arbitrarily controllable. In a physical system $A$, the dynamics of the system and $B$ the control surface are fixed. Hence depending on the choice of $A$ and $B$ the system is controllable(or not).

A system is controllable if the matrix

\begin{equation} \mathcal{C} = [B, AB, A^2B, A^3B \ldots A^{n+1}B] \end{equation}

has rank = n. In Matlab it is as simple as doing ctrb(A,B) to figure out the controllability. rank(ctrb(A,B)) == n test tells us if the system is controllable, not how controllable the system is.

Reachability

Vectors in the reachability set $R_t$ are such that they can be reached with a corresponding $u(t)$. Full reachability implies that all $R_t = \mathbb{R^n}$. A controllable system implies full reachability(and vice versa)

Degree of controllability

If we take [U, S, V] = svd(C, 'econ') then the singular vectors obtained(ordered by eigenvalues) represent the directions where the controls are most amenable(This is related to the Gramian).

There is a PBH test whereby $rank[A – \lambda I \hspace{0.2cm} B] = n$. That is B needs to have some component in each eigenvector direction for the system to be controllable. If B is a random vector then with high probability $(A,B)$ is ctrb

Stabalizable

If all unstable(and lightly damped) eigen vectors of $A$ are in the controllable subspace(the singular vectors above) then the system is stabalizable.

Inverted Pendulum on a Cart

The state of the system is given as $[x, \dot{x}, \theta, \dot{\theta}]^T$ which is the position of the cart, angle of the pendulum and their velocities. The dynamical equation can be derived using for example Lagrange equations. The equations are non linear so we need to linearize them around the fixed points. The natural fixed points are $[0, 0, 0, 0]^T$ and $[0, 0, \pi, 0]^T$

The equations are nicely derived in this video – Equations of Motion for the Inverted Pendulum (2DOF) Using Lagrange's Equations

The final equations are

\begin{align} (M+m)\ddot{x} – m l \ddot{\theta} + m l \dot{\theta}^2 sin(\theta) &= u(t) \nonumber \\ l \ddot{\theta} – \ddot{x}cos(\theta) – g sin(\theta) &= 0 \end{align}

If we linearize around zero here we can assume $sin(\theta) \approx \theta$ and $cos(\theta) \approx 1$ and that simplifies the equations into

\begin{align} (M+m)\ddot{x} – m l \ddot{\theta} &= u(t) \nonumber \\ l \ddot{\theta} – \ddot{x} – g \theta &= 0 \label{eq:lininvcart} \end{align}

Note that since $\theta \approx 0$ $\dot{\theta}^2$ is vanishingly small so we also ignore it.

We can now write the state space form of the equations \ref{eq:lininvcart} which gives us the $A$ and the $B$ matrices. If we check eig(A) then we can see that a eigen value is unstable so given the control we can check if this system is controllable. That is rank(ctrb(A,B)) == 4 is true than the system is controllable.

If the system is controllable then we find $K$ such that the eigenvalues of $(A – BK)$ are stable. We can use K = place(A,B,eigs) where eigs is a suitable vector with eigen values(in LHP). We can chose any eigs but the choice is eventually governed by the non linear dynamics(which we ignored) and hence the linearization if not accurate or we cannot generate the control effort to have those eigen values. There is an optimal controller however which give the optimal control K which is the best solution.

Linear Quadratic Regulator (LQR) Control

Identify a cost function which integrates what performance I want from the controller. The cost function can be defined as

\begin{equation} J = \int_{0}^{\infty} (x^T Q x + u^T R u) dt \end{equation}

where $Q$ is a diagonal matrix which represents penalty for not tracking the set point and $R$ represents effort spent on control which we want to minimize. In Matlab one can do K = lqr(A,B,Q,R) which gives the optimal gain matrix for the given system.

The minimization objective is of the order $O(n^3)$ which makes it intractable for large dimensional states.