(Đồ Án Hcmute) Research, Design And Construct Two-Wheel Balancing Robot For Object Tracking.pdf

Ho Chi Minh city, July, 2017 MINISTRY OF EDUCATION AND TRAINING HCMC UNIVERSITY OF TECHNOLOGY AND EDUCATION FACULTY OF HIGH QUALITY TRAINING ADVISOR NGUYEN VAN THAI, PhD STUDENT NGUYEN HOANG HAI NGUYE[.]

MINISTRY OF EDUCATION AND TRAINING HCMC UNIVERSITY OF TECHNOLOGY AND EDUCATION

FACULTY OF HIGH QUALITY TRAINING

ADVISOR: NGUYEN VAN THAI, PhD STUDENT: NGUYEN HOANG HAI NGUYEN DANG KHOA

CAPSTONE PROJECT AUTOMATION AND CONTROL ENGINEERING TECHNOLOGY

S KL01 0 0 2 8

RESEARCH, DESIGN AND CONSTRUCT TWO - WHEEL BALANCING ROBOT FOR OBJECT TRACKING

MINISTRY OF EDUCATION AND TRAINING HCMC UNIVERSITY OF TECHNOLOGY AND EDUCATION

FACULTY OF HIGH QUALITY TRAINING DEPARTMENT OF AUTOMATION CONTROL

-***    *** -

THESIS RESEARCH, DESIGN AND CONSTRUCT TWO- WHEEL BALANCING ROBOT FOR OBJECT

TRACKING

MAJOR: AUTOMATION AND CONTROL ENGINEERING

TECHNOLOGY ADVISOR: NGUYEN VAN THAI, PhD

HCMC UNIVERSITY OF TECHNOLOGY AND EDUCATION



FACULTY OF HIGH QUALITY TRAINING

THESIS RESEARCH, DESIGN AND CONSTRUCT TWO-

WHEEL BALANCING ROBOT FOR OBJECT

TRACKING

MAJOR: AUTOMATION AND CONTROL ENGINEERING

TECHNOLOGY ADVISOR: NGUYEN VAN THAI, PhD

HCMC UNIVERSITY OF TECHNOLOGY AND EDUCATION



FACULTY OF HIGH QUALITY TRAINING

THESIS RESEARCH, DESIGN AND CONSTRUCT TWO-

WHEEL BALANCING ROBOT FOR OBJECT

TRACKING

MAJOR: AUTOMATION AND CONTROL ENGINEERING

TECHNOLOGY ADVISOR: NGUYEN VAN THAI, PhD

SOCIALIST REPUBLIC OF VIETNAM Independence – Freedom – Happiness

*******

Ho Chi Minh city, July, 2017

TASKS OF THE THESIS

Name of student: Student ID:

Name of student: Student ID:

Major: Class:

Name of Advisor: Mobile phone:

1 Name of the thesis:

2 Initial figures and documents:

3 Content of the thesis:

4 Product:

Ho Chi Minh city, July ,2017 Ho Chi Minh city, July ,2017

SOCIALIST REPUBLIC OF VIETNAM Independence – Freedom – Happiness

*******

ADVISOR’S COMMENT SHEET

Name of student: Student ID:

Name of student: Student ID:

Major:

Name of the thesis:

Name of Advisor:

COMMENT 1 About the thesis’s content and workload:

2 Advantage:

3 Disadvantage:

4 Propose defending thesis? 5 Rating:

6 Mark: (In writing: )

SOCIALIST REPUBLIC OF VIETNAM Independence – Freedom – Happiness

*******

REVIEWER’S COMMENT SHEET

Name of student: Student ID:

Name of student: Student ID:

Major:

Name of the thesis:

Name of Reviewer:

COMMENT 3 About the thesis’s content and workload:

4 Advantage:

3 Disadvantage:

7 Propose defending thesis? 8 Rating:

9 Mark: (In writing: )

we get in stuck to find ideas We always remember the time when we first met him

to discuss about the thesis

Many thanks to our vice president of HCMC University of Technology and Education, Ngo Van Thuyen, PhD who is in charge of scientific research, international relations, publics relations, construction of ODA projects, deploy KPIs and quality assurance of our university He gave us the chances not only broaden our horizon but also strengthen our personality, soft skills

I would like to thank Nguyen Minh Tam, PhD Dean of Faculty of Electrical and Electronics Engineering and Truong Dinh Nhon, PhD They never hesitate to help us when we encounter problems And we could not have done the research without all opportunities which Le My Ha, PhD and MS Nguyen Tran Minh Nguyet, from Department of Automation Control of Faculty of High Quality Training, created for

