How to add disturbance to system to check robustness of my MPC controller

5 次查看（过去 30 天）
Ashutosh
Ashutosh 2023-10-12
回答： Sam Chak 2023-10-13
I'm a newcomer to MATLAB and Control Theory, and I'm currently working on my university project, which involves implementing Model Predictive Control (MPC) for a 2-wheel self-balancing robot. To assess the controller's robustness, I'm aiming to introduce an input impulse disturbance. However, I'm encountering difficulties in doing so. I'm including my code here kindly suggest me fix, thank you.
clear; close all; clc;
%% Inverted Pendulum Model Parameters
M = 0.5;
m = 0.2;
b = 0.1;
I = 0.006;
g = 9.8;
l = 0.3;
p = I*(M+m)+M*m*l^2; %denominator for the A and B matrices
A = [0 1 0 0;
0 -(I+m*l^2)*b/p (m^2*g*l^2)/p 0;
0 0 0 1;
0 -(m*l*b)/p m*g*l*(M+m)/p 0];
B = [ 0;
(I+m*l^2)/p;
0;
m*l/p];
C = [1 0 0 0;
0 0 1 0];
D = [0;
0];
%% Discretize the system
Ts = 0.1; % Sampling time [s]
sys = ss(A, B, C, D);
sys_d = c2d(sys, Ts, 'zoh'); % Discretized system
Ad = sys_d.a;
Bd = sys_d.b;
Cd = sys_d.c;
%% Design MPC Controller
N = 20; % Prediction horizon
Nu = 5; % Control horizon
Q = diag([10, 1, 100, 1]); % State weighting matrix
R = 0.1; % Control input weighting
% Create MPC controller object
mpcobj = mpc(sys_d, Ts);
mpcobj.PredictionHorizon = N;
mpcobj.ControlHorizon = Nu;
mpcobj.Weights.OutputVariables = [1000, 100];
mpcobj.Weights.ManipulatedVariables = R;
mpcobj.Weights.ManipulatedVariablesRate = 0.1;
% Constraints
mpcobj.MV(1).Min = -10; % Force min [N]
mpcobj.MV(1).Max = 10; % Force max [N]
%% Simulation
Nsim = 100; % Number of simulation steps
x0 = [0.2; 0; 0.1; 0]; % Initial state
% Preallocate arrays for simulation results
nx = size(Ad, 1);
nu = size(Bd, 2);
ny = size(Cd, 1);
x = zeros(nx, Nsim);
y = zeros(ny, Nsim);
u = zeros(nu, Nsim);
x(:,1) = x0;
y(:,1) = Cd * x0;
% Create initial MPC state
mpcstate = mpcstate(mpcobj, x(:,1));
% Main simulation loop
for k = 1:Nsim-1
% Measure the current output
ymeas = y(:,k);
% Compute the control action using the MPC controller
[u(:,k), mpc_info] = mpcmove(mpcobj, mpcstate, ymeas, []);
% Apply the control action to the system and obtain the next state
x(:,k+1) = Ad * x(:,k) + Bd * u(:,k);
% Calculate the output
y(:,k+1) = Cd * x(:,k+1);
% Update MPC state
mpcstate.Plant = x(:,k+1);
end
% Plot the results
figure(1)
subplot(3,1,1);
t = (0:Nsim-1) * Ts;
figure(1);
plot(t, y(1,:), 'LineWidth', 2);
ylabel('Cart Position (m)');
title('Inverted Pendulum MPC Control');
grid on;
subplot(3,1,2);
plot(t, y(2,:), 'LineWidth', 2);
ylabel('Pendulum Angle (rad)');
grid on;
subplot(3,1,3);
plot(t, u(:,1:Nsim), 'LineWidth', 2); % Corrected dimensions of u
xlabel('Time (s)');
ylabel('Force (N)');
grid on;

请先登录，再回答此问题。

回答（1 个）

Sam Chak
Sam Chak 2023-10-13
Ran in:
Hi @Ashutosh
I seldom employ MPC due to the challenges associated with deterministically controlling the controller's behavior from the Applied Mathematician's point of view. Nevertheless, I believe you can try describing the expected unmeasured input disturbance using setmpcsignals() and mpcobj.Model.Disturbance.
In the pendulum case, where the input disturbance is assumed to enter the system through the same channel as the control input, denoted as 'u' or the "Manipulated Variable" in MPC terminology, you will observe an additional column introduced in the state-space representation.
%% Inverted Pendulum
% Parameters
M = 0.5;
m = 0.2;
b = 0.1;
I = 0.006;
g = 9.8;
l = 0.3;
p = I*(M + m) + M*m*l^2; % denominator for the A and B matrices
% State-space representation
A = [0 1 0 0;
0 -(I+m*l^2)*b/p (m^2*g*l^2)/p 0;
0 0 0 1;
0 -(m*l*b)/p m*g*l*(M+m)/p 0];
B = [ 0 0; % Added one column for the input disturbance
(I+m*l^2)/p (I+m*l^2)/p;
0 0;
m*l/p m*l/p];
C = [1 0 0 0;
0 0 1 0];
D = 0*C*B;
sys = ss(A, B, C, D)
sys = A = x1 x2 x3 x4 x1 0 1 0 0 x2 0 -0.1818 2.673 0 x3 0 0 0 1 x4 0 -0.4545 31.18 0 B = u1 u2 x1 0 0 x2 1.818 1.818 x3 0 0 x4 4.545 4.545 C = x1 x2 x3 x4 y1 1 0 0 0 y2 0 0 1 0 D = u1 u2 y1 0 0 y2 0 0 Continuous-time state-space model.
%% Discretize the system
Ts = 0.1; % Sampling time [s]
sys_d = c2d(sys, Ts, 'zoh'); % Discretized system
Ad = sys_d.a;
Bd = sys_d.b;
Cd = sys_d.c;
%% Design MPC Controller
N = 20; % Prediction horizon
Nu = 5; % Control horizon
Q = diag([10, 1, 100, 1]); % State weighting matrix
R = 0.1; % Control input weighting
% Create MPC controller object
sys_d = setmpcsignals(sys_d, MV=[1], UD=[2]);
mpcobj = mpc(sys_d, Ts);
-->"PredictionHorizon" is empty. Assuming default 10. -->"ControlHorizon" is empty. Assuming default 2. -->"Weights.ManipulatedVariables" is empty. Assuming default 0.00000. -->"Weights.ManipulatedVariablesRate" is empty. Assuming default 0.10000. -->"Weights.OutputVariables" is empty. Assuming default 1.00000. for output(s) y1 and zero weight for output(s) y2
mpcobj.PredictionHorizon = N;
mpcobj.ControlHorizon = Nu;
mpcobj.Weights.OutputVariables = [1000, 100];
mpcobj.Weights.ManipulatedVariables = R;
mpcobj.Weights.ManipulatedVariablesRate = 0.1;
% Constraints
mpcobj.MV(1).Min = -10; % Force min [N]
mpcobj.MV(1).Max = 10; % Force max [N]
% Input Disturbance
mpcobj.Model.Disturbance = tf(1) % Laplace transform of Dirac delta function is 1
MPC object (created on 13-Oct-2023 16:36:45): --------------------------------------------- Sampling time: 0.1 (seconds) Prediction Horizon: 20 Control Horizon: 5 Plant Model: -------------- 1 manipulated variable(s) -->| 4 states | | |--> 2 measured output(s) 0 measured disturbance(s) -->| 2 inputs | | |--> 0 unmeasured output(s) 1 unmeasured disturbance(s) -->| 2 outputs | -------------- Indices: (input vector) Manipulated variables: [1 ] Unmeasured disturbances: [2 ] (output vector) Measured outputs: [1 2 ] Disturbance and Noise Models: Output disturbance model: default (type "getoutdist(mpcobj)" for details) Input disturbance model: user specified (type "getindist(mpcobj)" for more details) Measurement noise model: default (unity gain after scaling) Weights: ManipulatedVariables: 0.1000 ManipulatedVariablesRate: 0.1000 OutputVariables: [1000 100] ECR: 100000 State Estimation: Default Kalman Filter (type "getEstimator(mpcobj)" for details) Constraints: -10 <= MV1 <= 10, MV1/rate is unconstrained, MO1 is unconstrained MO2 is unconstrained Use built-in "active-set" QP solver with MaxIterations of 120.
indist = tf(getindist(mpcobj))
indist = From input "Noise#1" to output "UD1": 1 Static gain.
%% Simulation
Nsim = 100; % Number of simulation steps
x0 = [0.2; 0; 0.1; 0]; % Initial state
% Preallocate arrays for simulation results
nx = size(Ad, 1);
nu = size(Bd, 2);
ny = size(Cd, 1);
x = zeros(nx, Nsim);
y = zeros(ny, Nsim);
u = zeros(nu, Nsim);
x(:,1) = x0;
y(:,1) = Cd*x0;
% Create initial MPC state
mpcstate = mpcstate(mpcobj, x(:,1));
Assuming no disturbance added to measured output #1. -->Assuming output disturbance added to measured output #2 is integrated white noise. -->"Model.Noise" is empty. Assuming white noise on each measured output.
%% Main simulation loop
for k = 1:Nsim-1
% Measure the current output
ymeas = y(:,k);
% Compute the control action using the MPC controller
[u(:,k), mpc_info] = mpcmove(mpcobj, mpcstate, ymeas, []);
% Apply the control action to the system and obtain the next state
x(:,k+1) = Ad*x(:,k) + Bd*u(:,k);
% Calculate the output
y(:,k+1) = Cd*x(:,k+1);
% Update MPC state
mpcstate.Plant = x(:,k+1);
end
%% Plot the results
figure(1)
subplot(3,1,1);
t = (0:Nsim-1)*Ts;
plot(t, y(1,:), 'LineWidth', 2);
ylabel('Cart Position (m)');
title('Inverted Pendulum MPC Control');
grid on;
subplot(3,1,2);
plot(t, y(2,:), 'LineWidth', 2);
ylabel('Pendulum Angle (rad)');
grid on;
subplot(3,1,3);
plot(t, u(:,1:Nsim), 'LineWidth', 2); % Corrected dimensions of u
xlabel('Time (s)');
ylabel('Force (N)');
grid on;

类别

在 Help Center 和 File Exchange 中查找有关 Controller Creation 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by