at University of California, Irvine

# Rotary Inverted Pendulum Example

The rotary inverted pendulum control example is a case study to demonstrate model-based design of cyber-physical systems. Controlling an inverted pendulum is a classic example to show how feedback control loop can be used to stabilize an inherently unstable system, and how parameterization of physical and cyber attributes can change the behavior of the system.

In this article we use the inverted pendulum example to go through the typical CPS design process step by step. These steps include mathematical modeling, control algorithm design, simulation software development and design space exploration using a simulation-based approach.

A specific research problem we can address with this example is managing power and energy in cyber physical systems. In this problem, the main question is how to select cyber system parameters to get the optimal power consumption. Similarly, we can study the effect of physical system parameters as well as cyber parameter to obtain a holistic model of the CPS containing notion of both actuation and processing power. In detail we discuss the following items:

# Inverted Pendulum Problem Definition

The primary goal of the inverted pendulum system is to stabilize the pendulum in an upright position. Therefore, a servo arm has to be moved appropriately to keep the pendulum angle close to zero with respect to vertical direction. Beside the stability we also consider two non-functional aspects in the problem definition: the control quality and the power consumption requirements.

We implemented the pendulum system in our lab and modeled the system with Simulink and Modelica.

# Implementation

We implemented the inverted pendulum in our lab using the Batan S1213 R/C Servo and EKC-LM3S6965 Texas Instruments ARM Cortex-M3 evaluation board. Pendulum angle and angular speed is measured by S1 Optical Shaft Encoder.

The source codes of the ARM microcontroller is available at "use-batan-servo" branch of following Github repository:

# Modeling

In order to design the control algorithm and also simulate the system, we need to model it first. Rotary inverted pendulum is simple enough to make us able to use direct equation derivation method. In this approach, the mathematical equations are derived using laws of Physics.

Rotary inverted pendulum consists of mechanical and electro-mechanical parts. Therefore, using laws of Dynamics and Electromagnetic we can obtain the required equations. First, the equations of subsystems, i.e. pendulum dynamics and servo motor are generated and then they will be put together to build the overall physical system model.

### Dynamic Equations

We need to derive the differential equations which explains the relation between the variables $\alpha$ and $\theta$ and also the effect of motor torque $\tau$ (the actuation variable) to the servo arm angle $\alpha$.

Using Newton-Euler method we can derive these equations as following *:

* To see the symbol definitions used in the above equations and rest of the page click on Symbol Definitions.

 $\alpha$ Actuator angle $\alpha_m$ Motor angle (before gearbox) $\boldsymbol\alpha$ Angular acceleration vector of pendulum w.r.t. the Inertial reference frame $\mathbf{a}_X$ Acceleration of point X w.r.t. the Inertial reference frame $B_m$ Motor viscous friction coefficient $\mathbf{F}$ The total force applied by the actuator arm to the pendulum $\mathbf{H}_A$ Angular momentum of pendulum w.r.t. point A $I_{ij}$ Elements of pendulum inertia tensor ($i,j \in \{x,y,z\}$) $i_a$ Motor winding current $J$ Motor, Gearbox and actuator arm moment of inertia $k_b$ Motor back-emf constant $k_t$ Motor torque constant $l$ Actuator arm length $L$ Pendulum length
 $L_a$ Motor winding self inductance $m$ Mass of the pendulum weight $\mu_m$ Actuator viscous friction coefficient $\mu_p$ Pendulum joint friction coefficient $\mathbf{M}_A$ Total Momentum applied to the pendulum w.r.t. point A $n$ Gearbox ratio $\boldsymbol\omega$ Angular velocity vector of pendulum w.r.t. the Inertial reference frame $R_a$ Motor winding resistance $\mathbf{r}_{X/Y}$ Position vector of point X w.r.t. point Y $\mathbf{\tau}$ Torque applied by the actuator $\mathbf{\tau}_m$ Torque applied by the motor (before gearbox) $\theta$ Pendulum angle $v_m$ Voltage applied to the motor terminal $x_i$ i-component of vector $\mathbf{x}$ ($i \in \{x,y,z\}$)

Hide Symbol Definitions.

These equations are highly nonlinear and therefore hard to deal with using linear controllers. The good news is that we can work with the linearized version for the purpose of control algorithm design because when the system is stabilized, the variations of all of the variables in the equations are small and the non-linear dynamic behavior is not unfolded.

Although the linearized dynamic equations should be used for the control algorithm design, the original non-linear version can be used perfectly for the simulation purposes. To do this, we can solve the first two equations for variables $\ddot{\theta}$ and $\ddot{\alpha}$ to get the state-space equations in the following form:

These equations can be implemented easily in simulation tools like Matlab Simulink or Modelica.

