# The LQR Controller Design of Two-Wheeled Self-Balancing Robot Based on the Particle Swarm Optimization Algorithm

Authors: Jian Fang
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

The dynamics model is established in view of the self-designed, two-wheeled, and self-balancing robot. This paper uses the particle swarm algorithm to optimize the parameter matrix of LQR controller based on the LQR control method to make the two-wheeled and self-balancing robot realize the stable control and reduce the overshoot amount and the oscillation frequency of the system at the same time. The simulation experiments prove that the LQR controller improves the system stability, obtains the good control effect, and has higher application value through using the particle swarm optimization algorithm.


---

## Body

## 1. Introduction

The two-wheeled and self-balancing robot belongs to a multivariable, nonlinear, high order, strong coupling, and unstable essential motion control system, and it is a typical device of testing various control theories and control methods; therefore, the research has great theoretical and practical significance. Because it has the advantages of simple structure, stable running, high energy utilization rate, and strong environmental adaption, it has the broad application prospects whether in the military field or in the civilian field.Since 1980s, the scholars of various countries have conducted the system research on the two-wheeled self-balancing robot. The two-wheeled and self-balancing robot control system based on the fuzzy control can overcome the instability and nonlinear nature of the system, but it relies on the expert’s experience too much [1, 2]. The optimal LQR controller is designed on the basis of establishing the system structure model; the correctness and effectiveness of the LQR controller are verified, but it is difficult to determine the weighted matrix Q and R [3–5]. The genetic algorithm is successfully applied to the parameter optimization of the LQR controller of the inverted pendulum system, and it achieves the good control effect. However, the parameters are difficult to adjust, and it is easy to fall into the local optimization [6].This paper concerns the self-designed and two-round self-balancing robot as the research object, which uses the Newtonian mechanics equation method and the linear method near the balance point to establish the linearized mathematical model of the system. In view of the mathematical model of the system, LQR controller is designed based on the particle swarm optimization and makes full use of the searching capability of the particle swarm algorithm to optimize the matrixQ and matrix R of the LQR controller. It gains the global optimal solution of matrix Q and R of the LQR controller so as to design the optimal state feedback control matrix K and overcome the disadvantages of relying on the experience and the trial and error in the selection of matrix Q and R of the general LQR control design, making up for the inadequacy of big workload. This method has better control effect by simulation tests and comparison.


## 2. The Dynamics Model of the Two-Wheeled Self-Balancing Robot

