ch5_2.m

清华大学《matlab 控制系统应用与实例》第一部分M文件的源码

% DC-motor with elastic shaft
%
%Parameters (MKS)
%---------------------------------------------------------------

Lshaft=1.0;      %Shaft length
dshaft=0.02;     %Shaft diameter
shaftrho=7850;   %Shaft specific weight (Carbon steel)
G=81500*1e6;     %Modulus of rigidity
tauam=50*1e6;    %Shear strength
Mmotor=100;      %Rotor mass
Rmotor=.1;       %Rotor radius
Jmotor=.5*Mmotor*Rmotor^2; %Rotor axial moment of inertia                      
Bmotor=0.1;      %Rotor viscous friction coefficient (A CASO)
R=20;            %Resistance of armature
Kt=10;           %Motor constant
gear=20;         %Gear ratio
Jload=50*Jmotor; %Load NOMINAL moment of inertia
Bload=25;        %Load NOMINAL viscous friction coefficient
Ip=pi/32*dshaft^4;               %Polar momentum of shaft 
%(circular) section
Kth=G*Ip/Lshaft;                %Torsional rigidity 
%(Torque/angle)
Vshaft=pi*(dshaft^2)/4*Lshaft;   %Shaft volume
Mshaft=shaftrho*Vshaft;          %Shaft mass
Jshaft=Mshaft*.5*(dshaft^2/4);   %Shaft moment of inertia
JM=Jmotor; 
JL=Jload+Jshaft;
Vmax=tauam*pi*dshaft^3/16; %Maximum admissible torque
Vmin=-Vmax;

%Input/State/Output continuous time form
%---------------------------------------------------------------

AA=[0  1      0                 0;
    -Kth/JL       -Bload/JL     Kth/(gear*JL)     0;
    0             0             0                 1;
    Kth/(JM*gear) 0             -Kth/(JM*gear^2)  -(Bmotor+Kt^2/R)/JM];
                
BB=[0;0;0;Kt/(R*JM)];
Hyd=[1 0 0 0];
Hvd=[Kth 0 -Kth/gear 0];
Dyd=0;
Dvd=0;

% Define the LTI state-space model
sys=ss(AA,BB,[Hyd;Hvd],[Dyd;Dvd]);
ManipulatedVariables = struct('Min', -220, 'Max', 220, 'Units','V');
OutputVariables(1) = struct('Min', -Inf, 'Max', Inf, 'Units','rad');
OutputVariables(2) = struct('Min', -78.5, 'Max', 78.5, 'Units','Nm');
Weights = struct('Input', 0, 'InputRate', 0.05, 'Output', [10 0]);
Model.Plant = sys;
Model.Plant.OutputGroup = {[1], 'Measured' ; [2], 'Unmeasured'};  
Ts = 0.1;
PredictionHorizon = 10;
ControlHorizon = 2;

ServoMPC = mpc(Model, Ts, PredictionHorizon, ControlHorizon);

ServoMPC.PredictionHorizon = 12;

get(ServoMPC)

M = get(ServoMPC, 'ControlHorizon');


set(ServoMPC, 'Weights', Weights, ...
    'ManipulatedVariables', ManipulatedVariables, ...
    'OutputVariables', OutputVariables);

display(ServoMPC)


TimeSteps = round(10/Ts);
r = [pi 0];
[y, t, u] = sim(ServoMPC, TimeSteps, r);

subplot(311)
plot(t, y(:,1), [0 t(end)], pi*[1 1])
title('Angular Position (radians)');
subplot(312)
plot(t, y(:,2), [0 t(end)], [-78.5 -78.5])
title('Torque (nM)')
subplot(313)
stairs(t, u)
title('Applied Voltage (volts)')
xlabel('Elapsed Time (seconds)')