Embed Size (px)
Citation preview
ECOLE CENTRALE DE NANTES
European Mater of Advanced Robotics
Identification and Control of Robots
Identification of dynamic parameters
Practical work 2
Students: Negoiţă George Alexandru & Anand VAZHAPILLI
Professor: Wisama Khalil
- Nantes 2012 -
2
I. Using the trajectory generator of 5th degree in the joint space, find for the 2R robot the
evolution of joint positions, velocities and accelerations between six points assuming a
sampling time equal to 10 ms. Calculate the joint torques corresponding to this trajectory. Use
the Simulink block of assignment 1 of dynamic model (AMORO).
ZZ1R= 3.95 , ZZ2 = 0.25 Kg.m2
MX2= 0.4 , MY2= 0.15 Kg.m
Other inertial parameters equal to zero.
Fv1= 0.3 Fv2= 0.15 Nm/rad/s
Fc1 = 0.3 Fc2 = 0.25 Nm .
IA1=0, IA2=0 (direct drive).
The trajectory has been computed for the following set of points: 5 of them were already given and we
added the sixth one.
Figure 1: The points used for the trajectory generation.
*[
] , - [
] , - , - , -+
After setting the points for the trajectory generation, we ran the Simulink file and we saved the vectors
for the position, velocity, acceleration and joint torque evolution (.mat files).
qout.mat - position evolution vector;
qpout.mat - velocity evolution vector;
qdpout.mat - acceleration evolution vector;
gamma.mat - torque evolution vector.
After saving the values, we can now plot the evolutions of these vectors. The results are presented
below:
3
Figure 2: Position evolution
Figure 3: Velocity evolution.
4
Figure 4: Acceleration evolution.
Figure 5: Torque evolution.
5
Calculate the dynamic identification model of the robot 2R using software package SYMORO using the
standard parameters (the file is attached). Determine the base inertial parameters using the
numerical method based on the QR decomposition.
The dynamic parameters of link j and actuator j are composed of the inertial parameters of the link, the
actuator rotor inertia, and the friction parameters. We combine these parameters in the vector :
, -
Once the parameters related to each link are defined, it is possible to concatenate them in order to
obtain a vector describing the whole dynamic; in the specific case:
,
-
All the identification models can be written in the following general form:
( ) ( )
Applying the identification model at a sufficient number of points on some trajectories, we construct the
following over determined linear system of equation in :
( ) ( )
where W is an (rxc) observation matrix, or regressor, r is the total number of equations, c is the number
of parameters such that r>>c, and is the residual error vector.
The identification handbooks provide a large variety of deterministic and stochastic methods to estimate
from the previous system of equations. The solution can be found applying different methods, such as
the minimization of the residual error, using the ordinary least-square method. The analyzed linear
systems of equations can be simplified thanks to the SVD or QR decomposition. Taking in account the
given method, the estimated value of can be obtained from the previous equation such that:
‖ ‖
If is of full rank, the explicit solution of this problem is given by:
( )
Where is the pseudo inverse of .
The inverse dynamic model is linear in the dynamic parameters. It is given by the following equation:
( )
where is and (nxb) matrix, and b is the number of the base dynamic parameters.
From the above equation, we deduce that the ith column of , denoted by , is equal to :
6
(
The matrix can be consider as the Jacobian that put in relation the vector of the torque and the vector of the base inertial parameters:
The relation defining the single elements of the matrix are computed according to the software “SYMORO”. Given the matrix the system of equations to solve is given by:
( ) ( )
where:
[ ( ) ( )
] [ ( ) ( )
]
It is practical to sort the previous matrices in order to have a representation divided by joint:
[
] [
] [
]
where:
represent the equations of joint i for all the samples such that:
[ ( ) ( )
] [ ( )( )
( )( )
] [ ( ) ( )
]
If not all the parameters are fully identifiable, it means that in some way the matrix W is not full rank. In
order to discover which the base inertial parameters are, it is possible to use a different couple of
methods:
A symbolic method, based on determining the independent elements of the matrix W; this
method is quite fast and good if it is applied to serial and tree structured robot. When there are
closed loop robots, or parallel ones, the situation becomes a little bit more complicated and the
solution is not always guaranteed.
A numerical method, based on the study of the space spanned by the columns of the matrix W.
In order to do that is sufficient to use a huge set of random configurations. To study the
observation matrix W is possible to use different methods, such as the Singular Values
Decomposition (SVD) and the QR decomposition. In the particular case, it has been decided to
use the second proposal.
The QR decomposition of the matrix W is represented by:
7
[
( ) ]
where is a , - orthogonal matrix and R is a , - upper triangular matrix.
In our case the matrix R dimension is , -. Analyzing the elements on the diagonal it is possible to see
if some of the parameters can be identified. If the value on the diagonal is equal to 0, or very small
compared with the other values, then we can conclude that the corresponding parameter cannot be
identified. After performing the QR decomposition we found out that the 7th parameter corresponding
to the parameter presents a small value. This result was expected, since, as known from previous
analysis, the contribution given by the mass of the second link can be incorporated in , generating
the modified parameter . Consequently it is possible to reduce the problem of one dimension,
deleting the corresponding column of the matrix W and the corresponding element of the column
vector . As expected, all the Columbian and the viscous friction parameters cannot be deleted.
Figure 6: The matrix R obtained after the QR decomposition.
Figure 7: The diagonal elements of the R matrix.
As we can observe from the previous two figures, the corresponding element of presents a small
value as we discussed before.
8
II. Determine by identification the values of the base dynamic parameters. Make use of the data
generated in I-a and I-b.
Using the previous discussed theoretical aspects, we calculated the values of the parameters and we
compared these values to the ones that we had given. For this case the obtained values are exactly
matching the real values as we can see in the following plot:
Figure 8: The real and the calculated parameter values. The values are exactly matching.
Figure 9: The calculated values of the parameters.
Parameter Real value Calculated value 3.95 3.95 0.3 0.3 0.3 0.3 0.25 0.25 0.4 0.4 0.15 0.15 0.15 0.15 0.25 0.25 Table 1: Comparison between real and obtained values of the parameters.
9
III. Repeat the identification of the dynamic parameters supposing as available the joint positions
and torques. Calculate the joint velocities and acceleration using adequate filtering and
central differentiation. For the filtering use the function “filtfilt” and a Butterworth filter
(function « butter » of Matlab ). Calculate the condition number of the observation matrix and
the relative standard deviation of the estimation errors.
In the real robotic systems, usually the machine is endowed just with joint position sensors. This means
that it will be necessary to derivate numerically these values in order to get the joint velocities and
accelerations. Considering the fact that the derivation of the joint vectors can introduce in the system a
high frequency error, it is necessary to use some filtering in order to clean the position information. The
data for filtering is obtained from the sampling operated by the digital encoders. The sampling rate ,
according to the Nyquist frequency, should be much greater than the noise frequency. A good filter is
represented by the Butterworth one: it is characterized by a flat amplitude response in the bandwidth.
The recommended order of the filter, , is between 4 and 8. In the following case it will be possible
to observe some tests performed with different choices.
( ) ( ) ( ) ( )
( ) ( )
From the other side, the cut off frequency will be fixed at 0.2, according to the Shannon limit:
(
)
where B is the bandwidth, S is the total signal power and N the total noise power.
Once the filtered joint position data is ready, it is possible to compute its derivatives employing some
numerical methods, such as the central differential algorithm. For velocity estimation the formula
becomes:
( ) , ( ) ( )-
Similarly, for the acceleration:
( ) , ( ) ( )-
Once all these elements are known, it is possible to apply the same procedure described in the previous
chapter. Given the minimum base dynamic parameters, the solution is computed as:
Also the torque can be affected by noise. In order to clean the input data it is consequently preferable to
filter the column of the observation matrix and the torque itself, and , in order to obtain a new
filtered linear system:
10
To calculate the joint velocities and accelerations and parallel filtering we followed the following steps:
1- Calculate the parameters of a low path filter such as Butterworth of order between 4 and 8 using the
function “butter”. The cut off frequency should be around 0.2 that of Shannon.
2- Filter the joint positions using the previous filter and the function « filtfilt ».
3- Calculate the joint velocities by derivating the filtered joint positions using the function diffcent.
4- Calculate the joint accelerations by derivating the joint velocities using « diffcent ».
5- Calculate the matrix W and the vector Y (joints torques) then filter their columns using a decimat of
order 5.
6- Cancel the first and the last ten points to eliminate the boundary effects of derivation.
In our case we chose the order of the filter to be 5. The results (values of the parameters) before and
after filtering and derivating the points we obtained the following results, shown in the following table:
Parameter The real value Value before decimation
Value after decimation
3.95 3.9485 3.9497 0.3 0.3381 0.3291 0.3 0.2285 0.2457 0.25 0.2497 0.2499 0.4 0.4022 0.4005 0.15 0.1489 0.1486 0.15 0.1927 0.1602 0.25 0.2004 0.2370
Table 2: The obtained values before and after performing the decimation.
We notice that we obtain better results for the case of decimation. In the following figures we can see a
graphical representation between the real and obtained values before and after performing the
decimation.
11
Figure 9: Comparison between the real and the obtained values before performing the decimation.
Figure 10: Comparison between the real and the obtained values after performing the decimation.
In order to understand if the result obtained is reliable, and to compare the different methodologies, it
is necessary to define some evaluation methods. The most significant parameters that can be employed
are the condition number of the observation matrix, the covariance and the relative standard deviation
of the estimation error.
The condition numbers let to define specifically if the analysis is robust with respect to the modeling
errors and the noise. Computing the condition number it is possible to understand if the problem is well
posed, and consequently if the trajectory chosen for the identification procedure is reliable or not.
Observing the condition number is also possible to optimize the trajectory in such a way to obtain the
12
best result in the further operation. In particular, the condition number of the observation matrix can be
computed taking in account the 2.norm as:
The standard deviations of the estimated values are calculated by assuming that is deterministic and
is zero mean additive independent Gaussian noise, with standard deviation . The covariance matrix
is given by:
( )
where is the expectation operator and is the (rxr) identity matrix.
An unbiased estimation of can be as follows:
‖ ‖
( )
The covariance matrix of the estimation error is given by:
,( )( ) - (
) ( )
The standard deviation on the parameter is obtained from the (j,-j) element of :
√ ( )
The relative standard deviation can be used as a criterion to measure the quality of the identification
value for each parameter. It is obtained by:
| |
with | |
The condition number obtained after decimation is close to the one obtained for the real values:
Condition number Before decimation After decimation
Values 14.3142 15.3030 Table 3: The condition numbers.
The calculated values for the standard deviation and the relative standard deviation are shown in the
following table for each of the parameters:
Parameter Standard deviation Relative standard deviation 0.0008 0.0204 0.0014 0.4357 0.0021 0.8661 0.0003 0.1260
13
0.0007 0.1655 0.0004 0.2586 0.0024 1.4906 0.0020 0.8271
Table 4: The values for the standard deviation and for the relative standard deviation.
The torque of joint j is independent of the dynamic parameters of links 1,..., j-1, we can write the
dynamic model such that the matrix is upper block triangular
[
] [
]
[
]
where contains the identifiable parameters of both link and actuator j, and is the row vector to be
multiplied by in the equation for
Grouping the equations for each joint together we obtain:
[
] [
]
[
]
This form of the dynamic equation can be used to estimate the dynamic parameters of each link
individually, starting sequentially from link n and proceeding to link 1.
Now, we can plot the values of the real and predicted torques. We will observe that the values in this
case are very close and that the error is negligible.
Figure 11: Real vs Predicted values of torque.
14
Figure 12: Real values for the torques and the error between the real and the predicted values.
IV. Repeat the previous step using the real data given in the file “donnee.mat”. In which the
torque of joint j is denoted cmj and the position trajectory is denoted pmj and the time is
given in the array t. Calculate the condition number of the observation matrix, the relative
standard deviation of the estimation errors, the norm of the error vector and its standard
deviation, and draw the predicted torques and the real torques on the same plot, and the
error torques with the real torque on another plot.
Having loaded the new real values, we proceed as previously and perform the filtering and decimation
operations. The values of the parameters before and after the decimation are given in the following
table:
Parameter The real value Value before decimation
Value after decimation
3.95 3.2908 3.3907 0.3 0.3537 0.3093 0.3 0.3918 0.4163 0.25 0.0658 0.0644 0.4 0.2395 0.2426 0.15 0.0028 0.0028 0.15 0.0154 0.0148 0.25 0.1250 0.1270
Table 5: The obtained values before and after performing the decimation in the case of the real values.
We can conclude that in the case of the real values, the results obtained are different than the real
values so the error is bigger.
15
In the following figures we can see a graphical representation between the real and obtained values
before and after performing the decimation.
Figure 13: Comparison between the real and the obtained values before performing the decimation
(real case).
Figure 14: Comparison between the real and the obtained values after performing the decimation (real
case).
The condition number obtained after decimation is close to the one obtained for the real values:
Condition number Before decimation After decimation
Values 31.3292 32.3337 Table 6: The condition numbers (real case).
16
The values are also similar in this case, but compared to the previous case (subject III) we obtain a
double condition number.
The calculated values for the standard deviation and the relative standard deviation are shown in the
following table for each of the parameters:
Parameter Standard deviation Relative standard deviation 0.0211 0.6234 0.0572 18.4825 0.0270 6.4968 0.0020 3.1087 0.0027 1.1126 0.0025 88.2030 0.0073 49.3592 0.0211 16.5922
Table 7: The values for the standard deviation and for the relative standard deviation (real case).
We notice that we obtain big values for the standard deviation of the forces and for the parameter
. The standard deviation, when assume values greater than 10 times the minimum relative
standard deviation value, is not well identified. Consequently, it is possible to conclude that most of the
parameters belonging to the second link do not have an high quality, especially the dynamic viscosity
and the parameter .
Now, we can plot the values of the real and predicted torques. We will observe that the values in this
case are very close and that the error is negligible.
Figure 15: Real vs Predicted values of torque (real case).
17
Figure 16: Real values for the torques and the error between the real and the predicted values (real
case).
We can also say that in the case of the real values the error between the real and the obtained values of
the torque has increased and is not negligible anymore.
18
ANNEXE - The codes for this assignment:
The function that gives the identification model of the standard parameters for the 2R robot:
% (********************************************) % (** SYMORO+ : SYmbolic MOdelling of RObots **) % (**========================================**) % (** IRCCyN-ECN - 1, rue de la Noe **) % (** B.P.92101 **) % (** 44321 Nantes cedex 3, FRANCE **) % (** www.irccyn.ec-nantes.fr **) % (********************************************)
% Name of file : C:\Program Files\IRCCyN\Symoro+\Robots\scara\scara-TP-
2011.dim
% Geometric parameters
% j ant mu sigma gamma b alpha d theta r % 1 0 1 0 0 0 0 0 t1 0 % 2 1 1 0 0 0 0 L1 t2 0
% Inertial parameters
% j XX XY XZ YY YZ ZZ MX MY MZ M Ia
% 1 XX1 XY1 XZ1 YY1 YZ1 ZZ1 MX1 MY1 MZ1 M1 0
% 2 XX2 XY2 XZ2 YY2 YZ2 ZZ2 MX2 MY2 MZ2 M2 0
% External forces,friction parameters, joint velocities and accelerations
% j FX FY FZ CX CY CZ FS FV QP QDP
% 1 0 0 0 0 0 0 FS1 FV1 QP1 QDP1
% 2 0 0 0 0 0 0 FS2 FV2 QP2 QDP2
% Base velocity, base accelerations, and gravity
% j W0 WP0 V0 VP0 G
% 1 0 0 0 0 0
19
% 2 0 0 0 0 0
% 3 0 0 0 0 G3
% Dynamic identification model .
% Equations:
% Declaration of the function function W = scara(t2,QP1,QP2,QDP1,QDP2)
% Declaration of global input variables %global t2 QP1 QP2 QDP1 QDP2 L1 L1 = 0.5; % Declaration of global output variables % global DG1ZZ1 DG1FV1 DG1FS1 DG2ZZ2 DG1ZZ2 DG2MX2 DG1MX2 DG2MY2
DG1MY2 DG1M2 % global DG2FV2 DG2FS2
% Function description: S2=sin(t2); C2=cos(t2); DV331=-QP1.^2; W32=QP1 + QP2; WP32=QDP1 + QDP2; DV332=-W32.^2; VSP12=DV331.*L1; VSP22=L1.*QDP1; VP12=C2.*VSP12 + S2.*VSP22; VP22=-(S2.*VSP12) + C2.*VSP22; DG1ZZ1=QDP1; DG1FV1=QP1; DG1FS1=sign(QP1); DG2ZZ2=WP32; DG1ZZ2=WP32; DG2MX2=VP22; N1MX23=VP22 + L1.*(DV332.*S2 + C2.*WP32); DG1MX2=N1MX23; DG2MY2=-VP12; N1MY23=-VP12 + L1.*(C2.*DV332 - S2.*WP32); DG1MY2=N1MY23; N1M23=L1.*(S2.*VP12 + C2.*VP22); DG1M2=N1M23; DG2FV2=QP2; DG2FS2=sign(QP2);
vector_size = size(QP1); size_zero = zeros(vector_size(1),1); W = [DG1ZZ1 DG1FV1 DG1FS1 DG1ZZ2 DG1MX2 DG1MY2 DG1M2 size_zero size_zero; size_zero size_zero size_zero DG2ZZ2 DG2MX2 DG2MY2 size_zero DG2FV2
DG2FS2];
20
% *=* % Number of operations : 9 '+' or '-', 17 '*' or '/' end
The function that gives the numerical derivation using the central method:
function yd = diffcent(y,pas)
%DIFFCENT numerical derivation by central difference % DIFFCENT(Y, PAS), for a vector Y, is [Y(2)-Y(1) (Y(3)-Y(1))/2 ... Y(n)-
Y(n-1)]/PAS. % See also DIFF, GRADIENT, DEL2, INT, SYMVAR. % Copyright (c) by M. Gautier, IRCCyN Robotique
ny=length(y); yd=[(y(2)-y(1));(y(3:ny)-y(1:ny-2))/2;(y(ny)-y(ny-1))]/pas;
end
Function that calculates the values of the parameters before and after the decimation. Also in this
function the condition number, the standard deviation and the relative standard deviation are
computed (along with the presented graphs):
load('GAMMA.mat'); load('qout.mat'); load('qpout.mat'); load('qdpout.mat');
%% Identification of the values of the base dynamic parameters
W = scara(qout(:,2),qpout(:,1),qpout(:,2),qdpout(:,1),qdpout(:,2)); W_new = [W(:,1:6) W(:,8:9)]; Y = [GAMMA(:,1); GAMMA(:,2)];
Param = pinv(W_new)*Y; %subject II
%% III subject
Wn = 0.2; %the cutoff frequency n = 4; %the order of the filter pas = 0.001; %the period
[b,a] = butter(n,Wn,'low');
%filtering the position
21
qout1_filt = filtfilt(b,a,qout(:,1)); qout2_filt = filtfilt(b,a,qout(:,2));
%differentiating position to obtain velocity qpout1 = diffcent(qout1_filt,pas); qpout2 = diffcent(qout2_filt,pas);
%differentiating velocity to obtain acceleration qdpout1 = diffcent(qpout1,pas); qdpout2 = diffcent(qpout2,pas);
% calculating the new W matrix without decimation W_newnew = scara(qout2_filt,qpout1,qpout2,qdpout1,qdpout2); sizeW = size(W_newnew); rowsW = sizeW(1); W_withoutM2 = [W_newnew(11:(rowsW-10),1:6) W_newnew(11:(rowsW-10),8:9)]; Y_new = Y(11:(rowsW-10));
Param_new = pinv(W_withoutM2)*Y_new;
% calculating the new W matrix with decimation of order 5
r = 5; %the order of deciamtion
W_withoutM2d = [W_newnew(:,1:6) W_newnew(:,8:9)]; dec_columns = rowsW/r+1; W_decimated = zeros(dec_columns,8); for k = 1:8 W_decimated(:,k) = decimate(W_withoutM2d(:,k),r); end
Y_dec = decimate(Y,r);
Wd = W_decimated(11:(dec_columns-10),:); Yd = Y_dec(11:(dec_columns-10));
Param_new_dec = pinv(Wd)*Yd;
xaxes = 1:8;
figure(1); plot(xaxes, Param, 'o','MarkerSize',12); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); hold on; plot(xaxes, Param_new, 'rx','MarkerSize',12); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); title('Parameters values - real and before
decimation','fontweight','bold','fontsize',16); xlabel('Parameters','fontweight','bold','fontsize',12); ylabel('Real and before decimation -
values','fontweight','bold','fontsize',14); legend('Real values','Values before decimation');
22
figure(2); plot(xaxes, Param, 'o', 'MarkerSize',12); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); hold on; plot(xaxes, Param_new_dec, 'rx', 'MarkerSize',12); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); title('Parameters values - real and after
decimation','fontweight','bold','fontsize',16); xlabel('Parameters','fontweight','bold','fontsize',12); ylabel('Real and after decimation -
values','fontweight','bold','fontsize',14); legend('Real values','Values after decimation');
%% Getting the condition number
cond_n = cond(Wd);
%% calculating the standard deviation
sizeWd = size(Wd); row = sizeWd(1); column = sizeWd(2);
sigmap = (norm(Yd-Wd*Param_new_dec)^2)/(row-column);
covariance_mat = sigmap*inv(Wd'*Wd);
strd_dev = sqrt(diag(covariance_mat)); %standard deviation computed
rel_std = zeros(8,1); for k = 1:8 rel_std(k) = (strd_dev(k)/abs(Param_new_dec(k)))*100; end
X = Wd*Param_new_dec; error = Yd-X;
t = 1:1:row; figure(3); plot(t,Yd,'k','LineWidth',2); hold on; plot(t,X,'r','LineWidth',2); title('Predicted and real torques','fontweight','bold','fontsize',16); xlabel('Time','fontweight','bold','fontsize',12); ylabel('Torque values','fontweight','bold','fontsize',14); legend('Real','Predicted');
figure(4); plot(t,Yd,'k','LineWidth',2); hold on; plot(t,error,'r','LineWidth',2);
23
title('Real torques and the error between real and predicted
torques','fontweight','bold','fontsize',16); xlabel('Time','fontweight','bold','fontsize',12); ylabel('Values','fontweight','bold','fontsize',14); legend('Real torques','Error');
Function that calculates the values of the parameters before and after the decimation but for the real
case. Also in this function the condition number, the standard deviation and the relative standard
deviation are computed (along with the presented graphs):
variables = load('donnee.mat');
%% IV subject
Wn = 0.2; %the cutoff frequency n = 4; %the order of the filter pas = 0.01; %the period
[b,a] = butter(n,Wn,'low');
%filtering the position qout1_filt = filtfilt(b,a,variables.pm1); qout2_filt = filtfilt(b,a,variables.pm2);
%differentiating position to obtain velocity qpout1 = diffcent(qout1_filt,pas); qpout2 = diffcent(qout2_filt,pas);
%differentiating velocity to obtain acceleration qdpout1 = diffcent(qpout1,pas); qdpout2 = diffcent(qpout2,pas);
% calculating the new W matrix without decimation W_newnew = scara(qout2_filt,qpout1,qpout2,qdpout1,qdpout2); sizeW = size(W_newnew); rowsW = sizeW(1); W_withoutM2 = [W_newnew(11:(rowsW-10),1:6) W_newnew(11:(rowsW-10),8:9)];
Y = [variables.cm1; variables.cm2]; Y_new = Y(11:(rowsW-10));
Param_new = pinv(W_withoutM2)*Y_new;
% calculating the new W matrix with decimation of order 5
r = 5; %the order of deciamtion
W_withoutM2d = [W_newnew(:,1:6) W_newnew(:,8:9)]; dec_columns = rowsW/r+1; W_decimated = zeros(dec_columns,8);
24
for k = 1:8 W_decimated(:,k) = decimate(W_withoutM2d(:,k),r); end
Y_dec = decimate(Y,r);
Wd = W_decimated(11:(dec_columns-10),:); Yd = Y_dec(11:(dec_columns-10));
Param_new_dec = pinv(Wd)*Yd;
xaxes = 1:8;
figure(1); plot(xaxes, Param, 'o','MarkerSize',12); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); hold on; plot(xaxes, Param_new, 'rx','MarkerSize',12); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); title('Parameters values - real and before
decimation','fontweight','bold','fontsize',16); xlabel('Parameters','fontweight','bold','fontsize',12); ylabel('Real and before decimation -
values','fontweight','bold','fontsize',14); legend('Real values','Values before decimation');
figure(2); plot(xaxes, Param, 'o', 'MarkerSize',12); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); hold on; plot(xaxes, Param_new_dec, 'rx', 'MarkerSize',12); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); title('Parameters values - real and after
decimation','fontweight','bold','fontsize',16); xlabel('Parameters','fontweight','bold','fontsize',12); ylabel('Real and after decimation -
values','fontweight','bold','fontsize',14); legend('Real values','Values after decimation');
%% Getting the condition number
cond_n = cond(Wd);
%% calculating the standard deviation
sizeWd = size(Wd); row = sizeWd(1); column = sizeWd(2);
sigmap = (norm(Yd-Wd*Param_new_dec)^2)/(row-column);
25
covariance_mat = sigmap*inv(Wd'*Wd);
strd_dev = sqrt(diag(covariance_mat)); %standard deviation computed
rel_std = zeros(8,1); for k = 1:8 rel_std(k) = (strd_dev(k)/abs(Param_new_dec(k)))*100; end
X = Wd*Param_new_dec; error = Yd-X;
t = 1:1:row; figure(3); plot(t,Yd,'k','LineWidth',2); hold on; plot(t,X,'g','LineWidth',2); title('Predicted and real torques','fontweight','bold','fontsize',16); xlabel('Time','fontweight','bold','fontsize',12); ylabel('Torque values','fontweight','bold','fontsize',14); legend('Real','Predicted');
figure(4); plot(t,Yd,'k','LineWidth',2); hold on; plot(t,error,'g','LineWidth',2); title('Real torques and the error between real and predicted
torques','fontweight','bold','fontsize',16); xlabel('Time','fontweight','bold','fontsize',12); ylabel('Values','fontweight','bold','fontsize',14); legend('Real torques','Error');
Testing function that calculates the QR decomposition, represents the evolution of the position, velocity
and acceleration evolution:
sizeq = size(qout); rowq = sizeq(1); close all;
t = 1:1:rowq;
%% plotting position evolution figure(1); plot(t,qout(:,1),'-ro','LineWidth',2); hold on; plot(t,qout(:,2),'g','LineWidth',2); title('Position evolution','fontweight','bold','fontsize',16); xlabel('Time','fontweight','bold','fontsize',12); ylabel('Position of first and second
joints','fontweight','bold','fontsize',14); legend('Link1','Link2');
26
%% plotting velocity evolution figure(2); plot(t,qpout(:,1),'-ro','LineWidth',2); hold on; plot(t,qpout(:,2),'g','LineWidth',2); title('Velocity evolution','fontweight','bold','fontsize',16); xlabel('Time','fontweight','bold','fontsize',12); ylabel('Velocity of first and second
joints','fontweight','bold','fontsize',14); legend('Link1','Link2');
%% plotting acceleration evolution figure(3); plot(t,qdpout(:,1),'-ro','LineWidth',2); hold on; plot(t,qdpout(:,2),'g','LineWidth',2); title('Acceleration evolution','fontweight','bold','fontsize',16); xlabel('Time','fontweight','bold','fontsize',12); ylabel('Acceleration of first and second
joints','fontweight','bold','fontsize',14); legend('Link1','Link2');
%% ploting torque evolution figure(4); plot(t,GAMMA(:,1),'-ro','LineWidth',2); hold on; plot(t,GAMMA(:,2),'g','LineWidth',2); title('Torque evolution','fontweight','bold','fontsize',16); xlabel('Time','fontweight','bold','fontsize',12); ylabel('Torque of first and second
joints','fontweight','bold','fontsize',14); legend('Link1','Link2');
%% QR decomposition
W = scara(qout(:,2),qpout(:,1),qpout(:,2),qdpout(:,1),qdpout(:,2)); [Q,R] = qr(W); diagR = diag(R);
%% second topic - identifying the parameters
%xaxes = ['ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2' 'FV2'];
xaxes = 1:8;
figure(5); plot(xaxes, Param, 'o'); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); hold on; plot(xaxes, Param, 'rx'); set(gca, 'XTick',1:8, 'XTickLabel',{'ZZ1' 'FC1' 'FV1' 'ZZ2' 'MX2' 'MY2' 'FC2'
'FV2'}); title('Parameters values','fontweight','bold','fontsize',16); xlabel('Parameters','fontweight','bold','fontsize',12);
27
ylabel('Real and calculated values','fontweight','bold','fontsize',14); legend('real values','calculated values');
