Modeling and Control of Two-Wheels Self Balancing (TWSB) Robot (Updated 17 June 2023)

By F. Kung

In this post I will attempt to explain the theory behind two-wheels self-balancing (TWSB) robot as clearly as possible.  This post is not about the instruction or process of building such a robot, for this please refer to an earlier post here , where I discussed the micro-controller board that I used, the embedded software structure, and many other resources online.  For videos of all robots made by me, please see https://www.youtube.com/fkungms.

1.0 The Basic Principle

The TWSB robot is based on balancing the inverted pendulum principle.  Traditionally such a system is shown in Figure 1-1.  If the pendulum falls forward, the motorized cart will move forward, and vice versa if the pendulum falls backward.  By controlling the speed and acceleration of the motorized cart, it is possible to keep the inverted pendulum in an upright or vertical position with respect to the floor (See Figure 1-2).  We will call the angle made by the pendulum with respect to the vertical axis the tilt angle.

Figure 1-1 - An inverted pendulum system.

Figure 1-2 - Action of the motorized cart when the inverted pendulum falls toward the front and back, in keeping the pendulum upright.

By reducing the motorized cart into just a pair of motorized wheels, we obtain a two-wheels self-balancing machine, as illustrated in Figure 1-3.  From now on we will use the term TWSB Robot to refer to such machines.

Figure 1-3 - Converting the inverted pendulum platform into a two-wheels self-balancing machine.

2.0 High Level System Overview

In general, the TWSB robot requires a few key components, this is shown in Figure 2-1.  The sensor module for measuring the tilt angle and its rate-of-change is typically called the Inertia Measurement Unit (IMU).  The Motor Shaft Encoder is typically magnetic or infrared based, and it is used to inform the on-board computer of the angle and angular speed of the motor output shafts.  Note that this sensor is not compulsory, as will be shown in later section we do not need to know the motor turning speed and shaft angle to balance the robot upright, though knowing this parameter will enable better dynamic performance.  Also in some designs, another technique known as back EMF (electro-motive force) measurement can be used to measure the electric motor turning speed and direction.  Here I prefer to use an actual encoder as this approach is more accurate and has better resolution.  The electric motors propelling the robot can be brushed or brush-less electric motors.  Examples are brushed DC motor, brushed core-less DC motor, and brush-less motors such as stepper motor, hub motor etc.  For small TWSB robots (those that weight below 4 kg), DC motors and steppers are the common choice of propulsion.  Electrically, all these key components are linked together as shown in Figure 2-2.  Some of the symbols in Figure 2-2 will be explained in the next section. 

Figure 2-1 - Basic building blocks of TWSB robot.

Figure 2-2 - Electrical connection of the key components.

3.0 Mathematical Model of the Robot Platform

This section is a simplified derivation as put forth by [1].  Before we can derive the mathematical model, we need to agree on the symbols and coordinate system used to describe the robotic platform.  Note that different authors may use different coordinate systems, hence there may be small variation in the equations.  It will also be assumed that the reader is familiar with topics on mechanics such as linear and rotational dynamics [6].  The symbols and coordinate system we adopt are shown in Figure 3-1.

Figure 3-1 - Coordinate system, mass and moment inertia of the robot platform.

We assume both left and right motors are driven simultaneously with the same torque. Thus, the robot can only move forward or backward. If different torques are applied to the two motors, then the robot can also turn along a vertical axis parallel to the y-axis. However, we would ignore that consideration for now.  The robot platform can be divided into two parts:

A) The Main Body, which consist of the body and the two stators of the electric motor.
B) The Wheel Assembly, which consist of the wheel and the rotor of the electric motor.

If the electric motor contains gearbox, the gears, axles etc are part of the Wheel Assembly while the brackets for the gears, bearings would be part of the Main Body.  Figure 3-2 shows a diagram of a TWSB robot and the division of the robot platform into Main Body and Wheel Assembly.  We imagine the electric motor can be disassembled, one part goes to the wheel and another part stays with the body.  Thus, in Figure 3-1, mb is the sum of the mass of the body and the two stators, similarly Jw is the moment inertia of the rotor and the wheel, reference to the center axis of the wheel.

Figure 3-2 - Dividing the TWSB robot platform into the Main Body and Wheel Assemblies.

3.1 Equations of Motion for the TWSB Robot

The Wheel Assemblies and Main Body in Figure 3-2 are linked together by the magnetic forces generated between the rotor and stator of the electric motors, and also the mechanical forces on the bearings within the motors and gearbox.  Now we can apply Newtonian mechanics on the Main Body and Wheel Assembly separately, using the concept of Free-Body Diagram. The modeling technique we used here can be found in standard engineering mechanic text that deals with plane motion of rigid bodies, for instance [6]. The Free-Body Diagram for the Main Body and Wheel Assembly are shown in Figures 3-3.  Note that we have two Wheel Assemblies.

 

Figure 3-3 - Free-Body Diagram for the Main Body and Wheel Assemblies.

The forces P and H are due to the mechanical forces in the bearing within the electric motors, while torque Tw is due to the magnetic force between the rotor and stator in the electric motor.  The torque Tw is described with respect to the wheel assembly axis (Actually the position of the reference axis for the torque is not important, it can be anywhere on the Wheel Assembly and Main Body as long as it is parallel to the wheel axis, see Extra Note on torque applied to rigid body below). Bear in mind that force and torque are generated in opposite direction pairs.  Now lets apply the Newton Laws on the Wheel Assembly and Main Body with the assumption of non-slip movement of the wheel:

For one Wheel Assembly:

Figure 3-4A - Dynamic equations for the Wheel Assembly.

Main Body:

Figure 3-4B - Dynamic equations for the Main Body.

In deriving equations (3.2.5) and (3.2.6), we use the relationship between variable x and xG as shown in Figure 3-1. Eliminating the forces H and P from equations (3.2.5) to (3.2.7) results in the 1st equation of motion for the TWSB robot platform.  Next H is eliminated from (3.2.4) using (3.2.5), resulting in the 2nd equation of motion.

Figure 3-5 - The two equations of motion for the TWSB robotic platform.

Equations (3.2.8) and (3.2.9) can be rewritten in the following form.

Figure 3-6 - The two equations of motion after some algebraic manipulation.

Basically equation (3.2.10) says that the current acceleration of the robot along the surface (i.e. x direction) is dependent on the torque produced by the motor and also state of  tilt angle (i.e. current value and angular velocity).  In a similar manner equation (3.2.11) says the the current angular acceleration of the tilt angle is dependent on, again the torque from the motor and current state of the tilt angle.

Extra Note: Couple moment, torque and reaction torque on an electric motor

 

 

 

 

 

 

 

 

 

 

 

 

Please refer to many web resources, textbooks on mechanics, such as [6] for further details on torque and couple moment.

 

3.2 Linear Equations of Motion for TWSB Robot

The equations of motion in (3.2.10) and (3.2.11) are non-linear, due to the presence of sine, cosine and square operations in the equations.  Most of the time under normal operation the robot will be close to upright position, the rocking or rotation motion along the wheel axis small, i.e.:

With these assumptions, equations (3.2.10) and (3.2.11) can be simplified, or the term to use is "linearize" to:

 Figure 3-7 - The linear equations of motion for the TWSB robot.

By introducing following symbols, the linear equations of motion can be written in a more compact form.

Figure 3-8 - A more compact linear equations of motion for the TWSB robot.

Equations (3.2.15) and (3.2.16) can be expressed in matrix form, consistent with modern state-space representation for linear systems.  Here state refers to a set of parameters of the system that is sufficient to predict the system behavior in the next instant in time.  As the variables in (3.2.15) and (3.2.16) are function of time t, we can use Laplace transform and convert all the variables to function of complex frequency (s).  Figure 3-9 below shows the linear equations of motion for the robot, rewritten in matrix form for both time and frequency domains.

Figure 3-9 - Matrix representation for the linear TWSB robot system equations.

Equations (3.2.17a) and (3.2.17b) are known as the state-space model for the TWSB robot.  They are very important to predict how the robot will evolve with time when it is near upright position.  For instance, if we know the current output of the electric motors at time t = 0 second, e.g. the torque Tw, and also the current value of the state, e.g. x, dx/dt, thetab, d(thetab)/dt at t = 0 second, then we can predict the future values of the state, say at t = 0.1 second by integrating equation (3.2.17a). This idea is illustrated in Figure 3-10 below, and is the method used by computer software to simulate the system.  In Figure 3-10, va is the voltage applied to the electric motors ('a' stands for armature or stator), this voltage causes a current to flow into the electric motor and produces a torque Tw at the motor output shaft.

Figure 3-10 - How the state-space equation is used to predict the future robot state.

Typically in computer simulation, using mathematical software like MATLAB, Scilab or Octave, we key in the model parameters using the frequency-domain form of (3.2.17b).  When the computer performs the actual calculation in time-domain, it is the time-domain form of (3.2.17a) that is used to predict the output of the system after a short time interval (the time interval is set automatically by the simulation software to minimize calculation error and ensure numerical stability), with the integration in Figure 3-10 being carried out via numerical methods.  At this juncture equations (3.2.17a) or (3.2.17b) cannot be properly used yet, as we do not know how the torque applied at the wheels, Tw, which is supplied by the electric motor is related to the voltage (or current) that we are going to apply to the motors.  The next section will answer this.

4.0  The Complete TWSB Robot Model

With the linear model for the robot platform developed in Section 3, we are now in a position to create a complete model of the robot including the electric motor and a digital feedback controller.  Here we will focus on using geared DC motors and stepper motors.  First let's look at the approximate mathematical model for the geared DC motor.

4.1  Linear Model for Geared DC Motor

Working Principle of Geared DC Motor

Figure 4-1 shows the internals of a typical geared DC motor, with the notation for it parameters [2].  Readers who are not familiar with the operation of DC motor can refer to many resources online on how the DC motor works.

Figure 4-1 - Model of geared DC motor.

The simplified equivalent circuit of the Rotor is shown in Figure 4-2. The equivalent circuit of the coils on the Rotor is represented by the series connection of Ra and La.  Ra accounts for the resistive loss of the coils while La is the self-inductance of the coil (the coil generates its own magnetic field when voltage is applied across the motor terminals, thus behaves like an inductor). When a voltage va is applied across the motor terminals, a current flows into the coils on the Rotor.  The Rotor thus becomes an electromagnet and this react with the permanent magnets on the Stator to push the Rotor clockwise or anti-clockwise.  The torque generated on the Rotor is proportional to the intensity of the Rotor magnetic field, which in turn depends on the current ia through the coil (remember the Biot-Savart Law in magnetostatic), thus we have equation (4.1.1) in Figure 4-2. The torque produced by the DC motor is subsequently magnified by the gearbox and presented at the output shaft. Parameters kt and kb are coefficients that depends on the construction of the DC motors.

 Figure 4-2 - Equivalent circuit of DC motor Rotor.

As the coil rotates in the magnetic field created by the permanent magnets on the Stator, a back EMF (electro-motive force) is generated in the coil by virtue of Faraday's Second Law, this is modeled by the voltage source Vb in the diagram.  The magnitude of the back EMF depends on how fast the Rotor is turning with respect to the Stator, thus we have equation (4.1.2).  Both equations are then transformed into frequency-domain using Laplace Transform (because our ultimate aim is to obtain a transfer function linking the output torque of the motor and the voltage applied across the motor terminals).  From now on we will be considering the equation in frequency-domain form, so all parameters are understood to be in frequency-domain and will be written in CAPITAL format.  Should we need time-domain expression, we can perform inverse Laplace Transform procedures to get back the original time-domain expression.

Geared DC Motor Input-Output Relation
Using Kirchoff's Voltage Law, equations (4.1.1) and (4.1.2), the terminal voltage Va(s) across the rotor can be written as:

Because there is a gearbox between the motor and the output shaft, with gear ratio NG, the angle and torque relationship between the output shaft and motor are given by:

In equation (4.1.4a) we also include the effect of friction loss or viscosity (the 2nd term with coefficient of drag DG).  Generally the faster the output shaft rotates, the higher is the friction loss. Using the above equations, now we can write out the expression of the output torque in terms of terminal voltage and motor angle:

One final detail, because the motor enclosure or stator is not fixed, but free to rotate about the wheel axis, we also need to take this into account.  Now as stated in Figure 4-2, thetam is the relative angle between the motor shaft (or Rotor) and enclosure.  Since the motor enclosure can rotate, equation (4.1.5) needs to be modified as:

Simplified Geared DC Motor Relation
In most small DC gear motors, we can ignore the friction loss (DG is 0).  Also impedance of the rotor inductance is small compare to the resistance, sLa << Ra.  Hence the approximate form for geared DC motor input output relation, in frequency-domain is given by:

Most of the time for small DC gear motor, Cm2 is small compare to Cm1, and it can be further ignore. So that's it! A equation that links the input and output of the geared DC motor.  Essentially the voltage we apply at the motor terminal, and the current angle speed of the motor shaft and the robot tilt angle determine the torque at the output shaft.  Now let's look at an example on how to use equation (4.1.8) on a small DC motor.

Example 1 - Modeling a Practical DC Geared Motor

In this example we use Pololu's micrometal gear motor.  We use this particular example because the motor parameters are well documented.  Let's suppose we use the 6V HP series with 100:1 gear ratio (NG = 100).  The figures below show the calculation to obtain the simplified expression for this particular DC motor.

4.2  TWSB Robot Model with Geared DC Motor

When the geared DC motor is connected to the wheel, the angle of the motor shaft is equal to the wheel angle, and is related to the coordinate (x,0) of wheel axis.  If we refer back to Figure 3-4A, using equation (3.2.3) and the fact that the motor shaft and wheel axis is connected via gearbox:

 

Using equations (4.2.1) and (4.18), Tw can be eliminated in the TWSB robot state-space model of (3.2.17).  Let's consider the frequency-domain case of equation (3.2.17b).  The steps below shows how we can incorporate the geared DC motor model into the state-space matrix equation.

After eliminating Tw and simplifying, the final forms for both time and frequency-domains linear TWSB robot state-space model with geared DC motors are shown below:

Matrix equation (4.2.2) represent the evolution rule of the TWSB robot state given the current state and an input voltage va applied to both left and right motors.  This concept is illustrated in Figure 4-3.  In Linear System Theory, the 4x4 matrix in equation (4.2.2) which relates the rate-of-change of the state to the current state vector is often called the State Transition Matrix.  

Figure 4-3 - Linear mathematical model of TWSB robot with geared DC motors.

Scilab Software

In mathematical modeling software such as MATLAB Simulink or Scilab XCOS, the state-space model (4.2.2) is implemented by the CLSS (Continuous Linear State-Space) block as shown in Figure 4.4 (At present I am using Scilab 6.1.0).  Here I will focus on using Scilab as it is free and open source.   The method discussed here can be easily ported to MATLAB.

Figure 4-4 - Mathematical model of TWSB robot with DC geared motors, using Scilab Xcos.

Example 2 - Modeling a Small TWSB Robot 
In this example we consider a small TWSB robot, using the motor described in Example 1.  The Scilab scripts to calculate all the elements in the A, B, C, D matrices in the CLSS block is shown in Listing 1.

Listing 1 - Scilab script to calculate the Continuous Linear State-Space (CLSS) block representing the TWSB robot.

g = 9.8067;             // m/s^2 - Acceleration due to gravity.

// Model parameters of TWSB Prototype.
// --- Parameters of Geared Motor  ---
// From the datasheet of  Pololu's 100:1 HP micrometal gear motor
Tstall = 0.0216;       // N-m - stall torque in Kg/m.
RPMnoload = 320;        // No-load RPM at Vin=6V.
wnl = (RPMnoload*2*%pi)/60;     // No load gearbox output shaft rotation velocity in rad/sec.
Inl = 0.06;                  // No load current into motor, from measurement.
Vin = 6;                     // Volt. Input voltage to armature during test.
Istall = 1.6;                // Amp, stall armature current.
Ng = 100;                  // Gear ratio.
Dg = 0.00001;           // kgm/rad/sec - Viscous loss of gear.
La = 0;                      // Assume armature inductance is 0.

Ra = Vin/Istall;            // Armature resistance.
Kt = (Tstall/(Vin*Ng))*Ra;   // Armature torque constant.
Kb = (Vin-Ra*Inl)/(wnl*Ng);           // Armature back EMF constant.

// --- Parameters of Motor Driver  ---
Kmd = 1.0;             // Voltage gain of motor driver (This depends on the implementation of the motor
                               // driver,

// --- Parameters of Wheel Assembly ---
R = 0.035;                       // m - Radius of wheel, 35 mm.
mw = 0.0343;                  // kg - Mass of wheel.
Jw = 0.5*mw*R*R;        // kgm2 - Moment inertia of wheel assembly with reference to the center (axle).
                                        // Here we can assume uniform mass distribution for the wheel and the rotor, 
                                        // then work out the respective moment inertia of the wheel and rotor separately.  
                                        // The effect of gearbox is ignored.  Subsequently we add together 
                                        // the moment inertia for the wheel and rotor to get the approximate moment intertia
                                        // for the wheel assembly.   See any mechanics or physics texts on rotational dynamics.
// --- Parameters of Main Body ---
mb = 0.2445;                 // kg - Mass of the body and motor stators.
lb = 0.035;                     // m - Distance of center of mass to wheel axle.
Jb = 0.85*mb*lb*lb;     // kgm2 - Moment inertia of Main Body reference to the center of mass.
                                      // This is approximate only.

// Approximate constants for DC motor, gear box and wheel.
Cm1 = Ng*Kt/Ra;
Cm2 = Cm1*Kb;
M = mb + 2*(mw+Jw/(R*R)) - ((mb*mb*lb*lb)/(mb*lb*lb + Jb));
J = (mb*lb*lb) + Jb - ((mb*mb*lb*lb)/(mb + 2*(mw+Jw/(R*R))));
C1 = (1/M)*((mb*mb*lb*lb*g)/(Jb + mb*lb*lb));
C2 = (2/M)*((1/R) + ((mb*lb)/(Jb + mb*lb*lb)));
C3 = (mb*lb*g)/J;
C4 = (2/(J*R))*(R + ((mb*lb)/(mb + 2*(mw + Jw/(R*R)))));

