8. Fuzzy Control Fuzzy Logic

5 downloads 0 Views 3MB Size Report
Comparative Analysis between Fuzzy Logic Control,. LQR Control with Kalman Filter and PID Control for a Two Wheeled Inverted Pendulum. Karen V. Chate.

Latin American Congress on Automation and Robotics 2017

Comparative Analysis between Fuzzy Logic Control, LQR Control with Kalman Filter and PID Control for a Two Wheeled Inverted Pendulum Karen V. Chate Oscar E. Prado Carlos F. Rengifo

Schedule 1.

Introduction

2.

Motivation

3.

Mobile pendular robot InstaBot SRAT-2

4.

Mathematical modelling of a mobile pendular robot

5.

Performance Indexes

2

Schedule 6.

PID Control

7.

LQR Control

8.

Fuzzy Control

9.

Comparison of the implemented controllers

10.

Conclusions

3

1. Introduction

4

1. Introduction

This kind of mobile platforms have been used in academic and research environments, mainly in system modelling and control areas.

5

1. Introduction The main control objective is to keep the balance in the mobile pendular robot. Main application field : • Personal transport vehicles

Other application fields: • Agriculture • Medicine • Exploration

6

2. Motivation

7

2. Motivation • There is a lot of controversy between control based in mathematical models and control based in knowledge rules.

• There are no publications where a quantitative comparison based on performance indexes is carried out. • In most publications, the controllers comparison is carried out based on a visual inspection of the time response curves.

8

Research Question

• For the case of a pendular mobile robot, what advantages and disadvantages does the control based on fuzzy logic present with respect to linear techniques such as the PID and the LQR controller with Kalman filter?

9

3. Mobile Pendular Robot InstaBot SRAT-2

10

3. Mobile robot pendular InstaBot SRAT-2

11

1. Gyroscope + Accelerometer Sensor : MPU6050 Inertial Measurement Unit (speed, rotation and gravitational forces) with 6 DOF. • Input Voltage: 3 – 5 V • Accelerometer range: 250/500/1000/2000 °/s • Gyroscope range: 2/4/8/16 g

2.

Motor JGA25-371-21: • • • • •

Weight: 101g Encoder Resolution: 12 pulses per revolution Input Voltage: 6 – 12 V Output power: 1.25 W

12

3.

SainSmart Protoshield Shield V3.0: • • • • •

Input Voltage: 10 – 14 V 3 potentiometers 12 slots for servos conection L298P and LM1084 I2C Communication ports for MPU

4. Bluetooth Module HC-05: • Input voltage: 3.3 V • UART RS 232 serial protocol • Frequency: 2.4 GHz ISM band

13

InstaBot SRAT-2 Parameters

Most of these values were obtained through measures from laboratory instruments (weight scale and vernier scale). (3.1)

(3.2)

14

4. Mathematical Modelling of a Mobile Pendular Robot

15

4. Mathematical Modelling of a Mobile Pendular Robot The mathematical approach of the mobile pendular robot is realized by considering the existing similarities between this and the inverted pendulum nonlinear system.

r: Wheel radius 𝛳: Pendulum tilt angle. l: Pendulum length (Robot body)

16

It is defined the coordinates vector of the mass center of the wheel and robot body with respect to the coordinates axis system < X0, Y0 >.

Then it is calculated the system lagrangian by considering the kinetic and potential energies The lagrangian is defined as: Where:

𝑇 (𝑞, 𝑞) ሶ : Kinetic energy of the system U (𝑞, 𝑞) ሶ : Potential energy of the system

(4.1)

𝑞 , 𝑞ሶ :Generalized coordinates and speeds

17



The kinetic energy of a system is defined as: (4.2) For the mobile pendular robot the kinetic energy is:

(4.3)

𝐽1 𝐽2 𝑀 𝑚

: Inertia momentum of the robot wheels. : Inertia momentum of the robot body. : Mass of the robot wheels : Mass of the robot body

18

• The potential energy of a system is defined as: (4.4) For the mobile pendular robot is: (4.5) •

The lagrangian is: (4.6) And it is defined:

19

• Lagrange Formalism: (4.7) 𝑅(𝑞, 𝑞) ሶ contains the energy dissipation terms and 𝑒(𝑡) is the generalized efforts vector applied to the system. For the mobile pendular robot the lagrange equation is defined as:

(4.8)

Being 𝛤 the applied torque to the Wheel motors of the robot

20

• The obtained model , according to the lagrangian form is :

(4.9) • In matricial form, we obtain:

(4.10)

Being:

𝐼 (𝑞) ∶ Inertia Matrix. 𝐻 (𝑞, 𝑞) ሶ ∶ Vector of centrifugar effects and Coriolis. 𝐺(𝑞)

∶ Vector of gravitational forces.

21

Linearization of the mathematical model General representation of a linear state space model: (4.11) Where

Being:

𝑥 ∶ States vector 𝑥 ∈ 𝑅 𝑛 𝐴 ∶ Constant matrix 𝐴 ∈ 𝑅 𝑛𝑥𝑛 𝐵 ∶ Constant matrix 𝐵 ∈ 𝑅 𝑛𝑥𝑝 𝐶 ∶ Constant matrix 𝐶 ∈ 𝑅 𝑞𝑥𝑛 𝐷 ∶ Constant matrix 𝐷 ∈ 𝑅 𝑞𝑥𝑝 𝑢 ∶ Input vector 𝑢 ∈ 𝑅 𝑝 𝑦 ∶ Output vector 𝑦 ∈ 𝑅 𝑛

22

The mechanical model is then rewritten in terms of the variables of which the model depends.

(4.12)

The states vector is:

𝑥1 = 𝑥

𝑥2 = 𝜃 − 𝑥3 = 𝑥ሶ

𝜋 2

The defined equilibrium point is:

𝑥1 𝑥2 𝑥3 𝑥4 Γ

= = = = =

0 0 0 0 0

𝑥4 = 𝜃ሶ 23

The state space matrices which define the dynamical behavior of the robot are:

24

5. Performance Indexes

25

5. Performance Indexes The proposed performance indexes are shown below: •

Standard Deviation of error: 𝑁

𝜀𝑒 =

1 ෍ 𝜃𝑑 𝑘 − 𝜃 𝑘 𝑁 𝑘=1

2

(5.1)

Where: 𝜃𝑑 : Desired angle (0°). 𝜃 ∶ Output angle. 𝑁: Number of samples taken.

26

5. Performance Indexes • Standard deviation of control effort: 𝑁

𝜀𝑢 =

1 ෍ 𝑢2 (𝑘) 𝑁

(5.2)

𝑘=1

Where: 𝑢: Control effort 𝑁: Number of samples taken In order to calculate the proposed performance indexes, 11 experiments were carried out with a 0º reference, for which were obtained 3000 data per experiment.

27

Critical Control Angle •

Critical control angle: In order to determine the critical control angle it was carried out an experiment which consisted in initialize the robot with an initial inclination, assessing whether if the platform could keep the balance or not, the initial angle was gradually increased until the robot lost its balance.

28

6. PID Control

29

6. PID Control A PID operates based in the error signal, which is calculated through the difference between the set point and the measure of the controlled variable. The control law associated to a PID controller is defined in the following equation: 𝑡

𝑑𝑒 𝑡 𝑢 𝑡 = 𝐾𝑝 𝑒 𝑡 + 𝐾𝑖 න 𝑒 𝜏 𝑑𝜏 + 𝐾𝑑 𝑑𝑡 0

(6.1)

Being: 𝑒 𝑡 : 𝐾𝑝 : 𝐾𝑖 : 𝐾𝑑 :

Current value of error signal. Proportional constant. Integral constant. Derivative constant.

30

Variables Identification Accordingly, the following block diagram is defined :

31

Implementation A PID controller tuning was carried out through the variation of the parameters 𝐾𝑝 , 𝐾𝑖 , and 𝐾𝑑 . The values which achieved the robot stabilization were: 𝐾𝑝 = 47,5 ,

𝐾𝑖 = 0,05,

𝐾𝑑 = 0,2

By using the designed Matlab bluetooth interface it was possible to obtain the response curve of the inclination angle of the robot with the PID controller as a function of time. In order to tune the controller, a bluetooth application was used to allow the controller parameters to be modified remotely.

32

Experimental Results Below it is shown the time response of the PID controller with its respective control effort and angular velocity.

33

Performance Indexes The obtained performance indexes and its respective histograms are shown below

34

Critical Control Angle The critical control angle for the PID controller was found in the range [-4, 4]º

35

PID Controller Videos

36

PID Controller Videos

37

7. LQR Control with Kalman filter

38

7. LQR Control LQR control design is performed based in a linear state space model of the plant or process to control. The control law is defined by the following equation:

Being:

𝑢 𝑡 = −𝐾𝑥

(7.1)

𝐾: State feedback matrix 𝑥: System states vector. Then it is obtained the state space model described as follows:

𝑥ሶ = 𝐴 − 𝐵𝐾 𝑥

(7.2)

39

To calculate the gain matrix 𝐾 it is used the quadratic cost function that represents the total energy of the closed loop system:



𝐽=

1 න 𝑥 𝑇 𝑡 𝑊𝑥 𝑡 + 𝑢𝑇 𝑡 𝑅 𝑢(𝑡) 2

(7.3)

0

Where:

𝑊: Matrix of state penalty (𝑛𝑥𝑛), semidefinitive positive. 𝑅: Matrix of input penalties (𝑚𝑥𝑚), positive definite.

40

The penalty matrices with which the balance of the robot was achieved are shown below: 1 0 𝑊= 0 0

0 190 0 0

0 0 0 0 , 1 0 0 0,7

𝑅 = 0,1

Being the values of the states feedback matrix:

𝐾 = 3,1623 44,1759 6,2140 0,3223 The LQR control law for the mobile pendular robot is: 𝑈𝐿𝑄𝑅 = −𝐾2 𝑥2 − 𝐾4 𝑥4

41

7.1 Kalman Filter It is a recursive algorithm that uses a set of differential equations and input data to estimate the real states of a dynamic system subject to observation by minimizing the estimated variance of the error using least squares.

42

The mathematical process that the Kalman filter performs to obtain the estimate of the new state is based on a prediction and correction mechanism, where through gain compensation between the previous estimate and the current observation, it is possible to achieve convergence to the states of the system, which are unknown. (7.4) (7.5)

Update of the variables over time

(7.6) (7.7) (7.8)

Update of measurement and estimation values.

43

7.2. Sensor Fusion The following mathematical model of the InstaBot SRAT-2 sensor is defined: 𝑥ሶ = 𝑢 𝑦ො = 𝑥

(7.9)

ZOH

Where: 𝑥:ሶ 𝑢: 𝑦: ො 𝑥:

Angular velocity. Gyroscope readings. Estimated angular position. Angular position.

𝑥 𝑘ℎ + ℎ = 𝑥 𝑘ℎ + ℎ𝑢 𝑘ℎ 𝑦 𝑘ℎ = 𝑥(𝑘ℎ)

(7.10)

Where: 𝑘: Time index ℎ: Sample time

44

7.2. Sensor Fusion According to the above, the matrices 𝐴 = 1, 𝐵 = ℎ and 𝐶 = 1 are replaced in the Kalman filter equations to obtain the sensor fusion system described below:

Prediction

(7.11) (7.12) (7.13)

Update

(7.14) (7.15)

45

Experimental Results The controller logic was implemented in C language in the Arduino environment and the Matlab software was used to acquire and display the data

46

Performance Indexes The Kalman filter was implemented using the KalmanFilter library of TKJElectronics. The results of the 11 experiments with the LQR controller are shown below:

47

Critical Control Angle The critical control angle of the LQR controller with Kalman filter was found around [-24.4, 24.4] º

48

LQR Controller Videos

49

LQR Controller Videos

50

8. Fuzzy Control

51

8. Fuzzy Control More realistic approach to human thinking Fuzzy Logic

Inaccurate modes of reasoning

Degrees of membership in a range of values [0,1] The block diagram of a fuzzy controller is:

52

Fuzzy PD Control on the InstaBot SRAT-2 MISO controller with 2 inputs(E, EC) and 1 output (U).

Fuzzy Set definition

Input Variables: 3 fuzzy sets: • 𝑥1 ∶ Error • 𝑇(𝑋1 ) = {N, Z, P} • 𝑋1 = [-60, 60] °

Output Variable: 5 fuzzy sets. • 𝑦1 ∶ Control Law • 𝑇(𝑌1 ) = {LN,SN, NCL, SP, LP} • 𝑌1 = [-30, 30] %

• 𝑥2 ∶ Error_Change • 𝑇(𝑋2 ) = {NC, ZC, PC} • 𝑋2 = [-250, 250] °/s

53

54

EC

E

PC

ZC

NC

P

LP

SP

NCL

Z

SP

NCL

SN

N

NCL

SN

LN

(8.1)

Rules of fuzzy PD controller

55

56

Experimental Results Time response of the fuzzy PD controller with its respective control effort and angular velocity.

57

Performance Indexes The performance indexes found and their respective histograms

58

Critical control Angle The critical control angle of the fuzzy PD controller was found around [-28, 25] º

59

Fuzzy Controller Videos

60

Fuzzy Controller Videos

61

9. Performance Comparison of the implemented controllers

62

9. Performance Comparison of the implemented controllers Summary of performance indices obtained with each controller

(9.1)

Region of convergence of each controller implemented (9.2)

63

The ratios between linear and nonlinear controllers are presented below. εe 𝑃𝐼𝐷 εu 𝑃𝐼𝐷 ൗεe ൗεu 𝐹𝑢𝑧𝑧𝑦 𝐹𝑢𝑧𝑧𝑦

2,5361

3,0271

εe 𝐿𝑄𝑅 εu 𝐿𝑄𝑅 ൘εe ൘εu 𝐹𝑢𝑧𝑧𝑦 𝐹𝑢𝑧𝑧𝑦

1,5033

(9.3)

2,1224

64

10. Conclusions

65

10. Conclusions • The dynamic state space model of a mobile pendular robot was found taking into account the physical model (Lagrange formalism). Two linear controllers (PID, LQR) and one nonlinear (Fuzzy) controller were implemented. The design of the LQR controller was performed with the obtained linear model and a Kalman filter was implemented to improve the estimation of the orientation angle of the InstaBot SRAT-2. • Most of the parameters that were used in the pendular mobile robot model were found by measurements.

66

• The tuning of the PID controller was the most expensive to perform since 3 variables 𝐾𝑝 , 𝐾𝑖 𝑦 𝐾𝑑 , had to be configured, whereas in the LQR controller the tuning only corresponded to the calculation of 𝐾 and finally with the fuzzy controller it was only necessary to establish properly the rules and the ranges of the fuzzy sets used. •

According to the obtained results, it was concluded that the fuzzy PD controller is the best controller for the mobile platform Instabot SRAT-2 due to a smaller standard deviation of the error, a lower energy consumption and a greater region of convergence in comparison to the linear controllers PID and LQR.



The design of the PID controller and the Fuzzy was easier because the mathematical model of the robot was not needed, unlike the design of the LQR controller.

67

• The definition of the performance indexes and the convergence region of each controller allowed to have objective selection criteria to establish the best controller for the platform. The time response curves of the controllers were obtained and the corresponding performance indices were calculated for 11 experiments.

68

Thank You 69