IP Experiment Manual V2.00

GOOGOL
TECHNOLOGY
INVERTED PENDULUM
EXPERIMENTAL MANUAL
Suitable for GLIP Series
Second Edition, July, 2006
Be sure to give this instruction manual to customers! Thank you very much for purchasing INVERTED PENDULUM (GIP Series) of Googol Tech. Be sure to read this manual carefully before operation. For any technical trouble, call us or visit http://www.googoltech.com on the World Wide Web for consultation. After reading this manual, keep it handy so that it can be referred to at anytime.
Googol 2006
1
GOOGOL TECHNOLOGY
Copyright Statement
Googol Technology (HK) Limited All rights reserved Googol Technology owns the patent, copyright or any other intellectual property right of this product and its software: Linear inverted pendulum GLIP series including GLIP Series Installation and User Manual Inverted Pendulum and Automatic Control Theory Experiment and Under the copyright laws, no one shall directly or indirectly duplicate, produce, process or use this product and its relevant parts in any form without the prior written permission of Googol Technology.
Disclaimer
Googol Technology reserves the right of modifying the products and product specifications described in this manual without notification in advance. Googol Technology is not responsible for any direct, indirect, special, incidental or consequential loss or liability caused by using this manual or product incorrectly.
Trademarks
Windows and Microsoft are registered trademarks of Microsoft Corporation. Mathematica is registered trademark of Wolfram Research Inc. Matlab is trademark of Mathworks Inc.
Googol 2005
CAUTIONS ON SAFETY
Linear inverted pendulum systems are mainly used for education and research purpose. Before installation, operation and maintenance, please read this manual carefully. After reading this manual, keep it handy so that it can be referred to at anytime.
CAUTIONS ON USING
Before use (installation, operation, maintenance, examine and repair), please make sure to comprehend this manual and other related materials. Do not use the equipment until user get familiar with the machine property, safety caution and instructions. The safety instructions are classified to Danger, Caution, Compulsion and Prohibition.
Table 1-1 Warning Signs
Incorrect operation may result in such a dangerous situation as death or serious injury. Incorrect operation may result in such a dangerous situation as medium or slight injury. Operations must be done.
Prohibited operations.
Moreover, even the content in Caution may result in serious accident in certain situations. Please strictly comply all the instruction in operating the equipment. Although not in Danger or Caution, user should still comply the rules strictly.
Googol 2006
II
This manual describes general safety precautions only. It doesn't include every precaution measure for laboratory personnel in actual experimental environment. Sorry for any inconvenience caused. For safety reason, please make sure all the operation and maintenance staff be trained according to this manual. The operator should read carefully all the manuals of linear inverted pendulum system.
The figure or photo in this manual are demo examples, the real product might be slightly different in appearance; Please contact our sale department if the manual is lost or damaged; Please note that warranty will be voided if any modification is made on the product.
Machinery in motion can be dangerous. Please be careful to the possible dangerous situations.
Googol 2006
III
Table of Content
Foreword
This is an experimental instruction for undergraduate and postgraduate students major in the area of industrial automation and mechatronics. The book can be used in control courses as the experimental instruction. The main content of this book includes experiments of classical control and modern control theories. The classical control theory includes system modeling, root locus analysis and controller design, frequency response, PID control, etc. The modern control theory includes state space pole placement and linear quadratic regulator (LQR) method. All experiments are based on Googol inverted pendulum platform. Inverted pendulum is the typical experiment platform for the study of control theories. Since the control strategies of the inverted pendulum system is similar to the balance performance of the acrobats, and a lot of the abstract control concepts such as system stabilities, controllability, system anti-interference property etc can be displayed via the inverted pendulum system experiments, it becomes the essential control theory educational experiment facility in a lot of US and European institutes and colleges. Students can verify the control theories and algorithms of the automatic control theories via the inverted pendulum system experiments easily and directly. Not only is the inverted pendulum an excellent educational experiment apparatus, it is also an ideal experiment platform for carrying out researches on control theories. Since the inverted pendulum system possesses the characteristics of high order, instability, multiple-variables, non-linear and coupled, it is considered as a typical research target by many modern control theory researchers. New control strategies and control methods are discovered, and the related results are widely applied in the fields of aerospace technology and robotics. More complicated and different formats of inverted pendulum systems become the focal point in the fields of control theory research, and lot of outstanding papers appears in professional journals every year since the 90s. Googol Technology LTDGoogol for short here after provides experimental solutions for automatic control study in institutes and colleges base on inverted pendulum systems, including varies pendulum systems development and production, experiment set up and recommendation of textbooks and references related to automatic control theories. The inverted pendulum system developed by Googol includes linear series, circular series and configurable series. The main features are: Open architecture: Bu using 4 axes motion control card, the mechanical and electrical part can be easily extended according to the user requirement. Software interface is also open to the user. User may either use the provided experimental Googol 2006
IV
Table of Content
software or add user define function. Modularization: the mechanical part can be based on linear or rotary platform, and then configured to 1-stage, 2-stage or 3-stage inverted pendulum. 3-stage inverted pendulum can be easily disassembled to 2-stage inverted pendulum and 2-stage inverted pendulum can be disassembled to 1-stage inverted pendulum. Software is also modularized, customer may add or modify related function module conveniently. Easy and safety: the experimental system includes motion controller, electric box (integrated to mechanical body in rotary platform systems), mechanical body and PC, easy for maintenance and update. Moreover, there are protection designs on the mechanical part, motion controller and software, to ensure the safety of operator and equipments. ConvenienceInverted pendulum systems are easy to install, update and the software interface is user friendly. AdvanceThe system use industrial 4 axes motion control card as the core control system, advanced AC servo motor driver and high precision photoelectric coded compass. The design matches the direction of advanced motion control development. Experimental software diversity: the experimental software includes BorlandC++, VC++ and Matlab providing complete hardware and software interface, convenient for experiment and development. Complete experimental manuals: There are <<Operation Manual>> and <<Experimental Manual>>, user may set up experiments as instructed in the manual or design experiment by themselves. Googol inverted pendulum systems are suitable for experiment of the following courses: automation control principle, modern control theory, modern control engineering, optimal control, nonlinear system control, intelligent control, fuzzy control and neural network control, etc. In this book, a series of experiments are introduced based on Googol linear inverted pendulum systems. When setting up the related experiments, user should both have <<GT Series Motion Controller User Manual>>, <<GT Series Motion Controller Programming Manual>>, <<Panasonic AC Servo Motor Driver Operating Manual>> and manuals of other related equipment.
Googol 2006
Table of Content
Table of Content
COPYRIGHT STATEMENT ............................................................................................. I DISCLAIMER................................................................................................................ I TRADEMARKS ............................................................................................................. I CAUTIONS ON SAFETY........................................................................................ II CAUTIONS ON USING........................................................................................... II FOREWORD ...............................................................................................................IV TABLE OF CONTENT ..................................................................................................VI CHAPTER 1 OVERVIEW .........................................................................................1 1.1 1.2 1.3 1.4 1.5 INTRODUCTION ...................................................................................................1 INVERTED PENDULUM CLASSIFICATION ..............................................................1 INVERTED PENDULUM PROPERTIES .....................................................................3 CONTROLLER DESIGN METHOD ..........................................................................4 INVERTED PENDULUM EXPERIMENTS ..................................................................4
CHAPTER 2 MOTION CONTROL BASIC EXPERIMENT ................................6 2.1 ENCODER PRINCIPLE AND APPLICATION ..............................................................6 2.1.1 Encoder Principle.........................................................................................6 2.1.2 Angle Conversion ........................................................................................7 2.1.3 Encoder Experiment.....................................................................................7 2.2 MOTOR CONTROL IN MATLAB SIMULINK ..........................................................16 CHAPTER 3 LINEAR INVERTED PENDULUM MODELING, SIMULATION AND EXPERIMENT.................................................................................................25 3.1 1-STAGE LINEAR INVERTED PENDULUM SYSTEM MODEL .................................25 3.1.1 Differential Equations Methods .................................................................25 3.1.1.1 Newtons Mechanics............................................................................25 3.1.1.2 Lagrange Method .................................................................................29 3.1.2 System Parameters .....................................................................................33 3.1.3 Real System Model ....................................................................................33 3.1.4 System Controllability Analysis ................................................................34 3.1.5 System Step Response Analysis.................................................................36 3.2 1-STAGE LINEAR IP ROOT LOCUS CONTROL EXPERIMENT ................................38 3.2.1 Root Locus Analysis ..................................................................................38 3.2.2 Root Locus Method and Simulation ..........................................................40 3.2.2.1 Root Locus Method..............................................................................40 3.2.2.2 MATLAB Simulation...........................................................................43 3.2.2.3 MATLAB Simulink .............................................................................48 3.2.3 Root Locus Real Time Control Experiment...............................................54 3.2.4 Experiment Result and Report ...................................................................57 3.3 1-STAGE LINEAR INVERTED PENDULUM FREQUENCY RESPONSE ......................59 Googol 2006
VI
Table of Content
3.3.1 Frequency Response Analysis....................................................................59 3.3.2 Frequency Response Design and Simulation.............................................61 3.3.3 1-stage Linear IP Frequency Response Experiment ..................................70 3.3.4 Experiment Result and Report ...................................................................73 3.4 1-STAGE LINEAR IP PID CONTROL EXPERIMENT ..............................................74 3.4.1 PID Control Analysis .................................................................................74 3.4.2 PID Control parameters and Simulation ....................................................75 3.4.3 PID Control Experiment ............................................................................80 3.4.3.1 Experiment steps in MATLAB experimental software........................80 3.4.4 Experimental Result and Report ................................................................83 3.5 STATE SPACE POLE PLACEMENT CONTROL EXPERIMENT ..................................84 3.5.1 State Space Analysis ..................................................................................84 3.5.2 Pole Placement and Simulation .................................................................85 3.5.3 Pole Placement Control Experiment..........................................................97 3.5.4 Experiment Result and Report .................................................................100 3.6 LINEAR QUADRATIC LQR OPTIMAL CONTROL EXPERIMENT ..........................101 3.6.1 Linear Quadratic LQR Optimal Control Principle and Analysis .............101 3.6.2 LQR Control Parameters and Simulation ................................................102 3.6.3 1-stage Linear IP LQR Control Experiment ............................................106 3.6.4 Experiment Result and Report .................................................................109 CHAPTER 4 1-STAGE LINEAR PENDULUM MODELING AND EXPERIMENT ........................................................................................................ 110 4.1 1-STAGE LINEAR PENDULUM MODELING AND ANALYSIS ................................110 4.1.1 1-stage Linear Pendulum Modeling......................................................... 110 4.1.2 Real System Model ..................................................................................114 4.1.3 System Controllability Analysis .............................................................. 115 4.2 1-STAGE LINEAR PENDULUM ROOT LOCUS ANALYSIS ....................................116 4.3 1-STAGE LINEAR PENDULUM FREQUENCY RESPONSE ANALYSIS.....................117 4.4 1-STAGE LINEAR PENDULUM STEP RESPONSE ANALYSIS ................................118 4.5 1-STAGE LINEAR PENDULUM PID CONTROL SIMULATION AND EXPERIMENT..119 4.5.1 1-stage Linear Pendulum PID Control Analysis and Simulation.............119 4.5.2 1-stage Linear Pendulum PID Real Time Control ...................................123 4.6 1-STAGE LINEAR PENDULUM LQR CONTROL SIMULATION AND EXPERIMENT 126 4.6.1 1-stage Linear Pendulum LQR Control Analysis and Simulation ...........126 4.6.2 1-stage Linear Pendulum LQR Control Experiment ...............................130 CHAPTER 5 1-STAGE LINEAR IP SWING UP EXPERIMENT.....................134 5.1 ENERGY CONTROL IN IP SWING UP .................................................................134 5.2 1-STAGE LINEAR IP SWING UP EXPERIMENT ...................................................135 5.3 SWING UP EXPERIMENT BY OTHER ALGORITHMS ...........................................137 CHAPTER 6 2-STAGE LINEAR INVERTED PENDULUM ............................140 6.1 SYSTEM PHYSICAL MODEL .............................................................................140 6.2 SYSTEM CONTROLLABILITY ANALYSIS ...........................................................146 Googol 2006
VII
Table of Content
6.3 6.4 6.5 6.6 6.7
2-STAGE LINEAR IP MATLAB SIMULATION ...................................................148 LQR CONTROLLER DESIGN AND SIMULATION ................................................150 2-STAGE LINEAR IP LQR CONTROL EXPERIMENT ...........................................155 EXPERIMENT RESULT AND REPORT .................................................................159 CONTROL EXPERIMENT BY OTHER ALGORITHMS ............................................159
CHAPTER 7 3-STAGE LINEAR INVERTED PENDULUM ............................161 7.1 SYSTEM PHYSICAL MODEL .............................................................................161 CHAPTER 8 APPENDIX.......................................................................................172 8.1 LIST OF FIGURES .............................................................................................172 8.2 LIST OF PROGRAM...........................................................................................175
Googol 2006
VIII
Chapter 1 Overview
Chapter 1 OVERVIEW
1.1 Introduction
Inverted pendulum (IP) problem is the combination of research area like robotics, control theory, computer control, etc. The IP system has the property of unstable, higher order, multi-variable and highly coupled, which can be treated as a typical nonlinear control problem. The research started from 50th, 20 century, control scientist in MIT developed the first 1-stage IP system based on the theory of rocket propeller. There are lots of new control algorithms appear in recent years. People apply them to IP systems to examine their ability to control multi-variable, nonlinear and unstable systems. As a relatively ideal experimental means for the teaching, experimentation and scientific research of control theories, IP system provides an excellent experimental platform for examining specific control theories or typical solutions and thus promoting the development of the new theories. Since they are widely applied in different fields such as semiconductors, delicate devices processing, robot control technology, artificial intelligence, missiles interception control systems, aviation docking control technology, perpendicularity control in rocket launching, gesture control in satellite circling and general industrial applications, they will have a broad prospects for development and utilization. Inverted pendulum can vividly simulate the flight control of rockets and the stabling control in walking robots etc.
1.2 Inverted Pendulum Classification

There are many series of IP systems extended from linear 1-stage IP, such as linear IP, circular IP, planar IP and configurable IP. IP is the system with pendulum plants placed on motion modules. Diverse pendulum plants and motion modules constitute different IP series. The following IP systems are classified by structure: 1) Linear Inverted Pendulum Linear IP has pendulum plant on a linear motion module with one degree of freedom. The cart moves on the sliding shaft horizontally. There are different kinds of linear IP systems based on different pendulum plant structure such as the flexible IP, which has two carts on the sliding shaft with a spring connected. The linear IP series are shown in Figure 1-1. 2) Circular Inverted Pendulum Circular IP system has the pendulum plant on a circular motion module with one degree of freedom. The pendulum is on the arm end are rotates around the center of the circle. Different IP system can be set up by varying the stage number
Googol 2006
1
Chapter 1 Overview
in series or parallel as shown in Figure 1-2. 3) Planar Inverted Pendulum Planar IP system has the pendulum plant on the planar motion module with two degrees of freedom. There are two classes of planar motion module: XY table and SCARA robotic arm. The pendulum can also be divided by stage like 1-stage or 2-stage as depicted in Figure 1-3. 4) Configurable Inverted Pendulum Configurable IP is a new class of IP systems whose pendulum plant is composed of pendulum rod and connection rod. The connection rod can be configured to 3 modes: level, vertical upper and vertical down. Classified by pendulum stages, there are 1-stage, 2-stage, 3-stage and 4-stage IP systems. 1-stage IP is used for basic experiment of control theory while others are mostly used for development of advanced control algorithms. Control complexity increase dramatically as the pendulum stage increase. The feasible maximal stage for IP system is 4 currently.
Figure 1-1 Linear IP Series
Figure 1-2 Circular IP Series
Googol 2006
Chapter 1 Overview
Figure 1-3 Planar IP Series
Figure 1-4 Configurable IP
1.3 Inverted Pendulum Properties

Despite the different size and structure, all IP systems have the following properties in common: 1) Nonlinearity IP is a typical nonlinear system. In real control, the system model is usually linearized. There are also nonlinear control methods applied to IP which is becoming a hot topic recently. 2) Uncertainty Most uncertainties come from model uncertainty, mechanical transmission error and other resistances. In real control, uncertainties are reduced by controlling errors like, tighten the belt or screw to reduce the transmission error, or use ball bearing to reduce the friction. 3) Coupling There are coupling between each stage of IP and the motion module. We will Googol 2006
3
Chapter 1 Overview
decouple the IP near the equilibrium point and ignore some less important coupling variables. 4) Open loop instability There are two equilibrium states for IP systems: vertical upper and vertical down, in which vertical upper is the unstable equilibrium point and vertical down is the stable equilibrium point. 5) Limitations The IP system performance is limited by mechanisms like motion module travel distance, motor torque, etc. To make it convenient and reduce the cost, the structure size and the motor power of IP are required to be small. The effect of travel distance to IP swing up is especially evident: short travel distance easily gets the cart exceed the limit switch.
1.4 Controller Design Method

Controller design is the key content of IP systems. Controllers are used to stabilize the unstable system and make it robust to disturbances. The widely used control methods recently are: PID control, root locus and frequency response, state space, optimal control, fuzzy control, neural network, humanoid artificial intelligent, robust control, adaptive control and some more power algorithms which combine the above ones.
1.5 Inverted Pendulum Experiments

The experiments can be accomplished by IP platform in this book are as follows:
Table 1-1 Inverted Pendulum Experiments
Type Basic Experiment
No. 1. 2. 3.
Modeling Experiment
4. 5. 6. 7. 8.
Control Experiment
Experiment Equipment Encoder Principle and Application All IP Series Motor Control in Motor Control in All IP Series Matlab Simulink Linear Inverted Pendulum Modeling, 1-stage IP Simulation and Experiment 1-stage Linear Pendulum Modeling and 1-stage IP Analysis 2-stage Linear Inverted Pendulum 2-stage IP 3-stage Linear Inverted Pendulum 3-stage IP 1-stage Linear IP Root Locus Control 1-stage IP Experiment 1-stage IP Experiment Name
4
Googol 2006
Chapter 1 Overview
9. 10. 11. 12. 13. 14.
1-stage Linear IP PID Control Experiment 1-stage Linear Pendulum PID Control Simulation and Experiment 1-stage Linear Pendulum LQR Control Simulation and Experiment State Space Pole Placement Control Experiment Linear Quadratic LQR Optimal Control 2-stage Linear IP LQR Control Experiment
1-stage IP 1-stage IP 1-stage IP 1-stage IP 1-stage IP 2-stage IP
Linear 3-stage IP can be taken apart to 2-stage IP and 1-stage IP.
Googol 2006
Chapter 2 Motion Control Basic Experiment

The objective of this experiment is let users understand the basic principle of angle encoder; master the skill to collect data from encoder by computer and motion control card for computer control. Two important contents in mechatronics will be comprehended in IP system experiments: sensors and motion control techniques.
2.1 Encoder Principle and Application

2.1.1 Encoder Principle
Rotary encoder is a class of angle position sensor which can be classified as photoelectric, contact and electromagnetism. The photoelectric pulse encoder is the most widely used position sensor in close loop systems currently. Output Input
Photosensitive element
Lamp
Figure 2-1 Photoelectric Encoder Component and Structure
There are two kinds of rotary encoders: incremental encoder and absolute encoder. Figure 2-1 is the illustration of incremental photoelectric encoder, which is composed of irradiance device, photoelectric coded compass, photosensitive device and signal processing circuits. When the coded compass is rotating with the working axis simultaneously, light source will transmit the light signals through the light-blocking board of the photoelectric coded compass with alternative bright and dark in light intensity, the photosensitive device will convert the light signal into electrical signals, it will be outputted after rectifying, magnifying, frequency division, and number recording via the signal processing circuit. In order to measure the direction of rotation, make sure that the distance of the two slits of the light-blocking board is less than of the intervals of the coded compass, the output signals of the two light sensitive devices will be /2 out of phase, the direction of the rotation of the coded compass can be found by feeding the output signal into the phase-detector circuit. Googol 2006
6
The resolution of photoelectric incremental encoder depends on its minimal distinguishable angle, which is related to the number of intervals (line number) on the compass: 360 = n In which n denotes compass line number. Each time the photoelectric pulse encoder rotates a minimal distinguishable angle it outputs a pulse signal. The rotation angle of the current axis can be obtained from the pulse number, thus the linear distance can be calculated by transmission rate. The rotation speed of current axis can be calculated by pulse frequency and the rotation direction can be estimated by the sequence of consecutive signals. The absolute encoder detects the rotation angle by the output of light emitting diode (LED) and photo diode which correspond to each bit of output binary code. The sensor for measuring linear distance is grating bar which has the same principle with incremental encoder. The photoelectric encoder becomes more and more popular nowadays for its convenience as it outputs digital signal, which can be directly processed by computer without any operation like magnification or transformation.
2.1.2 Angle Conversion

For n-line encoder, if the frequency ratio is m , the angle conversion relationship will be:
2 3.14 360 o N (degree) N (radian) or = nm nm
In which rotation angle of encoder

N encoder reading For IP motor encoder, the encoder reading is required to be converted to the position of the cart by the following equation: 3.14 l= N nm In which l cart displacement diameter of the synchronization belt
2.1.3 Encoder Experiment

The object of this experiment is the photoelectric rotary encoder. The following experiments can be implemented based on the above principle.
Experiment 1 Pendulum rod angle measurement Please follow the instructions to finish the angle measurement experiment in Googol 2006
7
MATLAB. Note: Please read carefully of IP user manual and get familiar with MATLAB environment, make sure that MATLAB real time control toolbox and VisualC are installed. (Please refer to Googol MATLAB real time control software user manual). 1) Open MATLAB Simulink
Figure 2-2 MATLAB Simulink Environment
2) Press on the window to create a new window
Googol 2006
3) Open Googol Education Products\GT-400-SV Block Library in Simulink as depicted in the figure below
4) Select Get Current Axiss Position module and pull it to the new window untitled
5) Double click GetPos to open the following window, set axis number to 2, which is the encoder of the first stage fixed on the cart.
Googol 2006
Select Axis No.
6) Pull Scope from Simulink\Sinks to untitled
7) Connect the two modulesmove mouse to<when the arrowhead icon become left press the mouse and move to> then release the mouse
Googol 2006
10
8) Pull GT400-SV Initialization in Googol Products\GT-400-SV Block Library to the window Here
Education
9) SelectExternal as depicted in the above figure 10) Save file as EncoderTest set parameters inSimulation\Simulation Parameters
Googol 2006
11
Set Simulation time and Solver optionsas shown in the above figurethe stop timeinfdenotes infinity and the step size is set to 0.005s ClickReal-Time Workshop to open the following window
11) Press Browse to change the setting toReal-Time Windows Target
Googol 2006
12
12) Then press OK as shown below
13) Press to build the filethe compiling information will be displayed in Command window
Googol 2006
13
14) Power on the electric box 15) Press

to connect the program
16) Press to run 17) Double clickScope to watch the data
18) Rotate the stage counterclockwise by hand for about 360 degree and see the result. When data exceed the display range, click to resize the scale
Googol 2006
14
We can see from the above figure, the encoder reading is 2400, which is 4 times of encoder line number (600). Manually rotate the pendulum rod clockwise or counterclockwise and observe the reading and actual pendulum rod angle. 19) Record the experiment result, analyze the data and finish the report. NoteDetailed model please refer to EncoderTest.mdl in the following path matlabroot\toolbox\GoogolTech\InvertedPendulum\ which can also be opened in Simulink Enter MATLAB SimulinkGoogol Education Products, open Encoder Test Experiment fromInverted Pendulum\Basic Experimentsas depicted below
System model is as follow
Googol 2006
15
2.2 Motor Control in Matlab Simulink

The object of this experiment is the AC servo motor of IP system. Set the motion controller current axis to operate in speed control mode and also users are required to set maximum velocity and acceleration. In speed control mode, the initial velocity will keep increase in fixed acceleration until it reaches the maximum velocity. The motion direction is decided by velocity sign. These two parameters can be changed in motion. Experiment procedures are as follow: 1) Open MATLAB/Simulink;
Googol 2006
16
2) Click on the window to create aModelwindow:
3) Open Googol Education Products\GT-400-SV Block Library in Simulink Library Browsewindow as depicted below:
Googol 2006
17
4) Pull Set Current Axiss Acc and Vel module to the new window untitled :
5) Double click SetAccVel module to open the following window, set parameters as in the following figure:
Googol 2006
18
Axis number: select axis number1, the axis of cart motor Back velocity: define the cart velocity after the limit switch is on (the cart will move towards the inverse direction when reach the limit), unit in m/s. Back position define the cart target position after the limit switch is on (the cart will move towards the inverse direction when reach the limit), unit in pulse number. 6) Pull twoConstantfromSimulink\Souces tountitled window:
Googol 2006
19
7) Connect the following 3 modules as the following figure:
8) Set the 2Constantmodules parameter as follow:
Googol 2006
20
Set cart speed be 0.2m/s, acceleration be 0.1m/s2 9) Pull GT400-SV Initialization module in Googol Education Products\GT-400-SV Block Libraryto the window: Here
10) SelectExternal as depicted in the above figure: 11) Save file asMoterTest SelectSimulation\Simulation Parameters to set parameters:
Googol 2006
21
Set Simulation time and Solver options as depicted in the above means infinity and step size is 0.005s. figure, in which the simulation time inf ClickReal-Time Workshop to open the following window:
Googol 2006
22
12) ClickBrowseto change the setting as Real-Time Windows Target
13) ClickOKin the following figure:
14) Click to build the file compiling information will be displayed in the Command window:
Googol 2006
23
15) Power on the electric box; 16) Click to connect the program; 17) Click to run the program; 18) Observe the cart locus and record the experiment result. Change the maximum velocity and acceleration, observe and record the cart locus and finish experiment report. Reference model: Open Googol Education Products in MATLAB Simulink toolbox, Select Servo Motor Control Experiment in Inverted Pendulum\Basic Experiments, System diagram is as follow
Googol 2006
24
Chapter 3 Linear Inverted Pendulum Modeling, Simulation and Experiment

The objective of this experiment is to allow users master the method of modeling to real system, get familiar with system modeling using matlab simulink, applied control theories to design controller and experiment on real systems. Observe and analyze the experiment result and thus have the personal impression on the applications of controllers. 1-stage linear IP is the most frequently seen IP system which is composed of linear motion module and 1-stage pendulum plant, as shown in Figure 1-1
3.1 1-stage Linear Inverted Pendulum System Model

There are two classes of system modeling method: mechanism modeling and experimental modeling. The experimental modeling is established through the feeding of a series of predetermined input signals by the users, the study target is stimulated and the visible output is examined after it passes the sensor, the input/output relationship of the system is established by applying mathematical formulae. It includes the design selection of the input signals, precise detection of the output signals, and the study of mathematical calculations. Mechanism modeling is based on the foundation of understanding the motion patterns through physical, chemical and mathematical means to set up the internal input/output relationship in the system. There are difficulties in IP system modeling because of its instability. But after ignoring some less important factors, the IP system becomes a typical rigid body in motion. The dynamic equation will be obtained in the inertial frame by classical mechanics theory. Next we are going to use Newton-Euler and Lagrange methods to develop the 1-stage linear IP model respectively.
3.1.1 Differential Equations Methods

3.1.1.1 Newtons Mechanics
After ignoring the air resistance and other frictions, 1-stage linear IP can be simplified as a system of cart and even quality rod, as shown in Figure 3-1. Lets define: M cart mass m rod mass Googol 2006
25
b l I F x
friction coefficient of the cart distance from the rod axis rotation center to the rod mass center rod inertia force acting on the cart cart position
Pendulum Rod
F Cart x Sliding shaft
Figure 3-11-stage Linear Inverted Pendulum Model
angle between the rod and vertically upward direction angle between the rod and vertically downward directionconsider that the initial position of the rod is vertically downward Figure 3-2 is the force analysis of cart and rod system. N and P denote the interactive force of cart and rod in the horizontal and vertical direction respectively. Note: In real system the detection and implementation direction is fixed, the predefined positive vector is shown as the arrow in the following figure.
Figure 3-2 Cart and Rod Force Analysis
From the forces in the horizontal direction, we obtain:

& M&& = F bx N x
(3-1)
From the force acting on the rod in horizontal direction we get:
Googol 2006
26
d2 N = m 2 ( x + l sin ) dt That is
(3-2)
& & N = m&& + ml& cos ml 2 sin x Combining with equation (3-1)the first dynamic equation is obtained:
& & & ( M + m) && + bx + ml& cos ml 2 sin = F x
(3-3) (3-4)
To get the second dynamic equation, analyze the force in the vertical direction then we have: P mg = m d2 (l cos ) dt 2 (3-5)
& & P mg = ml&sin ml 2 cos
(3-6) (3-7) because
By moment conservation:
& Pl sin Nl cos = I& Note: the direction of moment is = + , cos = cos , sin = sin . Combining two, we get the second dynamic equation: & ( I + ml 2 )& + mgl sin = ml&& cos x
negative
(3-8)
Let = + is the angle between the rod and vertically upward direction assume is relatively small to 1unit in radian, which means <<1 d 2 ) = 0. dt Let u denotes the input force of the object, linearize the two dynamics equations: then following approximation can be obtained: cos = 1 sin = (
& I + ml 2 & mgl = ml&& x & ( M + m) && + bx ml& = u x &

The Laplace transformation of equation (3-9) are as follow:
2 2 2 I + ml ( s) s mgl ( s ) = mlX ( s ) s ( M + m) X ( s) s 2 + bX ( s) s ml ( s ) s 2 = U ( s )
(3-9)
(3-10)
Note: the initial condition is assumed to be 1 when deducing the transfer function. The output angle is , solving the first equation: ( I + ml 2 ) g X (s) = [ 2 ] ( s ) ml s
Googol 2006
27
(3-11)
or
..
mls 2 ( s) = X ( s ) ( I + ml 2 ) s 2 mgl
(3-12)
Let v = x , then:
( s) ml = 2 V ( s ) ( I + ml ) s 2 mgl Substituting the above equation to the second on in equation (3-10):

( I + ml 2 ) g ( I + ml 2 ) g ( M + m) ( s) s 2 + b + 2 ( s ) s ml( s ) s 2 = U ( s) ml s ml s
(3-13)
(3-14)
The transfer function is obtained after simplification:

ml 2 s ( s) q = U (s) b( I + ml 2 ) 3 ( M + m)mgl 2 bmgl s4 + s s s q q q
(3-15)
In which
q = [( M + m)( I + ml 2 ) (ml ) 2 ]
Assume the system state space equations are:

& X = AX + Bu y = CX + Du
(3-16)
Solve the algebraic equation of &&, & , the solutions are as follow: x &
& & x = x ( I + ml 2 )b m 2 gl 2 ( I + ml 2 ) && = & x x+ u + I ( M + m) + Mml 2 I ( M + m) + Mml 2 I ( M + m) + Mml 2 & & = mlb mgl ( M + m) ml & & x+ u + & = 2 2 I ( M + m) + Mml I ( M + m) + Mml I ( M + m) + Mml 2 The state space equation can then be written as:
0 & x && 0 x = & 0 & & 0 1 ( I + ml 2 )b I ( M + m) + Mml 2 0 mlb I ( M + m) + Mml 2 0 m 2 gl 2 I ( M + m) + Mml 2 0 mgl ( M + m) I ( M + m) + Mml 2
(3-17)
0 0 x I + ml 2 0 x 2 & + I ( M + m) + Mml u 1 0 ml 0 & I ( M + m) + Mml 2
Googol 2006
28
x & x 1 0 0 0 x 0 y= = + u 0 0 1 0 0 &
(3-18)
The first equation of (3-9) is:
& (I + ml )& mgl = ml&x&

2
To rod of even quality there is: 1 I = ml 2 3 So we have:
1 2 2 & x ml + ml & mgl = ml&& 3

After simplification:
& & =
. . ..
3g 3 + && x 4l 4l
(3-19)
Let X = {x, x, , } u ' = x then we have:

& x 0 && 0 x = & 0 0 & & 1 0 0 0 0 0 3g 0 4l 0 x 0 0 x 1 & ' 1 + 0 u 3 0 & 4l
x & x 1 0 0 0 x 0 ' y= = + u 0 0 1 0 0 &
(3-20)
The above state space equation can also be derived by applying the tf2ss command to equation (3-13) in MATLAB.
3.1.1.2 Lagrange Method

Next we are going to derive the system model by Lagrange equation. The Lagrange equation is: L ( q, q ) = T ( q, q ) V ( q, q )
. . .
(3-21)
In which L is the Lagrange operator, q is the generalized coordinates, T denotes the kinetic energy and V denotes potential energy of the system. Googol 2006
29
d L L = fi dt q. qi i
(3-22)
In which i1,2,3n f i is the acting force of system in the its generalized coordinate. In 1-stage IP system, there are 2 generalized coordinates: x and 1 . The system kinetic energy is:
T = TM + Tm
In which TM , Tm denotes the kinetic energy of cart and rod 1 respectively. The kinetic energy of the cart is:
. 2 1 TM = M x 2
The kinetic energy of the rod is:

Tm = Tm + Tm
' ''
In which Tm , Tm
'
''
denote the translational and rotational kinetic energy of the
pendulum rod respectively. Lets assume: xpend x axis coordinate of the rod mass center; ypend y axis coordinate of the rod mass center; There is:
xpend = x lSin ypend = lCos
The kinetic energy of the rod is:

Tm =
'
1 d ( xpend ) 2 d ( ypend ) 2 m ( ) +( ) 2 dt dt Tm =
'' . 2 . 2 1 1 J p 2 = ml 2 2 6
Then we obtained the system kinetic energy:

Tm = Tm + Tm =
' ''
d ( ypend ) 2 1 2 . 2 1 d ( xpend ) 2 m ( ) +( ) + ml dt dt 2 6
The system potential energy is: V = Vm = m g ypend = mglCos
Googol 2006
30
As there is only friction in the coordinates, we obtain:

. d L L =bx . dt
For 1-stage linear IP system, system state variables are:

. . x, , x,
To solve the following state space equation:

. X = AX + Bu ' Y = CX
We need to solve first. Assume:

.. . . ..
..
= f ( x, , x, , x )
Take Taylor expansion in the equilibrium point and linearize, we obtain:
.. . . ..
= k11 x + k12 + k13 x + k14 + k15 x

In which k11 =
f x
.
f | . . .. x x=0, =0, x=0, =0, x=0

|
k12 =
k14 = f
f | . . .. x =0, =0, x =0, =0, x =0

|
k13 =
x = 0 , = 0 , x = 0 , = 0 , x = 0
..
x = 0 , = 0 , x = 0 , = 0 , x = 0
..
k15 =
f x
..
x = 0 , = 0 , x = 0 , = 0 , x = 0
..
Before calculation, lets introduce Mathematica firstMore detail please refer to the website: www.wolfram.com Mathematica is a kind of new machine language that is able to calculate in various areas. It has wide influence in the area of computer calculation since its release in 1988. In this book, we mainly use the symbol calculation and equation solving function of Mathematica. Next we are going to use Mathematica to develop the 1-stage linear IP model: (program file please refer to L1DIP.nb in \GLIP2001\ModelFiles\MathematicaFile
Googol 2006
31
tpend = 1 2 m HHt xpendL ^2 + Ht ypendL ^2L + 1 6 m l^2 H'@tDL^2; Simplify@tpendD; v = m g ypend; lang = tpend v; Simplify@langD;
xpend = x@tD l Sin@@tDD; ypend = l Cos@@tDD;
PRO 3-1 1-stage Linear Inverted Pendulum Modeling Program
Solve@8fa 0 < , 8''@tD<D; add = ''@tD . %;
ldad = '@tD lang; Simplify@ldadD; fa = t ldad @tD lang; Simplify@faD;
k11 = x@tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0 k12 = @tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0 k13 = x'@tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0 k14 = '@tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0 k15 = x''@tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0 g = 9.8; l = 0.25; Simplify@k12D Simplify@k15D
Run programPress ShiftEnterto obtain: k11 = 0 3g k12 = 4l k13 = 0 k14 = 0 3 k15 = 4l Let X = {x, x, , } system state space equation become:
& X = AX + Bu ' y = CX + Du '
. .
Googol 2006
32
Then:
& x 0 && 0 x = & 0 0 && 1 0 0 0 0 0 3g 0 4l 0 x 0 0 x 1 & ' 1 + 0 u 3 0 & 4l
(3-23)
x & x 1 0 0 0 x 0 ' y= = + u 0 0 1 0 0 &
The state space equations derived from Lagrange method can be transformed to the same form with the one by Newtons mechanics. To different input there is different system state space equation. For simple 1-stage linear IP, Newtons mechanics is more convenient and simple, but for multi-stage IP, the programming of Lagrange method is more convenient.
3.1.2 System Parameters

The real system model parameters are as follows: M cart mass 1.096 Kg m rod mass 0.109 Kg b friction coefficient of the cart 0 .1N/m/sec l distance from rod rotation axis center to rod mass center 0.2 5m I rod inertia 0.0034 kg*m*m
Please check if the system parameters match with the real system in MATLAB Smiulink. If not, please use the real system data for calculation.
3.1.3 Real System Model

With the above parameters actual system model will be obtained. The transfer function of pendulum rod angle and cart displacement will be:
( s) 0.02725s 2 = X ( s ) 0.0102125s 2 0.26705
(3-24)
The transfer function of pendulum rod and cart acceleration is: Googol 2006
33
( s) 0.02725 = V ( s ) 0.0102125s 2 0.26705

( s) 2.35655s = 3 U ( s) s + 0.0883167s 2 27.9169s 2.30942
(3-25)
The transfer function of pendulum angle and external force acting on the cart is: (3-26)
The system state space equations with external force as input will be:
& x 0 1 0 && x 0 0.0883167 0.629317 = & 0 0 0 & & 0 0.235655 27.8285 0 x 0 x 0.883167 0 & u + 1 0 & 0 2.35655
x & x 1 0 0 0 x 0 y= = + u 0 0 1 0 0 &
(3-27)
The system state space equations with cart acceleration as input will be:
& x 0 && x = 0 & 0 & & 0 1 0 x 0 & 0 0 0 x 1 ' + u 0 0 1 0 & 0 29.4 0 3 0
x & x 1 0 0 0 x 0 ' y= = + u 0 0 1 0 0 &
(3-28)
Please note that, all controller design and program provided by Googol use the cart acceleration as the system input. In case of torque control mode, users may also use the external force as the input.
3.1.4 System Controllability Analysis

The principle of system controllability analysis is introduced in many books concerning modern control theory so we dont give the detail here. For continuous time system, there is:
The system is controllable if and only if vectors B, AB,..., A n 1 B are linear Googol 2006
34
independent, or the nn matrix [ B M AB M M A n1 B ] is full rank. The system output controllable condition: if and only if the rank of matrix [CB MCAB MCA 2 B M MCA n 1 B M D] equal to the dimension of vector y . Apply the above principle to estimate the system controllability,
0 0 A= 0 0 0 1 B= 0 3 1 0 0 0 0 0 0 29.4 0 0 1 0
1 0 0 0 C= 0 0 1 0 0 D= 0
Put them in the state space equation and calculate in MATLAB: clear; A=[ 0 1 0 0; 0 0 0 0; 0 0 0 1; 0 0 29.4 0]; B=[ 0 1 0 3]'; C=[ 1 0 0 0; 0 1 0 0]; D=[ 0 0 ]'; cona=[B A*B A^2*B A^3*B]; cona2=[C*B C*A*B C*A^2*B C*A^3*B D]; rank(cona) rank(cona2) Or we may use the command ctrb for the controllability and obsv for the observability.
Googol 2006
35
Uc=ctrb(A,B); Vo=obsv(A,C); rank(Uc) rank(Vo) Then there is: ans = 4 ans = 2 We can see that, the system state controllability matrix is full rank and the rank of output controllability matrix equal to the dimension of vector y , so the system is controllable and controller can be designed to stabilized the system. For controllability analysis of state space equations with external force as the input, readers may also follow the above calculations.
3.1.5 System Step Response Analysis

From above state space equations, we could conduct the step response analysis first, type the following command in MATLAB: clear; A=[ 0 1 0 0 0 0 0 0 0 0 0 29.4 B=[ 0 1 0 C=[ 1 0 0 0 1 0 D=[ 0 0 ]'; step(A, B ,C ,D) 0; 0; 1; 0]; 3]'; 0; 0];
The following result is obtained:
Googol 2006
36
Figure 3-3 1-stage Linear Inverted Pendulum Step Response Simulation
We can see that, the step responses of both cart position and pendulum angle diverge.
Googol 2006
37
3.2 1-stage Linear IP Root Locus Control Experiment

The close loop system transient response property is closely related with the system close loop poles. If system gain is adjustable, the place of close loop pole will depends on the selected system gain. From design point of view, some system could easily change the pole place to the required one by varying the system gain. However, when simply changing the system gain cannot reach the system performance requirement, controller design become critical. Some recently most popular used controller design methods are phase-lead, phase-lag and lead-lag compensator.
3.2.1 Root Locus Analysis

We already get IP system open loop transfer function from previous section, let the input be cart acceleration, and output be IP system pendulum rod angle, the transfer function become:
ml ( s) = 2 V ( s ) ( I + ml ) s 2 mgl
Adding disturbance to the system, output pendulum rod angle, the system diagram is as follow:
Figure 3-41-stage Linear IP Close Loop Diagramwith disturbance
Considering input r ( s) = 0 , the diagram structure becomes:
Figure 3-5 1-stage Linear IP Simplified Close Loop Diagramwith disturbance
The system output is:
Googol 2006
38
In which num numerator of plant transfer function;

den denominator of plant transfer function; numlead denlead numerator and denominator of phase-lead block
transfer function;
numlag denlag numerator and denominator of phase-lag block transfer

function;
k controller gain
The close loop transfer function can be obtained by MATLAB command. The real system open loop transfer function is: ( s) 0.02725 = V ( s ) 0.0102125s 2 0.26705 Create a new file in MATLAB, type in the following commands: clear; num=[0.02725 ]; den=[0.0102125 0 -0.26705]; rlocus(num,den) z=roots(num); p=roots(den); The following results are obtained: z= 0 0 p= 5.1136 -5.1136 The system has 2 zeros and 2 poles. One of the poles is positive. Plot the system close loop transfer function root locus as depicted in Figure 3-6. Googol 2006
39
There is a close loop pole on the right half plane. One root locus starts from this point towards left along the real axis and ends at zero. That means however the systems gain change; the locus will stay on the right half plane. The system is always unstable.
Figure 3-6 1-stage Linear IP Open Loop Root Locus
3.2.2 Root Locus Method and Simulation

3.2.2.1 Root Locus Method
1-stage linear IP root locus method can be formulated as the following problem: For system with transfer function: 0.02725 G ( s) = 0.0102125s 2 0.26705 Design a controller to make the system satisfy the following performance criteria: Response time t s = 0.5s (2%) Maximal overshot M p 10%
Root locus design steps are as follows: 1) Calculate the expecting close loop pole s d from maximal overshot M p = e ( /
1 2 )
10%
There is: = 0.591155 approximately let = 0.6 From = Cos ( ) we have:
= 0.938306(radian)
In which is the angle between the real axis negative direction and the line connecting the origin and the pole in the second quadrant.
Googol 2006
40
jw
wn wd 0
Figure 3-7 Performance Index and Root Locus
By ts = 4
0.5s
There is: n = 13.5328 . The expected close loop pole is: 13.5328(Cos jSin ) 2) The uncompensated system root locus is based on the real and imaginary axes, which dont pass the expected close loop pole. The phase-lead compensation is needed, design controller: K ( s) = a s + zc Ts + 1 = aTs + 1 s + p c (a 1)
3) Calculate the phase angle should be provided by phase-lead controller. The sum of phase angle of expected close loop poles and the system original poles is:
13.5328Sin 13.5328Sin 1 G ( s d ) = tan 1 tan + 6.28 13.5328Cos 5.1136 13.5328Cos + 5.1136 4.27676 So the phase angle should be provided by phase-lead controller is: = 3.14 (4.27676) = 1.13517 4) Design the phase-lead compensator, we know that: = 0.938306 The corresponding to maximal a can be calculated by the following equation: 1 = ( ) 2 1 There is: = ( ) = 0.534059 2
Googol 2006
41
Figure 3-8 1-stage Linear IP Root Locus Calculation
Plot corresponding straight line by optimal principle, the zero and pole of phase-lead compensator are:
z c = 6.92214; z p = 26.4568 The open loop transfer function after compensation is: Q = G ( s) K ( s) = 0.02725 K ( s + 6.92214) ( s + 26.4568) 0.0102125s 2 0.26705
5) By gain condition G ( s d ) H ( s d ) = 1 assume unit feedback, there is k = 141.137 ; 6) The system controller is obtained as: 141.137( s + 6.92214) G ( s) = s + 26.4568 7) The previous calculation might be complex manually, users are suggested to do the calculation by computer programming: Write program as follows in Mathematica for the above calculation: in The program file please refer to L1DIPRLocusCompute.nb \GLIP2001\ModelFiles\MathematicaFile.
Googol 2006
42
PRO 3-2 1-stage Linear IP Root Locus Calculation Mathematica Program
3.2.2.2 MATLAB Simulation

Write the following m file in MATLAB to run system simulation: Googol 2006
43
PRO 3-3 1-stage Linear IP Root Locus Calculation MATLAB Program
clear; num=[0.02725]; % Transfer function of the Linear 1 stage den=[0.0102125 0 -0.26705]; % inverted penulum numlead=-6.92214; % Controller Zeros denlead=-26.4568; % Controller Poles [Z,P,K]=tf2zp(num,den); Za=[Z;numlead]; Pa=[P;denlead];
% Add Zeros to the LIP system % Add Poles to the LIP system
[num2,den2]=zp2tf(Za,Pa,K); sys=tf(num2,den2); rlocus(sys); KK=141.137 % compensated system sys2=zpk(Za,Pa,KK*K); sysc=sys2/(1+sys2); t=0:0.005:5; step(sysc,t) % Response of the Close-loop system Run above program to get the result. The root locus after compensation is shown in the following figure:
Figure 3-9 Root Locus after Compensation
We can see from the figure, all root locus are on the left half plane. The system can be stabilized by properly choice of K. The system step response is as follow:
Googol 2006
44
Figure 3-10 Step Response after Compensation
Though the system has good stability but there is certain steady state error and large overshoot. To satisfy the system transient response requirements, users may use the following methods: The first one: increase damping ratio , follow the previous steps (the above program can be easily modified) to redesign until the system performance is satisfied. Users may design by themselves; details wont be described here. The second one: left move the zero of compensator while keeping unchanged, thus to reduce the influence of close loop zeros and poles. Users may use this method to redesign controller. Experienced users may directly adding zeros or poles to the system to control. To pull the root locus from right half plane to the left half plane, adding a zero on the left half planeassume its -7, the system root locus becomes:
Figure 3-11 Root Locus after Adding Zero
We see that, the system root locus are on the left half plane so the system is stabilizable. But for real system, the order of the transfer function molecular should be smaller than the denominator. We need to add a pole far away from the zero. After the Googol 2006
45
pole is added (for example -30), the system root locus will be like:
Figure 3-12 Root Locus after Adding Pole
System root locus are all on the left half plane. By selecting proper system gain(result: 236.1682) the close loop system will be stable. Close loop system step response will be as follow:
Figure 3-13 Close Loop System Step Response
The system can be stabilized in 1 second. The response time is short and overshot is small. The MATLAB program of the previous root locus design method is as follow: Enter MATLAB Simulink toolbox Googol Education Products Open Root Locus Control M Files in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Root Locus Experiments
Googol 2006
46
PRO 3-4 1-stage Linear IP Root Locus Control MATLAB Program
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Googol Linear 1 stage Inverted Pendulum Transfer Function % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; num=[0.02725]; % Transfer function of the Linear 1 stage den=[0.0102125 0 -0.26705]; % inverted penulum numlead=-7; % Controller Zeros denlead=-30; % Controller Poles subplot(2,2,1) rlocus(num,den) [Z,P,K]=tf2zp(num,den); Za=[Z;numlead]; sysaddzero=zpk(Za,P,K); subplot(2,2,2) rlocus(sysaddzero) Pa=[P;denlead]; % Add Poles to the LIP system
% Add Zeros to the LIP system
[num2,den2]=zp2tf(Za,Pa,K); sys=tf(num2,den2); subplot(2,2,3) rlocus(sys); [KK,poles]=rlocfind(sys) % Find the K and Poles of the %KK=141.137 % compensated system sys2=zpk(Za,Pa,KK*K); % Open loop transfer function % of the compensated system subplot(2,2,3) axis([-60 10 -10 10]) rlocus(sys2) % Draw the root locus of the % compensated system sysc=sys2/(1+sys2); % Close-loop transfer function t=0:0.005:5; subplot(2,2,4); step(sysc,t) % Response of the Close-loop system Different control effect will be obtained by varying the controller poles and zeros. User may design and analysis by themselves. Googol 2006
47
3.2.2.3 MATLAB Simulink

MATLAB provides a power graphic simulation tool Simulink Next we are going to model the 1-stage linear IP in Simulink, For first user of MATLAB Simulink, please get familiar with Simulink environment before experiment. Following we will introduce the steps of modeling in detail: 1) Open MATLAB Simulink Click in MATLAB to open the Simulink, Simulink window is as follow:
2) Click to create a new file, namedL1IPModelRLocus PullTransfer Fcn block inContinuous to new window,
Googol 2006
48
3) Rename the above transfer function block as L1IP Transfer Fcn, right click mouse, set BackGround Color Cyan and double click the block, open as block parameters window:
4) PullZero-Poleblock from Simulink to the window as the controller. Double click the block, set zeros, poles and controller gain KK by above program.
Googol 2006
49
5) ConnetControllerblock andL1IP L1IP Transfer Fcnblock.
6) Pull Sum block from Math Operations block in Simulink to L1IPModelRLocuswindow and double click the block to change it as followset++as+- :
7) Pull Step block from Sourses block in Simulink to the L1IPModelRLocuswindow and double click the block to set step signal parameter:
Googol 2006
50
8) PullScope fromSinksin Simulink toL1IPModelRLocuswindow as follow:
9) Connect each block as follow which constitutes a close loop control system.
10) ClickSimulationmenu and selectSimulation Parameters,
Open the following window:
Googol 2006
51
Set options like Simulation time Solver options the above window. Set and in simulation step size as 0.005s. block to view the simulation 11) Click to run simulation, double click Scope result:
Figure 3-14 1-stage Linear IP Root Locus Method Simulation Result (First Order Controller)
If the result exceed the visible range, click to resize. 12) The system track step signal well but there is still certain steady state error. Varying the controller poles and zeros can get different control effect. Users may select the best one after varying the parameters several times. For example, adding a pole and a zero to second order lead-lag controller. Details may refer to related text books. 13) PullManual SwitchinSimlulink\Signal Routingto the window:
Googol 2006
52
14) Copy a controller block to the window and reset parameters:
15) The final root locus simulation block will be as the following figure: Reference model: enter MATLAB Simulink tool boxGoogol Education Products, openRoot Locus Control SimulinkinInverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Root Locus Experiments
Figure 3-15 1-stage Linear IP Root Locus Simulation Model
Double click Manual Switch select the lower path, click to run and get to the following result:
Googol 2006
53
Figure 3-16 1-stage Linear IP Root Locus Control Simulation Result Second Order Controller
We can see, the steady state error decrease but overshoot increase. Users may analyze the reason and improve controller by themselves.
3.2.3 Root Locus Real Time Control Experiment

Please install Googol MATLAB real control software before experiment. Details please refer to software user manual. 1) Open MATLAB and Simulink as follow:
2) OpenGoogol Educational Products in Simulink and selectLinear Inverted Pendulum inInverted Pendulum block, chooseLinear 1 Stage Inverted Pendulum Root Locus Controlon the right window. 3) Open 1-stage linear IP root locus control window:
Enter MATLAB Simulink tool boxGoogol Education Products, openRoot Locus Control DemoinInverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Root Locus Experiments
Googol 2006
54
Figure 3-17 1-stage Linear IP Root Locus Real Control Block
4) Click build, after successful build there will be information notice in to MATLAB command window as followIf the control interface structure is not changed, not further build is needed after run once:
If not successful, please refer to Googol Technology MATLAB Real Time Control Software User Manual to find out the reason. 5) Click
to connect;
6) Click to run; 7) When program is running, the sound of motor servo on will be heard,
After motor servo on, no one is allowed to stand within the IP reachable area. Shut down the electrical box when there is any thing abnormal and check the status of IP before experiment each time.
8) Put the pendulum rod to vertical upward manually, the program will automatically run stabilizing control. 9) Double clickScopeto view the experiment data:
Googol 2006
55
Figure 3-18 1-stage Linear IP Root Locus Control Resultequilibrium
10) Double clickManual Switchto switch controller, observe the experiment data and record. The IP system response under certain disturbances can be observed:
Figure 3-191-stage Linear IP Root Locus Control Result(with disturbance
11) Users can also save the experiment data as file or output to MATLAB Workspaceas follow. Details please refer to MATLAB references. Googol 2006
56
12) Compare the experiment data with simulation result. 13) Users may also use other root locus means for controller design.
3.2.4 Experiment Result and Report

Please record the calculation steps, simulation and experiment results and finish the experiment report. The sample experiment report is as follow:
Googol 2006
57
Table 3-1 Linear IP Root Locus Realtime Control Experiment Report
Linear IP Root Locus Realtime Control Experiment Report

Student Name: Department: Class Name: Student Number: Date:
Objective
Equipment
Theory
Simulation and Results
Experiment Results
Experiment Results Analysis
Comments
Googol 2006
58
3.3 1-stage Linear Inverted Pendulum Frequency Response

The System response to sinusoidal signal is called frequency response. In frequency response method, we change the input signal frequency in certain range to study the system response. Frequency response can be analyzed in the following 2 convenient way: One is bode plot, which use two separate figure: one plots the relation between magnitude and frequency and another plots the relation between phase and frequency; the other is polar coordinates plot which gives the locus of vector G ( j )
G ( j ) when
varies from 0 to infinity. The polar coordinates plot is also called Nyquist plot. The Nyquist stability criterion helps us decide the close loop system absolute stability and relative stability according to the open loop frequency response property information.
3.3.1 Frequency Response Analysis

We already have 1-stage linear IP physical model previously, the real system open loop transfer function is: ( s) 0.02725 = V ( s ) 0.0102125s 2 0.26705 In which the input is cart acceleration V (s ) and output is the pendulum rod angle (s ) . Draw system Bode plot and Nyquist plot in MATLAB. The command for Bode plot is: Bode(sys) The command for Nyquist plot is: Nyquist(sys) Type the following command in MATLAB:
Googol 2006
59
clear; num=[0.02725]; den=[0.0102125 0 -0.26705]; z=roots(num); p=roots(den); subplot(2,1,1) bode(num,den) subplot(2,1,2) nyquist(num,den) The following result is obtained: z= Empty matrix: 0-by-1 p= 5.1136 -5.1136
Figure 3-20 1-stage Linear IP Bode Plot
Googol 2006
60
Figure 3-21 1-stage Linear IP Nyquist Plot
We could see that the system has no zero but 2 poles, one of which is on the right half s plane. According to Nyquist stability criterion, the sufficient and necessary condition of close loop system be stable is: when change from to + , the open loop transfer function G ( j ) encircle the point -1 p times, in which p is the number of poles of open loop transfer function on the right half plane. For 1-stage linear IP, From Figure 3-21 the open loop transfer function has a pole on the right half S plane, so G ( j ) needs to encircle the point -1 once. We could see the system Nyquist plot does not encircle the point -1 once, so the system is unstable. Further controller design is required to stabilize the system.
3.3.2 Frequency Response Design and Simulation

1-stage linear IP frequency response design can be formulated as the following problem: Consider a unit negative feedback system with open loop transfer function: G ( s) = ( s) 0.02725 = V ( s ) 0.0102125s 2 0.26705
Design controller Gc (s ) , let system static position constant be 10, phase margin be 50 o gain margin larger or equal to 10dB By above requirements, the controller design steps are as follow: 1) Select controller. We already get the system Bold plot previously. It can be observed that the design criterion could be satisfied by adding a phase-lead controller. Assume it be: Googol 2006
61
Ts + 1 = Kc Gc ( s ) = K c Ts + 1
1 T 1 s+ T s+
The controlled system has open loop transfer function Gc ( s )G ( s ) Assume G1 ( s ) = KG ( s ) = In which K = K c 2) Calculate control gain K according to steady state error requirement:
1 ) 0.02725 T = 10 K p = l im Gc ( s )G ( s ) = l im K c s 0 s 0 1 0.0102125s 2 0.26705 (s + ) T (s +
0.02725 K 0.0102125s 2 0.26705
There is: K c = 98 = K Then we have: G1 ( s ) = 0.02725 98 0.0102125s 2 0.26705
3) Bode plot of G1 ( s ) in MATLAB:
Googol 2006
62
Figure 3-22 1-stage Linear IP Bode Plot and Nyquist Plot after Adding Control Gain
4) We can see that the system phase margin is 0 o . According to design criterion, system phase margin should be 50 o which means 50 o more is needed. Adding phase-lead controller will also change the gain curve of Bode plot. The gain crossover frequency will move rightward. Thus we have to compensate for the phase lag increment of G1 ( j ) as a result of increasing gain crossover frequency. Assume the maximal required phase lead m is approximately 55 o . Since Sin m = We will get: = 0.0994 5) After determining the damping system, the lead controller corner frequency Googol 2006
63
1 1+
will also be determined = 1 / T and 1 /(T ) . It can be observed that the maximal phase lead angle m is on the geometric center of the corner frequency, which means = 1 /( T ) . At point = 1 /( T ) , as it contains the term of (Ts + 1) /(Ts + 1) , the gain increment will be:
1+ j =
=1 /( T )
1 + jT 1 + jT
= 1 1+ j
And
1 0.0994
= 10.0261 dB
As
G1 ( j ) = 10.0261 dB corresponds to = 28.5 rad/s we select this
frequency as new gain crossover frequency c this frequency corresponds to
= 1 /( T ) , that is c = 1 /( T ) , then
1 = c = 8.9854 T 1 = c = 90.3965 T 6) The controller is established: Gc ( s ) = K c Ts + 1 s + 8.9854 = Kc Ts + 1 s + 90.3965 K
Kc =
= 985.9155
7) The root locus and Nyquist plot after control are as follow:
Enter MATLAB Simulink tool box Googol Education Products . Open Frequency Response Control M Files in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Frequency Response Experiments
Googol 2006
64
PRO 3-5 1-stage Linear IP Frequency Response Control MATLAB Program
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Googol Linear 1 stage Inverted Pendulum Frequence Response % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; num=[0.02725]; num2=98*[0.02725]; den=[0.0102125 0 -0.26705]; [z,p,k]=tf2zp(num,den); subplot(2,1,1); bode(num2,den) subplot(2,1,2);nyquist(num2,den) za=[z;-8.9854]; pa=[p;-90.3965]; kk=985.9155; sys=zpk(za,pa,k*kk); % za=[z;-8.9854;-2]; pa=[p;-90.3965;-0.1988]; k=985.9155; % sys=zpk(za,pa,k); figure subplot(2,1,1) bode(sys) subplot(2,1,2) nyquist(sys); figure sysc=feedback(sys,1); t=0:0.005:5; impulse(sysc,t) We can see from Bode plot that system has satisfactory phase and gain margin. From Nyquist plot, the curve encircle -1 once, thus compensated system is stable.
Googol 2006
65
Figure 3-23 1-stage Linear IP Bode Plot and Nyquist Plot after ControlFirst Order Controller
System step response will be obtained as follow:
Figure 3-24 Step Response after Frequency Response ControlFirst Order Controller
Googol 2006
66
The system can be stabilized in 1 second after disturbed. However, the overshoot is large.
to 8) Open L1dofFreq.mdl run simulation in MATLAB Simulink We ignore the steps of all examples from now on, details user may refer to previous chapter . Enter MATLAB Simulink tool box Googol Education Products , open Frequency Response Control Simulink in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Frequency Response Experiments
Figure 3-25 1-stage Linear IP Frequency Response Control Simulation
Double clickController1 to set controller parameter:
Click to obtain the following simulation result: Googol 2006

67
9) It can be observed that there is certain steady state error. To achieve fast response as well as small steady state error, we use lag-lead controller (by applying lag-lead controller, low frequency gain increase, steady state error decrease and the system bandwidth as well as stability margins will both increase). Let the lag-lead controller be:
1 1 )( s + ) T1 T2 Gc (s) = K c 1 ( s + )( s + ) T1 T2 (s +
10) The detailed lag-lead controller design steps user may refer to related textbooks. The controller is: 1 1 )( s + ) T1 T2 s + 8.9854 s+2 Gc ( s ) = K c = 980 1 s + 90.3965 s + 0.1988 ( s + )( s + ) T2 T1 (s + The position error constant is obtained:
K p = l im Gc ( s )G ( s )
s 0
= l im 980
s 0
s + 8.9854 s+2 0.02725 s + 90.3965 s + 0.1988 0.0102125s 2 0.26705
= 100.6 This increased a lot than the one of phase-lead control. As zero -2 is close to pole -0.1988, the effect on phase margin is relative small. System Bode and Nyquist plot after lag-lead will be:
Googol 2006
68
Figure 3-26 Bode and Nyquist Plot after Frequency Response ControlSecond Order
SetController2as:
Googol 2006
69
Run simulation and obtain:
Figure 3-27 Step Response after Frequency Response Control SimulationSecond Order
It can be observed that system steady state error is small.
3.3.3
1-stage Linear IP Frequency Response Experiment
1) Ener MATLAB Simulink tool boxGoogol Education Products, open Frequency Response Control Demo inInverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Frequency Response Experiments Steps please refer to previous root locus control experiment:
Googol 2006
70
Figure 3-28 1-stage Linear IP Frequency Response Real Time Control Diagram
2) Click Manual Switch select controllers like Contrller1Controller2 to or 3) Double click Controller2, set parameters.
4) Click
to build and click
to connect after build.
5) Power on electrical box. 6) Click to run program. 7) When the motor servo on, pull the pendulum rod to vertically upward by hand. Release the rod when program started to control. Note that, the frequency response method only control the rod angle but not cart position, the cart may Googol 2006
71
reach the limit switch and stop control. Users are suggested to control the position of the cart by hand to avoid reaching the limit switch. 8) Double clickScopeto observe the result:
Figure 3-29 Frequency Response Real Time Control ResultFirst Order Controller
Please analyze in detail the relationship between the system actual stabilization time (about 0.25s as shown above) and design index. 9) Different performance index will get different controller parameters. Observe control effect with different parameters. 10) Click Manual Switch switch controller, the system control performance to is as follow:
Googol 2006
72
Figure 3-30 Frequency Response Real Time Control ResultSecond Order Controller
Please analysis the difference of control effect between this controller and the previous mentioned one.Stabilization time is about 0.5 second.

Record the experiment result and finish report.
Googol 2006
73
3.4 1-stage Linear IP PID Control Experiment

The objective of this experiment is to let users comprehend and master the PID control principle and apply to 1-stage linear IP control. PID control does not require precise analysis of system model so we can set control parameters according to experimental experience.
3.4.1 PID Control Analysis

Classical control theory mainly focused on the research of single input single output (SISIO) systems. The precise control plant model is often required in controller design. Thus PID controller is widely used in control applications for its simple structure, easy tuning and independence from system model. First of all, for IP system outputs pendulum rod angle whose equilibrium point is vertically upward. System control diagram will be as follow:
Figure 3-31 1-stage Linear IP Close Loop System Diagram
In which KD(s) is controller transfer function, G (s ) is control plant transfer function Considering input r ( s) = 0 , the structure diagram can be easily transformed to:
Figure 3-32 1-stage Linear IP Close Loop System Simplified Diagram
The system output is:
Googol 2006
74
num G ( s) den y ( s) = F (s) = F (s) (numPID)(num) 1 + KD ( s )G ( s ) 1+ (denPID)(den) num(denPID) = F ( s) (denPID)(den) + (numPID)(num)
In which
num numerator of control plant transfer function
den denominator of control plant transfer function

numPID numerator of PID controller transfer function denPID denominator of PID controller transfer function
The system performance can be obtained by analyzing previous equations. The transfer function of pendulum rod and cart acceleration can be obtained from equation (3-13): ( s) ml = 2 V ( s ) ( I + ml ) s 2 mgl PID controller transfer function will be:
2 K I K D s + K p s + K I numPID = = KD ( s ) = K D s + K p + s s denPID
The PID parameters should be carefully tuned to achieve satisfactory control effect. We only discussed the pendulum rod angle previously, then how will the cart position vary in control process? Cart position output is: X ( s) = V ( s) s 2 The cart position can be calculated by double integrate the control variable v .
3.4.2 PID Control parameters and Simulation

PID parameter set could refer to Modern Control Theory Chapter 10 PID Control and Robust Control. We use the following method to decide PID control parameters. By actual system physical model: ( s) 0.02725 = V ( s ) 0.0102125s 2 0.26705 Googol 2006
75
Setup the following 1-stage linear IP model in Simulink: Enter MATLAB Simulink real time control tool box Googol Education Products, openPID Control SimulinkinInverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ PID Experiments .
Figure 3-33 1-stage Linear IP PID Control MATLAB Simulation Model
In which PID Controller is the masked PID controller, double click the module to open parameters window,
Figure 3-34 PID Set Parameter Window
First set PID controller be P controller, let K p = 9, K i = 0, K D = 0 , the following simulation result is obtained:
Figure 3-35 1-stage Linear IP P Control Simulation ResultKp9
We can see from the figure that the system output diverges. Increase control gain, Googol 2006
76
K p = 40, K i = 0, K D = 0 , the simulation result is as follow:
Figure 3-36 1-stage Linear IP Proportional Control Simulation Result Kp40
From the figure, close loop system keeps vibrating and the period is about 0.7s. To eliminate system vibration, increase differential control gain K D , Let K p = 40, K i = 0, K D = 4 , the simulation result is:
Figure 3-37 1-stage Linear IP PD Control Simulation ResultKp40Kd4
From the figure, system stabilization time is long, about 4 second which is two vibration periods. So we increase differential control gain K D , Let: K p = 40, K i = 0, K D = 10 Simulation result will be:
Googol 2006
77
Figure 3-38 1-stage Linear IP PD Control Simulation ResultKp40Kd10
From the figure, system is stabilized in about 1.5 second, but there is certain steady state error. To eliminate the steady state error, lets increase the integral gain K i , Let: K p = 40, K i = 20, K D = 10 The following simulation result will be obtained:
Figure 3-39 1-stage Linear IP PID Control Simulation ResultKp40Ki20Kd4
From the simulation result, system can be well stabilized. But because of the integral factor, it is evident that the stabilization time increased. Double clickScope1, the cart position output is:
Figure 3-40 1-stage Linear IP PD Control Simulation Resultcart position curve
Googol 2006
78
We can see that, PID controller is SISO system. It only controls the pendulum rod but not the cart position. The cart might move towards one direction only. We can also simulate by programming M file. Enter MATLAB Simulink tool boxGoogol Education Products, openPID Control M Files in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ PID Experiments
PRO 3-6 1-stage Linear IP PID Control MATLAB Simulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Googol Linear 1 stage Inverted Pendulum PID Control % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; num=[0.02725]; den=[0.0102125 0 -0.26705]; kd=10 %pid close loop system pendant response for impluse signal k=40 ki=10 numPID= [ kd k ki ]; denPID= [ 1 0 ]; numc= conv ( num, denPID ) denc= polyadd ( conv(denPID, den ), conv( numPID, num ) ) t = 0 : 0.005 : 5; figure(1); impulse ( numc , denc , t ) The simulation result is as follow:
Figure 3-41 1-stage Linear IP PID Control MATLAB Simulation Resultimpulse disturbance
Googol 2006
79
3.4.3 PID Control Experiment

Real time control experiment is done in MATALB Simulink. Please read carefully the user manual before conducting experiment.
Before conducting MATLAB real time control experiment, please check IP system mechanical structure and electrical wiring to ensure the safety.
3.4.3.1 Experiment steps in MATLAB experimental software

1) Open 1-stage linear IP PID Control diagram as shown below: Enter MATLAB Simulink tool boxGoogol Education Products, openPID Control Demo in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ PID Experiments
Figure 3-42 1-stage Linear IP MATLAB Real Time Control Diagram
2) Double clickPIDblock to set PID parameters, as shown below:
Googol 2006
80
Set PID parameters as the simulation results, double clickOKto save parameters. 3) Click to build, click to connect computer and IP system after successful built. 4) Click to run. If motor is not servo on, users may refer to related chapter in IP user manual to solve the problem. Pull the pendulum rod vertically upward slowly by hand and release when the program started to control. When the cart is getting reach the limit switch, touch the rod to make it move backward. 5) Experiment result is as follow:
Figure 3-43 1-stage Linear IP PID Control Experiment Result 1
Googol 2006
81
From the figure, IP system is well stabilized. The pendulum rod angle is about 3.14 (rad). The same with simulation result, PID controller is not able to control the cart position, cart will move slightly along the sliding shaft. Under fixed disturbance, the cart position and pendulum rod angle will be as in the figure below:
Figure 3-44 1-stage Linear IP PID Control Experiment Result 2with disturbance
We can see, system balance well under disturbance and will back to the equilibrium point soon when disturbance stopped. Change the PID parameters, like:
Googol 2006
82
Observe the control result. We can see that system settling time decrease but small vibration occasionally appear during equilibrium time.
Figure 3-45 1-stage Linear IP PID Control Experiment Result 3change PID parameters
3.4.4 Experimental Result and Report

Record the calculation steps, simulation and experiment result and finish the report.
Googol 2006
83
3.5 State Space Pole Placement Control Experiment

Classical control theory mainly focused on the research of SISIO systems. The precise control plant model is often required in controller design. Modern control theory mainly uses modern mathematical tools to expand the control object of classical control theory to multi-input multi-output (MIMO) systems. Pole placement method places the close loop pole of MIMO system to the expected place by designing state feed back controller, thus to satisfy system transient and steady state performance requirement. We already have IP systems dynamic model previously. Next we are going to design controller by pole placement method and apply to 1-stage linear IP system.
3.5.1 State Space Analysis

For control system X = AX + Bu In which X State vector n dimension u Control vector A n n constant matrix B n 1 constant matrix Select control signal be: u = KX
Figure 3-46 State Feedback Close Loop Control Diagram
Solve the equation, we get x(t ) = ( A BK ) x(t ) The solution is: x(t ) = e ( A BK ) t x(0)
Googol 2006
84
We can see, if system state is controllable, for arbitrary initial state, x(t ) will go to 0 when time goes to infinity as long as K is properly chosen. Pole placement design steps: 1) Verify system controllability condition. 2) By characteristic polynomial of matrix A :
sI A = s n + a1 s n 1 + + a n 1 s + a n
Decide values of a1 , a 2 , a n . 3) Determine matrix T that transform the state space equations to controllable canonical form:
T = MW
In which
M is the controllability matrix

M = B M AB M M An 1 B a n 1 a n2 W = M a1 1 a n 2 L a1 a n 3 1 0 M M M 1 L 0 0 L 0
]
1 0 M 0 0
4) Formulate expected polynomial by expected eigenvalues: ( s 1 )( s 2 ) L ( s n ) = s n + 1 s n 1 + + n 1 s + n And decide the value of 1 , 2 , n . 5) State feed back gain matrix K is decided by following equation: K = [ n a n M n 1 a n 1 MLM 2 a 2 M 1 a1 ]T 1
3.5.2 Pole Placement and Simulation

We already have 1-stage linear IP state space model. The state space equations using cart acceleration as input are:
Googol 2006
85
& x 0 && x = 0 & 0 & & 0
0 x 0 & 0 0 0 x 1 ' + u 0 0 1 0 & 0 29.4 0 3 0
x & x 1 0 0 0 x 0 ' = = + y u 0 0 1 0 0 &
There are:
0 0 A= 0 0 0 1 B= 0 3 1 0 0 0 0 0 0 29.4 0 0 1 0
1 0 0 0 C= 0 0 1 0 0 D= 0
1-stage linear IP pole placement problem becomes: Design controller for above system. The system settling time is required to be short (about 3s) and the required damping ratio is about = 0.5 . Next we are going to use 4 methods to calculate feed back gain matrix K. Method 1: By previous pole placement steps. 1) Checking system controllability. From section 3.1.4 System Controllability Analysis, the system state controllability matrix rank equal to system state dimension(4), the system output controllability matrix rank equal to the dimension of system output vector(2), so the system is controllable.
Googol 2006
86
State Variables Control Force u

.
IP/Cart System
.
.
x = Ax + Bu
k1
k2 k3 k4
Figure 3-47 Inverted Pendulum Pole Placement Principle Diagram
2) Compute eigenvalues. According to the performance requirement with certain marginassume the settling time is 2s we select the expected close loop poles as s = i (i = 1,2,3,4) , in ,
which:
1 = 10, 2 = 10, 3 = 2 + j 2 3 , 4 = 2 j 2 3
In which 3 , 4 are a pair of dominant poles with = 0.5, n = 4 ; 1 , 2 locate on the left of dominant close loop poles thus have less impact. The expected characteristic polynomial is: ( s 1 )( s 2 )( s 3 )( s 4 ) = ( s + 10)( s + 10)( s + 2 2 3 j )( s + 2 + 2 3 j ) = s 4 + 24s 3 + 196s 2 + 720s + 1600 There is:
1 = 24, 2 = 196, 3 = 720, 4 = 1600

From system characteristic polynomial:
0 0 s 1 0 s 0 0 sI A = 0 0 1 s 0 0 29.4 s = s 4 29.4 s 2
So we have: a1 = 0, a 2 = 29.4, a3 = 0, a 4 = 0
Googol 2006
87
System feed back gain matrix is: K = [ 4 a 4 M 3 a3 M 2 a 2 M 1 a1 ]T 1 3) Determine matrix T that transform the state equation to controllable canonical form:
T = MW
In which:
M = B M AB M A 2 B M A 3 B 0 1 = 0 3 1 0 0 0 0 3 0 88.2 0 88.2 0 0
a3 a W = 2 a1 1 So we have:
a2 a1 1 0
a1 1 0 0
29.4 1 0 29.4 0 0 = 1 0 0 0 0 1 1 0 3 0 0 1 0 3
0 1 0 0
1 0 0 0
0 29.4 0 29.4 T = MW = 0 0 0 0
T 1
0 0.0113 0 0.034 0 0.034 0 0.0113 = 0 0 0.3333 0 0 0 0.3333 0
4) The state feed back gain matrix K is:

K = [ 4 a 4 M 3 a3 M 2 a 2 M 1 a1 ]T 1 0 0.0113 0 0.034 0 0.034 0 0.0113 = [1600 0 M 720 0 M196 + 29.4 M 24 0] 0 0 0.3333 0 0 0 0.3333 0
= [ - 54.4218 - 24.4898 93.2739 16.1633]
The control variable becomes:
= KX = 54.4218 x + 24.4898 x - 93.2739 - 16.1633

Above calculation can be conducted by MATLAB programming. Googol 2006
88
Run and obtain the following result:
Figure 3-48 Pole Placement Simulation Result
We could see, IP system can be stabilized in 2s after fixed disturbance to satisfy the design criterion.
PRO 3-7 1-stage Linear IP State Space Pole Placement MATLAB Program 1
Googol 2006
89
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Googol Linear 1 stage Inverted Pendulum Poles Placement Method1 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; A=[ 0 1 B=[ 0 1 0 0 0; 0 0 0 3]'; 0; 0 0 1 0]; 0; 0 0 0 1; 0 0 29.4 0];
C=[ 1 0 0 D=[ 0 0 ]';
J=[ -10 0 0 0; 0 -10 0 0 0 -2+2*sqrt(3)*i]; pa=poly(A);pj=poly(J); M=[B A*B A^2*B A^3*B]; W=[ pa(4) pa(2) pa(3) 1
0; 0
-2-2*sqrt(3)*i
0;
pa(2) 1; pa(3) pa(2) 0 0; 1 0
1 0
0; 0];
T=M*W; K=[pj(5)-pa(5)
pj(4)-pa(4) pj(3)-pa(3)
pj(2)-pa(2)]*inv(T)
Ac = [(A-B*K)]; Bc = [B]; Cc = [C]; Dc = [D]; T=0:0.005:5; U=0.2*ones(size(T)); Cn=[1 0 0 0]; Nbar=rscale(A,B,Cn,0,K); Bcn=[Nbar*B]; [Y,X]=lsim(Ac,Bcn,Cc,Dc,U,T); plot(T,X(:,1),'-'); hold on; plot(T,X(:,2),'-.'); hold on; plot(T,X(:,3),'.'); hold on; plot(T,X(:,4),'-') legend('CartPos','CartSpd','PendAng','PendSpd')
Enter MATLAB Simulink tool boxGoogol Education Products, openPoles Control M File1 in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage
Googol 2006
90
IP Experiment\ Poles Experiments Method 2: Users may also use the following method for pole placement calculation: is Eigenvalues of matrix ABK the solution of equation Is ( A BK ) = 0 :
s 0 0 0 0 0 0 0 s 0 0 0 0 s 0 0 0 0 s 0 1 0 0 0 0 0 0 1 + [k1 0 0 1 0 0 a 0 b
k2
k3
k4 ] = 0
The above equation can be rewritten as: s 4 + (k 2 + bk 4 ) s 3 + ( a + k1 + bk 3 ) s 2 ak 2 s ak1 = 0 The expected system eigenvalue can be achieved by selecting proper feed back gain coefficient k1 , k 2 , k 3 , k 4 . Let the 4 engenvalues 1 , 2 , 3 , 4 be roots of the algebraic equation, there are: s 4 (1 + 2 + 3 + 4 ) s 3 + (12 + 2 3 + 3 4 + 4 1 + 13 + 2 4 ) s 2 (12 3 + 2 3 4 + 13 4 + 4 1 2 ) s + 1 2 3 4 = 0 Compare two equations and we get k 2 + bk 4 = (1 + 2 + 3 + 4 ) a + k1 + bk 3 = 1 2 + 2 3 + 3 4 + 4 1 + 13 + 2 4 ak 2 = (1 2 3 + 2 3 4 + 13 4 + 4 1 2 ) ak1 = 1 2 3 4 If the given 1 , 2 , 3 , 4 are real numbers or conjugate complex numbers, then the right hand side of the equation is real number which can be used to solve real numbers k1 , k 2 , k 3 , k 4 . Assign eigenvalues as following
1 2 3 4 = 2 2 3 j -10, -10
And a = 29.4, b = 3 Googol 2006
91
The equations of k1 , k 2 , k 3 , k 4 can then be written as: k 2 + 3k 4 = 24 29.4 + k1 + 3k 3 = 196 29.4k 2 = 720 29.4k1 = 1600 The solutions are k1 = 54.4218 k 2 = 24.4898 k 3 = 93.2739 k 4 = 16.1633
The control force acting horizontally on the cart is:
= KX = 54.4218 x + 24.4898 x - 93.2739 - 16.1633

We could see, the result is the same from the one in method 1.
PRO 3-8 1-stage Linear IP State Space Pole Placement MATLAB Program 2
Googol 2006
92
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Googol Linear 1 stage Inverted Pendulum Poles Placement Method2 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; syms a s b k1 k2 k3 k4; A=[ 0 1 0 0 0 0 0 0 0 0 0 a B=[ 0 1 0 0; 0; 1; 0]; b]';
SS=[ s 0 0 0; 0 s 0 0; 0 0 s 0; 0 0 0 s]; K=[k1 k2 k3 k4]; J=[ -10 0 0 0; 0 -10 0 0; 0 0 -2-2*sqrt(3)*i 0; 0 0 0 -2+2*sqrt(3)*i]; ans=A-B*K; P=poly(ans) PJ=poly(J)
Enter MATLAB Simulink tool boxGoogol Education Products, openPoles Control M File2 in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Poles Experiments
Method 3: By Ackermann formula Feed back gain matrix determined by Ackermann formula K = [0 0 L 0 1] B M AB M M A n 1 B ( A)
1
In which
( A) = A 3 + 1 A 2 + 2 A + 3 I
This can be easily calculated by MATLAB, program as follow:
Googol 2006
93
PRO 3-9 1-stage Linear IP Pole Placement MATLAB Program 3Ackermann formula
clear; A=[ 0 0 0 0 B=[ 0 1 0 0 0 1 0 0; 0 0; 0 1; 29.4 0]; 0 3]';
M=[B A*B A^2*B A^3*B]; J=[ -10 0 0 0; 0 -10 0 0; 0 0 -2-2*sqrt(3)*i 0; 0 0 0 -2+2*sqrt(3)*i]; phi=polyvalm(poly(J),A); K=[ 0 0 0 1]*inv(M)*phi
Enter MATLAB Simulink tool boxGoogol Education Products, openPoles Control M File3 in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Poles Experiments Run and get
K= -54.4218 -24.4898 93.2739 16.1633
We could see, the result is the same with those from the previous two. Method 4: Directly use MATLAB pole placement function for calculation. [K,PREC,MESSAGE] = PLACE(A,B,P) Enter MATLAB Simulink tool boxGoogol Education Products, openPoles Control M File4 in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Poles Experiments
Googol 2006
94
PRO 3-10 1-stage Linear IP Pole Placement MATLAB Program 4Ackermann formula
clear; A=[ 0 1 0 0; 0 0 0 0; 0 0 0 1; 0 0 29.4 0]; B=[ 0 1 0 3]'; P=[-10-0.0001*j,-10+0.0001*j,-2-2*sqrt(3)*j,-2+2*sqrt(3)*j]; K=place(A,B,P); To match the format of function place(), change poles -10,-10 to
- 10 - 0.0001j, - 10 + 0.0001j , the additional imaginary number is small thus can be ignored. Run and get the following result: K= -54.4218 -24.4898 93.2739 16.1633
Both 4 methods obtain the same result. Use the previous result for simulation in MATLAB Simulink. Open 1-stage linear IP simulation model: Enter MATLAB Simulink real time control tool box Googol Education ,open Poles Control Simulink in Inverted Pendulum\Linear Inverted Products Pendulum\Linear 1-Stage IP Experiment\ Poles Experiments
Figure 3-49 1-stage Linear IP State Space Pole Placement MATLAB Simulink Simulation
is In which GL1IP State-Space the state space model of 1-stage linear IP, double click to open the following window:
Googol 2006
95
Double clickController1block to set parameters of state feed back matrix K:
Input the calculated K value to the above window. Run simulation and get the following result:
Googol 2006
96
Figure 3-50 1-stage Linear IP State Space Pole Placement MATLAB Simulation Result
It can be observed that with the existence of certain disturbance, system can be stabilized again in 3 seconds. Users may revise the expected performance criterion for new pole placement. Set new control parameters inController2block and clickManual Switchto switch control signal toController2
3.5.3 Pole Placement Control Experiment

Experimental steps: 1) Enter MATLAB Simulink \\matlab6p5\toolbox\GoogolTech\ InvertedPendulum \ Linear Inverted Pendulum, catalog, open 1-stage linear IP state space pole placement control program as follow: Enter MATLAB Simulink tool box Googol Education Products open Poles , Control Demo in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ Poles Experiments
Googol 2006
97
Figure 3-51 1-stage Linear IP State Space Pole Placement Real Control Diagram
2) ClickControllerblock to set controller parameters, input the parameters that obtain better simulation results to the block:
ClickOKto finish. 3) Click to build and click to connect computer and IP system after build finish. 4) Click to run. If motor is not servo on, users may refer to related chapter in IP user manual to solve the problem. Pull the pendulum rod vertically upward slowly by hand and release when the program started to control. 5) Double click Scope to observe experimental results as below:
Googol 2006
98
Figure 3-52 1-stage Linear IP State Space Pole Placement Real Control Result (equilibrium)
System is in equilibrium within small range of vibration. The cart vibration range is about 4 10 3 m, the pendulum vibration range is about 0.05rad. Note: different control parameters will have different result. System response under certain disturbance is depicted in below figure:
Figure 3-53 1-stage Linear IP State Space Pole Placement Real Control Result(in disturbance)
Googol 2006
99
From the figure, system settling time is about 3 seconds. The design criterion is satisfied.

Record the calculation steps, simulation and experiment result and finish the report. Note: Users may add state observer in pole placement experiment for further design, simulation and experiment.
Googol 2006
100
3.6 Linear Quadratic LQR Optimal Control Experiment

The content of this experiment is to allow users comprehend and master linear quadratic LQR optimal control principle and method, and apply to 1-stage linear IP system.
3.6.1 Linear Quadratic LQR Optimal Control Principle and Analysis

The linear quadratic LQR optimal control principle is, by system equations:
X = AX + Bu
Determine matrix K that gives the optimal control vector: u(t)=-K*x(t) Such that the performance index is minimized:
J = ( X * QX + u * Ru)dt
0
(4-1) (4-2)
(4-3)
In which
Qpositive definite (or semi-positive definite) hermitian or real symmetric matrix. Rpositive definite hermitian or real symmetric matrix.
Figure 3-54 Optimal LQR Control Diagram
The second term on the right of the equation is introduced in concern of energy loss. Matrix Q and R determine the relative importance of error and energy loss. Assume that the control vector u(t) is unbounded. For linear systems:
. X = AX + Bu Y = CX Select Q and R according to expected performance criterion, the matrix K can be easily obtained by MATLAB command lqr. K=lqr(ABQR) Change Q value will get different system response. The system will be more robust to disturbance and the settling time will be shorter if Q is larger (in certain range). The influence of Q will be described in experiment result analysis. Detailed theory of optimal LQR control please refer to related textbooks about
Googol 2006
101
modern control theory.
3.6.2 LQR Control Parameters and Simulation

We already have 1-stage linear IP system dynamic model previously. Next we are going to design LQR controller which not only stabilize the pendulum rod vertically upward but also control the cart position. The 1-stage linear IP system state space equations are:
& x 0 && x = 0 & 0 & & 0 1 0 x 0 & 0 0 0 x 1 ' + u 0 0 1 0 & 0 29.4 0 3 0
x & x 1 0 0 0 x 0 ' y= = + u 0 0 1 0 0 &
Applying linear feed back controller, suppose R is step input acting on the cart
& and the 4 state variables x, x, , represents cart position, cart velocity, pendulum rod
angle and pendulum rod velocity respectively. The output y = [ x, ] includes cart position and pendulum angle. Design controller such that, when acting step signal to the system, pendulum rod will be back to vertically upward after oscillation. The cart will reach new given position. Assume all state feed back can be realizable (4 state variable are observable), find out the vector K that determine the feed back control rule. Obtain the K that corresponds to optimal controller in MATLAB. There are 2 parameters for functions LqrR and Q, which are weightening coefficients of input and state variables. The most simple case is assume R = 1 , Q = C * C . The expected response could also be
achieved through adjusting the controller by changing nonzero element in matrix Q. 1 0 Q = C * C = 0 0 0 0 0 0 0 0 1 0 0 0 0 0
In which, Q1,1 is the cart position coefficient, Q3,3 is the pendulum rod angle coefficient, and the input coefficient R is 1. Next we are going to calculate matrix K, use Matlab command K = lqr ( A, B, Q, R) . The MATLAB programming is as follow:
Googol 2006
102
Enter MATLAB Simulink real time control tool box Googol Education Products,openLQR Control MFiles in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ LQR Experiments
PRO 3-11 1-stage Linear IP LQR Control MATLAB Program
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Googol Linear 1 stage Inverted Pendulum LQR Control % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; A=[ 0 1 0 0; 0 0 0 0; 0 0 0 1; 0 0 29.4 0]; B=[ 0 1 0 3]'; C=[ 1 0 0 0; 0 0 1 0]; D=[ 0 0 ]'; Q11=1000; Q33=200; Q=[Q11 0 0 0; 0 0 0 0; 0 0 Q33 0; 0 0 0 0]; R = 1; K = lqr(A,B,Q,R) Ac = [(A-B*K)]; Bc = [B]; Cc = [C]; Dc = [D]; T=0:0.005:5; U=0.2*ones(size(T)); Cn=[1 0 0 0]; Nbar=rscale(A,B,Cn,0,K); Bcn=[Nbar*B]; [Y,X]=lsim(Ac,Bcn,Cc,Dc,U,T); plot(T,X(:,1),'-');hold on; plot(T,X(:,2),'-.');hold on; plot(T,X(:,3),'.');hold on; plot(T,X(:,4),'-') legend('CartPos','CartSpd','PendAng','PendSpd') Let Q1,1 = 1 Q3,3 = 1 , there is
K = [ -1 -1.7855 25.422 4.6849]
Googol 2006
103
Set up 1-stage linear IP model in Simulink as shown in below figure: Enter MATLAB Simulink real time control tool box Googol Education Products , open LQR Control Simulink in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ LQR Experiments
Figure 3-55 1-stage Linear IP LQR Control Simulation Model
LQR Controllerblock is encapsulated. Right click on the block and selectLook under maskto open the block:
Double clickMatrix gain K to set control parameters:
Click
to run simulation and get the following result:
Googol 2006
104
Figure 3-56 1-stage Linear IP LQR Control Simulation Result 1
LQR control step response is depicted as above figure. CartPos is the cart position curve, CartSpd is the cart velocity curve, PendAng is the pendulum angle curve and PendSpd is the pendulum rod angular velocity curve. It can be observed from the figure, close loop system response overshoot is small, but the settling time is long. We could increase control gain to decrease the settling time. It can also be observed that in Q matrix, both the settling time and pendulum rod angle movement will decrease as Q11 increase. Let Q1,1 =1000, Q3,3 =200, Then K = [ -31.623 -20.151 72.718 13.155]
Set parameters and run, the system response is as follow:
From the figure, system response time improved dramatically. The response can even be faster by increasing Q1,1 and Q3,3 . But for real discrete time control system, Googol 2006
105
large control may result in oscillation
3.6.3 1-stage Linear IP LQR Control Experiment

Real time control experiment is implemented in MATALB Simulink environment. Please read carefully the user manual before operating.
Before conducting MATLAB real time control experiment, please check IP system mechanical structure and electrical wiring to ensure the safety. Operation steps: 1) Open 1-stage linear IP LQR real time control block. Enter MATLAB Simulink real time control tool box Googol Education Products,openLQR Control Demo in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Experiment\ LQR Experiments
Figure 3-58 1-stage Linear IP LQR Real Time Control Diagram
In which LQR Controller LQR controller block, Real Control real time is is control block. Double clickLQR Controllerto set control parameters as follow:
Googol 2006
106
Right click mouse onLQR Controllerblock and selectLook under maskto open the following block:
Double clickReal Controlblock to open real time control window as follow:
In whichPendulumblock is the IP system input output module, which inputs cart velocityVeland accelerationAcc and outputs cart positionPosand pendulum rod angleAngle. Double clickPendulumblock to open:
Googol 2006
107
In whichSet Carts Acc and Velmodule sets cart velocity and acceleration, Get Carts Position module reads cart current position andGet Pends Angle module reads pendulum rod current angle. More detailed description please refer to linear IP user manual. 2) Click to build,
connect and run. Make sure the motor is servo to to
on. Pull the pendulum rod vertically upward slowly by hand and release when the program started to control. 3) The experiment result will be like the figure below:
Figure 3-59 1-stage Linear IP LQR Real Time Control Experiment Result
The top of the figure is the cart position plot and the bottom part is the pendulum rod angle plot. From the figure, the cart can be stabilized in 1.5 second after being disturbed. The control effect is satisfactory. 4) Change the matrix Q value Q11 and Q 33 , set real control LQR parameters as those get from simulation. Run real control program and observe the response.
When implementing MATLAB real control experiment, please dont change controller parameters excessively. The system may get out of control with too large parameters. Please ensure safety in the experiment. Googol 2006
108

Record the calculation steps, simulation and experiment result and finish the report.
Googol 2006
109
Chapter 4
1-stage Linear Pendulum Modeling and Experiment
Chapter 4 1-stage Linear Pendulum Modeling and Experiment

1-stage linear IP will be in free down static state without external force. When the pendulum is disturbed by external force, the rod will swing freely for a long time if the friction is small. In many situations such long time oscillation is not expected. For example, when a crane is hoisting up an object, we wish the object could stop still quickly to the appointed position. Next we are going to model the 1-stage linear pendulum and do simulation and experiment.
4.1 1-stage Linear Pendulum Modeling and Analysis

4.1.1 1-stage Linear Pendulum Modeling
Similar with 1-stage linear IP modeling, both Newtons mechanics and Lagrange method can be used in the modeling process. The Newtons mechanics method modeling could refer to 1-stage Linear Inverted Pendulum System Model and we skip here. Next we will model the 1-stage pendulum system by Lagrange method. x
F Cart
Trank Pendulum Rod
Figure 4-1 1-stage Linear Pendulum Physical Model
The Lagrange equation is:

L ( q, q ) = T ( q, q ) V ( q, q )
. . .
(4-1)
In which L is Lagrange operator, q is system generalized coordinate, T is system Googol 2006

110
Chapter 4
kinetic energy and V is system potential energy.

(4-2)
i1,2,3n f i is the external force acting on the i th generalized coordinate.
In 1-stage pendulum system, there are 2 generalized coordinates: x and . First calculate the kinetic energy:
T = TM + Tm
In which TM , Tm are kinetic energy of cart and pendulum rod 1 respectively. Cart kinetic energy:
TM =
. 2 1 Mx 2
The pendulum rod kinetic energy is:

Tm = Tm + Tm
' ''
, in which Tm , Tm denote the translational and rotational kinetic
'
'
energy of the pendulum rod respectively. Assume: xpend x axis coordinate of the rod mass center; ypend y axis coordinate of the rod mass center; There are:
xpend = x + lSin ypend = lCos
The rod kinetic energy is:

Tm =
'
d ( ypend ) 2 1 d ( xpend ) 2 m ( ) +( ) dt dt 2 Tm =
'' . 2 . 2 1 1 J p = ml 2 2 6
Over all system kinetic energy is:

Tm = Tm + Tm
' ''
d ( ypend ) 2 1 2 . 2 1 d ( xpend ) 2 = m ( ) +( ) + ml dt dt 2 6
The system potential energy is:

V = Vm = m g ypend = mglCos
As there is only friction force acting on the system in the generalized coordinate , so: Googol 2006
111
Chapter 4
. d L L =bx . dt
To a 1-stage linear IP system, system state variables are:

. . x, , x,
Solve for the equation:

. X = AX + Bu ' Y = CX
We need to solve for , Assume

.. . . ..
..
= f ( x, , x, , x )
Take Taylor expansion in the equilibrium point and linearize, there is:
= k11 x + k12 + k13 x + k14 + k15 x

In which
k11 = k13 =
..
..
f | . . .. x x=0, =0, x=0, =0, x=0 f x f x

.. .
k12 =
f | . . .. x =0, =0, x =0, =0, x =0 f

.
x = 0 , = 0 , x = 0 , = 0 , x = 0
..
k14 =
x = 0 , = 0 , x = 0 , = 0 , x = 0
..
k15 =
x = 0 , = 0 , x = 0 , = 0 , x = 0
..
Calculate the above in Mathematica. Program file please refer to L1DP.nb MathematicaFile
in \GLIP2001\Modelfiles\
Googol 2006
112
Chapter 4
tpend = 1 2 m HHt xpendL ^2 + Ht ypendL ^2L + 1 6 m l^2 H'@tDL ^2; Simplify@tpendD; v = m g ypend; lang = tpend v; Simplify@langD;
xpend = x@tD + l Sin@@tDD; ypend = l Cos@@tDD;
PRO 4-1 1-stage Linear Pendulum Modeling Program
Solve@8fa 0 < , 8''@tD<D; add = ''@tD . %;
ldad = '@tD lang; Simplify@ldadD; fa = t ldad @tD lang; Simplify@faD;
g = 9.8; l = 0.25;
k11 = x@tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0 k12 = @tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0 k13 = x'@tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0 k14 = '@tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0 k15 = x''@tD add . x@tD 0 . @tD 0 . x'@tD 0 . '@tD 0 . x''@tD 0
Run program:
k11 = 0 k12 = k13 = 0 k14 = 0 k15 =
Simplify@k12D Simplify@k15D
3g 4l
3 4l
. .
Assume X = {x, x, , } system state space equations are:
Googol 2006
113
Chapter 4
There are:
& x 0 && 0 x = & 0 0 & &
1 0 0 0 0 0 3g 0 4l 0 x 0 0 x 1 & 1 + 0 u 3 0 & 4l
(4-3)
x & x 1 0 0 0 x 0 + u y= = 0 0 1 0 0 &
By transformation:
& & =
3g 3 && x 4l 4l
The transfer function of pendulum rod and cart position is: 3 2 s (s) = 4l 3g X (s) s2 + 4l The transfer function of pendulum rod angle and cart acceleration is: 3 (s) 4l = 3g V (s) s2 + 4l
4.1.2 Real System Model

Real system physical parameters could refer to related content in chapter System Parameters. Input parameters and system physical model is obtained. The transfer function of pendulum rod angle and cart position is:
( s) 3s 2 = X ( s ) s 2 + 29.4
(4-4)
The transfer function of pendulum rod angle and cart acceleration is: ( s) 3 = 2 V ( s ) s + 29.4
& x 0 && x = 0 & 0 & & 0 0 x 0 & 0 0 0 x 1 ' + u 0 0 1 0 & 0 29.4 0 3 0
114
(4-5)
The system state equations using cart acceleration as the input has the following form:
1
Googol 2006
Chapter 4
x & x 1 0 0 0 x 0 ' + u y= = 0 0 1 0 0 &
(4-6)
4.1.3 System Controllability Analysis

According to 3.1.4 System Controllability Analysis, there is:
0 0 A= 0 0 1 0 0 0 0 0 0 29.4 0 0 1 0
0 1 B= 0 3
1 0 0 0 C= 0 0 1 0 0 D= 0 Calculate in MATLAB:
clear; A=[ 0 1 0 0; 0 0 0 0; 0 0 0 1; 0 0 -29.4 0]; B=[ 0 1 0 -3]'; C=[ 1 0 0 0; 0 1 0 0]; D=[ 0 0 ]'; cona=[B A*B A^2*B A^3*B]; cona2=[C*B C*A*B C*A^2*B C*A^3*B D]; rank(cona) rank(cona2) Or directly use command ctrb and obsv to calculate.
Googol 2006
115
Chapter 4
Uc=ctrb(A,B); Vo=obsv(A,C); rank(Uc) rank(Vo) There are: ans = 4 ans = 2 It can be observed that, the rank of system state controllable matrix equal to state dimension and the rank of output controllability matrix equation to dimension of output vector y . So the system is controllable and will be stabilized by proper controller design.
4.2 1-stage Linear Pendulum Root Locus Analysis

After obtaining system transfer function, the root locus analysis can be conducted. Similar with 3.2 1-stage Linear IP Root Locus Control Experiment, create a new file in MATLAB, type the following commands: clear; num=[-3 ]; den=[1 0 29.4]; rlocus(num,den) z=roots(num); p=roots(den); The result is as follow, system poles and zeros are: z= 0 0 p= 0 + 5.4222i 0 - 5.4222i System root locus:
Googol 2006
116
Chapter 4
Figure 4-2 1-stage Linear Pendulum Root Locus Plot
It can be observed from the figure, system zero is infinity and there are two poles on imaginary axis.
4.3 1-stage Linear Pendulum Frequency Response Analysis

Based on control principle in 0 , analyze frequency response of 1-stage linear pendulum; type the following commands in MATLAB: clear; num=[-3]; den=[1 0 29.4]; subplot(2,1,1) bode(num,den) subplot(2,1,2) nyquist(num,den) The result will be like following:
Googol 2006
117
Chapter 4
Figure 4-3 1-stage Linear Pendulum Bode Plot
Figure 4-4 1-stage Linear Pendulum Nyquist Plot
It can be observed that, system Nyquist plot encircle point -1 once, system is not stable.
4.4 1-stage Analysis
Linear
Pendulum
Step
Response
Analyze the 1-stage pendulum step response as in 3.1.5 System Step Response Analysis. clear; num=[-3]; den=[1 0 29.4]; step(num,den) Googol 2006
118
Chapter 4
Run and get the following result:
Figure 4-5 1-stage Linear Pendulum Step Response Result
It can be observed from the figure, this is a typical undamped second order system. The system step response will not converge.
4.5 1-stage Linear Pendulum PID Control Simulation and Experiment

Similar with 1-stage linear IP, 1-stage linear pendulum can also be controlled by root locus, frequency response or state space method. The experiments mentioned above could refer to 1-stage linear IP experiments and 1-stage linear pendulum PID control experiment. For 1-stage linear pendulum, we only introduce PID and LQR control experiment here.
4.5.1 1-stage Linear Pendulum PID Control Analysis and Simulation

Modeling 1-stage linear pendulum in MTALAB Simulink:
Googol 2006
119
Chapter 4
The result will be as follow:
We could see, the result is the same with the previous section. Next we will add the PID controller: (Enter MATLAB Simulink real time control tool boxGoogol Education Products, open PID Control Simulink in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage Pendulum Experiment\ PID Experiments
Figure 4-6 1-stage Linear Pendulum PID Control Simulation
Double click PID block to set PID parameters:
Note: As there is already a - in system model, the PID parameter should be set a minus value. Run simulation and the result should be:
Googol 2006
120
Chapter 4
Figure 4-7 1-stage Linear Pendulum P Control Result
It can be observed that, with P controller, the system will vibrate without any attenuation. The differential gain should be added to control. Reset PID parameters as:
Run simulation and the result will be as follow:
The system vibrates excessively and the settling time is too long. The problem can be solved by adding differential gain:
Adding step signal of magnitude 0.01 to the system, the simulation result will be: Googol 2006
121
Chapter 4
Figure 4-8 1-stage Linear Pendulum PD Control Result
From the figure, the system can be stabilized in a short time though there exist certain steady state error. Adding integral gain, the steady state error will be further decreased.
Assume that the system is required to be stabilized in 1 second and the overshot be less than 20%. Change the control parameters and observe the result. Let:
Figure 4-9 1-stage Linear Pendulum PID Controller
Googol 2006
122
Chapter 4
The simulation result will be as follow:
Figure 4-10 1-stage Linear Pendulum PID Control Result
It can be observed that the simulation result is satisfactory.
4.5.2 1-stage Linear Pendulum PID Real Time Control

The real time control experiment is implemented in MATALB Simulink. Please read carefully the user manual before experiment.
Before conducting MATLAB real time control experiment, please check IP system mechanical structure and electrical wiring to ensure the safety. Experiment steps: 1) Open 1-stage linear pendulum real time control program in MATLAB Simulink. (Enter MATLAB Simulink real time control tool box Googol Education Products, open PID Control DemoinInverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage Pendulum Experiment\ PID Experiments .
Googol 2006
123
Chapter 4
Figure 4-11 1-stage Linear Pendulum PID Real Time Control Diagram
2) Double clickPID Controllerblock to open PID parameters window:
Input the parameters obtained from the simulation: 3) Click to build. There will be instruction in MATLAB command window to inform if build is success or not. If yes, continue to the next step. 4) Switch on the electric box power. 5) Click connect and click run after connecting succeed. Adding to to disturbance to the system when it is stabilized. The following real control result will be obtained:
Googol 2006
124
Chapter 4
Figure 4-12 1-stage Linear Pendulum PID Real Time Control Result
It can be observed that, after adding disturbance, system will be stabilized within 1 second and the overshoot is also small. Note: PID control is signal output control so we wont be able to control the cart position. In general, the cart will move towards one direction slowly and when the cart is near the limit switch of one side users should touch the pendulum rod slightly to avoid the cart reach the limit. 6) Change PID parameters and redo the experiment. Observe the different result. 7) Record the result and finish the report.
When implementing MATLAB real control experiment, please dont change controller parameters excessively. The system may get out of control with too large parameters. Please ensure safety in the experiment.
Googol 2006
125
Chapter 4
4.6 1-stage
Linear
Pendulum
LQR
Control
Simulation and Experiment

PID control as described in previous section can only control the pendulum rod angle but not the cart position. In order to control cart position and rod angle, a single-input (cart acceleration), multi-output (cart position and pendulum rod angle) controller is required. By the system state equation obtained previously, state space pole placement and LQR optimal control method can be applied to system control. Next we will control the system by LQR method; other algorithm design and experiment are left to users. Related LQR control algorithm principle please refer to 3.6.1 Linear Quadratic LQR Optimal Control Principle and Analysis or textbooks about modern control theory.
4.6.1 1-stage Linear Pendulum LQR Control Analysis and Simulation

We already have the system state equation using cart acceleration as the input:
& x 0 && x = 0 & 0 & & 0 1 0 x 0 & 0 0 0 x 1 ' + u 0 0 1 0 & 0 29.4 0 3 0
x & x 1 0 0 0 x 0 ' y= = + u 0 0 1 0 0 &
& The 4 state variables x, x, , represent cart position, cart velocity, pendulum
rod angle and pendulum rod angular velocity respectively. The output y = [ x, ] include cart position and pendulum rod angle. Next we are going to use LQR control algorithm to calculate: Assume optimal control gain matrix K: u(t)=-K*x(t) K=lqr(ABQR) Create the following m file in MATLAB: Set cart position gain be 1000, pendulum rod angle gain be 500. In general we set R1, simulation time be 5 second and simulation steps be 0.005 seond. Googol 2006
126
Chapter 4
1-stage linear pendulum LQR simulation file is as follow: (Enter MATLAB Simulink real time control tool box Googol Education Products, Open LQR Control M Files in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage Pendulum Experiment\ LQR Control Experiments)
PRO 4-2 1-stage Linear Pendulum LQR Control MATLAB Program
clear; A=[ 0 1 0 0; 0 0 0 0; 0 0 0 1; 0 0 -29.4 0]; B=[ 0 1 0 -3]'; C=[ 1 0 0 0; 0 0 1 0]; D=[ 0 0 ]'; Q11=1000; Q33=500; Q=[Q11 0 0 0; 0 0 0 0; 0 0 Q33 0; 0 0 0 0]; R = 1; K = lqr(A,B,Q,R) Ac = [(A-B*K)]; Bc = [B]; Cc = [C]; Dc = [D]; T=0:0.005:5; U=0.2*ones(size(T)); Cn=[1 0 0 0]; Nbar=rscale(A,B,Cn,0,K); Bcn=[Nbar*B]; [Y,X]=lsim(Ac,Bcn,Cc,Dc,U,T); plot(T,X(:,1),'-');hold on; plot(T,X(:,2),'-.');hold on; plot(T,X(:,3),'.');hold on; plot(T,X(:,4),'-') legend('CartPos','CartSpd','PendAng','PendSpd') Run program and obtain the following result: Feedback gain matrix is: K= 31.6228 14.4046 -22.3514 0.1188 It can be observed that, system will be stabilized in 3 seconds after being Googol 2006
127
Chapter 4
disturbed. The cart will be back to the original position and the pendulum rod will be in static free down state.
Figure 4-13 1-stage Linear Pendulum Simulation Result (with disturbance)
Similar with 1-stage linear IP, model 1-stage linear pendulum in MATLAB Simulink: (Enter MATLAB Simulink real time control tool box Googol Education Products Open LQR Control Simulink in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage Pendulum Experiment\ LQR Control Experiments).
Figure 4-14 1-stage Linear Pendulum Simulink Diagram
Double click LP1D block to open 1-stage linear pendulum model parameter window:
Googol 2006
128
Chapter 4
Set matrix A,B,C,D as the real system model value. Double click LQR Controller to open LQR control parameter window:
Input the calculated feedback control gain K to the window respectively. Double click Disturbance block to set disturbance signal. Set disturbance signal magnitude as 0.1 rad, the diagram is as follow. System will be stabilized again with disturbance adding to f(s).
Googol 2006
129
Chapter 4
After setting parameters, click to run simulation and the result will be as follow:
Figure 4-15 1-stage Linear Pendulum LQR Control Simulation Result
Users may modify the value of weighting matrix Q to obtain different feedback control parameter K and input to simulation model to observe the different results.
4.6.2 1-stage Linear Pendulum LQR Control Experiment

Real time control experiment is implemented in MATALB Simulink. Please read carefully the user manual before experiment.
Before conducting MATLAB real time control experiment, please check IP system mechanical structure and electrical wiring to ensure the safety. Googol 2006
130
Chapter 4
Operation procedures: 1) Open 1-stage linear pendulum LQR real time control block: (Enter MATLAB Simulink real time control tool box Googol Education Products. Open LQR Control Demo in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage Pendulum Experiment\ LQR Control Experiments).
Figure 4-16 1-stage Linear Pendulum LQR Real Time Control Diagram
In which LQR Controller is LQR control block, Real Control is real time control block. Double click LQR Controller block to set LQR controller parameters as follow:
Right click on LQR Controller block, select Look under mask to open the following diagram:
Googol 2006
131
Chapter 4
Double clickReal Controlblock to open real time control block as follow:
In whichPendulumblock is the IP system I/O module. The input is cart velocity Vel &acceleration Acc and the output is cart position Pos &pendulum angleAngle. Double click Pendulum block to open:
In which Set Carts Acc and Vel block sets cart velocity and acceleration. Get Carts Position block reads the cart actual position. Get Pends Angle block reads pendulum current angle. More details please refer to IP instruction manual. 2) Click to build and click to connect. Click to run program. When the program is running, the cart should be in the middle of the shaft and keep static downward. 3) The experiment result should be as in below figure:
Googol 2006
132
Chapter 4
Figure 4-17 1-stage Linear Pendulum LQR Control Result
The first chart is the cart position curve and the second one is the pendulum angle curve. It can be observed from the figure that, the cart can be stabilized again in 2 seconds with external disturbance. The control performance is satisfactory. 4) Change the value Q11 and Q 33 of matrix Q, input the simulation result of LQR parameters to real time control program, run and observe the different control performance. 5) Record and analyze the experiment result. Finish the report.
When implementing MATLAB real control experiment, please dont change controller parameters excessively. The system may get out of control with too large parameters. Please ensure safety in the experiment.
Googol 2006
133
Chapter 5 1-stage Linear IP Swing Up Experiment

For 1-stage linear IP, the initial status is static down. To transform the status to vertically upward, external force is required to be applied to pendulum rod. In previous experiments, we pull the pendulum rod up by hand. Next we are going to control the IP to swing up automatically.
5.1 Energy Control in IP Swing Up

The energy of a single unconstrained IP system is:
E=
1 2 J + mgl (cos 1) 2
There is:
. . . dE = J mgl Sin = mul Cos dt In which u control vector in horizontally right direction Applying Lyapunov method, let: 1 V = ( E E ref ) 2 2 Then: . dV = ( E E ref )mul Cos dt
Let: u = k ( E E ref ) Cos Notice that when 0 or Cos0 , u = 0 . Besides, the control variable can not be too large as a result of the physical limitations, there is:
. sign[( E E ref ) Cos ] ng v= 0 .
In which, sign() is sign function, n = v max / g is a constant.
Googol 2006
134
5.2 1-stage Linear IP Swing Up Experiment

Real time control experiment is implemented in MATALB Simulink environment, please read carefully the instruction manual before experiment.
Before conducting MATLAB real time control experiment, please check IP system mechanical structure and electrical wiring to ensure the safety. Experiment procedures: 1) Open 1-stage linear IP swing up program in MATLAB Simulink:
Enter MATLAB Simulink real time control toolbox Googol Education Products, open Swing-Up Control Demo in Inverted Pendulum\Linear Inverted Pendulum\Linear 1-Stage IP Swing-Up Control
Figure 5-1 1-stage Linear IP Swing Up Real Time Control Diagram
2) In whichSwing-up Controller is the swing up control mudule.
Googol 2006
135
Double clickSwing-up Controller block to open the following diagram:
In whichSwing-up is the programmed swing up function. Double clickSwing-upblock to set the energy coefficient:
3) After confirming parameters, click
to build program.
4) Click to connect after built successfully. 5) Click to run program, the experimental result will be as follow:
Googol 2006
136
Figure 5-2 1-stage Linear IP Swing Up Control Result (cart position)
Figure 5-3 1-stage Linear IP Swing Up Control Result (pendulum rod angle)
It can be observed that the maximum value of pendulum rod angle increase gradually. When the value is large enough in certain range, the system starts stabilization control. At this stage, the cart will be stabilized near the initial position (0) and pendulum rod will be vertically upward (the pendulum rod angle is pi relative to the static down angle which is 0 )
5.3 Swing Up Experiment by Other Algorithms

In MATLAB real time control software, users may apply their own algorithms conveniently. For SISO algorithm in classic control theory, users may substitute the controller in 3.41-stage Linear IP PID Control Experiment with their own controller:
Googol 2006
137
Apply user define controller
Figure 5-4 Apply user define controllerSISO
Data acquisition module like pendulum angle and cart position, and other signal converting module are not suggested to be modified except for some special case. By applying single-inputcart acceleration multi-outputpendulum rod angle and cart position algorithms of modern control theory, users may substitute controller like the LQR in 3.6Linear Quadratic LQR Optimal Control with their own.
Apply user define controller
Figure 5-5 Apply user define controllerMIMO
After changing controller, rebuild program and connect after built successful. Ensure the safety and run the program. Googol 2006
138
User may also use their own swing up algorithm as shown in following figure:
Apply user define swing up controller
Figure 5-6 Apply user define swing up controller
When implementing the user define controller, please ensure the safety. Users should not start real control without good simulation result. If the system can not be controlled, please cut off the power of electric box immediately.
Googol 2006
139
Chapter 6
2-stage Linear Inverted Pendulum
Chapter 6 2-stage Linear Inverted Pendulum

2-stage linear IP is composed of linear motion module and 2-stage IP main body.
6.1 System Physical Model

For simplicity, we ignore the air resistance and other frictions and assume the pendulum rod be rigid body. The 2-stage IP configuration is shown as in Figure 6-1:
Pendulum rod 2
Joint Pendulum rod1
1
F cart
Sliding shaft
x
Figure 6-1 2-stage Linear IP Physical Model
IP parameters are defined as follow: M cart mass

m1 m2
rod 1 mass rod 2 mass Joint mass distance from the rod1 axis rotation center to the rod mass center distance from the rod2 axis rotation center to the rod mass center angle between the rod1 and vertically upward direction
m3
l1 l2
Googol 2006
140
Chapter 6
2
F
angle between the rod2 and vertically upward direction external force acting on the system
Next we are going to derive the system kinematic equations by Lagrange method. The Lagrange equation is:
L( q, q ) = T ( q, q ) V ( q , q )
. . .
In which L is the Lagrange operator, q is the generalized coordinates, T denotes the kinetic energy and V denotes potential energy of the system.
In which i1,2,3n f i is the force acting on the system in the ith generalized coordinate. In 2-stage IP system, there are 3 generalized coordinates: x, 1 , 2 The system kinetic energy is:
T = TM + Tm1 + Tm 2 + Tm3
In which TM , Tm1 , Tm 2 , Tm3 denotes the kinetic energy of cart, rod 1, rod 2 and Jiont respectively. The kinetic energy of the cart is:
TM =
' '' ' ' . 2 1 Mx 2
Tm1 = Tm1 + Tm 2 In which Tm1 , Tm 2 denote the translational and rotational kinetic
energy of the pendulum rod 1 respectively.

Tm 2 = Tm 2 + Tm 2 In which Tm 2 , Tm 2 denote the translational and rotational kinetic
' '' ' '
energy of the pendulum rod 2 respectively. Lets assume: xpend1--- x axis coordinate of rod 1 mass center; yangle1 --- y axis coordinate of the rod 1 mass center; xpend2 --- x axis coordinate of rod 2 mass center; yangle2 --- y axis coordinate of the rod 2 mass center; xmass --- x axis coordinate of the joint mass center; ymass--- y axis coordinate of the joint mass center; There are: Googol 2006
141
Chapter 6
xpend1 = x l1 Sin 1 ypend1 = l1Cos 1 xpend 2 = x 2l1 Sin 1 l 2 Sin( 2 ) ypend 2 = 2l1Cos 1 + l 2 Cos 2 xmass = x 2l1 Sin 1 ymass = 2l1Cos 1 Then:
Tm1
' 2 2 1 d ( xpend1) d ( ypend1) = m1 + dt dt 2 . 2 . 2 1 1 2 J p1 = m1l1 1 2 6
Tm1 =
''
Same as above:
Tm 2 =
' 2 2 1 d ( xpend 2) d ( ypend 2) m2 + dt dt 2 . 2 . 2 1 1 2 = J p 2 2 = m2 l 2 2 2 6
Tm 2
''
Tm 3
2 2 1 d ( xmass) d ( ymass) = m3 + dt dt 2
The overall system kinetic energy will be:

T = TM + Tm1 + Tm 2 + Tm 3
2 2 . 2 . 2 1 1 d ( xpend1) d ( ypend1) 1 2 = M x + m1 + m1l1 1 + dt dt 2 2 6
2 2 . 2 1 d ( xpend 2) d ( ypend 2) 1 + m2 l 2 2 2 m2 + dt dt 2 6
2 2 1 d ( xmass) d ( ymass) + m3 + dt dt 2
The system potential energy become:

V = Vm1 + Vm 2 + Vm 3 = m1 ypend1 + m2 ypend 2 + m3 ymass
= m1l1Cos1 + m2 (2l1Cos1 + l 2 Cos 2 ) + 2m3 l1Cos1 There are no external force acting on the system under 1 , 2 generalized coordinates, there are:
Googol 2006
142
Chapter 6
L d L dt . = 0 1 1 d L L =0 . 2 dt 2 For 2-stage IP system, the state variables are:

. . . x,1 , 2 , x,1 , 2
To solve the state equations:

. X = AX + Bu Y = CX
The solution of 1 and 2 should be solved first. Assume:

. . . .. .. 1 = f 1 ( x , 1 , 2 , x, 1 , 2 , x ) .. . . . .. 2 = f 2 ( x,1 , 2 , x,1 , 2 , x)
..
..
Take Tyler expansion around the equilibrium point and linearize, there are:
. . . .. .. 1 = k11 x + k121 + k13 2 + k14 x + k15 1 + k16 2 + k17 x .. . . . .. 2 = k 21 x + k 221 + k 23 2 + k 24 x + k 25 1 + k 26 2 + k 27 x
In which:
k11 = k13 = f 1 | . . . .. x x =0,1 =0, 2 =0, x =0,1 =0, 2 =0, x =0 f 1 | . . . .. 2 x =0,1 =0, 2 =0, x =0,1 =0, 2 =0, x =0 k12 = f1 | . . . .. 1 x =0,1 =0, 2 =0, x =0,1 =0, 2 =0, x =0 f1 x
.
k14 =
x = 0 ,1 = 0 , 2 = 0 , x = 0 ,1 = 0 , 2 = 0 , x = 0
..
k15 =
f 1 1 f 1 x
.. .
x = 0 ,1 = 0 , 2 = 0 , x = 0 ,1 = 0 , 2 = 0 , x = 0
..
k16 =
f1 2
.
x = 0 ,1 = 0 , 2 = 0 , x = 0 ,1 = 0 , 2 = 0 , x = 0
..
k17 =
x = 0 ,1 = 0 , 2 = 0 , x = 0 ,1 = 0 , 2 = 0 , x = 0
..
k 21 =
f 2 | . . . .. x x =0,1 =0, 2 =0, x =0,1 =0, 2 =0, x =0 f 2 | . . . .. 2 x = 0,1 =0, 2 =0, x =0,1 =0, 2 =0, x =0
k 22 =
f 2 | . . . .. 1 x =0,1 =0, 2 =0, x =0,1 =0, 2 =0, x =0
k 23 =
Googol 2006
143
Chapter 6
k 24 =
f 2 x f 2 2
. .
x = 0 ,1 = 0 , 2 = 0 , x = 0 ,1 = 0 , 2 = 0 , x = 0
..
k 25 =
f 2 1 f 2 x
.. .
x = 0 ,1 = 0 , 2 = 0 , x = 0 ,1 = 0 , 2 = 0 , x = 0
..
k 26 =
x = 0 ,1 = 0 , 2 = 0 , x = 0 ,1 = 0 , 2 = 0 , x = 0
..
k 27 =
x = 0 ,1 = 0 , 2 = 0 , x = 0 ,1 = 0 , 2 = 0 , x = 0
..
The above equations can be solved in Mathematica: (Calculation file please refer to L2DIP.nb MathematicaFile.)
1 Mcar x @tD2 ; 2
in
\GLIP2002\Modelfiles\
PRO 6-1 2-stage Linear IP Modeling Mathematica Program
TM =
xpend1 = x@tD l1 Sin@1@tDD; ypend1 = l1 Cos@1@tDD; tpend1 = 1 2 m1 HHt xpend1L ^2 + Ht ypend1L ^2L + 1 2 H1 3 m1 l1 ^2L 1'@tD ^2;
ldad = 1'@tD lang; Simplify@ldadD; f1 = t ldad 1@tD lang; Simplify@f1D; ldbd = 2'@tD lang; Simplify@ldbdD; f2 = t ldbd 2@tD lang; Simplify@f2D;
lang = TM + tpend1 + tpend2 + tmass v; Simplify@langD;
v = m1 g ypend1 + m2 g ypend2 + m3 g ymass; Simplify@vD;
xmass = x@tD 2 l1 Sin@1@tDD; ymass = 2 l1 Cos@1@tDD; tmass = 1 2 m3 HHt xmassL ^2 + Ht ymassL ^2L;
xpend2 = x@tD 2 l1 Sin@1@tDD l2 Sin@2@tDD; ypend2 = 2 l1 Cos@1@tDD + l2 Cos@2@tDD; tpend2 = 1 2 m2 HHt xpend2L ^2 + Ht ypend2L ^2L + 1 2 H1 3 m2 l2 ^2L H2'@tDL ^2;
Googol 2006
Solve@8f1 0 , f2 == 0< , 81 ''@tD, 2 ''@tD<D; add = 1''@tD . %; bdd = 2''@tD . %;

144
Chapter 6
k11 = x@tD a dd . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k12 = 1@tD add . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k13 = 2@tD add . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k14 = x'@tD add . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . a'@tD 0 . 2'@tD 0 . x''@tD 0; k15 = 1'@tD add . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k16 = 2'@tD add . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k17 = x''@tD add . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0;
k21 = x@tD bdd . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k22 = 1@tD bdd . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k23 = 2@tD bdd . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k24 = x'@tD bdd . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k25 = 1'@tD bdd . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k26 = 2'@tD bdd . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; k27 = x''@tD bdd . x@tD 0 . 1@tD 0 . 2@tD 0 . x'@tD 0 . 1 '@tD 0 . 2'@tD 0 . x''@tD 0; Simplify@k12D Simplify@k13D Simplify@k17D Simplify@k22D Simplify@k23D Simplify@k27D k12 k13 k17 k22 k23 k27
m1 = 0.05 ; m2 = 0.13; m3 = 0.236; l1 = 0.0775; l2 = 0.25; g = 9.8 ;
Googol 2006
145
Chapter 6
Because the result of other terms are all 0, we only list 7 terms k12k13k17 k22k23k27 and the results are as follow:
k12 =
3g (m1 + 2(m2 + m3)) = 86.69 (4m1 + 3(m2 + 4m3))l1 9 gm2 = 21.62 2(4m1 + 3(m2 + 4m3))l1 3(2m1 + m2 4m3) = 6.64 2(4m1 + 3(m2 + 4m3))l1 9 g (m1 + 2(m2 + m3)) = 40.31 2(4m1 + 3(m2 + 4m3))l 2 3g (m1 + 3(m2 + m3)) = 39.45 (4m1 + 3(m2 + 4m3))l 2 3(m1 l 2m3) = -0.088 2(4m1 + 3(m2 + 4m3))l 2
& x . 0 0 1 0 0 . 0 0 0 0 2 = 0 .. x 0 0 0 .. 0 86.69 21.62 1 0 40.31 39.45 .. 2
k13 =
k17 =
k 22 =
k 23 =
k 27 =
Finally, the system state equations are:

1 0 0 0 0 0 0 1 0 0 0 0 0 x 0 0 1 0 1 2 0 & + u 0 x 1 . 0 1 6.64 . 0 2 0.088
x 1 x 1 0 0 0 0 0 0 2 y = 1 = 0 1 0 0 0 0 + 0 u x & 2 0 0 1 0 0 0 . 0 1 . 2
6.2 System Controllability Analysis

System state matrices A, B, C , D are as follow:
Googol 2006
146
Chapter 6
0 0 0 0 0 0 0 0 0 A= 0 0 0 0 86.69 21.62 0 40.31 39.45
1 0 0 0 1 0 0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 B= 1 6.64 0.088
1 0 0 0 0 0 C = 0 1 0 0 0 0 0 0 1 0 0 0 0 D = 0 0 The rank of system state controllability and observability matrices can be solved by MATLAB: clear; A=[ 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; 0 0 0 0 0 0; 0 86.69 -21.62 0 0 0; 0 -40.31 39.45 0 0 0]; B=[ 0 0 0 1 6.64 -0.088]'; C=[ 1 0 0 0 0 0 ; 0 1 0 0 0 0; 0 0 1 0 0 0]; D=[ 0 0 0 ]'; cona=[B A*B A^2*B A^3*B A^4*B A^5*B]; cona2=[C*B C*A*B C*A^2*B C*A^3*B C*A^4*B C*A^5*B D]; rank(cona) rank(cona2) The results are as follow:
Googol 2006
147
Chapter 6
ans = 6 ans = 3 The controllability and observability could also be obtained by commands ctrb and obsv in MATLAB. Uc=ctrb(A,B); Vo=obsv(A,C); rank(Uc) rank(Vo) Run to get the result: ans = 6 ans = 6 Therefore, both the system state and output are controllable and the system is observable.
6.3 2-stage Linear IP MATLAB Simulation

Setup 2-stage linear IP model in MATLAB Simulink:
Figure 6-2 2-stage Linear IP Open Loop Simulation
In which State-Space module is the 2-stage linear IP state space equations. Double click to open the module:
Googol 2006
148
Chapter 6
Controller module is the controller plant. Right click on block Controller and select Look under mask to see the model structure:
In which Matrix Gain K is the feedback matrix. Double click Controller block to set parameters:
Googol 2006
149
Chapter 6
First set all parameters be 1. Disturbance block stands for the external disturbance, which is used for adding a step signal to the system. Click to run the open loop system simulation. The result will be as follow:
Figure 6-3 2-stage Linear IP Open Loop Simulation Result
It can be observed from the result that the system will diverge. Controller should be added to stabilize the system.
6.4 LQR Controller Design and Simulation

Adding LQR controller to the system and the close loop system will be shown as Googol 2006
150
Chapter 6
in the following figure: ( Enter MATLAB Simulink real time control tool box Googol Education Products, open LQR Control Simulink in Inverted Pendulum\Linear Inverted Pendulum\Linear 2-Stage IP Experiments)
Figure 6-4 2-stage Linear IP LQR Control Simulation
Next we are going to design controller by linear quadratic optimal control (LQR) method. The principle of LQR method please refer to chapter of 1-stage linear IP Linear Quadratic LQR Optimal Control Experiment and other related textbooks. (Enter MATLAB Simulink real time control tool box Googol Education Products. Open LQR Control M Files in Inverted Pendulum\Linear Inverted Pendulum\Linear 2-Stage IP Experiments).
Googol 2006
151
Chapter 6
PRO 6-2 2-stage Linear IP LQR Control MATLAB Program
clear all; format long; k12=86.69;k13=-21.62;k17=6.64; k22=-40.31;k23=39.45;k27=-0.088; A=[0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; 0 0 0 0 0 0; 0 k12 k13 0 0 0; 0 k22 k23 0 0 0] B=[ 0 0 0 1 k17 k27]'; C=[1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0]; D=[0; 0; 0]; Q11=1;Q22=1;Q33=1;
Googol 2006
152
Chapter 6
Q=[Q11 0 0 0 Q22 0 0 0 Q33 0 0 0 0 0 0 0 0 0 R=1; K=lqr(A,B,Q,R) Ac=[(A-B*K)]; Bc=[B]; Cc=[C]; Dc=[D]; Cn=[1 0 0 0 0 0]; s=size(A,1); Z=[zeros([1,s]) 1];
0 0 0 0 0 0
0 0 0 0 0 0
0; 0; 0; 0; 0; 0];
N=inv([A,B;Cn,0])*Z'; Nx=N(1:s); Nu=N(1+s); Nbar=Nu+K*Nx; Bcn=[Nbar*B]; x0=[0 0 0.05 0 0 0]; [Y,X]=lsim(Ac,Bcn,Cc,Dc,U,T,x0); xpos=Y(:,1); xangle=Y(:,2); xangle2=Y(:,3); plot(T,xpos,':') hold on; plot(T,xangle,'--') hold on; plot(T,xangle2,'-') Set the LQR parameters as: K=[ 1 73.818 -83.941 2.0162 4.2791 -13.036] The simulation result will be as following:
Googol 2006
153
Chapter 6
It can be observed that the system stabilization time is too long. Therefore increase the weighting factor Q. Assume Q11=300;Q22=500;Q33=500; Run and obtain the simulation result: The LQR controller parameters are: K=[ 17.321 110.87 -197.57 18.468 2.7061 -32.142]
It can be observed from the picture that the system can be well stabilized. After adding disturbance signal, the system will be back to the equilibrium point within 2.5 seconds. Input above simulation parameters to Simulink model, Googol 2006
154
Chapter 6
Run and the result will be as follow:
Adjust the LQR parameters in simulation and observe the result. Select proper control parameters.
6.5 2-stage Linear IP LQR Control Experiment

Real time control is implemented in MATALB Simulink. Users are suggested to read the manual carefully before experiment.
Googol 2006
155
Chapter 6
When implementing MATLAB real time control experiment, please check the mechanical structure and electrical wiring of the IP system to ensure safety. Experimental steps: 6) Open 2-stage linear IP control diagram as in the following figure: (Enter MATLAB Simulink real time control tool box Googol Education Products, open LQR Control Demo in Inverted Pendulum\Linear Inverted Pendulum\Linear 2-Stage IP Experiments).
Figure 6-7 2-stage Linear IP LQR Real Time Control Diagram
The left part consists of cart position module and angle processing module for pendulum rod 1 and 2:
Googol 2006
156
Chapter 6
The output signals are cart position, cart velocity, pendulum rod 1 angle, pendulum rod 1 angular velocity, pendulum rod 2 angle and pendulum rod 2 angular velocity respectively. 7) LQR Controller is the LQR controller block, double click to set parameters:
Figure 6-8 2-sage Linear IP LQR Control Parameters
Input the control parameters to LQR controller. Please pay attention to the correspondency. 8) Real Control is the real time control block. The input is control cart acceleration and the output is cart position, pendulum rod 1 angle and pendulum rod 2 angle. Googol 2006
157
Chapter 6
9) After confirming parameters, click
to build.
10) When build successful, click to connect. 11) Click to run. When motor is servo on, gradually pull up the pendulum rod to the equilibrium position and release when the program start control. The experiment result will be as follow:
Figure 6-9 2-stage Linear IP LQR Control Result (cart position)
Figure 6-10 2-stage Linear IP LQR Control Result (pendulum rod 1 angle)
Figure 6-11 2-stage Linear IP LQR Control Result (pendulum rod 1 angle)
12) Adding disturbance to the pendulum and the system response will be as follow:
Googol 2006
158
Chapter 6
Figure 6-12 2-stage Linear IP LQR Control Result (with disturbance)
13) It can be observed that, without disturbance, the system can be well stabilized. Though there are small oscillations, the cart oscillate scale is about 15mm, pendulum rod 1 oscillate scale is about 0.02 radian and that of pendulum rod 2 is about 0.01 radian. 14) The system can be restabilized in 1 second after adding disturbance. The overshoot is also acceptable. 15) Change the control parameters and simulate again. Run experiment after simulation to observe the different control effect.
6.6 Experiment Result and Report

The report will include the experimental result. The result should be analyzed in the lab report.
6.7 Control Experiment by Other Algorithms

Many control algorithms can be applied to 2-stage IP system such like: fuzzy control, robust control, adaptive control, etc. User could use other controller by Googol 2006
159
Chapter 6
simply replacing the LQR Controller block in real time control diagram with their own.
Apply other algorithms by replacing the controller
Figure 6-13 2-stage Linear IP Controller Replacement
Googol 2006
160
Chapter 7
Chapter 7 3-stage Linear Inverted Pendulum

7.1 System Physical Model
For simplicity, we ignore the air resistance and other frictions. Assume the pendulum rod be rigid body. The structure of 3-stage linear IP is shown in the following figure:
Pendulum rod3 Joint2
Pendulum rod 2 Joint1
1
F Cart
Pendulum rod 1
Sliding shaft
Figure 7-1 3-stage Linear IP Physical Model
Pendulum parameters are defined as follow: M cart mass

m1 m2
pendulum rod 1 mass pendulum rod 2 mass pendulum rod 3 mass Joint1 mass Joint2 mass
161
m3
m4
m5
Googol 2006
Chapter 7
l1 l2
distance from rod 1 axis rotation center to rod 1 mass center distance from rod 2 axis rotation center to rod 2 mass center distance from rod 3 axis rotation center to rod 3 mass center angle between rod 1 and vertically upward direction angle between rod 2 and vertically upward direction angle between rod 3 and vertically upward direction
l3
1 2
F external force acting on the system Deduce the kinematics equations by Lagrangian equation: The Lagrangian equation is:
L( q, q ) = T ( q, q ) V ( q , q ) In which L is the Lagrangian operator, q is the system generalized coordinate. T is the system kinetic energy and V denotes the potential energy. d L L = fi dt q. qi i In which i 1,2,3n, f i is the external force acting on the ith generalized coordinate. In 3-stage IP system, there are 4 generalized coordinates: x, 1 , 2 , 3 . The system kinetic energy is: T = TM + Tm1 + Tm 2 + Tm3 + Tm 4 + Tm5 In which TM , Tm1 , Tm 2 , Tm3 , Tm 4 , Tm5 are kinetic energy of cart, pendulum rod 1, pendulum rod 2, pendulum rod 3, Joint1 and Joint2. The cart kinetic energy: TM =
' '' ' '' . 2 1 Mx 2 . . .
Tm1 = Tm1 + Tm1 In which Tm1 , Tm1 denote the translational and rotational kinetic

Tm 2 = Tm 2 + Tm 2 In which Tm 2 , Tm 2 denote the translational and rotational kinetic
' '' ' ''

Tm3 = Tm3 + Tm3 In which Tm3 , Tm 3
' '' ' ''
denote the translational and rotational kinetic

162
Googol 2006
Chapter 7
energy of the pendulum rod 3 respectively. For system, assume: xpend1: x axis coordinate of rod 1 mass center; ypend1: y axis coordinate of rod 1 mass center; xpend2: x axis coordinate of rod 2 mass center; ypend2: y axis coordinate of rod 2 mass center; xpend3: x axis coordinate of rod 3 mass center; ypend3: y axis coordinate of rod 3 mass center; xmass1: x axis coordinate of Joint1 mass center; ymass1: y axis coordinate of Joint1 mass center; xmass2: x axis coordinate of Joint2 mass center; ymass2: y axis coordinate of Joint2 mass center; There are:
xpend1 = x l1Sin1 ypend1 = l1Cos1 xmass1 = x 2l1Sin1 ymass1 = 2l1Cos1 xpend 2 = xmass1 l2 Sin 2 ypend 2 = ymass1 + l2Cos 2 xmass 2 = xmass1 2l2 Sin 2 ymass 2 = ymass1 + 2l2Cos 2 xpend 3 = xmass 2 l3 Sin 3 ypend 3 = ymass 2 + l3Cos 3
Then: Tm1 =
' 2 2 1 d ( xpend1) d ( ypend1) m1 + 2 dt dt . 2 . 2 1 1 2 J p1 1 = m1l1 1 2 6
Tm1 = The same are: Tm 2

'
''
2 2 1 d ( xpend 2) d ( ypend 2) = m2 + 2 dt dt . 2 . 2 1 1 2 = J p 2 2 = m2 l 2 2 2 6
Tm 2
'
''
Tm3
2 2 1 d ( xpend 3) d ( ypend 3) = m3 + 2 dt dt
Googol 2006
163
Chapter 7
Tm3 =
''
. 2 . 2 1 1 2 J p 3 3 = m3 l 3 3 2 6
Tm 4
2 2 1 d ( xmass1) d ( ymass1) = m4 + 2 dt dt 2 2 1 d ( xmass 2) d ( ymass 2) + m5 2 dt dt
Tm 5 =
The system overall kinetic energy is: T = TM + Tm1 + Tm 2 + Tm 3 + Tm 4 + Tm 5

2 2 . 2 . 2 1 1 d ( xpend1) d ( ypend1) 1 + m1l1 2 1 = M x + m1 + 2 2 dt dt 6 2 2 . 2 1 d ( xpend 2) d ( ypend 2) 1 2 + m2 + + m2 l 2 2 2 dt dt 6 2 2 . 2 1 d ( xpend 3) d ( ypend 3) 1 + m3 l 3 2 3 + m3 + 2 dt dt 6
2 2 1 d ( xmass1) d ( ymass1) + m4 2 dt dt
2 2 1 d ( xmass 2) d ( ymass 2) + m5 + 2 dt dt
System potential energy is: V = Vm1 + Vm 2 + Vm3 + Vm 4 + Vm 5 = m1 ypend1 + m 2 ypend 2 + m3 ypend 3 + m 4 ymass1 + m5 ymass2 The system has no external force under 1 , 2 , 3 generalized coordinate, so there are: L d L dt . = 0 1 1 d L L =0 . dt 2 2 d L L =0 . dt 3 3 For 3-stage IP system, system variables are selected as:
. . . . x , 1 , 2 , 3 , x, 1 , 2 , 3
To solve the state equations: Googol 2006

164
Chapter 7
. X = AX + Bu Y = CX
1 , 2 and 3 should be solved first, therefore assume:

. . . . .. .. 1 = f 1 ( x, 1 , 2 , 3 , x , 1 , 2 , 3 , x ) . . . . .. .. 2 = f 2 ( x, 1 , 2 , 3 , x, 1 , 2 , 3 , x) .. . . . . .. 3 = f 3 ( x, 1 , 2 , 3 , x, 1 , 2 , 3 , x)
..
..
..
Taking Taylor expansion around the equilibrium point and linearize, the variables will have the following form:
. . . . .. .. 1 = k11 x + k12 1 + k13 2 + k14 3 + k15 x + k16 1 + k17 2 + k18 3 + k19 x . . . . .. .. 2 = k 21 x + k 22 1 + k 23 2 + k 24 3 + k 25 x + k 26 1 + k 27 2 + k 28 3 + k 29 x .. . . . . .. 3 = k 31 x + k 32 1 + k 33 2 + k 34 3 + k 35 x + k 36 1 + k 37 2 + k 38 3 + k 39 x
In which k ij is the partial derivative of f i to its j th variable, for example:

k11 = f 1 | . . . . .. x x =0,1 = 0, 2 = 0, 3 =0, x =0,1 = 0, 2 =0,, 3 = 0 x =0
Do above calculation in Mathematica:
Googol 2006
165
Chapter 7
PRO 7-1 3-stage Linear IP Modeling Mathematica Program
Googol 2006
166
Chapter 7
Googol 2006
167
Chapter 7
Googol 2006
168
Chapter 7
Googol 2006
169
Chapter 7
(The program file please refer to \GLIP2003\ModelFiles\MathematicaFile ) Run the program and the following result will be obtained:
L3DIP.nb
in
Googol 2006
170
Chapter 7
k 12 = 159 . 504 k 13 = 38 . 4356 k 14 = 0 . 784646 k 19 = 6 . 64904 k k k k

22 23 24 29
= 127 . 9 = 57 . 9951 = 17 . 189 = 0 . 15831
k 32 = 2 . 57086 k 33 = 16 . 9245 k 34 = 39 . 3485 k 39 = 0 . 00318211

The system state equations are as follow:
& x 0 0 0 . 0 1 . 0 0 0 0 0 0 0 .2 0 3 0 0 0 0 .. = 0 0 0 x 0 .. 0 159.504 38.4356 0.784646 1 .. 0 127.9 57.9951 17.189 ..2 0 2.57086 16.9245 39.3485 3
1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 1 0 0 0 0 0
0 x 0 0 1 0 0 2 0 3 1 0 u & x + 0 . 1 1 0 . 6.64904 0 2 0.15831 0 3 0.00318211
x 1 0 y = 1 = 2 0 3 0
0 1 0 0
0 0 1 0
0 0 0 1
0 0 0 0
0 0 0 0
0 0 0 0
x 1 0 2 0 0 3 x + 0 u & 0 . 0 0 .1 0 2 3
Note: The real system model might be different with this one. This model is not verified by experiment yet. User may check its validity when using the system.
Googol 2006
171
Appendix
Chapter 8 Appendix
8.1 List of Figures
Figure 1-1 Linear IP Series ....................................................................................2 Figure 1-2 Circular IP Series .................................................................................2 Figure 1-3 Planar IP Series ....................................................................................3 Figure 1-4 Configurable IP ....................................................................................3 Figure 2-1 Photoelectric Encoder Component and Structure ................................6 Figure 2-2 MATLAB Simulink Environment........................................................8 Figure 3-11-stage Linear Inverted Pendulum Model...........................................26 Figure 3-2 Cart and Rod Force Analysis..............................................................26 Figure 3-3 1-stage Linear Inverted Pendulum Step Response Simulation ..........37 Figure 3-41-stage Linear IP Close Loop Diagramwith disturbance ............38 Figure 3-5 1-stage Linear IP Simplified Close Loop Diagramwith disturbance ......................................................................................................................38 Figure 3-6 1-stage Linear IP Open Loop Root Locus..........................................40 Figure 3-7 Performance Index and Root Locus...................................................41 Figure 3-8 1-stage Linear IP Root Locus Calculation .........................................42 Figure 3-9 Root Locus after Compensation.........................................................44 Figure 3-10 Step Response after Compensation..................................................45 Figure 3-11 Root Locus after Adding Zero..........................................................45 Figure 3-12 Root Locus after Adding Pole ..........................................................46 Figure 3-13 Close Loop System Step Response ..................................................46 Figure 3-14 1-stage Linear IP Root Locus Method Simulation Result (First Order Controller)....................................................................................................52 Figure 3-15 1-stage Linear IP Root Locus Simulation Model.............................53 Figure 3-16 1-stage Linear IP Root Locus Control Simulation ResultSecond Order Controller .......................................................................................54 Figure 3-17 1-stage Linear IP Root Locus Real Control Block...........................55 Figure 3-18 1-stage Linear IP Root Locus Control Resultequilibrium ........56 Figure 3-191-stage Linear IP Root Locus Control Result(with disturbance ...56 Figure 3-20 1-stage Linear IP Bode Plot .............................................................60 Figure 3-21 1-stage Linear IP Nyquist Plot .........................................................61 Figure 3-22 1-stage Linear IP Bode Plot and Nyquist Plot after Adding Control Gain..............................................................................................................63 Figure 3-23 1-stage Linear IP Bode Plot and Nyquist Plot after ControlFirst Order Controller .......................................................................................66 Figure 3-24 Step Response after Frequency Response Control First Order Controller .................................................................................................66 Figure 3-25 1-stage Linear IP Frequency Response Control Simulation ............67 Googol 2006
172
Appendix
Figure 3-26 Bode and Nyquist Plot after Frequency Response ControlSecond Order ........................................................................................................69 Figure 3-27 Step Response after Frequency Response Control Simulation Second Order .......................................................................................70 Figure 3-28 1-stage Linear IP Frequency Response Real Time Control Diagram ......................................................................................................................71 Figure 3-29 Frequency Response Real Time Control Result First Order Controller .................................................................................................72 Figure 3-30 Frequency Response Real Time Control ResultSecond Order Controller .................................................................................................73 Figure 3-31 1-stage Linear IP Close Loop System Diagram ...............................74 Figure 3-32 1-stage Linear IP Close Loop System Simplified Diagram .............74 Figure 3-33 1-stage Linear IP PID Control MATLAB Simulation Model ..........76 Figure 3-34 PID Set Parameter Window .............................................................76 Figure 3-35 1-stage Linear IP P Control Simulation ResultKp9 .............76 Figure 3-36 1-stage Linear IP Proportional Control Simulation Result Kp40 ......................................................................................................................77 Figure 3-37 1-stage Linear IP PD Control Simulation ResultKp40Kd4 ......................................................................................................................77 Figure 3-38 1-stage Linear IP PD Control Simulation Result Kp40Kd10 ......................................................................................................................78 Figure 3-39 1-stage Linear IP PID Control Simulation Result Kp40 20 Ki Kd4.......................................................................................................78 Figure 3-40 1-stage Linear IP PD Control Simulation Result cart position curve ......................................................................................................................78 Figure 3-41 1-stage Linear IP PID Control MATLAB Simulation Result impulse disturbance ...............................................................................................79 Figure 3-42 1-stage Linear IP MATLAB Real Time Control Diagram ...............80 Figure 3-43 1-stage Linear IP PID Control Experiment Result 1........................81 Figure 3-44 1-stage Linear IP PID Control Experiment Result 2 with disturbance ...............................................................................................82 Figure 3-45 1-stage Linear IP PID Control Experiment Result 3change PID parameters ................................................................................................83 Figure 3-46 State Feedback Close Loop Control Diagram..................................84 Figure 3-47 Inverted Pendulum Pole Placement Principle Diagram ...................87 Figure 3-48 Pole Placement Simulation Result ...................................................89 Figure 3-49 1-stage Linear IP State Space Pole Placement MATLAB Simulink Simulation ....................................................................................................95 Figure 3-50 1-stage Linear IP State Space Pole Placement MATLAB Simulation Result ...........................................................................................................97 Figure 3-51 1-stage Linear IP State Space Pole Placement Real Control Diagram ......................................................................................................................98 Figure 3-52 1-stage Linear IP State Space Pole Placement Real Control Result (equilibrium) ................................................................................................99 Googol 2006
173
Appendix
Figure 3-53 1-stage Linear IP State Space Pole Placement Real Control Result(in disturbance)..................................................................................................99 Figure 3-54 Optimal LQR Control Diagram .....................................................101 Figure 3-55 1-stage Linear IP LQR Control Simulation Model ........................104 Figure 3-56 1-stage Linear IP LQR Control Simulation Result 1 .....................105 Figure 3-57 1-stage Linear IP LQR Control Simulation Result 2 .....................105 Figure 3-58 1-stage Linear IP LQR Real Time Control Diagram......................106 Figure 3-59 1-stage Linear IP LQR Real Time Control Experiment Result......108 Figure 4-1 1-stage Linear Pendulum Physical Model........................................110 Figure 4-2 1-stage Linear Pendulum Root Locus Plot.......................................117 Figure 4-3 1-stage Linear Pendulum Bode Plot.................................................118 Figure 4-4 1-stage Linear Pendulum Nyquist Plot ............................................118 Figure 4-5 1-stage Linear Pendulum Step Response Result..............................119 Figure 4-6 1-stage Linear Pendulum PID Control Simulation ..........................120 Figure 4-7 1-stage Linear Pendulum P Control Result ......................................121 Figure 4-8 1-stage Linear Pendulum PD Control Result ...................................122 Figure 4-9 1-stage Linear Pendulum PID Controller.........................................122 Figure 4-10 1-stage Linear Pendulum PID Control Result................................123 Figure 4-11 1-stage Linear Pendulum PID Real Time Control Diagram ..........124 Figure 4-12 1-stage Linear Pendulum PID Real Time Control Result ..............125 Figure 4-13 1-stage Linear Pendulum Simulation Result (with disturbance)....128 Figure 4-14 1-stage Linear Pendulum Simulink Diagram.................................128 Figure 4-15 1-stage Linear Pendulum LQR Control Simulation Result............130 Figure 4-16 1-stage Linear Pendulum LQR Real Time Control Diagram.........131 Figure 4-17 1-stage Linear Pendulum LQR Control Result ..............................133 Figure 5-1 1-stage Linear IP Swing Up Real Time Control Diagram ...............135 Figure 5-2 1-stage Linear IP Swing Up Control Result (cart position) .............137 Figure 5-3 1-stage Linear IP Swing Up Control Result (pendulum rod angle) .137 Figure 5-4 Apply user define controllerSISO ............................................138 Figure 5-5 Apply user define controllerMIMO .........................................138 Figure 5-6 Apply user define swing up controller .............................................139 Figure 6-1 2-stage Linear IP Physical Model ....................................................140 Figure 6-2 2-stage Linear IP Open Loop Simulation.........................................148 Figure 6-3 2-stage Linear IP Open Loop Simulation Result .............................150 Figure 6-4 2-stage Linear IP LQR Control Simulation......................................151 Figure 6-5 2-stage Linear IP LQR Control Simulation Result 1 .......................154 Figure 6-6 2-stage Linear IP LQR Control Simulation Result 2 .......................155 Figure 6-7 2-stage Linear IP LQR Real Time Control Diagram........................156 Figure 6-8 2-sage Linear IP LQR Control Parameters ......................................157 Figure 6-9 2-stage Linear IP LQR Control Result (cart position) .....................158 Figure 6-10 2-stage Linear IP LQR Control Result (pendulum rod 1 angle) ....158 Figure 6-11 2-stage Linear IP LQR Control Result (pendulum rod 1 angle) ....158 Figure 6-12 2-stage Linear IP LQR Control Result (with disturbance).............159 Figure 6-13 2-stage Linear IP Controller Replacement .....................................160 Googol 2006
174
Appendix
Figure 7-1 3-stage Linear IP Physical Model ....................................................161
Table 1-1 Warning Signs....................................................................................... II Table 1-1 Inverted Pendulum Experiments............................................................4 Table 3-1 Linear IP Root Locus Realtime Control Experiment Report...............58
8.2 List of Program

PRO 3-1 1-stage Linear Inverted Pendulum Modeling Program.........................32 PRO 3-2 1-stage Linear IP Root Locus Calculation Mathematics Program........43 PRO 3-3 1-stage Linear IP Root Locus Calculation MATLAB Program............44 PRO 3-4 1-stage Linear IP Root Locus Control MATLAB Program ..................47 PRO 3-5 1-stage Linear IP Frequency Response Control MATLAB Program ...65 PRO 3-6 1-stage Linear IP PID Control MATLAB Simulation ..........................79 PRO 3-7 1-stage Linear IP State Space Pole Placement MATLAB Program 1 89 PRO 3-8 1-stage Linear IP State Space Pole Placement MATLAB Program 2 ...................................................................................................92 PRO 3-9 1-stage Linear IP Pole Placement MATLAB Program 3Ackermann formula .....................................................................................................94 PRO 3-10 1-stage Linear IP Pole Placement MATLAB Program 4Ackermann formula .....................................................................................................95 PRO 3-11 1-stage Linear IP LQR Control MATLAB Program.........................103 PRO 4-1 1-stage Linear Pendulum Modeling Program.....................................113 PRO 4-2 1-stage Linear Pendulum LQR Control MATLAB Program..............127 PRO 6-1 2-stage Linear IP Modeling Mathematica Program............................144 PRO 6-2 2-stage Linear IP LQR Control MATLAB Program...........................152 PRO 7-1 3-stage Linear IP Modeling Mathematica Program............................166
Googol 2006
175

IP Experiment Manual V2.00

Uploaded by

Document Information

Original Description:

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

IP Experiment Manual V2.00

Uploaded by

Copyright:

Available Formats

GOOGOL

Second Edition, July, 2006

6.3 6.4 6.5 6.6 6.7

1.2 Inverted Pendulum Classification

Figure 1-1 Linear IP Series

Figure 1-2 Circular IP Series

Figure 1-3 Planar IP Series

Figure 1-4 Configurable IP

1.3 Inverted Pendulum Properties

1.4 Controller Design Method

1.5 Inverted Pendulum Experiments

Type Basic Experiment

9. 10. 11. 12. 13. 14.

1-stage IP 1-stage IP 1-stage IP 1-stage IP 1-stage IP 2-stage IP

Linear 3-stage IP can be taken apart to 2-stage IP and 1-stage IP.

Chapter 2 Motion Control Basic Experiment

Chapter 2 Motion Control Basic Experiment

2.1 Encoder Principle and Application

Figure 2-1 Photoelectric Encoder Component and Structure

Chapter 2 Motion Control Basic Experiment

2.1.2 Angle Conversion

2 3.14 360 o N (degree) N (radian) or = nm nm

In which rotation angle of encoder

2.1.3 Encoder Experiment

Chapter 2 Motion Control Basic Experiment

Figure 2-2 MATLAB Simulink Environment

2) Press on the window to create a new window

Chapter 2 Motion Control Basic Experiment

Chapter 2 Motion Control Basic Experiment

Select Axis No.

6) Pull Scope from Simulink\Sinks to untitled

Chapter 2 Motion Control Basic Experiment

Chapter 2 Motion Control Basic Experiment

11) Press Browse to change the setting toReal-Time Windows Target

Chapter 2 Motion Control Basic Experiment

12) Then press OK as shown below

Chapter 2 Motion Control Basic Experiment

14) Power on the electric box 15) Press

16) Press to run 17) Double clickScope to watch the data

Chapter 2 Motion Control Basic Experiment

System model is as follow

Chapter 2 Motion Control Basic Experiment

2.2 Motor Control in Matlab Simulink

Chapter 2 Motion Control Basic Experiment

2) Click on the window to create aModelwindow:

Chapter 2 Motion Control Basic Experiment

Chapter 2 Motion Control Basic Experiment

Chapter 2 Motion Control Basic Experiment

7) Connect the following 3 modules as the following figure:

8) Set the 2Constantmodules parameter as follow:

Chapter 2 Motion Control Basic Experiment

Chapter 2 Motion Control Basic Experiment

Chapter 2 Motion Control Basic Experiment

12) ClickBrowseto change the setting as Real-Time Windows Target

13) ClickOKin the following figure:

Chapter 2 Motion Control Basic Experiment

Chapter 3 Linear Inverted Pendulum Modeling, Simulation and Experiment

Chapter 3 Linear Inverted Pendulum Modeling, Simulation and Experiment

3.1 1-stage Linear Inverted Pendulum System Model

3.1.1 Differential Equations Methods

Chapter 3 Linear Inverted Pendulum Modeling, Simulation and Experiment

F Cart x Sliding shaft

Figure 3-11-stage Linear Inverted Pendulum Model