Update reference tracking path and add curves

This commit is contained in:
Thomas Dehaeze 2020-03-13 13:12:29 +01:00
parent 2a5c629df7
commit a08e141f3d
16 changed files with 475 additions and 319 deletions

File diff suppressed because it is too large Load Diff

Binary file not shown.

Before

Width:  |  Height:  |  Size: 86 KiB

After

Width:  |  Height:  |  Size: 123 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 124 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 70 KiB

After

Width:  |  Height:  |  Size: 96 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 66 KiB

After

Width:  |  Height:  |  Size: 83 KiB

Binary file not shown.

Binary file not shown.

Before

Width:  |  Height:  |  Size: 57 KiB

After

Width:  |  Height:  |  Size: 99 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 124 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 112 KiB

View File

@ -41,11 +41,15 @@
* Introduction :ignore: * Introduction :ignore:
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}$. 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}$.
In order to compute the pose error $\bm{\epsilon}_\mathcal{X}$ from the actual pose $\bm{\mathcal{X}}$ and the reference position $\bm{r}_\mathcal{X}$, some computation is required and explained in section [[sec:compute_pose_error]].
Depending of the measured quantity, different control architectures can be used: Depending of the measured quantity, different control architectures can be used:
- If the struts length $\bm{\mathcal{L}}$ is measured, a decentralized control architecture can be used (Section [[sec:decentralized]]) - If the struts length $\bm{\mathcal{L}}$ is measured, a decentralized control architecture can be used (Section [[sec:decentralized]])
- If the pose of the mobile platform $\bm{\mathcal{X}}$ is directly measured, a centralized control architecture can be used (Section [[sec:centralized]]) - If the pose of the mobile platform $\bm{\mathcal{X}}$ is directly measured, a centralized control architecture can be used (Section [[sec:centralized]])
- If both $\bm{\mathcal{L}}$ and $\bm{\mathcal{X}}$ are measured, we can use an hybrid control architecture (Section [[sec:hybrid]]) - If both $\bm{\mathcal{L}}$ and $\bm{\mathcal{X}}$ are measured, we can use an hybrid control architecture (Section [[sec:hybrid]])
The control configuration are compare in section [[sec:comparison]].
* Decentralized Control Architecture using Strut Length * Decentralized Control Architecture using Strut Length
<<sec:decentralized>> <<sec:decentralized>>
** Matlab Init :noexport: ** Matlab Init :noexport:
@ -95,29 +99,8 @@ Then, a diagonal (decentralized) controller $\bm{K}_\mathcal{L}$ is used such th
[[file:figs/control_measure_rotating_2dof.png]] [[file:figs/control_measure_rotating_2dof.png]]
** Initialize the Stewart platform ** Initialize the Stewart platform
#+begin_src matlab #+begin_src matlab :noweb yes
stewart = initializeStewartPlatform(); <<init-sim>>
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);
#+end_src
#+begin_src matlab
ground = initializeGround('type', 'rigid');
payload = initializePayload('type', 'none');
controller = initializeController('type', 'open-loop');
#+end_src
#+begin_src matlab
disturbances = initializeDisturbances();
references = initializeReferences(stewart);
#+end_src #+end_src
** Identification of the plant ** Identification of the plant
@ -278,26 +261,67 @@ The obtained loop gains corresponding to the diagonal elements are shown in Figu
[[file:figs/loop_gain_decentralized_L.png]] [[file:figs/loop_gain_decentralized_L.png]]
** Simulation ** Simulation
#+begin_src matlab Let's define some reference path to follow.
t = linspace(0, 10, 1000); #+begin_src matlab :noweb yes
<<init-ref>>
r = zeros(6, length(t));
r(1, :) = 5e-3*sin(2*pi*t);
references = initializeReferences(stewart, 't', t, 'r', r);
#+end_src #+end_src
The reference path is shown in Figure [[fig:tracking_control_reference_path]].
#+begin_src matlab :export none
figure;
plot3(squeeze(references.r.Data(1,1,:)), squeeze(references.r.Data(2,1,:)), squeeze(references.r.Data(3,1,:)));
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/tracking_control_reference_path.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:tracking_control_reference_path
#+caption: Reference path used for Tracking control ([[./figs/tracking_control_reference_path.png][png]], [[./figs/tracking_control_reference_path.pdf][pdf]])
[[file:figs/tracking_control_reference_path.png]]
#+begin_src matlab #+begin_src matlab
controller = initializeController('type', 'ref-track-L'); controller = initializeController('type', 'ref-track-L');
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
sim('stewart_platform_model') set_param('stewart_platform_model', 'StopTime', '10')
sim('stewart_platform_model');
simout_D = simout; simout_D = simout;
#+end_src #+end_src
#+begin_src matlab
save('./mat/control_tracking.mat', 'simout_D', '-append');
#+end_src
** Results ** Results
The reference path and the position of the mobile platform are shown in Figure [[fig:decentralized_control_3d_pos]].
#+begin_src matlab :export none
figure;
hold on;
plot3(simout_D.x.Xr.Data(:, 1), simout_D.x.Xr.Data(:, 2), simout_D.x.Xr.Data(:, 3), 'k-');
plot3(squeeze(references.r.Data(1,1,:)), squeeze(references.r.Data(2,1,:)), squeeze(references.r.Data(3,1,:)), '--');
hold off;
xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]');
view([1,1,1])
zlim([0, inf]);
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/decentralized_control_3d_pos.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:decentralized_control_3d_pos
#+caption: Reference path and Obtained Position ([[./figs/decentralized_control_3d_pos.png][png]], [[./figs/decentralized_control_3d_pos.pdf][pdf]])
[[file:figs/decentralized_control_3d_pos.png]]
#+begin_src matlab :exports none #+begin_src matlab :exports none
figure; figure;
subplot(2, 3, 1); subplot(2, 3, 1);
@ -469,29 +493,8 @@ We can think of two ways to make the plant more diagonal that are described in s
#+end_important #+end_important
** Initialize the Stewart platform ** Initialize the Stewart platform
#+begin_src matlab #+begin_src matlab :noweb yes
stewart = initializeStewartPlatform(); <<init-sim>>
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);
#+end_src
#+begin_src matlab
ground = initializeGround('type', 'rigid');
payload = initializePayload('type', 'none');
controller = initializeController('type', 'open-loop');
#+end_src
#+begin_src matlab
disturbances = initializeDisturbances();
references = initializeReferences(stewart);
#+end_src #+end_src
** Identification of the plant ** Identification of the plant
@ -706,14 +709,8 @@ The controller $\bm{K} = \bm{K}_\mathcal{L} \bm{J}$ is computed.
*** Simulation *** Simulation
We specify the reference path to follow. We specify the reference path to follow.
#+begin_src matlab #+begin_src matlab :noweb yes
t = linspace(0, 10, 1000); <<init-ref>>
r = zeros(6, length(t));
r(1, :) = 5e-3*sin(2*pi*t);
references = initializeReferences(stewart, 't', t, 'r', r);
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
@ -722,8 +719,10 @@ We specify the reference path to follow.
We run the simulation and we save the results. We run the simulation and we save the results.
#+begin_src matlab #+begin_src matlab
set_param('stewart_platform_model', 'StopTime', '10')
sim('stewart_platform_model') sim('stewart_platform_model')
simout_L = simout; simout_L = simout;
save('./mat/control_tracking.mat', 'simout_L', '-append');
#+end_src #+end_src
** Diagonal Control - Cartesian Frame ** Diagonal Control - Cartesian Frame
@ -932,14 +931,8 @@ The controller $\bm{K} = \bm{J}^{-T} \bm{K}_\mathcal{X}$ is computed.
*** Simulation *** Simulation
We specify the reference path to follow. We specify the reference path to follow.
#+begin_src matlab #+begin_src matlab :noweb yes
t = linspace(0, 10, 1000); <<init-ref>>
r = zeros(6, length(t));
r(1, :) = 5e-3*sin(2*pi*t);
references = initializeReferences(stewart, 't', t, 'r', r);
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
@ -948,8 +941,10 @@ We specify the reference path to follow.
We run the simulation and we save the results. We run the simulation and we save the results.
#+begin_src matlab #+begin_src matlab
set_param('stewart_platform_model', 'StopTime', '10')
sim('stewart_platform_model') sim('stewart_platform_model')
simout_X = simout; simout_X = simout;
save('./mat/control_tracking.mat', 'simout_X', '-append');
#+end_src #+end_src
** Diagonal Control - Steady State Decoupling ** Diagonal Control - Steady State Decoupling
@ -1106,7 +1101,6 @@ We have that $\bm{K}_0(s)$ commutes with $\bm{G}^{-1}(\omega = 0)$ and thus the
*** Simulation Results *** Simulation Results
The position error $\bm{\epsilon}_\mathcal{X}$ for both centralized architecture are shown in Figure [[fig:centralized_control_comp_Ex]]. The position error $\bm{\epsilon}_\mathcal{X}$ for both centralized architecture are shown in Figure [[fig:centralized_control_comp_Ex]].
The corresponding leg's length errors $\bm{\epsilon}_\mathcal{L}$ are shown in Figure [[fig:centralized_control_comp_El]].
Based on Figure [[fig:centralized_control_comp_Ex]], we can see that: Based on Figure [[fig:centralized_control_comp_Ex]], we can see that:
- There is some tracking error $\epsilon_x$ - There is some tracking error $\epsilon_x$
@ -1177,31 +1171,6 @@ Based on Figure [[fig:centralized_control_comp_Ex]], we can see that:
#+caption: Comparison of the position error $\bm{\epsilon}_\mathcal{X}$ for both centralized controllers ([[./figs/centralized_control_comp_Ex.png][png]], [[./figs/centralized_control_comp_Ex.pdf][pdf]]) #+caption: Comparison of the position error $\bm{\epsilon}_\mathcal{X}$ for both centralized controllers ([[./figs/centralized_control_comp_Ex.png][png]], [[./figs/centralized_control_comp_Ex.pdf][pdf]])
[[file:figs/centralized_control_comp_Ex.png]] [[file:figs/centralized_control_comp_Ex.png]]
#+begin_src matlab :exports none
figure;
hold on;
plot(simout_L.r.r.Time, squeeze(simout_L.r.rL.Data(6, 1, :))-simout_L.y.dLm.Data(:, 6), 'DisplayName', '$K_\mathcal{L}$')
plot(simout_X.r.r.Time, squeeze(simout_X.r.rL.Data(6, 1, :))-simout_X.y.dLm.Data(:, 6), 'DisplayName', '$K_\mathcal{X}$')
for i = 2:6
set(gca,'ColorOrderIndex',1);
plot(simout_L.r.r.Time, squeeze(simout_L.r.rL.Data(i, 1, :))-simout_L.y.dLm.Data(:, i), 'HandleVisibility', 'off')
plot(simout_X.r.r.Time, squeeze(simout_X.r.rL.Data(i, 1, :))-simout_X.y.dLm.Data(:, i), 'HandleVisibility', 'off')
end
hold off;
xlabel('Time [s]');
ylabel('$\epsilon_\mathcal{L}$ [m]');
legend();
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/centralized_control_comp_El.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:centralized_control_comp_El
#+caption: Comparison of the leg's length error $\bm{\epsilon}_\mathcal{L}$ for both centralized controllers ([[./figs/centralized_control_comp_El.png][png]], [[./figs/centralized_control_comp_El.pdf][pdf]])
[[file:figs/centralized_control_comp_El.png]]
** Conclusion ** Conclusion
Both control architecture gives similar results even tough the control in the Leg's frame gives slightly better results. Both control architecture gives similar results even tough the control in the Leg's frame gives slightly better results.
@ -1304,29 +1273,8 @@ This second loop is responsible for the reference tracking.
[[file:figs/hybrid_reference_tracking.png]] [[file:figs/hybrid_reference_tracking.png]]
** Initialize the Stewart platform ** Initialize the Stewart platform
#+begin_src matlab #+begin_src matlab :noweb yes
stewart = initializeStewartPlatform(); <<init-sim>>
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);
#+end_src
#+begin_src matlab
ground = initializeGround('type', 'rigid');
payload = initializePayload('type', 'none');
controller = initializeController('type', 'open-loop');
#+end_src
#+begin_src matlab
disturbances = initializeDisturbances();
references = initializeReferences(stewart);
#+end_src #+end_src
** First Control Loop - $\bm{K}_\mathcal{L}$ ** First Control Loop - $\bm{K}_\mathcal{L}$
@ -1638,20 +1586,16 @@ Then we include the Jacobian in the controller matrix.
** Simulations ** Simulations
We specify the reference path to follow. We specify the reference path to follow.
#+begin_src matlab #+begin_src matlab :noweb yes
t = linspace(0, 10, 10000); <<init-ref>>
r = zeros(6, length(t));
r(1, :) = 5e-3*sin(2*pi*t);
references = initializeReferences(stewart, 't', t, 'r', r);
#+end_src #+end_src
We run the simulation and we save the results. We run the simulation and we save the results.
#+begin_src matlab #+begin_src matlab
set_param('stewart_platform_model', 'StopTime', '10')
sim('stewart_platform_model') sim('stewart_platform_model')
simout_H = simout; simout_H = simout;
save('./mat/control_tracking.mat', 'simout_H', '-append');
#+end_src #+end_src
The obtained position error is shown in Figure [[fig:hybrid_control_Ex]]. The obtained position error is shown in Figure [[fig:hybrid_control_Ex]].
@ -1712,7 +1656,88 @@ The obtained position error is shown in Figure [[fig:hybrid_control_Ex]].
** Conclusion ** Conclusion
* Position Error computation * Comparison of all the methods
<<sec:comparison>>
Let's load the simulation results and compare them in Figure [[fig:reference_tracking_performance_comparison]].
#+begin_src matlab
load('./mat/control_tracking.mat', 'simout_D', 'simout_L', 'simout_X', 'simout_H');
#+end_src
#+begin_src matlab :exports none
figure;
subplot(2, 3, 1);
hold on;
plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(1, 1, :))-simout_D.x.Xr.Data(:, 1))
plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(1, 1, :))-simout_L.x.Xr.Data(:, 1))
plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(1, 1, :))-simout_X.x.Xr.Data(:, 1))
plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(1, 1, :))-simout_H.x.Xr.Data(:, 1))
hold off;
xlabel('Time [s]');
ylabel('Dx [m]');
subplot(2, 3, 2);
hold on;
plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(2, 1, :))-simout_D.x.Xr.Data(:, 2))
plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(2, 1, :))-simout_L.x.Xr.Data(:, 2))
plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(2, 1, :))-simout_X.x.Xr.Data(:, 2))
plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(2, 1, :))-simout_H.x.Xr.Data(:, 2))
hold off;
xlabel('Time [s]');
ylabel('Dy [m]');
subplot(2, 3, 3);
hold on;
plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(3, 1, :))-simout_D.x.Xr.Data(:, 3))
plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(3, 1, :))-simout_L.x.Xr.Data(:, 3))
plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(3, 1, :))-simout_X.x.Xr.Data(:, 3))
plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(3, 1, :))-simout_H.x.Xr.Data(:, 3))
hold off;
xlabel('Time [s]');
ylabel('Dz [m]');
subplot(2, 3, 4);
hold on;
plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(4, 1, :))-simout_D.x.Xr.Data(:, 4))
plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(4, 1, :))-simout_L.x.Xr.Data(:, 4))
plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(4, 1, :))-simout_X.x.Xr.Data(:, 4))
plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(4, 1, :))-simout_H.x.Xr.Data(:, 4))
hold off;
xlabel('Time [s]');
ylabel('Rx [rad]');
subplot(2, 3, 5);
hold on;
plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(5, 1, :))-simout_D.x.Xr.Data(:, 5))
plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(5, 1, :))-simout_L.x.Xr.Data(:, 5))
plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(5, 1, :))-simout_X.x.Xr.Data(:, 5))
plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(5, 1, :))-simout_H.x.Xr.Data(:, 5))
hold off;
xlabel('Time [s]');
ylabel('Ry [rad]');
subplot(2, 3, 6);
hold on;
plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(6, 1, :))-simout_D.x.Xr.Data(:, 6), 'DisplayName', 'Decentralized - L')
plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(6, 1, :))-simout_L.x.Xr.Data(:, 6), 'DisplayName', 'Centralized - L')
plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(6, 1, :))-simout_X.x.Xr.Data(:, 6), 'DisplayName', 'Centralized - X')
plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(6, 1, :))-simout_H.x.Xr.Data(:, 6), 'DisplayName', 'Hybrid')
hold off;
xlabel('Time [s]');
ylabel('Rz [rad]');
legend();
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/reference_tracking_performance_comparison.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:reference_tracking_performance_comparison
#+caption: Comparison of the position errors for all the Control architecture used ([[./figs/reference_tracking_performance_comparison.png][png]], [[./figs/reference_tracking_performance_comparison.pdf][pdf]])
[[file:figs/reference_tracking_performance_comparison.png]]
* Compute the pose error of the Stewart Platform
<<sec:compute_pose_error>> <<sec:compute_pose_error>>
Let's note: Let's note:
@ -1813,3 +1838,46 @@ Now we want to express ${}^VT_R$:
Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery)); Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery));
Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery)); Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery));
#+end_src #+end_src
* Useful Blocks :noexport:
** Initialize Simulation
#+name: init-sim
#+begin_src matlab
% 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 and Payload
ground = initializeGround('type', 'rigid');
payload = initializePayload('type', 'none');
% Controller
controller = initializeController('type', 'open-loop');
% Disturbances and References
disturbances = initializeDisturbances();
references = initializeReferences(stewart);
#+end_src
** Initialize Reference Path
#+name: init-ref
#+begin_src matlab
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);
#+end_src