Arduino Neural Network Self Balancing Two Wheel Robot

This is a very simple application of a neural net solution for controlling a two wheeled self balancing robot. The robot does not do anything more than self balance; no control for directional control. But the project was simply to see if I could figure out how to use a neural net rather than a PID controller. Currently, I have a very primitive understanding of neural nets and I understand the basic concepts but I believe a lot more sophistication could be utilized in applying an ANN (Artificial Neural Network) to a self balancing robot. I hope there will be some comments by persons in the know about NNs with some ideas that could be applied here.

I adapted Sean Hodgins neural net code and you can find more specifics on how it works on his instructable here: https://www.instructables.com/id/Arduino-Neural-Net...

I adapted midhun_s excellent code for balancing my robot: https://www.instructables.com/id/Arduino-Self-Bala...

His code is some of the simplest and most easily implemented code that I have found for the Arduino for self balancing. However, I modified his code to get rid of the interrupts and instead just use a millis() timing routine so my code is even simpler.

I combined both of these excellent routines into a single program. It uses a neural net with a single input node, two hidden nodes and a single output node. The program first calls a training routine which takes only a few seconds so you can run it every time you start up the robot. Then the neural net routine runs and uses the standard sigmoid activation function which contains the node values to within 0 to 1. You simply need to map your input sensor values (which is just the tilt angle of the robot) to between 0 and 1, input the values into the neural net function which then returns an output in the value between 0 and 1. Then map that output value to a useful value for your motors, between -255 and 255 in this case.

A single input neural net is kind of overkill for this robot but as I explain later, more inputs could be used. This will give you a starting point for using neural nets on the Arduino.

1. FRAME: My frame is simply a couple of aluminum servo brackets bolted together to two vertical pieces of plywood, also held together with the servo brackets. It really doesn't matter what the frame is made of or how it is configured. You probably should make it a little high and put the battery on top - how tall is always a question, too high and the motors won't have the torque to turn the wheels fast enough, too low and the motors may be too slow to catch the tilting of the robot. A bottom horizontal piece of plywood holds the Arduino Uno and the motor controller.

2. MOTORS: I used two of the ubiquitous yellow geared motors and wheels you can find everywhere for a couple dollars each. They have about a 110 rpm speed and are just enough to balance but it would be nice if they were about 200 or 300 rpm. They have a little gear slop in them so the robot will always be oscillating a little bit.You should probably twist the two motor leads around each other before you connect them to the motor controller to stop stray electromagnetic fields from interfering with your Arduino. Attaching a couple of capacitors across the motor leads might be a good idea too. I attached my motors to the frame with a couple zip ties and it works fine.

3. MOTOR CONTROLLER: I used an L293D mini controller and I really like it because I can use a single 2s lipo battery to power the controller which also powers the Arduino Uno - no need for a second battery. Nice weight saver and less weight means an easier to balance robot.

4. MPU6050 Gyro/Accelerometer: This is a nice little module that measures the angle of the tilt of the robot. Very simple to call in a function. I mounted mine above the arduino and the axis of tilt of the robot. Some say it should be higher and some say lower but it works find where it is.

5. Arduino Uno: neural net will easily run in 2k.

6. Power Switch: it is really worth while attaching a power switch to turn the battery on and off. Makes using the robot easier than having to plug in the battery every time.

7. LIPO Battery: I use an 800mah 2s battery that powers everything. Battery life is usually about 20 minutes or more of continuous operation. Plenty enough for testing and playing with.

8. SCHEMATIC: the last picture is a schematic of how I connected all the modules and motors. MCU6050 should say MPU6050.

1. MPU6050 CALIBRATION: the first you need to do before actually running your robot is to calibrate the GYRO/ACCEROMETER. Download the calibration sketch located here: http://forum.arduino.cc/index.php?action=dlattach;... Before it executes, stand your robot straight up and don't move it while the calibration routine is running. You only have to run the calibration routine one time unless you happen to move the MPU6050 to a new position on the robot.

When you run it it will output 6 values to the Arduino serial monitor of which you will need three to put into your sketch.

2. NeuralNet-SelfBalancingRobot Sketch: load the following sketch to your Arduino Uno. You need to change the GYRO/ACC parameters to those from your calibration run. Then run the sketch and see if the robot balances. My robot will balance pretty decently on carpet or the bed but runs all over and then falls on a smooth floor.

