26 KiB
26 KiB
Control in the Frame of the Legs applied on the Simscape Model
Introduction ignore
In this document, we apply some decentralized control to the NASS and see what level of performance can be obtained.
Decentralized Control
Control Schematic
The control architecture is shown in Figure fig:decentralized_reference_tracking_L.
The signals are:
- $\bm{r}_\mathcal{X}$: wanted position of the sample with respect to the granite
- $\bm{r}_{\mathcal{X}_n}$: wanted position of the sample with respect to the nano-hexapod
- $\bm{r}_\mathcal{L}$: wanted length of each of the nano-hexapod's legs
- $\bm{\tau}$: forces applied in each actuator
- $\bm{\mathcal{L}}$: measured displacement of each leg
- $\bm{\mathcal{X}}$: measured position of the sample with respect to the granite
\begin{tikzpicture}
% Blocs
\node[block={2.0cm}{2.0cm}] (P) {};
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
\coordinate[] (outputX) at ($(P.south east)!0.7!(P.north east)$);
\coordinate[] (outputL) at ($(P.south east)!0.3!(P.north east)$);
\node[block, left= of inputF] (K) {$\bm{K}_\mathcal{L}$};
\node[addb={+}{}{}{}{-}, left= of K] (subr) {};
\node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};
\node[block, align=center, left= of J] (Ex) {Compute\\Pos. Error};
% Connections and labels
\draw[->] (outputL) -- ++(1, 0) node[above left]{$\bm{\mathcal{L}}$};
\draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$};
\draw[->] (K.east) -- (inputF) node[above left]{$\bm{\tau}$};
\draw[->] (outputX) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{X}}$};
\draw[->] ($(outputX) + (1.4, 0)$)node[branch]{} -- ++(0, -2.5) -| (Ex.south);
\draw[->] (Ex.east) -- (J.west) node[above left]{$\bm{r}_{\mathcal{X}_n}$};
\draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$};
\draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
\end{tikzpicture}
Initialize the Simscape Model
We initialize all the stages with the default parameters.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 1);We set the references that corresponds to a tomography experiment.
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1); initializeDisturbances();Open Loop.
initializeController('type', 'ref-track-L');
Kl = tf(zeros(6));And we put some gravity.
initializeSimscapeConfiguration('gravity', true);We log the signals.
initializeLoggingConfiguration('log', 'all');Identification of the plant
Let's identify the transfer function from $\bm{\tau}$ to $\bm{\mathcal{L}}$.
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Controller/Reference-Tracking-L/Sum'], 1, 'openoutput'); io_i = io_i + 1; % Leg length error
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};Plant Analysis
The diagonal and off-diagonal terms of the plant are shown in Figure fig:decentralized_control_plant_L.
We can see that:
- the diagonal terms have similar dynamics
- the plant is decoupled at low frequency
<<plt-matlab>>
Controller Design
The controller consists of:
- A pure integrator
- An integrator up to little before the crossover
- A lead around the crossover
- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin
The obtained loop gains corresponding to the diagonal elements are shown in Figure fig:decentralized_control_L_loop_gain.
wc = 2*pi*20;
h = 1.5;
Kl = diag(1./diag(abs(freqresp(G, wc)))) * ...
wc/s * ... % Pure Integrator
((s/wc*2 + 1)/(s/wc*2)) * ... % Integrator up to wc/2
1/h * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
1/(1 + s/3/wc) * ... % Low pass Filter
1/(1 + s/3/wc);<<plt-matlab>>
We add a minus sign to the controller as it is not included in the Simscape model.
Kl = -Kl;Simulation
initializeController('type', 'ref-track-L'); load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2'); sim('nass_model'); decentralized_L = simout;
save('./mat/tomo_exp_decentalized.mat', 'decentralized_L', '-append');Results
The reference path and the position of the mobile platform are shown in Figure fig:decentralized_L_position_errors.
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
load('./mat/tomo_exp_decentalized.mat', 'decentralized_L');<<plt-matlab>>
HAC-LAC (IFF) Decentralized Control
Introduction ignore
We here add an Active Damping Loop (Integral Force Feedback) prior to using the Decentralized control architecture using $\bm{\mathcal{L}}$.
Control Schematic
The control architecture is shown in Figure fig:decentralized_reference_tracking_L.
The signals are:
- $\bm{r}_\mathcal{X}$: wanted position of the sample with respect to the granite
- $\bm{r}_{\mathcal{X}_n}$: wanted position of the sample with respect to the nano-hexapod
- $\bm{r}_\mathcal{L}$: wanted length of each of the nano-hexapod's legs
- $\bm{\tau}$: forces applied in each actuator
- $\bm{\mathcal{L}}$: measured displacement of each leg
- $\bm{\mathcal{X}}$: measured position of the sample with respect to the granite
\begin{tikzpicture}
% Blocs
\node[block={3.0cm}{3.0cm}] (P) {};
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
\coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
\coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);
\node[block, above= of P] (Kiff) {$\bm{K}_\text{IFF}$};
\node[addb, left= of inputF] (addF) {};
\node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$};
\node[addb={+}{}{}{}{-}, left= of K] (subr) {};
\node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};
\node[block, align=center, left= of J] (Ex) {Compute\\Pos. Error};
% Connections and labels
\draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
\draw[->] (Kiff.west) -| (addF.north);
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
\draw[->] (outputL) -- ++(1, 0) node[above left]{$\bm{\mathcal{L}}$};
\draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$};
\draw[->] (K.east) -- (addF.west);
\draw[->] (outputX) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{X}}$};
\draw[->] ($(outputX) + (1.4, 0)$)node[branch]{} -- ++(0, -2.5) -| (Ex.south);
\draw[->] (Ex.east) -- (J.west) node[above left]{$\bm{r}_{\mathcal{X}_n}$};
\draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$};
\draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
\end{tikzpicture}
Initialize the Simscape Model
We initialize all the stages with the default parameters.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 1);We set the references that corresponds to a tomography experiment.
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1); initializeDisturbances();Open Loop.
initializeController('type', 'ref-track-L');
Kl = tf(zeros(6));And we put some gravity.
initializeSimscapeConfiguration('gravity', true);We log the signals.
initializeLoggingConfiguration('log', 'all');Initialization
initializeController('type', 'ref-track-iff-L');
K_iff = tf(zeros(6));
Kl = tf(zeros(6));Identification for IFF
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
%% Run the linearization
G_iff = linearize(mdl, io, 0);
G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};Integral Force Feedback Controller
w0 = 2*pi*50;
K_iff = -5000/s * (s/w0)/(1 + s/w0) * eye(6); K_iff = -K_iff;Identification of the damped plant
%% Name of the Simulink DehaezeFile
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Controller/Reference-Tracking-IFF-L/Sum'], 1, 'openoutput'); io_i = io_i + 1; % Leg length error
%% Run the linearization
Gd = linearize(mdl, io, 0);
Gd.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
Gd.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};Controller Design
wc = 2*pi*300;
h = 3;
Kl = diag(1./diag(abs(freqresp(Gd, wc)))) * ...
((s/(2*pi*20) + 1)/(s/(2*pi*20))) * ... % Pure Integrator
((s/(2*pi*50) + 1)/(s/(2*pi*50))) * ... % Integrator up to wc/2
1/h * (1 + s/wc*h)/(1 + s/wc/h) * ...
1/(1 + s/(2*wc)) * ...
1/(1 + s/(3*wc)); isstable(feedback(Gd*Kl, eye(6), -1)) Kl = -Kl;Simulation
initializeController('type', 'ref-track-iff-L'); load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2'); sim('nass_model'); decentralized_iff_L = simout;
save('./mat/tomo_exp_decentalized.mat', 'decentralized_iff_L', '-append');