Self-balancing Robot

1. Non-Linear Mechanical System

1.1. Modeling

	[1]
	[2]

	[3]

[4]

 

Shift forces fromto

[1] →		[5]
[2] →		[6]

First governing equation

[4], [5] →

[7]

Second governing equation

[3], [5], [6] →

[8]

Solving [7] and [8] gives:

1.2. Simulation

1.2.1. Simulation inputs (external force, physical parameters)

1.2.2. Differential equations

1.2.3. Simulation Setup

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

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.

2. Linerization and Space-State representation

First governing equation

[7] →

[9]

Second governing equation

[8] →

[10]

Solving [9] and [10] gives:

	[11]
	[12]

And the corresponding Space State representation:

3. Control Synthesis

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.

4. Motor and Wheels modeling

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

   

5. Fabrication ressources

The robot is built by assembly of available boards:

• Nucleo F401RE for the CPU part

• Arduino Motor Power Shield (fixed on the Nucleo)

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

• You can get here the schematic of the mother board.
• You can get here the mechanical design (sketchup source model).

6. Embedded Software

• Get project from Gitlab

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