TTU

Department of Mechatronic Engineering

Inverted Pendulum by
PIC Control
Self-Standing Robot

5. LQ Control

May 2017
Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 1
 Table of Contents

1. Object..................................................................................................................................... 2

2. Basics of Optimum Regulator (Linier Quadratic Control).........................................................2

3. Application to the Inverted Pendulum......................................................................................2

4. Simulation & Programming.....................................................................................................2

Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 2
1. Object
The basic lectures of inverted pendulum, about system dynamics, mathematical simulation,
electrical circuit and software, have been explained in passed lectures. And following documents
have been submitted.

1. Motion Equation
2. Control & Simulation
3. Circuit Diagram
4. Programming

We studied the state feedback control method at lecture 2 of “Control & Simulation”. In this lecture
5 studies about optimum control of state feedback. In generally the optimum control is called
optimum regulator or “Linier Quadratic Control” (LQ Control).
At designing the state feedback system, the feedback gain matrix F is determined by using pole
assignment method. But that method is not so good to imagine actual system activity. In other words,
it is hard to imagine that how the behavior of the system turns out by the position of poles. If a
system is high-dimensional, it will become difficult all the more.
On the other hand, at the optimum regulator method, you can set an evaluation function, and then
the optimum regulator will calculate the optimum gain F to make minimize that evaluation function.
You can set the value of evaluation function freely and it is easy way to understand the system
behavior. The LQ control is a popular method of modern control algorithm.

Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 3
2. Basics of Optimum Regulator (Linier Quadratic Control)
Consider the following system. The system must be controllable to apply LQ control method.

Ẋ =AX + BU (1)

Z=MX

On the optimum regulator theory, set the following evaluation function and calculate the state
feedback U that lets minimum J.

∞
J=∫ ( Z Z +U RU ) dt
T T

(2)
The state feedback U and feedback gain F are as follows

U =−FX=−R B PX
−1 T (3)

F=−R B P
−1 T (4)

The R is a constant symmetric matrix of positive. The P is a result of following Riccati equation,
and constant symmetric matrix of positive.

T
A P+ PA−PBR B P+ M M =0
−1 T T (5)

Furthermore, ZT Z= X T M T MX and then set to M T M =Q

(2) and (5) are as follows

∞
J=∫ ( X QX +U RU )dt
T T

(6)
T
A P+ PA−PBR B P+Q=0
−1 T (7)

Optimum state feedback U and optimum gain F same as (3),(4)

The Q and R are weight constant, Q’=Q, Q>0 and R’=R, R>0

The method of solving the (7) Riccati equation is, set to initial value P(0)=0 and calculate it in
reverse time.
Resolve (8) in reverse time, then P(t) converges in a constant value.

T
− Ṗ= A P+ PA−PBR B P+Q
−1 T (8)

Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 4
3. Application to the Inverted Pendulum
As the controllability of inverted pendulum system has been proved at lecture 2, then it is possible
to design the optimum regulator. The state matrix is as follows.

Ẋ =AX + BVs X T =[ ẋ x θ̇ θ] (9)

Y =CX

[ ] [ ][ ] [ ]
ẍ a11 0 a13 a 14 ẋ b1
ẋ = 10 0 0 x+ 0 V (10)
S
θ̈ a31 0 a33 a 34 θ̇ b3
θ̇ 0 0 10 θ 0

[ ][ ]
10 0 0 ẋ
Y= 0 10 0 x (11)
0 0 10 θ̇
0001 θ

2 2 2
a 0=J θ ( M + m) + Mm L b 1=( J ¿ ¿ θ+ m L )2 K F /a0 a11=−(J ¿ ¿ θ+m L )C B /a0 ¿ ¿
b 3=−mL2 K F /a0
a 13=m L/a 0
2 2
a 14=−m L g /a0
a 31=mL C B /a0
a 33=−(M +m)C R /a0
a 34=( M +m)mgL /a0

The exact numbers of state equation matrix are as follows.

Table-3.1 Exact number of state equation matrix

Arduino Type H8/3069F Type PIC18F Type
a0 1.00x10 -3 1.37x10 -3
a 11 -1.28x10 -2 -1.38x10 -2
a 13 3.81x10 -1 3.56x10 -1
a 14 -3.57 -4.26
a 31 9.53x10 -2 8.91x10 -2
a 33 -10.7 -7.59
a 34 100 90.7
b1 1.26x10 -2 1.37x10 -2
b3 -9.46x10 -2 -8.84x10 -2

As Q’=Q, Q>0, and Vs is a scalar input then set to as follows.

Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 5
 [ ]
Q11 0 0 0
0 Q 22 0 0
Q= (12)
0 0 Q33 0
0 0 0 Q44

R (scalar) (13)

For example, set the weight of Q and R as follows.

[ ]
10 0 0
Q= 0 10 0 (13)
0 0 100 0
0 0 0 100

R = 0.01 (14)

Use the SCILAB to solve (8) Riccati equation. Next section describes about detail procedure.

Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 6
4. Simulation & Programming
Basic operation of SCILAB is already explained in the lecture 2 “Control and Simulation”. And
the “sec” program and “Xcos” GUI program already exist. In this lecture, add the calculating
program of Riccati equation into to the existing “sec” file. The feedback gain F is only changed at
this LQ control, and the “Xcos” GUI program is not need to modify.
Calculating procedures of optimum gain F is as follows.

Open the “INV_PEND_simulation_NO2.sec” file. This file is made at “2 Control and Simulation”.
Add following program.

Fig-4.1 SciNotes Window

This “XX=riccati(A,B*inv(R)*B',Q,'c','eigen');” is a program to solve the Riccati equation. And

“XX” is a solution.
The “F2” is a converted feedback gain for programming. Because the A/D convert data of rotation
speed from gyro sensor is binary format, but above result from SCILAB is floating point format.
So that it should be better to change the feedback gain F to suitable binary data in order to simplify
the PIC18 program. The LSB of H8 type system is 0.00878 rad/s. This LSB is depending on the
characteristics of gyro and OPamp. Then you have to measure and determine the LSB of new PIC
system.

Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 7
 Depress the “save and execute” button in the “SciNotes” window. Next key input “F” in the
console window, then calculated feedback gain are displayed.

F = - 27.84039 - 3.1622777 - 153.84498 - 2118.2406

Open the “INV_PEND_simulation_NO2.zcos” file to start simulation. This file is made at “2

Control and Simulation”. Start Xcos, then following figures are displayed.

Fig-4.2 Time Response of State Variables (X)

Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 8
 Fig-4.3 Time Response of Control Input (Vs)

Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 9
 When previous Fig-4.2 and Fig-4.3 are displayed, confirm the difference of Section 2 simulation
results. The wave forms are a little changed. Especially, overshoot of control input Vs is almost
disappeared at LQ control. It means that the LQ control minimize the control input.
Next is that change the weight of Q and R as you want, and confirm how simulation changes. And
check the position of poles and display Nyquist diagram.

Key input “F2” into the console window, then the converted feedback gain “F2” is displayed as
follows. This “F2” is the feedback gain for PIC programming.

Control Input (Vs)=K θ̇∗θ̇n + Kθ∗θ n+ K Ẋ∗ Ẋ n+ KX∗X n

F2 = - 27.84039 - 3.1622777 - 0.7523019 - 10.358197

It’s not necessary to change the PIC program, only must to modify the feedback gain.

Title No Revision 0
Inverted Pendulum by PIC Control
5. LQ Control Page 10