Skip to content

Inverted Pendulum Project V2

MEEN 667-Mechatronics

Term Project:

Real Time Control of an Inverted Pendulum

Objective and Significance

The objective of this project is to design and implement a two wheeled inverted pendulum system using real time feedback and control. This system is highly nonlinear by nature and is also open loop unstable necessitating the use of feedback control to create a closed loop stable system. In this project we will be utilizing the Arduino Mega 1280 microprocessor and a 6-DOF Inertial Measurement Unit (IMU) to measure the angle of the inverted pendulum. The concept of the inverted pendulum can be seen with the cart and pole mechanism shown in Figure 1. The actual concept of the inverted pendulum to be tested by the group can be compared to balancing an umbrella on the tip of one’s finger or to that of a Segway as can be seen in Figure 2.

figure-1

Figure 1: Inverted Pendulum-cart and pole

figure-2

Figure 2: Inverted pendulum abstractions

The significance of this project represents design and implementation of a highly nonlinear open loop unstable dynamic system. While the inverted pendulum may appear to be a simple dynamical system, the actual dynamics are highly nonlinear. This project will be dealing with 2-D dynamics only, and assumes that the inverted pendulum system will be traveling only forward and backward restricted to 2-D planar motion. Finally, the proposed inverted pendulum will be suitable in testing the Arduino microcontroller that will be used throughout the project.

figure-3

Figure 3:Arduino Mega 1280 Microprocessor

Robot Design and Methodology

Overall Design

As can be seen in Figure 4 and Figure 5, the inverted pendulum consists of two wheels connected by two DC geared motors mounted axially in line. The chassis is constructed from 1/8 inch thickness birch plywood and the frame consists of four 6 inch long threaded rods. These materials where chosen for ease of construction and the ability to modify the center of gravity of the chassis as needed by raising or lowering the cross frame plates.

figure-4

Figure 4: Two Wheeled Inverted Pendulum (TWIP)

invert_Pend_1st_Page.png

Figure 5:Exploded View of TWIP (Two Wheeled Inverted Pendulum)

            The Cad Model was first created in SolidWorks based on estimated material properties. Using the assembly model shown in Figure 5 Brushed DC motors were chosen based on the torque ratings necessary to rotate the pendulum from a worst case scenario 90 degree (α) from vertical case.

Cart_FBD

Figure 6:FBD of TWIP

From Figure 6 if α is 90 degrees, this will represent a worst case scenario for the amount of torque to be provided by the DC motors. Using SolidWorks Mass Properties, the mass properties of the system without the wheels are as follows, given a coordinate system based on the center of the axis of the motor shafts.

 

Mass properties of Inverted Pendulum (Assembly Configuration – no wheels )

Mass = 157.003383 grams

Center of mass: ( millimeters )

X = -0.001150

Y = 56.544130

Z = 0.033447

Moments of inertia (Taken at the output coordinate system.): ( grams * square millimeters )

 

Ixx = 1379568.226939 Ixy = 62.122893 Ixz = 0.853052
Iyx = 62.122893 Iyy = 420646.947037 Iyz = 22.717876
Izx = 0.853052 Izy = 22.717876 Izz = 1013611.993436

 

Using the CG of the chassis or inverted pendulum, located at 56.54413 mm above the Coordinate system, a simple moment calculation gives the maximum torque required:

From the calculations shown above the motors chosen will have more than enough torque to drive the inverted pendulum. Other considerations such as the ratings of the motor controller were also factored into selection of the Geared DC motor.

TB6612FNG.jpg

Figure 7: TB6612FNG Breakout Board H-Bridge dual channel motor controller

 

The motor driver chosen was a breakout board with integrated TB6612FNG (Figure 5) two channel H-Bridge, one channel for each motor. While the H-Bridge is capable of 3.2A maximum output at 15V, the average Iout per channel was limited to 1.2 A. Since the motors will be battery driven, we chose to use 4 D-Cell 1.5v alkaline batteries due to cost constraints.

The motor driver limited our choice of DC motors; however we were able to find two that met our requirements. The Pololu 35:1 metal gear motors were an ideal choice, whose properties are shown below in

35-1_metal_Gearmotor.jpg

Figure 8: 35:1 Pololu Metal Gear DC motor

The specifications for the DC motor are as follows:

  • @ 6 V
  • 460 RPM free run @ 60 mA
  • 13 oz-in (1.0 kg-cm) and 0.8 A stall torque and Current

With the motor specifications as listed above, we find that two of these motors have more than enough torque to lift the pendulum from a worst case scenario of a 90 degree (alpha) orientation.

MPU6050.jpg

 

Figure 9: MPU 6050 6-DOF IMU

            The 6-Degree of Freedom IMU chosen to measure the angle of the pendulum was the MPU6050 unit. This particular IMU features a 3 axis accelerometer and a 3 axis gyroscope all built onto the same IC die so as to reduce issues associated with misalignment of the sensor axes. One of the major advantages of this board is that the sensor fusion is pre-programmed and performed via an onboard math processor using unspecified algorithms which can output euler angles, quaternion representation, or angular rates directly. The board is also capable of simple raw gyro and accelerometer output, however after some simple testing it was determined that the output from the pre-processed data in terms of reliability was sufficient such that creating and implementing our own filtering algorithm would be unnecessary.

  • The MPU 6050 features a DMP (Digital Motion Processor)
    • Performs sensor fusion and filtering to combine the Gyro and accelerometer values
  • Outputs:
    • Euler Angles
    • Quaternion Representation
    • Gravity readings
    • Linear Acceleration with and without gravity compensation
    • We utilized the Euler angle output for this robot

The sensor is subject to slight amounts of drift initially, however when initializing the sensor, the algorithm compensates after a couple seconds.

 Arduino_Wiring.png

Figure 10:Wiring Diagram

Due to issues with the on/off electrical impulses of the motors overloading the sensors and causing the arduino to reset, I soldered resistors between the leads of the electrical motors as well as individually between each lead of the motor to the outside of the metal case of the electric motors.

De_Coupling

Figure 11: Decoupling the electric motors

Math Model

Using the Free Body Diagram shown in Figure 4 and the symbols listed in Table 1 we construct a math model via the Lagrange Method.

 

table-1.png

page-7

page-8

figure-10

Figure 11: Step Response

Experimental Results & Discussion

After construction and debugging of the robot, we were able to get the robot to balance, however it has at best an oscillatory response. The TWIP is able to balance for several seconds general before the gain causes it to oscillate into the unstable region.

There were initially some issues with the sensor readings while the motor controller was turning on and off. Using the oscilloscope to probe the voltage input to the sensor unit, we found that the voltage supply was rippling causing the sensor logic circuit to give wildly false readings. The solution was to use a De-Coupling capacitor across the power input supply as well as Capacitors across the voltage input on the sensor and across the DC motor leads to filter out high frequency noise. The H-Bridge motor controller that we implemented already had flyback diodes incorporated, so it was unnecessary to add them to the circuit.

The TWIP itself mostly showed oscillatory behavior, which I believe the discrepancy between the Math model and the PID parameters found via MATLAB and the real world Kp and Kd values has to do with inertia of the motors as well as a significant amount of gearbox driveline slop.

Also the sensor readings can drift slowly over time, so when the robot is first initialized it is best to hold it steady for several seconds until the sensor readings stabilize, after which the robot can be set down and allowed to stabilize.

Another issue is that the TWIP itself does not only travel in linear motion as the two DC motors do not supply exactly the same amount of torque. This yaw motion around the z axis also affects the dynamics of the cart and would have to be accounted for with a separate PD controller loop in an actual 6-DOF inverted pendulum cart.

Conclusions

We were able to create a TWIP which is able to stabilize for several seconds. Due to the difficulty in obtaining good sensor readings and other variables which we were unable to account for, the response is not optimal; however we did succeed in creating an inverted pendulum robot which can stabilize for several seconds.

 

Appendix

The Matlab code to implement this is shown here:

 

%Inverted Pendulum Project

%MEEN 667

clear;

clc;

% syms mp mw Iw r lcg Ip alpha g;

g=9.81e3;

%Gravity in mm/s^2

mp=157.003383;

%Mass of pendulum w/o 2 wheels (grams)

mw=22.269958*2;

%mass of 2 wheels (grams)

Iw=33191*2;

%Moment of inertia of 2 wheels (grams-mm^2)