// The elements of the state-transition (A) matrix
a11 = 0;
a12 = 1;
a13 = 0;
a14 = 0;
a21 = 0;
a22 = -C2*Cm2/(R*Ng);
a23 = -C1;
a24 = C2*Cm2;
a31 = 0;
a32 = 0;
a33 = 0;
a34 = 1;
a41 = 0;
a42 = C4*Cm2/(R*Ng);
a43 = C3;
a44 = -C4*Cm2;

// The elements of B matrix
b11 = 0;
b21 = C2*Cm1*Kmd;
b31 = 0;
b41 = -C4*Cm1*Kmd;

A few points need to be clarified in the Listing 1 on the physical parameters.  The mass of the body, wheels, rotors and stators can be measured using small electronic scale.  In the case of the DC motor, the motor is carefully dismantled to measure the mass of the rotor and stator (also the diameter of the rotor), then put back together again.  The center of mass of the Main Body can be estimated using the bi-filar method, where we hang the Main Body at two positions using a thin thread or fishing line.  The two positions must lie at the plane which divide the Main Body equal into right and left halves. The idea is shown in Figure 4-5.

Figure 4-5 - Dismantling the DC motor and measuring the CM of the main body.

5.0  Control and Balancing the TWSB Robot: State-Feedback Control

Now we are ready to implement a feedback control system to balance the TWSB upright.  A direct way to do this is shown in the block diagram of Figure 5-1.  This method of feedback control is usually called the State Feedback Control [2] because instead of feeding back a single parameter, we are feeding back an array of parameters representing the current state of the system, and applying a control parameter to each.

Figure 5-1 - State-feedback control of the TWSB robot with geared DC motors.

Compare to classical feedback control system, the state feedback control system shown in Figure 5-1 is a MIMO (Multi-Input Multi-Output) system.  The input is the User Set Vector, and the output is the State Vector, which contain 4 elements. In contrast classical feedback control system is typically SISO (Single-Input Single-Output).  In Figure 5-1 the feedback controller is simply a matrix multiplier, which multiply each element of the Error Vector with a coefficient.  For instance coefficient kx multiply with the position error, kv multiply with linear velocity error and so forth. Of course the coefficients have to be selected carefully so that the TWSB robot can balance properly.

There are formal control theory techniques to analyze the state-feedback control to make sure the above system in Figure 5-1 is feasible, a detailed discussion of these is beyond the scope of this article.  For instance we can show that the TWSB robot model of equation (4.2.2) is 4th order (because the State Transition Matrix A is 4x4 and the rank of sI-A is 4, where I is a 4x4 identity matrix). We can also show that the TWSB robot model of equation (4.2.2) is Controllable [2] subject to reasonable physical parameters. Controllable simply means it is possible for the input signal, vc in this case, to affect all the parameters in the state vector. It is always good to perform controllability test before implementing a state-feedback control, otherwise no feasible solution for kx , kv , ktheta and komega may be found for uncontrollable system. We will not be discussing the principles behind controllability test, interested reader can refer to standard texts on control theory. The Appendix will provide some background on controllability test with Scilab. Using the values in Example 2, it will be shown that the TWSB robot model with state-feedback control is controllable. 

Assuming the system in Figure 5-1 is feasible or controllable, many techniques can be used to find the 4 feedback control coefficients kx , kv , ktheta and komega. For instance the most direct approach is trial-and-error or experimental method, where we try different combinations of the feedback control coefficients until satisfactory system response is obtained!  There are some rules to systematically arrive at the values of the coefficients experimentally which I will summarize in Section 5.5.  There are also a few analytical methods to find the feedback control coefficients, for instance a method called Pole Placement in control theory [2], see also this tutorial https://www.youtube.com/watch?v=M_jchYsTZvM&t=663s.  In this method the coefficients are chosen such that the system exhibit poles and zeros in the complex plane that produce satisfactory time-domain behavior.  Another popular method, the Linear Quadrature Regulator (LQR) [3] is based on computer optimization, where the coefficients are chosen such that a cost function that is based on the error vector, the control signal (vc) and any other parameters of interest is minimized.  The cost function has a mathematical form called the quadrature form (whose output is always positive), thus the name of the method. Check out this tutorial on LQR method https://www.youtube.com/watch?v=1_UobILf3cc&t=419s. Demonstration of using LQR method and pole placement method to find the feedback control coefficients will also be shown in the Appendix.

In order to for the TWSB robot to balance upright, the user set vector can be set as in Figure 5-2.  Basically this means we want the robot to remain upright (thetab = 0) without rocking back and forth, and also to remain stationary at the same location (x = 0).

Figure 5-2 - User set vector for the TWSB robot to balance upright and remain at same location.

Based on the TWSB robot model in Example 2, I created a complete Scilab XCOS model of the TWSB robot with state feedback controller as depicted in Figure 5-3.

 Figure 5-3 - Scilab XCOS model of the TWSB robot with state feedback controller.

Of course in Figure 5-3, it is assumed that there are sensors to measure the state vector elements such as the tilt angle, tilt angle angular velocity, linear position and linear velocity.  Also note that the user setting is a step function. 

5.1  Some Computer Simulation Results with State-Feedback Control

Here we run a few simulations with different combination of the feedback control coefficient values. Figure 5-4A and 5-4B show the setting of the blocks in the Scilab XCOS model for the TWSB robot.  The idea is to inject a small step disturbance to the user setting and see if the TWSB robot can remain in upright position for each set of feedback control coefficients.
  

Figure 5-4A - Setting of the blocks for the TWSB robot with state feedback controller.

Figure 5-4B - Setting of the CLSS block by using the 'Simulation' tab.

  In Figure 5-4A the robot is initially set to tilt angle 0.04 rad at time t = 0.0, then at time t = 1.0 second the tilt angle is abruptly set to 0.0 rad, with all other set parameters remain at 0.0.  The sequences of figures below illustrate the result of stable, not-so-stable and unstable feedback control coefficient values. The stable coefficient values are arrived at experimentally using the method described in Section 5.5.  The not-so-stable coefficients result in a TWSB robot platform that requires drift longer distance on the ground and take longer time to reach the final state.