The two-wheeled and self-balancing robot structure is mainly composed of the body and the two wheels, and the robot is the coaxial two wheels, driven by the independent motor; the parameters of quality, moment of inertia, and radius of the two wheels are regarded as the same, so the body center of gravity is inverted above the axletree and it keeps balance through sports.The two-wheeled and self-balancing robot can be regarded as the vehicle-mounted inverted pendulum, so the dynamic system analysis process is more complex. This paper separates the wheel from the pendulum analysis first in the process of modeling, and then it deduces the dynamics state equation of the two-wheeled self-balancing robot through the simultaneous two parts [7, 8].The two wheels are regarded as the research object, Figure1 is a diagram revolver force analysis. According to the revolver force equation can be obtained in the following according to the Newton’s law [9] and the rotational torque formula [10]:
(1)
M
w
x
¨
=
H
f
R
-
H
R
,
I
ω
θ
¨
ω
=
C
R
-
H
f
R
·
R
.Figure 1
The force analysis of revolver.The right wheel force equation is as follows:(2)
M
w
x
¨
=
H
f
L
-
H
R
,
I
ω
θ
¨
ω
=
C
L
-
H
f
L
·
R
.After finishing, it can obtain(3)
2
(
M
w
+
I
ω
R
2
)
x
¨
=
C
R
+
C
L
R
-
(
H
R
+
H
L
)
.Among them,M
w is the weight of the wheel; I
ω is the moment of inertia of the wheel; R is the radius of the wheel; X
¨ is the wheel acceleration of X axis; C
R and C
L are the right and left wheel torque; H
L and H
R are the Z axis forces of the left and right wheels with the car body; H
f
R and H
f
L are the interatomic forces of the right and left wheels with the ground; and θ
ω is the angle of wheel around the Z axis direction.The car body of the two-wheeled and self-balancing robot is modelled as an inverted pendulum; the car body force analysis is shown in Figure2. Using Newton’s second law, the horizontal force is as follows:
(4)
∑
F
x
=
(
H
R
+
H
L
)
-
M
p
l
θ
¨
p
sin
⁡
θ
p
-
M
l
θ
¨
p
cos
⁡
⁡
θ
p
=
M
p
·
x
¨
p
=
M
p
(
x
+
l
sin
⁡
θ
¨
p
)
.Figure 2
The analysis of car body force.Among them,x
p is the displacement of the car body centre of gravity relative to the ground,
(5)
x
p
=
x
+
l
sin
θ
p
.Using Newton’s second law, the force of the vertical direction of the car body is as follows:(6)
∑
F
x
p
=
M
p
g
sin
⁡
θ
-
(
P
R
+
P
L
)
sin
⁡
θ
p
+
(
H
R
+
H
L
)
cos
⁡
⁡
θ
p
=
M
p
(
x
+
l
sin
⁡
θ
¨
p
)
cos
⁡
θ
p
.The sum of the torques of the car body mass center is as follows:(7)
∑
M
0
=
I
p
θ
p
¨
=
(
P
R
+
P
l
)
sin
θ
p
-
(
H
H
+
H
L
)
cos
⁡
θ
p
.On the small angle scope,θ
p
2
≈
0, sin
⁡
θ
p
≈
θ
p, cos
⁡
⁡
θ
p
≈
1, the linearized equations are gotten after the linearization process:
(8)
(
M
P
+
2
M
W
I
W
R
2
)
x
¨
=
C
R
+
C
L
R
-
2
M
P
l
θ
p
,
h
(
2
M
P
l
2
+
I
p
)
θ
¨
p
=
M
p
g
l
θ
p
-
M
p
l
x
⃛
.Among them,θ
p is the angle of the car body deviating from the Z axis direction; I
p is the moment of inertia of the car body; M
P is the weight of the car body; and l is the height of which the car body is apart from the shaft. The output torque of the wheel is C
R
=
C
L
=
I
R
(
d
ω
/
d
t
)
=
(
k
m
/
R
)
U
a
-
(
-
k
m
k
e
/
R
)
θ
˙
ω; the state equation of the two-wheeled self-balancing robot is obtained:
(9)
[
x
˙
x
¨
θ
˙
p
θ
¨
p
]
=
[
0
1
0
0
0
2
k
m
k
e
(
M
p
l
r
-
I
p
-
M
P
l
2
)
R
r
2
A
M
p
2
g
l
2
A
0
0
0
0
1
0
2
k
m
k
e
(
r
B
-
M
p
l
)
R
r
2
A
M
p
g
l
β
A
0
]
[
x
x
˙
θ
p
θ
p
˙
]
+
[
0
2
k
m
(
I
p
+
M
p
l
2
-
M
p
l
r
)
R
r
A
0
2
k
m
(
M
p
l
-
r
B
)
R
r
A
]
U
a
.Among them,A
=
[
I
p
β
+
2
M
p
l
2
(
M
w
+
(
I
w
/
r
2
)
)
]; B
=
(
2
M
w
+
(
2
I
w
/
r
2
)
+
M
p
).The output equation is as follows:(10)
y
=
[
0
0
1
0
]
[
x
x
˙
θ
p
θ
p
˙
]
.


## 3. The LQR Controller Parameter Optimization Based on the Particle Swarm Optimization Algorithm

### 3.1. The Design of the Self-Balancing Robot LQR Controller

