hệ xe 2 bánh tự cân bằng là hệ phi tuyến dễ dàng bị mất ổn định khi không có tín hiệu điều khiển tích cực tác động vào. Các bộ điều khiển như PID, Fuzzy hoạt động rất tốn năng lượng để giữ được hệ thống ổn định tĩnh. Vì vậy việc thiết kế 1 bộ điều khiển tối ưu là rất cần thiết để tối ưu hóa năng lượng trong quá trình điều khiển
Trang 11
Hanoi University of Science and Technology
School of Electrical Engineering
**********
PROJECT II
TOPIC: MODELLING AND DESIGN OPTIMIZATION CONTROLLER FOR 2-WHEELS SELF BALANCING ROBOT
Doctor: Nguyen Thanh Huong Student: Nguyen Tuan Hung 20176941
Class: CTTT-HTĐ.TDH-K62
Hanoi, 1/2021
Trang 2CONTENTS
I Introduction……… …… 4
1.1 Role of the 2 wheeled balance robot……… … …… 4
1.2 Aim of project……… … 4
II Mathematics model ……… …….4
2.1 Reference system and object parameters………………… …… ……4
2.2 Dynamics model……… ……… 6
III Controller design……….………8
3.1 LQR controller … ……….……… 8
3.2 Simulation……… ……….10
3.2.1 Linearization……….……….10
3.2.2 Define the K control matrix.……… 10
IV Results.………11
4.1 The reference value is set to 0……… 11
4.2 The reference value is set 𝜃 = 𝜋 2 and 𝜙 = 𝜋 2……….13
4.3 When the system has disturbance……….15
4.4 Discussion……….18
V Conclusion……….18
REFERENCE……….18
Trang 3Table of Figure: Page
Figure 1: 3D model of the 2 wheeled balance robot 4
Figure 2: Mathematical model of object in MATLAB/Simulink 10
Figure 3 : Closed loop model of LQR controller in MATLAB/Simulink 11
Figure 4 : The medium angle of the left and right wheels when the reference value is set to 0 12
Figure 5 : The inclined angle of robot’s body when the reference value is set to 0 12
Figure 6 : The robot's rotation angle in the xy plane when the reference value is set to 0 13
Figure 7 : The medium angle of the left and right wheels when the reference value is set 𝜃 =𝜋 2 and 𝜙 =𝜋 2 14
Figure 8 : The inclined angle of robot’s body when the reference value is set 𝜃 =𝜋 2 and 𝜙 =𝜋 2 14
Figure 9 : The robot's rotation angle in the xy plane when the reference value is set 𝜃 =𝜋 2 and 𝜙 = 𝜋 2 15
Figure 10 : The medium angle of the left and right wheels when the system has disturbance 15
Figure 11 : The inclined angle of robot’s body when the system has disturbance 16
Figure 12 : The robot's rotation angle in the xy plane when the system has disturbance 16
Figure 13 : The medium angle of the left and right wheels when change Q matrix 17
Figure 14 : The inclined angle of robot’s body when change Q matrix 17
Figure 15 : The robot's rotation angle in the xy plane when change Q matrix 18
Trang 4I Introduction
1.1.Role of the 2 wheeled balance robot
Nowadays, self balancing robots are widely used in many fields of the life Self balancing robots are self-moving and balancing in 2-wheels vehicles and versatile with many indoor and outdoor application.However, the control of 2-wheels balancing robot is not easy because the 2WBR is an underactuated system This unstable causes the PID and Fuzzy controllers to use a lot of energy to keep the system stable An optimized control algorithm is designed (LQR) to optimize control energy so that the energy loss while keeping the system stable is minimal
The following parts are presented as follows: Section II describes the object modeling method Next, section III will present the LQR control algorithm, section VI will present the simulation results Finally, section V concludes on the optimal control method for 2-wheelers self-balancing robot
1.2.Aim of project
This project aims to model and simulate to find the optimal control method as well as parameters for the controller to save energy for the robot during the control process
II Mathematics model
2.1.Reference system and object parameters
Diagram and reference system corresponding to the robot's coordinate axis
Figure 1 3D model of the 2 wheeled balance robot
TABLE 1: SPECIFICATION OF THE ROBOT
Trang 5PARAMETER DESCRIPTION VALUE UNITS
M Mass of the robot 2 kg
m Mass of the wheel 0.05 kg
R Radius of the wheel 0.05 m
W The width of robot 0.15 m
D The thickness of robot 0.03 m
H The height of robot 0.3 m
L The distance from the robot's center of
mass to the wheel shaft
0.15 m
𝑓𝑤 Coefficient of friction of wheel with
moving surface
0.18
𝑓𝑚 Coefficient of friction of robot with wheel
shaft
0.002
𝐽𝑚 Moment of inertia of DC motor 0.1 𝑘𝑔𝑚2
𝑅𝑚 Resistor of DC motor 50 Ω
𝐾𝑏 Coefficient of EMF of DC motor 0.468 Vs/rad
𝐾𝑡 Torque of DC motor 0.317 Nm/A
N Deceleration ratio 40
g Gravitational acceleration 9.81 𝑚/𝑠2
𝜃 The medium angle of the left and right
wheels
rad
𝜃 𝑙,𝑟 The angle of the left and right wheels rad
𝜓 The inclined angle of robot’s body rad
𝜙 The robot's rotation angle in the xy plane rad
𝑥𝑙, 𝑦𝑙, 𝑧𝑙 Coordinate of left wheel m
𝑥𝑟, 𝑦𝑟, 𝑧𝑟 Coordinate of left wheel m
𝑥𝑚, 𝑦𝑚, 𝑧𝑚 Medium coordinate m
𝐹𝜃, 𝐹𝜓, 𝐹𝜙 Torque of actuation according to each
coordinate system
Nm
𝐹𝑙,𝑟 Torque of actuation according to each left
and right wheel
Nm
𝑖𝑙, 𝑖𝑟 Current of left and right motor A
Trang 6𝑣𝑙, 𝑣𝑟 Voltage of left and right motor V
𝐽𝜓 Moment of inertial of inclined angle of
robot’s body 𝑘𝑔𝑚
2
𝐽𝜙 Moment of inertial of the robot's rotation
angle in the xy plane
𝑘𝑔𝑚2
2.2.Dynamics model
The medium angle of the left and right wheels:
𝜃 = 1/2 × (𝜃𝑙+ 𝜃𝑟) (1) The robot's rotation angle in the xy plane:
∅ = 𝑅
𝑊× (𝜃𝑙− 𝜃𝑟) (2) Medium coordinate in inertial reference system:
[
𝑥𝑚
𝑦𝑚
𝑧𝑚] = [
∫ 𝑥̇𝑚
∫ 𝑦̇𝑚
𝑅 ] = [
∫ 𝑅𝜃̇ cos 𝜙
∫ 𝑅𝜃̇ sin 𝜙 𝑅 ] (3)
Coordinate of left wheel in inertial reference system:
[
𝑥𝑙
𝑦𝑙
𝑧𝑙 ] = [
𝑥𝑚−𝑊
2 sin 𝜙
𝑦𝑚+𝑊
2 cos 𝜙
𝑧𝑚
] (4)
Coordinate of right wheel in inertial reference system:
[
𝑥𝑟
𝑦𝑟
𝑧𝑟] = [
𝑥𝑚+𝑊
2 sin 𝜙
𝑦𝑚−𝑊
2 cos 𝜙
𝑧𝑚
] (5)
Symmetrical center coordinates between two motors in inertial reference system:
[
𝑥𝑏
𝑦𝑏
𝑧𝑏] = [
𝑥𝑚+ 𝐿 sin 𝜓 cos 𝜙
𝑦𝑚+ 𝐿 sin 𝜓 cos 𝜙
𝑧𝑚+ 𝐿 cos 𝜓
] (6)
Torque of actuation according to each coordinate system
[
𝐹𝜃
𝐹𝜓
𝐹𝜙 ] = [
𝐹𝑙+ 𝐹𝑟
−𝑛𝐾𝑡𝑖𝑙− 𝑛𝐾𝑡𝑖𝑟− 𝑓𝑚(𝜓̇ − 𝜃̇𝑙) − 𝑓𝑚(𝜓̇ − 𝜃̇𝑟)
𝑊
𝑅 (𝐹𝑙− 𝐹𝑟)
] (7)
Torque of actuation according to each left and right wheel
Trang 7𝐹𝑙 = 𝑛𝐾𝑡𝑖𝑙+ 𝑓𝑚(𝜓̇ − 𝜃̇𝑙) − 𝑓𝑤𝜃̇𝑙 (8)
𝐹𝑟 = 𝑛𝐾𝑡𝑖𝑟+ 𝑓𝑚(𝜓̇ − 𝜃̇𝑟) − 𝑓𝑤𝜃̇𝑟 (9) Using PWM pulse to control the motor torque, so a formula of current and voltage relation is derived:
𝐿𝑚𝑖𝑙,𝑟 = 𝑣𝑙,𝑟+ 𝐾𝑏(𝜓̇ − 𝜃̇𝑙,𝑟) − 𝑅𝑚𝑖𝑙,𝑟 (10) Ignore inductance of armature of DC motor:
𝑖𝑙,𝑟 =𝑣𝑙,𝑟 +𝐾𝑏(𝜓̇−𝜃̇𝑙,𝑟)
𝑅𝑚 (11) Torque of actuation according to each coordinate system:
[
𝐹𝜃
𝐹𝜓
𝐹𝜙 ] = [
𝛼(𝑣𝑙+ 𝑣𝑟) − 2(𝛽 + 𝑓𝑤)𝜃̇ + 2𝛽𝜓̇
−𝛼(𝑣𝑙+ 𝑣𝑟) + 2𝛽𝜃̇ − 2𝛽𝜓̇
𝑊 2𝑅𝛼(𝑣𝑙− 𝑣𝑟) −𝑊2
2𝑅 2(𝛽 + 𝑓𝑤)𝜙̇
] (12)
With 𝛼 =𝑛𝐾𝑡
𝑅𝑚 and 𝛽 =𝑛𝐾𝑡 𝐾 𝑏
𝑅𝑚 + 𝑓𝑚 Kinetic equation of translational motion:
𝐾1 =1
2𝑚(𝑥̇𝑙2+ 𝑦̇𝑙2+ 𝑧̇𝑙2) +1
2𝑚(𝑥̇𝑟2 + 𝑦̇𝑟2 + 𝑧̇𝑟2) +1
2𝑀(𝑥̇𝑏2+ 𝑦̇𝑏2+ 𝑧̇𝑏2) (13) Kinetic equation of rotation:
𝐾2 = 1
2𝐽𝑤𝜃̇𝑙2+1
2𝐽𝑤𝜃̇𝑟2+1
2𝐽𝑤𝜓̇2 +1
2𝐽𝜓𝜓̇2+1
2𝐽𝜙𝜙̇2+1
2𝑛2𝐽𝑚(𝜃̇𝑙− 𝜓̇)2 +1
2𝑛2𝐽𝑚(𝜃̇𝑟− 𝜓̇)2 (14) Potential energy equation:
U = 𝑚𝑔𝑧𝑙+ 𝑚𝑔𝑧𝑟+ 𝑀𝑔𝑧𝑏 (15) Lagrange equation:
L = 𝐾1+ 𝐾2− 𝑈 (16)
𝑑
𝑑𝑡(𝜕𝐿
𝜕𝜃̇) −𝜕𝐿
𝜕𝜃= 𝐹𝜃 (17)
𝑑
𝑑𝑡(𝜕𝐿
𝜕𝜓̇) −𝜕𝐿
𝜕𝜓= 𝐹𝜓 (18)
𝑑
𝑑𝑡(𝜕𝐿
𝜕𝜙̇) − 𝜕𝐿
𝜕𝜙= 𝐹𝜙 (19) Derivative L according to the variables Obtain dynamic equations describing robot motion: [(2𝑚 + 𝑀)𝑅2+ 2𝐽𝑤 + 2𝑛2𝐽𝑚 ]𝜃̈ + (𝑀𝐿𝑅 cos Ψ − 2𝑛2𝐽𝑚)Ψ̈ − 𝑀𝐿𝑅Ψ̇2sin Ψ =
𝛼(𝑣𝑙 + 𝑣𝑟) − 2(𝛽 + 𝑓𝑤)𝜃̇ + 2𝛽Ψ̇ (20)
Trang 8(𝑀𝐿𝑅 cos Ψ − 2𝑛2𝐽𝑚)𝜃̈ + (𝑀𝐿2+ 𝐽Ψ+ 2𝑛2𝐽𝑚)Ψ̈ − 𝑀𝑔𝐿 sin Ψ − 𝑀𝐿2Φ2sin Ψ cos Ψ =
−𝛼(𝑣𝑙 + 𝑣𝑟) + 2𝛽𝜃̇ − 2𝛽Ψ̇ (21) [1
2𝑚𝑊2+ 𝐽Φ+ 𝑊2
2𝑅 2(𝐽𝑤 + 𝑛2𝐽𝑚) + 𝑀𝐿2sin Ψ2] Φ̈2+ 2𝑀𝐿2Ψ̇Φ̇ sin Ψ cos Ψ =
𝑊
2𝑅𝛼(𝑣𝑟 − 𝑣𝑙) − 𝑊2
2𝑅 2(𝛽 + 𝑓𝑤)Φ̇ (22)
III Control Design
3.1 LQR controller
The Linear Quadratic Regulator (LQR) is a well-known method that provides
optimally controlled feedback gains to enable the closed-loop stable and high performance design of systems Plant must be linearized about the origin to obtain the optimal control,
linear time system to form a continuous time system
From dynamic equations, we have equations describing robot motion:
{
𝑥1̇=𝑥2 𝑥2̇=𝑓1(𝑥1,𝑥2,𝑥3,𝑥4,𝑥5,𝑥6,𝑣𝑟,𝑣𝑙)
𝑥4̇=𝑥5 𝑥5̇=𝑓2(𝑥1,𝑥2,𝑥3,𝑥4,𝑥5,𝑥6,𝑣𝑟,𝑣𝑙)
𝑥7̇=𝑥8 𝑥8̇=𝑓3(𝑥1,𝑥2,𝑥3,𝑥4,𝑥5,𝑥6,𝑣𝑟,𝑣𝑙)
(23)
With:
𝑥1 = 𝜃, 𝑥2 = 𝜃̇, 𝑥3 = 𝜃̈, 𝑥4 = Ψ, x5 = Ψ̇, 𝑥6 = Ψ̈, 𝑥7 = Φ, x8 = Φ̇, 𝑥9 = Φ̈ (24) Plant must be linearized about the origin to obtain the optimal control, choose balance control point:
𝑥0 = [0 0 0 0 0 0 ]𝑇 and 𝑢0 = [0 0]𝑇 (25) Linearization of the system, the state space model:
𝑥̇ = 𝐴𝑥 + 𝐵𝑢 (26)
A , B is the matrix of states
𝐴 =
[
0
𝜕𝑓2
𝜕𝑥 1
1
𝜕𝑓2
𝜕𝑥 2
0
𝜕𝑓2
𝜕𝑥 4
0
𝜕𝑓2
𝜕𝑥 5
0
𝜕𝑓2
𝜕𝑥 7
0
𝜕𝑓2
𝜕𝑥 8
0 0 0 1 0 0
𝜕𝑓 4
𝜕𝑥1
0
𝜕𝑓 6
𝜕𝑥1
𝜕𝑓 4
𝜕𝑥2
0
𝜕𝑓 6
𝜕𝑥2
𝜕𝑓 4
𝜕𝑥4
0
𝜕𝑓 6
𝜕𝑥4
𝜕𝑓 4
𝜕𝑥 5
0
𝜕𝑓 6
𝜕𝑥5
𝜕𝑓 4
𝜕𝑥7
0
𝜕𝑓 6
𝜕𝑥7
𝜕𝑓 4
𝜕𝑥8
1
𝜕𝑓 6
𝜕𝑥8] (27)
Trang 9𝐵 =
[
0
𝜕𝑓 2
𝜕𝑣𝑙
0
0
𝜕𝑓 2
𝜕𝑣 𝑟
0
𝜕𝑓 4
𝜕𝑣 𝑙
0
𝜕𝑓4
𝜕𝑣 𝑟
0
𝜕𝑓6
𝜕𝑣 𝑙
𝜕𝑓6
𝜕𝑥 1] (28)
With 𝑥 = 𝑥0 , 𝑢 = 𝑢0
‘x’ is the all state of plant
𝑥 = [𝜃 𝜃̇ Ψ Ψ̇ Φ Φ̇ ]𝑇 (29)
‘u’ is the control signal of LQR controller
𝑢 = [𝑣𝑣𝑙
𝑟] (30) State feedback law gives the cost function for optimal control approach
J =1
2∫ (x0∞ TQx + UTRU)dt→ minimal (31) Here: Q is symmetric positive semi definite matrix
𝑄 =
[
𝑄1 0 0 0 0 0
0
𝑄2 0 0 0 0
0 0
𝑄3 0 0 0
0 0 0
𝑄4 0 0
0 0 0 0
𝑄5 0
0 0 0 0 0
𝑄6] (32)
R is symmetric positive definite matrix
𝑅 = [𝑅1 0
0 𝑅2] (33) Optimal control input is given as
U = - Kx (34) Where ‘K’ is state feedback gain
Parameters 𝑄1, 𝑄2, 𝑄3, 𝑄4, 𝑄5, 𝑄6 are optimal parameters for each state 𝜃 𝜃̇ Ψ Ψ̇ Φ Φ̇ ,
depending on the stability priority of which state we will adjust the corresponding parameter on the larger Q matrix, 𝑅1, 𝑅2 are parameters for two motor, 𝑅1 and 𝑅2 must be equal
The K feedback gain matrix is constructed so that the energy consumed to pull the system in steady state is minimal when the system is knocked out of steady state by noise To find the matrix K we replace the values into the Riccati equation or we can find K through the lqr(A, B,
Q, R) command supported by MATLAB
Trang 103.2 Simulation
3.2.1 Linearization
Based on the mathematical model built in 2.2 we conduct object simulation on
MATLAB/Simulink Object is robot with input signal PWM voltage to motor and target output
is robot states using Fcn function in MATLAB to describe the dynamic model of 2-wheels
balancing robot
Figure 2 Mathematical model of object in MATLAB/Simulink
With system parameter in TABLE 1, the matrix of state A, B:
𝐴 = [
0 0 0 0 0 0
1 −3.7875 0
−3.7711 0 0
0 30.8417 0 30.8613 0 0
0 0.0059 1
−0.0016 0 0
0 0 0 0 0 0
0 0 0 0 1 −0.0188]
𝐵 = [
0 0.0062 0
0 0.0062 0
−0.0017 0
−0.0017 0
−0.0053 0.0053 ]
3.2.2 Define K control matrix
Choose matrix Q and R:
Trang 11𝑄 = [
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1
0 0 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1]
; 𝑅 = [1 0
0 1]
Find matrix K using command K=lqr(A,B,Q,R) in MATLAB
𝐾 = [−0.7 415.1 −3483.7 −864.8 −0.7 −9.9
−0.7 415.1 −3483.7 −864.8 0.7 9.9 ]
Figure 3 Closed loop model of LQR controller in MATLAB/Simulink
IV Result
4.1.The reference value is set to 0
The system was initially unstable, but was immediately pulled back to the set state by the LQR controller
Steady state error 𝑒∞≈ 0
Over shoot 𝑃𝑂𝑇 = 0.521%
Rise time 𝑡𝑟 = 6.140𝑠
Settling time 𝑡𝑥𝑙 = 47.498𝑠
Trang 12Figure 4 The medium angle of the left and right wheels Steady state error 𝑒∞≈ 0
Over shoot 𝑃𝑂𝑇 = 0.0153%
Settling time 𝑡𝑥𝑙 = 28.363𝑠
Figure 5 The inclined angle of robot’s body
Trang 13Steady state error 𝑒∞≈ 0
Over shoot 𝑃𝑂𝑇 = 1.732%
Settling time 𝑡𝑥𝑙 = 30
Figure 6 The robot's rotation angle in the xy plane
4.2.The reference value is set 𝜽 =𝝅
𝟐 and 𝝓 =𝝅
𝟐
The robot moves a distance of half the circumference of the wheel and the robot rotates
90 degrees around its axis
Steady state error 𝑒∞≈ 0
Over shoot 𝑃𝑂𝑇 = 3.646%
Rise time 𝑡𝑟 = 20.527𝑠
Settling time 𝑡𝑥𝑙 = 29.491𝑠
Trang 14Figure 7 The medium angle of the left and right wheels Steady state error 𝑒∞≈ 0
Over shoot 𝑃𝑂𝑇 = 1.892%
Settling time 𝑡𝑥𝑙 = 32.451𝑠
Figure 8 The inclined angle of robot’s body Steady state error 𝑒∞≈ 0
Trang 15Over shoot 𝑃𝑂𝑇 = 3.958%
Rise time 𝑡𝑟 = 25.232𝑠
Settling time 𝑡𝑥𝑙 = 36.493𝑠
Figure 9 The robot's rotation angle in the xy plane
4.3.When the system has disturbance (noise power 0.001)
The system is still sticking to the reference value but the effect of high frequency interference on the medium angle of the left and right wheels is relatively large steady state error 𝑒∞≈ 0.02
Trang 16Figure 10 The medium angle of the left and right wheels
Noise impacted on the inclined angle of robot’s body is relatively small, steady state error 𝑒∞≈
0
Figure 11 The inclined angle of robot’s body Almost no effect of noise on the robot's rotation angle in the xy plane so, parameter chosen for ∅ angle is optimal
Figure 12 The robot's rotation angle in the xy plane
Trang 17Because of the maximum effect of noise on 𝜃 angle, Q matrix should be change
𝑄 = [
20 0 0 0 0 0
0 1 0 0 0 0
0 0 1
0 0 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1]
𝐾 = [−3.2 421.5 −3629.2 −890.3 −0.7 −9.9
−3.2 421.5 −3629.2 −890.3 0.7 9.9 ] The effect of strongly reduced interference shows that 𝜃 angle is very easily unstable by
harmonic, 𝜃 angle should be controlled priority over Ψ and ∅
Figure 13 The medium angle of the left and right wheels when change Q matrix
Trang 18Figure 14 The inclined angle of robot’s body when change Q matrix
Figure 15 The robot's rotation angle in the xy plane when change Q matrix
4.4.Discussion
The LQR controller responds well, the robot always adheres to the set signal even if it is affected by system noise However, the settling time is long and overshoot is still large
V Conclusion
Trang 19From the construction of the mathematical description of the 2-wheels self balance model, the paper proves that the self-balancing vehicle is the control nonlinear object Linearization around balance operation point, robot model is unstable system in vertical position Using the LQR optimal controller allows stabilizing the rotation angle of the vehicle body and the robot's rotation angle on the yz/xz and xy coordinate systems Simulation results show that the system is kept in balance after using the LQR controller However, the settling times are long and susceptible to measurement disturbance To overcome this shortcoming, the development direction
of the topic is using GA (Genetic Algorithms) to find the matrix K and the Kalman filter algorithm
to filter high harmonic
References
[1] Book
Nguyen Doan Phuoc, Book Title: Lý thuyết điều khiển tự động, 150−60−4/2/2005, NXB Văn hóa
dân tộc, Hà Nội, Việt Nam, 7/2005
[2] Book
Huỳnh Thái Hoàng, Nguyễn Thị Phương Hà, Mô tả hệ rời rạc dùng MATLAB In Lý thuyêt điều
khiển tuyến tính, 2005, NXB ĐHQGTPHCM, Hồ Chí Minh, Viet Nam, pp 272, 2005
[3] Conference Paper and Symposium
Conference Paper in Print Proceedings
Nguyễn Minh Tâm, Lê Văn Tuấn, Nguyễn Văn Đông Hải, Modelling and Optimal Control for
Two-wheeled self-balancing robot, journal for research, ResearchGate, TPHCM, Việt Nam,
December 2015
Online (Nguyễn Văn Đông Hải)
Nguyễn Văn Đông Hải, Hướng dẫn điều khiển LQR cho hệ xe hai bánh tự cân bằng,
12/01/2016 Retrieved from
https://www.youtube.com/watch?v=eeaqPCHMAXg&list=LL&index=3&t=3s