nass-simscape/org/control_cascade.org

Cascade Control applied on the Simscape Model

• Introduction
• Initialization
• Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
  • Identification
  • Plant
  • Root Locus
  • Controller and Loop Gain
• High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
  • Identification of the damped plant
  • Obtained Plant
  • Controller Design and Loop Gain
• Primary Controller in the task space - $\bm{K}_\mathcal{X}$
  • Identification of the linearized plant
  • Obtained Plant
  • Controller Design
• Simulation
• Results

Introduction   ignore

The control architecture we wish here to study is shown in Figure fig:cascade_control_architecture.

  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \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=0.4 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, left= of J] (Kp) {$\bm{K}_\mathcal{X}$};

    \node[block, align=center, left= of Kp] (Ex) {Compute\\Pos. Error};

    % Connections and labels
    \draw[->] (outputF) -- ++(1.0, 0);
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch](taum){} node[below]{$\bm{\tau}_m$} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};

    \draw[->] (outputL) -- ++(1.8, 0);
    \draw[->] ($(outputL) + (1.4, 0)$)node[branch]{} node[above]{$d\bm{\mathcal{L}}$} -- ++(0, -1.2) node(Plinse){} -| (subr.south);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
    \draw[->] (K.east) -- (addF.west) node[above left=0 and 8pt]{$\bm{\tau}^\prime$};

    \draw[->] (outputX) -- ++(2.6, 0);
    \draw[->] ($(outputX) + (2.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -3.0) -| (Ex.south);

    \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
    \draw[->] (Ex.east) -- (Kp.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
    \draw[->] (Kp.east) -- (J.west) node[above left=0 and 6pt]{$\bm{r}_{\mathcal{X}_n}$};
    \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$};

    \begin{scope}[on background layer]
      \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, opacity=0, inner sep=10pt] (Pdamped) {};

      \node[fit={(Pdamped.north-|J.west) (Plinse)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Plin) {};
      \node[anchor={north west}] at (Plin.north west){$P_\text{lin}$};

      \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, fill=black!40!white, draw, dashed, inner sep=10pt] (Pdamped) {};
      \node[anchor={north west}] at (Pdamped.north west){$P_\text{damped}$};
    \end{scope}

  \end{tikzpicture}

Cascaded Control consisting of (from inner to outer loop): IFF, Linearization Loop, Tracking Control in the frame of the Legs

This cascade control is designed in three steps:

• In section sec:lac_iff: an active damping controller is designed. This is based on the Integral Force Feedback and applied in a decentralized way
• In section sec:hac_joint_space: a decentralized tracking control is designed in the frame of the legs. This controller is based on the displacement of each of the legs
• In section sec:primary_controller: a controller is designed in the task space in order to follow the wanted reference path corresponding to the sample position with respect to the granite

Initialization

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', 'cascade-hac-lac');

And we put some gravity.

  initializeSimscapeConfiguration('gravity', true);

We log the signals.

  initializeLoggingConfiguration('log', 'all');

  Kp = tf(zeros(6));
  Kl = tf(zeros(6));
  Kiff = tf(zeros(6));

Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$

<<sec:lac_iff>>

Identification

Let's first identify the plant for the IFF controller.

  %% 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'};

Plant

<<plt-matlab>>

IFF Plant (png, pdf)

Root Locus

<<plt-matlab>>

Root Locus for the IFF control (png, pdf)

The maximum damping is obtained for a control gain of $\approx 3000$.

Controller and Loop Gain

We create the $6 \times 6$ diagonal Integral Force Feedback controller. The obtained loop gain is shown in Figure fig:cascade_iff_loop_gain.

  w0 = 2*pi*50;
  Kiff = -3000/s*eye(6);

<<plt-matlab>>

Obtained Loop gain the IFF Control (png, pdf)

High Authority Control in the joint space - $\bm{K}_\mathcal{L}$

<<sec:hac_joint_space>>

Identification of the damped plant

We now identify the transfer function from $\tau^\prime$ to $d\bm{\mathcal{L}}$ as shown in Figure fig:cascade_control_architecture.

  %% Name of the Simulink File
  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, '/Micro-Station'], 3, 'output', [], 'Dnlm');  io_i = io_i + 1; % Leg Displacement

  %% Run the linearization
  Gl = linearize(mdl, io, 0);
  Gl.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  Gl.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};

There are some unstable poles in the Plant with very small imaginary parts. These unstable poles are probably not physical, and they disappear when taking the minimum realization of the plant.

  isstable(Gl)
  Gl = minreal(Gl);
  isstable(Gl)

Obtained Plant

The obtain plant is shown in Figure fig:cascade_hac_joint_plant.

We can see that the plant is quite well decoupled.

<<plt-matlab>>

Plant for the High Authority Control in the Joint Space (png, pdf)

Controller Design and Loop Gain

The controller consists of:

• A pure integrator
• A Second integrator up to half the wanted bandwidth
• A Lead around the cross-over frequency
• A low pass filter with a cut-off equal to two times the wanted bandwidth

  wc = 2*pi*400; % Bandwidth Bandwidth [rad/s]

  h = 2; % Lead parameter

  % Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc*2 + 1)/(s/wc*2)) * (1/(1 + s/wc/2));
  Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s;

  % Normalization of the gain of have a loop gain of 1 at frequency wc
  Kl = Kl.*diag(1./diag(abs(freqresp(Gl*Kl, wc))));

<<plt-matlab>>

Loop Gain for the High Autority Control in the joint space (png, pdf)

Primary Controller in the task space - $\bm{K}_\mathcal{X}$

<<sec:primary_controller>>

Identification of the linearized plant

We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\mathcal{X}$.

  %% Name of the Simulink File
  mdl = 'nass_model';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'],  1, 'input'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En');      io_i = io_i + 1; % Position Errror

  %% Run the linearization
  Gx = linearize(mdl, io, 0);
  Gx.InputName  = {'rL1', 'rL2', 'rL3', 'rL4', 'rL5', 'rL6'};
  Gx.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

As before, we take the minimum realization.

  isstable(Gx)
  Gx = minreal(Gx);
  isstable(Gx)

Obtained Plant

<<plt-matlab>>

Plant for the Primary Controller (png, pdf)

Controller Design

  wc = 2*pi*10; % Bandwidth Bandwidth [rad/s]

  h = 2; % Lead parameter

  Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * (s + 2*pi*5)/s * 1/(1+s/2/pi/20);

  % Normalization of the gain of have a loop gain of 1 at frequency wc
  Kp = Kp.*diag(1./diag(abs(freqresp(Gx*Kp, wc))));

<<plt-matlab>>

Loop Gain for the primary controller (outer loop) (png, pdf)

Simulation

  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '2');

And we simulate the system.

  sim('nass_model');

  cascade_hac_lac = simout;
  save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac');

Results

  load('./mat/experiment_tomography.mat', 'tomo_align_dist');
  load('./mat/cascade_hac_lac.mat', 'cascade_hac_lac');

  n_av = 4;
  han_win = hanning(ceil(length(cascade_hac_lac.Em.En.Data(:,1))/n_av));

  t = cascade_hac_lac.Em.En.Time;
  Ts = t(2)-t(1);

  [pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
  [pxx_ca, ~] = pwelch(cascade_hac_lac.Em.En.Data, han_win, [], [], 1/Ts);

<<plt-matlab>>

ASD of the position error (png, pdf)

<<plt-matlab>>

Cumulative Amplitude Spectrum of the position error (png, pdf)

<<plt-matlab>>

Results of the Tomography Experiment (png, pdf)