The LQR method is the most mature controller design method in the development of modern control theory [11]; LQR optimal control is to seek the control amount u
*
(
t
) to make the system reach the steady state and guarantee the performance index J to take the minimum value:
(11)
J
=
∫
0
∞
(
X
T
Q
X
+
u
T
R
u
)
d
t
.Among them,u
*
=
-
R
-
1
B
T
P
x
=
-
K
x, and P is the solution of the algebraic equation P
A
+
A
T
P
+
Q
-
P
B
R
-
1
B
T
P
=
0 of the matrix Riccati [12]. In (13), the matrix Q and matrix R are mutually restricted, and the size of the Q value is proportional to the anti-interference ability of the system; increasing the Q value, the anti-interference ability of the system is enhanced, and the adjustment time of the system is shortened. However, at the same time, the oscillation of the system is strengthened, and the consumption of energy increases. The increase of R value makes the energy consumed by the system less, but the adjustment time increases. Therefore, the design key is to find the right weight matrix Q and matrix R. As long as we make sure of the matrix Q and matrix R, the state feedback matrix K is the only confirmation. However, the selection of the Q matrix and R matrix entirely depends on the experience and trial and error method in the process of LQR controller design, so the subjectivity is larger, resulting in the imperfection of the controller design and affecting the control effect.


### 3.2. The Parameter Optimization Principle of the LQR Controller Based on the Particle Swarm Optimization Algorithm

As a new optimization algorithm has been developed in recent years, the particle swarm optimization algorithm is abbreviated as the PSO. The particle swarm optimization algorithm is a kind of evolutionary algorithms, and it starts from the random solutions and searches for the optimal solution through the iterative algorithm [13].AssumingX particles are composed of a group in n dimensional space, among them, the position and velocity of i particle in the space are x
i
=
(
x
i
1
,
x
i
2
,
…
,
x
i
D
), v
i
=
(
v
i
1
,
v
i
2
,
v
i
3
,
…
,
v
i
D
), i
=
1,2
,
…
,
m, the best position that the i particle experiences is denoted by p
bset
(
i
), and the best position that all the particles in the group experience is denoted by g
bset
(
i
). The whole particle swarm updates the velocity and position through tracking the individual extreme value and the optimal value [14]. The particle optimization process is expressed in
(12)
v
i
d
k
+
1
=
w
v
i
d
k
+
c
1
∂
(
p
i
d
k
-
x
i
d
k
)
+
c
2
β
(
p
g
d
k
-
x
i
d
k
)
,
x
i
d
k
+
1
(
t
+
1
)
=
x
i
d
k
(
t
)
+
v
i
d
k
(
t
)
.Among them,ω is the inertia weight; c
1 is the weight coefficient of the optimal value that the particle tracks its history; c
2 is weight coefficients that particle track the optimal value; and ∂ and β are the random numbers changing in [
0
,
1
]. p
i
d
k is the individual optimal solution of the particle after the k iterations; p
g
d
k is the global optimal solution of the group in the k iterations. To make the algorithm a more accurate search scope, the movement speed of the particle is limited in [
-
v
max
⁡
,
v
max
⁡
]; if v
max
⁡ is too large, the particle will fly over the optimal solution; if it is too small, it is easy to fall into the local optimum. Assuming that the particle position is defined as the interval [
-
v
max
⁡
,
v
max
⁡
], the two-wheeled self-balancing robot state variables [
x
,
x
˙
,
θ
p
,
θ
˙
p
]
T are regarded as the particles, and the particle’s position and the initial value of the speed are produced at random in a certain range. The fitness function is an important link in using the particle swarm optimization algorithm, and it is the standard of the whole particle swarm algorithm iterative evolution. Because what we have designed is the quadratic optimal control regulator, we adopt the linear quadratic performance index formula (10) as the fitness function. The Q is a symmetric positive semidefinite matrix of 6
*
6; R is a constant positive definite matrix. In order to simplify the problem and make the weighted matrix a clear physical meaning, we choose the weighted matrix Q as the diagonal matrix, so that the performance index can be represented as
(13)
J
=
∫
0
∞
(
q
1
x
1
2
+
q
2
x
2
2
+
q
3
x
3
2
+
q
4
x
4
2
+
R
u
2
)
d
t
.Among them,q
1, q
2, q
3, and q
4 are the weights of the position, speed, angle, and angular velocity of the two-wheeled self-balancing robot, respectively. R is the square weight of the control amount u in the objective function.


