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**

*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**

**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

**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.

*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

*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.

*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.

*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.

*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**

**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.:**Figure 3-7**- The linear equations of motion for the TWSB robot.

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

*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.

*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*, d

*x*/d

*t*,

*thetab*, d(

*thetab*)/d

*t*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,

*v*

*a*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.

**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

### 4.1 Linear Model for Geared DC Motor

**Working Principle of Geared DC Motor**

**2**]. Readers who are not familiar with the operation of DC motor can refer to many resources online on how the DC motor works.

*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.

*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:

*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:

*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**

*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:

*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.

*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:

*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

**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***matrices in the CLSS block is shown in Listing 1.*

**D****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.

*is 4x4 and the rank of*

**A***s*-

**I****is 4, where**

*A***is a 4x4 identity matrix). We can also show that the TWSB robot model of equation (4.2.2) is**

*I**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)

- 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).

*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:

**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*)/d

*t*,

*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*)/d

*t*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)

- First
set
*k*_{x },*k*,_{v}*k*,_{theta}*k*and_{w}*thetab*_{(default)}to 0. Use a support to hold the robot upright. - Slowly
increase (or decrease)
*k*until the robot oscillate around the upright position. The polarity of_{theta}*k*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._{theta} - Slowly
increase (or decrease)
*k*until the robot can balance briefly. The robot will move or drift when it is attempting to balance upright because_{w}*k*and_{x}*k*are still 0._{v} - Now
adjust
*k*and_{x}*k*until the robot can balance without drifting a lot. From experience_{v}*k*and_{x}*k*usually have similar polarity and are negative. Coefficients_{v}*k*and_{x}*k*affects the dynamic response of the robot when it is pushed, so once the initial_{v}*k*and_{x}*k*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_{v}*k*and_{x}*k*should allow the robot to gradually return to the initial position. Adjust_{v}*k*and_{x}*k*to reduce overshoot/undershoot in the robot movement when pushed and to achieve the best damping ratio._{v} - 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. **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.**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

- F. Grasser,
A. D’Arrigo, S. Colombi, A. C. Rufer, “JOE: A mobile, inverted pendulum,” IEEE
Transactions on Industrial Electronics, vol. 49, no.
- N. S. Nise, "Control systems engineering", John-Wiley, 6th edition, 2011.
- G. F. Franklin, J. D. Powell, M. Workman, "Digital control of dynamic systems", Pearson (Addison Wesley), 3rd edition, 1998.
- 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.
- A. Morar, "Stepper motor model for dynamic simulation", Acta ElectroTehnica, Vol. 44, No. 3, 117-122, 2003.
- 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

*n*th order system, if the rank of

**is also**

*CM**n*, then the linear system is completely controllable [

**2**]. Matrix

**is given by:**

*CM*** 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

**in the form of the**

*CM**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.

` `

` `