r=45;

%Radius of wheel in mm

lcg=44.048260;

%CG of pendulum w/o wheels mm (y axis)

Ip=1079994.871901;

%Moment of inertia of pendulum w/o wheels (gram-mm^2)

a=mp+mw+Iw/r^2;

b=mp*lcg;

c=mp*lcg;

d=mp*lcg^2+Ip;

e=mp*g*lcg;

beta=a*e/(a*d-b*c);

gamma=-(c/(a*d-b*c));

delta=-b*e/(a*d-b*c);

epsilon=(b*c+a*d-b*c)/((a^2)*d-a*b*c);

A=[0 1 0;

beta 0 0;

delta 0 0];

B=[0;gamma/r;epsilon/r];

C=[1 0 0];

D=0;

sys=ss(A,B,C,D);

[num,den]=ss2tf(A,B,C,D);

tfa=tf([num],[den])

tfa=tf(A,B,C,D);

 

 

The Arduino Code is shown here:

 

#include “I2Cdev.h”

 

#include “MPU6050_6Axis_MotionApps20.h”

 

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include “Wire.h”

#endif

 

 

MPU6050 mpu;

//MPU6050 mpu(0x69); // <– use for AD0 high

 

#define OUTPUT_READABLE_YAWPITCHROLL

 

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

bool blinkState = false;

 

#define PWMA 3 //PWMA out to the motor controller for speed control

#define PWMB 4

//#define SENSOR 0 //SENSOR in pin 0 from potentiometer

#define AIN1 53 //Digital out from pin 22 to motor controller for direction

#define AIN2 51 //Digital out from pin 24 to motor controller for direction

#define BIN1 50 //Digital out from pin 22 to motor controller for direction

#define BIN2 52 //Digital out from pin 24 to motor controller for direction

 

// MPU control/status vars

bool dmpReady = false; // set true if DMP init was successful

uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU

uint8_t devStatus;     // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize;   // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount;     // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

 

int errorcw = 0;

int errorccw = 0;

double errorpid=0;

int correction = 0;

double kp = 24;

double ki = 0;

double kd = 5;

int PID = 0;

int error = 0;

int last_error = 0;

int angle = 0;

int sp = 0;

 

// orientation/motion vars

Quaternion q;           // [w, x, y, z]         quaternion container

VectorInt16 aa;         // [x, y, z]           accel sensor measurements

VectorInt16 aaReal;     // [x, y, z]           gravity-free accel sensor measurements

VectorInt16 aaWorld;   // [x, y, z]           world-frame accel sensor measurements

VectorFloat gravity;   // [x, y, z]           gravity vector

float euler[3];         // [psi, theta, phi]   Euler angle container

float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

 

// packet structure for InvenSense teapot demo

uint8_t teapotPacket[14] = { ‘$’, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, ‘\r’, ‘\n’ };

 

 

 

// ================================================================

// ===               INTERRUPT DETECTION ROUTINE              ===

// ================================================================

 

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

void dmpDataReady() {

mpuInterrupt = true;

}

 

 

 

// ================================================================

// ===                     INITIAL SETUP                       ===

// ================================================================

 

void setup() {

// join I2C bus (I2Cdev library doesn’t do this automatically)

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

Wire.begin();

TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

Fastwire::setup(400, true);

#endif

 

// initialize serial communication

Serial.begin(115200);

while (!Serial); // wait for Leonardo enumeration, others continue immediately

 

// initialize device

Serial.println(F(“Initializing I2C devices…”));

mpu.initialize();

 

// verify connection

Serial.println(F(“Testing device connections…”));

Serial.println(mpu.testConnection() ? F(“MPU6050 connection successful”) : F(“MPU6050 connection failed”));

 

// wait for ready

Serial.println(F(“\nSend any character to begin DMP programming and demo: “));

while (Serial.available() && Serial.read()); // empty buffer

while (!Serial.available());                 // wait for data

while (Serial.available() && Serial.read()); // empty buffer again

 

// load and configure the DMP

Serial.println(F(“Initializing DMP…”));

devStatus = mpu.dmpInitialize();

 

// supply your own gyro offsets here, scaled for min sensitivity

mpu.setXGyroOffset(220);

mpu.setYGyroOffset(60);

mpu.setZGyroOffset(-85);

mpu.setYAccelOffset(50);

mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

 

// make sure it worked (returns 0 if so)

if (devStatus == 0) {

// turn on the DMP, now that it’s ready

Serial.println(F(“Enabling DMP…”));

mpu.setDMPEnabled(true);

 

// enable Arduino interrupt detection

Serial.println(F(“Enabling interrupt detection (Arduino external interrupt 0)…”));

attachInterrupt(0, dmpDataReady, RISING);

mpuIntStatus = mpu.getIntStatus();

 

// set our DMP Ready flag so the main loop() function knows it’s okay to use it

Serial.println(F(“DMP ready! Waiting for first interrupt…”));

dmpReady = true;

 

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

} else {

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it’s going to break, usually the code will be 1)

Serial.print(F(“DMP Initialization failed (code “));

Serial.print(devStatus);

Serial.println(F(“)”));

}

pinMode(PWMA, OUTPUT); //set pin 13 as the output for the pwm to motor control

pinMode(PWMB, OUTPUT); //set pin 13 as the output for the pwm to motor control

pinMode(AIN1, OUTPUT);

pinMode(AIN2, OUTPUT);

pinMode(BIN1, OUTPUT);

pinMode(BIN2, OUTPUT);

// configure LED for output

pinMode(LED_PIN, OUTPUT);

}

 

 

 

// ================================================================

// ===                   MAIN PROGRAM LOOP                     ===

// ================================================================

 

void loop() {

// if programming failed, don’t try to do anything

if (!dmpReady) return;

 

// wait for MPU interrupt or extra packet(s) available

while (!mpuInterrupt && fifoCount < packetSize) {

angle=(ypr[1] * 180/M_PI);

//errorpid=sp-angle;

if(angle < sp)

{

errorcw = (sp-angle);

correction = calcPid(errorcw);

digitalWrite(AIN2, LOW);

digitalWrite(BIN2, LOW);

digitalWrite(AIN1, HIGH);

digitalWrite(BIN1, HIGH);

analogWrite(PWMA, correction);

analogWrite(PWMB, correction);

}

if(angle > sp)

{

errorccw = (angle);

correction = calcPid(errorccw);

digitalWrite(AIN1, LOW);

digitalWrite(BIN1, LOW);

digitalWrite(AIN2, HIGH);

digitalWrite(BIN2, HIGH);

analogWrite(PWMA, correction);

analogWrite(PWMB, correction);

}

}

 

// reset interrupt flag and get INT_STATUS byte

mpuInterrupt = false;

mpuIntStatus = mpu.getIntStatus();

 

// get current FIFO count

fifoCount = mpu.getFIFOCount();

 

// check for overflow (this should never happen unless our code is too inefficient)

if ((mpuIntStatus & 0x10) || fifoCount == 1024) {

// reset so we can continue cleanly

mpu.resetFIFO();

Serial.println(F(“FIFO overflow!”));

 

// otherwise, check for DMP data ready interrupt (this should happen frequently)

} else if (mpuIntStatus & 0x02) {

// wait for correct available data length, should be a VERY short wait

while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

 

// read a packet from FIFO

mpu.getFIFOBytes(fifoBuffer, packetSize);

 

// track FIFO count here in case there is > 1 packet available

// (this lets us immediately read more without waiting for an interrupt)

fifoCount -= packetSize;

 

 

//#ifdef OUTPUT_READABLE_YAWPITCHROLL

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

Serial.print(“ypr\t”);

Serial.print(ypr[0] * 180/M_PI);

Serial.print(“\t”);

Serial.print(ypr[1] * 180/M_PI);

Serial.print(“\t”);

Serial.println(ypr[2] * 180/M_PI);

//#endif

 

 

// blink LED to indicate activity

blinkState = !blinkState;

digitalWrite(LED_PIN, blinkState);

}

}

 

int calcPid(int error)

{

PID = (kp*(error)) + (ki*(error + last_error)) + (kd*(last_error-error));

last_error = error;

 

return constrain(PID, 0, 255);

}

 

For convenience I have attached a .zip file with the solidworks files and arduino code.

Cad files and programs

Advertisements
%d bloggers like this: