Given
$$ \dot{X} = f(X) + g(X) u $$Find \( X \rightarrow Z \)
$$ \left[ \begin{array}{c} \dot{Z}_1 \\ \dot{Z}_2 \\ \vdots \\ \dot{Z}_n \end{array} \right] = \left[ \begin{array}{c} Z_2 \\ Z_3 \\ \vdots \\ b(Z) \end{array} \right] + \left[ \begin{array}{c} 0 \\ 0 \\ \vdots \\ a(Z) \end{array} \right] u $$Linearize above using an auxillary control input \( v \) as,
$$ v = b(Z) + a(Z) u \implies u = a(Z)^{-1}(v - b(Z))$$The system dynamics after subtituting for \(u \) becomes,
$$ \left[ \begin{array}{c} \dot{Z}_1 \\ \dot{Z}_2 \\ \vdots \\ \dot{Z}_n \end{array} \right] = \left[ \begin{array}{c} Z_2 \\ Z_3 \\ \vdots \\ 0 \end{array} \right] + \left[ \begin{array}{c} 0 \\ 0 \\ \vdots \\ 1 \end{array} \right] v $$Or the system reduces to,
$$ \frac{d^n Z}{dt^n} = v $$By choosing
$$ v = -k_0Z -k_1Z_1 \dots -k_{n-1} Z_{n-1} $$The system reduces to,
$$ \frac{d^n Z}{dt^n} + k_{n-1} \frac{d^{n-1} Z}{dt^{n-1}} \dots k_1 \frac{dZ}{dt} + k_0 Z = 0 $$Consider the example of the system given by,
$$ \dot{X}_1 = -2 X_1 + a X_2 + sin(X_1) $$ $$ \dot{X}_2 = - X_2 cos(X_1) + cos(2 X_1) u $$With transformation,
$$ z_1 = X_1 $$ $$ z_2 = sin(X_1) + aX_2 $$The states of the system becomes,
$$ \dot{z}_1 = -2 z_1 + z_2 $$ $$ \dot{z}_2 = cos(z_1) \dot{z_1} + a \dot{X}_2 = cos(z_1) (-2 z_1 + z_2) - aX_2 cos(X_1) + acos(2 X_1) u $$$$ \dot{z}_2 = cos(z_1) (-2 z_1 + z_2) + (sin(z_1)-z_2) cos(z_1) + a cos(2 z_1) u $$$$ \dot{z}_2 = -2 z_1 cos(z_1) + sin(z_1)cos(z_1) + a cos(2 z_1) u $$The system reduces to,
$$ \dot{z}_1 = -2 z_1 + z_2 $$ $$ \dot{z}_2 = v $$TO
$$ \dot{z}_1 = -2 z_1 + z_2 $$ $$ \dot{z}_2 = v $$$$ u = \frac{1}{a cos(2 z_1)} (v + 2 z_1 cos(z_1) - sin(z_1)cos(z_1) ) $$Dividing by \( cos(2 z_1 ) \) could cause the input \( u \) to go to infinity.
As there is no control input \( u \), we compute derivative of the measurement,
$$ \dot{y} = cos(X_1) \dot{X}_1 = cos(X_1) X_2^2 + cos(X_1) sin(X_3) $$As there is no control input \( u \) in \( \dot{y} \), we compute derivative of the measurement,
$$ \ddot{y} = -sin(X_1) X_2^2 \dot{X}_1 + 2 cos(X_1) X_2 \dot{X}_2 -sin (X_1) sin(X_3) \dot{X}_1 +cos(X_1) cos(X_3) \dot{X}_3 $$$$ \ddot{y} = -sin(X_1) X_2^2 (X_2^2 + sin(X_3) )+ 2 cos(X_1) X_2 cos(X_2) -sin (X_1) sin(X_3) (X_1^2+sin(X_3)) +cos(X_1) cos(X_3) (X_1 + u ) $$$$ \ddot{y} = \underbrace{2 cos(X_1)^2 X_2 + X_1 cos(X_1) cos(X_3) - (X_2^2 + sin(X_3))^2 sin(X_1)}_{a(X)} + \underbrace{cos(X_1) cos(X_3)}_{b(X)} u $$Measurement \( X_1 \).
$$ Z_1 = X_1 $$There is no control input in this term, so we differentiate and get,
$$ Z_2 = \dot{Z}_1 = \dot{X}_1 = sin(X_2) + (X_2+1)X_3 $$There is no control input in this term, so we differentiate again and get,
$$ Z_3 = \dot{Z}_2 = cos(X_2) \dot{X_2} + (X_2+1)\dot{X}_3 + X_3\dot{X}_2 = cos(X_2) (X_1^5 + X_3)+ (X_2+1)( X_1^2 + u ) + X_3 (X_1^5 + X_3) $$ $$ Z_3 = \ddot{Z}_1 = (X_3 + cos(X_2)) (X_1^5 + X_3)+ (X_2+1)X_1^2 + (X_2+1)u $$Therefore, choosing \( v = (X_3 + cos(X_2)) (X_1^5 + X_3)+ (X_2+1)X_1^2 + (X_2+1)u \), we get,
$$ \ddot{Z}_1 = v $$The transformed system is a second order linear system, and control input \( v \) can be chosen to have any second order behavior of \( Z_1 \).
Zero dynamics are the dynamics of system when the output is zero.
Consider a system in canonical form,
$$ \dot{X} = \left[ \begin{array}{cccc} 0 & 1 & 0 & \dots \\ 0 & 0 & 1 & \dots \\ \vdots & \vdots & \vdots & \vdots \\ - a_0 & -a_1 & \dots & -a_{n-1} \end{array} \right] X + \left[ \begin{array}{c} 0 \\ 0 \\ \vdots \\ 1 \end{array}\right] u $$with a single measurement
$$ y = \left[ \begin{array}{cccccccc} b_0 & b_1 & b_2 & \dots & b_m & (n-m -1)~zeros \end{array} \right] X $$The measurement equation has first \( m \) terms, and remaing \( n-m -1 \) terms are zero. We first rewrite measurement equation as,
$$ y = \left[ \begin{array}{cccccccc} b_0 & b_1 & b_2 & \dots & b_m & 0 & \dots & 0 \end{array} \right] X = b_0 X_1 + b_1 X_2 + \dots + b_m X_{m+1} $$By taking derivative of measurement \( y \), we get,
$$ \dot{y} = b_0 \dot{X}_1 + b_1 \dot{X}_2 + \dots + b_m \dot{X}_{m+1} = \left[ \begin{array}{cccccccc} 0 & b_0 & b_1 & b_2 & \dots & b_m & (n-m-2)~zeros\end{array} \right] X $$Therefore, by taking derivative, the b-values shifted one to right. Similarly, it can be shown that by taking \( n-m-1\) derivatives, we get
$$ y^{(n-m-1)} = \left[ \begin{array}{cccccccc} (n-m)~zeros & b_0 & b_1 & b_2 & \dots & b_m \end{array} \right] X $$Taking derivative once again gives,
$$ y^{(n-m)} = \left[ \begin{array}{cccccccc} (n-m+1)~zeros & b_0 & b_1 & b_2 & \dots & b_{m-1} \end{array} \right] X + b_m \dot{X_n} = u + additional~terms = v $$Note, for this linear system, the number of poles was \( n \) and number of zeros are \( m \), and the relative degree \( r \) of the system is the difference of number of poles and zeros.
The transfer function between \( x \) and \(z \) can be written as,
$$ Z(s) = (b_0 + b_1 s + \dots + b_m s^m) X(s) $$$$ \frac{X(s)}{Z(s)} = \frac{1}{b_0 + b_1 s + \dots + b_m s^m}$$Consider the system given by,
$$ \dot{X}_1 = X_2 + u $$ $$ \dot{X}_2 = u $$Say we are interested in controlling \( X_1 \). Then the zero dyanmics are given by,
$$ z = X_1 = 0 $$Taking derivative gives,
$$ \dot{z} = \dot{X}_1 = X_2 + u = 0$$Therefore,
$$ u = - X_2 $$Substituting this ins the last equation gives,
$$ \dot{X}_2 = u = - X_2 $$Therefore, by choosing a reduced order system \( [z, \dot{z}] \), we get a stable overall performance because the zero dynamics is stable.
By following the same steps above, we get, the zero dynamics as,
$$ \dot{X}_2 = X_2 $$Measurement,
$$ Z_1 = X_1 $$Compute zero dynamics
When \(Z_1= 0\), we get, the zero dynamics as,
$$ 0 = sin(X_2) + (X_2+1)X_3 $$$$ \dot{X}_2 = X_3 $$$$ \dot{X}_3 = u $$$$ 0 = (X_3 + cos(X_2)) X_3 + (X_2+1)u $$After simplifications, the zero dynamics becomes,
$$ \dot{X}_2 = X_3 $$$$ \dot{X}_3 = - \frac{(X_3 + cos(X_2)) X_3}{(X_2+1)} $$The zero dynamics in this case is not stable, because if \(X_3 << 0 \), then \( X_2 \rightarrow 0 \) and \(X_3 \) will diverge to minus infinity.
with measurements,
$$ y = h(X) $$Recall \( L_f h(X) = \frac{\partial h}{ \partial X} f(X) \)
$$ \dot{y} = \frac{d h}{d X} \dot{X} = \frac{d h}{d X} (f(X) + g(X) u) = \frac{d h}{d X} f(X) = L_f h(X) $$\( L_g h(X) = 0 \) becuase system has relative degree \( r \). Further \( L_gL_f^{(i)} h(X) = 0 \) for \( i<r \) becuase system has relative degree \( r \).
Differentiating until \( r-1 \)
$$ y^{(r-1)} = L_f^{(r-1)} h(X) $$Differentiating again,
$$ y^{(r)} = \frac{ d L_f^{(r-1)} h(X)}{dX} \dot{X} = \frac{ d L_f^{(r-1)} h(X)}{dX} f(X) + \frac{ d L_f^{(r-1)} h(X)}{dX} g(X) u $$$$ y^{(r)} = L_f^{(r)} h(X) + L_g L_f^{(r-1)} h(X) u $$Choosing,
$$ u = L_g L_f^{(r-1)} h(X) ^{-1} \left( v - L_f^{(r)} h(X) \right) $$gives,
$$ y^{(r)} = v $$Given $$ \dot{X} = f(X) + g(X) u $$
Choose \( [ \mu_1~\mu_2~\dots ~\mu_r]^T = [ y~ \dot{y}~\dots ~y^{(r)}]^T\) and additional \( n-r \) variables \( \psi \) so the transformed system becomes,
$$ \dot{\mu} = \left[ \begin{array}{c} \mu_2 \\ \mu_3 \\ \dots \\ \alpha(\mu,\psi) + \beta(\mu,\psi) u \end{array} \right] $$$$ \dot{\psi} = w(\mu,\psi) $$with output \( y = \mu_1 \).
\( \psi \) must satisfy,
$$ \nabla \psi_i g = L_g \psi_i = 0 ~ for ~1\leq i \leq n-r $$Consider the system,
$$ \dot{X} = \left[ \begin{array}{c} -X_1 \\ 2X_1 X_2 + sin(X_2) \\ 2X_2 \end{array} \right] + \left[ \begin{array}{c} e^{2X_2} \\ .5 \\ 0 \end{array} \right] u $$with measurement \( y = X_3 \). Following the procedure above, we first define,
$$ \mu_1 = y = X_3$$ $$ \mu_2 = \dot{\mu}_1 = \dot{y} = \dot{X}_3 = 2X_2 $$ $$ \dot{\mu}_2 = \ddot{y} = 2\dot{X}_2 = 2(2X_1 X_2 + sin(X_2) ) + u $$We now need to find a third variable \( \psi \) such that \( L_g \psi = 0\)
$$ L_g \psi = \frac{\partial \psi }{\partial X} g = \left[\begin{array}{ccc} \frac{\partial \psi }{\partial X_1} & \frac{\partial \psi }{\partial X_2} & \frac{\partial \psi }{\partial X_3} \end{array}\right] \left[ \begin{array}{c} e^{2X_2} \\ .5 \\ 0 \end{array} \right] = 0 $$$$ e^{2X_2} \frac{\partial \psi }{\partial X_1} + .5 \frac{\partial \psi }{\partial X_2} = 0$$Choose,
$$\psi = 1 + X_1 - e^{2X_2} $$Therefore, varaible transformation,
$$ \mu_1 = X_3 $$$$ \mu_2 = 2 X_2 $$$$ \psi = 1 + X_1 - e^{2X_2} $$The states \(X \) can be obtained using,
$$ X_3 = \mu_1 $$$$ X_2 = \frac{\mu_2}{2} $$$$ X_1 = \psi + e^{\mu_2}-1$$Transforms the original system into,
$$ \dot{\mu}_1 = \mu_2 $$$$ \dot{\mu}_2 = 2( (\psi + e^{\mu_2}) \mu_2 + sin(\mu_2/2) ) + u $$$$ \dot{\psi} = \dot{X}_1 - 2e^{2X_2} \dot{X}_2 = (1-\psi-e^{\mu_2})(1+2\mu_2e^{\mu_2} - 2 sin(\mu_2/2)e^{\mu_2} $$Note, the zero dynamics is,
$$ \dot{\psi} = - \psi $$Given a nonlinear system, can we find a variable transformation such that the resulting system is a \( n \) order system, and has a compact linear-like form?
Lie bracket between two vector fields is given by,
$$ adj_f g = [f,g] (X) = \frac{\partial g}{\partial X} f(X) - \frac{\partial f}{\partial X} g(X) $$Lie brackets can be interpreted as a new direction that can be obtained by combining vector fields \( f \) and \( g\).
We call a group of vectors \( g: [g_1, g_2, \dots , g_n] \) involutive, if lie bracket between any two vector fields results in a linear combination of vectors in \( g \). i.e. dynamics along no new directions can be obtained by combining vector fields in \( g \).
Given a system,
$$ \dot{X} = f(X) + g(X) u $$Transformation of variables from \( X \) to \(Z\) such that, \( Z = [ Z_1 ~ \dot{Z}_1~ \ddot{Z}_2 ~ \dots ~ Z_1^{(n-1)} ] \) with
$$ Z_1^{(n)} = \alpha(X) + \beta(X) u = v $$exists if,
where \( \nabla z_1 \) is the gradient of \( z_1 \).
linear transform of the system can be obtained by following these steps,
Consider the robot joint given by,
$$ I \ddot{q}_1 + MgL sin(q_1) + K(q_1 - q_2 ) = 0$$ $$ J \ddot{q}_2 - K(q_1 - q_2 ) = u $$We wish to linearize this system using the input-control linear transform scheme presented before.
We first express the system in a state space form using states \( X = \left[ q_1 ~ q_2 ~ \dot{q}_1 ~ \dot{q}_2 \right]^T \). The state dynamics can be written as,
$$ \dot{X} = \left[ \begin{array}{c} X_3 \\ X_4 \\ -\frac{MgL}{I} sin(X_1) -\frac{k}{I} (X_1 - X_2) \\ \frac{K}{J}(X_1 - X_2) \\ \end{array} \right] + \left[ \begin{array}{c} 0 \\ 0 \\ 0 \\ \frac{1}{J} \\ \end{array} \right] u $$The controllability matrix form, \(\left[ \begin{array}{cccccc} adj_f^0 g(X) & adj_f^1 g(X) & adj_f^2 g(X) & adj_f^{3} g(X) \end{array} \right] \) using,
$$ adj_f^0 g(X) g(X) = \left[ \begin{array}{c} 0 \\ 0 \\ 0 \\ \frac{1}{J} \\ \end{array} \right] $$Therefore, the controllability matrix is,
$$\left[ \begin{array}{cccccc} adj_f^0 g(X) & adj_f^1 g(X) & adj_f^2 g(X) & adj_f^{3} g(X) \end{array} \right] = \left[ \begin{array}{c} 0 & 0 & 0 & - \frac{K}{JI} \\ 0 & -\frac{1}{J} & 0 & - \frac{K}{J^2} \\ 0 & 0 & - \frac{K}{JI} & 0 \\ \frac{1}{J} & 0 & - \frac{K}{J^2} & 0 \end{array} \right] $$Controllability matrix is constant and has full rank. Futher, as terms are constant, its involutive (as lie derivatives are zero).
we need to find a \(z_1 \) such that,
$$ \left[ \begin{array}{cccc} adj_f^0 g(X) & adj_f^1 g(X) & adj_f^2 g(X) & adj_f^{3} g(X) \end{array} \right] \nabla z_1 = \left[ \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right]$$ $$ \left[ \begin{array}{c} 0 & 0 & 0 & - \frac{K}{JI} \\ 0 & -\frac{1}{J} & 0 & - \frac{K}{J^2} \\ 0 & 0 & - \frac{K}{JI} & 0 \\ \frac{1}{J} & 0 & - \frac{K}{J^2} & 0 \end{array} \right] \left[ \begin{array}{c} \frac{\partial z_1}{X_1} \\ \frac{\partial z_1}{X_2} \\ \frac{\partial z_1}{X_3} \\ \frac{\partial z_1}{X_4} \end{array} \right] = \left[ \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right]$$Therefore, we choose \( z_1 = x_1 \), the state transforms can be computed as,
$$ z_1 = x_1 $$ $$ z_2 = \dot{z}_1 = L_fz_1 = X_3 $$ $$ z_3 = \dot{z}_2 = L_f^2z_1 = -\frac{MgL}{I} sin(X_1) -\frac{k}{I} (X_1 - X_2) $$ $$ z_4 = \dot{z}_3 = L_f^3 z_1 = -\frac{MgL}{I} cos(X_1) -\frac{k}{I} (X_3 - X_4) $$The control appears in the derivative of \( z_4 \) as \( L_gL_f^3z_1 \neq 0 \) and \( L_gL_f^iz_1 = 0 \) for i = 0,1 and 2.
$$ \dot{z}_4 = L_f^4z_1 + L_gL_f^3z_1 u $$$$ \dot{z}_4 = \frac{MgL}{I}sin(X_1) \left( X_3^2 + \frac{MgL}{I} cos(X_1) + \frac{K}{I} \right) + \frac{K}{I}(X_1 - X_2 ) \left( \frac{K}{I} + \frac{K}{J} + \frac{MgL}{I} cos(X_1) \right) + \frac{K}{JI}u = v $$ $$ \ddddot{Z}_1 = Z^{(4)}_1= \frac{d^4z_1}{dt^4} = v $$