Self-balancing Robot

 
This article describes the design of a simple Self-Balancing Robot. It all started as a funny student self-project and then evolved as new teaching platform in the 2nd year control class as it requires State-Space understanding. You'll find here about everything you may need to grab the control fundamentals, build and program your own robot if you wish. All the parts are easy to find or to prototype using home CNC or 3D printing.
 
segway_000.png
 
The robot in action, after developping an Android control app:
 
 

1. Non-Linear Mechanical System

 

1.1. Modeling

image002.png
 
Summing Forces on the Pendulum @G
image014.png [1]
image016.png [2]
 
Summing Moments on the Pendulum @G
image018.png  
image020.png [3]
 
Summing Forces on Cart @{0,0}
image022.png [4]
 

Shift forces fromimage024.pngtoimage026.png

image028.png
image030.png
image032.png
image034.png
image036.png
image038.png
[1] →
image040.png
[5]
[2] →
image042.png
[6]

First governing equation

[4], [5] →
image044.png
[7]

Second governing equation

[3], [5], [6] →
image046.png
[8]

Solving [7] and [8] gives:

image048.png
 
image050.png
 

1.2. Simulation

 
Above equations can be simply represented within Matlab Simulink® or Scilab XCOS® simulator.
 

1.2.1. Simulation inputs (external force, physical parameters)

xcos_001.png
 

1.2.2. Differential equations

 
xcos_002.png
 

1.2.3. Simulation Setup

  • Initial tilt = a small shift from vertical, no speed
  • No external force
  • Simulation time =10s
xcos_003.png
 

1.2.4. Simulation Results

The simulation represents a free fall of the pendulum, from the "almost" vertical starting point to the upside down position.

xcos_004.png
 
 

2. Linerization and Space-State representation

 

image061.png
 
Linearization around the vertical position:
image063.png
image065.png
image067.png
image069.png
 
Further assumptions:
image071.png
image073.png
 
It comes:
 

First governing equation

[7] →
image105.png
[9]

Second governing equation

[8] →
image106.png
[10]

Solving [9] and [10] gives:

image107.png
[11]
image108.png
[12]

 

And the corresponding Space State representation:

image109.png
 
image110.png
 

3. Control Synthesis

 
The linear optimal LQ full-state gain K is calculated using Scilab® 'lqr' function both in continuous and discrete time representations:
 

clear;

// Mechanical parameters
M = 0.5;        // Mass of carriage (kg)
m = 0.95;       // Mass of pendulum (kg)
b = 0.1;        // Drag Coefficient (N/m/s)
I = 0.0076;     // Pendulum Moment of Inertia (kg.m^2)
g = 9.8;        // Gravity acceleration (m/s^2)
l = 0.155;      // Half-length of pendulum (m)

//Space State Matrices
p = I*(M+m)+M*m*l^2;

A = [0, 1, 0, 0;
     0, -(I+m*l^2)*b/p, (m^2*g*l^2)/p, 0;
     0, 0, 0, 1;
     0, -(m*l*b)/p, m*g*l*(M+m)/p, 0];

B = [0;
     (I+m*l^2)/p;
     0;
     m*l/p ];

C = [1, 0, 0, 0;
     0, 0, 1, 0];

D = [0;
     0];
     
// *****************
// Contiunous domain
// *****************
     
// Define linear system in continuous domain
sbr_cont = syslin('c', A,B,C,D);
disp(sbr_cont);

// Print poles (showing instability)
disp(spec(A), "poles:");

// Check controlability
disp(rank(cont_mat(A,B)), "controlability matrix rank:");

// Check observability
disp(rank(obsv_mat(A,C)), "observability matrix rank:");

// Linear Quadratic control (lqr) effort
Q=diag([100,0,10,0]);
R = 1;

// Compute C1 and D12
[w,wp]=fullrf(sysdiag(Q,R));
C1=wp(:,1:4);
D12=wp(:,$:$);

// Compute correction matrix K
sbr_cont_k = syslin('c',A, B, C1, D12);
[K,X]=lqr(sbr_cont_k);

// Display new poles and K matrix
disp(spec(A+B*K), "new poles (continuous)");
disp(K,"K (continuous");

// Controlled system
Ac = [A + (B*K)];
Bc = [B];
Cc = [C];
Dc = [D];

// Set initial angle of 8°
X0 = [0; 0; -0.1416; 0]

// Defines controlled linear system
sbr_cont_ctrl = syslin('c', Ac, Bc, Cc, Dc, X0);


// ***************
// Discrete domain
// ***************

// Continuous -> Discrete
per = 0.05;                        // 50ms control loop
sbr_disc = dscr(sbr_cont,per);
disp(sbr_disc);

// Retreive matrices
A_d = sbr_disc(2);
B_d = sbr_disc(3);
C_d = sbr_disc(4);
D_d = sbr_disc(5);

// Check controlability
disp(rank(cont_mat(A_d, B_d)), "controlability matrix rank:");

// Check observability
disp(rank(obsv_mat(A_d, C_d)), "observability matrix rank:");

// Linear Quadratic control (lqr) effort
Q_d=diag([100,0,10,0]);
R_d = 1;