us Especially, M.Eng Nguyen Tran Minh Nguyet was very closed to us, we learnt really a lot from our discussions and her encouragement as well as life lessons of Le

My Ha, PhD

Last but not least, our deepest gratitude goes to our family for their continuous presence and uncommon strength We are eternally grateful to our parents for their unflagging love and support throughout our life

Bên cạnh thực hiện xe hai bánh tự cân bằng, nhóm sẽ phát triển xe để ứng dụng

xử lý ảnh trên hệ điều hành android nhằm theo dõi và bám mục tiêu cụ thể là quả bóng tennis Để robot bám theo quả bóng tennis và duy trì khoảng cách mong muốn, cần kết hợp thêm các bộ điều khiển PID vào bộ điều khiển LQR để robot vừa cân bằng vừa bám theo trái banh

Luận văn này sẽ trình bày bao gồm cả phần cứng bao gồm: Arduino Mega 2560, cảm biến góc nghiêng MPU 6050, board nhóm thiết kế tích hợp mạch cầu H L298 max 50V-2A và giảm áp LM2596 1.23-27V, module Bluetooth HC-05,…

Besides making “Two-wheel balancing robot”, our team will upgrade one new function for this robot – monitoring and tracking the object by using image processing

on android After several times of considerations, we have chosen a solution that smartphone identifies, calculates the distance between the robot and the object and sends data to Arduino Mega 2560 for processing through module Bluetooth HC-05 because of the convenience of using a smartphone instead of raspberry pi To make the robot track the tennis ball and maintain the desired distance from it requiring combination between PID controllers and LQR controller so the robot would be able

to balance and track the ball at the same time

We will present the hardware of system includes Arduino Mega 2560, sensor MPU 6050, board designed by our team integrated in H-bridge L298 50V-2A and LM2596 regulator 1.23-27V, module Bluetooth HC-05…

TABLE OF CONTENTS ACKNOWLEDGEMENTS I TÓM TẮT II ABSTRACT III TABLE OF CONTENTS IV ABBREVIATIONS AND ACRONYMS VII LIST OF FIGURES VIII LIST OF TABLES XI REFERENCES XII

CHAPTER 1: GENERAL 1

1.1.INTRODUCTION: 1

1.2.APPLICATION: 1

1.3.GENERALCURRENTSITUATIONOFRESEARCH: 1

1.4.OBJECTIVESOFTHETHESIS: 1

1.5.OBJECTANDRESEARCHINGSCOPE: 2

1.6.RESEARCHINGMETHOD: 2

1.6.1 THEORETICAL METHOD: 2

1.6.2 PRACTICAL METHOD: 3

1.7.THECONTENTOFTHESIS: 3

CHAPTER 2: THEORETICAL BASIS 5

2.1.THEPRINCIPLEOFBALANCINGROBOT: 5

2.2.SYSTEMDYNAMICS: 6

2.3.LQRCONTROLLER: 11

2.3.1 CONTINUOUS TIME LQR 12

2.3.2 DISCRETE – TIME LQR 13

2.3.3 OPTIMAL CONTROL FOR TWO-WHEEL BALANCING ROBOT SYSTEM 13

2.4.KALMANFILTER 17

2.5.ANDROIDOPERATINGSYSTEM: 25

2.6.OPENCVLIBRARY: 27

2.6.1 RGB AND HSV COLOR SPACE: 28

2.6.2 CONVERT RGB TO HSV: 30

2.6.3 IDENTIFYING OBJECT’S COLOR BY HSV COLOR SPACE: 31

2.6.4 EDGE DETECTION BY CANNY EDGE DETECTION: 32

2.6.4.1 IMAGE SMOOTHING: 33

2.6.4.2 CALCULATION OF THE STRENGTH AND DIRECTION OF EDGES: 34

2.6.4.3 NON-MAXIMUM SUPPRESSION: 35

2.6.4.4 THRESHOLDING WITH HYSTERESIS: 36

2.6.5 DETECT LOCATION OF CIRCLE: 37

2.6.5.1 CONVOLUTION AND NON-MAXIMUM SUPPRESSION: 37

2.6.5.2 CONTOUR TRACING: 42

2.6.5.3 FINDING CENTER AND RADIUS OF THE CIRCLE: 47

2.6.6 MORPHOLOGICAL TRANSFORMATIONS: 49

2.6.6.1 EROSION: 49

2.6.6.2 DILATION: 50

2.6.6.3 OPENING: 50

2.6.6.4 CLOSING: 51

2.6.7 CALCULATING THE DISTANCE BETWEEN ROBOT AND BALL: 51

2.7.PIDCONTROLLER: 53

CHAPTER 3: SYSTEM DESIGN 56

3.1.HARDWAREGENERAL 56

3.2.HARDWAREANDFUNCTIONS 56

3.2.1 MICROPROCESSOR 56

3.2.2 MPU6050 SENSOR 58

3.2.3 MOTOR AND ENCODERS 64

3.3.H-BRIDGEMOTORDRIVER: 66

3.4.POWERSUPPLY: 68

3.5.MODULEBLUETOOH: 72

3.6.MAINBOARD 74

CHAPTER 4: ALGORITHMS 75

4.1.GENERAL: 75

4.2.GENERALFLOWCHARTOFIMAGEPROCESSING: 76

4.2.1 TRANSFER DATA: 76

4.2.2 IDENTIFY COLOR & MORPHOLOGICAL TRANSFORMATION: 77 4.2.3 DETECT THE BALL: 78

4.3.FLOWCHARTOFROBOTCONTROL: 79

CHAPTER 5: RESULTS 80

5.1.PRACTICALMODEL: 80

5.2.MATLABSIMULATION: 81

5.2.1 COMPUTER INTERFACE: 82

5.3.EXPERIMENTALRESULT: 83

5.3.1 EXPERIMENTAL RESULT OF MANUAL MODE: 83

5.3.2 EXPERIMENTAL RESULT OF TRACKING MODE: 83

CHAPTER 6: CONCLUSION AND DEVELOPING ORIENTATION 84

6.1.CONCLUSION: 84

6.2.DEVELOPINGORIENTATION: 84

ABBREVIATIONS AND ACRONYMS

MPU: Micro Processor Unit

SPI: Serial Peripheral Bus

I2C: Inter-Integrated Circuit

SCL: Serial Clock

BJT: Bipolar Junction Transistor

IDE: Integrated Development Environment

LQR: Linear Quadratic Regulator

PID: Proportional Integral Derivative

HSV: Hue Saturation Value

LIST OF FIGURES

Figure 2.1: The operating principle of two wheels balancing robot 5

Figure 2.2: The balancing status and moving forward mode 6

Figure 2.3: The robot goes uphill and downhill 6

Figure 2.4: The model of two wheel balancing robot 7

Figure 2.5: The LQR control structure 11

Figure 2.6: LQR control diagram 12

Figure 2.7: LQR controller diagram for Two-wheel self-balancing robot 16

Figure 2.8: Complete process of Kalman filter 17

Figure 2.9: Combination of two Gaussian curves 20

Figure 2.10: Uncertainty around xk. 23

Figure 2.11: Logo of Android Operating System 25

Figure 2.12: OpenCV Library 27

Figure 2.13: Cube describes RGB color space 28

Figure 2.14: HSV color space 30

Figure 2.15: The figure is described in RGB color space (above) and The figure is described in HSV color space (below) 31

Figure 2.16: The image after processing by InRange function 32

Figure 2.17: Block diagram of the stages of Canny edge detector [10] 33

Figure 2.18: One of four angles to be used in Non-Maximum Suppression 35

Figure 2.19: Linear window at the angle of 135 degree, 90 degree, 45 degree and 0 degree 35

Figure 2.20: An example of Non-Maximum Suppression 36

Figure 2.21: Three stages of detecting location of circle 37

Figure 2.22: (a) 1-dimensional change in intensity (b) Derivative of the change in intensity 37

Figure 2.23: The kernels used to find Horizontal gradient orientation 38

Figure 2.24: Kernels used to find vertical gradient orientation 39

Figure 2.25: Range of angles of pixel direction 42

Figure 2.26: Representing the priority of the pixels to be scanned based on the pixel direction 2 44

Figure 2.27: Representing the priority of the pixels to be scanned based on the pixel direction 6 45

Figure 2.28: Representing the priority of the pixels to be scanned based on the pixel direction 1, 3, 4, 5, 7, 8 46

Figure 2.29: Arcs of the circle with edge direction in ideal conditions 46

Figure 2.30: Block 3x3 connecting the arcs to find 4 major arcs 47

Figure 2.31: All the combinations of major arcs which can form a circle 47

Figure 2.32: Finding center of the circle using the perpendicular bisector property of the chord 48

Figure 2.33: R1 and R2 radius found using the arc with pixel direction 3 and 1 respectively 49

Figure 2.34: The original image 49

Figure 2.35: Erosion using a 5x5 kernel 49

Figure 2.36: Dilation using a 5x5 kernel 50

