LQR Controller for an Inverted Pendulum on a Cart - Maple Help

Home : Support : Online Help : Applications and Example Worksheets : Science and Engineering : LQR Controller for an Inverted Pendulum on a Cart

LQR Controller for an Inverted Pendulum on a Cart

Introduction

This worksheet derives the equations that describe the dynamics of an inverted pendulum on a cart, creates a linear quadratic state (LQR) controller that stabilizes the position of the pendulum, and animates the motion of the controlled cart.

Note: This application uses the LQR command from optional Control Design Toolbox to calculate the parameters of an LQR controller. The parameters generated by this command are copied into this application. This allows you to run the application without the toolbox.

 > $\mathrm{restart}:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{with}\left(\mathrm{DynamicSystems}\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{with}\left(\mathrm{LinearAlgebra}\right):$

Model Of Inverted Pendulum on Cart

We will use the following variables and constants in the analysis.

 • $\mathrm{x}\left(\mathrm{t}\right)$ is the position of the cart
 • $\mathrm{\theta }\left(\mathrm{t}\right)$ is the counter-clockwise angular displacement of the pendulum from the upright position
 • $\mathrm{\phi }\left(\mathrm{t}\right)$ is the angular velocity of the pendulum
 • u(t) is horizontal force applied to the cart
 • L is the half-length of the pendulum
 • m is the mass of the pendulum
 • M is the mass of the cart
 • P is the downward force exerted by the pendulum on the cart
 • N is the horizontal force exerted by the pendulum on the cart
 • g is the gravitational constant

Let $\mathrm{y}\left(\mathrm{t}\right)=\frac{ⅆ}{ⅆ\mathrm{t}}\mathrm{x}\left(\mathrm{t}\right)$ and $\mathrm{φ}\left(\mathrm{t}\right)=\frac{ⅆ}{ⅆ\mathrm{t}}\mathrm{theta}\left(\mathrm{t}\right)$.

 > $\mathrm{EQ1}≔\stackrel{\mathbf{.}}{\mathrm{x}}\left(\mathrm{t}\right)=\mathrm{y}\left(\mathrm{t}\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{EQ2}≔\stackrel{\mathbf{.}}{\mathrm{\theta }}\left(\mathrm{t}\right)=\mathrm{\phi }\left(\mathrm{t}\right):$

is the position of the center of mass of the pendulum.

 >

The acceleration of the center of mass is then:

 > $\mathrm{acc}≔\frac{{ⅆ}^{2}}{{ⅆ}^{\phantom{\rule[-0.0ex]{0.2em}{0.0ex}}}{\mathrm{t}}^{2}}\mathrm{r}$
 ${\mathrm{acc}}{≔}\left[\stackrel{{\mathbf{..}}}{{x}}{}\left({t}\right){-}{L}{}\stackrel{{\mathbf{..}}}{{\mathrm{\theta }}}{}\left({t}\right){}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){+}{L}{}{\stackrel{{\mathbf{.}}}{{\mathrm{\theta }}}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){,}{-}{L}{}\stackrel{{\mathbf{..}}}{{\mathrm{\theta }}}{}\left({t}\right){}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){-}{L}{}{\stackrel{{\mathbf{.}}}{{\mathrm{\theta }}}{}\left({t}\right)}^{{2}}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right)\right]$ (2.1)

Rewrite the acceleration in terms of the cart velocity and angular velocity of pendulum.

 > $\mathrm{rpp}≔\mathrm{subs}\left(\left[\mathrm{EQ1},\mathrm{EQ2}\right],\mathrm{acc}\right)$
 ${\mathrm{rpp}}{≔}\left[\stackrel{{\mathbf{.}}}{{y}}{}\left({t}\right){-}{L}{}\stackrel{{\mathbf{.}}}{{\mathrm{\phi }}}{}\left({t}\right){}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){+}{L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){,}{-}{L}{}\stackrel{{\mathbf{.}}}{{\mathrm{\phi }}}{}\left({t}\right){}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){-}{L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right)\right]$ (2.2)

Apply F=ma in the horizontal (x-direction) to the pendulum.

 >

${\mathrm{eq1}}{≔}{N}{=}{m}{}\left(\stackrel{{\mathbf{.}}}{{y}}{}\left({t}\right){-}{L}{}\stackrel{{\mathbf{.}}}{{\mathrm{\phi }}}{}\left({t}\right){}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){+}{L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right)\right)$

