Project on automatic control two wheeled self balancing robot

such asrocket model, self-balancing two-wheeled vehicle model, etc., therebyovercoming the inherent disadvantages of classic two- or three-wheeled robots.Classic two- or three-wheeled ro

Ministry of Education and Training

Ho Chi Minh City University of Technology and Education Faculty of Electrical and Electronics Engineering

Department of Automatic Control -⸙∆⸙ -

Project on Automatic Control Two-Wheeled Self-Balancing Robot

STUDENT’S NAME  : TRAN TUAN HIEU –  20151275

NGUYEN DINH HOAN - 20151277

Ho Chi Minh City, JANUARY 4th 2022

CHAPTER 1: OVERVIEW OF SELF-BALACED VEHICLES 1

1.1 what is two wheels self-balancing? 1

1.2 Models of self-balancing 2 wheels in labrotary 3

1.2.1 nBot 3

1.2.2 JOE 4

1.2.3 NXTway-GS 5

1.3 Some actual products are based on self-balancing 2 wheels robot model 5 1.3.1 Segway Personal Transporter 5

1.3.2 Winglet Toyota 6

1.3.3 Iswing Toyota 7

CHAPTER 2: THEORETICAL BASIC 9

2.1 PID controller 9

2.2 Modeling and symbols 9

2.3 Kinetic model of DC motor 10

2.4 Two wheels self-balancing robot 11

2.5 Simulation 15

2.5.1 System parameter declaration 15

2.5.2 System stability investigation 15

2.5.3 System controlability investigation 17

2.5.4 System observability investigation 18

2.5.5 controller construction 19

CHAPTER 3: SYSTEM DESIGN 22

3.1 Hardware 22

3.1.1 Arduino nano 22

3.1.2 CNC shield V4 24

3.1.3 Driver A4988 25

3.1.4 Stepper motor size 42 (1.8º) 25

3.1.5 GY-521 MPU 6050 angel sensor 26

3.1.6 65mm wheels 28

3.1.7 Battery 28

3.2 Control alogrithm flowchart 29

3.3 Software design 29

3.3.1 Main program 29

3.3.2 sensor library program 33

3.4 Realistic model 37

3.5 Realistic testing 38

3.5.1 Standard PID 38

3.5.2 Kp increased/decreased 39

3.5.3 Ki increased/decreased 41

3.5.4 Kd increased/decreased 42

CHAPTER 4: CONCLUSION AND DEVELOPMENT 44

4.1 Results 44

4.2 Limtitations and development directions 44

4.2.1 Limitations 44

4.2.2 Development directions 44

REFERENCES 45

CHAPTER 1: OVERVIEW OF SELF-BALACED VEHICLES

1.1 what is two wheels self-balancing?

In the field of automation - automatic control in general and cybernetics in particular, the inverted pendulum model is one of the typical research objects and

is characterized by the unstable dynamic characteristics of the model, so thecontrol controlling this object in fact poses as a challenge

The research results of the basic inverted pendulum model, such as the  pendulum model, the rotating inverted pendulum, etc can be applied andinherited to other similar models but have more practical application such asrocket model, self-balancing two-wheeled vehicle model, etc., therebyovercoming the inherent disadvantages of classic two- or three-wheeled robots.Classic two- or three-wheeled robots, which consist of a drive wheel and a freewheel (or whatever else) to support the robot's weight If a lot of weight is placed

car-on the rudder, the robot will be unstable and prcar-one to falling, and if put car-on manytail wheels, the two main wheels will lose their ability to grip Many robot designscan move well on flat terrain but cannot move up and down on convex or inclinedterrain When moving up a hill, the robot's weight is placed on the rear of thevehicle, causing it to lose its ability to grip and slip

3-wheel robot moves on flat terrain, the weight is divided equally betweenthe rudder and the small guide wheel

Figure 1) 3-wheeled robot moves on flat terrain3-wheel robot when going downhill, gravity on the rear wheel makes the carcan capsized

