stewart-simscape/kinematic-study.org

13 KiB

Kinematic Study of the Stewart Platform

Needed Actuator Stroke

The goal is to determine the needed stroke of the actuators to obtain wanted translations and rotations.

Stewart architecture definition

We use a cubic architecture.

  opts = struct(...
      'H_tot', 90, ... % Total height of the Hexapod [mm]
      'L',     200/sqrt(3), ... % Size of the Cube [mm]
      'H',     60, ... % Height between base joints and platform joints [mm]
      'H0',    200/2-60/2 ... % Height between the corner of the cube and the plane containing the base joints [mm]
      );
  stewart = initializeCubicConfiguration(opts);
  opts = struct(...
      'Jd_pos', [0, 0, 100], ... % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]
      'Jf_pos', [0, 0, -50]  ... % Position of the Jacobian for force location from the top of the mobile platform [mm]
      );
  stewart = computeGeometricalProperties(stewart, opts);
  opts = struct(...
      'stroke', 50e-6 ... % Maximum stroke of each actuator [m]
      );
  stewart = initializeMechanicalElements(stewart, opts);

  save('./mat/stewart.mat', 'stewart');

Wanted translations and rotations

We define wanted translations and rotations

  Tx_max = 15e-6; % Translation [m]
  Ty_max = 15e-6; % Translation [m]
  Tz_max = 15e-6; % Translation [m]
  Rx_max = 30e-6; % Rotation [rad]
  Ry_max = 30e-6; % Rotation [rad]

Needed stroke for "pure" rotations or translations

First, we estimate the needed actuator stroke for "pure" rotations and translation.

  LTx = stewart.Jd*[Tx_max 0 0 0 0 0]';
  LTy = stewart.Jd*[0 Ty_max 0 0 0 0]';
  LTz = stewart.Jd*[0 0 Tz_max 0 0 0]';
  LRx = stewart.Jd*[0 0 0 Rx_max 0 0]';
  LRy = stewart.Jd*[0 0 0 0 Ry_max 0]';
From -1.2e-05[m] to 1.1e-05[m]: Total stroke = 22.9[um]

Needed stroke for combined translations and rotations

Now, we combine translations and rotations, and we try to find the worst case (that we suppose to happen at the border).

  Lmax = 0;
  Lmin = 0;
  pos = [0, 0, 0, 0, 0];
  for Tx = [-Tx_max,Tx_max]
  for Ty = [-Ty_max,Ty_max]
  for Tz = [-Tz_max,Tz_max]
  for Rx = [-Rx_max,Rx_max]
  for Ry = [-Ry_max,Ry_max]
      lmax = max(stewart.Jd*[Tx Ty Tz Rx Ry 0]');
      lmin = min(stewart.Jd*[Tx Ty Tz Rx Ry 0]');
      if lmax > Lmax
          Lmax = lmax;
          pos = [Tx Ty Tz Rx Ry];
      end
      if lmin < Lmin
          Lmin = lmin;
      end
  end
  end
  end
  end
  end

We obtain a needed stroke shown below (almost two times the needed stroke for "pure" rotations and translations).

From -3.1e-05[m] to 3.1e-05[m]: Total stroke = 61.5[um]

Maximum Stroke

From a specified actuator stroke, we try to estimate the available maneuverability of the Stewart platform.

  [X, Y, Z] = getMaxPositions(stewart);
  figure;
  plot3(X, Y, Z, 'k-')

Functions

getMaxPositions

  function [X, Y, Z] = getMaxPositions(stewart)
      Leg = stewart.Leg;
      J = stewart.Jd;
      theta = linspace(0, 2*pi, 100);
      phi = linspace(-pi/2 , pi/2, 100);
      dmax = zeros(length(theta), length(phi));

      for i = 1:length(theta)
          for j = 1:length(phi)
              L = J*[cos(phi(j))*cos(theta(i)) cos(phi(j))*sin(theta(i)) sin(phi(j)) 0 0 0]';
              dmax(i, j) = Leg.stroke/max(abs(L));
          end
      end

      X = dmax.*cos(repmat(phi,length(theta),1)).*cos(repmat(theta,length(phi),1))';
      Y = dmax.*cos(repmat(phi,length(theta),1)).*sin(repmat(theta,length(phi),1))';
      Z = dmax.*sin(repmat(phi,length(theta),1));
  end

getMaxPureDisplacement

  function [max_disp] = getMaxPureDisplacement(Leg, J)
      max_disp = zeros(6, 1);
      max_disp(1) = Leg.stroke/max(abs(J*[1 0 0 0 0 0]'));
      max_disp(2) = Leg.stroke/max(abs(J*[0 1 0 0 0 0]'));
      max_disp(3) = Leg.stroke/max(abs(J*[0 0 1 0 0 0]'));
      max_disp(4) = Leg.stroke/max(abs(J*[0 0 0 1 0 0]'));
      max_disp(5) = Leg.stroke/max(abs(J*[0 0 0 0 1 0]'));
      max_disp(6) = Leg.stroke/max(abs(J*[0 0 0 0 0 1]'));
  end

computeJacobian: Compute the Jacobian Matrix

<<sec:computeJacobian>>

This Matlab function is accessible here.

Function description

  function [stewart] = computeJacobian(stewart)
  % computeJacobian -
  %
  % Syntax: [stewart] = computeJacobian(stewart)
  %
  % Inputs:
  %    - stewart - With at least the following fields:
  %        - As [3x6] - The 6 unit vectors for each strut expressed in {A}
  %        - Ab [3x6] - The 6 position of the joints bi expressed in {A}
  %
  % Outputs:
  %    - stewart - With the 3 added field:
  %        - J [6x6] - The Jacobian Matrix
  %        - K [6x6] - The Stiffness Matrix
  %        - C [6x6] - The Compliance Matrix

Compute Jacobian Matrix

  stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];

Compute Stiffness Matrix

  stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;

Compute Compliance Matrix

  stewart.C = inv(stewart.K);

inverseKinematics: Compute Inverse Kinematics

<<sec:inverseKinematics>>

This Matlab function is accessible here.

Function description

  function [Li, dLi] = inverseKinematics(stewart, args)
  % inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
  %
  % Syntax: [stewart] = inverseKinematics(stewart)
  %
  % Inputs:
  %    - stewart - A structure with the following fields
  %        - Aa   [3x6] - The positions ai expressed in {A}
  %        - Bb   [3x6] - The positions bi expressed in {B}
  %    - args - Can have the following fields:
  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
  %
  % Outputs:
  %    - Li   [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
  %    - dLi  [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}

Optional Parameters

  arguments
      stewart
      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
      args.ARB (3,3) double {mustBeNumeric} = eye(3)
  end

Theory

For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.

From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as

\begin{align*} l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\ &= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i \end{align*}

To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:

\begin{equation} l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right] \end{equation}

Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:

\begin{equation} l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i} \end{equation}

If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation. Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.

Compute

  Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa));
  dLi = Li-stewart.l;

forwardKinematicsApprox: Compute the Approximate Forward Kinematics

<<sec:forwardKinematicsApprox>>

This Matlab function is accessible here.

Function description

  function [P, R] = forwardKinematicsApprox(stewart, args)
  % forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
  %                           the Jacobian Matrix
  %
  % Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
  %
  % Inputs:
  %    - stewart - A structure with the following fields
  %        - J  [6x6] - The Jacobian Matrix
  %    - args - Can have the following fields:
  %        - dL [6x1] - Displacement of each strut [m]
  %
  % Outputs:
  %    - P  [3x1] - The estimated position of {B} with respect to {A}
  %    - R  [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}

Optional Parameters

  arguments
      stewart
      args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
  end

Computation

From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the position and orientation of {B} with respect to {A} using the following formula: \[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]

  X = stewart.J\args.dL;

The position vector corresponds to the first 3 elements.

  P = X(1:3);

The next 3 elements are the orientation of {B} with respect to {A} expressed using the screw axis.

  theta = norm(X(4:6));
  s = X(4:6)/theta;

We then compute the corresponding rotation matrix.

  R = [s(1)^2*(1-cos(theta)) + cos(theta) ,        s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
       s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta),         s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
       s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];