www.gusucode.com > mpc_featured 案例源码 matlab代码程序 > mpc_featured/GainScheduledMPCControlOfAnInvertedPendulumOnACartExample.m
%% Gain Scheduled MPC Control of an Inverted Pendulum on a Cart % This example uses a gain scheduled model predictive controller to control % an inverted pendulum on a cart. % Copyright 1986-2015 The MathWorks, Inc. %% Product Requirement % This example requires Simulink(R) Control Design(TM) software to define the % MPC structure by linearizing a nonlinear Simulink model. if ~mpcchecktoolboxinstalled('slcontrol') disp('Simulink Control Design(R) is required to run this example.') return end %% Pendulum/Cart Assembly % The plant for this example is the following cart/pendulum assembly, where % _x_ is the cart position and _theta_ is the pendulum angle. % % <<../pendulumDiagram.png>> % % This system is controlled by exerting a variable force _F_ on the cart. % The controller needs to keep the pendulum upright while moving the cart % to a new position or when the pendulum is nudged forward by an impulse % disturbance _dF_ applied at the upper end of the inverted pendulum. %% % This plant is modeled in Simulink with commonly used blocks. mdlPlant = 'mpc_pendcartPlant'; load_system(mdlPlant); open_system([mdlPlant '/Pendulum and Cart System'],'force'); %% Control Objectives % Assume the following initial conditions for the cart/pendulum assembly: % % * The cart is stationary at _x_ = |0|. % % * The inverted pendulum is stationary at the upright position % _theta_ = |0|. % % The control objectives are: % % * Cart can be moved to a new position between |-15| and |15| with a step % setpoint change. % % * When tracking such a setpoint change, the rise time should be less than % 4 seconds (for performance) and the overshoot should be less than |5| % percent (for robustness). % % * When an impulse disturbance of magnitude of |2| is applied to the % pendulum, the cart should return to its original position with a maximum % displacement of |1|. The pendulum should also return to the upright % position with a peak angle displacement of |15| degrees (|0.26| radian). %% % The upright position is an unstable equilibrium for the inverted % pendulum, which makes the control task more challenging. %% The Choice of Gain Scheduled MPC % In <docid:mpc_examples.bu7ultl>, a single MPC controller % is able to move the cart to a new position between -10 and 10. However, % if you increase the step setpoint change to 15, the pendulum fails to % recover its upright position during the transition. % % To reach the longer distance within the same rise time, the controller % applies more force to the cart at the beginning. As a result, the % pendulum is displaced from its upright position by a larger angle such as % |60| degrees. At such angles, the plant dynamics differ significantly % from the LTI predictive model obtained at _theta_ = |0|. As a result, % errors in the prediction of plant behavior exceed what the built-in MPC % robustness can handle, and the controller fails to perform properly. % % A simple workaround to avoid the pendulum falling is to restrict pendulum % displacement by adding soft output constraints to _theta_ and reducing % the ECR weight on constraint softening. %% % % mpcobj.OV(2).Min = -pi/2; % mpcobj.OV(2).Max = pi/2; % mpcobj.Weights.ECR = 100; % %% % However, with these new controller settings it is no longer possible to % reach the longer distance within the required rise time. In other words, % controller performance is sacrificed to avoid violation of soft output % constraints. % % To move the cart to a new position between -15 and 15 while maintaining % the same rise time, the controller needs to have more accurate models at % different angles so that the controller can use them for better % prediction. Gain scheduled MPC allows you to solve a nonlinear control % problem by designing multiple MPC controllers at different operating % points and switching between them at run time. %% Control Structure % For this example, use a single MPC controller with: %% % * One manipulated variable: Variable force _F_. % * Two measured outputs: Cart position _x_ and pendulum angle _theta_. % * One unmeasured disturbance: Impulse disturbance _dF_. mdlMPC = 'mpc_pendcartGainSchedulingMPC'; open_system(mdlMPC); %% % Although cart velocity _x_dot_ and pendulum angular velocity _theta_dot_ % are available from the plant model, to make the design case more % realistic, they are excluded as MPC measurements. %% % While the cart position setpoint varies (step input), the pendulum angle % setpoint is constant (|0| = upright position). %% Linear Plant Model % Since the MPC controller requires a linear time-invariant (LTI) plant % model for prediction, linearize the Simulink plant model at three % different operating points. %% % Specify linearization input and output points io(1) = linio([mdlPlant '/dF'],1,'openinput'); io(2) = linio([mdlPlant '/F'],1,'openinput'); io(3) = linio([mdlPlant '/Pendulum and Cart System'],1,'openoutput'); io(4) = linio([mdlPlant '/Pendulum and Cart System'],3,'openoutput'); %% % Create specifications for the following three operating points where both % cart and pendulum are stationary: %% % * Pendulum is at 80 degrees, pointing right (_theta_ = |-4*pi/9|) % * Pendulum is upright (_theta_ = |0|) % * Pendulum is at 80 degrees, pointing left (_theta_ = |4*pi/9|) angles = [-4*pi/9 0 4*pi/9]; for ct=1:length(angles) %% % Create operating point specification. opspec(ct) = operspec(mdlPlant); %% % The first state is cart position _x_. opspec(ct).States(1).Known = true; opspec(ct).States(1).x = 0; %% % The second state is cart velocity _x_dot_ (not at steady state). opspec(ct).States(2).SteadyState = false; %% % The third state is pendulum angle _theta_. opspec(ct).States(3).Known = true; opspec(ct).States(3).x = angles(ct); %% % The fourth state is angular velocity _theta_dot_ (not at steady % state). opspec(ct).States(4).SteadyState = false; end %% % Compute operating point using these specifications. options = findopOptions('DisplayReport',false); [op,opresult] = findop(mdlPlant,opspec,options); %% % Obtain the linear plant model at the specified operating points. plants = linearize(mdlPlant,op,io); %% bdclose(mdlPlant); %% Multiple MPC Designs % At each operating point, design an MPC controller with the corresponding % linear plant model. status = mpcverbosity('off'); for ct=1:length(angles) %% % Get a single plant model. plant = plants(:,:,ct); plant.InputName = {'dF'; 'F'}; plant.OutputName = {'x'; 'theta'}; %% % The plant has two inputs, _dF_ and _F_, and two outputs, _x_ and _theta_. % In this example, _dF_ is specified as an unmeasured disturbance used % by the MPC controller for prediction. Set the plant % signal types. plant = setmpcsignals(plant,'ud',1,'mv',2); %% % To control an unstable plant, the controller sample time cannot be % too large (poor disturbance rejection) or too small (excessive % computation load). Similarily, the prediction horizon cannot be too % long (the plant unstable mode would dominate) or too short % (constraint violations would be unforeseen). Use the following % parameters for this example: Ts = 0.01; PredictionHorizon = 50; ControlHorizon = 5; mpcobj = mpc(plant,Ts,PredictionHorizon,ControlHorizon); %% % Specify nominal input and output values based on the operating point. mpcobj.Model.Nominal.Y = [0;opresult(ct).States(3).x]; mpcobj.Model.Nominal.X = [0;0;opresult(ct).States(3).x;0]; mpcobj.Model.Nominal.DX = [0;opresult(ct).States(2).dx;0;opresult(ct).States(4).dx]; %% % There is a limitation on how much force we can apply to the cart, % which is specified as hard constraints on manipulated variable _F_. mpcobj.MV.Min = -200; mpcobj.MV.Max = 200; %% % It is good practice to scale plant inputs and outputs before % designing weights. In this case, since the range of the manipulated % variable is greater than the range of the plant outputs by two orders % of magnitude, scale the MV input by 100. mpcobj.MV.ScaleFactor = 100; %% % To improve controller robustness, increase the weight on the MV rate % of change from |0.1| to |1|. mpcobj.Weights.MVRate = 1; %% % To achieve balanced performance, adjust the weights on the plant % outputs. The first weight is associated with cart position _x_ and % the second weight is associated with angle _theta_. mpcobj.Weights.OV = [1.2 1]; %% % To achieve more aggressive disturbance rejection, increase the state % estimator gain by by multiplying the default disturbance model gains % by a factor of |10|. %% % Update the input disturbance model. disturbance_model = getindist(mpcobj); setindist(mpcobj,'model',disturbance_model*10); %% % Update the output disturbance model. disturbance_model = getoutdist(mpcobj); setoutdist(mpcobj,'model',disturbance_model*10); %% % Save the MPC controller to the MATLAB workspace. assignin('base',['mpc' num2str(ct)],mpcobj); end mpcverbosity(status); %% Closed-Loop Simulation % Validate the MPC design with a closed-loop simulation in Simulink. open_system([mdlMPC '/Scope']); sim(mdlMPC); %% % In the nonlinear simulation, all the control objectives are successfully % achieved. %% bdclose(mdlMPC);