Figure 2) Robot with 3 wheels when going downhill3-wheel robot when going uphill, the weight is put on the front wheel, makingthe friction force to help the car stick on the road surface is not guaranteed

Figure 3) 3-wheel robot when going uphill

In contrast, coaxial two-wheeled robots balance very flexibly when movingmaneuvering over complex terrain, even though the robot itself is an unstablesystem determined When the robot moves on steep terrain, it automatically tiltsforward and holds for the weight to be transferred to the two main wheels.Similarly, when moving downhill, it lean back and keep the center of gravity onthe main wheel So there is never present The vehicle's center of gravity fallsoutside the wheel support area, which can cause a rollover

Robot nBot built by David P.Anderson The principle of controlling nBot

is as follows: the wheels will run in the direction where the upper part of the robot

is about to fall, if the wheels can be steered in a way that keeps the robot's center

of gravity, the robot will be balanced

The control process uses signals from two sensors: the sensor for the tiltangle of the robot body relative to the direction of gravity and the encodermounted on the wheel to measure the robot's position This signal forms fourvariables: robot body tilt angle, tilt angular velocity, robot position and robotspeed; These 4 variables are calculated into the motor control voltage for therobot

The control system consists of two separate "state space" controllers,controlling the motor to keep the system balance State information is provided

 by two optical encoders and two sensors: angular accelerometer and gyro

The JOE is controlled by an RC remote controller The central controllerand signal processing unit is a digital signal processing board (DSP) developed

 by the group itself and by the Federal Institute, in conjunction with XILINC'sFPGA

Figure 6) JOE

1.2.3 NXTway-GS

Figure 7) NXTway-GS by LEGO MINDSTORMS

1.3 Some actual products are based on self-balancing 2 wheels robotmodel

1.3.1 Segway Personal Transporter

Segway PT (short for Segway Personal Transporter), commonly known asSegway for short, is a two-wheeled personal transport vehicle, operating on a self- balancing mechanism invented by Dean Kamen This vehicle is manufactured bySegway Inc in the state of New Hampshire, USA The word "Segway" is

 pronounced akin to "segue" (an original Italian word meaning "to move gently").The outstanding feature of the Segway is the self-balancing mechanism thanks tothe computer system, engine and gyroscope located inside the vehicle, whichhelps the vehicle, even with only one axis of movement with two wheels, butalways in a state of balance Equally, the user only needs to lean forward or backward to control the car to go forward or backward

Figure 8) Segway Personal Transporter 

1.3.2 Winglet Toyota

The Winglet, developed by Toyota and introduced in 2008, has a base thesize of an A3 sheet of paper The vehicle is based on a self-balancing 2-wheelmodel and has sensors to recognize the movement of the driver, thereby givingcommands to the vehicle to operate according to the wishes of the driver

Figure 9) Winglet Toyota1.3.3 Iswing Toyota

Figure 10) Iswing Toyota

Iswing is known as one of the boldest ideas of the auto industry in recenttimes Making its debut at the 2005 Tokyo Motor Show, the Iswing is a symbol

of the future of personal vehicles because it is far different from conventionalcars The movement, control, communication between people and vehicles are allhuman For example, when placing the seat in the "minimum body opening"mode, the driver's eyes will be level with the person standing, so it is easy to talk

and create a friendly feeling The i-swing is controlled via handles and buttons,similar to how people use electronic devices every day

to operate optimally according to the user's wishes is the PID controller.

Figure 11) PID controller

To design a PID controller, we must set up a negative feedback controller

to get the error value between the output and the set value and then put it into thePID controller From the PID controller, it is easy to supply a control voltage sothat the system reaches the equilibrium position

2.2 Modeling and symbols

We have a two-wheeled balanced robot model as follows:

Figure 12) Self-balancing 2-wheeled robot model on a flat surface

Sign  Unit Meaning 

W 

Kb Vsec/rad  EMF factor of DC motor

a Rad/s2  Engine acceleration

m

l  Distance from the center of the wheel to the center of

gravity of the vehicle body

C L ,C R   Moment of engine acting on the wheel

