1.2 Effect of the Flexible Joint stiffness and Actuator amplification on the Dynamics
+1.2 Effect of the Flexible Joint stiffness and Actuator amplification on the Dynamics
We add some stiffness and damping in the flexible joints and we re-identify the dynamics. @@ -403,8 +403,8 @@ The new dynamics from force actuator to force sensor is shown in Figure
1.3 Obtained Damping
+1.3 Obtained Damping
The control is a performed in a decentralized manner. @@ -429,8 +429,8 @@ The root locus is shown in figure 3.
1.4 Conclusion
+1.4 Conclusion
@@ -461,8 +461,8 @@ To run the script, open the Simulink Project, and type run active_damping_
2.1 Identification of the Dynamics with perfect Joints
+2.1 Identification of the Dynamics with perfect Joints
We first initialize the Stewart platform without joint stiffness. @@ -520,8 +520,8 @@ The transfer function from actuator forces to force sensors is shown in Figure <
2.2 Effect of the Flexible Joint stiffness and Actuator amplification on the Dynamics
+2.2 Effect of the Flexible Joint stiffness and Actuator amplification on the Dynamics
We add some stiffness and damping in the flexible joints and we re-identify the dynamics. @@ -557,8 +557,8 @@ The new dynamics from force actuator to force sensor is shown in Figure
2.3 Obtained Damping
+2.3 Obtained Damping
The control is a performed in a decentralized manner. @@ -590,8 +590,8 @@ The root locus is shown in figure 6 and the obtained p
2.4 Conclusion
+2.4 Conclusion
@@ -623,8 +623,8 @@ To run the script, open the Simulink Project, and type run active_damping_
3.1 Identification of the Dynamics with perfect Joints
+3.1 Identification of the Dynamics with perfect Joints
We first initialize the Stewart platform without joint stiffness. @@ -687,8 +687,8 @@ The transfer function from actuator forces to relative motion sensors is shown i
3.2 Effect of the Flexible Joint stiffness and Actuator amplification on the Dynamics
+3.2 Effect of the Flexible Joint stiffness and Actuator amplification on the Dynamics
We add some stiffness and damping in the flexible joints and we re-identify the dynamics. @@ -724,8 +724,8 @@ The new dynamics from force actuator to relative motion sensor is shown in Figur
3.3 Obtained Damping
+3.3 Obtained Damping
The control is a performed in a decentralized manner. @@ -750,8 +750,8 @@ The root locus is shown in figure 10.
3.4 Conclusion
+3.4 Conclusion
@@ -762,6 +762,7 @@ Joint stiffness does increase the resonance frequencies of the system but does n
4 Compliance and Transmissibility Comparison
Created: 2020-03-02 lun. 17:57
+Created: 2020-03-12 jeu. 18:06
1.2 Initialization
+1.2 Initialization
We first initialize the Stewart platform. @@ -417,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: @@ -430,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'); @@ -456,8 +456,8 @@ G_ol.OutputName = {'Dx', -1.3.2 HAC - IFF
++1.3.2 HAC - IFF
controller = initializeController('type', 'iff'); @@ -483,8 +483,8 @@ G_iff.OutputName = {'Dx', -1.3.3 HAC - DVF
++1.3.3 HAC - DVF
-controller = initializeController('type', 'dvf'); @@ -548,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
-@@ -564,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. @@ -600,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. @@ -638,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
-@@ -654,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. @@ -690,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. @@ -847,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. @@ -879,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'); @@ -909,8 +909,8 @@ G_ol.OutputName = {'Dx', -2.2.2 HAC - DVF
++2.2.2 HAC - DVF
-controller = initializeController('type', 'dvf'); @@ -1008,8 +1008,8 @@ There are mainly three different cases:-3.1 Initialization
++3.1 Initialization
-We first initialize the Stewart platform. @@ -1040,8 +1040,8 @@ payload = initializePayload('type',
-3.2 Identification
++3.2 Identification
-controller = initializeController('type', 'dvf'); @@ -1156,8 +1156,8 @@ The overall controller is then \(K(s) = W_1 K_s(s)\) as shown in Figure-3.3.3 Results
++3.3.3 Results
We identify the transmissibility and compliance of the Stewart platform under open-loop and closed-loop control. @@ -1205,8 +1205,8 @@ The results are shown in figure
4 Time Domain Simulation
--4.1 Initialization
++4.1 Initialization
-We first initialize the Stewart platform. @@ -1329,8 +1329,8 @@ K_hac_dvf = inv(stewart.kinematics.J')
-4.4 Results
++4.4 Results
-figure; @@ -1391,7 +1391,7 @@ ylabel('Orientation error [rad]');@@ -1437,7 +1439,7 @@ ylabel('Orientation error [rad]');@@ -1427,6 +1427,8 @@ ylabel('Orientation error [rad]'); controller.type = 5; case 'ref-track-X' controller.type = 6; + case 'ref-track-hac-dvf' + controller.type = 7; endarguments - args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X'})} = 'open-loop' + args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X', 'ref-track-hac-dvf'})} = 'open-loop' end-diff --git a/docs/control-tracking.html b/docs/control-tracking.html index 1b1b89c..4ea878b 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-11 mer. 18:59
+Created: 2020-03-12 jeu. 18:06
Stewart Platform - Tracking Control @@ -248,42 +248,42 @@
- 1. Decentralized Control Architecture using Strut Length
-
- 1.1. Control Schematic
-- 1.2. Initialize the Stewart platform
-- 1.3. Identification of the plant
-- 1.4. Plant Analysis
-- 1.5. Controller Design
-- 1.6. Simulation
+- 1.1. Control Schematic
+- 1.2. Initialize the Stewart platform
+- 1.3. Identification of the plant
+- 1.4. Plant Analysis
+- 1.5. Controller Design
+- 1.6. Simulation
- 1.7. Results
-- 1.8. Conclusion
+- 1.8. Conclusion
- 2. Centralized Control Architecture using Pose Measurement
--
- 2.1. Control Schematic
-- 2.2. Initialize the Stewart platform
-- 2.3. Identification of the plant
+- 2.1. Control Schematic
+- 2.2. Initialize the Stewart platform
+- 2.3. Identification of the plant
- 2.4. Diagonal Control - Leg’s Frame
- 2.5. Diagonal Control - Cartesian Frame
- 2.6. Diagonal Control - Steady State Decoupling
- 2.7. Comparison @@ -292,12 +292,29 @@
- 2.7.2. Simulation Results
- 2.8. Conclusion
+- 2.8. Conclusion
3. Hybrid Control Architecture - HAC-LAC with relative DVF 4. Position Error computation @@ -325,8 +342,8 @@ Depending of the measured quantity, different control architectures can be used:-1.1 Control Schematic
++-1.1 Control Schematic
The control architecture is shown in Figure 1. @@ -349,8 +366,8 @@ Then, a diagonal (decentralized) controller \(\bm{K}_\mathcal{L}\) is used such
-1.2 Initialize the Stewart platform
++1.2 Initialize the Stewart platform
-stewart = initializeStewartPlatform(); @@ -382,11 +399,11 @@ references = initializeReferences(stewart);-1.3 Identification of the plant
++1.3 Identification of the plant
--Let’s identify the transfer function from \(\bm{\tau}\) to \(\bm{L}\). +Let’s identify the transfer function from \(\bm{\tau}\) to \(\bm{\mathcal{L}}\).
%% Name of the Simulink File @@ -406,8 +423,8 @@ G.OutputName = {'L1', '-1.4 Plant Analysis
++-1.4 Plant Analysis
The diagonal terms of the plant is shown in Figure 2. @@ -441,15 +458,14 @@ We see that the plant is decoupled at low frequency which indicate that decentra
-1.5 Controller Design
++1.5 Controller Design
-The controller consists of:
@@ -472,8 +488,8 @@ Kl = diag(1./diag(abs(freqresp(G, wc))))
- 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
-1.6 Simulation
++1.6 Simulation
-t = linspace(0, 10, 1000); @@ -518,8 +534,8 @@ simout_D = simout;-1.8 Conclusion
++1.8 Conclusion
-Such control architecture is easy to implement and give good results. @@ -540,8 +556,8 @@ However, as \(\mathcal{X}\) is not directly measured, it is possible that import
-2.1 Control Schematic
++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). @@ -579,8 +595,8 @@ It is indeed a more complex computation explained in section -
2.2 Initialize the Stewart platform
++2.2 Initialize the Stewart platform
-stewart = initializeStewartPlatform(); @@ -612,11 +628,11 @@ references = initializeReferences(stewart);-2.3 Identification of the plant
++2.3 Identification of the plant
-Let’s identify the transfer function from \(\bm{\tau}\) to \(\bm{L}\). +Let’s identify the transfer function from \(\bm{\tau}\) to \(\bm{\mathcal{X}}\).
--2.4.1 Control Architecture
++-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. @@ -665,8 +681,8 @@ Note here that the transformation from the pose error \(\bm{\epsilon}_\mathcal{X
-2.4.2 Plant Analysis
++-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. @@ -716,15 +732,14 @@ Thus \(J \cdot G(\omega = 0) = J \cdot \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\
-2.4.3 Controller Design
++-2.4.3 Controller Design
The controller consists of:
@@ -755,8 +770,8 @@ The controller \(\bm{K} = \bm{K}_\mathcal{L} \bm{J}\) is computed.
- 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
-2.4.4 Simulation
++2.4.4 Simulation
--2.5.1 Control Architecture
++-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. @@ -816,8 +831,8 @@ The final implemented controller is \(\bm{K} = \bm{J}^{-T} \cdot \bm{K}_\mathcal
-2.5.2 Plant Analysis
++-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. @@ -939,15 +954,14 @@ This control architecture can also give a dynamically decoupled plant if the Cen
-2.5.3 Controller Design
++-2.5.3 Controller Design
The controller consists of:
@@ -978,8 +992,8 @@ The controller \(\bm{K} = \bm{J}^{-T} \bm{K}_\mathcal{X}\) is computed.
- 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
-2.5.4 Simulation
++2.5.4 Simulation
--2.6.1 Control Architecture
++-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. @@ -1043,8 +1057,8 @@ The control architecture is shown in Figure 16.
-2.6.2 Plant Analysis
++2.6.2 Plant Analysis
The plant is pre-multiplied by \(\bm{G}^{-1}(\omega = 0)\). @@ -1072,8 +1086,8 @@ The diagonal elements of the shaped plant are shown in Figure -
2.6.3 Controller Design
++-2.6.3 Controller Design
We have that: @@ -1147,8 +1161,8 @@ This error is much lower when using the diagonal control in the frame of the leg
-2.8 Conclusion
++-2.8 Conclusion
-Both control architecture gives similar results even tough the control in the Leg’s frame gives slightly better results. @@ -1231,68 +1245,332 @@ Thus, this method should be quite robust against parameter variation (e.g. the p
-3.1 Control Schematic
+++ +3.1 Control Schematic
+++Let’s consider the control schematic shown in Figure 22. +
+ ++The first loop containing \(\bm{K}_\mathcal{L}\) is a Decentralized Direct (Relative) Velocity Feedback. +
+ ++A reference \(\bm{r}_\mathcal{L}\) is computed using the inverse kinematics and corresponds to the wanted motion of each leg. +The actual length of each leg \(\bm{\mathcal{L}}\) is subtracted and then passed trough the controller \(\bm{K}_\mathcal{L}\). +
+ ++The controller is a diagonal controller with pure derivative action on the diagonal. +
+ ++The effect of this loop is: +
++
+ +- it adds damping to the system (the force applied in each actuator is proportional to the relative velocity of the strut)
+- it however does not go “against” the reference path \(\bm{r}_\mathcal{X}\) thanks to the use of the inverse kinematics
++Then, the second loop containing \(\bm{K}_\mathcal{X}\) is designed based on the already damped plant (represented by the gray area). +This second loop is responsible for the reference tracking. +
++-
Figure 22: Centralized Controller
+Figure 22: Hybrid Control Architecture
+++ +3.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); ++++3.3 First Control Loop - \(\bm{K}_\mathcal{L}\)
+++++ +3.3.1 Identification
++++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'}; ++++ +3.3.2 Obtained Plant
+ ++3.3.3 Controller Design
+++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 25. +
+ +++ + +Kl = 1e4 * s / (1 + s/2/pi/1e4) * eye(6); ++++ +3.4 Second Control Loop - \(\bm{K}_\mathcal{X}\)
+++++ +3.4.1 Identification
+++++ +Kx = tf(zeros(6)); +controller = initializeController('type', 'ref-track-hac-dvf'); ++++%% 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, '/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'}; ++++ +3.4.2 Obtained Plant
++++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 26. +
+ + +++3.4.3 Controller Design
++++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
+++ + + + +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)); + +% 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; ++++ +3.5 Simulations
++++We specify the reference path to follow. +
+++ +t = linspace(0, 10, 10000); + +r = zeros(6, length(t)); + +r(1, :) = 5e-3*sin(2*pi*t); + +references = initializeReferences(stewart, 't', t, 'r', r); +++We run the simulation and we save the results. +
+++ +sim('stewart_platform_model') +simout_H = simout; +++The obtained position error is shown in Figure 28. +
+ + + +++3.6 Conclusion
+4 Position Error computation
-@@ -1359,7 +1677,7 @@ Erz = atan2(-T(1, 2)/-
- \(\{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
--We have then computed: +Let’s note:
-
- \({}^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
+- \(\{W\}\) the fixed measurement frame (corresponding to the metrology frame / the frame where the wanted displacement are expressed). +The center of the frame if \(O_W\)
+- \(\{M\}\) is the frame fixed to the measured elements. +\(O_M\) is the point where the pose of the element is measured
+- \(\{R\}\) is a virtual frame corresponding to the wanted pose of the element. +\(O_R\) is the origin of this frame where the we want to position the point \(O_M\) of the element.
+- \(\{V\}\) is a frame which its axes are aligned with \(\{W\}\) and its origin \(O_V\) is coincident with the \(O_M\)
Reference Position with respect to fixed frame {W}: \({}^WT_R\)
-@@ -1300,47 +1578,89 @@ WTr(1:4, 4) = [Dx ; Dy ; Dz; 1]; Measured Position with respect to fixed frame {W}: \({}^WT_M\)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]; +Dxr = 0; +Dyr = 0; +Dzr = 0.1; +Rxr = pi; +Ryr = 0; +Rzr = 0;-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]; +Dxm = 0; +Dym = 0; +Dzm = 0; +Rxm = pi; +Rym = 0; +Rzm = 0;-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 measure the position and orientation (pose) of the element represented by the frame \(\{M\}\) with respect to frame \(\{W\}\). +Thus we can compute the Homogeneous transformation matrix \({}^WT_M\).
++%% Measured Pose +WTm = zeros(4,4); + +WTm(1:3, 1:3) = [cos(Rzm) -sin(Rzm) 0; + sin(Rzm) cos(Rzm) 0; + 0 0 1] * ... + [cos(Rym) 0 sin(Rym); + 0 1 0; + -sin(Rym) 0 cos(Rym)] * ... + [1 0 0; + 0 cos(Rxm) -sin(Rxm); + 0 sin(Rxm) cos(Rxm)]; +WTm(1:4, 4) = [Dxm ; Dym ; Dzm; 1]; ++-We have: +We can also compute the Homogeneous transformation matrix \({}^WT_R\) corresponding to the transformation required to go from fixed frame \(\{W\}\) to the wanted frame \(\{R\}\). +
+++ +%% Reference Pose +WTr = zeros(4,4); + +WTr(1:3, 1:3) = [cos(Rzr) -sin(Rzr) 0; + sin(Rzr) cos(Rzr) 0; + 0 0 1] * ... + [cos(Ryr) 0 sin(Ryr); + 0 1 0; + -sin(Ryr) 0 cos(Ryr)] * ... + [1 0 0; + 0 cos(Rxr) -sin(Rxr); + 0 sin(Rxr) cos(Rxr)]; +WTr(1:4, 4) = [Dxr ; Dyr ; Dzr; 1]; +++We can also compute \({}^WT_V\). +
+++ +WTv = eye(4); +WTv(1:3, 4) = WTm(1:3, 4); +++Now we want to express \({}^MT_R\) which corresponds to the transformation required to go to wanted position expressed in the frame of the measured element. +This homogeneous transformation can be computed from the previously computed matrices: +\[ {}^MT_R = ({{}^WT_M}^{-1}) {}^WT_R \]
-\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 +%% Wanted pose expressed in a frame corresponding to the actual measured pose MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr; +++Now we want to express \({}^VT_R\): +\[ {}^VT_R = ({{}^WT_V}^{-1}) {}^WT_R \] +
+++ +%% Wanted pose expressed in a frame coincident with the actual position but with no rotation +VTr = [WTv(1:3,1:3)', -WTv(1:3,1:3)'*WTv(1:3,4) ; 0 0 0 1] * WTr; +++%% Extract Translations and Rotations from the Homogeneous Matrix T = MTr; Edx = T(1, 4); Edy = T(2, 4); @@ -1350,8 +1670,6 @@ Edz = T(3, 4); 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/cubic-configuration.html b/docs/cubic-configuration.html index ab70d41..bed01ba 100644 --- a/docs/cubic-configuration.html +++ b/docs/cubic-configuration.html @@ -4,7 +4,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - +Created: 2020-03-11 mer. 19:01
+Created: 2020-03-12 jeu. 18:06
Cubic configuration for the Stewart Platform @@ -252,33 +252,34 @@1.2. Cubic Stewart platform centered with the cube center - Jacobian not estimated at the cube center 1.3. Cubic Stewart platform not centered with the cube center - Jacobian estimated at the cube center 1.4. Cubic Stewart platform not centered with the cube center - Jacobian estimated at the Stewart platform center -1.5. Conclusion +1.5. Conclusion 2. Configuration with the Cube’s center above the mobile platform 3. Cubic size analysis 4. Dynamic Coupling in the Cartesian Frame 5. Dynamic Coupling between actuators and sensors of each strut 6. Functions @@ -826,8 +827,8 @@ stewart = initializeCylindricalPlatforms(stewart, 'Fpr' -1.5 Conclusion
++1.5 Conclusion
-@@ -1164,9 +1165,100 @@ FOc = H + MO_B; % Cente
-2.2 Conclusion
+++ +2.2 Size of the platforms
+++The minimum size of the platforms depends on the cube’s size and the height between the platform and the cube’s center. +
+ ++Let’s denote: +
++
+ +- \(H\) the height between the cube’s center and the considered platform
+- \(D\) the size of the cube’s edges
++Let’s denote by \(a\) and \(b\) the points of both ends of one of the cube’s edge. +
+ ++Initially, we have: +
+\begin{align} + a &= \frac{D}{2} \begin{bmatrix}-1 \\ -1 \\ 1\end{bmatrix} \\ + b &= \frac{D}{2} \begin{bmatrix} 1 \\ -1 \\ 1\end{bmatrix} +\end{align} + ++We rotate the cube around its center (origin of the rotated frame) such that one of its diagonal is vertical. +\[ R = \begin{bmatrix} + \frac{2}{\sqrt{6}} & 0 & \frac{1}{\sqrt{3}} \\ + \frac{-1}{\sqrt{6}} & \frac{1}{\sqrt{2}} & \frac{1}{\sqrt{3}} \\ + \frac{-1}{\sqrt{6}} & \frac{-1}{\sqrt{2}} & \frac{1}{\sqrt{3}} +\end{bmatrix} \] +
+ ++After rotation, the points \(a\) and \(b\) become: +
+\begin{align} + a &= \frac{D}{2} \begin{bmatrix}-\frac{\sqrt{2}}{\sqrt{3}} \\ -\sqrt{2} \\ -\frac{1}{\sqrt{3}}\end{bmatrix} \\ + b &= \frac{D}{2} \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ -\sqrt{2} \\ \frac{1}{\sqrt{3}}\end{bmatrix} +\end{align} + ++Points \(a\) and \(b\) define a vector \(u = b - a\) that gives the orientation of one of the Stewart platform strut: +\[ u = \frac{D}{\sqrt{3}} \begin{bmatrix} -\sqrt{2} \\ 0 \\ -1\end{bmatrix} \] +
+ ++Then we want to find the intersection between the line that defines the strut with the plane defined by the height \(H\) from the cube’s center. +To do so, we first find \(g\) such that: +\[ a_z + g u_z = -H \] +We obtain: +
+\begin{align} + g &= - \frac{H + a_z}{u_z} \\ + &= \sqrt{3} \frac{H}{D} - \frac{1}{2} +\end{align} + ++Then, the intersection point \(P\) is given by: +
+\begin{align} + P &= a + g u \\ + &= \begin{bmatrix} + H \sqrt{2} \\ + D \frac{1}{\sqrt{2}} \\ + H +\end{bmatrix} +\end{align} + ++Finally, the circle can contains the intersection point has a radius \(r\): +
+\begin{align} + r &= \sqrt{P_x^2 + P_y^2} \\ + &= \sqrt{2 H^2 + \frac{1}{2}D^2} +\end{align} + ++By symmetry, we can show that all the other intersection points will also be on the circle with a radius \(r\). +
+ ++For a small cube: +\[ r \approx \sqrt{2} H \] +
++2.3 Conclusion
+-We found that we can have a diagonal stiffness matrix using the cubic architecture when \(\{A\}\) and \(\{B\}\) are located above the top platform. @@ -1283,8 +1375,8 @@ We also find that \(k_{\theta_x} = k_{\theta_y}\) and \(k_{\theta_z}\) are varyi
-3.2 Conclusion
++-3.2 Conclusion
We observe that \(k_{\theta_x} = k_{\theta_y}\) and \(k_{\theta_z}\) increase linearly with the cube size. @@ -1653,8 +1745,8 @@ This was expected as the mass matrix is not diagonal (the Center of Mass of the
-4.3 Conclusion
++4.3 Conclusion
-+@@ -1743,6 +1835,12 @@ controller = initializeController('type',
++disturbances = initializeDisturbances(); +references = initializeReferences(stewart); ++
@@ -1838,8 +1936,8 @@ And we identify the dynamics from the actuator forces \(\tau_{i}\) to the relati
-5.3 Conclusion
++5.3 Conclusion
@@ -1942,8 +2040,7 @@ H = stewart.geometry.H;
We define the useful points of the cube with respect to the Cube’s center. -\({}^{C}C\) are the 6 vertices of the cubes expressed in a frame {C} which is -located at the center of the cube and aligned with {F} and {M}. +\({}^{C}C\) are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}.
@@ -2019,7 +2116,7 @@ stewart.platform_M.Mb = Mb;-diff --git a/matlab/stewart_platform_model.slx b/matlab/stewart_platform_model.slx index 7a17f24..ea6498c 100644 Binary files a/matlab/stewart_platform_model.slx and b/matlab/stewart_platform_model.slx differ diff --git a/org/cubic-configuration.org b/org/cubic-configuration.org index 812db57..941668e 100644 --- a/org/cubic-configuration.org +++ b/org/cubic-configuration.org @@ -484,6 +484,67 @@ However, the rotational stiffnesses are increasing with the cube's size but the | -8e-17 | 0 | -3e-17 | -6.1e-19 | 0.094 | 0 | | -6.2e-18 | 7.2e-17 | 5.6e-17 | 2.3e-17 | 0 | 0.37 | +** Size of the platforms +The minimum size of the platforms depends on the cube's size and the height between the platform and the cube's center. + +Let's denote: +- $H$ the height between the cube's center and the considered platform +- $D$ the size of the cube's edges + +Let's denote by $a$ and $b$ the points of both ends of one of the cube's edge. + +Initially, we have: +\begin{align} + a &= \frac{D}{2} \begin{bmatrix}-1 \\ -1 \\ 1\end{bmatrix} \\ + b &= \frac{D}{2} \begin{bmatrix} 1 \\ -1 \\ 1\end{bmatrix} +\end{align} + +We rotate the cube around its center (origin of the rotated frame) such that one of its diagonal is vertical. +\[ R = \begin{bmatrix} + \frac{2}{\sqrt{6}} & 0 & \frac{1}{\sqrt{3}} \\ + \frac{-1}{\sqrt{6}} & \frac{1}{\sqrt{2}} & \frac{1}{\sqrt{3}} \\ + \frac{-1}{\sqrt{6}} & \frac{-1}{\sqrt{2}} & \frac{1}{\sqrt{3}} +\end{bmatrix} \] + +After rotation, the points $a$ and $b$ become: +\begin{align} + a &= \frac{D}{2} \begin{bmatrix}-\frac{\sqrt{2}}{\sqrt{3}} \\ -\sqrt{2} \\ -\frac{1}{\sqrt{3}}\end{bmatrix} \\ + b &= \frac{D}{2} \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ -\sqrt{2} \\ \frac{1}{\sqrt{3}}\end{bmatrix} +\end{align} + +Points $a$ and $b$ define a vector $u = b - a$ that gives the orientation of one of the Stewart platform strut: +\[ u = \frac{D}{\sqrt{3}} \begin{bmatrix} -\sqrt{2} \\ 0 \\ -1\end{bmatrix} \] + +Then we want to find the intersection between the line that defines the strut with the plane defined by the height $H$ from the cube's center. +To do so, we first find $g$ such that: +\[ a_z + g u_z = -H \] +We obtain: +\begin{align} + g &= - \frac{H + a_z}{u_z} \\ + &= \sqrt{3} \frac{H}{D} - \frac{1}{2} +\end{align} + +Then, the intersection point $P$ is given by: +\begin{align} + P &= a + g u \\ + &= \begin{bmatrix} + H \sqrt{2} \\ + D \frac{1}{\sqrt{2}} \\ + H +\end{bmatrix} +\end{align} + +Finally, the circle can contains the intersection point has a radius $r$: +\begin{align} + r &= \sqrt{P_x^2 + P_y^2} \\ + &= \sqrt{2 H^2 + \frac{1}{2}D^2} +\end{align} + +By symmetry, we can show that all the other intersection points will also be on the circle with a radius $r$. + +For a small cube: +\[ r \approx \sqrt{2} H \] + ** Conclusion #+begin_important We found that we can have a diagonal stiffness matrix using the cubic architecture when $\{A\}$ and $\{B\}$ are located above the top platform. @@ -1119,6 +1180,11 @@ No flexibility below the Stewart platform and no payload. controller = initializeController('type', 'open-loop'); #+end_src +#+begin_src matlab + disturbances = initializeDisturbances(); + references = initializeReferences(stewart); +#+end_src + #+begin_src matlab :exports none displayArchitecture(stewart, 'labels', false, 'view', 'all'); #+end_src @@ -1509,8 +1575,7 @@ This Matlab function is accessible [[file:../src/generateCubicConfiguration.m][h :UNNUMBERED: t :END: We define the useful points of the cube with respect to the Cube's center. -${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is -located at the center of the cube and aligned with {F} and {M}. +${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}. #+begin_src matlab sx = [ 2; -1; -1];Created: 2020-03-03 mar. 15:51
+Created: 2020-03-12 jeu. 18:06
+
+
+
+
+
+