Figure 5-5A - Simulation results for stable feedback control coefficients.

Figure 5-5B - Simulation results for not-so-stable feedback control coefficients.

Figure 5-5C - Simulation results for unstable feedback control coefficients.

Here is a video of an example of TWSB robot using state-feedback control:

5.2  Special Case 1: Degenerating into PID Feedback Control

Although the linear mathematical model of the TWSB robot contains 4 parameters in the state, in principle we can still balance the robot upright by just focusing on the tilt angle and ignore the rest of the state parameters like x and dx/dt.  This is illustrated in Figure 5-6.  Because there is only one input parameter (thetab(set)) and one output parameter (thetab), this becomes a classical feedback control system (single-input single-output or SISO).  Here we perform further operations on the error signal to obtain the differential and integration terms, and used PID controller to balance the robot upright.  Although this will work, the dynamical performance of the TWSB robot will not be good generally compare to state-space feedback control.  Because we ignore the position (x) and velocity (dx/dt), the robot will 'drift' a lot while balancing upright, and it is sensitive to disturbance such as when you knock into the robot, it will move about erratically.

Figure 5-6 - Ignoring all state parameters except for tilt angle, we can then use PID feedback control.

We can derive the transfer function for the robot when only focusing on the tilt angle error as shown in Figure 5-7. Here to be more realistic I also include the dead-band effect of the DC motor driver into the model.  The coefficients of the PID controller can be determined analytically (say using Bode plot or Root Locus method) or experimentally.  Here I arrived at the coefficients experimentally, first by tuning Kp, then Kd until the robot can balance, and finally Ki to reduce the drifting.

Figure 5-7 - Scilab XCOS model when ignoring all state parameters except for tilt angle, with PID feedback control.

Again we run the computer simulation from 0 to 15 seconds.  The results are shown in Figure 5-8.  Notice that because we do not take into account the state parameters x and dx/dt, the robot drifts about the floor while it is balancing upright, first x goes to almost 1 meter from the initial location, then gradually moves backwards until x becomes negative.  We can reduce this drifting by tuning the default tilt angle thetab setting. However, generally using PID feedback control, it is very difficult to achieve zero drift, i.e. making the robot return to its original location. 

Figure 5-8 - Simulation results for PID feedback control.

Here is a video of an example of TWSB robot using just PID feedback control, notice that the robot is sensitive to collision with objects, especially at the end of the video when the robot toppled over:

5.3  Special Case 2: TWSB Robot Using Brushless DC motors or Stepper Motors (Updated 2nd Sep 2021)

Brushless DC motor and stepper motor are brushless motors, i.e. they do not use commutator (See Figure 4.1) to invert the electric current in the electromagnet to rotate the rotor or armature of the motor.  Instead both types of motors rely on electronic switching circuit to alternate the electromagnet polarity to rotate the rotor or armature. See some online resources here on how brushless DC motor and stepper motor works.  Actually the operating principles for brushless DC motor and stepper motor are different but I am lumping them together because:

• Both types of motors do not use commutator and 
• Both types of motors require external switching circuit or driver module to switch the direction of the electric current in the electromagnet (on the stator) in order to rotate.
• The rotational speed of both types of motors is proportional to the switching circuit frequency.
• The torque produced by both type of motors is more or less constant at low rotation speed.  Typically the driver module uses a current source to drive the electromagnet in the stator, rendering the output shaft torque of the motor having constant average value at low rotation speed (when back EMF effect is small).

Because the output torque is almost constant, it is not very useful to achieve fine control of the TWSB robot state using equations (3.2.17a) and (3.2.17b), you either turn on the wheel motors at full torque or turn off the wheel motors with zero torque, unlike the brushed DC motor where we can adjust the output torque by adjusting the armature voltage supply va of the motors. Instead the mechanical parameter that we can control effectively for brushless DC motor and stepper motor is the output shaft rotation speed or angle.  Note that the output shaft rotation speed can be expressed as the angular velocity of the shaft. In principle we can either make the TWSB robot maintain upright position by either controlling the motor output shaft angular velocity or angle, however the algorithm for control using output shaft angle is more complicated when implemented in computer codes, so here I will only focus on control using the output shaft angular velocity. Basically for both types of electric motors the supply voltage is fixed, but by changing the frequency of the switching circuit we can control the rotation speed of the output shaft.

In principle we can derive a mathematical model of the brushless DC motor and stepper motor, as can be found in some literature, (see [4], [5]), and proceed as in the brushed DC motor case by putting the expression in equations (3.2.17a) or (3.2.17b) to obtain the final state equation of the TWSB robot with multiple inputs, say the supply voltage to the armature, the frequency of the switching circuit and other inputs.  However here I will be using a different approach.  This approach will be simpler but it is a less accurate model, which is fine as in practice we usually use computer simulation to guess the initial values of the feedback control coefficients and then arrive at the final values through experimentation. I will just focus on stepper motor and the case for brushless DC motor will be similar.

Here I will assume the stepper motor will be rotating at small angular velocity and that the output shaft is directly connected to the wheel without any gearbox.  The former assumption is reasonable when the TWSB robot is close to upright position, the stepper motors will just rotate slowly clockwise or counter clockwise to keep the robot upright.  At slow rotation the output torque from the stepper motors will be maximum and will be sufficient to propel the robot forward or backward.  With this in mind let us consider equations (3.2.15) and (3.2.16) in frequency-domain form:

We can remove the variable Tw from the above equations by substituting (3.2.16) into (3.2.15):

Assuming non-slip motion, the velocity of the TWSB robot and the wheel angular velocity are related to the wheel radius R:

Putting (5.3.2) into (5.3.1) and rearranging, we obtain a transfer function that relates the wheel angular velocity and the body tilt angle of the TWSB robot:

Equation (5.3.3) means that the body tilt angle qb is dependent on the wheel's angular velocity. By properly controlling the wheel angular velocity, we can in principle maintain the body tilt angle qb close to 0 (i.e. make the TWSB robot upright).  Thus, the model for a TWSB robot using stepper motors (or brushless DC motors) with speed control degenerates to the classical SISO (single-input single output) model.  With SISO model, standard classical control methods, such as lead-lag controller or PID controller can be used with the transfer function of (5.3.3) to keep the TWSB robot upright.  An example of the model in Scilab XCOS for the TWSB robot with stepper motors is shown in Figure 5-9. Here the system samples the body tilt angle qb and compare this with the user setting (the set point).  For instance to balance the TWSB robot upright the user setting should be close to 0.  The error between the actual body tilt angle and user setting will be fed into the PID controller, which generate an output that is proportional to the required wheel speed.  Thus, the software in the on-board computer takes the PID Controller output and maps it to an appropriate wheel speed, how this is achieved depends on the hardware details of the system.  

 

Figure 5-9 - Scilab XCOS model for TWSB robot using stepper motors for propulsion with velocity as input control.  

A straight forward approach to map the PID controller output to the wheel angular velocity is to control the frequency of the periodic pulse signal used to drive the stepper motor driver modules (the block "Stepper Motor Driver Gain" in Figure 5-9 encompasses this relationship). A stepper motor is driven by pulse, each pulse will rotate the motor shaft by a fixed angle clockwise or counter-clockwise. This fixed angle is called the Step Angle qstep and can be set by the user. Typical values for step angle is 1.8 degree for full-step, 0.9 degree for half-step. As an illustration, assuming we are using bipolar stepper motor under half-step mode, 400 pulses will be needed to drive the motor shaft one rotation (360/0.9 = 400). If a rotation speed of 0.5 rotation/second, or 3.142 radian/second is needed, we would need to send 200 pulses to the stepper motor driver every second, or a frequency of 200 Hz. Thus, the stepper motor rotation speed is given by qstep/T. where T is the period of one pulse. The PID controller output must be mapped to the correct value of T for the stepper motor to turn at the required angular velocity as shown in equation (5.3.6).

 

 

Because of the inversely proportional relationship between stepper motor angular velocity and T, we need to use a non-linear function to relate T with PID controller output. Any point to note is since we are going to generate the control pulses using digital hardware, the value of T will be discrete, some values of T cannot be synthesized and need to be approximated to the nearest value supported by the hardware. A typical relationship that is popular for mapping PID controller output to stepper motor driver is shown in equation (5.3.7):

 

 

 

 

 

In equation (5.3.7), Δt is the time resolution, it is dependent on the micro-controller hardware such as the timer resolution, clock frequency etc, with typical value of 5-50 micro-seconds. The parameters a and b are set by the user to adjust the slope or gain of the PID Controller versus stepper motor speed curve. For example if you are using Arduino Uno (with Atmega 328P micro-controller) running at 16 MHz clock frequency (0.0625 micro-second per cycle), it will be impossible to have Δt  less than 10 micro-seconds as you would not have enough clock cycles to execute other codes for the robot controller.

