-
diff --git a/docs/control-study.html b/docs/control-study.html
index 82df9e9..747a52f 100644
--- a/docs/control-study.html
+++ b/docs/control-study.html
@@ -4,7 +4,7 @@
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
-
+
Created: 2020-03-03 mar. 16:03
+Created: 2020-03-11 mer. 19:00
-
-
- 3.1. Initialization -
- 3.2. Identification +
- 3.1. Initialization +
- 3.2. Identification
- 3.3. Steady State Decoupling
- 3.3.1. Pre-Compensator Design @@ -303,9 +303,17 @@
- 3.4. Decoupling at Crossover
- - 4. Functions +
- 4. Time Domain Simulation + +
- 5. Functions
+
-
+
- 5.1.
initializeController: Initialize the Controller- Function description
- Optional Parameters @@ -319,6 +327,28 @@ +
- Sensors located in each strut: relative motion, force sensor, inertial sensor +
- Sensors measuring the relative motion between the fixed base and the mobile platform +
- Inertial sensors located on the mobile platform +
- Reference Tracking +
- Active Damping +
- Vibration Isolation +
- 1. First Control Architecture +
- 1. Decentralized Control Architecture using Strut Length +
- 2. Centralized Control Architecture using Pose Measurement + + +
- 3. Hybrid Control Architecture - HAC-LAC with relative DVF + + +
- 4. Position Error computation
- If the struts length \(\bm{\mathcal{L}}\) is measured, a decentralized control architecture can be used (Section 1) +
- If the pose of the mobile platform \(\bm{\mathcal{X}}\) is directly measured, a centralized control architecture can be used (Section 2) +
- If both \(\bm{\mathcal{L}}\) and \(\bm{\mathcal{X}}\) are measured, we can use an hybrid control architecture (Section 3) +
- A pure integrator +
- A lead around the crossover frequency to increase the phase margin +
- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin +
- reference path \(\bm{r}_\mathcal{X} = \begin{bmatrix} \epsilon_x & \epsilon_y & \epsilon_z & \epsilon_{R_x} & \epsilon_{R_y} & \epsilon_{R_z} \end{bmatrix}\) +
- tracking error \(\bm{\epsilon}_\mathcal{X} = \begin{bmatrix} \epsilon_x & \epsilon_y & \epsilon_z & \epsilon_{R_x} & \epsilon_{R_y} & \epsilon_{R_z} \end{bmatrix}\) +
- actuator forces \(\bm{\tau} = \begin{bmatrix} \tau_1 & \tau_2 & \tau_3 & \tau_4 & \tau_5 & \tau_6 \end{bmatrix}\) +
- payload pose \(\bm{\mathcal{X}) = \begin{bmatrix} x & y & z & R_x & R_y & R_z \end{bmatrix}\) +
- A pure integrator +
- A lead around the crossover frequency to increase the phase margin +
- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin +
- A pure integrator +
- A lead around the crossover frequency to increase the phase margin +
- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin +
- all the leg stiffness are equal +
- the controller equal to a \(\bm{K}_0(s) = k(s) \bm{I}_6\) +
- 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. +
- \(\{W\}\) the initial fixed frame (base in which the measurement is done) +
- \(\{R\}\) the reference frame corresponding to the wanted pose of the sample +
- \(\{M\}\) the frame corresponding to the measured pose of the sample +
- \({}^W\bm{T}_R\) which corresponds to the wanted pose of the sample with respect to the granite +
- \({}^W\bm{T}_M\) which corresponds to the measured pose of the sample with respect to the granite +
- 2. Simulation Configuration - Configuration reference
- 3. Subsystem Reference
- 4. Subsystem - Fixed base and Mobile Platform -
- 5. Subsystem - Struts - - +
- 5. Subsystem - Struts
- 6. Other Elements +
- 7. Initialize Disturbances + + +
- 8. Initialize References + +
- Universal Joint* connected on the Fixed base -
- Prismatic Joint* for the actuator -
- Spherical Joint* connected on the Mobile platform +
- Universal Joint connected on the Fixed base +
- Prismatic Joint for the actuator +
- Spherical Joint connected on the Mobile platform
+Control architectures can be divided in different ways. +
+ ++It can depend on the sensor used: +
+-
+
+It can also depends on the control objective: +
+-
+
1 HAC-LAC (Cascade) Control - Integral Control
@@ -355,8 +385,8 @@ First, the LAC loop is closed (the LAC control is described -1.2 Initialization
++1.2 Initialization
-We first initialize the Stewart platform. @@ -387,8 +417,8 @@ payload = initializePayload('type',
-1.3 Identification
++1.3 Identification
-We identify the transfer function from the actuator forces \(\bm{\tau}\) to the absolute displacement of the mobile platform \(\bm{\mathcal{X}}\) in three different cases: @@ -400,8 +430,8 @@ We identify the transfer function from the actuator forces \(\bm{\tau}\) to the
-1.3.1 HAC - Without LAC
++1.3.1 HAC - Without LAC
controller = initializeController('type', 'open-loop'); @@ -426,8 +456,8 @@ G_ol.OutputName = {'Dx', -
1.3.2 HAC - IFF
++1.3.2 HAC - IFF
controller = initializeController('type', 'iff'); @@ -453,8 +483,8 @@ G_iff.OutputName = {'Dx', -
1.3.3 HAC - DVF
++1.3.3 HAC - DVF
-controller = initializeController('type', 'dvf'); @@ -518,12 +548,12 @@ We then design a controller based on the transfer functions from \(\bm{\mathcal{
-1.6 HAC - DVF
++1.6 HAC - DVF
--1.6.1 Plant
++1.6.1 Plant
-@@ -534,8 +564,8 @@ We then design a controller based on the transfer functions from \(\bm{\mathcal{-1.6.2 Controller Design
++-1.6.2 Controller Design
We design a diagonal controller with equal bandwidth for the 6 terms. @@ -570,8 +600,8 @@ Finally, we pre-multiply the diagonal controller by \(\bm{J}^{-T}\) prior implem
-1.6.3 Obtained Performance
++-1.6.3 Obtained Performance
We identify the transmissibility and compliance of the system. @@ -608,12 +638,12 @@ We identify the transmissibility and compliance of the system.
-1.7 HAC - IFF
++1.7 HAC - IFF
--1.7.1 Plant
++1.7.1 Plant
-@@ -624,8 +654,8 @@ We identify the transmissibility and compliance of the system.-1.7.2 Controller Design
++-1.7.2 Controller Design
We design a diagonal controller with equal bandwidth for the 6 terms. @@ -660,8 +690,8 @@ Finally, we pre-multiply the diagonal controller by \(\bm{J}^{-T}\) prior implem
-1.7.3 Obtained Performance
++1.7.3 Obtained Performance
We identify the transmissibility and compliance of the system. @@ -750,26 +780,33 @@ Let’s define the system as shown in figure 13.
- - -Exogenous Inputs -\(\bm{\mathcal{X}}_w\) -Ground motion -\(\bm{\mathcal{F}}_d\) -External Forces applied to the Payload -- \(\bm{r}\) -Reference signal for tracking +Symbol +Meaning + + +Exogenous Inputs +\(\bm{\mathcal{X}}_w\) +Ground motion ++ + +\(\bm{\mathcal{F}}_d\) +External Forces applied to the Payload ++ + + +\(\bm{r}\) +Reference signal for tracking +Exogenous Outputs \(\bm{\mathcal{X}}\) Absolute Motion of the Payload @@ -810,8 +847,8 @@ Let’s define the system as shown in figure 13. --2.1 Initialization
++2.1 Initialization
-We first initialize the Stewart platform. @@ -842,12 +879,12 @@ payload = initializePayload('type',
-2.2 Identification
++2.2 Identification
--2.2.1 HAC - Without LAC
++2.2.1 HAC - Without LAC
controller = initializeController('type', 'open-loop'); @@ -872,8 +909,8 @@ G_ol.OutputName = {'Dx', -
2.2.2 HAC - DVF
++2.2.2 HAC - DVF
-controller = initializeController('type', 'dvf'); @@ -971,8 +1008,8 @@ There are mainly three different cases:
-3.1 Initialization
++3.1 Initialization
-We first initialize the Stewart platform. @@ -1003,8 +1040,8 @@ payload = initializePayload('type',
-3.2 Identification
++@@ -429,23 +442,17 @@ As always, the parameters that define the geometry are taken from the3.2 Identification
-controller = initializeController('type', 'dvf'); @@ -1164,14 +1201,171 @@ The results are shown in figure
-4 Functions
++4 Time Domain Simulation
--+ +4.1
+initializeController: Initialize the Controller++ +4.1 Initialization
++We first initialize the Stewart platform. +
+++ +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3); +stewart = generateGeneralConfiguration(stewart); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart); +stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical'); +stewart = initializeCylindricalPlatforms(stewart); +stewart = initializeCylindricalStruts(stewart); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeInertialSensor(stewart, 'type', 'none'); +
++The rotation point of the ground is located at the origin of frame \(\{A\}\). +
+++ +ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A); +payload = initializePayload('type', 'none'); +
+++load('./mat/motion_error_ol.mat', 'Eg') +
+++ +4.2 HAC IFF
+++++ +controller = initializeController('type', 'iff'); +K_iff = -(1e4/s)*eye(6); + +%% Name of the Simulink File +mdl = 'stewart_platform_model'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N] +io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad] + +%% Run the linearization +G_iff = linearize(mdl, io); +G_iff.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; +G_iff.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; + +Gc_iff = minreal(G_iff)/stewart.kinematics.J'; +Gc_iff.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +
+++ +wc = 2*pi*100; % Wanted Bandwidth [rad/s] + +h = 1.2; +H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h)); + +Kd_iff = diag(1./abs(diag(freqresp(1/s*Gc_iff, wc)))) .* H_lead .* 1/s; +K_hac_iff = inv(stewart.kinematics.J')*Kd_iff; +
+++controller = initializeController('type', 'hac-iff'); +
+++ +4.3 HAC-DVF
+++++ +controller = initializeController('type', 'dvf'); +K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6); + +%% Name of the Simulink File +mdl = 'stewart_platform_model'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N] +io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad] + +%% Run the linearization +G_dvf = linearize(mdl, io); +G_dvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; +G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; + +Gc_dvf = minreal(G_dvf)/stewart.kinematics.J'; +Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +
+++ +wc = 2*pi*100; % Wanted Bandwidth [rad/s] + +h = 1.2; +H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h)); + +Kd_dvf = diag(1./abs(diag(freqresp(1/s*Gc_dvf, wc)))) .* H_lead .* 1/s; + +K_hac_dvf = inv(stewart.kinematics.J')*Kd_dvf; +
+++controller = initializeController('type', 'hac-dvf'); +
+++4.4 Results
+++++figure; +subplot(1, 2, 1); +hold on; +plot(Eg.Time, Eg.Data(:, 1), 'DisplayName', 'X'); +plot(Eg.Time, Eg.Data(:, 2), 'DisplayName', 'Y'); +plot(Eg.Time, Eg.Data(:, 3), 'DisplayName', 'Z'); +hold off; +xlabel('Time [s]'); +ylabel('Position error [m]'); +legend(); + +subplot(1, 2, 2); +hold on; +plot(simout.Xa.Time, simout.Xa.Data(:, 1)); +plot(simout.Xa.Time, simout.Xa.Data(:, 2)); +plot(simout.Xa.Time, simout.Xa.Data(:, 3)); +hold off; +xlabel('Time [s]'); +ylabel('Orientation error [rad]'); +
++-5 Functions
++++5.1
+ @@ -1197,7 +1391,7 @@ The results are shown in figureinitializeController: Initialize the Controller@@ -1239,7 +1437,7 @@ The results are shown in figure@@ -1229,6 +1423,10 @@ The results are shown in figure controller.type = 3; case 'hac-dvf' controller.type = 4; + case 'ref-track-L' + controller.type = 5; + case 'ref-track-X' + controller.type = 6; endarguments - args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf'})} = 'open-loop' + args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X'})} = 'open-loop' end-diff --git a/docs/control-tracking.html b/docs/control-tracking.html index 73434e8..af26d3f 100644 --- a/docs/control-tracking.html +++ b/docs/control-tracking.html @@ -4,7 +4,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - +Created: 2020-03-02 lun. 17:57
+Created: 2020-03-11 mer. 18:59
Stewart Platform - Tracking Control @@ -246,30 +246,105 @@Table of Contents
-
-
-+ +1 First Control Architecture
++Let’s suppose the control objective is to position \(\bm{\mathcal{X}}\) of the mobile platform of the Stewart platform such that it is following some reference position \(\bm{r}_\mathcal{X}\). +
+ ++Depending of the measured quantity, different control architectures can be used: +
+-
+
++1 Decentralized Control Architecture using Strut Length
@@ -328,43 +410,956 @@ G.OutputName = {'L1', '1.1 Control Schematic
+@@ -293,8 +368,15 @@ stewart = initializeInertialSensor(stewart, 'type'+The control architecture is shown in Figure 1. +
-++The required leg length \(\bm{r}_\mathcal{L}\) is computed from the reference path \(\bm{r}_\mathcal{X}\) using the inverse kinematics. +
+ ++Then, a diagonal (decentralized) controller \(\bm{K}_\mathcal{L}\) is used such that each leg lengths stays close to its required length. +
+ + +
+
Figure 1: Decentralized control for reference tracking
-+ +ground = initializeGround('type', 'none'); +
ground = initializeGround('type', 'rigid'); payload = initializePayload('type', 'none'); +controller = initializeController('type', 'open-loop'); +
++disturbances = initializeDisturbances(); +references = initializeReferences(stewart);
1.4 Plant Analysis
+-Diagonal terms -Compare to off-diagonal terms +The diagonal terms of the plant is shown in Figure 2.
+ ++All the diagonal terms are equal. +
+ + + + ++The off-diagonal terms are shown in Figure 3. +
+ ++We see that the plant is decoupled at low frequency which indicate that decentralized control may be a good idea. +
+ + ++ +1.5 Controller Design
+-One integrator should be present in the controller. +The controller consists of:
+-
+
-A lead is added around the crossover frequency which is set to be around 500Hz. +The obtained loop gains corresponding to the diagonal elements are shown in Figure 4.
-+ + + +% wint = 2*pi*100; % Integrate until [rad] -% wlead = 2*pi*500; % Location of the lead [rad] -% hlead = 2; % Lead strengh - -% Kl = 1e6 * ... % Gain -% (s + wint)/(s) * ... % Integrator until 100Hz -% (1 + s/(wlead/hlead)/(1 + s/(wlead*hlead))); % Lead - -wc = 2*pi*100; -Kl = 1/abs(freqresp(G(1,1), wc)) * wc/s * 1/(1 + s/(3*wc)); -Kl = Kl * eye(6); +
wc = 2*pi*30; +Kl = diag(1./diag(abs(freqresp(G, wc)))) * wc/s * 1/(1 + s/3/wc);
++ +1.6 Simulation
+++++ +t = linspace(0, 10, 1000); + +r = zeros(6, length(t)); + +r(1, :) = 5e-3*sin(2*pi*t); + +references = initializeReferences(stewart, 't', t, 'r', r); +
+++ +controller = initializeController('type', 'ref-track-L'); +
+++sim('stewart_platform_model') +simout_D = simout; +
+++ +1.7 Results
+ +++1.8 Conclusion
++++Such control architecture is easy to implement and give good results. +The inverse kinematics is easy to compute. +
+ ++However, as \(\mathcal{X}\) is not directly measured, it is possible that important positioning errors are due to finite stiffness of the joints and other imperfections. +
+++ +2 Centralized Control Architecture using Pose Measurement
+ +++ +2.1 Control Schematic
++++The centralized controller takes the position error \(\bm{\epsilon}_\mathcal{X}\) as an inputs and generate actuator forces \(\bm{\tau}\) (see Figure 7). +The signals are: +
+-
+
++ +
+
+Figure 7: Centralized Controller
++Instead of designing a full MIMO controller \(K\), we first try to make the plant more diagonal by pre- or post-multiplying some constant matrix, then we design a diagonal controller. +
+ ++We can think of two ways to make the plant more diagonal that are described in sections 2.4 and 2.5. +
+ + +++ +2.2 Initialize the Stewart platform
+++++ +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3); +stewart = generateGeneralConfiguration(stewart); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart); +stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p'); +stewart = initializeCylindricalPlatforms(stewart); +stewart = initializeCylindricalStruts(stewart); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3); +
+++ +ground = initializeGround('type', 'rigid'); +payload = initializePayload('type', 'none'); +controller = initializeController('type', 'open-loop'); +
+++disturbances = initializeDisturbances(); +references = initializeReferences(stewart); +
+++ +2.3 Identification of the plant
++++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, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Relative Displacement Outputs [m] + +%% Run the linearization +G = linearize(mdl, io); +G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; +G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; +
+++ +2.4 Diagonal Control - Leg’s Frame
+ +++ +2.4.1 Control Architecture
++++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 8 +
+ ++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. +
+ + +++
+
+Figure 8: Controller in the frame of the legs
+++ +2.4.2 Plant Analysis
++++We now multiply the plant by the Jacobian matrix as shown in Figure 8 to obtain a more diagonal plant. +
+ +++ + + + +Gl = stewart.kinematics.J*G; +Gl.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; +
++All 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. +
+ ++The off-diagonal terms of the controller are shown in Figure 10. +
+ + + + ++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. +
+++ +2.4.3 Controller Design
++++The controller consists of: +
+-
+
+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; ++++2.4.4 Simulation
++++We specify the reference path to follow. +
+++ +t = linspace(0, 10, 1000); + +r = zeros(6, length(t)); + +r(1, :) = 5e-3*sin(2*pi*t); + +references = initializeReferences(stewart, 't', t, 'r', r); +
+++ +controller = initializeController('type', 'ref-track-X'); +
++We run the simulation and we save the results. +
+++sim('stewart_platform_model') +simout_L = simout; +
+++ +2.5 Diagonal Control - Cartesian Frame
+ +++ +2.5.1 Control Architecture
++++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}\). +
+ + +++
+
+Figure 12: Controller in the cartesian frame
+++ +2.5.2 Plant Analysis
++++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. +
++ + +
+ ++ + ++ + + + + + + + + + + + + +4.75e-08 +-1.9751e-24 +7.3536e-25 +5.915e-23 +3.2093e-07 +5.8696e-24 ++ + +-7.1302e-25 +4.75e-08 +2.8866e-25 +-3.2093e-07 +-5.38e-24 +-3.2725e-23 ++ + +7.9012e-26 +-6.3991e-25 +2.099e-08 +1.9073e-23 +5.3384e-25 +-6.4867e-40 ++ + +1.3724e-23 +-3.2093e-07 +1.2799e-23 +5.1863e-06 +4.9412e-22 +-3.8269e-24 ++ + +3.2093e-07 +7.6013e-24 +1.2239e-23 +6.8886e-22 +5.1863e-06 +-2.6025e-22 ++ + +7.337e-24 +-3.2395e-23 +-1.571e-39 +9.927e-23 +-3.2531e-22 +1.7073e-06 ++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\}\). +
+++ +2.5.3 Controller Design
++++The controller consists of: +
+-
+
+The obtained loop gains corresponding to the diagonal elements are shown in Figure 15. +
+ +++ + + + +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; +
+++2.5.4 Simulation
++++We specify the reference path to follow. +
+++ +t = linspace(0, 10, 1000); + +r = zeros(6, length(t)); + +r(1, :) = 5e-3*sin(2*pi*t); + +references = initializeReferences(stewart, 't', t, 'r', r); +
+++ +controller = initializeController('type', 'ref-track-X'); +
++We run the simulation and we save the results. +
+++sim('stewart_platform_model') +simout_X = simout; +
+++ +2.6 Diagonal Control - Steady State Decoupling
+ +++ +2.6.1 Control Architecture
++++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 16. +
+ + +++
+
+Figure 16: Static Decoupling of the Plant
+++ +2.6.2 Plant Analysis
++++The plant is pre-multiplied by \(\bm{G}^{-1}(\omega = 0)\). +The diagonal elements of the shaped plant are shown in Figure 17. +
+ +++ + + + + + +G0 = G*inv(freqresp(G, 0)); ++++2.6.3 Controller Design
++++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: +
+-
+
+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. +
+++ +2.7 Comparison
+++++ +2.7.1 Obtained MIMO Controllers
+ +++2.7.2 Simulation Results
++++The position error \(\bm{\epsilon}_\mathcal{X}\) for both centralized architecture are shown in Figure 20. +The corresponding leg’s length errors \(\bm{\epsilon}_\mathcal{L}\) are shown in Figure 21. +
+ ++Based on Figure 20, we can see that: +
+-
+
++2.8 Conclusion
++++Both control architecture gives similar results even tough the control in the Leg’s frame gives slightly better results. +
+ ++The main differences between the control architectures used in sections 2.4 and 2.5 are summarized in Table 1. +
+ ++
+ +Table 1: Comparison of the two centralized control architectures + ++ + ++ + + + + + + + + + +Leg’s Frame +Cartesian Frame +Static Decoupling ++ + +Plant Meaning +\(\delta\mathcal{L}_i/\tau_i\) +\(\delta\mathcal{X}_i/\mathcal{F}_i\) +No physical meaning ++ + +Obtained Decoupling +Decoupled at DC +Dynamical decoupling except few terms +Decoupled at DC ++ + +Diagonal Elements +Identical with all the Resonances +Different, resonances are cancel out +No Alternating poles and zeros ++ + +Mechanical Architecture +Architecture Independent +Better with Cubic Architecture ++ + +Robustness to Uncertainty +Good (only depends on \(J\)) +Good (only depends on \(J\)) +Bad (depends on the mass) ++These decoupling methods only uses the Jacobian matrix which only depends on the Stewart platform geometry. +Thus, this method should be quite robust against parameter variation (e.g. the payload mass). +
+++ + + +3 Hybrid Control Architecture - HAC-LAC with relative DVF
+ +++3.1 Control Schematic
++ ++++
+
+Figure 22: Centralized Controller
++4 Position Error computation
++ +-
+
+We have then computed: +
+-
+
+Reference Position with respect to fixed frame {W}: \({}^WT_R\) +
+++ +Dx = 0; +Dy = 0; +Dz = 0.1; +Rx = pi; +Ry = 0; +Rz = 0; + +WTr = zeros(4,4); + +R = [cos(Rz) -sin(Rz) 0; + sin(Rz) cos(Rz) 0; + 0 0 1] * ... + [cos(Ry) 0 sin(Ry); + 0 1 0; + -sin(Ry) 0 cos(Ry)] * ... + [1 0 0; + 0 cos(Rx) -sin(Rx); + 0 sin(Rx) cos(Rx)]; + + +WTr(1:3, 1:3) = R; +WTr(1:4, 4) = [Dx ; Dy ; Dz; 1]; +
++Measured Position with respect to fixed frame {W}: \({}^WT_M\) +
+++ +Dx = 0; +Dy = 0; +Dz = 0; +Rx = pi; +Ry = 0; +Rz = 0; + +WTm = zeros(4,4); + +R = [cos(Rz) -sin(Rz) 0; + sin(Rz) cos(Rz) 0; + 0 0 1] * ... + [cos(Ry) 0 sin(Ry); + 0 1 0; + -sin(Ry) 0 cos(Ry)] * ... + [1 0 0; + 0 cos(Rx) -sin(Rx); + 0 sin(Rx) cos(Rx)]; + + +WTm(1:3, 1:3) = R; +WTm(1:4, 4) = [Dx ; Dy ; Dz; 1]; +
++We would like to compute \({}^M\bm{T}_R\) which corresponds to the wanted pose of the sample expressed in a frame attached to the top platform of the nano-hexapod (frame \(\{M\}\)). +
+ ++We have: +
+\begin{align} + {}^M\bm{T}_R &= {}^M\bm{T}_W \cdot {}^W\bm{T}_R \\ + &= {}^W{\bm{T}_M}^{-1} \cdot {}^W\bm{T}_R +\end{align} + ++% Error with respect to the top platform +MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr; + +T = MTr; +Edx = T(1, 4); +Edy = T(2, 4); +Edz = T(3, 4); + +% The angles obtained are u-v-w Euler angles (rotations in the moving frame) +Ery = atan2( T(1, 3), sqrt(T(1, 1)^2 + T(1, 2)^2)); +Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery)); +Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery)); + +[Edx, Edy, Edz, Erx, Ery, Erz] +
-diff --git a/docs/simscape-model.html b/docs/simscape-model.html index 8ece79e..f610d31 100644 --- a/docs/simscape-model.html +++ b/docs/simscape-model.html @@ -4,7 +4,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - +Created: 2020-03-02 lun. 17:57
+Created: 2020-03-11 mer. 19:00
Stewart Platform - Simscape Model @@ -250,25 +250,21 @@stew ---5.1 Strut Configuration
--For the Stewart platform, the 6 struts are identical. Thus, all the struts used in the Stewart platform are referring to the same subsystem called
stewart_strut.slxand shown in Figure 4.-This strut as the following structure: +This strut has the following structure:
-
-
@@ -486,7 +493,6 @@ Both inertial sensors are described bellow.
-6 Other Elements
@@ -504,9 +510,9 @@ This Matlab function is accessible here-Function description
-++Function description
+-function [payload] = initializePayload(args) % initializePayload - Initialize the Payload that can then be used for simulations and analysis @@ -536,9 +542,9 @@ This Matlab function is accessible here
-Optional Parameters
-++Optional Parameters
+-arguments args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'cartesian'})} = 'none' @@ -600,9 +606,9 @@ This Matlab function is accessible here.-Function description
-++Function description
+-function [ground] = initializeGround(args) % initializeGround - Initialize the Ground that can then be used for simulations and analysis @@ -626,9 +632,9 @@ This Matlab function is accessible here.
-Optional Parameters
-+++Optional Parameters
+arguments args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'none' @@ -680,10 +686,157 @@ ground.C = args.C;++ +7 Initialize Disturbances
+ + +++ +Function Declaration and Documentation
+++++function [disturbances] = initializeDisturbances(args) +% initializeDisturbances - Initialize the disturbances +% +% Syntax: [disturbances] = initializeDisturbances(args) +% +% Inputs: +% - args - + +
+++ + +Optional Parameters
+++++arguments + args.Fd double {mustBeNumeric, mustBeReal} = zeros(6,1) + args.Fd_t double {mustBeNumeric, mustBeReal} = 0 + args.Dw double {mustBeNumeric, mustBeReal} = zeros(6,1) + args.Dw_t double {mustBeNumeric, mustBeReal} = 0 +end ++++ +Structure initialization
+++++disturbances = struct(); +
+++ +Ground Motion
+++++disturbances.Dw = timeseries([args.Dw], args.Dw_t); +
+++Direct Forces
+++++disturbances.Fd = timeseries([args.Fd], args.Fd_t); +
++8 Initialize References
+ + +++ +Function Declaration and Documentation
+++++function [references] = initializeReferences(stewart, args) +% initializeReferences - Initialize the references +% +% Syntax: [references] = initializeReferences(args) +% +% Inputs: +% - args - + +
+++ +Optional Parameters
+++++arguments + stewart + args.t double {mustBeNumeric, mustBeReal} = 0 + args.r double {mustBeNumeric, mustBeReal} = zeros(6, 1) +end ++++ +8.1 Compute the corresponding strut length
+++++rL = zeros(6, length(args.t)); + +for i = 1:length(args.t) + R = [cos(args.r(6,i)) -sin(args.r(6,i)) 0; + sin(args.r(6,i)) cos(args.r(6,i)) 0; + 0 0 1] * ... + [cos(args.r(5,i)) 0 sin(args.r(5,i)); + 0 1 0; + -sin(args.r(5,i)) 0 cos(args.r(5,i))] * ... + [1 0 0; + 0 cos(args.r(4,i)) -sin(args.r(4,i)); + 0 sin(args.r(4,i)) cos(args.r(4,i))]; + + [Li, dLi] = inverseKinematics(stewart, 'AP', [args.r(1,i); args.r(2,i); args.r(3,i)], 'ARB', R); + rL(:, i) = dLi; +end +
+++References
+++++references.r = timeseries(args.r, args.t); +references.rL = timeseries(rL, args.t); +
+-diff --git a/org/control-tracking.org b/org/control-tracking.org index 2fbc519..81b4b4b 100644 --- a/org/control-tracking.org +++ b/org/control-tracking.org @@ -989,7 +989,7 @@ The control architecture is shown in Figure [[fig:centralized_reference_tracking *** Plant Analysis The plant is pre-multiplied by $\bm{G}^{-1}(\omega = 0)$. -The diagonal elements of the shaped plant are shown in Figure [[fig:centralized_reference_tracking_SD]]. +The diagonal elements of the shaped plant are shown in Figure [[fig:plant_centralized_diagonal_SD]]. #+begin_src matlab G0 = G*inv(freqresp(G, 0));Created: 2020-03-02 lun. 17:57
+Created: 2020-03-11 mer. 18:59
- 5.1.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
