Stewart Platform - Tracking Control

The pose error \(\bm{\epsilon}_\mathcal{X}\) is first converted in the frame of the leg by using the Jacobian matrix. Then a diagonal controller \(\bm{K}_\mathcal{L}\) is designed. The final implemented controller is \(\bm{K} = \bm{K}_\mathcal{L} \cdot \bm{J}\) as shown in Figure 9

Note here that the transformation from the pose error \(\bm{\epsilon}_\mathcal{X}\) to the leg’s length errors by using the Jacobian matrix is only valid for small errors.

We now multiply the plant by the Jacobian matrix as shown in Figure 9 to obtain a more diagonal plant.

Gl = stewart.kinematics.J*G;
Gl.OutputName  = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};

The bode plot of the plant is shown in Figure 10. We can see that the diagonal elements are identical. This will simplify the design of the controller as all the elements of the diagonal controller can be made identical.

We can see that this totally decouples the system at low frequency.

This was expected since: \[ \bm{G}(\omega = 0) = \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0) = \bm{J}^{-1} \frac{\delta\bm{\mathcal{L}}}{\delta\bm{\tau}}(\omega = 0) = \bm{J}^{-1} \text{diag}(\mathcal{K}_1^{-1} \ \dots \ \mathcal{K}_6^{-1}) \]

Thus \(J \cdot G(\omega = 0) = J \cdot \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0)\) is a diagonal matrix containing the inverse of the joint’s stiffness.

The controller consists of:

• A pure integrator
• 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 11.

wc = 2*pi*30;
Kl = diag(1./diag(abs(freqresp(Gl, wc)))) * wc/s * 1/(1 + s/3/wc);

The controller \(\bm{K} = \bm{K}_\mathcal{L} \bm{J}\) is computed.

K = Kl*stewart.kinematics.J;

We specify the reference path to follow.

t = [0:1e-4:10];

r = zeros(6, length(t));

r(1, :) = 1e-3.*t.*sin(2*pi*t);
r(2, :) = 1e-3.*t.*cos(2*pi*t);
r(3, :) = 1e-3.*t;

references = initializeReferences(stewart, 't', t, 'r', r);

controller = initializeController('type', 'ref-track-X');

We run the simulation and we save the results.

set_param('stewart_platform_model', 'StopTime', '10')
sim('stewart_platform_model')
simout_L = simout;
save('./mat/control_tracking.mat', 'simout_L', '-append');

A diagonal controller \(\bm{K}_\mathcal{X}\) take the pose error \(\bm{\epsilon}_\mathcal{X}\) and generate cartesian forces \(\bm{\mathcal{F}}\) that are then converted to actuators forces using the Jacobian as shown in Figure e 12.

The final implemented controller is \(\bm{K} = \bm{J}^{-T} \cdot \bm{K}_\mathcal{X}\).

We now multiply the plant by the Jacobian matrix as shown in Figure 12 to obtain a more diagonal plant.

Gx = G*inv(stewart.kinematics.J');
Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

The diagonal terms are not the same. The resonances of the system are “decoupled”. For instance, the vertical resonance of the system is only present on the diagonal term corresponding to \(D_z/\mathcal{F}_z\).

Here the system is almost decoupled at all frequencies except for the transfer functions \(\frac{R_y}{\mathcal{F}_x}\) and \(\frac{R_x}{\mathcal{F}_y}\).

This is due to the fact that the compliance matrix of the Stewart platform is not diagonal.

One way to have this compliance matrix diagonal (and thus having a decoupled plant at DC) is to use a cubic architecture with the center of the cube’s coincident with frame \(\{A\}\).

This control architecture can also give a dynamically decoupled plant if the Center of mass of the payload is also coincident with frame \(\{A\}\).

The controller consists of:

• A pure integrator
• 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 14.

wc = 2*pi*30;
Kx = diag(1./diag(abs(freqresp(Gx, wc)))) * wc/s * 1/(1 + s/3/wc);

The controller \(\bm{K} = \bm{J}^{-T} \bm{K}_\mathcal{X}\) is computed.

K = inv(stewart.kinematics.J')*Kx;

We specify the reference path to follow.

t = [0:1e-4:10];

r = zeros(6, length(t));

r(1, :) = 1e-3.*t.*sin(2*pi*t);
r(2, :) = 1e-3.*t.*cos(2*pi*t);
r(3, :) = 1e-3.*t;

references = initializeReferences(stewart, 't', t, 'r', r);

controller = initializeController('type', 'ref-track-X');

We run the simulation and we save the results.

set_param('stewart_platform_model', 'StopTime', '10')
sim('stewart_platform_model')
simout_X = simout;
save('./mat/control_tracking.mat', 'simout_X', '-append');

The plant \(\bm{G}\) is pre-multiply by \(\bm{G}^{-1}(\omega = 0)\) such that the “shaped plant” \(\bm{G}_0 = \bm{G} \bm{G}^{-1}(\omega = 0)\) is diagonal at low frequency.

Then a diagonal controller \(\bm{K}_0\) is designed.

The control architecture is shown in Figure 15.

The plant is pre-multiplied by \(\bm{G}^{-1}(\omega = 0)\). The diagonal and off-diagonal elements of the shaped plant are shown in Figure 16.

G0 = G*inv(freqresp(G, 0));

We have that: \[ \bm{G}^{-1}(\omega = 0) = \left(\frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0)\right)^{-1} = \left( \bm{J}^{-1} \frac{\delta\bm{\mathcal{L}}}{\delta\bm{\tau}}(\omega = 0) \right)^{-1} = \text{diag}(\mathcal{K}_1^{-1} \ \dots \ \mathcal{K}_6^{-1}) \bm{J} \]

And because:

• all the leg stiffness are equal
• the controller equal to a \(\bm{K}_0(s) = k(s) \bm{I}_6\)

We have that \(\bm{K}_0(s)\) commutes with \(\bm{G}^{-1}(\omega = 0)\) and thus the overall controller \(\bm{K}\) is the same as the one obtain in section 2.4.

The position error \(\bm{\epsilon}_\mathcal{X}\) for both centralized architecture are shown in Figure 18.

Based on Figure 18, we can see that:

• There is some tracking error \(\epsilon_x\)
• The errors \(\epsilon_y\), \(\epsilon_{R_x}\) and \(\epsilon_{R_z}\) are quite negligible
• There is some error in the vertical position \(\epsilon_z\). The frequency of the error \(\epsilon_z\) is twice the frequency of the reference path \(r_x\).
• There is some error \(\epsilon_{R_y}\). This error is much lower when using the diagonal control in the frame of the leg instead of the cartesian frame.

Let’s identify the transfer function from \(\bm{\tau}\) to \(\bm{L}\).

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

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],        1, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Stewart Platform'],  1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]

%% Run the linearization
Gl = linearize(mdl, io);
Gl.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gl.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};

The obtained plant is shown in Figure 20.

We apply a decentralized (diagonal) direct velocity feedback. Thus, we apply a pure derivative action. In order to make the controller realizable, we add a low pass filter at high frequency. The gain of the controller is chosen such that the resonances are critically damped.

The obtain loop gain is shown in Figure 21.

Kl = 1e4 * s / (1 + s/2/pi/1e4) * eye(6);

We use the Jacobian matrix to apply forces in the cartesian frame.

Gx = G*inv(stewart.kinematics.J');
Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

The obtained plant is shown in Figure 22.

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 three times the wanted bandwidth

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

h = 3; % Lead parameter

Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc*2 + 1)/(s/wc*2)) * (1/(1 + s/wc/3));

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

Then we include the Jacobian in the controller matrix.

Kx = inv(stewart.kinematics.J')*Kx;