diff --git a/DOC/RP_2015_Rozenheck.pdf b/DOC/RP_2015_Rozenheck.pdf index ed543ef..038c105 100644 Binary files a/DOC/RP_2015_Rozenheck.pdf and b/DOC/RP_2015_Rozenheck.pdf differ diff --git a/DOC/_[2015][Rozenheck][A PI Controler Distance-based Formation Tracking].pdf b/DOC/_[2015][Rozenheck][A PI Controler Distance-based Formation Tracking].pdf index d5efc36..16ddde2 100644 Binary files a/DOC/_[2015][Rozenheck][A PI Controler Distance-based Formation Tracking].pdf and b/DOC/_[2015][Rozenheck][A PI Controler Distance-based Formation Tracking].pdf differ diff --git a/SOURCE/IMG/fig2.png b/SOURCE/IMG/fig2.png new file mode 100644 index 0000000..0a2d60f Binary files /dev/null and b/SOURCE/IMG/fig2.png differ diff --git a/SOURCE/RP_2015_Rozenheck.tex b/SOURCE/RP_2015_Rozenheck.tex index 79aaf1e..88c96d0 100644 --- a/SOURCE/RP_2015_Rozenheck.tex +++ b/SOURCE/RP_2015_Rozenheck.tex @@ -29,7 +29,7 @@ % ASSIGNMENT INFORMATION %---------------------------------------------------------------------------------------- -\title{Controlling a Triangular Formation of Mobile Agent} % Title of the assignment +\title{Kendali Proposional-Integral Pada Pelacakan Formasi Berdasarkan Jarak} % Title of the assignment \author{Anggoro Dwi Nur Rohman\\ \texttt{anggoro\_dwi@student.ub.ac.id}} % Author name and email address @@ -38,42 +38,120 @@ %---------------------------------------------------------------------------------------- \begin{document} - \maketitle % Print the title - \section*{Pendahuluan} -Akan dirangkum dari penelitian +Rangkuman ini ditujukan untuk memahami dan review jurnal. Judul dalam bahasa inggris +\textit{A Proportional-Integral Controller for Distance-Based Formation Tacking} yang +diteliti oleh Oshri Rozenheck dari Israel Institute of Technology, Haifa, Israel. +Peneliti menerangkan pada jurnal ini tentang permasalahan kendali formasi pada +multi-agent apabila pada salah satu agent nya diberikan kecepatan tambahan sebagai +refrensi. -\section{Formasi Segitiga} +%% \begin{table} +\section{Notasi-Notasi} + %% \caption{Notasi-notasi} + \begin{tabular}{| l | l |} + \hline + Notasi & Keterangan \\ \hline + $n \triangleq |V|$ & ... \\ \hline -Robot ditandai dengan 1,2,3. Apabila robot 1 mengikuti robot 2 maka dinotasikan dengan $[1] = 2$. jarak antara $i$ dan $[i]$ dinotasikan $d_i$. + $m \triangleq |\varepsilon| $ & .. \\ \hline -Koordinat vector dari agent $i$ dinotasikan dengan $x_i$ terhadap global koordinat yang fiks, dan $y_{ij}$ adalah posisi robot $j$ terhadap sistem koordinat dari $i$ yang telah tentukan. Apabila $R_i$ dan $\tau_i$ adalah matriks rotasi dan vector translasi maka $y_{ij} = R_ix_j + \tau_i, j \in \{1,2,3\}$. Penelitian ini menggunakan kinematik yang sedarhana -\begin{eqnarray*} - \dot{y}_{ii} &=& u_i \quad i \in \{1,2,3\} \\ - \dot{x}_{i} &=& R_i^{-1} u_i -\end{eqnarray*} + $A_1, \dots, A_n \in \mathbb{R}^{p\times q}$ & ... \\ \hline -\section*{Referensi} -%% \printbibliography + $x = \begin{bmatrix} x_1^T \\ \dots\\ x_n^T \end{bmatrix} \in \mathbb{R}^{2n}$ & + Konfigurasi titik \\ + $x_i \in \mathbb{R}^n$ & \\ + $ x_i \neq x_j ; \forall i \neq j$ & \\ \hline -%% \begin{refsection} -%% @INPROCEEDINGS{Cao2007, -%% author={M. {Cao} and A. S. {Morse} and C. {Yu} and B. D. O. {Anderson} and S. {Dasguvta}}, -%% booktitle={2007 46th IEEE Conference on Decision and Control}, -%% title={Controlling a triangular formation of mobile autonomous agents}, -%% year={2007}, -%% volume={}, -%% number={}, -%% pages={3603-3608}, -%% abstract={This paper proposes a distributed control law for maintaining a triangular formation in the plane consisting of three mobile autonomous agents. It is shown that the control law can cause any initially non-collinear, positively-oriented {resp. negatively-oriented} triangular formation to converge exponentially fast to a desired positively-oriented {resp. negatively- oriented} triangular formation. It is also shown that there is a thin set of initially collinear formations which remain collinear and may drift off to infinity as t rarr infin. These findings complement and extend earlier findings cited below.}, -%% keywords={distributed control;mobile robots;multi-robot systems;spatial variables control;triangular formation;mobile autonomous agents;collinear formations;distributed control law;Autonomous agents;USA Councils;Distributed control;H infinity control;Differential equations;Information technology;Art;Australia Council}, -%% doi={10.1109/CDC.2007.4434757}, -%% ISSN={0191-2216}, -%% month={Dec},} -%% \end{refsection} + $e_k \triangleq x_j - x_i$ & Relative position vector \\ \hline + $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 + \end{tabular} +%% \end{table} +\section{ Edge Function} +Diketahui framework $G(x)$ dengan vector edge ${e}_{k=1}^m$, +$F : \mathbb{R}^{2n} \times G \rightarrow \mathbb{R}^m$, maka dapat didefinisi +\begin{equation} + F(x,G) \triangleq = \begin{bmatrix}|e_1||^2 \\ \vdots \\ ||e_m||^2 \end{bmatrix} +\end{equation} +Peneliti mendefinisi \textit{rigidity matrix R(x)} sebagai fungsi jacobian dari +Edge function, +\begin{equation*} + R(x) = \frac{\partial F(x,G)}{\partial x} \in \mathbb{R}^{m \times 2n} +\end{equation*} +dan menyingkat perhitungannya dan menghasilkan persamaan +\begin{equation} + R(x) = diag(e_i^T)(E^T \otimes I_2) +\end{equation} + +\section{Kendali} + +Error jarak dinotasikan dengan $\delta \in \mathbb{R}^m$ dimana +\begin{equation} + \delta_k = || e_k ||^2 - d_k^2, k \in \{1, \dots, m\} +\end{equation} + +Peneliti menggunakan fungsi potensial +\begin{equation} + \Phi(e) = \frac{1}{2} \sum_{k-1}^{m} \Big( || e_k ||^2 - d_k^2 \Big)^2 = + \frac{1}{2} \sum_{k=1}^{m} \delta_k^2 +\end{equation} +digunakan untuk kendali pada setiap robot dengan cara negative gradient +dari fungsi potensial. +\begin{equation} + u_i(t) = - \Big(\frac{\partial \Phi(e)}{\partial x_i} \Big) = + - \sum_{j ~ o} \Big( || e_k ||^2 - d_k^2 \Big) e_k +\end{equation} +Apabila ditulis ulang dalam bentuk state-space +\begin{equation} + \dot{x}(t) = - R(x)^T R(x) x(t) + R(x)^Td +\end{equation} + +\begin{figure} + \centering + \includegraphics[scale=.7]{./IMG/fig2.png} + \caption{Skema General} + \label{f1} +\end{figure} + +Menggunakan skema seperti pada gambar.\ref{f1} maka dapat diperoleh +\begin{eqnarray} + \dot{x} &=& u(t) + B.v_{ref} \\ + u(t) &=& -R(x)^T.C.\Big(R(x).x(t) - d \Big) \label{controller} +\end{eqnarray} +dan cikal bakal $C$ sebagai controller. + +dengan menerapkan Proportional-integrator pada persamaan \eqref{controller} +maka diperoleh +\begin{equation} + u(t) = -R(x)^T k_p ( R(x).x(T) - d ) - + R(x)^T.K_i \int_0^T (R(x).x(\tau) - d) d \tau +\end{equation} +dimana $kp_p$ dan $k_i$ adalah konstanta. + +Pada bagian integrator menghasilkan state baru +\begin{equation} + \dot{\xi} = k_i(R(x).x(t) -d) +\end{equation} +lalu kombinasi persamaan tersebut diperoleh state- space close loop + +\begin{equation} + \begin{bmatrix} \dot{x}(t) \\ \dot{\xi(t)} \end{bmatrix} = + \begin{bmatrix} -k_p.R(x)^T.R(x) & -R(x)^T \\ k_i.R(x) & 0 \end{bmatrix} + \begin{bmatrix} x(t) \\ \xi(t) \end{bmatrix} + + \begin{bmatrix} k_p.R(x)^T \\ -k_i.I \end{bmatrix}.d + + \begin{bmatrix} B \\ 0 \end{bmatrix}.v_{ref} +\end{equation} + +\subsection{Implementasi} + +Pada seksi implementasi ini akan dibahas mengenai code yang digunakan +untuk mensimulasi persamaan yang diciptakan oleh peneliti. \end{document} diff --git a/SOURCE/SM_2015_Rozenheck.m b/SOURCE/SM_2015_Rozenheck.m new file mode 100644 index 0000000..b7b110f --- /dev/null +++ b/SOURCE/SM_2015_Rozenheck.m @@ -0,0 +1,92 @@ +clear -all +close all +%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1; 5 3 1; 5 1 1;6 4 1; 6 2 1; 5 6 1]; +conRobot = [1 2 1; + 1 4 1; + 1 5 1; + 1 6 1; + 2 3 1; + 2 4 1; + 3 4 1; + 4 5 1; + 5 6 1;]; +%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1;]; + +%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1; -1; -1; 0; -1; ]; % xy xy xy +corRobot = [2; 2.5; + 2.7; 2.6; + 3; 2.3; + 2.5; 1.5; + 1.8; 1.5; + 1.5; 2; + ]; % xy xy xy +%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1;]; % xy xy xy +d = 3; +%% figure(1) +[R,K] = rigidityMatrixFnc(conRobot); +vref = [-5; 5]; +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 = 2; +ki = 3; + +sInit = [corRobot; zeros(size(R(corRobot,K),1),1)]; +tspan = linspace(1,30); +dydt = @(t, y) systm_robot(t, y,d, R,K, kp, ki, B, vref); + +[t,y] = ode45(dydt, tspan, sInit); +%% figure(2) +str_tmp = "plot("; +for i = 1:length(corRobot)-1 + str_tmp = strcat(str_tmp,sprintf("y'(%i,:),",i)); +endfor +str_tmp = strcat(str_tmp,sprintf("y'(%i,:));",i+1)); +eval(str_tmp); +hold on +str_tmp = " plot_rb = @(t) plot( ["; +for i = 1:2:length(corRobot) + str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i)); +endfor +str_tmp = strcat( str_tmp, sprintf("], [")); +for i = 2:2:length(corRobot) + str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i)); +endfor +str_tmp = strcat( str_tmp, sprintf("], \"^\");")); +eval(str_tmp) + +xrb = 1:2:length(corRobot); +yrb = 2:2:length(corRobot); +t = 1; +plot_rb(t); +for i = 1:length(conRobot) + plot([y'(xrb(conRobot(i, 1)),t), y'(xrb(conRobot(i, 2)),t)], + [y'(yrb(conRobot(i,1)),t), y'(yrb(conRobot(i,2)),t)], "r" ) +endfor +t = 20; +plot_rb(t); +for i = 1:length(conRobot) + plot([y'(xrb(conRobot(i, 1)),t), y'(xrb(conRobot(i, 2)),t)], + [y'(yrb(conRobot(i,1)),t), y'(yrb(conRobot(i,2)),t)], "r" ) +endfor + +t = 40; +plot_rb(t); +for i = 1:length(conRobot) + plot([y'(xrb(conRobot(i, 1)),t), y'(xrb(conRobot(i, 2)),t)], + [y'(yrb(conRobot(i,1)),t), y'(yrb(conRobot(i,2)),t)], "r" ) +endfor + +t = 80; +plot_rb(t); +for i = 1:length(conRobot) + plot([y'(xrb(conRobot(i, 1)),t), y'(xrb(conRobot(i, 2)),t)], + [y'(yrb(conRobot(i,1)),t), y'(yrb(conRobot(i,2)),t)], "r" ) +endfor + + +%% plot_rb(20); +%% plot_rb(40); +%% plot_rb(60); +%% plot_rb(80); +%% plot_rb(100); + + diff --git a/SOURCE/example.m b/SOURCE/example.m deleted file mode 100644 index 4ac7d34..0000000 --- a/SOURCE/example.m +++ /dev/null @@ -1,18 +0,0 @@ -clear -all -conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1]; -corRobot = [0; 0; 1; 0; 2; 2; 1; 1; ]; % xy xy xy -d = 3; -figure(1) -[R,K] = rigidityMatrixFnc(conRobot); -vref = 5; -B = [1;1; 0;0; 0;0; 0; 0;]; -kp = 2; -ki = 10; - -sInit = [corRobot; 0;0;0;0;0;]; -tspan = linspace(1,10); -dydt = @(t, y) systm_robot(t, y,d, R,K, kp, ki, B, vref); - -[t,y] = ode45(dydt, tspan, sInit); -figure(2) -plot(y'(1,:),y'(2,:),y'(3,:),y'(4,:),y'(5,:),y'(6,:),y'(7,:),y'(8,:)) diff --git a/SOURCE/octave-workspace b/SOURCE/octave-workspace index 3b4567f..a553d0d 100644 Binary files a/SOURCE/octave-workspace and b/SOURCE/octave-workspace differ diff --git a/SOURCE/rigidityMatrixFnc.m b/SOURCE/rigidityMatrixFnc.m index 896a21c..faeae22 100644 --- a/SOURCE/rigidityMatrixFnc.m +++ b/SOURCE/rigidityMatrixFnc.m @@ -6,7 +6,7 @@ function [R,K] = rigidityMatrixFnc (edgeL) % generasi koneksi bentuk incidence tmp = edgeL2adj(edgeL); gInc = adj2inc(tmp); - drawCircGraph(tmp) + %% drawCircGraph(tmp) tmp = getNodes(gInc,'adjacency') % generasi vector error menggunakan matrix % koneksi diff --git a/SOURCE/systm_robot.m b/SOURCE/systm_robot.m index f5b348f..3d44082 100644 --- a/SOURCE/systm_robot.m +++ b/SOURCE/systm_robot.m @@ -5,10 +5,13 @@ 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 - B = B * 0; - endif + %% 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)')*(ones(size(R(x1,K)',2),1)*d))+(B*vref); - %% ki*R(x1,K)*x1-(ki*ones(m,1))*d; dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*ones(m,1))*d; endfunction