LQRDESIGN

1/30/2014Control Tutorials for MATLAB and Simulink - Inverted Pendulum: State-Space Methods for Controller Design Tips TIPS Effects ABO UT Search Control Tutorials BASIC S INDEX NEXT ► INTR O DUC TIO N SYSTEM MO DELING ANALYSIS C R UISE C O NTR O L Inverted Pendulum: State-Space Methods for Controller Design Key MATLAB commands used in this tutorial are: s s, e i g, l s i m, l q r ,c t r b, p l o t y y, o b s v, p l a c e CONTROL PID R O O T LO C US FR EQ UENC Y STATE-SPAC E DIGITAL Contents Open-loop poles Linear Quadratic Regulation (LQR) Adding precompensation Observer-based control SIMULINK MO DELING C O NTR O L From the main problem, the dynamic equations of the inverted pendulum system in state-space form are the following: (1) (2) To see how this problem was originally set up and the system equations were derived, consult the Inverted Pendulum: System Modeling page. For this problem displacement ( where the outputs are the cart's in radians) in meters) and the pendulum angle ( represents the deviation of the pedulum's position from . 1/15 equilibrium, that is, http://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum&section=ControlStateSpace 35 radians) Pendulum angle from the vertical Steady-state error of less than 2% for and As you may have noticed if you went through some of the other inverted pendulum examples. the design criteria for this example are different. we are attempting to keep the pendulum vertical while controlling the cart's position to move 0. We did not attempt to control the cart's position. represents the step command of the cart's position. rather than using the system's outputs for feedback.Inverted Pendulum: State-Space Methods for Controller Design The design criteria for this system for a 0.2 meters to the right.5 seconds never more than 20 degrees (0.umich. In the other examples we were attemping to keep the pendulum vertical in response to an impulsive disturbance force applied to the cart.edu/CTMS/index.2-m step in desired cart position are as follows: and of less than 5 seconds Settling time for Rise time for of less than 0.php?example=InvertedPendulum&section=ControlStateSpace contains 2/15 both the position of the cart and the angle of the pendulum. We want . In this example.1/30/2014 Control Tutorials for MATLAB and Simulink . Open-loop poles In this problem. The 4 states represent the position and velocity of the cart and the angle and angular velocity of the pendulum. Note that here we feedback all of the system's states. This problem can be solved using full-state feedback.engin. The output http://ctms. The schematic of this type of control system is shown below where is a matrix of control gains. A statespace design approach is well suited to the control of multiple outputs as we have here. the pendulum should be displaced.edu/CTMS/index. ( I + m * l ^ 2 ) / p .php?example=InvertedPendulum&section=ControlStateSpace s y s _ s s=s s ( A . After execution in the MATLAB command window. http://ctms.Inverted Pendulum: State-Space Methods for Controller Design both the position of the cart and the angle of the pendulum. 1 . 1 . To view the system's open-loop response please refer to the Inverted Pendulum: System Analysis page. ' i n p u t n a m e ' . The first step in designing a full-state feedback controller is to determine the open-loop poles of the system. s t a t e s={ ' x '' x _ d o t '' p h i '' p h i _ d o t ' } . g=9 . the output will list the open-loop poles (eigenvalues of shown below. m * g * l * ( M + m ) / p 0 ] . vertical) and the cart should move to its new commanded position. m=0 . b=0 . 0 .engin. i n p u t s={ ' u ' } . 2 .e. C=[ 1000 .' p h i ' } . ' o u t p u t n a m e ' 3/15 . C . but eventually return to zero (i. i n p u t s . 0010 ] . D .% d e n o m i n a t o rf o rt h eAa n dBm a t r i c e s A=[ 0 1 0 0 . p=I * ( M + m ) + M * m * l ^ 2 . 0 . 8 . ) as M=0 .1/30/2014 Control Tutorials for MATLAB and Simulink . We want to design a controller so that when a step reference is given to the system. 3 . Enter the following lines of code into an m-file. ' s t a t e n a m e ' .umich. I=0 . s t a t e s . 0( I + m * l ^ 2 ) * b / p ( m ^ 2 * g * l ^ 2 ) / p 0 0 0 0( m * l * b ) / p B=[ 0 . m * l / p ] . 5 . o u t p u t s={ ' x ' . 0 0 6 . B . 0 ] . l=0 . D=[ 0 . 5651. B . ' i n p u t n a m e ' . the controllability matrix must have rank where the rank of a matrix is the number of independent rows (or columns). and reference equal to zero (consult your textbook for more details). 5 6 5 1 As you can see. The controllability matrix of the system takes the form shown below. we will first verify that the system is controllable. can measure) all four of the state variables.umich. s t a t e s . Adding additional terms to the controllability matrix with higher powers of the matrix will not increase the rank of the controllability matrix since these additional terms will just be linear combinations of the earlier terms. This should confirm your intuition that the system is unstable in open loop. 6 0 4 1 0 . C .php?example=InvertedPendulum&section=ControlStateSpace 4/15 . the rank of the matrix must be 4.engin. http://ctms.edu/CTMS/index. 1 4 2 8 5 . quadratic cost function. Before we design our controller. there is one right-half plane pole at 5. i n p u t s . If you know the desired closed-loop pole locations. Another option is to use the l q rcommand which returns the optimal controller gain assuming a linear plant.Inverted Pendulum: State-Space Methods for Controller Design s y s _ s s=s s ( A . ' o u t p u t n a m e ' p o l e s=e i g ( A ) p o l e s= 0 5 . you can use the MATLAB commands p l a c eor a c k e r . (3) Since our controllability matrix is 4x4. This can be accomplished in a number of ways. Linear Quadratic Regulation (LQR) The next step in the design process is to find the vector of statefeedback control gains assuming that we have access (i. For the system to be completely state controllable.1/30/2014 Control Tutorials for MATLAB and Simulink .e. Satisfaction of this property means that we can drive the state of the system anywhere we like in finite time (under the physical constraints of the system). The number corresponds to the number of state variables of the system. ' s t a t e n a m e ' . D . engin. Adding the following additional commands to your m-file and running in the MATLAB command window will produce the following output. and places equal importance on the control and the state variables which are outputs (the pendulum's angle and the cart's position).Inverted Pendulum: State-Space Methods for Controller Design Since our controllability matrix is 4x4. the lqr method allows for the control of both outputs. Specifically. . The simplest case is to assume . enter the following into the MATLAB observe the structure of command window to see the output given below. The controller can be tuned by changing the nonzero elements in the matrix to achieve a desirable response. In this case.php?example=InvertedPendulum&section=ControlStateSpace 5/15 . in the cost function that you are trying to optimize. the rank of the matrix must be 4. Q=C ' * C Q= 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 http://ctms. We will use the MATLAB command c t r bto generate the controllability matrix and the MATLAB command r a n kto test the rank of the matrix. To . c o n t r o l l a b i l i t y=r a n k ( c o ) c o n t r o l l a b i l i t y= 4 Therefore.edu/CTMS/index.1/30/2014 Control Tutorials for MATLAB and Simulink . The and . we will use the linear quadratic regulation method for determining our state-feedback control gain matrix MATLAB function l q rallows you to choose two parameters. we have verified that our system is controllable and thus we should be able to design a controller that achieves the given requirements.umich. it is pretty easy to do. The cost function corresponding to this and . c o=c t r b ( s y s _ s s ) . Essentially. respectively. which will balance the relative importance of the control effort ( ) and error (deviation from 0). C c=[ C ] . D c . y ( : . 1 ) . ' p e n d u l u ma n g l e( r a d i a n s ) ' t i t l e ( ' S t e pR e s p o n s ew i t hL Q RC o n t r o l ' ) K= 1 . B c=[ B ] . ' c a r tp o s i t i o n( m ) ' ) s e t ( g e t ( A X ( 2 ) . ' S t r i n g ' . Now that we know how to interpret the can experiment to find the matrix that will give us a "good" matrix and plot the controller. 2 ) . x ] = l s i m ( s y s _ c l .umich. The input weighting 1. We will go ahead and find the response all in one step so that changes can be made in the control and seen automatically in the response. we absolute values. R ) A c=[ ( A B * K ) ] .Inverted Pendulum: State-Space Methods for Controller Design The element in the (1. C c .edu/CTMS/index. and the response plot shown Q=C ' * C .1) position of represents the weight on the cart's position and the element in the (3. ' S t r i n g ' . t ) .3) position represents the weight on the pendulum's angle. s t a t e s . t . t=0 : 0 . not their matrix. Q . i n p u t s . 6 8 5 4 3 . B . s e t ( g e t ( A X ( 1 ) . B c . K=l q r ( A . [ y .php?example=InvertedPendulum&section=ControlStateSpace . i n p u t s={ ' r ' } . o u t p u t s={ ' x ' . 4 5 9 4 6/15 http://ctms. ' Y l a b e l ' ) . 0 0 0 0 1 . r= 0 .1/30/2014 Control Tutorials for MATLAB and Simulink . D c=[ D ] . ' p l o t ' ) .engin. 6 5 6 7 1 8 . s t a t e s={ ' x '' x _ d o t '' p h i '' p h i _ d o t ' } . H 2 ]=p l o t y y ( t . s y s _ c l=s s ( A c . 0 1 : 5 . [ A X . Add the following commands to the end of your m-file and run in the MATLAB command window to get the following value for below. ' Y l a b e l ' ) . r . ' s t a t e n a m e ' . t . ' i n p u t n a m e ' . Ultimately what matters is the relative value of will remain at and . R=1 . H 1 . y ( : . 2 * o n e s ( s i z e ( t ) ) .' p h i ' } . but their settling times need improvement and the cart's rise time needs to be reduced. In other words. you are putting more weight on the errors at the cost of increased control effort . and lowers the angle the pendulum moves. B .umich. Go back to your m-file and change the matrix to see if you can get a better response. the cart's final position is also not near the desired location but has in fact moved in the opposite direction.1/30/2014 Control Tutorials for MATLAB and Simulink . As I'm sure you have noticed.edu/CTMS/index.engin. 4 5 9 4 The curve in green represents the pendulum's angle in radians. As you can see. and the curve in blue represents the cart's position in meters. Modifying your m-file so that the (1. this plot is not satisfactory.1) and (3. Q ( 1 .Inverted Pendulum: State-Space Methods for Controller Design 1 . 6 5 6 7 1 8 .3) and the step element is 100. 6 8 5 4 3 . 0 0 0 0 1 . 3 )=1 0 0 R=1 . Q ( 3 . will produce the following value of response shown below. Q . Q=C ' * C . 1 )=5 0 0 0 .1) element of is 5000 and the (3. K=l q r ( A . The pendulum and cart's overshoot appear fine. You will find that increasing the (1. R ) http://ctms.php?example=InvertedPendulum&section=ControlStateSpace A c=[ ( A B * K ) ] . This error will be dealt with in the next section and right now we will focus on the settling and rise times.3) elements makes the settling and rise times go down. 7/15 . ' i n p u t n a m e ' .umich. H 1 . C c=[ C ] . 9 2 3 8 0 0 0 0 0 0 1 0 0 0 0 0 0 0 http://ctms. 2 * o n e s ( s i z e ( t ) ) .' p h i ' } . s y s _ c l=s s ( A c . 8 3 4 5 1 0 5 . 1 ) . t . t ) . H 2 ]=p l o t y y ( t . B c . i n p u t s={ ' r ' } . ' Y l a b e l ' ) . 0 1 : 5 . ' Y l a b e l ' ) . [ y . t=0 : 0 . y ( : . D c .edu/CTMS/index. 7 1 0 7 3 7 . [ A X . ' p l o t ' ) . i n p u t s . y ( : . s t a t e s={ ' x '' x _ d o t '' p h i '' p h i _ d o t ' } . D c=[ D ] .1/30/2014 Control Tutorials for MATLAB and Simulink . t . ' c a r tp o s i t i o n( m ) ' ) s e t ( g e t ( A X ( 2 ) . r . 5 2 9 8 2 0 . r= 0 .engin. C c .php?example=InvertedPendulum&section=ControlStateSpace 8/15 . ' s t a t e n a m e ' . x ] = l s i m ( s y s _ c l . o u t p u t s={ ' x ' . ' S t r i n g ' . s t a t e s .Inverted Pendulum: State-Space Methods for Controller Design A c=[ ( A B * K ) ] . B c=[ B ] . ' S t r i n g ' . 2 ) . ' p e n d u l u ma n g l e( r a d i a n s ) ' t i t l e ( ' S t e pR e s p o n s ew i t hL Q RC o n t r o l ' ) Q= 5 0 0 0 0 0 0 K= 7 0 . s e t ( g e t ( A X ( 1 ) . edu/CTMS/index.umich.engin.1/30/2014 Control Tutorials for MATLAB and Simulink . you could improve the response even more. http://ctms. with a fullstate feedback controller we are feeding back all of the states.m as shown below. but would require greater control force . B . 0 ) . s y s _ s s=s s ( A .). We need to compute what the steady-state value of the states should be. In contrast to the other design methods. This can be done by adding a constant gain relationship: after the reference. and use a new value as our "reference" for computing the input. multiply that by the chosen gain . The schematic below shows this We can find this factor by employing the used-defined function matrix is modified to reflect the fact rscale. The that the reference is a command only on cart position.Inverted Pendulum: State-Space Methods for Controller Design You may have noted that if you increased the values of the elements of even higher. C n . but now we must address the steady-state error. More control effort generally corresponds to greater cost (more energy. larger actuator. The reason this weighting was chosen was because it just satisfies the transient design requirements. Increasing the magnitude of more would make the tracking error smaller. Adding precompensation The controller we have designed so far meets our transient requirements. C n=[ 1000 ] . where we feedback the output and compare it to the reference input to compute an error.php?example=InvertedPendulum&section=ControlStateSpace 9/15 . etc. K ) N b a r= 7 0 .Inverted Pendulum: State-Space Methods for Controller Design s y s _ s s=s s ( A . H 2 ]=p l o t y y ( t . B c * N b a r . t ) . ' S t r i n g ' . C n . ' p l o t ' ) .edu/CTMS/index. 0 ) .engin. t . s y s _ c l=s s ( A c . s e t ( g e t ( A X ( 1 ) . r . ' c a r tp o s i t i o n( m ) ' ) s e t ( g e t ( A X ( 2 ) . N b a r=r s c a l e ( s y s _ s s . 10/15 . C c . mis not a standard function in MATLAB. B .1/30/2014 Control Tutorials for MATLAB and Simulink . More information can be found here. Extras: rscale. 2 ) . ' Y l a b e l ' ) .m. Now you can plot the step response by adding the above and following lines of code to your m-file and re-running at the command line. the rise and settle times are met. ' s t a t e n a m e ' . s t a t e s . 7 1 0 7 Note that the function r s c a l e . y ( : . ' S t r i n g ' . 1 ) . D c . r= 0 . ' p e n d u l u ma n g l e( r a d i a n s ) ' t i t l e ( ' S t e pR e s p o n s ew i t hP r e c o m p e n s a t i o na n dL Q RC o n t r o l ' ) Now. H 1 .php?example=InvertedPendulum&section=ControlStateSpace design criteria. 0 1 : 5 . t . y ( : . x ] = l s i m ( s y s _ c l .umich. the steady-state error is within our limits. ' i n p u t n a m e ' t=0 : 0 . [ y . [ A X . 2 * o n e s ( s i z e ( t ) ) . You will have to download it here and place it in your current directory. ' Y l a b e l ' ) . and the pendulum's overshoot is within range of the http://ctms. even in the presence of model uncertainty and step disturbances.umich. without the precompensator . A useful technique is to combine the precompensator with integral control to leverage the advantages of each approach. To address the situation where not all state variables are measured.Inverted Pendulum: State-Space Methods for Controller Design design criteria. You may recall that the addition of integral control may also be used to eliminate steady-state error. The observability matrix is defined as follows. Before we design our estimator. The precompensator on the other hand is able to anticipitate the steady-state offset using knowledge of the plant model. The tradeoff with using integral control is that the error must first develop before it can be corrected for. Therefore.php?example=InvertedPendulum&section=ControlStateSpace 11/15 . For an example of how to implement integral control in the state space setting. Similar to the process for verifying controllability. a system is observable if its observability matrix is full rank.1/30/2014 Control Tutorials for MATLAB and Simulink .engin. (4) http://ctms. a state estimator must be designed. the system may be slow to respond. therefore. Note that the precompensator employed above is calculated based on the model of the plant and further that the precompensator is located outside of the feedback loop. we will first verify that our system is observable. which is not necessarily valid. but was based on the assumption of full-state feedback.edu/CTMS/index. Observer-based control The response achieved above is good. A schematic of state-feedback control with a full-state estimator is shown below. see the Motor Position: State-Space Methods example. The property of observability determines whether or not based on the measured outputs of the system we can estimate the state of the system. if there are errors in the model (or unknown disturbances) the precompensator will not correct for them and there will be steady-state error. This can be verified by the fact that o b s v ( A . Now let's look at the dynamics of the error in the state estimate. we would not be able to estimate the full state of the system.edu/CTMS/index. it is full rank and our system is observable.umich.1/30/2014 Control Tutorials for MATLAB and Simulink . : ) )produces an observability matrix that is not full rank. The observability matrix in this case is not square since our system has two outputs. Specifically. we will now describe the process for designing a state estimator. the dynamics of the state estimate are described by the following equation. the last term corrects the state estimate based on the difference between the actual output and the estimated output . the state estimate error dynamics are described by (7) and the error will approach zero ( will approach ) if the matrix is stable (has negative eigenvalues). (6) Therefore.php?example=InvertedPendulum&section=ControlStateSpace (eigenvalues of ). As is with the case for control.Inverted Pendulum: State-Space Methods for Controller Design We can employ the MATLAB command o b s v to contruct the observability matrix and the r a n k command to check its rank as shown below.engin. the speed of convergence depends on the poles of the estimator http://ctms. o b s e r v a b i l i t y=r a n k ( o b ) o b s e r v a b i l i t y= 4 Since the observability matrix is 8x4 and has rank 4. C ( 2 . Since we know that we can estimate our system state. o b=o b s v ( s y s _ s s ) . (5) The spirit of this equation is similar to that of closed-loop control in that last term is a correction based on feedback. Since we plan to use the state estimate as the input 12/15 . Based on the above diagram. Note that if we could only measure the pendulum angle output. 7592. we would like the state estimate to converge faster than is desired from our overall closed-loop system. 8 3 0 9 i 4 . That is. we would like the observer poles to be faster than the controller poles. we will place our estimator poles at -40.php?example=InvertedPendulum&section=ControlStateSpace 13/15 . 7 5 9 2+0 . To do this.engin. P ) ' http://ctms. 9 2 8 3 i 8 . 9 2 8 3 i 4 . A common guideline is to make the estimator poles 4-10 times faster than the slowest controller pole. copy the following code to the end of your m-file. Since the closed-loop estimator dynamics are determined by a matrix () that has a similar form to the matrix that determines the dynamics of the state-feedback system ( ). 7 5 9 2-0 .Inverted Pendulum: State-Space Methods for Controller Design (eigenvalues of ). Making the estimator poles too fast can be problematic if the measurement is corrupted by noise or there are errors in the sensor measurement in general. you should see the following poles in the MATLAB command window. Since we plan to use the state estimate as the input to our controller. 8 3 0 9 i The slowest poles have real part equal to -4.edu/CTMS/index. matrix and generate the P=[ 4 04 14 24 3 ] . p o l e s=e i g ( A c ) p o l e s= 8 . Based on this logic. Specifically. Add the following commands to your m-file to calculate the output shown below.1/30/2014 Control Tutorials for MATLAB and Simulink . If you employed the updated matrix. C ' . since taking the transpose of leaves the eigenvalues unchanged and produces a result that exactly matches the form of .umich. Recalling that the p l a c ecommand cannot place poles of multiplicity greater than one. we can use the same commands for finding the estimator gain as we can for finding the state-feedback gain . L=p l a c e ( A ' . we must first find the controller poles. therefore. we will place the observer poles as follows. 4 9 1 0-7 . we can use the a c k e ror p l a c ecommands. 4 9 1 0+7 . Inverted Pendulum: State-Space Methods for Controller Design L= 1 . z e r o s ( s i z e ( B ) ) ] . 0 0 1 0 0 . ' i n p u t n a m e ' t=0 : 0 . 0 4 0 2 0 . C c e . s t a t e s={ ' x '' x _ d o t '' p h i '' p h i _ d o t '' e 1 '' e 2 '' e 3 '' e 4 ' } . D c e . Now we will combine our state-feedback controller from before with our state estimator to get the full compensator. C c e=[ C cz e r o s ( s i z e ( C c ) ) ] . 0 e + 0 3* 0 . 7 6 0 4 We are using both outputs (the angle of the pendulum and the position of the cart) to design the observer. 0 8 3 2 1 . http://ctms. (8) (9) The closed-loop system described above can be implemented in MATLAB by adding the following commands to the end of your m-file. The resulting closedloop system is described by the following matrix equations. 0 1 : 5 .php?example=InvertedPendulum&section=ControlStateSpace 14/15 . 0 ] . After running the m-file the step response shown will be generated. ' s t a t e n a m e ' . r=0 . B c e=[ B * N b a r .1/30/2014 Control Tutorials for MATLAB and Simulink . B c e .edu/CTMS/index. z e r o s ( s i z e ( A ) )( A L * C ) ] . 0 7 6 2 0 . 0 8 2 6 1 . 0 0 1 4 0 . 6 9 9 2 0 . D c e=[ 0 . s t a t e s . s y s _ e s t _ c l=s s ( A c e .engin. A c e=[ ( A B * K )( B * K ) . o u t p u t s={ ' x ' . i n p u t s={ ' r ' } .umich.' p h i ' } . 2 * o n e s ( s i z e ( t ) ) . ' p e n d u l u ma n g l e( r a d i a n s ) ' t i t l e ( ' S t e pR e s p o n s ew i t hO b s e r v e r B a s e dS t a t e F e e d b a c kC o n t r o l ' This response is almost identical to the response achieved when it was assumed that we had full access to the state variables.14 © Copyright 2012 A ll rights res erved. x ] = l s i m ( s y s _ e s t _ c l . [ A X . y ( : . ' S t r i n g ' .edu/CTMS/index. http://ctms. and because the model we assumed for the observer is identical to the model of the actual plant (including the same initial conditions). 2 ) .1/30/2014 Control Tutorials for MATLAB and Simulink . multi-output systems with the state-space method than with the other methods we have presented. y ( : . t . s e t ( g e t ( A X ( 1 ) .php?example=InvertedPendulum&section=ControlStateSpace 15/15 . No further iteration is needed.engin. Therefore. all of the design requirements have been met with the minimal control effort expended. H 1 .umich. H 2 ]=p l o t y y ( t . r . ' p l o t ' ) . ' S t r i n g ' . ' Y l a b e l ' ) . Published with MATLAB® 7. t ) . t . N o part of this public ation may be reproduc ed or trans mitted without the expres s written c ons ent of the authors . ' Y l a b e l ' ) . 1 ) . This example demonstrates that it is much easier to control multiinput. This is because the observer poles are fast.Inverted Pendulum: State-Space Methods for Controller Design [ y . ' c a r tp o s i t i o n( m ) ' ) s e t ( g e t ( A X ( 2 ) .