How-To: Simulate ROCKO
Part 1 - Introduction
ROCKO is an inverted pendulum (pictured right). This means ROCKO will stay perfectly still when balanced, but even a slight angle to either side will cause ROCKO to fall over. In order to keep ROCKO balanced, a force must be applied on the wheels to keep the center of mass (the large black circle on the image to the right) underneath the pole propping it up.
The goal of the following writeup is to show how to create a mathematical model of ROCKO, show how to get the inertial values for the mathematical equations, apply the mathematical equations in MATLAB (or any other simulation software) to simulate ROCKO, and then simulate the control system to ‘tune’ ROCKO without having to actually run him.
Part 2 - Mathematical Model
The goal of this section is to come up with the equations of motion that describe the motion of ROCKO:
The goal is to find the equations of motion that describe the rotation of the robot’s head/leg, and the movement of the robot’s wheel.
The following derivation is for illustration purposes only, and if you are only interested in the results of the equations, skip to the end of this section for the equations of motion.
A Maple script is also attached for the calculations, and it recommended that Maple, or any other math program like Wolfram, be used for accuracy and to prevent tedious calculations.
Step 1, Calculate Lagrange Equation:
As mentioned above ROCKO is an inverted pendulum, which is definitely not a new concept. This means there are many valid approaches to modeling ROCKO, but this specific section concentrates on using the Euler-Lagrange method to derive the equations of motion. This method uses energy as a basis for the equations:
It is important to note that there is a distinction between x and x(t), as well as ẋ and ẋ(t). The ones listed in the Lagrangian equation (x and ẋ) are ‘generic' x’s, and represent all the position variables (which in our case is x(t) and θ(t)).
x(t) and ẋ(t) are specific to ROCKO, and represent the position of the wheel and the velocity of the wheel respectively.
Another point of note is that the Euler-Lagrange method can be used on any robotic/mechanical system, not just ROCKO.
In ROCKO’s case he is treated as a ‘two jointed’ system, one joint is the movement of the wheel and the other join is the rotation of head/leg.
If additional ‘joints’ are added (such as a motor to control the movement up and down of the head), this same method could be used to describe that robot’s motion.
For ROCKO, there are four total sources of kinetic energy:
The rotational speed of the wheel
The linear speed of the wheel
The rotational speed of the head/leg assembly
The linear speed of the head/leg assembly
There is one source of potential energy:
Height of the head/leg assembly
This then gives the following equations for potential and kinetic energy:
One thing to notice is that the rotational speed and the linear velocity of the wheel are actually coupled together (assuming the wheel does not slip), so the following substitution can be made:
This allows for a simplification in the kinetic energy equation:
Since the ratio between the linear speed and rotational speed of the wheel depends on the radius of the wheel (r), a new inertial term is used to combine the rotational and linear inertia of the wheel (referred to as m[wheel,effective] in equations)
More information about how to exactly calculate this value is in part 3.
This gives the following Lagrange equation:
Step 2, Calculate Equations of Motion:
Having obtained the Lagrange equation, next we need to use a little bit of calculus to calculate the equations of motion:
First, take the derivative of the lagrangian with respect to the position variables (x and theta):
Next take the derivative of the lagrangian with respect to the velocity variables (x- and theta-dot):
Now take the derivative of the above equations with respect to time:
Now (9) and (13), as well as (10) and (14), can be put be plugged into (8) to obtain (15) and (16):
The right side of the equations are equivalent to the input force at wheel for (15), and the torque acting on the ROCKO body for (16).
Since ROCKO’s wheels are actually driven by belts that are rotated by a motor, and not a force that pushes on the wheel, variable 'u' will be used to represent the force acting on the wheel from the motor, instead of a force variable, F.
To see how the torque from the motor is converted to an equivalent force at the wheel, look further down in the ‘Simulation in MATLAB' section.
Equation 16 has a torque on the right side of the equation, but since it is assumed that no external force is pressing on the head/leg assembly (gravity is accounted for in the equation), the torque is assumed to be zero.
Step 3, Separate Equations of Motion:
The previous equations, (15) and (16), both contain the second derivative of the position variables (x and theta) with respect to time. However to actually simulate the system, separate equations that contain just the second derivative of x and just the second derivative of theta are required. Using a little bit of linear algebra, this can be achieved:
First, a G matrix and H matrix are calculated from (15) and (16), then the G matrix is inverted and multiplied by the H matrix to obtain separated equations for the second derivatives:
Equation of motion for the wheel position:
Equation of motion for the robot’s angle:
We now have the required equations to simulate ROCKO. Now, all we need to do is find the necessary values and code the simulation.
Part 3 - Obtaining Inertial Values From CAD
Now that the equations of motion are derived, the variables in the equations need to be found.
For ROCKO, this was mostly be done in SolidWorks, but any Computer Aided Design (CAD) software will do.
Step 1, Intro to SolidWorks CAD Model:
Step 2, Obtaining Mass Values:
Step 3, Obtaining Moments of Inertia:
Step 4, Effective Wheel Mass Explained:
Part 4 - Simulation in MATLAB
The main script for ROCKO’s simulation was done in MATLAB, primarily utilizing the ode45 function. This function works by computing a system of equations, usually the state of something over a period of time. In our case, we simulate ROCKO for a very short period of time, before re-computing the state of the system, and then calculating the next time step:
Step 1, Setting Up Non-Linear Model/System of Equations in MATLAB:
Step 2, Calculating Torque/Force of Motors:
Step 3, Graphing States Over Time:
Step 4, Graphing ROCKO:
Part 5 - Simulating Control System
Step 1, Simulating Refresh Rate:
Step 2, Simulating Resolution:
Step 3, Simulating Offset:
Step 4, Simulating Integral & Derivations of PID Loops: