nass-simscape/org/control_decentralized.org

Control in the Frame of the Legs applied on the Simscape Model

• Introduction
• Decentralized Control
  • Control Schematic
  • Initialize the Simscape Model
  • Identification of the plant
  • Plant Analysis
  • Controller Design
  • Simulation
  • Results
• HAC-LAC (IFF) Decentralized Control
  • Introduction
  • Control Schematic
  • Initialize the Simscape Model
  • Initialization
  • Identification for IFF
  • Integral Force Feedback Controller
  • Identification of the damped plant
  • Controller Design
  • Simulation
  • Results
• Conclusion

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);

    % \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$};
    % \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$};
    % \draw[->] (G.east) node[above right]{$\bm{\mathcal{L}}$} -| ($(G.east)+(1, -1)$) -| (subr.south);
  \end{tikzpicture}

Decentralized control for reference tracking

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>>

Transfer Functions from forces applied in each actuator $\tau_i$ to the relative motion of each leg $d\mathcal{L}_i$ (png, pdf)

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>>

Obtained Loop Gain (png, pdf)

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>>

Position Errors when using the Decentralized Control Architecture (png, pdf)

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}

Decentralized control for reference tracking

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');

Results

Conclusion