Apply F=ma in the direction perpendicular to the pendulum.

 > $\mathrm{eq2}≔\left(\mathrm{P}+\mathrm{g}\right)\mathrm{sin}\left(\mathrm{\theta }\left(\mathrm{t}\right)\right)-\mathrm{N}\mathrm{cos}\left(\mathrm{\theta }\left(\mathrm{t}\right)\right)=\mathrm{m}\mathrm{L}\stackrel{\mathbf{.}}{\mathrm{\phi }}\left(\mathrm{t}\right)-\mathrm{m}\stackrel{\mathbf{.}}{\mathrm{y}}\left(\mathrm{t}\right)$

${\mathrm{eq2}}{≔}\left({P}{+}{g}\right){}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){-}{N}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){=}{m}{}{L}{}\stackrel{{\mathbf{.}}}{{\mathrm{\phi }}}{}\left({t}\right){-}{m}{}\stackrel{{\mathbf{.}}}{{y}}{}\left({t}\right)$

Apply F=ma in the horizontal direction to the cart.

 > $\mathrm{eq3}≔\mathrm{u}\left(\mathrm{t}\right)-\mathrm{N}=\mathrm{M}\stackrel{\mathbf{.}}{\mathrm{y}}\left(\mathrm{t}\right)$

${\mathrm{eq3}}{≔}{u}{}\left({t}\right){-}{N}{=}{M}{}\stackrel{{\mathbf{.}}}{{y}}{}\left({t}\right)$

Apply $\mathrm{M}=\mathrm{I}\mathrm{alpha}$ to the pendulum, where $\mathrm{M}$ is the sum of all moments about the pendulum's center of mass, $\mathrm{I}$ is the pendulum's moment of inertia, and $\mathrm{alpha}$ is its angular acceleration.

 >

${\mathrm{eq4}}{≔}{P}{}{L}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){-}{N}{}{L}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){=}\frac{{m}{}{{L}}^{{2}}{}\stackrel{{\mathbf{.}}}{{\mathrm{\phi }}}{}\left({t}\right)}{{3}}$

 > $\mathrm{eqns}≔\mathrm{solve}\left(\left[\mathrm{eq1},\mathrm{eq2},\mathrm{eq3},\mathrm{eq4}\right],\left[\mathrm{N},\mathrm{P},\stackrel{\mathbf{.}}{\mathrm{\phi }}\left(\mathrm{t}\right),\stackrel{\mathbf{.}}{\mathrm{y}}\left(\mathrm{t}\right)\right]\right)\left[\right]$

${\mathrm{eqns}}{≔}\left[{N}{=}\frac{{2}{}{L}{}{M}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{-}{3}{}{M}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{-}{3}{}{u}{}\left({t}\right){}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{u}{}\left({t}\right){}{m}}{{-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}}{,}{P}{=}\frac{{2}{}{L}{}{M}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{-}{L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{{m}}^{{2}}{-}{3}{}{M}{}{{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{-}{3}{}{u}{}\left({t}\right){}{{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right)}^{{2}}{}{m}{+}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{M}{+}{2}{}{u}{}\left({t}\right){}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{m}{+}{u}{}\left({t}\right){}{m}}{\left({-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}\right){}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right)}{,}\stackrel{{\mathbf{.}}}{{\mathrm{\phi }}}{}\left({t}\right){=}{-}\frac{{3}{}\left({L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{{m}}^{{2}}{-}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{M}{-}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{m}{-}{u}{}\left({t}\right){}{m}\right)}{\left({-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}\right){}{m}{}{L}}{,}\stackrel{{\mathbf{.}}}{{y}}{}\left({t}\right){=}{-}\frac{{2}{}{L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{-}{2}{}{u}{}\left({t}\right)}{{-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}}\right]$

 > $\mathrm{EQ3}≔\stackrel{\mathbf{.}}{\mathrm{y}}\left(\mathrm{t}\right)=\mathrm{eval}\left(\stackrel{\mathbf{.}}{\mathrm{y}}\left(\mathrm{t}\right),\mathrm{eqns}\right)$
 ${\mathrm{EQ3}}{≔}\stackrel{{\mathbf{.}}}{{y}}{}\left({t}\right){=}{-}\frac{{2}{}{L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{-}{2}{}{u}{}\left({t}\right)}{{-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}}$ (2.3)



 > $\mathrm{EQ4}≔\stackrel{\mathbf{.}}{\mathrm{\phi }}\left(\mathrm{t}\right)=\mathrm{eval}\left(\stackrel{\mathbf{.}}{\mathrm{\phi }}\left(\mathrm{t}\right),\mathrm{eqns}\right)$

${\mathrm{EQ4}}{≔}\stackrel{{\mathbf{.}}}{{\mathrm{\phi }}}{}\left({t}\right){=}{-}\frac{{3}{}\left({L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{{m}}^{{2}}{-}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{M}{-}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{m}{-}{u}{}\left({t}\right){}{m}\right)}{\left({-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}\right){}{m}{}{L}}$

The final nonlinear model is:

 > $\mathrm{sysEqs}≔\mathrm{Vector}\left(\left[\mathrm{EQ1},\mathrm{EQ2},\mathrm{EQ3},\mathrm{EQ4}\right]\right)$

${\mathrm{sysEqs}}{≔}\left[\begin{array}{c}\stackrel{{\mathbf{.}}}{{x}}{}\left({t}\right){=}{y}{}\left({t}\right)\\ \stackrel{{\mathbf{.}}}{{\mathrm{\theta }}}{}\left({t}\right){=}{\mathrm{\phi }}{}\left({t}\right)\\ \stackrel{{\mathbf{.}}}{{y}}{}\left({t}\right){=}{-}\frac{{2}{}{L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{-}{2}{}{u}{}\left({t}\right)}{{-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}}\\ \stackrel{{\mathbf{.}}}{{\mathrm{\phi }}}{}\left({t}\right){=}{-}\frac{{3}{}\left({L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{{m}}^{{2}}{-}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{M}{-}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{m}{-}{u}{}\left({t}\right){}{m}\right)}{\left({-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}\right){}{m}{}{L}}\end{array}\right]$

Linearization and State Space Model

Assign values to the parameters.

 > $\mathrm{param_values}≔\mathrm{g}=9.8,\mathrm{M}=10.,\mathrm{m}=2.,\mathrm{L}=3.0:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{assign}\left(\mathrm{param_values}\right)\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$

The linearization point:

 >

Linearize the system and create a state-space model.

 >
 ${\mathrm{sysLin}}{≔}\left[\begin{array}{c}{\mathbf{State Space}}\\ {\mathrm{continuous}}\\ {\mathrm{2 output\left(s\right); 1 input\left(s\right); 4 state\left(s\right)}}\\ {\mathrm{inputvariable}}{=}\left[{{u}}_{{1}}{}\left({t}\right)\right]\\ {\mathrm{outputvariable}}{=}\left[{{y}}_{{1}}{}\left({t}\right){,}{{y}}_{{2}}{}\left({t}\right)\right]\\ {\mathrm{statevariable}}{=}\left[{{x}}_{{1}}{}\left({t}\right){,}{{x}}_{{2}}{}\left({t}\right){,}{{x}}_{{3}}{}\left({t}\right){,}{{x}}_{{4}}{}\left({t}\right)\right]\end{array}\right{,}\left[{{x}}_{{1}}{}\left({t}\right){=}{\mathrm{\phi }}{}\left({t}\right){,}{{x}}_{{2}}{}\left({t}\right){=}{\mathrm{\theta }}{}\left({t}\right){,}{{x}}_{{3}}{}\left({t}\right){=}{x}{}\left({t}\right){,}{{x}}_{{4}}{}\left({t}\right){=}{y}{}\left({t}\right)\right]{,}\left[{{u}}_{{1}}{}\left({t}\right){=}{u}{}\left({t}\right)\right]{,}\left[{{y}}_{{1}}{}\left({t}\right){=}{\mathrm{\theta }}{}\left({t}\right){,}{{y}}_{{2}}{}\left({t}\right){=}{x}{}\left({t}\right)\right]$ (3.1)

Note that the states are in the order given in z.

 > $\mathrm{z}≔⟨\mathrm{φ}(\mathrm{t}),\mathrm{θ}(\mathrm{t}),\mathrm{x}(\mathrm{t}),\mathrm{y}(\mathrm{t})⟩:$

Hence, the A and B matrices are:

 > $\mathrm{A}≔\mathrm{sysLin}\left[1\right]:-\mathrm{a}$
 > $\mathrm{B}≔\mathrm{sysLin}\left[1\right]:-\mathrm{b}$

The transfer function of the system is:

 > $\mathrm{sysTF}≔\mathrm{TransferFunction}\left(\mathrm{sysLin}\left[1\right]\right);\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{tf}≔\mathrm{sysTF}:-\mathrm{tf}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$
 ${\mathrm{sysTF}}{≔}\left[\begin{array}{c}{\mathbf{Transfer Function}}\\ {\mathrm{continuous}}\\ {\mathrm{2 output\left(s\right); 1 input\left(s\right)}}\\ {\mathrm{inputvariable}}{=}\left[{{u}}_{{1}}{}\left({s}\right)\right]\\ {\mathrm{outputvariable}}{=}\left[{{y}}_{{1}}{}\left({s}\right){,}{{y}}_{{2}}{}\left({s}\right)\right]\end{array}\right$
 ${\mathrm{tf}}{≔}\left[\begin{array}{c}\frac{{0.0555555555599999973}}{{{s}}^{{2}}{-}{3.26666666699999997}}\\ \frac{{0.1111111111}{}{{s}}^{{2}}{-}{0.2722222222}}{{{s}}^{{4}}{-}{3.266666667}{}{{s}}^{{2}}}\end{array}\right]$ (3.2)

Controllability and Observability Matrices

 > $\mathrm{CC}≔\mathrm{ControllabilityMatrix}\left(\mathrm{sysLin}\left[1\right]\right)$
 > $\mathrm{OO}≔\mathrm{ObservabilityMatrix}\left(\mathrm{sysLin}\left[1\right]\right)$
 ${\mathrm{OO}}{≔}\left[\begin{array}{cccc}{0}& {1}& {0}& {0}\\ {0}& {0}& {1}& {0}\\ {1.}& {0.}& {0.}& {0.}\\ {0.}& {0.}& {0.}& {1.}\\ {0.}& {3.26666666699999997}& {0.}& {0.}\\ {0.}& {1.63333333299999994}& {0.}& {0.}\\ {3.26666666699999997}& {0.}& {0.}& {0.}\\ {1.63333333299999994}& {0.}& {0.}& {0.}\end{array}\right]$ (4.1)

The controllability and observability matrices have full rank; hence, the system is controllable and observable.

 > $\mathrm{Rank}\left(\mathrm{CC}\right);\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{Rank}\left(\mathrm{OO}\right)$

${4}$

${4}$

The eigenvalues of A:

 > $\mathrm{fnormal}\left(\mathrm{Eigenvalues}\left(\mathrm{A}\right)\right)$

$\left[\begin{array}{c}{-1.807392228}{+}{0.}{}{I}\\ {1.807392228}{+}{0.}{}{I}\\ {0.}{}{I}\\ {0.}{}{I}\end{array}\right]$

The poles of the transfer function are:

 > $\mathrm{fsolve}\left(\mathrm{denom}\left(\mathrm{tf}\left[2,1\right]\right),\mathrm{s}\right);$

${-1.807392228}{,}{0.}{,}{0.}{,}{1.807392228}$

The eigenvalues are equal to the poles of the transfer function since the system is controllable and observable. Since there is an eigenvalue in the left half plane, the system is unstable.

LQR Controller Design

Specify the weights on the input variables.

 > $\mathrm{Q}≔\mathrm{DiagonalMatrix}\left(\left[1,0,1,0\right]\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{R}≔\mathrm{IdentityMatrix}\left(1\right):$

Note: The following command uses the optional Control Design Toolbox. The output of this command is copied below so that you can use this application without the toolbox.

 > $#\mathrm{K≔ControlDesign:-LQR}\left(\mathrm{sysLin}\left[1\right],\mathrm{Q},\mathrm{R}\right)\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{K}≔\left[\begin{array}{cccc}84.4997540652218& 147.768218117330& -1.& -6.02607321694750\end{array}\right]:$

Hence, the controlled input is:

 > $\mathrm{controller}≔\mathrm{u}\left(\mathrm{t}\right)={\left(-\mathrm{K}·\mathrm{z}\right)}_{1}$

${\mathrm{controller}}{≔}{u}{}\left({t}\right){=}{-}{84.4997540652218}{}{\mathrm{\phi }}{}\left({t}\right){-}{147.768218117330}{}{\mathrm{\theta }}{}\left({t}\right){+}{1.}{}{x}{}\left({t}\right){+}{6.02607321694750}{}{y}{}\left({t}\right)$

Simulate the Controlled System

Substitute the controller into the system equations.

 > $\mathrm{sysEqsController}≔\mathrm{eval}\left(\mathrm{sysEqs},\mathrm{controller}\right)$

${\mathrm{sysEqsController}}{≔}\left[\begin{array}{c}\stackrel{{\mathbf{.}}}{{x}}{}\left({t}\right){=}{y}{}\left({t}\right)\\ \stackrel{{\mathbf{.}}}{{\mathrm{\theta }}}{}\left({t}\right){=}{\mathrm{\phi }}{}\left({t}\right)\\ \stackrel{{\mathbf{.}}}{{y}}{}\left({t}\right){=}{-}\frac{{2}{}{L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{+}{168.9995081}{}{\mathrm{\phi }}{}\left({t}\right){+}{295.5364362}{}{\mathrm{\theta }}{}\left({t}\right){-}{2.}{}{x}{}\left({t}\right){-}{12.05214643}{}{y}{}\left({t}\right)}{{-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}}\\ \stackrel{{\mathbf{.}}}{{\mathrm{\phi }}}{}\left({t}\right){=}{-}\frac{{3}{}\left({L}{}{{\mathrm{\phi }}{}\left({t}\right)}^{{2}}{}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{{m}}^{{2}}{-}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{M}{-}{\mathrm{sin}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{g}{}{m}{-}\left({-}{84.4997540652218}{}{\mathrm{\phi }}{}\left({t}\right){-}{147.768218117330}{}{\mathrm{\theta }}{}\left({t}\right){+}{1.}{}{x}{}\left({t}\right){+}{6.02607321694750}{}{y}{}\left({t}\right)\right){}{m}\right)}{\left({-}{3}{}{\mathrm{cos}}{}\left({\mathrm{\theta }}{}\left({t}\right)\right){}{m}{+}{2}{}{M}{+}{2}{}{m}\right){}{m}{}{L}}\end{array}\right]$

The initial conditions (note that the angle of the pendulum is not vertical):

 >
 > $\mathrm{res}≔\mathrm{dsolve}\left(\left\{\mathrm{sysEqsController}\left[1\right],\mathrm{sysEqsController}\left[2\right],\mathrm{sysEqsController}\left[3\right],\mathrm{sysEqsController}\left[4\right],\mathrm{ics}\right\},\left\{\mathrm{\phi }\left(\mathrm{t}\right),\mathrm{\theta }\left(\mathrm{t}\right),\mathrm{x}\left(\mathrm{t}\right),\mathrm{y}\left(\mathrm{t}\right)\right\},\mathrm{type}=\mathrm{numeric},\mathrm{output}=\mathrm{listprocedure}\right):$
 > $\mathrm{res_x}≔\mathrm{eval}\left(\mathrm{x}\left(\mathrm{t}\right),\mathrm{res}\right);$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{res_theta}≔\mathrm{eval}\left(\mathrm{\theta }\left(\mathrm{t}\right),\mathrm{res}\right)$

${\mathrm{res_x}}{≔}{\mathbf{proc}}\left({t}\right)\phantom{\rule[-0.0ex]{0.5em}{0.0ex}}{...}\phantom{\rule[-0.0ex]{0.5em}{0.0ex}}{\mathbf{end proc}}$

${\mathrm{res_theta}}{≔}{\mathbf{proc}}\left({t}\right)\phantom{\rule[-0.0ex]{0.5em}{0.0ex}}{...}\phantom{\rule[-0.0ex]{0.5em}{0.0ex}}{\mathbf{end proc}}$

The angular displacement of the pendulum with respect to time.

 > $\mathrm{plots}:-\mathrm{odeplot}\left(\mathrm{res},\left[\mathrm{t},\mathrm{\theta }\left(\mathrm{t}\right)\right],0..20,\mathrm{background}=\mathrm{ColorTools}:-\mathrm{Color}\left("RGB",\left[218/255,223/255,225/255\right]\right),\mathrm{axis}=\left[\mathrm{gridlines}=\left[\mathrm{color}=\mathrm{RGB}\left(1,1,1\right)\right]\right],\mathrm{size}=\left[800,400\right],\mathrm{axesfont}=\left[\mathrm{Calibri}\right],\mathrm{labelfont}=\left[\mathrm{Calibri}\right]\right)$

The cart displacement with respect to time.

 > $\mathrm{plots}:-\mathrm{odeplot}\left(\mathrm{res},\left[\mathrm{t},\mathrm{x}\left(\mathrm{t}\right)\right],0..20,\mathrm{background}=\mathrm{ColorTools}:-\mathrm{Color}\left("RGB",\left[218/255,223/255,225/255\right]\right),\mathrm{axis}=\left[\mathrm{gridlines}=\left[\mathrm{color}=\mathrm{RGB}\left(1,1,1\right)\right]\right],\mathrm{size}=\left[800,400\right],\mathrm{axesfont}=\left[\mathrm{Calibri}\right],\mathrm{labelfont}=\left[\mathrm{Calibri}\right]\right)$

The pendulum returns to its upright position and the cart returns to its initial position, as intended by the design of the controller.

Animation of Controlled Cart

 >
 >
 >