Figure 2.37: The original image with noise and noise removed after using Opening 50

Figure 2.38: The original image with holes and holes removed after using Closing 51

Figure 2.39: The system of convergent lens 51

Figure 2.40: PID Controller 53

Figure 2.41: Overall algorithm diagram of the system 55

Figure 3.1: Overall block diagram of the system 56

Figure 3.2: Board Arduino Mega 2560 56

Figure 3.3: Piezo Electric Accelerometer 58

Figure 3.4: Piezo Electric Gyroscope 59

Figure 3.5: Module GY521 59

Figure 3.6: Schematic diagram 60

Figure 3.7: Connect module GY521 to Arduino Mega 64

Figure 3.8: Free body diagram of the balancing robot 64

Figure 3.9: Motor Nisca NF5475E 66

Figure 3.10: Schematic of H-bridge 66

Figure 3.11: L298 footprint 67

Figure 3.12: LM2596-ADJ footprint 68

Figure 3.13: Internal block diagram of LM2596 69

Figure 3.14: Inductor Value Selection Graph 70

Figure 3.15: Step-down Converter Schematic 72

Figure 3.16: Module Bluetooth HC05 72

Figure 3.17: HC05 Schematic 73

Figure 3.18: Mainboard Schematic 74

Figure 3.19: PCB Layout 74

Figure 4.1: The general diagram of the system 75

Figure 4.2: The basic step of image processing 75

Figure 4.3: Flowchart of image processing in this thesis 76

Figure 4.4: The diagram of transfer data 76

Figure 4.5: Color detection and morphological transformation 77

Figure 4.6: Detect the ball 78

Figure 4.7: Flowchart of controlling robot 79

Figure 5.1: The practical model 80

Figure 5.2: Completed board 80

Figure 5.3: Simulation of Robot using LQR controller 81

Figure 5.4: Output response 82

Figure 5.5: Communicating interface 82

Figure 5.6: Experimental result of manual mode 83

Figure 5.7: Experimental result of tracking mode 83

LIST OF TABLES

Table 2.1: Table of symbol, coefficients and parameters 8

Table 2.2: Conditions used to determine the pixel direction 41

Table 2.3: Image with linear window in color corresponding to the pixel direction and gradient magnitude and result of NMS 42

Table 2.4: Relation of Pixel and Chain Code with current pixel 43

Table 2.5: Coordinates of next pixel calculated based on the Chain Code for current pixel (5, 5) 43

Table 3.1: Technical specifications 57

Table 3.2: Pins of module MPU6050 60

Table 3.3: List of available type of motors 65

Table 3.4: Specification of Motor Nisca NF5475E 66

Table 3.5: Inductor Manufacturers Part Numbers 71

Table 3.6: Specification of module Bluetooth HC-05 73

REFERENCES

Tiếng Việt

[1] Lê Phước Vạn, Trương Lê Hữu Phát (2016), “Robot Hai Bánh Tự Cân Bằng

Dùng Giải Thuật LQR”, 56 trang

[2] Nguyễn Ngọc Sáng, Phạm Chánh Trực (2016), “Robot Di Động Theo Dõi và

Bám Mục Tiêu Ứng Dụng Xử Lý Ảnh Trên Hệ Điều Hành Android” (2016),

54 trang

English

[3] C Sun and P Vallotton (2006), “Fast Linear Feature Detection Using Mutiple

Directional Non-Maximum Suppression”, Proc Int Conf Pattern

Recognition, pp.288-291

[4] F.Yan, X Shao, G Li, Z Sun, and Z.Yang (2008), “Edge Detection of Tank

Level IR Imaging Based On The Auto-Adaptive Double-Threshold Canny

Operator”, Intell Inform Technology Applicant Research Assoc., pp.366-370

[5] Grag Welch and Gary Bishop (2006), “A Introduction to the Kalman Filter”,

UNC-Chapel Hill, pp.1-16

[6] G Q Lu, H G Xu; and Y B Li (2005), “Line Detection Based On Chain

Code Detection”, IEEE Int Conf Vehicular Safety, pp.98-103

[7] H Freeman (1974), “Computer Processing of Line Drawing Images”,

Computing Surveys, pp 57-97

[8] J Canny (1986), “A Computational Approach to Edge Detection”, IEEE

Trans Pattern Anal And Mach Intell, pp 679-714

[9] M Z Zhang and H R Cao (2008), “A New Method of Circle’s Center and

Radius Detection in Image Processing”, IEEE Int Conf Automation and

Logistics, pp 2239-2242

