205 lines
7.2 KiB
Matlab
205 lines
7.2 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
|
|
|
|
% Let's define the system parameters on Matlab.
|
|
|
|
k = 1e6; % Stiffness [N/m]
|
|
c = 4e2; % Damping [N/(m/s)]
|
|
m = 10; % Mass [kg]
|
|
|
|
|
|
|
|
% And now the system dynamics $G(s)$ and $G_d(s)$.
|
|
|
|
G = 1/(m*s^2 + c*s + k); % Plant
|
|
Gd = (c*s + k)/(m*s^2 + c*s + k); % Disturbance
|
|
|
|
|
|
|
|
% #+HTML: <details><summary>Hint</summary>
|
|
% #+LATEX: \tcbsubtitle{Hint}
|
|
% You can follow this procedure:
|
|
% 1. In order to have good disturbance rejection at low frequency, add a simple or double *integrator*
|
|
% 2. In terms of the loop gain, the *bandwidth* can be defined at the frequency $\omega_c$ where $|l(j\omega_c)|$ first crosses 1 from above.
|
|
% Therefore, adjust the *gain* such that $L(j\omega)$ crosses 1 at around 10Hz
|
|
% 3. The roll-off at high frequency for noise attenuation should already be good enough.
|
|
% If not, add a *low pass filter*
|
|
% 4. Add a *Lead* centered around the crossover frequency (10 Hz) and tune it such that sufficient phase margin is added.
|
|
% Verify that the gain margin is good enough.
|
|
% #+HTML: </details>
|
|
% #+end_exercice
|
|
|
|
% Let's say we came up with the following controller.
|
|
|
|
K = 14e8 * ... % Gain
|
|
1/(s^2) * ... % Double Integrator
|
|
1/(1 + s/2/pi/40) * ... % Low Pass Filter
|
|
(1 + s/(2*pi*10/sqrt(8)))/(1 + s/(2*pi*10*sqrt(8))); % Lead
|
|
|
|
|
|
|
|
% The bode plot of the Loop Gain is shown in Figure [[fig:loop_gain_manual_afm]] and we can verify that we have the wanted stability margins using the =margin= command:
|
|
|
|
[Gm, Pm, ~, Wc] = margin(G*K);
|
|
|
|
|
|
|
|
% #+RESULTS:
|
|
% | Requirements | Manual Method |
|
|
% |-----------------------------+---------------|
|
|
% | Gain Margin $> 3$ [dB] | 3.1 |
|
|
% | Phase Margin $> 30$ [deg] | 35.4 |
|
|
% | Crossover $\approx 10$ [Hz] | 10.1 |
|
|
|
|
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G*K, freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-4, 1e1])
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G*K, freqs, 'Hz')))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
yticks(-360:90:360); ylim([-360, 0]);
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
% Example of the $\mathcal{H}_\infty$ Loop Shaping Synthesis
|
|
% <<sec:h_infinity_open_loop_shaping_example>>
|
|
|
|
% To apply the $\mathcal{H}_\infty$ Loop Shaping Synthesis, the wanted shape of the loop gain should be determined from the specifications.
|
|
% This is summarized in Table [[tab:open_loop_shaping_specifications]].
|
|
|
|
% Such shape corresponds to the typical wanted Loop gain Shape shown in Figure [[fig:open_loop_shaping_shape]].
|
|
|
|
% #+name: tab:open_loop_shaping_specifications
|
|
% #+caption: Wanted Loop Shape corresponding to each specification
|
|
% #+attr_latex: :environment tabularx :width \linewidth :align lXX
|
|
% | | *Specification* | *Corresponding Loop Shape* |
|
|
% |--------------+----------------------------------------+------------------------------------------|
|
|
% | *Dist. Rej.* | Highest possible rejection below 1Hz | Slope of -40dB/dec at low frequency |
|
|
% | *Pos. Speed* | Bandwidth of approximately 10Hz | $L$ crosses 1 at 10Hz |
|
|
% | *Noise Att.* | Roll-off of -40dB/decade past 30Hz | Roll-off of -40dB/decade past 30Hz |
|
|
% | *Robustness* | $\Delta G > 3dB$, $\Delta \phi > 30^o$ | Slope of -20dB/decade near the crossover |
|
|
|
|
% Then, a (stable, minimum phase) transfer function $L_w(s)$ should be created that has the same gain as the wanted shape of the Loop gain.
|
|
% For this example, a double integrator and a lead centered on 10Hz are used.
|
|
% Then the gain is adjusted such that the $|L_w(j2 \pi 10)| = 1$.
|
|
|
|
% Using Matlab, we have:
|
|
|
|
Lw = 2.3e3 * ...
|
|
1/(s^2) * ... % Double Integrator
|
|
(1 + s/(2*pi*10/sqrt(3)))/(1 + s/(2*pi*10*sqrt(3))); % Lead
|
|
|
|
|
|
|
|
% The $\mathcal{H}_\infty$ open loop shaping synthesis is then performed using the =loopsyn= command:
|
|
|
|
[K, ~, GAM] = loopsyn(G, Lw);
|
|
|
|
|
|
|
|
% The obtained Loop Gain is shown in Figure [[fig:open_loop_shaping_hinf_L]] and matches the specified one by a factor $\gamma \approx 2$.
|
|
|
|
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G*K, freqs, 'Hz'))), 'DisplayName', '$L(s)$');
|
|
plot(freqs, abs(squeeze(freqresp(Lw, freqs, 'Hz'))), 'k--', 'DisplayName', '$L_w(s)$');
|
|
plot(freqs, abs(squeeze(freqresp(Lw, freqs, 'Hz')))*GAM, 'k-.', 'DisplayName', '$L_w(s) / \gamma$, $L_w(s) \cdot \gamma$');
|
|
plot(freqs, abs(squeeze(freqresp(Lw, freqs, 'Hz')))/GAM, 'k-.', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
legend('location', 'northeast');
|
|
ylim([1e-4, 1e2]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G*K, freqs, 'Hz')))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
yticks(-360:90:360); ylim([-360, 0]);
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
|
|
|
|
% #+name: fig:open_loop_shaping_hinf_L
|
|
% #+caption: Obtained Open Loop Gain $L(s) = G(s) K(s)$ and comparison with the wanted Loop gain $L_w$
|
|
% #+RESULTS:
|
|
% [[file:figs/open_loop_shaping_hinf_L.png]]
|
|
|
|
|
|
% #+begin_important
|
|
% When using the $\mathcal{H}_\infty$ Synthesis, it is usually recommended to analyze the obtained controller.
|
|
|
|
% This is usually done by breaking down the controller into simple elements such as low pass filters, high pass filters, notches, leads, etc.
|
|
% #+end_important
|
|
|
|
% Let's briefly analyze the obtained controller which bode plot is shown in Figure [[fig:open_loop_shaping_hinf_K]]:
|
|
% - two integrators are used at low frequency to have the wanted low frequency high gain
|
|
% - a lead is added centered with the crossover frequency to increase the phase margin
|
|
% - a notch is added at the resonance of the plant to increase the gain margin (this is very typical of $\mathcal{H}_\infty$ controllers, and can be an issue, more info on that latter)
|
|
|
|
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
plot(freqs, abs(squeeze(freqresp(K, freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
|
|
ax2 = nexttile;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K, freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
yticks(-360:90:360); ylim([-180, 90]);
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
|
|
|
|
% #+name: fig:open_loop_shaping_hinf_K
|
|
% #+caption: Obtained controller $K$ using the open-loop $\mathcal{H}_\infty$ shaping
|
|
% #+RESULTS:
|
|
% [[file:figs/open_loop_shaping_hinf_K.png]]
|
|
|
|
|
|
% Let's finally compare the obtained stability margins of the $\mathcal{H}_\infty$ controller and of the manually developed controller in Table [[tab:open_loop_shaping_compare]].
|
|
|
|
|
|
[Gm_2, Pm_2, ~, Wc_2] = margin(G*K);
|