### 3.3. The Parameter Optimization Steps of the LQR Controller Based on the Practical Swarm Optimization Algorithm

Step 1.
Initialize the particle swarm. Set the speed coefficientsc
1, c
2, the maximum evolution algebra gen, the size of the group pop, and the location of the initial search point and its speed, and each particle has the value of the current position.Step 2.
Calculate the fitness valueF
i
d
[
i
] of each particle.Step 3.
Compare the fitness valueF
i
d
[
i
] with the individual extremum p
best
(
i
) of each particle; if F
i
d
[
i
]
>
p
best
(
i
), use F
i
d
[
i
] to replace p
best
(
i
).Step 4.
Compare the fitness valueF
i
d
[
i
] with the global extremum g
best of each particle; if F
i
d
[
i
]
>
p
best
(
i
), use F
i
t
[
i
] to replace g
best.Step 5.
Update the particle’s speedv
i and position x
i according to formula (12).Step 6.
If it meets the end condition (the error is good enough or it reaches the maximum cycle times), it will exit, or it returns to Step2.


## 3.1. The Design of the Self-Balancing Robot LQR Controller

The LQR method is the most mature controller design method in the development of modern control theory [11]; LQR optimal control is to seek the control amount u
*
(
t
) to make the system reach the steady state and guarantee the performance index J to take the minimum value:
(11)
J
=
∫
0
∞
(
X
T
Q
X
+
u
T
R
u
)
d
t
.Among them,u
*
=
-
R
-
1
B
T
P
x
=
-
K
x, and P is the solution of the algebraic equation P
A
+
A
T
P
+
Q
-
P
B
R
-
1
B
T
P
=
0 of the matrix Riccati [12]. In (13), the matrix Q and matrix R are mutually restricted, and the size of the Q value is proportional to the anti-interference ability of the system; increasing the Q value, the anti-interference ability of the system is enhanced, and the adjustment time of the system is shortened. However, at the same time, the oscillation of the system is strengthened, and the consumption of energy increases. The increase of R value makes the energy consumed by the system less, but the adjustment time increases. Therefore, the design key is to find the right weight matrix Q and matrix R. As long as we make sure of the matrix Q and matrix R, the state feedback matrix K is the only confirmation. However, the selection of the Q matrix and R matrix entirely depends on the experience and trial and error method in the process of LQR controller design, so the subjectivity is larger, resulting in the imperfection of the controller design and affecting the control effect.


## 3.2. The Parameter Optimization Principle of the LQR Controller Based on the Particle Swarm Optimization Algorithm

As a new optimization algorithm has been developed in recent years, the particle swarm optimization algorithm is abbreviated as the PSO. The particle swarm optimization algorithm is a kind of evolutionary algorithms, and it starts from the random solutions and searches for the optimal solution through the iterative algorithm [13].AssumingX particles are composed of a group in n dimensional space, among them, the position and velocity of i particle in the space are x
i
=
(
x
i
1
,
x
i
2
,
…
,
x
i
D
), v
i
=
(
v
i
1
,
v
i
2
,
v
i
3
,
…
,
v
i
D
), i
=
1,2
,
…
,
m, the best position that the i particle experiences is denoted by p
bset
(
i
), and the best position that all the particles in the group experience is denoted by g
bset
(
i
). The whole particle swarm updates the velocity and position through tracking the individual extreme value and the optimal value [14]. The particle optimization process is expressed in
(12)
v
i
d
k
+
1
=
w
v
i
d
k
+
c
1
∂
(
p
i
d
k
-
x
i
d
k
)
+
c
2
β
(
p
g
d
k
-
x
i
d
k
)
,
x
i
d
k
+
1
(
t
+
1
)
=
x
i
d
k
(
t
)
+
v
i
d
k
(
t
)
.Among them,ω is the inertia weight; c
1 is the weight coefficient of the optimal value that the particle tracks its history; c
2 is weight coefficients that particle track the optimal value; and ∂ and β are the random numbers changing in [
0
,
1
]. p
i
d
k is the individual optimal solution of the particle after the k iterations; p
g
d
k is the global optimal solution of the group in the k iterations. To make the algorithm a more accurate search scope, the movement speed of the particle is limited in [
-
v
max
⁡
,
v
max
⁡
]; if v
max
⁡ is too large, the particle will fly over the optimal solution; if it is too small, it is easy to fall into the local optimum. Assuming that the particle position is defined as the interval [
-
v
max
⁡
,
v
max
⁡
], the two-wheeled self-balancing robot state variables [
x
,
x
˙
,
θ
p
,
θ
˙
p
]
T are regarded as the particles, and the particle’s position and the initial value of the speed are produced at random in a certain range. The fitness function is an important link in using the particle swarm optimization algorithm, and it is the standard of the whole particle swarm algorithm iterative evolution. Because what we have designed is the quadratic optimal control regulator, we adopt the linear quadratic performance index formula (10) as the fitness function. The Q is a symmetric positive semidefinite matrix of 6
*
6; R is a constant positive definite matrix. In order to simplify the problem and make the weighted matrix a clear physical meaning, we choose the weighted matrix Q as the diagonal matrix, so that the performance index can be represented as
(13)
J
=
∫
0
∞
(
q
1
x
1
2
+
q
2
x
2
2
+
q
3
x
3
2
+
q
4
x
4
2
+
R
u
2
)
d
t
.Among them,q
1, q
2, q
3, and q
4 are the weights of the position, speed, angle, and angular velocity of the two-wheeled self-balancing robot, respectively. R is the square weight of the control amount u in the objective function.


## 3.3. The Parameter Optimization Steps of the LQR Controller Based on the Practical Swarm Optimization Algorithm

Step 1.
Initialize the particle swarm. Set the speed coefficientsc
1, c
2, the maximum evolution algebra gen, the size of the group pop, and the location of the initial search point and its speed, and each particle has the value of the current position.Step 2.
Calculate the fitness valueF
i
d
[
i
] of each particle.Step 3.
Compare the fitness valueF
i
d
[
i
] with the individual extremum p
best
(
i
) of each particle; if F
i
d
[
i
]
>
p
best
(
i
), use F
i
d
[
i
] to replace p
best
(
i
).Step 4.
Compare the fitness valueF
i
d
[
i
] with the global extremum g
best of each particle; if F
i
d
[
i
]
>
p
best
(
i
), use F
i
t
[
i
] to replace g
best.Step 5.
Update the particle’s speedv
i and position x
i according to formula (12).Step 6.
If it meets the end condition (the error is good enough or it reaches the maximum cycle times), it will exit, or it returns to Step2.


## 4. The Simulation Experiment of the LQR Controller Based on the Particle Swarm Algorithm

The parameter symbols, description, and the actual value of the two-wheeled self-balancing robot are shown in Table1.Table 1
The parameter of the robot.
Symbol
Actual value
k
m
0.0136 Nm/A
k
e
0.01375 V/(rad/s)
R
1.6 Ω
M
P
0.52 kg
M
w
0.02 kg
l
0.16 m
I
p
0.0038 kg·m2
g
9.8 m/s2
I
w
0.0032 kg·m2
r
0.025 mThe actual parameters of this system are substituted into the state equation; the actual state equation is obtained as follows:(14)
[
X
˙
X
˙
ϕ
˙
ϕ
¨
]
=
[
0
1
0
0
0
-
0.1038
25.5862
0
0
0
0
1
0
-
0.5015
238.4685
0
]
[
X
X
˙
ϕ
ϕ
˙
]
+
[
0
0.4891
0
2.3634
]
V
a
.The initial state of self-balancing robot system is as follows:[
x
,
x
˙
,
θ
p
,
θ
˙
p
]
T
=
[
0,0
,
0,0
]
T, selecting c
1
=
c
2
=
1.2 to make experiment, at the same time, selecting the inertia weight formula [15]:
(15)
ω
k
=
ω
max
⁡
-
k
gen
(
ω
max
⁡
-
ω
min
⁡
)
.Among them,ω
max
⁡
=
1, ω
min
⁡
=
0.3, and gen
=
30; it indicates the iterative number of the algorithm evolution, and k is the current evolution algebra.For an initial population of40
*
8 matrix, the four dimensions in the front represent particle updated location and the four dimensions in the latter represent particle updated speed. The particle swarm updated position curve is shown in Figure 3. The particle motion curve shown in Figure 3 is not lost regularity, so there is only one particle position in critical condition. The PSO updated rate curve is shown in Figure 4. Figure 4 shows the particle movement speed can be controlled, not beyond the intended scope. Selecting the inertia weight formula is appropriate.Figure 3
Particle swarm position initialization.Figure 4
Particle swarm speed initialization.PSO algorithm in this case has a total of 50 times iterative and adaptive values. The number of iterations with the curve is shown in Figure5.Figure 5
Q and R iterative optimization Figure based on PSO algorithm.Seen from Figure5, Q and R parameters after 21 iterations to achieve the optimization.The global optimal solution can be gotten through the particle swarm algorithm programming:(16)
Q
=
[
221.5326
0
0
0
0
169.2376
0
0
0
0
121.2542
1
0
0
0
187.6532
]
,
R
=
1.7682
.With the aid of MATLAB functionK
=
lqr
(
A
,
B
,
Q
,
R
) to work out the optimal feedback matrix: K
=
[
-
11.1932
,
-
16.2145
,
-
72.4045
,
-
17.7329
].The dynamic response curves of the two kinds of algorithms of the LQR controller and the LQR controller based on the particle swarm algorithm are shown, respectively, in Figure6.Figure 6
Based on self-balancing robot angle PSO optimized response.The simulation curves of Figure6 show that when the initial conditions of self-balancing robot are zero, the LQR control and LQR control after the particle swarm optimization algorithm can make the system stable. However, the latter algorithm has the advantages of stable short time, less overshoot with fewer shocks.In order to verify the LQR algorithm based on PSO optimization is better than that LQR algorithm in the literature [4] and literature [7]. LQR algorithm comparison and each algorithm control indicators are shown in Table 2.Table 2
The effective comparison table between other methods and proposed methods.
Symbol
Stability
Overshoot
Regulationtime
Oscillationfrequency
Literature [4]
Stable
62%
1.8 s
1
Literature [7]
Stable
21%
3.2 s
3
Proposed method
Stable
18%
2.5 s
1According to the above results, three kinds of algorithms can all achieve the stability of the system. The proposed algorithm on the overshoot and oscillation frequency is out of the more obvious advantages. That is because the algorithm can find the optimal solution of the LQR matrixQ and matrix R.


## 5. Conclusion

Using the particle swarm algorithm to optimize the selection of weighted matrixQ and matrix R can overcome the blindness of selecting matrix Q and matrix R in the traditional LQR optimal control. This paper uses the characteristics that the particle swarm optimization algorithm can achieve an intelligent search, gradual optimization, and rapid convergence. Therefore, it is not easy to fall into local optimum, but easy to be implemented on the basis of the linear model of the two-wheeled and self-balancing robot to obtain the global optimal solution of the Q, R, achieving the optimal LQR controller design through the MATLAB simulation experiments. It can be found that the design response speed of the LQR optimal controller is faster with less overshoot amount, and it can keep the steady-state error zero, so the control effect is better.



---

*Source: 729095-2014-06-11.xml*