To see the details of dynamic equation derivation process click on Derivations.

Assuming the mass of pendulum rod is negligible, we can write the Newton-Euler equations for the rigid body consisting of pendulum weight and rod in the $\{xyz\}$ coordinate frame as following:

\begin{align}
\sum \mathbf{F} &= m \mathbf{a}_G \label{eq:n-e-f} \\
\sum \mathbf{M}_A &= \mathbf{r}_{G/A} \times m \mathbf{a}_G + \frac{d}{dt} \mathbf{H}_A, \label{eq:n-e-m}
\end{align}

Using the definition

\begin{align}
\mathbf{s} = \mathbf{r}_{G/A} \times m \mathbf{a}_A ,
\end{align}

\eqref{eq:n-e-f} and \eqref{eq:n-e-m} can be expanded for the three coordinate axes as following:

\begin{align}
\sum F_x &= m a_x, \\
\sum F_y &= m a_y, \\
\sum F_z &= m a_z, \\
\sum {M_A}_x &= I_{xx} \alpha_x - (I_{yy} - I_{zz}) \omega_y \omega_z + s_x , \label{eq:n-e-m-x}\\
\sum {M_A}_y &= I_{yy} \alpha_y - (I_{zz} - I_{xx}) \omega_z \omega_x + s_y ,\\
\sum {M_A}_z &= I_{zz} \alpha_z - (I_{xx} - I_{yy}) \omega_x \omega_y + s_z ,
\end{align}

in which $I_{ii}$ are the components of inertia tensor matrix. The off-diagonal component of that matrix are all zero and the diagonal components are:

\begin{align}
I_{xx} = mL^2 , \quad I_{yy} = mL^2 , \quad I_{zz} = 0. \label{eq:inertia-tensor}
\end{align}

Next, we will calculate the terms appeared in above equations in terms of angles $\alpha$ and $\theta$ and their derivatives using kinematic equations. The auxiliary coordinate frames $\{x'y'z'\}$ and $\{XYZ\}$ are used for this purpose (see the Figure).

\begin{align}
\boldsymbol\omega &= \dot{\alpha} \mathbf{K} + \dot{\theta} \mathbf{i} \implies \boldsymbol\omega = \dot{\theta} \mathbf{i} + \dot{\alpha}\sin \theta \mathbf{j} + \dot{\alpha}\cos \theta \mathbf{k} \\
\omega_x &= \dot{\theta},\quad \omega_y = \dot{\alpha}\sin \theta,\quad \omega_z = \dot{\alpha}\cos \theta \\
\alpha_x &= \ddot{\theta},\quad \alpha_y = \dot{\theta}\dot{\alpha}\cos \theta + \ddot{\alpha} \sin \theta, \alpha_z = -\dot{\theta}\dot{\alpha}\sin \theta + \ddot{\alpha} \cos \theta \\
\mathbf{s} &= L\mathbf{k} \times m(-l\dot{\alpha}^2\mathbf{i}+l\ddot{\alpha}\mathbf{j'}) = -mLl\dot{\alpha}^2 \mathbf{j'} - mLl\ddot{\alpha}\cos \theta \mathbf {i} \label{eq:vec_s}
\end{align}

Substituting the terms in \eqref{eq:n-e-m-x} by what we have in \eqref{eq:inertia-tensor} to \eqref{eq:vec_s} and adding up all the momentum applied by different forces on the pendulum rigid body system we will get

\begin{align}
-\mu_p \dot{\theta} + mgL\sin \theta = mL^2 \ddot{\theta} - mL^2 \dot{\alpha}^2 \sin \theta \cos \theta - mLl\ddot{\alpha}\cos \theta \label{eq:thdd}
\end{align}

Since we have two variables, we need another equation in addition to \eqref{eq:thdd} to build the state-space (SS) model. Also, we need to involve the torque applied by the actuator in the new equation. For this purpose, torque equation around the axis $Z$ of the $\{XYZ\}$ can be written as following

\begin{align}
J \ddot{\alpha} = \tau - \mathbf{K}.(l\mathbf{i}\times\mathbf{F}) - \mu_m \dot{\alpha}. \label{eq:aldd}
\end{align}

Then, we need to calculate $\mathbf{F}$ using \eqref{eq:n-e-f} and substitute the result in \eqref{eq:aldd}. To do this, first we need to obtain $\mathbf{a}_G$ using following equation:

\begin{align}
\mathbf{a}_G =& \mathbf{a}_A + \alpha \times \mathbf{r}_{G/A} + \boldsymbol\omega \times (\boldsymbol\omega \times \mathbf{r}_{G/A}) \nonumber \\
=& (-l \dot{\alpha}^2 + L \sin \theta \ddot{\alpha} ) \mathbf {i}
+ (l\cos\theta \ddot{\alpha} - L \ddot{\theta} + L \sin \theta \cos \theta \dot{\alpha}^2) \mathbf{j} \nonumber \\
&+ (-l\sin \theta \ddot{\alpha} - L \sin^2 \theta \dot{\alpha}^2 - L \dot{\theta}^2) \mathbf{k} \nonumber \\
=& F_x \mathbf{i} +F_y \mathbf{j} + F_z \mathbf{k} \label{eq:F}
\end{align}

Now we can use \eqref{eq:F} to rewrite \eqref{eq:aldd} as following

\begin{align}
J \ddot{\alpha} = \tau - lF_y \cos \theta + lF_x \sin \theta - \mu_m \dot{\alpha}. \label{eq:aldd2}
\end{align}

Hide Derivations.

### Actuator Modeling

In our example, the Actuator is a geared DC motor. Following diagram shows the widely used for DC motor which is used to derive the actuator equations

The equations that describes the actuator dynamics are

\begin{align*}
L_a \dot{i}_a + R_a i_a &= v_m - K_b \dot{\alpha}_m \\
\tau_m &= K_t i_a - B_m \dot{\alpha}_m
\end{align*}

Also, assuming the gearbox attached to the DC motor is ideal we will have

\begin{align*}
\tau &= \tau_m n \\
\alpha &= \alpha_m / n
\end{align*}

where $n$ is the gear ration.

These above equations are linear and we neglect the nonlinearities of DC motor dynamics.

# Control Algorithm Design

With the models of physical plant and actuator we can design the controller. The overall system model including the controller is like the block diagram shown here. All the physical components including the actuator (dc motor and gearbox) and pendulum dynamics are shown inside the dashed box. The controller sees the whole thing as single component.

LQR controller has been used for stabilizing the inverted pendulum. Actually, it is also used in many similar works. The main objective of LQR design is to minimize a linear combination of control performance and an energy consumption metrics. The coefficients can be tuned to get the required trade-off between those metrics. One example of a const function is

\begin{align*}
J = \int_0^\infty \left( q||\alpha-\alpha_r||^2 +v_m^2 \right) dt \label{eq:cost2}
\end{align*}

$||\alpha-\alpha_r||^2$ represents the tracking error , that is the difference between the actual servo arm angle and the commanded angle. Therefore, it is a metric of control performance. $v_m^2$ is the squared value of the motor terminal voltage which is roughly proportional to the motor power consumption. The constant q can be used to adjust the trade-off between the control performance and power consumption.

From LQR theory we know that the control law will be the simple proportional state feedback. In our example it means that we should apply the motor voltage using the following equation:

\begin{align*}
v_m = - K \mathbf{x},
\end{align*}

$\mathbf{x}$ is the vector containing all the system states and $K$ is a matrix gain. $K$ can be calculated using off-the-shelf packages like lqr function of Matlab Control toolbox. Before doing that we should linearize the system and express in the standard linear state-space form. We also need to combined the stat-space models of components inside the dashed block (actuator and pendulum dynamics into a single state-space model.

To see the linear state-space equation of our example click on linear state-space model.

Linearized State-Space model of pendulum dynamics:

\begin{align}
\dot{\mathbf{x}}_p = A_p \mathbf {x}_p + B_p u_p, \label{eq:ss}
\end{align}

where

\begin{align}
\mathbf{x}_p &=(\begin{array}{cccc}\alpha & \dot{\alpha} & \theta & \dot{\theta}\end{array})^T, \quad u_p = \tau \\
A_p &= \left. \frac{\partial \mathbf{f}}{\partial \mathbf{x}_p} \right|_{\mathbf{x}_p=\mathbf{x_{p0}}=\mathbf{0}} = \left( \begin{array}{cccc}
0 & 1& 0& 0 \\
0 & \frac{-\mu_m}{J}& \frac{glm}{J}& -\frac{l\mu_p}{JL} \\
0 & 0& 0& 1 \\
0 & -\frac{l\mu_m}{JL}& \frac{g(ml^2 + J)}{JL}& -\frac{\mu_p(ml^2 + J)}{JL^2m}
\end{array} \right) \\
B_p &= \left. \frac{\partial \mathbf{f}}{\partial u_p} \right|_{u_p=u_{p0}=0} =
\left( \begin{array}{cccc} 0 & \frac{1}{J} & 0 & \frac{l}{JL} \end{array} \right)^T
\end{align}

Linearized State-Space model of actuator (DC motor):

\begin{align}
\dot{x}_a &= A_a x_a + B_a \mathbf{u}_a = -\frac{R_a}{L_a} x_a + (\begin{array}{cc} \frac{1}{L} & -\frac{K_a}{L_a} \end{array}) \mathbf{u}_a \label{eq:mot-ss1} \\
y &= C_a x_a + D_a \mathbf{u}_a = K_t x_a + (\begin{array}{cc} 0 & -B_m \end{array}) \mathbf{u}_a \label{eq:mot-ss2}
\end{align}

where

\begin{align}
\end{align}

Hide linear state-space model.

# Simulation

### Servo Angle(deg) vs Time(s)

3D animation shown above demonstrates the results of simulation. The simulation model is built using both Simulink and OpenModelica. Click on "Free Fall" and "LQR Control" button to see the two different scenarios:

• Free fall: in this scenario, no torque is applied by the actuator to the pendulum. Therefore, just a small deviation from vertical angle causes the pendulum to fall down. Thanks to nonlinear equations we can observe the large signal behaviour of the physical system.
• LQR Control: in this scenario, the LQR controller is in place and keeps the pendulum in upright position while moves it from one place to another back and forth. The square wave signal in the servo angle plots shows the commanded signal and the yellow curve shows the actual value which is the result of closed-loop control. Although the angular deviations are relatively small, the nonlinear simulation is still useful because it can be used to verify the goodness of approximation caused by linearizing the original non-linear equations.

We can use the simulation not only to verify the design process, but also for design space exploration and fine tuning the system design done so far or to explore the dimensions which are not included in the initial system design. For example, all the design process at previous stages include only continuous-time modeling of the whole system (physical system + controller). But, in CPS we are implementing the continuous-time controller using a discrete-time cyber system. Simulation model gives us the opportunity to study all the imperfections caused by discretization process.

# Design Space Exploration

The simulation software developed in previous stages can be used as a tool to explore the design space. For this reason, we can simply run the simulation software for every setting point in the design space. However, since the design space grows exponentially as we add more dimensions to the problem, running the simulation model is costly. To reduce the computation time, we can consider two solutions to mitigate this issue. First, we should try to produce the fastest possible version of the simulation software. Second, we should leverage parallel computing tools because different setting points in the design space are independent of each other.

We have used OpenModelica to develop simulation software and the increase the speed of each simulation run. OpenModelica translate the simulation model to native C++ software which causes considerable speed up as opposed to interpreter-based solutions like Matlab Simulink. Although Simulink also comes with a compiler that can translate the simulation model, our tests showed that Modelica provides better performance and flexibility especially when we change the sampling time parameter of the controller subsystem.

To realize the second solution, i. e. to get effective use of parallel computing platforms, we have used multiprocessing package in the Python script which is developed to run the simulation software in nested iterations of each design dimension. By doing this, we can make use of all the CPU cores available on the parallel processing platform.

The explained methodology can be used to answer many CPS design question. Two examples of these questions are explained as follows:

#### What is the relation of the pendulum length to the controller clock speed?

From experience with pendulums with different length we know intuitively that longer pendulum length results in slower physical dynamics. Therefore we expect that we can increase control task period which leads to lower CPU frequencies. But, what is the exact relation.

To get the answer to this question we can run the experiment for different values of length and control task period and see how the system response changes. The figure shown above shows the result of such analysis. For a fixed value of pendulum length, the system goes toward instability as we increase the control task period. Same thing happens for a constant control task period as we decrease the pendulum length. The region of design space in which we have relative stability is shown roughly in the figure.

#### What is the optimal setting for the control task period to get the minimum energy consumption?

The overall energy consumption is the sum of actuation energy and processing energy. We saw that the control design cost function somehow considers the actuation energy but for two main reasons we need to extend our model to support following properties of the actual system: 1- The cost function represents the continuous-time actuation energy usage, but after discretizing the system (running the controller as a embedded system task with some period) it is not a valid energy expression. 2- The dc-motor power is not exactly proportional to the squared value of terminal voltage because it is not a pure resistive load.

We can use the simulation to calculate the energy using more accurate equation comparing to what is considered the LQR cost function. Also, we can add the processing energy to the total energy consumption in the simulation model. For shorter control task periods we expect better control performance and less actuation energy consumption. In the other hand processing power is higher for shorter task periods because we have to invoke the control procedure more frequently in unit of time in that case. This intuition suggests that there should be an optimal setting for control task period that minimizes the total energy consumption. Following figure made by running the simulation software for different values of the control period (sampling time) shows that this intuition is true in our example. In this figure, $q$ is the weight parameter in LQR controller cost function. (see Control Algorithm Design in this page)

# Related Publications

[1] Hamid Mirzaei, Steffen Peter, and Tony Givargis; Including Variability of Physical Models into the Design Automation of Cyber-Physical Systems; 52nd Design Automation Conference (DAC); 2015

 i = 0;loadAngles();