www.gusucode.com > control_featured 案例源码程序 matlab代码 > control_featured/GainScheduledAutopilotExample.m

    %% Tuning of Gain-Scheduled Three-Loop Autopilot
% This example uses |systune| to generate smooth gain schedules for a
% three-loop autopilot. 

% Copyright 1986-2015 The MathWorks, Inc.

%% Airframe Model and Three-Loop Autopilot 
% This example uses a three-degree-of-freedom model of the pitch axis
% dynamics of an airframe. The states are the Earth coordinates $(X_e,Z_e)$,
% the body coordinates $(u,w)$, the pitch angle $\theta$, and the pitch rate
% $q = \dot\theta$. Figure 1 summarizes the relationship between the inertial
% and body frames, the flight path angle $\gamma$, the incidence angle $\alpha$,
% and the pitch angle $\theta$. 
%
% <<../gsautopilot1.png>>
%
% *Figure 1: Airframe dynamics.*
%
% We use a classic three-loop autopilot structure to control the flight path
% angle $\gamma$. This autopilot adjusts the flight path by delivering adequate
% bursts of normal acceleration $a_z$ (acceleration along $w$). In turn, normal
% acceleration is produced by adjusting the elevator deflection $\delta$ 
% to cause pitching and vary the amount of lift. The autopilot uses 
% Proportional-Integral (PI) control in the pitch rate loop $q$ and 
% proportional control in the $a_z$ and $\gamma$ loops. The closed-loop system
% (airframe and autopilot) are modeled in Simulink.

open_system('rct_airframeGS')


%% Autopilot Gain Scheduling
% The airframe dynamics are nonlinear and the aerodynamic forces and moments
% depend on speed $V$ and incidence $\alpha$. To obtain suitable performance
% throughout the $(\alpha,V)$ flight envelope, the autopilot gains must 
% be adjusted as a function of $\alpha$ and $V$ to compensate for changes in plant
% dynamics. This adjustment process is called "gain scheduling" and $\alpha,V$ 
% are called the scheduling variables. In the Simulink model, gain
% schedules are implemented as look-up tables driven by measurements of  
% $\alpha$ and $V$.
%
% Gain scheduling is a linear technique for controlling nonlinear or time-varying 
% plants. The idea is to compute linear approximations
% of the plant at various operating conditions, tune the controller gains at 
% each operating condition, and swap gains 
% as a function of operating condition during operation. 
% Conventional gain scheduling involves three major steps:
%
% # Trim and linearize the plant at each operating condition
% # Tune the controller gains for the linearized dynamics at each operating condition
% # Reconcile the gain values to provide smooth transition between
%   operating conditions.
%
% In this example, we combine Steps 2. and 3. by parameterizing the autopilot
% gains as first-order polynomials in $\alpha,V$ and directly tuning the polynomial 
% coefficients for the entire flight envelope. This approach eliminates Step 3.
% and guarantees smooth gain variations as a function of 
% $\alpha$ and $V$. Moreover, the gain schedule coefficients can be automatically
% tuned with |systune|.

%% Trimming and Linearization
% Assume that the incidence $\alpha$ varies between -20 and 20 degrees and that
% the speed $V$ varies between 700 and 1400 m/s. When neglecting gravity, the
% airframe dynamics are symmetric in $\alpha$ so consider only positive values
% of $\alpha$. Use a 5-by-9 grid of linearly spaced $(\alpha,V)$ pairs to cover
% the flight envelope:

nA = 5;  % number of alpha values
nV = 9;  % number of V values
[alpha,V] = ndgrid(linspace(0,20,nA)*pi/180,linspace(700,1400,nV));

%%
% For each flight condition $(\alpha,V)$, linearize the airframe dynamics
% at trim (zero normal acceleration and pitching moment).
% This requires computing the elevator deflection $\delta$ and pitch rate
% $q$ that result in steady $w$ and $q$. To do this, first isolate the
% airframe model in a separate Simulink model.

open_system('rct_airframeTRIM')

%%
% Use |operspec| to specify the trim condition, use
% |findop| to compute the trim values of $\delta$ and $q$, and linearize the 
% airframe dynamics for the resulting operating point. See the 
% "Trimming and Linearizing an Airframe" example in Simulink Control Design 
% for details. Repeat these steps for the 45 flight conditions $(\alpha,V)$.

