This is part of a series of posts covering the development of a self-balancing robot:
- This post
- Building a self-balancing robot
- Dertermining the centre of gravity for a self-balancing robot
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/s2
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.m2
α = the angular acceleration of the body, rad/s2
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
2 thoughts on “Modelling an inverted pendulum – deriving a mathematical model”