[10] Prajwal Shetty (2011), “Circle Detection in Image”, pp 1-30

[11] Ramsey Faragher (2012), “Understanding The Basic of the Kalman Filter Via

a Simple and Intuitive Derivation”, IEEE Signal Processing Magazine, pp

128-132

CHAPTER 1: GENERAL

1.1 INTRODUCTION:

The two-wheel balancing robot is based on the principle of balance of the inverted pendulum Research and development of self-balancing robot are the premise to make personal vehicles capable of self-balance with some advantages such

as less space using and flexible movement Besides that, in modern life, the robot also has some features which can replace the sense of human For this reason, the combination of sensor and image processing makes robot perfectly interacting with the environment around us

1.2 APPLICATION:

In terms of science, the research of two-wheel balancing robot has gained momentum over the last decade due to its nonlinear and unstable dynamics system Various control algorithms have been implemented by numerous researchers to control the two-wheel balancing robot, so this is kind of a “prototype” for researchers

to investigate the efficiency of various controllers in control system

The purpose of two-wheel balancing robots is difficult to limit to a specific role

as they can complete numerous tasks with the necessary attachments installed One purpose could be to access a hazardous or confined environment which would be difficult to move around for multi-wheeled vehicle This is easily achieved by a two-wheel balancing robot as it can turn on the spot by rotating the right wheel while the left rotates backward and vice versa

It also could be utilized as a warehouse robot with the main task is tracking item status and update new information to a server system through a wireless connection

In a fire warden role, the robot could access a filled smoke area and evaluate the risks before any human lives would be placed in danger A camera would provide visual images to the fire commander while sensor would measure temperature

1.3 GENERAL CURRENT SITUATION OF RESEARCH:

There is still not any information about the two-wheel balancing robot made by Vietnam There are only a few models made by university student groups

1.4 OBJECTIVES OF THE THESIS:

The project objectives are divided into several key goals which our team need

to concentrate:

 Research will be conducted into the theory of an inverted pendulum and the various considerations that may be necessary during the construction of a two-wheel balancing robot

 Researching and using the suitable and compatible components such as sensor, microcontroller, motor, driver, power supply, communication…In detail, this thesis will mainly focus on using MPU6050 and Arduino Mega 2560

 Using Kalman filter to read and guess data from MPU 6050 sensor

 Designing circuit board which can combine and replace basic modules, regulator effectively

 Designing a solid frame which can withstand during movement

 Researching and applying LQR, PID controller for robot

 Researching OpenCV and Android operating system for tracking the green ball with consistent distance The target is not over 10% error of accuracy

1.5 OBJECT AND RESEARCHING SCOPE:

There are many kinds of two-wheel balancing robot In the scope of this thesis, the microprocessor is effectively applying LQR, PID algorithms on two-wheel balancing robot while using smartphone for tracking a green ball and moving on even terrain The result will be presented and checked by using Matlab/Simulink and practical result

This is the first time our team conducts the research for “Two-wheel balancing robot tracking the ball” in conditions:

 The time allowed is one semester

 Lacking the practical experience

 Components are not perfectly synchronous

Therefore, this thesis exists some limitations such as Robot can track the ball beyond the visibility of camera or move so fast

1.6 RESEARCHING METHOD:

1.6.1 THEORETICAL METHOD:

The research project was divided into chapters, each a sequential step in the process of developing and building the two-wheel balancing robot This approach was utilized in an attempt to progress the project from one task to the next as it was undertaken Each is defined so that it builds on the previous task thus evolving the robot within the goals and requirements generated This ultimately led to the completion of the two-wheel balancing robot that met the objectives within the timeframe available

The first step is to define the key point and objective to deeply understanding what a two – wheeled balancing robot actually is Besides that, it is critical to determining plans for conducting your research and performing the suitable design and programming

The second step is to refer the previous projects It is really useful to have appropriate approaches This established the foundations for making an informed decision based on the previous experiences to avoid mistakes and obsolete design

The third step in theoretical method was to apply knowledge studied to control system This approach provides valuable data of the likely stability capability of the control system by finding the suitable parameters through many days

The next step is to manufacture PCB, chassis, drive shaft, etc…and assembly all hardware components after finishing manufacture In parallel, for making the good system, the following step is to analyze the performance of the system This also provides the chances to calibrate and perform additional changes to making the final system

1.6.2 PRACTICAL METHOD:

The practical method is to directly design mechanism and PCB This approach allows for testing on real system and making the final assessment by the response of two – wheel balancing robot

1.7 THE CONTENT OF THESIS:

The thesis “Two-wheel balancing robot tracking the ball” includes the following chapters:

Chapter I: General: This chapter provides a brief overview of the requirements of

the report including introduction, goals, scope and content of the thesis

Chapter II: Theoretical Basic: This chapter provides the basic knowledge about this

thesis such as the principle of balancing robot, system dynamics, LQR and PID controller, Kalman filter and image processing

Chapter III: System Design: This chapter provides the robot’s design, choosing

hardware After that, constructing the robot based on the design

Chapter IV: Algorithms: This chapter provides algorithms, diagrams

Chapter V: Result: This chapter shows the result of this thesis

Chapter VI: Conclusion and Developing Orientation: This chapter provides the

conclusion in term of advantage and limitation of this thesis This also concludes the contribution and proposing ideas, orientations to develop the project

CHAPTER 2: THEORETICAL BASIS

2.1 THE PRINCIPLE OF BALANCING ROBOT:

A robot has the ability to maintain balance when move on its two wheels is known as a two – wheeled balancing robot The process of balancing is one of the most important part of two – wheel balancing robot and also typically referred to as stability control The two wheels are also the main components and are situated below the base of robots to keep the robot move forward and backward while still upright all time

Figure 2.1: The operating principle of two wheels balancing robot

This type of robot provides researchers many challenges while frequently encounter problems with an unstable condition The theory used to maintain the stability of this robot is based on the inverted pendulum theory This theory requires

a deep knowledge of the parameters within the system to develop a reliable and capable control system Inverted pendulum theory is well known as a Pole and Cart theory Although it does not seem to be the same, there are some common principles between the two-wheel balancing robot and Pole & Cart model, for example within the system model, the cart equates to the wheels while the pole is not much different with the robot’s chassis

It is necessary to generalize the effects of the left and right wheels and incorporate them together under combined term “wheels” This makes the calculation easily as both wheels will operate in unison to maintain stability

The aim of the inverted pendulum principle to keep the wheels beneath the centre of the robot chassis’s mass If the chassis tilts forward, the two wheels will move forward so that the tilt angle is zero then the robot will maintain the balance In the other hand, if the chassis tilts backward, the two wheels will move backward In

order to stop the robot, the two-wheel balancing robot just has to move in the opposite direction then the speed will decrease gradually If this principle is not maintained, the robot will simply fall over

Figure 2.2: The balancing status and moving forward mode

Figure 2.3: The robot goes uphill and downhill

When the system goes downhill, it tilts in the opposite direction to reduce speed and balance In the order side, the robot goes uphill, the tilt angle of the robot is greater than the angle of slope

2.2 SYSTEM DYNAMICS:

Build up the equation of state of robot describing the self-balancing robot:

Figure 2.4: The model of two wheel balancing robot

H D

Table 2.1: Table of symbol, coefficients and parameters

m [kg] Mass of the wheels

M [kg] Mass of the robot

R [m] Diameter of the wheel

D [m] Horizontal of robot

L [m] Distance between the center of robot

and the axis of the wheel

𝑓𝑤 The coefficient of friction between the

wheel and moving plane

𝑓𝑚 The coefficient of friction between

robot and DC motor

𝐽𝑚 [kg.m2] DC motor inertia torque

𝑅𝑚 [Ω] DC motor resistor

K b [V sec/rad] The EMF coefficient of DC motor

K t [Nm/A] Torsional Momentum of DC motor

g [m/s2] Acceleration of gravity

𝜃 [rad] The average angle of left and right

wheel

𝜃𝑙,𝑟 [rad] The angle of left and right wheel

𝜓 [rad] Chassis tilt angle

𝜙 [rad] Robot rotational angle

𝑥𝑙, 𝑦𝑙, 𝑧𝑙 [m] Left wheel coordinate

𝑥𝑟, 𝑦𝑟, 𝑧𝑟 [m] Right wheel coordinate

𝑥𝑚, 𝑦𝑚, 𝑧𝑚 [m] The average of coordinate

𝐹𝜃, 𝐹ψ, 𝐹𝜙 [Nm] Launching moment in different

directions

𝐹𝑙,𝑟 [Nm] Launching moment of left and right

motor

𝑖𝑙 ,𝑖𝑟 [A] Left and right DC motor current

𝑣𝑙 , 𝑣𝑟 [V] Left and right DC motor voltage

The approach to building up kinetic model is using Euler-Lagrange method Supposing at time t = 0, the two-wheel balancing robot moves in the positive x-axis,

we have the following equations:

[𝜃

𝜙] = [

1

2(𝜃𝑙+𝜃𝑟)𝑅

𝑇1 =1

2𝑚(𝑥̇𝑙2+ 𝑦̇𝑙2+ 𝑧̇𝑙2) +1

2𝑚(𝑥̇𝑟2+ 𝑦̇𝑟2+ 𝑧̇𝑟2) +1

2𝑚(𝑥̇𝑏2+ 𝑦̇𝑏2+ 𝑧̇𝑏2) (2.7) The equation of kinetic energy of rotational movement :

Lagrange equation :

𝐿 = 𝑇1+ 𝑇2− 𝑈 (2.11) 𝑑

[

𝐹𝜃

𝐹𝜓

𝐹𝜙] = [

𝐹𝑙+ 𝐹𝑟

𝐹𝜓𝑊 2𝑅(𝐹𝑙− 𝐹𝑟)

and 𝐹𝑙 = 𝑛𝐾𝑡𝑖𝑙+ 𝑓𝑚(𝜓̇ − 𝜃̇𝑙) − 𝑓𝑤𝜃̇𝑙 (2.19)

𝐹𝑟 = 𝑛𝐾𝑡𝑖𝑟 + 𝑓𝑚(𝜓̇ − 𝜃̇𝑟) − 𝑓𝑤𝜃̇𝑟 (2.20)

𝐹𝜓 = −𝑛𝐾𝑡𝑖𝑙− 𝑛𝐾𝑡𝑖𝑟 − 𝑓𝑚(𝜓̇ − 𝜃̇𝑙) − 𝑓𝑚(𝜓̇ − 𝜃̇𝑟) (2.21) Using PWM method to control DC motors so need to convert current to voltage:

𝐿𝑚𝑖̇̇𝑙,𝑟 = 𝑣𝑙,𝑟 + 𝐾𝑏(𝜓̇ − 𝜃̇𝑙,𝑟) − 𝑅𝑚𝑖𝑙,𝑟 (2.22) See the relatively small (approx 0) armature inductance, which can be ignored, infer:

𝑖𝑙,𝑟 =𝑣𝑙,𝑟+𝐾𝑏(𝜓̇−𝜃̇𝑙,𝑟)

From that, the momentum is generated:

𝐹𝜃 = 𝛼(𝑣𝑙+ 𝑣𝑟) − 2(𝛽 + 𝑓𝑤)𝜃̇ + 2𝛽𝜓̇ (2.24)

𝐹𝜓 = −𝛼(𝑣𝑙 + 𝑣𝑟) + 2𝛽𝜃̇ − 2𝛽𝜓̇ (2.25) Với 𝛼 = 𝑛𝐾𝑡

𝑅 𝑚 và 𝛽 =𝑛𝐾𝑡𝐾𝑏

𝑅 𝑚 + 𝑓𝑚 (2.26)

𝐹𝜙 = 𝑊2𝑅𝛼(𝑣𝑟− 𝑣𝑙) − 𝑊2

2𝑅 2(𝛽 + 𝑓𝑤)𝜙̇ (2.27) Obtain the dynamic equation describing the movement of robot as follows:

[(2𝑚 + 𝑀)𝑅2+ 2𝐽𝑤+ 2𝑛2𝐽𝑚]𝜃̈ + (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2𝐽𝑚)𝜓̈ − 𝑀𝐿𝑅𝜓̇2𝑠𝑖𝑛𝜓 =

𝛼(𝑣𝑙+ 𝑣𝑟) − 2(𝛽 + 𝑓𝑤)𝜃̇ + 2𝛽𝜓̇ (2.28) (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2𝐽𝑚)𝜃̈ + (𝑀𝐿2+ 𝐽𝜓+ 2𝑛2𝐽𝑚)𝜓̈ − 𝑀𝑔𝐿𝑠𝑖𝑛𝜓 −

𝑀𝐿2𝜙̇2𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 = −𝛼(𝑣𝑙+ 𝑣𝑟) + 2𝛽𝜃̇ − 2𝛽𝜓̇ (2.29)

[1

2𝑚𝑊2+ 𝐽𝜙+ 𝑊2

2𝑅2(𝐽𝑤 + 𝑛2𝐽𝑚) + 𝑀𝐿2𝑠𝑖𝑛2𝜓] 𝜙̈2+ 2𝑀𝐿2𝜓̇𝜙̇ 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 =𝑊

of this type of control system is shown in Fig 2.5

Figure 2.5: The LQR control structure

r NON-LINEAR TWO WHEEL

BALANCING ROBOT

K

A control system designed in optimal working conditions is always in optimum conditions according to a certain quality standard (performance index) The optimal state is achieved depending on the quality requirements set, on the understanding and effects of objects and on the operating conditions of the control system

The setting of a controller control either a machine or process are found by using

a mathematical algorithm that minimize a cost function with weighting factors supplied by human The cost function is usually defined as a sum of the deviations of the main elements include process temperature or desired altitude, from their desired value

J



Designing diagram:

Figure 2.6: LQR control diagram

The technique involves choosing a control law u(t) = -Kx(t) which stabilizes the origin while minimizing the performance index Where K is the gain matrix defined as:

1 T

And P is solved from the Riccati equation:

1

0

Where Q is a positive determinant matrix (or semi-positive determinant matrix),

R is the positive determinant matrix Q and R are weighting matrices corresponding

to state variables and input signals To make a component to be optimally prioritized for control, choose the corresponding weighting matrix which is of great value

x x x x x x x

0 0 0 0 0 0

0 0

T T

x u

v

 

  

0 0

0 0

0 0

0 0

0 0

The weighting matrices Q, R can be represented at this time

1 2 3 4 5 6

Q Q Q

R R

R

Parameters Q1, Q2, Q3, Q4, Q5, Q6, R1 and R2 to refine the LQR controller

In which parameter Q1, Q2, Q3, Q4, Q5, Q6 is considered respectively optimal weights for  ,  ,  ,  ,  ,  To prioritize any state variable, increase its corresponding weight parameter in relative with other weight parameters

After selecting the corresponding control parameters, we construct the feedback parameter K with the control signal u = - K * x

K parameter is calculated based on Riccati equation which is supported by Matlab with lqr() function when matrices A, B, Q, R are figured out

Figure 2.7: LQR controller diagram for Two-wheel self-balancing robot

2.4 KALMAN FILTER

For the purpose of computational practicality, a discrete-time Kalman filter is used The implementation will be kept simple since all processing would be done on

an Arduino microprocessor with element-wise computational capabilities

The Kalman filter is an algorithm that uses a series of measurements over time [11], in which case the main measurements are the values obtained from the accelerometer and the gyro sensor The Kalman filter equation is divided into two groups: the equation for updating the time and the equation for updating the measured value The time update predictive (time-based) equation uses the present value and covariance estimates to predict the prior estimate for the next moment The update of the measured value that is responsible for feedback means combining the new value with the prior estimate to correct the posterior estimate After each step updating the time and updating the measurement, the process is repeated with the previous post-estimation to predict a new prior estimate

Figure 2.8: Complete process of Kalman filter

Supposing we have a simple state having only position and velocity:

p x

v

 

  

What actual position and velocity are, there are range of possible combinations

of position and velocity that might be true, but some of them are more likely than others The Kalman filter assumes both variables (position and velocity) are random and Gaussian distributed Each variable has a mean value μ, which is the center of the random distribution (and its most likely state), and a variance σ2, which is the uncertainty We are estimating a new position based on an old one The higher the velocity, the more it’s likely that the position will be more distant One measurement tells us something about the others could be This correlation is captured by a covariance matrix

Call the estimate at time k: xˆk, the known external influences:  and its covariance matrix Pk To represent the prediction step, use a matrix F But every

state in original estimate could move to a range of states Each point in xˆk 1

 is moved to somewhere inside a Gaussian blob with covariance Qk This produce a new Gaussian blob with a difference covariance but the same mean Expand the covariance by simply adding Qk , giving our complete expression for the prediction step:

This is a linear model: states written by vectors, while variables are written in matrices

ˆ

k T

k

z

So now we have two Gaussian blobs: One surrounding the mean of transformed prediction and one surrounding the sensor reading To find out the most likely state, the two Gaussian blobs are combined together

Consider as 1D Gaussian bell curve with variance σ2 and mean μ [5] is defined as:

2 2

( ) 2

1 ( , , )

Figure 2.9: Combination of two Gaussian curves

N(x,μ0,σ0)⋅N(x,μ1,σ1)=?N(x,μ′,σ′) (2.53) Substitute equation (2.52) into equation (2.53) to obtain: Combination of two Gaussian curves

' T T T k

HP H HPH KHPH (2.62)

From (2.58), Kalman gain is: K  HPHT( HPHT  R )1 (2.63)

Knock an H off the the front of every term in (2.61) and (2.62) (one is hiding inside K), and an HT off the end of all terms in the equation for P [11] '

 Estimate priori state

 Based on the measurement results to correct the estimate

 Repeat

 Implement Kalman filter on C language:

For implementing on an IMU sensor, here the system state at time k: