241 lines
7.0 KiB
Matlab
241 lines
7.0 KiB
Matlab
% Matlab Init :noexport:ignore:
|
|
|
|
%% dcm_active_damping_iff.m
|
|
% Test of Integral Force Feedback Strategy
|
|
|
|
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
%% Path for functions, data and scripts
|
|
addpath('./mat/'); % Path for data
|
|
|
|
%% Simscape Model - Nano Hexapod
|
|
addpath('./STEPS/')
|
|
|
|
%% Initialize Parameters for Simscape model
|
|
controller.type = 0; % Open Loop Control
|
|
|
|
%% Options for Linearization
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Open Simulink Model
|
|
mdl = 'simscape_dcm';
|
|
|
|
open(mdl)
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(1, 3, 1000);
|
|
|
|
% Identification
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Force Sensor {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 3, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Extraction of the dynamics
|
|
G_fs = linearize(mdl, io);
|
|
|
|
G_fs.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_fs.OutputName = {'fs_ur', 'fs_uh', 'fs_d'};
|
|
|
|
|
|
|
|
% The Bode plot of the identified dynamics is shown in Figure [[fig:iff_plant_bode_plot]].
|
|
% At high frequency, the diagonal terms are constants while the off-diagonal terms have some roll-off.
|
|
|
|
|
|
%% Bode plot for the plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur');
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', 'off-diag');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-13, 1e-7]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(3,3), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
% Controller - Root Locus
|
|
% We want to have integral action around the resonances of the system, but we do not want to integrate at low frequency.
|
|
% Therefore, we can use a low pass filter.
|
|
|
|
|
|
%% Integral Force Feedback Controller
|
|
Kiff_g1 = eye(3)*1/(1 + s/2/pi/20);
|
|
|
|
%% Root Locus for IFF
|
|
gains = logspace(9, 12, 200);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
plot(real(pole(G_fs)), imag(pole(G_fs)), 'x', 'color', colors(1,:), ...
|
|
'DisplayName', '$g = 0$');
|
|
plot(real(tzero(G_fs)), imag(tzero(G_fs)), 'o', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Optimal gain
|
|
g = 8e10;
|
|
clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
|
|
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ...
|
|
'DisplayName', sprintf('$g=%.0e$', g));
|
|
hold off;
|
|
axis square;
|
|
xlim([-2700, 0]); ylim([0, 2700]);
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
|
|
|
|
|
|
% #+name: fig:iff_root_locus
|
|
% #+caption: Root Locus plot for the IFF Control strategy
|
|
% #+RESULTS:
|
|
% [[file:figs/iff_root_locus.png]]
|
|
|
|
|
|
%% Integral Force Feedback Controller with optimal gain
|
|
Kiff = g*Kiff_g1;
|
|
|
|
%% Save the IFF controller
|
|
save('mat/Kiff.mat', 'Kiff');
|
|
|
|
|
|
|
|
% #+name: fig:schematic_jacobian_frame_fastjack_iff
|
|
% #+caption: Use of Jacobian matrices to obtain the system in the frame of the fastjacks
|
|
% #+RESULTS:
|
|
% [[file:figs/schematic_jacobian_frame_fastjack_iff.png]]
|
|
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Force Sensor {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Load DCM Kinematics
|
|
load('dcm_kinematics.mat');
|
|
|
|
%% Identification of the Open Loop plant
|
|
controller.type = 0; % Open Loop
|
|
G_ol = J_a_111*inv(J_s_111)*linearize(mdl, io);
|
|
G_ol.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_ol.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
|
|
%% Identification of the damped plant with IFF
|
|
controller.type = 1; % IFF
|
|
G_dp = J_a_111*inv(J_s_111)*linearize(mdl, io);
|
|
G_dp.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
|
|
|
|
|
|
% The dynamics from $\bm{u}$ to $\bm{d}_{\text{fj}}$ (open-loop dynamics) and from $\bm{u}^\prime$ to $\bm{d}_{\text{fs}}$ are compared in Figure [[fig:comp_damped_undamped_plant_iff_bode_plot]].
|
|
% It is clear that the Integral Force Feedback control strategy is very effective in damping the resonances of the plant.
|
|
|
|
|
|
%% Comparison of the damped and undamped plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd - OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh - OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur - OL');
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'd - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'uh - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'ur - IFF');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-12, 1e-6]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|