// Compute C1_d and D12_d
[w_d,wp_d]=fullrf(sysdiag(Q_d,R_d));
C1_d=wp_d(:,1:4);
D12_d=wp_d(:,$:$);

// Compute correction matrix K_d
sbr_disc_k = syslin('d',A_d, B_d, C1_d, D12_d);
[K_d,X_d]=lqr(sbr_disc_k);

// Display new poles and K matrix
disp(spec(A_d+B_d*K_d), "new poles (discrete)");
disp(K_d,"K (discrete)");

// Controlled discrete system
Ac_d = [A_d + (B_d*K_d)];
Bc_d = [B_d];
Cc_d = [C_d];
Dc_d = [D_d];

// Set initial angle of 8°
X0_d = [0; 0; -0.1416; 0]

// Defines controlled linear system
sbr_disc_ctrl = syslin('d', Ac_d, Bc_d, Cc_d, Dc_d, X0_d);


// **********
// Simulation
// **********

t = 0:per:4;                    // Time vector with 'per' resolution
sz = size(t);
r = zeros(1, sz(:,2));          // Input vector (no input)

y = csim(r,t,sbr_cont_ctrl);    // Continuous model
pos  = y(1,:);
tilt = y(2,:);

y_d = dsimul(sbr_disc_ctrl,r);  // Discrete model
pos_d  = y_d(1,:);
tilt_d = y_d(2,:);

figure(1);
clf(1);

subplot(2,1,1);
plot(t,pos_d, 'c.');
plot(t,pos,'b');
title('Cart Position');
xlabel('time (s)');
ylabel('position (m)');

subplot(2,1,2);
plot(t, tilt_d, 'm.')
plot(t,tilt,'k');
title('Pendulum tilt');
xlabel('time (s)');
ylabel('angle (rd)');

close(1);

 

The output of the script provides coefficients for the K matrix:

--> exec('D:\Users\Laurent\Documents\Projets\Segway\Simulation\segway_pomad.sce', -1)

!lss  A  B  C  D  X0  dt  !
   0.   1.          0.          0.
   0.  -0.1356273   9.4726416   0.
   0.   0.          0.          1.
   0.  -0.656432    93.278984   0.
   0.
   1.3562732
   0.
   6.5643197
   1.   0.   0.   0.
   0.   0.   1.   0.
   0.
   0.
   0.
   0.
   0.
   0.
 c

 poles:

   0.  
  -0.0689621  
  -9.6917327  
   9.6250675  

 controlability matrix rank:

   4.

 observability matrix rank:

   4.

 new poles (continuous)

  -9.6984204 + 1.1263055i
  -9.6984204 - 1.1263055i
  -1.9033796 + 1.7678988i
  -1.9033796 - 1.7678988i

 K (continuous)

   10.   7.7757962  -43.074845  -5.120725

!lss  A  B  C  D  X0  dt  !
   1.   0.0498292   0.0120455   0.0001993
   0.   0.993111    0.490607    0.0120455
   0.  -0.0008347   1.1187499   0.0519645
   0.  -0.0339979   4.8392845   1.1187499
   0.0017078
   0.0688905
   0.0083473
   0.3399792
   1.   0.   0.   0.
   0.   0.   1.   0.
   0.
   0.
   0.
   0.
   0.
   0.
   0.05

 controlability matrix rank:

   4.

 observability matrix rank:

   4.

 new poles (discrete)

   0.6148045 + 0.0350013i
   0.6148045 - 0.0350013i
   0.9056886 + 0.0803006i
   0.9056886 - 0.0803006i

 K (discrete)

   5.6181261   4.5784775  -31.721148  -3.6762483

Simulation results show perfect matching between continuous and discrete-time models. In addition, K-matrix coeficients have been tried in the XCOS simulation together with the non-linear model of the Self-Balanced robot. Figure below compares the results obtained for both models and simulators when the robot is released with a small initial tilt angle. You can play with the Q matrix coef. The higher, the more feedback you get on the control, the more it costs in terms of power on the motors.

xcos_005.png
 
 
xcos_006.png

4. Motor and Wheels modeling

 

At first order, DC motor torque is propotional to the supplied current:
image082.png

image083.png

Summing the torque applied to the wheel gives:
image085.png
 
The robot is assumed to be steady, so that regulation works around:
image087.png    image089.png
 
It comes:
image091.png
 
Stalled motor resistance and torque/current relationship are provided by the motor manufacturer:
image093.png
image095.png
 
At first order, for r=5cm, and taking account for the 2 wheels, we have almost u(N)=2*VM(V).
 
 

5. Fabrication ressources

 

The robot is built by assembly of available boards:

  • Nucleo F401RE for the CPU part
nucleo.png
  • Arduino Motor Power Shield (fixed on the Nucleo)

motor_shield.png

  • Invensens MPU 9250 breakout board for the IMU

 

Everything is attached to a mother board screwed on the robot carbon vertical plate. The board includes a socket for RF module (BT, XBEE) to allow remote control.

s_b_robot_sketchup.png
 
 

6. Embedded Software

The project archive contains a straightforward working code that keeps the robot (barely) steady.