A mapping example is shown in Figure 5.10 below for qstep = 0.9 degree, Δt = 20 microsecond, a = 4000, b = 3, with PID Controller output capped to between -400 to +400. Due to the need to cast the PID Controller output to integer (because the pulse generator requires integer input to it's timing registers), the mapping exhibits nonlinear steps as indicated in Figure 5-10. Hence, the user should set the mapping function carefully to minimize the nonlinear steps within the typical wheel velocity range.

 

 

 

 

 

 

 

 

 

 

 

 

 

Figure 5-10 -  Mapping of PID Controller output to wheel angular velocity with a slope around 0.4.

 

With the model in Figure 5-9, we can use standard analytical method in control theory to obtain the coefficients for the controller, or we can simple do it via trial-and-error.  A sample result for the model in Figure 5-9 is shown in Figure 5-11.  Similar to the idea in Section 5.1 and Section 5.2, where initially we assume the TWSB robot to be upright, then we inject a small step change in the user setting.  If the system is stable the TWSB robot should be able to regain balance with some changes in it's dynamic parameters.  In Figure 5-11 the user settings is changed from 0 degree to 2 degree at t = 3.0 seconds.  We observe that the stepper motor based TWSB robot regain balance after 1.0 second.  However to remain upright at body tilt angle of 2 degrees, the TWSB robot has to continuously accelerate at roughly 0.6 m/s^2.  Thus the robot will keep on moving faster and faster until it topples over, unless we reduce the user settings. More detailed explanation can be found in this paper.

Figure 5-11 - Scilab XCOS simulated results when user setting is changed from 0 to 2 degrees.

 

Videos, Design Files and Codes

Here is a video of an example of TWSB robot using NEMA 17 stepper motors, with PID feedback controller setting the rotation velocity of the stepper motor shaft. You can see the design details here. Note that I am not using Arduino for this particular build.

It is also possible to write equation (5.3.3) in state-space form, using the phase variable approach [2], this is illustrated below. However, since we have only one 2nd order system equation in the case of stepper motor, the state matrix for the robot is a 2x2 square matrix, i.e. only 2 parameters are needed to determine the system state. First we define two new state parameters:

 

 

 

Then we modify (5.3.3) using the parameters above:

 

 

This can then be rewritten in matrix form:

 

 

State-feedback control method as described in Section 5.1 can then be applied to equation (5.3.5) to keep the robot platform upright.

5.4  Non-Ideal Effects

In coming up with the control schemes in previous sections we have made some assumptions.  First and foremost is the fact that the perturbation of the robot platform from the required state is small and the dynamical equations constraining the robot platform behavior can be linearized.  The second important assumption is that we can measure all the physical parameters such as the tilt angle thetab, d(thetab)/dt, x and dx/dt accurately and without any time delay.  Thirdly any non-linearity effects in the motor, motor driver circuit are ignored. Typically there will be some error and delay in estimation of the state vector of the robot platform due to the way the sensors work, noise and the algorithms used, for example the Kalman Filter and Complementary Filter approaches are usually used to obtain an estimate of tilt angle thetab, d(thetab)/dt from the accelerometer and gyroscope.  Also the micro-controller can take a few tens to hundreds of micro-seconds to perform all the calculations.  Finally there are non-linear frictional force in the motor and gearbox, and saturation effect in the motor driver circuit.  All these can be modeled mathematically and included in our Scilab XCOS model to give a more realistic prediction of the robot platform behavior.

5.5  Some Suggestions for Tuning the State-Feedback Coefficients (Updated on 11 July 2021)

In this section I will share some suggestions for tuning the coefficients by trial-and-error approach.  Here I will only focus on state-feedback control and not the special case of PID feedback control, which already has many resources online.  The suggestions below have many similarities with the Nichol Ziegler method for PID tuning.  The following summary is just based on my experience and a bit of deduction based on the mathematical model of the TWSB robot.

1. First set k_x , k_v , k_theta , k_w and thetab_(default) to 0.  Use a support to hold the robot upright.
2. Slowly increase (or decrease) k_theta until the robot oscillate around the upright position.  The polarity of k_theta should be such that when the robot tilts forward, the wheels should be driven to move the robot forward, vice versa when the robot tilts backward.
3. Slowly increase (or decrease) k_w until the robot can balance briefly.  The robot will move or drift when it is attempting to balance upright because k_x and k_v are still 0.
4.  Now adjust k_x and k_v until the robot can balance without drifting a lot.  From experience k_x and k_v usually have similar polarity and are negative.  Coefficients k_x and k_v affects the dynamic response of the robot when it is pushed, so once the initial k_x and k_v are obtained to stop the robot from drifting, we should tune these coefficients to optimize the dynamic response when disturbed.  Typically, this is done by observing the plot of velocity and distance traveled versus time when the robot is pushed.  A properly tuned k_x and k_v should allow the robot to gradually return to the initial position.  Adjust k_x and k_v to reduce overshoot/undershoot in the robot movement when pushed and to achieve the best damping ratio. 
5.  Finally tune thetab_(default) by putting the robot upright (with the motors not running), and then activating the balancing routine.  If thetab_(default) is not right the robot will drift a short distance when balancing routine is engaged.  thetab_(default) should be tuned until the robot can ‘stands’ and balance at the starting location without too much movement when balancing routine is engaged.
6. Deadband correction for motor driver - Typically we need to add an offset voltage to the motor driver board, to overcome the internal friction of the motor, gearbox and the wheels. This value can be discovered experimentally by trial-and-error. With the correct offset voltage, the robot will have minimum rocking motion while balancing at a location.
   
7. Optimizing the dynamic response to disturbance - After completing step 1-6, the robot should be able to balance on two-wheels quite well. We can now improve it’s balancing ability further by optimizing the 4 control coefficients k_x , k_v , k_p , k_w and _b(default). One of the interesting observation is this, if the coefficient corresponding to a parameter is not optimum (say too small), then when the robot is disturb, say be pushing it, then that particular parameter will exhibit large disturbance. An example if coefficient k_w which corresponds to d_b(default)/dt has a magnitude that is too small, the robot can balance on two-wheels. But when pushed, it will rock to front and back while remaining almost stationary at same location x. Thus if the magnitude of k_x is too small then the robot will drift. And if magnitude of k_v too small, the robot will move back and forth when pushed. But pushing the robot platform and continuous carrying out small adjust to the values of the control coefficients, when can arrive at a set of coefficients that will give a robust robot dynamics.

 

6.0 The Next Step

The steps described in this article are just to allow the robotic platform to balance reliably on two-wheels. To have the TWSB work, we need to make it perform higher order motions, such as move at constant velocity in a preset direction, turning, tackling slope or incline surface, going through small obstacles, carrying loads etc. This entails further refinements to the algorithms presented here. Probably I will write another article to describe these refinements. For some interesting videos, please see this channel.

 

References

1.       F. Grasser, A. D’Arrigo, S. Colombi, A. C. Rufer, “JOE: A mobile, inverted pendulum,” IEEE Transactions on Industrial Electronics, vol. 49, no. 1, pp. 107-113, 2002. 
2.       N. S. Nise, "Control systems engineering", John-Wiley, 6th edition, 2011. 
3.       G. F. Franklin, J. D. Powell, M. Workman, "Digital control of dynamic systems", Pearson (Addison Wesley), 3rd edition, 1998.
4.       B. C. Kuo, G. Singh, R. Yackel, "Modeling and simulation of a stepping motor", IEEE Transactions on Automatic Control, Vol. 14, No. 6, 745-747, 1969.
5.       A. Morar, "Stepper motor model for dynamic simulation", Acta ElectroTehnica, Vol. 44, No. 3, 117-122, 2003.
6.       F. P. Beer, E. R. Johnston Jr., E. Eisenberg, W. Clausen, P. J. Cornwell, "Vector mechanics for engineers - Dynamics", McGraw-Hill, 8th edition, 2006.
   

      

Appendix 1 - Controllability of TWSB Robot Model

      Test for controllability of a linear system in state-space form can be done by checking the Rank of the Controllability Matrix CM. For an nth order system, if the rank of CM is also n, then the linear system is completely controllable [2].  Matrix CM is given by:

      Figure A1-1 - The controllability matrix.

      Thus for TWSB robot model, the rank of the CM must be 4 in order to be completely controllable. Scilab has built-in library to compute CM in the form of the cont_mat( ) function. Please refer to the online help of Scilab for further details.

Appendix 2 - Using Pole Placement and LQR Method to Find the Coefficients for State Feedback Control

      Scilab also has built-in library for the pole placement and LQR algorithm in the form of the ppol( ) and lqr( ) function. In order to use these functions the state-space system must be controllable. Listing 2 below is a modification of the Listing 1 scripts, where I include the the controllability test and the steps to compute the feedback control coefficients using LQR algorithm, and also calculate the final values of all the poles in the closed-loop system.  The script is created as a Scinote file and executed on Scilab console. The results are shown in Figure A2-1. As can be seen in Figure A2-1, the TWSB robot model with state-feedback has CM with rank of 4, thus completely controllable.

Listing 2 - Scilab script to calculate the Continuous Linear State-Space (CLSS) parameters representing the TWSB robot, and also to perform controllability test and calculate the LQR feedback control coefficients.

       g = 9.8067;             // m/s^2 - Acceleration due to gravity.

// Model parameters based on V2 Prototype.
// --- Parameters of Geared Motor  ---
// From datasheet of 100:1 HP micrometal gear motor
Tstall = 0.021602;       // N-m - stall torque (2.2 kg/m).
RPMnoload = 320;        // No-load RPM at Vin=6V.
wnl = (RPMnoload*2*%pi)/60;     // No load gearbox output shaft rotation velocity in rad/sec.
Inl = 0.06;             // No load current into motor, from measurement.
Vin = 6;                // Volt. Input voltage to armature during test.
Istall = 1.6;           // Amp, stall armature current.
Ng = 100;                // Gear ratio.
Dg = 0.00001;           // kgm/rad/sec - Viscous loss of gear.
La = 0;                 // Assume armature inductance is 0.

Ra = Vin/Istall;            // Armature resistance.
Kt = (Tstall/(Vin*Ng))*Ra;   // Armature torque constant.
Kb = (Vin-Ra*Inl)/(wnl*Ng);           // Armature back EMF constant.

// --- Parameters of Motor Driver  ---
Kmd = 3.16;             // Voltage gain of motor driver.

// --- Parameters of Wheel ---
R = 0.035;              // m - Radius of wheel, 70mm 3D printed wheels.
mw = 0.0343;            // kg - Mass of wheel.
Jw = 0.5*mw*R*R;        // kgm2 - Moment inertia of wheel reference to the center (axle).
                        // This is approximate only, assuming uniform mass distribution.
// --- Parameters of Robot Body ---
mb = 0.2445;            // kg - Mass of body.
lb = 0.035;             // m - Distance of center of mass to wheel axle.
Jb = 0.1*mb*lb*lb;     // kgm2 - Moment inertia of Body reference to the center of mass.
                        // This is approximate only.

// Approximate constants for motor, gear box and wheel.
// See the derivation, V3, 31 March 2017.
Cm1 = Ng*Kt/Ra;
Cm2 = Cm1*Kb;
M = mb + 2*(mw+Jw/(R*R)) - ((mb*mb*lb*lb)/(mb*lb*lb + Jb));
J = (mb*lb*lb) + Jb - ((mb*mb*lb*lb)/(mb + 2*(mw+Jw/(R*R))));
C1 = (1/M)*((mb*mb*lb*lb*g)/(Jb + mb*lb*lb));
C2 = (2/M)*((1/R) + ((mb*lb)/(Jb + mb*lb*lb)));
C3 = (mb*lb*g)/J;
C4 = (2/(J*R))*(R + ((mb*lb)/(mb + 2*(mw + Jw/(R*R)))));

// The elements of the state-transition (A) matrix.
a11 = 0;
a12 = 1;
a13 = 0;
a14 = 0;
a21 = 0;
a22 = -(C2*Cm2)/(R*Ng);
a23 = -C1;
a24 = C2*Cm2;
a31 = 0;
a32 = 0;
a33 = 0;
a34 = 1;
a41 = 0;
a42 = (C4*Cm2)/(R*Ng);
a43 = C3;
a44 = -C4*Cm2;
// From the A matrix.
A = [a11 a12 a13 a14;a21 a22 a23 a24;a31 a32 a33 a34;a41 a42 a43 a44];

// The elements of B vector.
b11 = 0;
b21 = C2*Cm1*Kmd;
b31 = 0;
b41 = -C4*Cm1*Kmd;
// Form the B vector.
B = [b11;b21;b31;b41];

C = eye(4,4); // Create a 4x4 identity matrix.

// Form the linear system model.
// x = Ax + Bu
// y = Cx + Du, where D = 0, u = input.
P = syslin("c", A, B, C);

// Perform controllability test.
CM = cont_mat(A,B);
disp('Rank of CM is:');
rank(CM)

// --- LQR Method to find feedback coefficients ---
// The cost function weights for LQR algorithm.
Q = diag([2.0 2.0 0.5 0.1]);    // Emphasize x, v errors, deemphasize theta and 
                                // d(theta)/dt errors.
R = 2.0;                        // Emphasize control voltage magnitude.  We do 
                                // not want a control voltage that is too big,
                                // prevent wastage of energy.
disp('LQR coefficients');
Klqr = lqr(P,Q,R)                 // Calculate the LQR coefficients.

// Finding the composite state transition matrix.
// Note: We can write as A - B*(-Kq)
Aux = A + B*Klqr;
// Finding the roots of the compensated system, e.g the eigenvalue of
// the composite state transition matrix.
disp('Poles of the closed-loop system using LQR');
spec(Aux)

// --- Pole Placement Method to find feedback coefficients ---
// Here we are assuming a percent overshoot of roughly 15%, and settling
// time of roughly 4.0 seconds. This would (see [2]) requires damping factor
// of 0.5 and natural frequency of 2.667 rad/seconds, and a dominant
// complex poles of
// P1, P2 = -1.333+j2.309 and -1.333-j2.309
// We also select the 3rd and 4th poles as real poles, sufficiently far away
// from the dominant poles. This would mean the 3rd and 4th poles should be at
// least 10x further away from the imaginary axis compare to the dominant poles.
// So we arbitraryly take P3 = -13.333 + j0 and P4 = -26.666 + j0
Real = -1.333;
Imaginary = 2.309;
p1 = Real+%i*Imaginary;
p2 = Real-%i*Imaginary;
p3 = -13.333;
p4 = -26.666;
disp('Pole placement coefficients');
Kpp = ppol(A,B,[p3, p4, p2, p1])
Aux2 = A - B*Kpp;
disp('Poles of the closed-loop system using pole placement');
spec(Aux2)

 

 Figure A2-1 - Scilab console output after executing the scripts in Listing 2.

 Figure A2-1, the state-feedback coefficients obtained using LQR method are:

kx = -1.000

kv = -1.575

ktheta = -5.165

komega = -0.334

     Note that we need to multiply the output of lqr( ) with -1 due to the polarity of feedback value to the summing network assumed by the Scilab lqr( ) function, which is positive instead of negative as in Figure 5-1. Comparing the values above with the coefficients used in Figure 5-4A and we can see that the coefficients above are within the same range.     

      Also from Figure A2-1, the state-feedback coefficients obtained using pole placement method are:   

kx = -1.909

kv = -0.931

ktheta = -3.659

komega = -0.158

  If we use both sets of state-feedback coefficient in our actual robot or in Scilab XCOS simulation as in Figure 5-4A, 

   can check that the system should be stable.