21 KiB
Stewart Platform - Control Study
First Control Architecture
Control Schematic
\begin{tikzpicture}
% Blocs
\node[block] (J) at (0, 0) {$J$};
\node[addb={+}{}{}{}{-}, right=1 of J] (subr) {};
\node[block, right=0.8 of subr] (K) {$K_{L}$};
\node[block, right=1 of K] (G) {$G_{L}$};
% Connections and labels
\draw[<-] (J.west)node[above left]{$\bm{r}_{n}$} -- ++(-1, 0);
\draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{L}$};
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{L}$};
\draw[->] (K.east) -- (G.west) node[above left]{$\bm{\tau}$};
\draw[->] (G.east) node[above right]{$\bm{L}$} -| ($(G.east)+(1, -1)$) -| (subr.south);
\end{tikzpicture}
Initialize the Stewart platform
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
Identification of the plant
Let's identify the transfer function from $\bm{\tau}}$ to $\bm{L}$.
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
Plant Analysis
Diagonal terms
Compare to off-diagonal terms
Controller Design
One integrator should be present in the controller.
A lead is added around the crossover frequency which is set to be around 500Hz.
% wint = 2*pi*100; % Integrate until [rad]
% wlead = 2*pi*500; % Location of the lead [rad]
% hlead = 2; % Lead strengh
% Kl = 1e6 * ... % Gain
% (s + wint)/(s) * ... % Integrator until 100Hz
% (1 + s/(wlead/hlead)/(1 + s/(wlead*hlead))); % Lead
wc = 2*pi*100;
Kl = 1/abs(freqresp(G(1,1), wc)) * wc/s * 1/(1 + s/(3*wc));
Kl = Kl * eye(6);
Transmissibility Analysis
Initialize the Stewart platform
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
We set the rotation point of the ground to be at the same point at frames $\{A\}$ and $\{B\}$.
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'rigid');
Transmissibility
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances/D_w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
%% Run the linearization
T = linearize(mdl, io, options);
T.InputName = {'Wdx', 'Wdy', 'Wdz', 'Wrx', 'Wry', 'Wrz'};
T.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
freqs = logspace(1, 4, 1000);
figure;
for ix = 1:6
for iy = 1:6
subplot(6, 6, (ix-1)*6 + iy);
hold on;
plot(freqs, abs(squeeze(freqresp(T(ix, iy), freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-5, 10]);
xlim([freqs(1), freqs(end)]);
if ix < 6
xticklabels({});
end
if iy > 1
yticklabels({});
end
end
end
From cite:preumont07_six_axis_singl_stage_activ, one can use the Frobenius norm of the transmissibility matrix to obtain a scalar indicator of the transmissibility performance of the system:
\begin{align*} \| \bm{T}(\omega) \| &= \sqrt{\text{Trace}[\bm{T}(\omega) \bm{T}(\omega)^H]}\\ &= \sqrt{\Sigma_{i=1}^6 \Sigma_{j=1}^6 |T_{ij}|^2} \end{align*} freqs = logspace(1, 4, 1000);
T_norm = zeros(length(freqs), 1);
for i = 1:length(freqs)
T_norm(i) = sqrt(trace(freqresp(T, freqs(i), 'Hz')*freqresp(T, freqs(i), 'Hz')'));
end
And we normalize by a factor $\sqrt{6}$ to obtain a performance metric comparable to the transmissibility of a one-axis isolator: \[ \Gamma(\omega) = \|\bm{T}(\omega)\| / \sqrt{6} \]
Gamma = T_norm/sqrt(6);
figure;
plot(freqs, Gamma)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Compliance Analysis
Initialize the Stewart platform
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
We set the rotation point of the ground to be at the same point at frames $\{A\}$ and $\{B\}$.
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'rigid');
Compliance
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances/F_ext'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
%% Run the linearization
C = linearize(mdl, io, options);
C.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
C.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
freqs = logspace(1, 4, 1000);
figure;
for ix = 1:6
for iy = 1:6
subplot(6, 6, (ix-1)*6 + iy);
hold on;
plot(freqs, abs(squeeze(freqresp(C(ix, iy), freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-10, 1e-3]);
xlim([freqs(1), freqs(end)]);
if ix < 6
xticklabels({});
end
if iy > 1
yticklabels({});
end
end
end
We can try to use the Frobenius norm to obtain a scalar value representing the 6-dof compliance of the Stewart platform.
freqs = logspace(1, 4, 1000);
C_norm = zeros(length(freqs), 1);
for i = 1:length(freqs)
C_norm(i) = sqrt(trace(freqresp(C, freqs(i), 'Hz')*freqresp(C, freqs(i), 'Hz')'));
end
figure;
plot(freqs, C_norm)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Functions
Compute the Transmissibility
<<sec:computeTransmissibility>>
Function description
function [T, T_norm, freqs] = computeTransmissibility(args)
% computeTransmissibility -
%
% Syntax: [T, T_norm, freqs] = computeTransmissibility(args)
%
% Inputs:
% - args - Structure with the following fields:
% - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm
% - freqs [] - Frequency vector to estimate the Frobenius norm
%
% Outputs:
% - T [6x6 ss] - Transmissibility matrix
% - T_norm [length(freqs)x1] - Frobenius norm of the Transmissibility matrix
% - freqs [length(freqs)x1] - Frequency vector in [Hz]
Optional Parameters
arguments
args.plots logical {mustBeNumericOrLogical} = false
args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000)
end
freqs = args.freqs;
Identification of the Transmissibility Matrix
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances/D_w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
%% Run the linearization
T = linearize(mdl, io, options);
T.InputName = {'Wdx', 'Wdy', 'Wdz', 'Wrx', 'Wry', 'Wrz'};
T.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
If wanted, the 6x6 transmissibility matrix is plotted.
p_handle = zeros(6*6,1);
if args.plots
fig = figure;
for ix = 1:6
for iy = 1:6
p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
hold on;
plot(freqs, abs(squeeze(freqresp(T(ix, iy), freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
if ix < 6
xticklabels({});
end
if iy > 1
yticklabels({});
end
end
end
linkaxes(p_handle, 'xy')
xlim([freqs(1), freqs(end)]);
ylim([1e-5, 1e2]);
han = axes(fig, 'visible', 'off');
han.XLabel.Visible = 'on';
han.YLabel.Visible = 'on';
ylabel(han, 'Frequency [Hz]');
xlabel(han, 'Transmissibility [m/m]');
end
Computation of the Frobenius norm
T_norm = zeros(length(freqs), 1);
for i = 1:length(freqs)
T_norm(i) = sqrt(trace(freqresp(T, freqs(i), 'Hz')*freqresp(T, freqs(i), 'Hz')'));
end
T_norm = T_norm/sqrt(6);
if args.plots
figure;
plot(freqs, T_norm)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Transmissibility - Frobenius Norm');
end
Compute the Compliance
<<sec:computeCompliance>>
Function description
function [C, C_norm, freqs] = computeCompliance(args)
% computeCompliance -
%
% Syntax: [C, C_norm, freqs] = computeCompliance(args)
%
% Inputs:
% - args - Structure with the following fields:
% - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm
% - freqs [] - Frequency vector to estimate the Frobenius norm
%
% Outputs:
% - C [6x6 ss] - Compliance matrix
% - C_norm [length(freqs)x1] - Frobenius norm of the Compliance matrix
% - freqs [length(freqs)x1] - Frequency vector in [Hz]
Optional Parameters
arguments
args.plots logical {mustBeNumericOrLogical} = false
args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000)
end
freqs = args.freqs;
Identification of the Compliance Matrix
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances/F_ext'], 1, 'openinput'); io_i = io_i + 1; % External forces [N, N*m]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
%% Run the linearization
C = linearize(mdl, io, options);
C.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
C.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
If wanted, the 6x6 transmissibility matrix is plotted.
p_handle = zeros(6*6,1);
if args.plots
fig = figure;
for ix = 1:6
for iy = 1:6
p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
hold on;
plot(freqs, abs(squeeze(freqresp(C(ix, iy), freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
if ix < 6
xticklabels({});
end
if iy > 1
yticklabels({});
end
end
end
linkaxes(p_handle, 'xy')
xlim([freqs(1), freqs(end)]);
han = axes(fig, 'visible', 'off');
han.XLabel.Visible = 'on';
han.YLabel.Visible = 'on';
xlabel(han, 'Frequency [Hz]');
ylabel(han, 'Compliance [m/N, rad/(N*m)]');
end
Computation of the Frobenius norm
freqs = args.freqs;
C_norm = zeros(length(freqs), 1);
for i = 1:length(freqs)
C_norm(i) = sqrt(trace(freqresp(C, freqs(i), 'Hz')*freqresp(C, freqs(i), 'Hz')'));
end
if args.plots
figure;
plot(freqs, C_norm)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance - Frobenius Norm');
end