% Compute trim condition for each (alpha,V) pair
clear op
for ct=1:nA*nV
   alpha_ini = alpha(ct);      % Incidence [rad]
   v_ini = V(ct);              % Speed [m/s]
   
   % Specify trim condition
   opspec = operspec('rct_airframeTRIM');
   % Xe,Ze: known, not steady
   opspec.States(1).Known = [1;1];
   opspec.States(1).SteadyState = [0;0];
   % u,w: known, w steady
   opspec.States(3).Known = [1 1];
   opspec.States(3).SteadyState = [0 1];
   % theta: known, not steady
   opspec.States(2).Known = 1;
   opspec.States(2).SteadyState = 0;
   % q: unknown, steady
   opspec.States(4).Known = 0;
   opspec.States(4).SteadyState = 1;
   
   % TRIM
   Options = findopOptions('DisplayReport','off');
   op(ct) = findop('rct_airframeTRIM',opspec,Options);
end

% Linearize at trim conditions
G = linearize('rct_airframeTRIM',op);
G = reshape(G,[nA nV]);
G.u = 'delta';
G.y = {'alpha' 'V' 'q' 'az' 'gamma' 'h'};

%%
% This produces a 5-by-9 array of linearized plant models at the 45  
% flight conditions $(\alpha,V)$. The plant dynamics vary substantially
% across the flight envelope.

sigma(G), title('Variations in airframe dynamics')

%% Tunable Gain Surface
% The autopilot consists of four gains $K_p, K_i, K_a, K_g$ to be
% "scheduled" (adjusted) as a function of $\alpha$ and $V$. 
% Practically, this means tuning 88 values in each of the corresponding four
% look-up tables. Rather than tuning each table entry separately, parameterize
% the gains as a two-dimensional gain surfaces, for example, surfaces
% with a simple multi-linear dependence on $\alpha$ and $V$:
%
% $$ K(\alpha,V) = K_0 + K_1 \alpha + K_2 V + K_3 \alpha V $$.
%
% This cuts the number of variables from 88 down to 4 for each lookup table. 
% Use the |tunableSurface| object to parameterize each gain surface. Note that:
%
% * |TuningGrid| specifies the "tuning grid" (design points). This grid
% should match the one used for linearization but needs not match the
% loop-up table breakpoints
% * |ShapeFcn| specifies the basis functions for the surface 
% parameterization ($\alpha$, $V$, and $\alpha V$)
%
% Each surface is initialized to a constant gain using the 
% tuning results for $\alpha$ = 10 deg and $V$ = 1050 m/s 
% (mid-range design).

TuningGrid = struct('alpha',alpha,'V',V);
ShapeFcn = @(alpha,V) [alpha,V,alpha*V];

Kp = tunableSurface('Kp', 0.1, TuningGrid, ShapeFcn);
Ki = tunableSurface('Ki', 2, TuningGrid, ShapeFcn);
Ka = tunableSurface('Ka', 0.001, TuningGrid, ShapeFcn);
Kg = tunableSurface('Kg', -1000, TuningGrid, ShapeFcn);

%%
% Next create an |slTuner| interface for tuning the gain surfaces. 
% Use block substitution to replace the nonlinear plant model by the 
% linearized models over the tuning grid. Use |setBlockParam| to 
% associate the tunable gain surfaces |Kp|, |Ki|, |Ka|, |Kg|
% with the Interpolation blocks of the same name.

BlockSubs = struct('Name','rct_airframeGS/Airframe Model','Value',G);
ST0 = slTuner('rct_airframeGS',{'Kp','Ki','Ka','Kg'},BlockSubs);

% Register points of interest 
ST0.addPoint({'az_ref','az','gamma_ref','gamma','delta'})

% Parameterize look-up table blocks
ST0.setBlockParam('Kp',Kp);
ST0.setBlockParam('Ki',Ki);
ST0.setBlockParam('Ka',Ka);
ST0.setBlockParam('Kg',Kg);

%% Autopilot Tuning
% |systune| can automatically tune the gain surface coefficients
% for the entire flight envelope. Use |TuningGoal| objects to specify the
% performance objectives:
%
% * $\gamma$ loop: Track the setpoint with a 1 second response time, less than 2% 
%   steady-state error, and less than 30% peak error.

Req1 = TuningGoal.Tracking('gamma_ref','gamma',1,0.02,1.3);
viewSpec(Req1)

%%
% * $a_z$ loop: Ensure good disturbance rejection at low frequency 
%   (to track acceleration demands) and past 10 rad/s
%   (to be insensitive to measurement noise). 

% Note: The disturbance is injected at the az_ref location
RejectionProfile = frd([0.02 0.02 1.2 1.2 0.1],[0 0.02 2 15 150]);
Req2 = TuningGoal.Gain('az_ref','az',RejectionProfile);
viewSpec(Req2)