H L  H R  P L  P R   Impact force between wheel and robot body

H fR ,  H fL Friction force milk wheel and plane

2.3 Kinetic model of DC motor

 ̇̇ = 0 1 0    +    0 0   1     (1)Where Vais power supplied to motor, τa is moment of motor

2.4 Two wheels self-balancing robot

Figure 13) Analysis of forces acting on 2 wheels

Apply Newton's law to calculate the total force acting on the wheel in thehorizontal direction:

From the toxicology equation and the differential equation of the DCmotor, we have the output torque of the motor:

 =     =      ̇      +      (7)Substituting equation (7) into equation (5) we get:

    ̈   =      ̇      +           (8)

We have:

 =      ̇  +        ̈   (9)Left wheel equation:

  ̈ =      ̇      +          ̈       (10)Right wheel equation:

  ̈ =      ̇      +          ̈       (11)Since linear motion acts on the motor shaft, the angular velocity can beconverts to angular velocity according to the following equation:

2M w  +    2     ̈  = 2   2       ̇  +2   2        +    (12)

Figure 14) Analysis of the force acting on the body of the robot

    = 0 Then we have linear model of the system:

With the above model, it is only true when the car is always on the plane and thefriction of the wheel with the plane is ignored, and the force is negligible

2.5 Simulation2.5.1 System parameter declaration

The result is:

HTXE1 =-51.33 s^2 + 5.47e-13 s + 2.325e04 -

s^4 - 6.843 s^3 - 453 s^2 - 23.3 sContinuous-time transfer function

HTXE2 =-0.03946 s^2 - 3668 s + 2.316e-12 -s^4 - 6.843 s^3 - 453 s^2 - 23.3 sContinuous-time transfer function

nghiem =

024.9996-18.1048-0.0515

Figure 15) Pole-Zero of systemThere are 2 poles (0 & 25) are not on the left side of Imagine Axis So thesystem is unstable

2.5.3 System controlability investigationBased on the theory of automatic control with a linear system, the statespace equation is as follows:

̇ =  +  

 =  + 

WhereA ∈ ℝ nn ,B ∈ ℝ nu ,C ∈ ℝ rn ,D ∈ ℝ rm Controllable matrix construction:

P = B,AB,A  B, ,A n− B 

The necessary and sufficient condition for a system to be describedmathematically as a controllable state equation is rank(P)=n Where n is thenumber of state variables of the system

Using Matlab to check controllability of the system:

P = [B A*B A^2*B A^3*B] 

rank(P) 

The result is:

The rank of the matrix P is equal to the number of state variables of thesystem So the system is controllable

2.5.4 System observability investigationChoosing matrixC = 1 0 0 0 0 0 1 0 

We have:. = 1 0 0 0 0 0 1 0  ̇ =   ̇  

Observable matrix construction:

L = C,AC,A  C, ,A n− C 

Using Matlab to check observability of the system:

L = [C; C*A; C*A^2; C*A^3] 

rank(L) 

The result is:

The rank of the matrix L is equal to the number of state variables of thesystem So the system is observable

Comment:  From the above 2 tests, we can conclude that the system iscontrollable and we can observe all 4 state variables of the system And throughsetting the matrix C to be able to observe two state variables, namely the robot's position and the robot's tilt angle, if these two variables are stable, the wholesystem will be stable

2.5.5 controller construction

Figure 16) system description with controller

In the self-balancing 2-wheel robot control system, we have to stablycontrol 2 parameters, namely the tilt angle and the position of the system, so weseparate it into 2 parallel or nested controllers and adjust the parameters Kp Ki

Kd to control the voltage applied to the motor to get the desired response

In order to stabilize the balance vehicle, we must first adjust the stability

of the vehicle's tilt angle first, and then adjust it to further stabilize the positioncontroller for the vehicle

Matlab Simulink construction:

Subsystem:

Figure 17) Simulink simulation

Figure 19) position simulation

CHAPTER 3: SYSTEM DESIGN

