second order, need add on error analysis
a2nr
parent
06667063ba
commit
02f9ae1bbf
|
|
@ -69,8 +69,10 @@ refrensi.
|
|||
$e = \begin{bmatrix} e_1^T \\ \vdots \\ e_m^T \end{bmatrix} \in \mathbb{R}^{2m}$ & Edge Vector \\
|
||||
|
||||
$E \in R^{n\times m}$ & Incidence matrix dimana isinya adalah $\{0, \pm 1\}$ \\
|
||||
& barisnya menandakan vertices dan kolom nya menandakan edge \\ \hline
|
||||
\hline
|
||||
& barisnya menandakan vertices dan kolom nya menandakan edge \\ \hline
|
||||
$A_1, \dots, A_i \in \mathbb{R}^{p \times q}$ & \\
|
||||
$diag(A_i) \triangleq blkdiag\{A_1, \dots, A_n\} \in \mathbb{R}^{np \times nq}$ & https://www.mathworks.com/help/matlab/ref/blkdiag.html \\
|
||||
\hline
|
||||
\end{tabular}
|
||||
%% \end{table}
|
||||
|
||||
|
|
@ -182,7 +184,7 @@ Dengan menerapkan kendali PI
|
|||
Menghasilkan state baru dan Ditulis ulang
|
||||
\begin{align}
|
||||
\dot{x}_1 =& x_{2}(t)\\
|
||||
\dot{x}_2 =& -k_{p1}x_2(t) -R(x_1)^T k_{p2}(R(x_1)x_1(t) - d )) - \xi_1 - R(x_1) \xi_2 \\
|
||||
\dot{x}_2 =& -k_{p1}x_2(t) -R(x_1)^T k_{p2}(R(x_1)x_1(t) - d )) - \xi_1 - R(x_1)^T \xi_2 \\
|
||||
\dot{\xi}_1 =& k_{i1} x_2(t)\\
|
||||
\dot{\xi}_2 =& k_{i2} (R(x_1)x_1(t) - d)
|
||||
\end{align}
|
||||
|
|
@ -193,7 +195,7 @@ Dalam bentuk matrix
|
|||
\end{bmatrix} =
|
||||
\begin{bmatrix}
|
||||
0 & 1 & 0 & 0\\
|
||||
-k_{p2}R(x_1)^T R(x_1) & -k_{p1} & -1 & -R(x_1) \\
|
||||
-k_{p2}R(x_1)^T R(x_1) & -k_{p1} & -1 & -R(x_1)^T \\
|
||||
0 & k_{i1} & 0 & 0 \\
|
||||
k_{i2}R(x_1) & 0 & 0 & 0 \\
|
||||
\end{bmatrix}
|
||||
|
|
|
|||
|
|
@ -1,85 +1,48 @@
|
|||
clear -all
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
|
||||
conRobot = [1 2 1;
|
||||
%% 2 1 -1;
|
||||
1 5 1;
|
||||
%% 5 1 -1;
|
||||
1 6 1;
|
||||
%% 6 1 -1;
|
||||
2 3 1;
|
||||
%% 3 2 -1;
|
||||
2 4 1;
|
||||
%% 4 2 -1;
|
||||
3 4 1;
|
||||
%% 4 3 -1;
|
||||
4 5 1;
|
||||
%% 5 4 -1;
|
||||
5 6 1;
|
||||
%% 6 5 -1;
|
||||
1 4 1;
|
||||
%% 4 1 -1;
|
||||
5 2 1;
|
||||
%% 2 5 -1
|
||||
%% 6 3 1;
|
||||
%% 6 4 1;
|
||||
%% 6 2 1;
|
||||
%% 3 1 1;
|
||||
%% 3 5 1;
|
||||
3 2 1;
|
||||
3 1 1;
|
||||
];
|
||||
length_d = 3;
|
||||
length_d = .5;
|
||||
dRobot = [ 1;
|
||||
1;
|
||||
1;
|
||||
1;
|
||||
1;
|
||||
1;
|
||||
1;
|
||||
1;
|
||||
1.41;
|
||||
1.41;
|
||||
%% 2;
|
||||
%% 3.014;
|
||||
%% 3.014;
|
||||
%% 3.014;
|
||||
%% 3.014;
|
||||
];
|
||||
corRobot = [1.5; 1.7;
|
||||
2; 2.5;
|
||||
2.5; 2.8;
|
||||
2.5; 2;
|
||||
2.2; 1;
|
||||
1.5; 0.2;
|
||||
]*10; % xy xy xy
|
||||
]; % xy xy xy
|
||||
B = [
|
||||
1 0; 0 1;
|
||||
0 0; 0 0;
|
||||
0 0; 0 0;
|
||||
0 0; 0 0;
|
||||
0 0; 0 0;
|
||||
0 0; 0 0;
|
||||
];
|
||||
kp = 30;
|
||||
ki = 3;
|
||||
%% fvref = fncSpeedRef('ysin',100,2);
|
||||
%% fvref = fncSpeedRef('xsin',50,2);
|
||||
%% fvref = fncSpeedRef('cw',-100,1);
|
||||
fvref = fncSpeedRef('s',0,0);
|
||||
kp = 80;
|
||||
ki = 1;
|
||||
% fvref = fncSpeedRef('ysin',50,2);
|
||||
% fvref = fncSpeedRef('xsin',50,2);
|
||||
% fvref = fncSpeedRef('cw',-300,1);
|
||||
fvref = fncSpeedRef('s',50,50);
|
||||
fvrefans = fncSpeedRef('s',0,0);
|
||||
tspan = 1:0.1:5;
|
||||
tspan = 1:0.1:10;
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
|
||||
|
||||
[R,K,d] = rigidityMatrixFnc(conRobot);
|
||||
_zero = zeros(size(R(corRobot,K),1),1);
|
||||
sInit = [corRobot; _zero;];
|
||||
sAnsInit =[_zero; _zero;];
|
||||
s2ndInit = [corRobot; %x1
|
||||
zeros(length(corRobot),1); %x2
|
||||
zeros(length(corRobot),1); %xi1
|
||||
_zero]; %xi2
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
|
||||
printf("Mulai memecakan masalah \n\n");
|
||||
startExe = tic;
|
||||
|
||||
dydt = @(t, y) systm_robot(t, y,(dRobot*length_d), R,K, kp, ki, B, fvref(t));
|
||||
[t,y] = ode45(dydt, tspan, sInit);
|
||||
|
||||
dydt = @(t, y) systm_2nd_order_robot(t, y,(dRobot*length_d), R,K, kp,kp, ki,ki, B, fvref(t));
|
||||
[t,y] = ode45(dydt, tspan, s2ndInit);
|
||||
cntr = 1;
|
||||
bypassCntr = @(c) cntr;
|
||||
plusPlusCntr = @(c) cntr++;
|
||||
|
|
@ -133,7 +96,7 @@ function plot_con (pltRb, yOut, conIn,xm,ym, time)
|
|||
endfunction
|
||||
|
||||
plot_con(plot_rb, y', conRobot, xrb, yrb, 1);
|
||||
plot_con(plot_rb, y', conRobot, xrb, yrb, round(length(tspan)/2));
|
||||
% plot_con(plot_rb, y', conRobot, xrb, yrb, round(length(tspan)/2));
|
||||
plot_con(plot_rb, y', conRobot, xrb, yrb, length(tspan));
|
||||
|
||||
|
||||
|
|
@ -171,6 +134,7 @@ str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
|
|||
eval(str_tmp)
|
||||
title("Norm error setiap edge robot")
|
||||
|
||||
save DataOutMotion.data y
|
||||
save DataErrorEdge.data yans
|
||||
csvwrite("DataOutMotion.csv", [t' y'])
|
||||
% save DataOutMotion.data y
|
||||
% save DataErrorEdge.data yans
|
||||
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ function [R,K, d] = rigidityMatrixFnc (edgeL)
|
|||
endfor
|
||||
str = strcat(str,sprintf("errVec(%i:%i)' ));",i+2,i+3));
|
||||
% return sebagai fungsi R
|
||||
eval(str);
|
||||
eval(str); %% replicate from https://www.mathworks.com/help/matlab/ref/blkdiag.html
|
||||
R = @(x,k) errBlockDiagonal(edgeVector(x,k))*k;
|
||||
|
||||
endfunction
|
||||
|
|
|
|||
|
|
@ -0,0 +1,32 @@
|
|||
|
||||
%% systm_2nd_robot
|
||||
%% param :
|
||||
%% t -> time
|
||||
%% x -> state
|
||||
%% d -> distance antara robot nya
|
||||
%% R, K -> return dari fungsi rigidityMatrixFnc()
|
||||
%% kp -> konstanta proporsional
|
||||
%% ki -> konstanta integral
|
||||
%% B -> matrix selctor vref
|
||||
%% vref -> kecepatan refrensi
|
||||
function dxdt = systm_2nd_order_robot(t,x,d,R,K,kp1,kp2,ki1,ki2,B,vref)
|
||||
n = length(K(1,:))/2;
|
||||
l = length(R(ones(n*2,1),K));
|
||||
m = length(K(:,1))/2;
|
||||
dxdt = zeros(length(x),1);
|
||||
x1 = x(1:n*2);
|
||||
len = length(x1);
|
||||
x2 = x(len+1:len+length(x1));
|
||||
len = len + length(x2);
|
||||
x3 = x(len+1:len+length(x2));
|
||||
len = len + length(x3);
|
||||
x4 = x(len+1:length(x));
|
||||
|
||||
dxdt(1:n*2) = x2;
|
||||
len = length(x1);
|
||||
dxdt(len+1:len+length(x1)) = -kp1*x2 -kp2*R(x1,K)'*R(x1,K)*x1 +kp2*R(x1,K)'*d -x3 -R(x1,K)'*x4 + (B*vref);
|
||||
len = len + length(x2);
|
||||
dxdt(len+1:len+length(x2)) = ki1*x2;
|
||||
len = len + length(x3);
|
||||
dxdt(len+1:length(x)) = ki2*R(x1,K)*x1 -ki2*d;
|
||||
endfunction
|
||||
|
|
@ -1,3 +1,13 @@
|
|||
%% systm_robot
|
||||
%% param :
|
||||
%% t -> time
|
||||
%% x -> state
|
||||
%% d -> distance antara robot nya
|
||||
%% R, K -> return dari fungsi rigidityMatrixFnc()
|
||||
%% kp -> konstanta proporsional
|
||||
%% ki -> konstanta integral
|
||||
%% B -> matrix selctor vref
|
||||
%% vref -> kecepatan refrensi
|
||||
function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
|
||||
n = length(K(1,:))/2;
|
||||
l = length(R(ones(n*2,1),K));
|
||||
|
|
@ -5,14 +15,6 @@ function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
|
|||
dxdt = zeros(l,1);
|
||||
x1 = x(1:n*2);
|
||||
x2 = x(length(x1)+1:length(x));
|
||||
%% if (t > 5) && (t < 15)
|
||||
%% vref = [5; 10];
|
||||
%% else
|
||||
%% if (t > 15) && (t > 20)
|
||||
%% vref = [5; -10];
|
||||
%% endif
|
||||
%% endif
|
||||
%% dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
|
||||
dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
|
||||
dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*d);
|
||||
endfunction
|
||||
|
|
|
|||
Loading…
Reference in New Issue