## Aim

Accurately model an inverted pendulum to use for control algorithms development for physical implementation.

Personal goal: refresh basic understanding of modelling and control.

## Newton’s law

The cornerstone for obtaining a mathematical model, or the equations of motion for any mechanical system is Newton’s law [1]

**F** = the vector sum of all forces applied to each body in the system, newtons [N]**a** = vector acceleration of each body with respect to an inertial reference frame, m/s^{2}

m = mass of body, kg

The application of Newton’s law to one-dimensional rotational systems requires the equation to be modified to

M = the sum of all external moments about the centre of mass of a body, N.m

I = the body’s mass moment of inertia about it’s centre of mass, kg.m^{2}

α = the angular acceleration of the body, rad/s^{2}

## Free body diagram

Inverted pendulums usual take one of three forms, either an inverted pendulum on a linear track, inverted pendulum on a cart or a self-balancing robot.

## Equations of motion

With Newton’s law and the self-balancing robot’s free body diagram we can go ahead and write the equations of motion for the system. In this example, the equations of motion for the x and y directions will be derived using the free body diagram of a pendulum on a cart.

### X-axis

Starting with the x-axis and , we can consider the self-balancing robot consisting of a cart (the wheels and motors) with mass M and a pendulum (everything else free to rotate in respect to the wheel and motors) with mass m. The pendulum’s mass is located a distance from the pivot point. For the self-balancing robot the pivot point is the axle.

Summing the forces acting on the robot in the x-direction we have

u = the force provided by the motors and wheels to move the robot in the x-direction

b = friction constant

= first derivative of the robot’s position, it’s velocity

Considering the cart part and the pendulum part we have

= cart’s mass

= cart’s position

= pendulum’s mass

= pendulum’s position

We see the pendulum’s position is dependent on the angle and the cart’s position. With all this information we can now complete the equations of motion in the x-direction for the robot. Starting with Newton’s equation

we have determined consists of the force provided by the wheels/motors and the friction the wheels encounter with the surface

The mass and acceleration, , is that of the combined cart and pendulum. This can be split into the cart’s mass and acceleration and the pendulum’s mass and acceleration resulting in

= cart’s acceleration

= cart’s position

= pendulum’s acceleration

= pendulum’s position

We have already determined the pendulum’s position, , relative to above. This can now be added to the equation to complete the equation of motion.

With the differentiation sum rule this is split into

which can then be rewritten as

As is a constant it can be moved from within the differentiation to join . For easier referencing later, let’s call this equation 1.

[1]

Almost there, only the second derivative of left to calculated using the chain rule. The first derivative is calculated as

This is differentiated once again using the differentiation product and chain rules

All of this can now be plugged back into equation 1 to finish the equation of motion in the x-direction.

[2]

### Y-axis

The equation for motion in the y-direction is a bit easier. The free body diagram we are working with is all the way back at the top of the post. Let’s show it again and consider it for the y-direction.

There is a single force acting in the y-direction, the pendulum’s weight. Knowing this suggests that the equation of motion in this direction will be very easy, but this is not to be the case. Although the force is in one direction the resulting movement will be in both the x and y-directions as the pendulum rotates about the pivot point. To make life a bit easier for ourselves it is easier to write the equation of motion in the direction of travel of the pendulum indicated by the red vector in the figure below.

We start again with Newton’s second law. The force F along the red vector is

As we have written the force along red vector, we need to do the same with the the acceleration vector . Previously we have already determined the pendulum’s x-position . We now do the same for its y-position .

= pendulum’s x-position

= pendulum’s y-position

The x and y-positions are then written as their components along the red vector.

Replacing and with their definition we get

Luckily we have already calculated the second order derivative of and can reuse it here. Along with calculating the second order derivative of we get

This can be simplified to equation 3, the equation of motion in the y-direction. The simplification is achieved by two of the terms in the equation above adding to 0 and using the identity below

[3]

## Linearisation

Both equations [2] and [3] are clearly non-linear. While we can attempt to do some non-linear control, for now it is best to keep things simple. As we want to balance the robot, we wish for the value of to be around zero. We will linearise the equations around this point. Later on when we feel brave we can linearise at some other points as well and maybe use gain scheduling for control over a wider angle range, but later.

Linearising around close to zero degrees we can do the following approximations

### X-axis

Starting with the equation of motion in the x-direction, using these approximations we can take equation 2

[2]

and simplify it to

[4]

We are still left with in the equation that can make life difficult. As we have already said the angle should remain 0, we can also assume that the angular speed will be close to 0, i.e. before we have significant angular speed, we aim to control the angle and bring it back to zero. Thus, if we take we can further simplify the equation to

[5]

### Y-axis

Moving to the equation of motion in the y-direction, doing the same with equation 3

[3]

we get

[6]

## State space representation

To complete the modelling of the self-balancing robot I will simulate its behaviour, probably using MATLAB Simulink. It can also be done in Python, C, or really anything else, including Excel. The non-linear equations of motion we have derived above is also already sufficient for that purpose, but ultimately the goal is to develop a control strategy. To assist it makes sense to represent the linearised equation of motion in state-space as well as a transfer function. Having both state-space and transfer function representations will allow us to later explore different methods of controlling the robot.

So, what is the state-space representation? Wikipedia or any textbook does a much better job at explaining this than I ever will, but in short, from Wikipedia, it is a mathematical model of a physical system as a set of input, output and state variables related by first order differential equations. The block diagram illustrates this statement.

The output, , can be written as the sum of the product of the input, , and the vector and the product of state, and vector . The state in turn is writen as the sum of product of the input, , and the vector and the product of state and vector . This is written as

With this limited background on the state space representation we proceed to writing the state space representation of our linearised equations of motion.

First we define our state variables

With the state space representation, we will write our equation of motion in the following form leaving us to calculate the values of the matrices A, B, C and D.

The outputs of the system we are interested in are position and angle . We have already defined these as 2 of our state variables therefor we already have the matrices C and D defined:

Next, we need to determine the values of the A and B matrices. This can be done by solved using the linearised equations 5 and 6 describing the motion in both the x and y-direction.

[5]

[6]

Some mathenatical manipulation is required. Taking equation 5 we can rewrite this to

We notice is already defined in equation 6 to be equal to . Replacing with we get

With this we determine

[7]

This we can write in terms of our state variables

[8]

The only unknown to complete our state space representation now is better known as . As we now know what is, see equation 7 above, we can just pop that back into equation 6 to determine

[6]

This we can write in terms of our state variables

[9]

With that we have all the information we need about our state variables

Writing this in matrix form, along with the output in matrix form we already done before, we complete the state space representation.

[10]

[11]

## Transfer functions

Writing the transfer function should be a little bit easier, most of what we need we have already calculated. The transfer function takes the equations of motion and write them as a function of the output to the input, that is with the output and the input. For the self-balancing robot, we will have two transfer functions, one translating the input to the angle output and another translating input to the position output .

The Laplace transform is vital tool to solve differential equations and for us to write the transfer functions . We will not go into detail of the function but is recommended to read and understand its purpose.

It is worth noting that a transform exist to move between the state space representation and a transfer function.

### Angle transfer function

To start we will determine the transfer function for the input to angle output . That is

To do this we take the Laplace transform of the equations of motion 5 and 6 calculated earlier and then solve them for . As a reminder our equations of motion are

[5]

[6]

#### Laplace transform

The Laplace transform of

[5]

is

[12]

Next we get the Laplace transform of

[6]

to be

[13]

We can rewrite equation 13 with as the subject and to replace in equation 12 to enable us to relate directly to .

[13]

[14]

Let’s take equation 14 and replace with this in equation 12

[12]

This simplify to

Which can now be written as our transfer function in equation 15

[15]

### Position transfer function

The same steps as for the angle transfer function are followed to get the position transfer function. We already have the Laplace transforms and don’t need to do that again. Instead of rewriting equation 13 with as the subject, rewrite it with as the subject.

[13]

[16]

As before, we can now pop equation 16 back into equation 12 to replace

[12]

This can be simplified to

And then rewritten as our transfer function in equation 17

[17]

## Modelling

The last step in this post is to model our different interpretations of the equations of motion. Using tools such as MATLAB and MATLAB Simulink we can model the non-linear equations of motion (equations 2 and 3) with our linearised state-space and transfer function representation. Hopefully every line up and makes sense. If not, we will quickly see if we have made a mistake along the way.

### Modelling non-linear equation of motion in Simulink

The non-linearised equations of motion, equations 2 and 3, are modelled in Simulink by rewriting the functions with and the subjects. We take our equations of motion

[2]

[3]

and rearrange them to have

These equations are represented in the function Simulink block as below. We take the output of the and calculations and integrate them to get the first derivative and values of and . For simplicity the initial values of all integrals, the position and angle are set to zero.

### State space and transfer function representation in Simulink

The state space representation and transfer functions we have calculated are modelled in Simulink using the continuous State Space and transfer function blocks.

### Comparison

With all the different models and representations of our self-balancing robot we can compare the outputs.

As expected, the output of the two linearised representations, state space and transfer functions, match exactly. What is more interesting is to look at the difference between the non-linear and linear models

The non-linear and linearised models compare very well for small values of validating the linearisation assumptions we have made. In the example we apply a 0.2 step at 1 s. The models compare well up until 1.6 s where they start to diverge. At this time the pendulum angle is -0.5 radians or -28 degrees.

## Next steps

The next step is to use our linearised models to design a control system to control our self-balancing robot. Before this can be done the robot itself needs to be built to measure its properties to plug into the model.

The non-linear model will be used as the plant. It is worth noting that all the models we have made in this post make use of continuous time. Our control system will be running on discreet time. This is very important and will need to be taken into account during the design of our control strategy as well as modelling of the controller and plat later on.

## Bibliography

- Gene F Franklin, J David Powell and Abbas Emami-Naeni: Feedback control of dynamic systems. 4th Edition. Prentice Hall, 2002
- P. Frankovský, L. Dominik, A. Gmiterko, I. Virgala: Modeling of two-wheeled self-balancing robot driven by DC gearmotors. Department of Mechatronics, Faculty of Mechanical Engineering, Technical University of Košice, 2017
- Hanna Hellman, Henrik Sunnderman: Two-wheeled self-balancing robot. KTH Royal Institute of Technology, Stockholm, Sweden
- State space representation, Wikipedia: https://en.wikipedia.org/wiki/State-space_representation