3.1 Hardware3.1.1 Arduino nano

Figure 20) Arduinano pins

Analog:

Digital:

3.1.2 CNC shield V4

Figure 21) CNC shield V4 pinsCNC shield V4 features:

 3 axis stepper motor driver

 Compatible with micro-drive laser engraving machine, three-axisCNC engraving machine

 Can control the two-phase four-wire stepper motor up to 2A

 Digital IO interface enables easy connections to other modules, such

3.1.3 Driver A4988Circuit connection:

Figure 22) A4988 connectionWorking mode:

3.1.4 Stepper motor size 42 (1.8º)

Figure 23) stepper motor

3.1.5 GY-521 MPU 6050 angel sensor

Figure 24) MPU 6050MPU 6050 pinout configuration:

MPU6050 Features

 MEMS 3-aixs accelerometer and 3-axis gyroscope values combined

 Power Supply: 3-5V

 Communication: I2C protocol

 Built-in 16-bit ADC provides high accuracy

 Built-in DMP provides high computational power

 Can be used to interface with other IIC devices like magnetometer

 Configurable IIC Address

 In-built Temperature sensor

3.1.6 65mm wheels

Figure 25) wheelsSpecifictions:

 Material: Hard plastic, foam padding, good rubber

 Diameter: 65mm

 Wheel width: 27mm3.1.7 Battery

Figure 26) Pin lipo 3s

 Voltage: 11.1V (12.6V fully charge) 

 Size: 106mm x 33mm x 23mm

 Weight: 166g3.2 Control alogrithm flowchart

Figure 27) flowchart3.3 Software design

3.3.1 Main program

 CNC shield configuration:

#define   Enable     //D8 //PORTB 0  

#define   Step_3     //D7 //PORTD 7  

#define   Step_2     //D6 //PORTD 6  

#define   Step_1     //D5 //PORTD 5  

#define   Dir_3     //D4 //PORTD 4  

#define   Dir_2     //D3 //PORTD 3  

#define   Dir_1     //D2 //PORTD 2  

#define   MS3     //D9 //PORTB 1

#define   MS2   10   //D10 //PORTB 2

#define   MS1   11   //D11 //PORTB 3

 Timer2:

 fo=16.000.000/8=2.000.000 HzTo=1/fo=1/2.000.000 s=0.5ustimer=40*0.5=20us

else  PORTD &= 0b 10111111 ;

Count_timer3++;

else  PORTD &= 0b 01111111 ;

 Speed function:

 V=60/(x*3200*20*10^(-6)) rpmExample: X=1 => V=937.5 rpm

X=10 => V=93.75 rpmX=400 => V=2.3 rpmvoid   Speed_L ( int16_t   x)    

pin_INI () ; //Khai báo PIN Arduino đấu nối 3 DRIVER

if   output > - 5  && output < 5  output = 0

while   loop_timer > micros ()) ;

void   init ( int   address)    

this -> i2cAddress  = address;

this -> gyroXOffset  = 0

this -> gyroYOffset  = 0

this -> gyroZOffset  = 0

this -> xAngle  = 0

Wire write ( )  

Wire endTransmission ( true ) Wire beginTransmission (this -> i2cAddress ) Wire write ( 0x 1B )  

Wire write ( )  

Wire endTransmission ( true ) Wire beginTransmission (this -> i2cAddress ) Wire write ( 0x 1C )  

Wire write ( )  

Wire endTransmission ( true ) }

void   calibrate ( int   times)    

for   int  i = 0 ; i < times; i++ ) { Wire beginTransmission (this -> i2cAddress ) Wire write ( 0x 43 )  

Wire endTransmission ( false ) Wire requestFrom (this -> i2cAddress , 6 true )

gyroRawX = Wire read ()  << 8  | Wire read () ; gyroRawY = Wire read ()  << 8  | Wire read () ; gyroRawZ = Wire read ()  << 8  | Wire read () ; gyroXTotal += gyroRawX;

gyroYTotal += gyroRawY;

gyroZTotal += gyroRawZ;