nass-simscape/org/control.org

Control of the Nano-Active-Stabilization-System

• Introduction
• Control Configuration - Introduction
• Tracking Control - Basic Architectures
  • Introduction
  • Control in the frame of the Legs
  • Control in the Cartesian frame
• Active Damping Architecture - Collocated Control
  • Introduction
  • Integral Force Feedback
  • Direct Relative Velocity Feedback
• HAC-LAC Architectures
  • Introduction
  • HAC-LAC using IFF and Tracking control in the frame of the Legs
  • HAC-LAC using IFF and Tracking control in the Cartesian frame
• Cascade Architectures
  • Introduction
  • Cascade Control with HAC-LAC Inner Loop and Primary Controller in the task space
  • Cascade Control with HAC-LAC Inner Loop and Primary Controller in the joint space
• Sensor Fusion Architectures
• $\mathcal{H}_\infty$ Architectures
• Force Control
• Bibliography

Introduction   ignore

The system consist of the following inputs and outputs (Figure fig:control_architecture_plant):

• $\bm{\tau}$: Forces applied in each leg
• $\bm{\tau}_m$: Force sensor located in each leg
• $\bm{\mathcal{X}}$: Measurement of the payload position with respect to the granite
• $d\bm{\mathcal{L}}$: Measurement of the (small) relative motion of each leg

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

    \draw[->] (outputF) -- ++(1, 0) node[above left]{$\bm{\tau}_m$};
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};

    \draw[<-] (inputF) -- ++(-1, 0) node[above right]{$\bm{\tau}$};
  \end{tikzpicture}

Block diagram with the inputs and outputs of the system

In order to position the Sample with respect to the granite, we must use the measurement $\bm{\mathcal{X}}$ in the control loop. The wanted position of the sample with respect to the granite is represented by $\bm{r}_\mathcal{X}$. From $\bm{r}_\mathcal{X}$ and $\bm{\mathcal{X}}$, we can compute the required small change of pose of the nano-hexapod's top platform expressed in the frame of the nano-hexapod's base as shown in Figure fig:control_architecture_pos_error.

This can we considered as:

• the position error $\bm{\epsilon}_{\mathcal{X}_n}$ expressed in a frame attach to the base of the nano-hexapod
• the wanted (small) pose displacement $\bm{r}_{d\mathcal{X}_n}$ of the nano-hexapod mobile platform with respect to its base

  \begin{tikzpicture}
    \node[block, align=center] (Ex) {Compute\\Pos. Error};
    \draw[<-] (Ex.west)  -- ++(-1, 0)node[above right]{$\bm{r}_{\mathcal{X}}$};
    \draw[<-] (Ex.south) -- ++(0, -1)node[above right]{$\bm{\mathcal{X}}$};
    \draw[->] (Ex.east)  -- ++(1,  0)node[above left ]{$\bm{r}_{\mathcal{X}_n}$};
  \end{tikzpicture}

Block diagram corresponding to the computation of the position error in the frame of the nano-hexapod

In this document, we see how the different outputs of the system can be used to control of position $\bm{\mathcal{X}}$.

Control Configuration - Introduction

In this section, we discuss the control configuration for the NASS.

From cite:skogestad07_multiv_feedb_contr:

We define the control configuration to be the restrictions imposed on the overall controller $K$ by decomposing it into a set of local controllers with predetermined links and with a possibly predetermined design sequence where subcontrollers are designed locally.

Some elements used to build up a specific control configuration are:

• Cascade controllers. The output from one controller is the input to another
• Decentralized controllers. The control system consists of independent feedback controllers which interconnect a subset of the output measurements with a subset of the manipulated inputs. These subsets should not be used by any other controller
• Feedforward elements. Link measured disturbances and manipulated inputs
• Decoupling elements. Link one set of manipulated inputs with another set of manipulated inputs. They are used to improve the performance of decentralized control systems.

Decoupling elements will be used to convert quantities from the joint space to the task space and vice-versa.

Decentralized controllers will be largely used both for Tracking control (Section sec:tracking_control) and for Active Damping techniques (Section sec:active_damping)

Combining both can be done in an HAC-LAC topology presented in Section sec:hac_lac.

The use of decentralized controllers is proposed in Section sec:cascade_control.

Tracking Control - Basic Architectures

<<sec:tracking_control>>

Introduction   ignore

In this section, we suppose that we want to track some reference position $\bm{r}_{\mathcal{X}_n}$ corresponding to the pose of the nano-hexapod's mobile platform with respect to its fixed base.

To do so, we have to the use the leg's length measurement $d\bm{\mathcal{L}}$.

However, thanks to the forward and inverse kinematics, the controller can either be designed in the task space or in the joint space.

These to configuration are described in the next two sections.

Control in the frame of the Legs

<<sec:tracking_control_leg_frame>>

From the wanted small change in pose of the nano-hexapod's mobile platform $\bm{r}_{d\mathcal{X}_n}$, we can use the Inverse Kinematics of the nano-hexapod to compute the corresponding small change of the leg length of the nano-hexapod $\bm{r}_{d\mathcal{L}}$. Then, this is subtracted by the measurement of the leg relative displacement $d\bm{\mathcal{L}}$ to obtain to displacement error of each leg $\bm{\epsilon}_{d\mathcal{L}}$. Finally, a diagonal (Decentralized) controller $\bm{K}_\mathcal{L}$ can be used.

  \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, left= of inputF] (K) {$\bm{K}_\mathcal{L}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};
    \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[above left]{$\bm{\tau}_m$};
    \draw[->] (outputX) -- ++(1.0, 0) node[above left]{$\bm{\mathcal{X}}$};
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
    \draw[->] (K.east) -- (inputF) node[above left]{$\bm{\tau}$};

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

Control in the frame of the legs

Control in the Cartesian frame

<<sec:tracking_control_task_frame>>

From the relative displacement of each leg $d\bm{\mathcal{L}}$, the pose of the nano-hexapod's mobile platform $\bm{\mathcal{X}_n}$ is estimated. It is then subtracted from reference pose of the nano-hexapod $\bm{r}_{\mathcal{X}_n}$ to obtain the pose error $\bm{\epsilon}_{\mathcal{X}_n}$. A diagonal controller $\bm{K}_\mathcal{X}$ is used to generate forces and torques applied on the payload in a frame attached to the nano-hexapod's base. These forces are then converted to forces applied in each of the nano-hexapod's actuators by the use of the Jacobian $\bm{J}^{-T}$.

  \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, align=center, below=0.4 of P] (Forkin) {Forward\\Kinematics};

    \node[block, left= of inputF] (J) {$\bm{J}^{-T}$};
    \node[block, left= of J] (K) {$\bm{K}_\mathcal{X}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[above left]{$\bm{\tau}_m$};
    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} |- (Forkin.east);
    \draw[->] (Forkin.west) -| (subr.south) node[below right]{$\bm{\mathcal{X}_n}$};

    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}_n}$};
    \draw[->] (K.east) -- (J.west) node[above left]{$\bm{\mathcal{F}}$};
    \draw[->] (J.east) -- (inputF.west) node[above left]{$\bm{\tau}$};
    \draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0);
  \end{tikzpicture}

Control in the cartesian Frame (rotating frame attached to the nano-hexapod's base)

Active Damping Architecture - Collocated Control

<<sec:active_damping>>

Introduction   ignore

From cite:preumont18_vibrat_contr_activ_struc_fourt_edition:

Active damping is very effective in reducing the settling time of transient disturbances and the effect of steady state disturbances near the resonance frequencies of the system; however, away from the resonances, the active damping is completely ineffective and leaves the closed-loop response essentially unchanged. Such low-gain controllers are often called Low Authority Controllers (LAC), because they modify the poles of the system only slightly.

Two very well known active damping techniques are Integral Force Feedback and Direct Velocity Feedback.

These two active damping techniques are collocated control techniques.

Integral Force Feedback

<<sec:active_damping_iff>>

In this active damping technique, the force sensors in each leg is used.

The controller $\bm{K}_\text{IFF}$ is a diagonal matrix, each of its diagonal element consists of:

• an pure integrator
• a gain $g$ that can be tuned to achieve a maximum damping

\begin{equation} \bm{K}_\text{IFF}(s) = \frac{g}{s} \bm{I}_{6} \end{equation}

A lead-lag can also be used instead of a pure integrator.

  \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) {};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};

    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
    \draw[<-] (addF.west) -- ++(-1, 0) node[above right]{$\bm{\tau}^\prime$};
  \end{tikzpicture}

Integral Force Feedback

Direct Relative Velocity Feedback

<<sec:active_damping_dvf>>

The controller $\bm{K}_\text{DVF}$ is a diagonal matrix. Each diagonal element consists of:

• a derivative action up to some frequency $\omega_0$
• a gain $g$ that can be tuned to achieve a maximum damping

\begin{equation} \bm{K}_\text{DVF}(s) = \frac{g s}{\omega_0 + s} \bm{I}_{6} \end{equation}

  \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, below=0.4 of P] (Kdvf) {$\bm{K}_\text{DVF}$};
    \node[addb={+}{}{}{}{-}, left= of inputF] (addF) {};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[above left]{$\bm{\tau}_m$};
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};

    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} |- (Kdvf.east);
    \draw[->] (Kdvf.west) -| (addF.south);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
    \draw[<-] (addF.west) -- ++(-1, 0) node[above right]{$\bm{\tau}^\prime$};
  \end{tikzpicture}

Direct Velocity Feedback

HAC-LAC Architectures

<<sec:hac_lac>>

Introduction   ignore

Here we can combine Active Damping Techniques (Low authority control) with a tracking controller (high authority control). Usually, the low authority controller is designed first, and the high authority controller is designed based on the damped plant.

From cite:preumont18_vibrat_contr_activ_struc_fourt_edition:

The HAC/LAC approach consist of combining the two approached in a dual-loop control as shown in Figure fig:control_architecture_hac_lac. The inner loop uses a set of collocated actuator/sensor pairs for decentralized active damping with guaranteed stability ; the outer loop consists of a non-collocated HAC based on a model of the actively damped structure. This approach has the following advantages:

• The active damping extends outside the bandwidth of the HAC and reduces the settling time of the modes which are outsite the bandwidth
• The active damping makes it easier to gain-stabilize the modes outside the bandwidth of the output loop (improved gain margin)
• The larger damping of the modes within the controller bandwidth makes them more robust to the parmetric uncertainty (improved phase margin)

  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputH) at ($(P.south west)!0.3!(P.north west)$);
    \coordinate[] (inputL) at ($(P.south west)!0.7!(P.north west)$);
    \coordinate[] (outputH) at ($(P.south east)!0.3!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.7!(P.north east)$);

    \node[block, above=0.6 of P] (Klac) {$\bm{K}_\text{LAC}$};
    \node[block, left= of inputH] (Khac) {$\bm{K}_\text{HAC}$};
    \node[addb={+}{}{}{}{-}, left= of Khac] (subr) {};

    % Connections and labels
    \draw[->] (outputL) -- ++(1, 0);
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} node[below]{$x^\prime$} |- (Klac.east);
    \draw[->] (Klac.west) -| ($(inputL) + (-0.6, 0)$) -- (inputL) node[above left]{$u^\prime$};

    \draw[<-] (subr.west)node[above left]{$r$} -- ++(-1, 0);
    \draw[->] (outputH) -- ++(1, 0);
    \draw[->] ($(outputH) + (0.6, 0)$)node[branch]{} node[above]{$x$} -- ++(0, -1.5) -| (subr.south);
    \draw[->] (subr.east) -- (Khac.west) node[above left]{$\epsilon$};
    \draw[->] (Khac.east) -- (inputH) node[above left]{$u$};
  \end{tikzpicture}

HAC-LAC Control Architecture

If there is only one input to the system, the HAC-LAC topology can be represented as depicted in Figure fig:control_architecture_hac_lac_one_input. Usually, the Low Authority Controller is first design, and then the High Authority Controller is designed based on the damped plant.

  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (input)   at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputH) at ($(P.south east)!0.2!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);

    \node[block, above=0.6 of P] (Klac) {$\bm{K}_\text{LAC}$};
    \node[addb, left= of input] (addF) {};
    \node[block, left= of addF] (Khac) {$\bm{K}_\text{HAC}$};
    \node[addb={+}{}{}{}{-}, left= of Khac] (subr) {};

    % Connections and labels
    \draw[->] (outputL) -- ++(0.6, 0) coordinate(eastlac) |- (Klac.east);
    \node[above right] at (outputL){$x^\prime$};
    \draw[->] (Klac.west) -| (addF.north);
    \draw[->] (addF.east) -- (input) node[above left]{$u^\prime$};

    \draw[<-] (subr.west)node[above left]{$r$} -- ++(-1, 0);
    \draw[->] (outputH) -- ++(1.8, 0);
    \draw[->] ($(outputH) + (1.2, 0)$)node[branch]{} node[above]{$x$} -- ++(0, -1.5) -| (subr.south);
    \draw[->] (subr.east) -- (Khac.west) node[above left]{$\epsilon$};
    \draw[->] (Khac.east) -- (addF.west) node[above left=0 and 0.2]{$u$};

    \begin{scope}[on background layer]
      \node[fit={(Klac.north-|eastlac) (addF.west|-P.south)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Pi) {};
      \node[anchor={north west}] at (Pi.north west){$\text{Damped Plant}$};
    \end{scope}
  \end{tikzpicture}

HAC-LAC Architecture with a system having only one input

HAC-LAC using IFF and Tracking control in the frame of the Legs

  \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};

    % 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]{$d\bm{\mathcal{L}}$};
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
    \draw[->] (K.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};

    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};

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

IFF + Control in the frame of the legs

HAC-LAC using IFF and Tracking control in the Cartesian frame

  \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] (J) {$\bm{J}^{-T}$};
    \node[block, left= of J] (K) {$\bm{K}_\mathcal{X}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};

    \node[block, align=center, below=0.4 of P] (Forkin) {Forward\\Kinematics};

    % 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]{$d\bm{\mathcal{L}}$};
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} |- (Forkin.east);
    \draw[->] (Forkin.west) -| (subr.south) node[below right]{$\bm{\mathcal{X}_n}$};

    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};

    \draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}_n}$};
    \draw[->] (K.east) -- (J.west) node[above left]{$\bm{\mathcal{F}}$};
    \draw[->] (J.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
  \end{tikzpicture}

IFF + Control in the cartesian frame

Cascade Architectures

<<sec:cascade_control>>

Introduction   ignore

The principle of Cascade control is shown in Figure fig:control_architecture_cascade_control and explained as follow:

To follow two objectives with different properties in one control system, usually a hierarchy of two feedback loops is used in practice. This kind of control topology is called cascade control, which is used when there are several measurements and one prime control variable. Cascade control is implemented by nesting the control loops, as shown in Figure fig:control_architecture_cascade_control. The output control loop is called the primary loop, while the inner loop is called the secondary loop and is used to fulfill a secondary objective in the closed-loop system. – cite:taghirad13_paral

  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (input)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputi) at ($(P.south east)!0.3!(P.north east)$);
    \coordinate[] (outputo) at ($(P.south east)!0.7!(P.north east)$);

    \node[block, left= of input] (Ki) {$K_s$};
    \node[addb={+}{}{}{}{-}, left= of Ki] (subi) {};
    \node[block, left= of subi] (Ko) {$K_p$};
    \node[addb={+}{}{}{}{-}, left= of Ko] (subo) {};

    \node[above=0 of Ko, align=center] {Primary\\Controller};
    \node[above=0 of Ki, align=center] {Secondary\\Controller};

    % Connections and labels
    \draw[->] (outputi) -- ++(1.0, 0);
    \draw[->] ($(outputi) + (0.6, 0)$)node[branch](easti){} node[above]{$y_s$} -- ++(0, -1.3)coordinate(southi) -| (subi.south);

    \draw[->] (outputo) -- ++(1.8, 0);
    \draw[->] ($(outputo) + (1.4, 0)$)node[branch](easto){} node[above]{$y_p$} -- ++(0, -3.1)coordinate(southo) -| (subo.south);

    \draw[<-] (subo.west) node[above left=0 and 0.2]{$r_p$} -- ++(-1, 0);
    \draw[->] (subo.east) -- (Ko.west) node[above left]{$\epsilon_p$};
    \draw[->] (Ko.east) -- (subi.west) node[above left=0 and 0.2]{$r_s$};
    \draw[->] (subi.east) -- (Ki.west) node[above left]{$\epsilon_s$};
    \draw[->] (Ki.east) -- (input)     node[above left]{$u$};

    \begin{scope}[on background layer]
      \node[fit={(P.north-|easti) (subi.west|-southi)}, inner sep=8pt, opacity=0] (Plin) {};
      \node[fit={(Plin.north-|easto) (subo.west|-southo)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Po) {};
      \node[fit={(P.north-|easti) (subi.west|-southi)}, fill=black!40!white, draw, dashed, inner sep=8pt] (Pi) {};
      \node[anchor={north west}] at (Po.north west){$\text{Outer Loop}$};
      \node[anchor={north west}] at (Pi.north west){$\text{Inner Loop}$};
    \end{scope}
  \end{tikzpicture}

Cascade Control Architecture

This control topology seems adapted for the NASS, as indeed we have more inputs than outputs

In the NASS's case:

• The primary objective is to position the sample with respect to the granite, thus the outer loop (and primary controller) should corresponds to a motion control loop

The inner loop can be composed of the system controlled with the HAC-LAC topology.

Cascade Control with HAC-LAC Inner Loop and Primary Controller in the task space

  \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] (Kx) {$\bm{K}_\mathcal{X}$};

    \node[block, align=center, left= of Kx] (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) -- (Kx.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
    \draw[->] (Kx.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

Cascade Control with HAC-LAC Inner Loop and Primary Controller in the joint space

  \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, left= of subr] (Kl) {$\bm{K}_\mathcal{L}$};
    \node[block, align=center, left= of Kl] (J) {Inverse\\Kinematics};

    \node[block, align=center, left= of J] (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) -- (J.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
    \draw[->] (J.east) -- (Kl.west) node[above left]{$\bm{r}_{\mathcal{L}}$};
    \draw[->] (Kl.east) -- (subr.west) node[above left=0 and 6pt]{$\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-|subr.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 Cartesian Frame

Sensor Fusion Architectures

<<sec:sensor_fusion>>

$\mathcal{H}_\infty$ Architectures

<<sec:h_infinity>>

Force Control

Signals:

• $\bm{r}_\mathcal{F}$ is the wanted total force/torque to be applied to the payload
• $\bm{\epsilon}_\mathcal{F}$ is the force/torque errors that should be applied to the payload
• $\bm{\tau}$ is the force applied in each actuator

  \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, left= of inputF] (Jt) {$\bm{J}^{-T}$};
    \node[block, left= of Jt] (K) {$\bm{K}_\mathcal{F}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subF) {};

    \node[block, below=0.4 of P] (J) {$\bm{J}^{T}$};

    % Connections and labels
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};

    \draw[->] (outputF) -- ++(1.8, 0);
    \draw[->] ($(outputF) + (1.4, 0)$)node[branch]{} node[above]{$\bm{\tau}_m$} |- (J.east);
    \draw[->] (J.west) -| (subF.south)node[below right]{$\bm{\mathcal{F}}_m$};
    \draw[->] (subF.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{F}}$};
    \draw[->] (K.east) -- (Jt.west) node[above left]{$\bm{\mathcal{F}}$};
    \draw[->] (Jt.east) -- (inputF) node[above left]{$\bm{\tau}$};
    \draw[<-] (subF.west) -- ++(-1, 0) node[above right]{$\bm{r}_{\mathcal{F}}$};
  \end{tikzpicture}

Bibliography   ignore

bibliographystyle:unsrt bibliography:ref.bib