I have PID code for my robot and it balances a little differently than it does with the Neuro Net but with the NN there was basically no tuning, just load the sketch and it balances. The PID routine required a lot of playing with.

I can upload my PID controller to the SB robot with no modifications for comparing PID vs NN software. The NN wins with less oscillating around the balance point but it loses to the PID with disturbances. But I haven't really tuned the NN at all yet.

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// neural net program using sigmoid function and applied to simple self balancing robot

// created by Jim Demello, Shangluo University, May 2018

// adapted Sean Hodgins neural net code: https://www.instructables.com/id/Arduino-Neural-Ne...

/ adapted midhun_s self balancing robot code: https://www.instructables.com/id/Arduino-Self-Bala...

/ built my own self balancing robot

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "MPU6050.h"

#include "math.h"

/******************************************************************

Network Configuration - customized per network

******************************************************************/

const int PatternCount = 2;

const int InputNodes = 1;

const int HiddenNodes = 3;

const int OutputNodes = 1;

const float LearningRate = 0.3;

const float Momentum = 0.9;

const float InitialWeightMax = 0.5;

const float Success = 0.0015;

float Input[PatternCount][InputNodes] = {

{ 0 }, // left lean

{ 1 } // right lean

// { -1} // left lean

// { 0, 1, 1, 0 }, // LIGHT ON LEFT AND RIGHT

// { 0, 1, 0, 0 }, // LIGHT ON LEFT

// { 1, 1, 1, 0 }, // LIGHT ON TOP, LEFT, and RIGHT

};

const float Target[PatternCount][OutputNodes] = {

{ 0, }, // left lean

{ 1, } // right lean

//{ -1, } // movement on left

// { 0.65, 0.55 }, //LEFT MOTOR SLOW

// { 0.75, 0.5 }, //LEFT MOTOR FASTER

};

/******************************************************************

End Network Configuration

******************************************************************/

int i, j, p, q, r;

int ReportEvery1000;

int RandomizedIndex[PatternCount];

long TrainingCycle;

float Rando;

float Error = 2;

float Accum;

float Hidden[HiddenNodes];

float Output[OutputNodes];

float HiddenWeights[InputNodes + 1][HiddenNodes];

float OutputWeights[HiddenNodes + 1][OutputNodes];

float HiddenDelta[HiddenNodes];

float OutputDelta[OutputNodes];

float ChangeHiddenWeights[InputNodes + 1][HiddenNodes];

float ChangeOutputWeights[HiddenNodes + 1][OutputNodes];

#define leftMotorPWMPin 6

#define leftMotorDirPin 7

#define rightMotorPWMPin 5

#define rightMotorDirPin 4

#define sampleTime 0.005

MPU6050 mpu;

int16_t accY, accZ, gyroX;

int motorPower, gyroRate;

float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;

byte count=0;

long previousMillis = 0;

unsigned long currentMillis;

long loopTimer = 4;

void setMotors(int leftMotorSpeed, int rightMotorSpeed) {

// Serial.print("leftMotorSpeed= ");Serial.print(leftMotorSpeed); Serial.print("rightMotorSpeed= ");Serial.println(rightMotorSpeed);

if(leftMotorSpeed >= 0) {

analogWrite(leftMotorPWMPin, leftMotorSpeed);

digitalWrite(leftMotorDirPin, LOW);

}

else { // if leftMotorSpeed is < 0 then set dir to reverse

analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed);

digitalWrite(leftMotorDirPin, HIGH);

}

if(rightMotorSpeed >= 0) {

analogWrite(rightMotorPWMPin, rightMotorSpeed);

digitalWrite(rightMotorDirPin, LOW);

}

else {

analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed);

digitalWrite(rightMotorDirPin, HIGH);

}

}

void setup() {

Serial.begin(115200);

Serial.println("Starting program");

randomSeed(analogRead(A1)); //Collect a random ADC sample for Randomization.

ReportEvery1000 = 1;

for ( p = 0 ; p < PatternCount ; p++ ) {

RandomizedIndex[p] = p ;

}

Serial.println("do train_nn");

train_nn();

delay(1000);

// set the motor control and PWM pins to output mode

pinMode(leftMotorPWMPin, OUTPUT);

pinMode(leftMotorDirPin, OUTPUT);

pinMode(rightMotorPWMPin, OUTPUT);

pinMode(rightMotorDirPin, OUTPUT);

// initialize the MPU6050 and set offset values

mpu.initialize();

mpu.setYAccelOffset(2113); // from calibration routine

mpu.setZAccelOffset(1122);

mpu.setXGyroOffset(7);

Serial.print("End Initialize MPU at: ");Serial.println(millis());

}

///////////////

/ main loop /

/////////////

void loop() {

drive_nn();

}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////

/USES TRAINED NEURAL NETWORK TO DRIVE ROBOT

void drive_nn()

{

Serial.println("Running NN Drive ");

while (Error < Success) {

currentMillis = millis();

float TestInput[] = {0, 0};

if(currentMillis - previousMillis > loopTimer) { //do calculation every 5 or more milliseconds

Serial.print("currentMillis= ");Serial.println(currentMillis);

////////////////////////////////////////

// calculate the angle of inclination //

////////////////////////////////////////

accY = mpu.getAccelerationY();

accZ = mpu.getAccelerationZ();

gyroX = mpu.getRotationX();

accAngle = atan2(accY, accZ)*RAD_TO_DEG;

gyroRate = map(gyroX, -32768, 32767, -250, 250);

gyroAngle = (float)gyroRate*sampleTime;

///////////////////////////////////////////////////////////////////

// complementary filter ///////////////////////////////////////////

//////////////////////////////////////////////////////////////////

currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);

//Serial.print("currentAngle= ");Serial.print(currentAngle);Serial.print(" error=");Serial.println(error);

//error = currentAngle - targetAngle; // not used

float nnInput = currentAngle ;

//Serial.print(" nnInput=");Serial.println(nnInput);

nnInput = map(nnInput,-30 ,30 , 0, 100); // map tilt angle range to 0 to 100

TestInput[0] = float(nnInput) / 100; //convert to 0 to 1

// Serial.print("testinput=");Serial.println(TestInput[0]);

InputToOutput(TestInput[0]); //INPUT to ANN to obtain OUTPUT

//Serial.print("output=");Serial.println(Output[0]);

///////////////////////////////////////////

// set motor power after constraining it //

///////////////////////////////////////////

motorPower = Output[0] * 100; // convert from 0 to 1

// if (motorPower < 50) motorPower = motorPower * -1;

motorPower = map(motorPower,0, 100, -300, 300);

motorPower = motorPower + (motorPower * 6.0); // need multiplier to get wheels spinning fast enough when close to balance point

//Serial.print("motorPower=");Serial.println(motorPower);

motorPower = constrain(motorPower, -255, 255);

prevAngle = currentAngle;

previousMillis = currentMillis;

} // end millis loop

// if (abs(error) > 30) motorPower = 0; // if fall over then shut off motors

//motorPower = motorPower + error;

setMotors(motorPower , motorPower );

}

} //end of drive_nn() function

//DISPLAYS INFORMATION WHILE TRAINING

void toTerminal()

{

for ( p = 0 ; p < PatternCount ; p++ ) {

Serial.println();

Serial.print (" Training Pattern: ");

Serial.println (p);

Serial.print (" Input ");

for ( i = 0 ; i < InputNodes ; i++ ) {

Serial.print (Input[p][i], DEC);

Serial.print (" ");

}

Serial.print (" Target ");

for ( i = 0 ; i < OutputNodes ; i++ ) {

Serial.print (Target[p][i], DEC);

Serial.print (" ");

}

/******************************************************************

Compute hidden layer activations

******************************************************************/

for ( i = 0 ; i < HiddenNodes ; i++ ) {

Accum = HiddenWeights[InputNodes][i] ;

for ( j = 0 ; j < InputNodes ; j++ ) {

Accum += Input[p][j] * HiddenWeights[j][i] ;

}

Hidden[i] = 1.0 / (1.0 + exp(-Accum)) ; // activation function

}

/******************************************************************

Compute output layer activations and calculate errors

******************************************************************/

for ( i = 0 ; i < OutputNodes ; i++ ) {

Accum = OutputWeights[HiddenNodes][i] ;

for ( j = 0 ; j < HiddenNodes ; j++ ) {

Accum += Hidden[j] * OutputWeights[j][i] ;

}

Output[i] = 1.0 / (1.0 + exp(-Accum)) ;

}

Serial.print (" Output ");

for ( i = 0 ; i < OutputNodes ; i++ ) {

Serial.print (Output[i], 5);

Serial.print (" ");

}

}

}

void InputToOutput(float In1)

{

float TestInput[] = {0};

TestInput[0] = In1;

// TestInput[1] = In2; // not used

// TestInput[2] = In3; // not used

// TestInput[3] = In4; // not used

/******************************************************************

Compute hidden layer activations

******************************************************************/

for ( i = 0 ; i < HiddenNodes ; i++ ) {

Accum = HiddenWeights[InputNodes][i] ;

for ( j = 0 ; j < InputNodes ; j++ ) {

Accum += TestInput[j] * HiddenWeights[j][i] ;

}

Hidden[i] = 1.0 / (1.0 + exp(-Accum)) ;

}

/******************************************************************

Compute output layer activations and calculate errors

******************************************************************/

for ( i = 0 ; i < OutputNodes ; i++ ) {

Accum = OutputWeights[HiddenNodes][i] ;

for ( j = 0 ; j < HiddenNodes ; j++ ) {

Accum += Hidden[j] * OutputWeights[j][i] ;

}

Output[i] = 1.0 / (1.0 + exp(-Accum)) ;

}

//#ifdef DEBUG

Serial.print (" Output ");

for ( i = 0 ; i < OutputNodes ; i++ ) {

Serial.print (Output[i], 5);

Serial.print (" ");

}

//#endif

}

//TRAINS THE NEURAL NETWORK

void train_nn() {

/******************************************************************

Initialize HiddenWeights and ChangeHiddenWeights

******************************************************************/

int prog_start = 0;

Serial.println("start training...");

//digitalWrite(LEDYEL, LOW);

for ( i = 0 ; i < HiddenNodes ; i++ ) {

for ( j = 0 ; j <= InputNodes ; j++ ) {

ChangeHiddenWeights[j][i] = 0.0 ;

Rando = float(random(100)) / 100;

HiddenWeights[j][i] = 2.0 * ( Rando - 0.5 ) * InitialWeightMax ;

}

}

//digitalWrite(LEDYEL, HIGH);

/******************************************************************

Initialize OutputWeights and ChangeOutputWeights

******************************************************************/

//digitalWrite(LEDRED, LOW);

for ( i = 0 ; i < OutputNodes ; i ++ ) {

for ( j = 0 ; j <= HiddenNodes ; j++ ) {

ChangeOutputWeights[j][i] = 0.0 ;

Rando = float(random(100)) / 100;

OutputWeights[j][i] = 2.0 * ( Rando - 0.5 ) * InitialWeightMax ;

}

}

//digitalWrite(LEDRED, HIGH);

//SerialUSB.println("Initial/Untrained Outputs: ");

//toTerminal();

/******************************************************************

Begin training

******************************************************************/

for ( TrainingCycle = 1 ; TrainingCycle < 2147483647 ; TrainingCycle++) {

/******************************************************************

Randomize order of training patterns

******************************************************************/

for ( p = 0 ; p < PatternCount ; p++) {

q = random(PatternCount);

r = RandomizedIndex[p] ;

RandomizedIndex[p] = RandomizedIndex[q] ;

RandomizedIndex[q] = r ;

}

Error = 0.0 ;

/******************************************************************

Cycle through each training pattern in the randomized order

******************************************************************/

for ( q = 0 ; q < PatternCount ; q++ ) {

p = RandomizedIndex[q];

/******************************************************************

Compute hidden layer activations

******************************************************************/

//digitalWrite(LEDYEL, LOW);

for ( i = 0 ; i < HiddenNodes ; i++ ) {

Accum = HiddenWeights[InputNodes][i] ;

for ( j = 0 ; j < InputNodes ; j++ ) {

Accum += Input[p][j] * HiddenWeights[j][i] ;

}

Hidden[i] = 1.0 / (1.0 + exp(-Accum)) ;

}

//digitalWrite(LEDYEL, HIGH);

/******************************************************************

Compute output layer activations and calculate errors

******************************************************************/

//digitalWrite(LEDRED, LOW);

for ( i = 0 ; i < OutputNodes ; i++ ) {

Accum = OutputWeights[HiddenNodes][i] ;

for ( j = 0 ; j < HiddenNodes ; j++ ) {

Accum += Hidden[j] * OutputWeights[j][i] ;

}

Output[i] = 1.0 / (1.0 + exp(-Accum)) ;

OutputDelta[i] = (Target[p][i] - Output[i]) * Output[i] * (1.0 - Output[i]) ;

Error += 0.5 * (Target[p][i] - Output[i]) * (Target[p][i] - Output[i]) ;

}

// Serial.println(Output[0]*100);

//digitalWrite(LEDRED, HIGH);

/******************************************************************

Backpropagate errors to hidden layer

******************************************************************/

//digitalWrite(LEDYEL, LOW);

for ( i = 0 ; i < HiddenNodes ; i++ ) {

Accum = 0.0 ;

for ( j = 0 ; j < OutputNodes ; j++ ) {

Accum += OutputWeights[i][j] * OutputDelta[j] ;

}

HiddenDelta[i] = Accum * Hidden[i] * (1.0 - Hidden[i]) ;

}

//digitalWrite(LEDYEL, HIGH);

/******************************************************************

Update Inner-->Hidden Weights

******************************************************************/

//digitalWrite(LEDRED, LOW);

for ( i = 0 ; i < HiddenNodes ; i++ ) {

ChangeHiddenWeights[InputNodes][i] = LearningRate * HiddenDelta[i] + Momentum * ChangeHiddenWeights[InputNodes][i] ;

HiddenWeights[InputNodes][i] += ChangeHiddenWeights[InputNodes][i] ;

for ( j = 0 ; j < InputNodes ; j++ ) {

ChangeHiddenWeights[j][i] = LearningRate * Input[p][j] * HiddenDelta[i] + Momentum * ChangeHiddenWeights[j][i];

HiddenWeights[j][i] += ChangeHiddenWeights[j][i] ;

}

}

//digitalWrite(LEDRED, HIGH);

/******************************************************************

Update Hidden-->Output Weights

******************************************************************/

//digitalWrite(LEDYEL, LOW);

for ( i = 0 ; i < OutputNodes ; i ++ ) {

ChangeOutputWeights[HiddenNodes][i] = LearningRate * OutputDelta[i] + Momentum * ChangeOutputWeights[HiddenNodes][i] ;

OutputWeights[HiddenNodes][i] += ChangeOutputWeights[HiddenNodes][i] ;

for ( j = 0 ; j < HiddenNodes ; j++ ) {

ChangeOutputWeights[j][i] = LearningRate * Hidden[j] * OutputDelta[i] + Momentum * ChangeOutputWeights[j][i] ;

OutputWeights[j][i] += ChangeOutputWeights[j][i] ;

}

}

//digitalWrite(LEDYEL, HIGH);

}

/******************************************************************

Every 100 cycles send data to terminal for display and draws the graph on OLED

******************************************************************/

ReportEvery1000 = ReportEvery1000 - 1;

if (ReportEvery1000 == 0)

{

int graphNum = TrainingCycle / 100;

int graphE1 = Error * 1000;

int graphE = map(graphE1, 3, 80, 47, 0);

Serial.print ("TrainingCycle: ");

Serial.print (TrainingCycle);

Serial.print (" Error = ");

Serial.println (Error, 5);

toTerminal();

if (TrainingCycle == 1)

{

ReportEvery1000 = 99;

}

else

{

ReportEvery1000 = 100;

}

}

/******************************************************************

If error rate is less than pre-determined threshold then end

******************************************************************/

if ( Error < Success ) break ;

}

}

1. The parameters could probably be played with a bit, especially the multiplier that increases the NN output values. You have to use this multiplier to increase the rpm of the motor when it is close to balancing. Turns out this almost forces the robot into a bang-bang, on and off balancing robot. If the values of the motor near the balancing point are not high enough then the robot will fall over before the motors have enough rpm to catch the fall.

2. Perhaps a better activation function could be used than the sigmoid function. Some say the tanf function is more useful. I think a simple f(x) function is all that is really required. Would be really interested in anyone's research in this area.

3. This is a simple single input, multiple hidden node, and single output neural net and definitely overkill as a PID controller would be simpler and you can actually achieve balance with a simple P controller which is just one line of code. Yet, I did not not have to tune this NN like you would a PID controller, so that was cool. It would be interesting to use more inputs and you could simply make the gyroscope values two inputs and the accelerometer as another one for a three input neural net. Then you would not need a complementary filter as the neural net would be acting as the filter. Not exactly sure how to do that but it might be interesting to try.