We consider one joint of a robot manipulator (or a 1-DOF rotary test rig) described by a standard rigid-body dynamics equation with an additive disturbance. The joint coordinate is \( q \), and we assume that the students already know how to obtain the dynamics from the Lagrangian or using existing robotics libraries.

Our main objectives are:

• Implement robust tracking of a reference trajectory \( q_d(t) \) despite unknown but bounded disturbance \( d(t) \).
• Compare a nominal computed-torque controller with a robustified version combining a sliding term and/or a disturbance observer.
• Quantify robustness in terms of tracking error norms and maximum disturbance amplitude for which stability and performance are maintained.

We use a 1-DOF model for coding simplicity, but the formulas extend directly to multi-DOF manipulators with matrices \( \mathbf{M}(\mathbf{q}), \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}), \mathbf{g}(\mathbf{q}) \).

\[ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + d(t), \quad |d(t)| \le \bar{d} \]

The tracking error and its derivative are \( e = q - q_d \), \( \dot{e} = \dot{q} - \dot{q}_d \). We will use the sliding variable \( s = \dot{e} + \lambda e \) with design parameter \( \lambda > 0 \).

Recall from the sliding-mode control lesson that we can design a robust controller by enforcing the dynamics of the sliding variable \( s = \dot{e} + \lambda e \) to satisfy a reaching condition such as \( \dot{V} \le -\eta |s| \) for some \( \eta > 0 \), where \( V = \tfrac{1}{2} M(q) s^2 \) is a Lyapunov function candidate.

For the 1-DOF dynamics with disturbance we have

\[ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + d(t). \]

Differentiating the sliding variable and using the dynamics, we obtain (after standard algebra using known properties of robot dynamics):

\[ M(q)\dot{s} = \tau - \tau_{\text{eq}} + d_{\text{eq}}(t), \]

where \( \tau_{\text{eq}} \) is the equivalent (nominal) control cancelling the known dynamics and imposing nominal error dynamics, and \( d_{\text{eq}}(t) \) collects modeling errors and the external disturbance \( d(t) \). In practice we only know an approximate model yielding \( \hat{M}(q), \hat{C}(q,\dot{q}), \hat{g}(q) \).

We choose a control structure

\[ \tau = \tau_{\text{nom}} + \tau_{\text{rob}} \]

with

\[ \tau_{\text{nom}} = \hat{M}(q)\bigl(\ddot{q}_d - \lambda(\dot{q}-\dot{q}_d)\bigr) + \hat{C}(q,\dot{q})\dot{q} + \hat{g}(q), \]

which corresponds to model-based feedback linearization of the nominal system on the sliding surface \( s=0 \), and a robust term

\[ \tau_{\text{rob}} = -k_s \operatorname{sat}\!\left(\frac{s}{\phi}\right), \quad k_s > 0,\ \phi > 0, \]

where \( \operatorname{sat}(\cdot) \) is the standard saturation function \( \operatorname{sat}(\sigma) = \sigma \) if \( |\sigma| \le 1 \) and \( \operatorname{sat}(\sigma) = \operatorname{sgn}(\sigma) \) otherwise. Using the usual bounds on modeling uncertainty and disturbance, one can show that if \( k_s \) exceeds a known upper bound on \( |d_{\text{eq}}(t)| \), then the closed-loop satisfies a reaching condition and \( s(t) \rightarrow 0 \), so the tracking error \( e(t) \) is driven to zero exponentially on the sliding surface.

In this lab we will implement this law and verify experimentally (in simulation or on real hardware) how the tracking error behaves when the disturbance amplitude is varied.

The saturation-based robust term can introduce chattering when implemented with finite sampling rates and noisy measurements. A complementary strategy, studied in the previous lesson, is to estimate the disturbance with a disturbance observer and compensate it.

For the 1-DOF system we define a disturbance estimate \( \hat{d}(t) \) with a first-order observer of the form

\[ \dot{\hat{d}} = -\alpha \hat{d} + \alpha\bigl(M(q)\ddot{q}_{\text{est}} + C(q,\dot{q})\dot{q} + g(q) - \tau\bigr), \quad \alpha > 0, \]

where \( \ddot{q}_{\text{est}} \) is obtained by differentiating filtered velocity measurements (in the lab we approximate it numerically). In steady state, the term in parentheses converges to the disturbance, and the low-pass filter with gain \( \alpha \) ensures noise attenuation.

We then modify the control law to compensate the estimated disturbance:

\[ \tau = \tau_{\text{nom}} + \tau_{\text{rob}} - \hat{d}, \]

where \( \tau_{\text{rob}} \) now uses a smaller gain \( k_s \) (since the residual disturbance \( d(t) - \hat{d}(t) \) is reduced), thereby reducing chattering. The lab will explore this trade-off between robustness, disturbance-observer bandwidth \( \alpha \), and control smoothness.

The following workflow should be followed in all implementations (Python, C++, Java, MATLAB/Simulink, Mathematica). This is the conceptual algorithm you will map to each programming environment.

flowchart TD
  A["Define model: M(q), C(q,dq), g(q) for 1 DOF"] --> B["Choose reference q_d(t), dq_d(t), ddq_d(t)"]
  B --> C["Select lambda, k_s, phi, alpha for robust control"]
  C --> D["Implement simulation loop: integrate q, dq with control law"]
  D --> E["Inject disturbance d(t) (e.g. sin, steps, friction)"]
  E --> F["Log e(t), s(t), tau(t), d(t), d_hat(t)"]
  F --> G["Analyze robustness: errors vs disturbance amplitude"]
        

In numerical implementations, ensure that the integration time step is sufficiently small relative to the dynamics and controller bandwidths, and that any numerical differentiation (used for \( \ddot{q}_{\text{est}} \)) is filtered to avoid amplifying noise.

The block-level architecture of the robust tracking experiment is shown below. This is what your code or Simulink diagram should implement conceptually.

flowchart TD
  REF["q_d, dq_d, ddq_d"] --> CTRL["Robust controller (nominal + sliding)"]
  MEAS["q, dq"] --> CTRL
  CTRL --> SUM["Compute tau = tau_nom + tau_rob - d_hat"]
  SUM --> PLANT["Robot joint model + disturbance d(t)"]
  PLANT --> MEAS
  MEAS --> ERR["Compute error e, s"]
  PLANT --> DOB["Disturbance observer"]
  DOB --> SUM
        

In code, the main loop will update the plant state, compute the disturbance, update the disturbance observer, and then compute \( \tau \) according to the control law. Logged data will be used to plot tracking error and control inputs.

We now implement the lab in Python, using numpy for numerics and scipy.integrate.solve_ivp for simulation. For multi-DOF robots, one would typically use a library such as roboticstoolbox-python or pinocchio to evaluate \( \mathbf{M}, \mathbf{C}, \mathbf{g} \). Here we stay with 1-DOF for clarity.

import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt

# Physical parameters of 1-DOF rotary link
m = 1.0     # kg
l = 0.5     # m
g = 9.81    # m/s^2
I = m * l**2

# Robust control gains
lam = 4.0       # lambda in s = e_dot + lam * e
k_s = 5.0       # sliding gain
phi = 0.1       # boundary layer width
alpha = 30.0    # disturbance observer bandwidth

# Reference trajectory and derivatives
def q_d(t):
    return 0.5 * np.sin(1.0 * t)

def dq_d(t):
    return 0.5 * np.cos(1.0 * t)

def ddq_d(t):
    return -0.5 * np.sin(1.0 * t)

# Disturbance model: smooth + Coulomb-like component
def disturbance(t, q, dq):
    return 1.5 * np.sin(3.0 * t) + 0.5 * np.sign(dq + 1e-6)

def sat(s, phi):
    sigma = s / phi
    if sigma > 1.0:
        return 1.0
    elif sigma < -1.0:
        return -1.0
    else:
        return sigma

def dynamics(t, x):
    # state x = [q, dq, d_hat]
    q, dq, d_hat = x

    # tracking errors
    e = q - q_d(t)
    de = dq - dq_d(t)
    s = de + lam * e

    # nominal control
    # gravity term for pendulum-like link
    g_term = m * g * l * np.sin(q)
    tau_nom = I * (ddq_d(t) - lam * de) + g_term

    # robust sliding term with boundary layer
    tau_rob = -k_s * sat(s, phi)

    # total control: with disturbance compensation
    tau = tau_nom + tau_rob - d_hat

    # true disturbance
    d = disturbance(t, q, dq)

    # plant dynamics: I * ddq + g(q) = tau + d
    ddq = (tau + d - g_term) / I

    # approximate q_ddot using model (for observer)
    # here we use model-based estimate: q_ddot_est = ddq
    q_ddot_est = ddq

    # disturbance observer dynamics
    d_hat_dot = -alpha * d_hat + alpha * (I * q_ddot_est + g_term - tau)

    return [dq, ddq, d_hat_dot]

# Initial conditions
x0 = [0.0, 0.0, 0.0]  # q(0), dq(0), d_hat(0)

tf = 20.0
sol = solve_ivp(dynamics, [0.0, tf], x0, max_step=1e-3, rtol=1e-6, atol=1e-8)
t = sol.t
q = sol.y[0]
dq = sol.y[1]
d_hat = sol.y[2]

# Compute reference and errors for analysis
qd = q_d(t)
dqd = dq_d(t)
e = q - qd
de = dq - dqd
s = de + lam * e

# Plot results
plt.figure()
plt.plot(t, qd, label="q_d(t)")
plt.plot(t, q, "--", label="q(t)")
plt.xlabel("t [s]")
plt.ylabel("position [rad]")
plt.legend()
plt.title("Robust tracking with disturbance observer")

plt.figure()
plt.plot(t, e, label="e(t)")
plt.plot(t, s, label="s(t)")
plt.xlabel("t [s]")
plt.ylabel("error")
plt.legend()
plt.title("Tracking and sliding variables")

plt.figure()
plt.plot(t, d_hat, label="d_hat(t)")
plt.xlabel("t [s]")
plt.ylabel("disturbance estimate")
plt.legend()
plt.title("Disturbance estimate")

plt.show()
      

You can repeat the experiment with different disturbance amplitudes or gain values k_s and alpha and observe how the tracking error and control smoothness change. For more complex robots, replace the scalar inertia and gravity term by calls to a robotics dynamics library.

In a real-time controller, C++ is often used together with a rigid-body dynamics library such as RBDL or Pinocchio for multi-DOF robots. Below is a self-contained 1-DOF implementation using Euler integration. For multi-DOF systems, replace the scalar dynamics by calls to your chosen dynamics library.

#include <iostream>
#include <cmath>

struct State {
    double q;
    double dq;
    double d_hat;
};

const double m = 1.0;
const double l = 0.5;
const double g = 9.81;
const double I = m * l * l;

// control gains
const double lam = 4.0;
const double k_s = 5.0;
const double phi = 0.1;
const double alpha = 30.0;

double q_d(double t) {
    return 0.5 * std::sin(t);
}

double dq_d(double t) {
    return 0.5 * std::cos(t);
}

double ddq_d(double t) {
    return -0.5 * std::sin(t);
}

double disturbance(double t, double q, double dq) {
    // simple bounded disturbance
    double sign_dq = 0.0;
    if (dq > 0.0) sign_dq = 1.0;
    if (dq < 0.0) sign_dq = -1.0;
    return 1.5 * std::sin(3.0 * t) + 0.5 * sign_dq;
}

double sat(double s, double phi) {
    double sigma = s / phi;
    if (sigma > 1.0) return 1.0;
    if (sigma < -1.0) return -1.0;
    return sigma;
}

State dynamics(double t, const State& x) {
    State dx;
    double q = x.q;
    double dq = x.dq;
    double d_hat = x.d_hat;

    double e = q - q_d(t);
    double de = dq - dq_d(t);
    double s = de + lam * e;

    double g_term = m * g * l * std::sin(q);

    double tau_nom = I * (ddq_d(t) - lam * de) + g_term;
    double tau_rob = -k_s * sat(s, phi);
    double tau = tau_nom + tau_rob - d_hat;

    double d = disturbance(t, q, dq);
    double ddq = (tau + d - g_term) / I;

    double q_ddot_est = ddq;
    double d_hat_dot = -alpha * d_hat + alpha * (I * q_ddot_est + g_term - tau);

    dx.q = dq;
    dx.dq = ddq;
    dx.d_hat = d_hat_dot;
    return dx;
}

int main() {
    double t = 0.0;
    const double tf = 20.0;
    const double dt = 0.001;

    State x{0.0, 0.0, 0.0};

    while (t < tf) {
        State k1 = dynamics(t, x);
        // simple explicit Euler for illustration
        x.q += dt * k1.q;
        x.dq += dt * k1.dq;
        x.d_hat += dt * k1.d_hat;

        t += dt;

        if (std::fmod(t, 0.5) < dt) {
            double e = x.q - q_d(t);
            std::cout << t << " " << x.q << " " << e << "\n";
        }
    }

    return 0;
}
      

In a full robotics project, the function dynamics would evaluate the multi-DOF dynamics using, for instance, RBDL::CompositeRigidBodyAlgorithm for \( \mathbf{M}(\mathbf{q}) \) and RBDL::NonlinearEffects for \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) \).

Java is often used for robotics software stacks where hard real-time is not required (e.g., high-level control or simulation). Here we implement the same control law with a fixed-step integrator. Libraries such as EJML can be used for matrix operations in multi-DOF settings.

public class RobustTrackingLab1DOF {

    static final double m = 1.0;
    static final double l = 0.5;
    static final double g = 9.81;
    static final double I = m * l * l;

    static final double lam = 4.0;
    static final double k_s = 5.0;
    static final double phi = 0.1;
    static final double alpha = 30.0;

    static double q_d(double t) {
        return 0.5 * Math.sin(t);
    }

    static double dq_d(double t) {
        return 0.5 * Math.cos(t);
    }

    static double ddq_d(double t) {
        return -0.5 * Math.sin(t);
    }

    static double disturbance(double t, double q, double dq) {
        double sign_dq = 0.0;
        if (dq > 0.0) sign_dq = 1.0;
        if (dq < 0.0) sign_dq = -1.0;
        return 1.5 * Math.sin(3.0 * t) + 0.5 * sign_dq;
    }

    static double sat(double s, double phi) {
        double sigma = s / phi;
        if (sigma > 1.0) return 1.0;
        if (sigma < -1.0) return -1.0;
        return sigma;
    }

    public static void main(String[] args) {
        double t = 0.0;
        double tf = 20.0;
        double dt = 0.001;

        double q = 0.0;
        double dq = 0.0;
        double d_hat = 0.0;

        while (t < tf) {
            double e = q - q_d(t);
            double de = dq - dq_d(t);
            double s = de + lam * e;

            double g_term = m * g * l * Math.sin(q);

            double tau_nom = I * (ddq_d(t) - lam * de) + g_term;
            double tau_rob = -k_s * sat(s, phi);
            double tau = tau_nom + tau_rob - d_hat;

            double d = disturbance(t, q, dq);
            double ddq = (tau + d - g_term) / I;

            double q_ddot_est = ddq;
            double d_hat_dot = -alpha * d_hat + alpha * (I * q_ddot_est + g_term - tau);

            // Euler integration
            q += dt * dq;
            dq += dt * ddq;
            d_hat += dt * d_hat_dot;

            t += dt;

            if (Math.abs((t % 0.5) - 0.25) < dt) {
                System.out.println(t + " " + q + " " + e);
            }
        }
    }
}
      

This Java implementation mirrors the continuous-time design but uses a simple explicit integration scheme. For more accurate simulations, use higher-order integrators or link to a numerical ODE solver library.

MATLAB is particularly suited for rapid prototyping of robot control and for building Simulink diagrams that match the conceptual architecture. Below is a script that implements the robust controller and disturbance observer using ode45. You can replicate the structure in Simulink using standard blocks.

function robust_tracking_lab_matlab
    m = 1.0;
    l = 0.5;
    g = 9.81;
    I = m * l^2;

    lam = 4.0;
    k_s = 5.0;
    phi = 0.1;
    alpha = 30.0;

    x0 = [0.0; 0.0; 0.0];  % [q; dq; d_hat]
    tf = 20.0;

    [t, x] = ode45(@(t,x) dyn(t, x, m, l, g, I, lam, k_s, phi, alpha), [0 tf], x0);

    q = x(:,1);
    dq = x(:,2);
    d_hat = x(:,3);

    qd = q_d(t);
    dqd = dq_d(t);
    e = q - qd;
    de = dq - dqd;
    s = de + lam * e;

    figure;
    plot(t, qd, t, q, '--');
    xlabel('t [s]'); ylabel('q [rad]');
    legend('q_d', 'q');
    title('Robust tracking with disturbance observer');

    figure;
    plot(t, e, t, s);
    legend('e', 's');
    xlabel('t [s]'); ylabel('error');
    title('Tracking error and sliding variable');

    figure;
    plot(t, d_hat);
    xlabel('t [s]'); ylabel('d\_hat');
    title('Disturbance estimate');

end

function y = q_d(t)
    y = 0.5 * sin(t);
end

function y = dq_d(t)
    y = 0.5 * cos(t);
end

function y = ddq_d(t)
    y = -0.5 * sin(t);
end

function d = disturbance(t, q, dq)
    sign_dq = zeros(size(dq));
    sign_dq(dq > 0) = 1;
    sign_dq(dq < 0) = -1;
    d = 1.5 * sin(3.0 * t) + 0.5 * sign_dq;
end

function dx = dyn(t, x, m, l, g, I, lam, k_s, phi, alpha)
    q = x(1);
    dq = x(2);
    d_hat = x(3);

    e = q - q_d(t);
    de = dq - dq_d(t);
    s = de + lam * e;

    g_term = m * g * l * sin(q);

    tau_nom = I * (ddq_d(t) - lam * de) + g_term;

    sigma = s / phi;
    if sigma > 1
        sat_s = 1;
    elseif sigma < -1
        sat_s = -1;
    else
        sat_s = sigma;
    end
    tau_rob = -k_s * sat_s;

    tau = tau_nom + tau_rob - d_hat;

    d = disturbance(t, q, dq);
    ddq = (tau + d - g_term) / I;

    q_ddot_est = ddq;
    d_hat_dot = -alpha * d_hat + alpha * (I * q_ddot_est + g_term - tau);

    dx = [dq; ddq; d_hat_dot];
end
      

In Simulink, implement the blocks: (i) a plant block integrating q and dq; (ii) a controller block computing tau from q, dq, q_d, dq_d, ddq_d; (iii) a disturbance block adding d(t); (iv) a disturbance observer block implementing the first-order filter for d_hat. Use a Rate Transition block if you connect to a discrete sampling subsystem.

Mathematica is useful for symbolic derivations and quick numerical experiments. Below is a direct translation of the continuous-time dynamics with the robust controller and disturbance observer.

m = 1.0; l = 0.5; g = 9.81;
I = m l^2;

lam = 4.0;
kS = 5.0;
phi = 0.1;
alpha = 30.0;

qD[t_] := 0.5 Sin[t];
dqD[t_] := 0.5 Cos[t];
ddqD[t_] := -0.5 Sin[t];

disturbance[t_, q_, dq_] :=
  1.5 Sin[3.0 t] + 0.5 Sign[dq + 10^-6];

sat[s_, phi_] := Module[{sigma = s/phi},
  Which[
    sigma > 1, 1,
    sigma < -1, -1,
    True, sigma]
  ];

eqs = {
  q'[t] == dq[t],
  dq'[t] == (tau[t] + disturbance[t, q[t], dq[t]] - m g l Sin[q[t]])/I,
  dHat'[t] == -alpha dHat[t] +
    alpha (I dq'[t] + m g l Sin[q[t]] - tau[t])
  };

e[t_] := q[t] - qD[t];
de[t_] := dq[t] - dqD[t];
s[t_] := de[t] + lam e[t];

tau[t_] := I (ddqD[t] - lam de[t]) + m g l Sin[q[t]] - kS sat[s[t], phi] - dHat[t];

ics = {q[0] == 0, dq[0] == 0, dHat[0] == 0};
sol = NDSolve[Flatten[{eqs, ics}], {q, dq, dHat}, {t, 0, 20},
  MaxStepSize -> 0.001];

Plot[{qD[t], q[t] /. sol[[1]]}, {t, 0, 20},
  PlotLegends -> {"q_d(t)", "q(t)"},
  AxesLabel -> {"t", "position"}]

Plot[{e[t] /. sol[[1]], s[t] /. sol[[1]]}, {t, 0, 20},
  PlotLegends -> {"e(t)", "s(t)"},
  AxesLabel -> {"t", "error"}]

Plot[dHat[t] /. sol[[1]], {t, 0, 20},
  PlotLegends -> {"d_hat(t)"},
  AxesLabel -> {"t", "disturbance estimate"}]
      

Mathematica also allows symbolic derivation of Lyapunov derivatives and automatic generation of C or other language code, which can be useful when moving from theory to embedded implementation.

In this lab we implemented a robust tracking controller for a robot joint subject to unknown but bounded disturbances. Using the sliding variable \( s = \dot{e} + \lambda e \), we designed a nominal computed-torque law and augmented it with a robust sliding term and a disturbance observer. We showed, via Lyapunov analysis, how the sliding gain and boundary layer affect the reaching condition and steady-state error. Implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica demonstrated how the same control architecture can be realized across different environments and prepared you for extending these ideas to multi-DOF manipulators using robotics dynamics libraries.