%%
% * $q$ loop: Ensure good disturbance rejection up to 10 rad/s. The
% disturbance is injected at the plant input |delta|.

Req3 = TuningGoal.Gain('delta','az',600*tf([0.25 0],[0.25 1]));
viewSpec(Req3)

%%
% * Transients: Ensure a minimum damping ratio of 0.35 for 
%   oscillation-free transients

MinDamping = 0.35;
Req4 = TuningGoal.Poles(0,MinDamping);

%%
% Using |systune|, tune the 16 gain surface coefficients to best meet these 
% performance requirements at all 45 flight conditions.

ST = systune(ST0,[Req1 Req2 Req3 Req4]);

%%
% The final value of the combined objective is close to 1, indicating that
% all requirements are nearly met. Visualize the resulting gain surfaces.

% Get tuned gain surfaces
TGS = getBlockParam(ST);

% Plot gain surfaces
clf
subplot(221), viewSurf(TGS.Kp), title('Kp')
subplot(222), viewSurf(TGS.Ki), title('Ki')
subplot(223), viewSurf(TGS.Ka), title('Ka')
subplot(224), viewSurf(TGS.Kg), title('Kg')

%% Validation
% First validate the tuned autopilot at the 45 flight conditions considered
% above. Plot the response to a step change in flight path angle and the
% response to a step disturbance in elevator deflection.

clf
subplot(211), step(getIOTransfer(ST,'gamma_ref','gamma'),5), grid
title('Tracking of step change in flight path angle')
subplot(212), step(getIOTransfer(ST,'delta','az'),3), grid
title('Rejection of step disturbance at plant input')

%%
% The responses are satisfactory at all flight conditions. Next validate
% the autopilot against the nonlinear airframe model. First use
% |writeBlockValue| to apply the tuning results to the Simulink model.
% This evaluates each gain surface formula at the breakpoints specified in
% the two Prelookup blocks and writes the result in the corresponding 
% Interpolation block.

writeBlockValue(ST)

%%
% Now simulate the autopilot performance for a maneuver that takes the 
% airframe through a large portion of its flight envelope. The code below
% is equivalent to pressing the Play button in the Simulink model and
% inspecting the responses in the Scope blocks.

% Initial conditions
h_ini = 1000;
alpha_ini = 0;
v_ini = 700;

% Simulate
SimOut = sim('rct_airframeGS', 'ReturnWorkspaceOutputs', 'on');

% Extract simulation data
SimData = get(SimOut,'sigsOut');
Sim_gamma = getElement(SimData,'gamma');
Sim_alpha = getElement(SimData,'alpha');
Sim_V = getElement(SimData,'V');
Sim_delta = getElement(SimData,'delta');
Sim_h = getElement(SimData,'h');
Sim_az = getElement(SimData,'az');
t = Sim_gamma.Values.Time;

% Plot the main flight variables
clf
subplot(211)
plot(t,Sim_gamma.Values.Data(:,1),'r--',t,Sim_gamma.Values.Data(:,2),'b'), grid
legend('Commanded','Actual','location','SouthEast')
title('Flight path angle \gamma in degrees')
subplot(212)
plot(t,Sim_delta.Values.Data), grid
title('Elevator deflection \delta in degrees')

%%

subplot(211)
plot(t,Sim_alpha.Values.Data), grid
title('Incidence \alpha in degrees')
subplot(212)
plot(t,Sim_V.Values.Data), grid
title('Speed V in m/s')

%%

subplot(211)
plot(t,Sim_h.Values.Data), grid
title('Altitude h in meters')
subplot(212)
plot(t,Sim_az.Values.Data), grid
title('Normal acceleration a_z in g''s')


%%
% Tracking of the flight path angle profile remains good throughout the maneuver.
% Note that the variations in incidence $\alpha$ and speed $V$ cover most of
% the flight envelope considered here ([-20,20] degrees for $\alpha$ and
% [700,1400] for $V$). And while the autopilot was tuned for a nominal altitude
% of 3000 m, it fares well for altitude changing from 1,000 to 10,000 m.
%
% The nonlinear simulation results confirm that the gain-scheduled autopilot
% delivers consistently high performance throughout the flight envelope.
% The "gain surface tuning" procedure provides simple explicit formulas for 
% the gain dependence on the scheduling variables. Instead of using look-up 
% tables, you can use these formulas directly for an more memory-efficient 
% hardware implementation.