-p-formation-control/BAB2/bab2.tex

\chapter{\babDua}\label{bab:dua}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

\section{Kendali Formasi Berdasarkan Jarak}
Pembahasan kendali formasi mutli-robot dikutip dari paper oleh \kutip{Rozenheck2015}.
Dimana peneliti membahas mengenai kendali formasi robot berdasarkan jaraknya lalu dikendalikan dengan kendali PI\@.
Dari subbab ini akan dirangkum dari paper tersebut, yaitu mulai dari pendahuluan sampai kendalinya.


\subsection{Pendahuluan Formasi Multi Robot}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

\subsubsection{Teori Graf}
Suatu graf $\graf$, dinotasikan sebagai $\graf=(\simpul, \sisi)$, merupakan pasangan $\simpul$ dan $\sisi$,
di mana $\simpul$ merupakan himpunan tak kosong berisikan simpul pada graf tersebut dan $\sisi$
merupakan himpunan sisi pada graf tersebut. Secara formal,
himpunan $\sisi$ dapat dinyatakan sebagai suatu koleksi subhimpunan berkardinalitas dua dari himpunan $\simpul$,
atau dalam notasi matematika $\sisi \subseteq \simpul \times \simpul $.
Sebuah $\graf$ diakatakan tak berarah (\textit{undirected graph}),
dimana himpunan sisi terdiri dari pasangan node $(i,j)$,
maka sisi tersebut tidak memiliki urutan arah antara node $i$ dengan $j$. Dinotasikan
$n \triangleq | \simpul |$ sebagai jumlah dari node dan $m \triangleq | \sisi |$ sebagai jumlah dari sisinya.
Apabila $(i,j) \in \sisi$ maka dapat disebut node $i$ dan $j$ berdekatan(\textit{adjecent}).
Himpunan dari node yang terhubung dari setiap simpul $i$ dinotasikan dengan $\tetangga_i \triangleq \{ j \in \simpul: (i,j) \in \sisi \}$, dan juga $i \sim j$.
Matiks insidensi (\textit{incidence}), $E \in \mathbb{R}^{n\times m}$, adalah matrik $\{0,\pm 1\}$ dimana
baris matrik mengindikasikan simpulnya dan kolomnya sebagai sisinya.
Matriks \textit{laplacian} didefinisikan dengan $L(\simpul)=EE^T$

\subsubsection{Teori Kekakuan Graf}
Koordinat multi dimensi adalah konfigurasi matrik vector yang terdisi dari beberapa koordinat node,
$p = \begin{bmatrix} x_1^T & \dots & x_n^T \end{bmatrix}^T \mathbb{R}^{2n}$, dimana
$x_i \in \mathbb{R}^2$ dan $x_i \neq x_j$ untuk semua $i \neq j$.
Difinisi sebuah kerangka (\textit{framework}), dinotasikan dengan $\graf(p)$,
adalah graf tak berarah $\graf$ dengan konfigurasi $x$, dimana simpul $i$ pada graf dipetakan
kedalam koordinat $x_i$. Misalkan $(i,j)\in \sisi$ sama dengan sisi ke $k$ dari graf langsung
dan mendefinisikan vektor sisi dari kerangka, atau dapat disebut sebagai vektor posisi relatif,
dengan $ e_k \triangleq x_j - x_i$. Untuk semua vektor sisi dapat dinotasikan dengan
$e=\begin{bmatrix}e_1^t & \dots & e_m^T\end{bmatrix} \in \mathbb{R}^{2m}$.

Apabila kerangka $\simpul(p)$ dengan vektor sisi $\{e_k\}_{k=1}^m$, maka didefinisisi fungsi sisi (\textit{edge function}), $F:\mathbb{R}^{2n} \times \simpul \rightarrow \mathbb{R}^m$
dengan
\begin{align}
  F(p,\sisi) & \triangleq
  \begin{bmatrix}
    ||e_1||^2 & \dots & ||e_m||^2
  \end{bmatrix}^T
\end{align}
Matrik kekakuan $R(p)$ yang berhubungan erat dengan kerangka $\graf(p)$ dapat didefinisikan
dengan \textit{Jacobian} dari fungsi sisi (\kutip{Rozenheck2015}),
\begin{align}
  R(p) & \triangleq \frac{\partial F(p,\graf)}{\partial x} \in \mathbb{R}^{m\times 2n} \nonumber \\
       & \triangleq diag(e_i^T)(E^T \otimes I_2)
\end{align}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

\subsection{Kendali Formasi Multi-Robot}
\label{subbab:KendaliFormasi}

Dikarenakan pada penelitian sebelumnya menggunakan model sederhana,
maka akan menggabungkan literatur oleh \kutip{Oh2014} dengan penyesuaian metode sebelumnya.

Pembahasan kendali dari formasi multi robot menggunakan gradient control.
Apabila $n(n\geq 2)$ dimodelkan sebagai titik yang memiliki masa jenis bergerak diatas
dimensi 2(\textit{Euclidean Space}), maka pergerakan dimodelkan dengan
\begin{align}
  \begin{cases}
    \dot{x}_i(t) = & v_i(t) \quad i = 1, \dots, n. \\
    \dot{v}_i(t) = & u_i(t)
  \end{cases}
  \label{eq:modelorde2}
\end{align}
dimana $v_i(t) \in \mathbb{R}^2$ dan $u_i(t)\in \mathbb{R}^2$adalah  kecepatan dan input dari robot-$i$. Dinotasikan $d \in \mathbb{R}^m$ adalah vector jarak dimana isi
dari matrik tersebut adalah $d_k^2$ yang mempresentasikan jarak yang dinginkan antara
setiap robot $i$ dan $j$ untuk sisi $(i,j)\in \sisi$.
Lalu didefinisi persamaan potensial yang memiliki hubungan antara jarak robot yang diinginkan
dengan jarak yang sekarang
\begin{align}
  \Phi(e) & = \frac{1}{2} \sum_{i \in \simpul }||v_i||^2 + \frac{1}{2} \sum_{k=1}^{m} \big( ||e_k||^2 - d_k^2 \big)^2
\end{align}
Pengamatan dilakukan agar $\Phi(e) =0$ jika dan hanya jika $||e_k||^2 = d_k^2,$   $\forall k = 1, \dots, m$.
Kendali dari setiap robot menggunakan gradien negatif dari fungsi potensial
\begin{align}
  \begin{cases}
    \dot{p} & = \frac{\partial \Phi(e)}{\partial v}   + Bv_{ref}                                         \\
            & = v(t) + Bv_{ref}                                                                          \\
    \dot{v} & = -C \Big( \frac{\partial \Phi(e)}{\partial v} + \frac{\partial \Phi(e)}{\partial p} \Big) \\
            & = -C(v(t) + R(p)^T(R(p)x(t) - d ))                                                         \\
            & = u(t)
  \end{cases}
  \label{eq:dynmState}
\end{align}
Dimana $v \in \mathbb{R}^{2n}$ adalah vector kecepatan dari seluruh node. Penambahan refrensi kecepatan pada salah satu robot dapat menjadikan formasi bermanuver.
Dimana $B \in \mathbb{R}^{2n \times 2}$ digunakan untuk indikasi robot ke $i$ sebagai leader atau penerima kecepatan referensinya, $v_{ref} \in \mathbb{R}^2$ sebagai kecepatan referensi,
dan $C$ adalah konstanta pengendali yang akan digantikan dengan algoritma kendali.
Dengan menerapkan kendali \textit{Proportional}, konstanta $C$ pada persamaan~\eqref{eq:kontrolinput}
dapat diubah dengan

\begin{align}
  \begin{cases}
    u_i(t)        & = u_{k_{p1}}(t) + u_{k_{p2}}(t)          \\
    u_{k_{p1}}(t) & = -k_{p1}v_i(t)                          \\
    u_{k_{p2}}(t) & = -R(p(t))^T k_{p2}(R(p(t))x_1(t) - d )) \\
  \end{cases}
  \label{eq:kontrolinput}
\end{align}
Dengan itu dapat digabungkan menjadi persamaan \textit{state-space} menggunakan persamaan~\eqref{eq:dynmState}

\begin{align}
  \begin{bmatrix}
    \dot{x} \\ \dot{v}
  \end{bmatrix} & =
  \begin{bmatrix}
    0                  & 1      \\
    -k_{p2}R(x)^T R(x) & -k_{p1}\\
  \end{bmatrix}
  \begin{bmatrix}
    x \\ v
  \end{bmatrix} +
  \begin{bmatrix}
    0 \\  k_{p2}R(x)^T
  \end{bmatrix} d+Bv_{ref} \nonumber              \\
  \dot{x}_f(t)               & = A_f(x)x_f(t)+B_f(x)d+Bv_{ref}
  \label{eq:ss-formasi}
\end{align}

\begin{figure}
  \centering
  \includegraphics[scale=.35]{BAB2/img/plotMotion3Robot.png}
  \caption{Plot motion 3 robot dengan nilai kp=80 dan ki=1}
\end{figure}

\section{Pemodelan Robot}

Pembahasan pemodelan robot akan dirangkum dari jurnal oleh \kutip{CORREIA20127}.

Robot menggunakan 3 aktuator penggerak dengan roda \textit{omniwheel}, sehingga robot dapat bergerak kesegala arah.
Pemasangan roda \textit{omniwheel} memiliki sudut $120^\circ$ terhadap roda lainnya.
Sehingga setiap roda memilki gaya dengan arah $90^\circ$ dari sudut pemasangannya.
Agar robot bergerak kesegala arah, ketiga aktuator harus dikendalikan untuk menghasilkan resultan gaya dengan arah yang diinginkan.
\begin{figure}
  \centering
  \begin{subfigure}[t]{.4\textwidth}
    %[hbt!]
    \centering
    \includegraphics[scale=.4]{BAB2/img/MDC_fig1.png}
    %% \caption{Geometri Robot (\kutip{CORREIA20127})}
    \caption{}
    % \label{fig:1}
  \end{subfigure}
  \begin{subfigure}[t]{.4\textwidth}
    \centering
    \includegraphics[scale=.5]{BAB2/img/ADNR_fig1.png}
    %% \caption{Grafik Gaya Robot}
    \caption{}
    % \label{fig:adnr1}
  \end{subfigure}
  \caption{(a) Geometri Robot (\kutipLs{CORREIA20127}) (b) Grafik Gaya Robot}
\end{figure}

Kinematika robot dapat dirumuskan menjadi
\begin{align}
  \dot{\textbf{x}}_p & = R^T(\theta)\dot{\textbf{x}}_r \label{eq:kinematika_robot}
\end{align}
dimana $R(\theta)$ adalah matrik rotasi ortogonal
\begin{align*}
  R(\theta) =
  \begin{bmatrix}
    \cos(\theta)  & \sin(\theta) & 0 \\
    -\sin(\theta) & \cos(\theta) & 0 \\
    0             & 0            & 1 \\
  \end{bmatrix}
\end{align*}
Koordinat robot dideskripsikan menggunakan vector $\textbf{x}_p = \begin{bmatrix} x_p & y_p & \theta \end{bmatrix}^T$,
dimana $x_p$ dan $y_p$ adalah titik pusat, $P$, pada frame robot dan $\theta_p$ adalah selisih sudut antara \textit{angular} frame global dengan robot.
Vector $\dot{\textbf{x}}_r = \begin{bmatrix} \dot{x}_r & \dot{y}_r & \dot{\theta}_r \end{bmatrix}^T$ mendeskripsikan variable kecepatan terhadap titik pusat, $P$,dimana $w$ sebagai kecepatan angular robot terhadap frame global.
Karena robot memiliki aktuator, maka kecepatan roda memiliki hubungan terhadap kecepatan robot, dengan kata lain
kecepatan pada titik pusat adalah sebuah fungsi dengan kecepatan roda sebagai parameternya.
Untuk mendapatkan persamaan tersebut, maka dapat dianalisis dengan hukum \textit{Power}.
Apabila didefinisi hubungan antara gaya resultan robot dengan gaya yang dihasilkan roda
\begin{align*}
  F_{\dot{x}_r} & = F_{w1}(t)\cos({90^\circ})+ F_{w2}(t) \cos({30^\circ})+ -F_{w3}(t) \cos({30^\circ}) \\
  F_{\dot{y}_r} & = -F_{w1}(t) + F_{w2}(t) \cos({60^\circ})+ F_{w3}(t) \cos({60^{\circ}})              \\
  \Gamma        & = d.F_{w1}(t) + d.F_{w2}(t) + d.F_{w3}(t)
\end{align*}
dimana $d$ adalah jarak dari titik $P$ ke lokasi roda, maka akan didapat matriks geometri
antara $F_R = \begin{bmatrix} F_{\dot{x}_r} & F_{\dot{y}_r} & \Gamma \end{bmatrix}^T$ dengan
$F_w = \begin{bmatrix} F_{w1} & F_{w2} & F_{w3} \end{bmatrix}^T$
\begin{align}
  F_R & = \begin{bmatrix}
    0  & \frac{\sqrt{3}}{2} & -\frac{\sqrt{3}}{2} \\
    -1 & \frac{1}{2}        & \frac{1}{2}         \\
    l  & l                  & l
  \end{bmatrix}F_W \nonumber \\
  F_R & = AF_w \label{eq:gaya_robot}
\end{align}
Dalam kasus robot, \textit{power} yang dihasilkan oleh setiap roda sama dengan \textit{power} dari robot itu sendiri
(\kutipLs{Hacene2019}).
Dengan menggunakan persamaan~\eqref{eq:gaya_robot} akan menghasilkan persamaan kinematika robot menggunakan 3 roda
\textit{omniwheel}  \begin{align}
  P_{w}              & = P_{R} \nonumber                                 \\
  F_w^T\dot{x}_w     & = F_R^T\dot{\textbf{x}}_r \nonumber               \\
  F_w^T\dot{x}_w     & = {(A.F_w)}^T\dot{\textbf{x}}_r \nonumber         \\
  \dot{x}_w          & = A^T\dot{\textbf{x}}_r \nonumber                 \\
  \dot{\textbf{x}}_r & = {(A^T)}^{-1}\dot{x}_w\label{eq:kecepatan_robot}
\end{align}
dengan mensubtitusi persamaan~\eqref{eq:kecepatan_robot} pada~\eqref{eq:kinematika_robot}
\begin{align}
  \dot{\textbf{x}}_p & = R^T{(\theta)}{(A^T)}^{-1}\dot{x}_w
\end{align}

Pergerakan robot juga dideskripsikan secara dinamika menggunakan hukum pergerakan dari \textit{Newton}.
\begin{align}
  F_{\dot{x}_r}(t) - B_{\dot{x}_r}\dot{x}_r(t) - C_{\dot{x}_r}sgn(\dot{x}_r(t))         & = M\ddot{x}_r(t)    \\
  F_{\dot{y}_r}(t) - B_{\dot{y}_r}\dot{y}_r(t) - C_{\dot{y}_r}sgn(\dot{y}_r(t))         & = M\ddot{y}_r(t)    \\
  \Gamma(t) - B_{\dot{\theta}}\dot{\theta}(t)  - C_{\dot{\theta} }sgn(\dot{\theta}(t) ) & = I\ddot{\theta}(t)
\end{align}
Dimana $B_i$ adalah \textit{viscous firctions} yang mempresentasikan perbandingan terbalik dari gaya yang
bersifat linier terhadap gaya dorong dan kecepatan robot. $C_i.sgn(\dot{i})$ adalah \textit{coulumb frictions}
yang mempresentasikan perbandingan terbalik terhadap perubahan kecepatan, dimana tanda bilangan berubah kebalikan
dari kecepatannya.
\begin{align*}
  sgn(\alpha) = \begin{cases}
    1,  & \alpha >0  \\
    0,  & \alpha = 0 \\
    -1, & \alpha < 0
  \end{cases}
\end{align*}
seperti pada persamaan~\eqref{eq:gaya_robot}, resultan gaya robot berhubungan dengan gaya roda.
Maka gaya roda dapat dideskripsikan dengan menghubungkan antara gaya yang dihasilkan oleh motor
\begin{align}
  F_{wi} & = \frac{\tau_i(t)}{r_i}
\end{align}
dimana $\tau_i(t)$ adalah torsi dari motor
\begin{align}
  \tau_i(t) = l_i K_{ti} i_{ai}(t)
\end{align}
Untuk mendapatkan persamaan $i_{ai}(t)$, dapat digunakan deskripsi persamaan dinamika motor
\begin{align}
  u_i(t) = L_{ai}\frac{di_{ai}(t)}{dt} + R_{ai}i_{ai}(t) + K_{vi}w_{mi} \label{eq:dyn_motor}
\end{align}
dimana $L_{ai}$ dan $R_{ai}$ adalah Induktasi dan resistansi armature motornya.
$K_{vi}$ adalah konstanta torsi motor dimana dalam satuan SI yang sama dengan $K_v$.
Dalam praktiknya apabila motor dalam kecepatan \textit{stady state} maka $\frac{di_{ai}}{dt}$
bernilai kecil, dan dalam persamaan~\eqref{eq:dyn_motor} nilai induktansi dapat diabaikan.

Penjabaran dinamika robot bisa diubah dalam bentuk \textit{state-space}
\begin{align}
  \dot{x}(t) & = A_r x(t) + B_r u(t) + ksgn(x(t)) \label{eq:ss1} \\
  y(t)       & = Cx(t) \label{eq:ss2}
\end{align}
dimana vektor \textit{state} adalah $x(t) = \begin{bmatrix} x_p & y_p & \theta & \dot{x}_r & \dot{y}_r & \dot{\theta}_r \end{bmatrix}^T$, dan
vektor output $y(t) = \begin{bmatrix} x_p & y_p & \theta \end{bmatrix}^T$.
Dimana $l = l_{1 \dots 3}, r = r_{1 \dots 3} R_a = R_{a1 \dots 3}$ and $K_t = K_{t1 \dots 3}$, maka didapat matriks yang dapat mendeskripsikan
sistem robot
\begin{align*}
  A_r & = \begin{bmatrix}
    0 & 0 & 0 & 1                                                        & 0                                                        & 0                                                             \\
    0 & 0 & 0 & 0                                                        & 1                                                        & 0                                                             \\
    0 & 0 & 0 & 0                                                        & 0                                                        & 1                                                             \\
    0 & 0 & 0 & -\frac{3.l^2.K_t^2}{2.M.R_a.r^2}-\frac{B_{\dot{x}_r}}{M} & 0                                                        & 0                                                             \\
    0 & 0 & 0 & 0                                                        & -\frac{3.l^2.K_t^2}{2.M.R_a.r^2}-\frac{B_{\dot{y}_r}}{M} & 0                                                             \\
    0 & 0 & 0 & 0                                                        & 0                                                        & -\frac{3.l^2.K_t^2}{2.I.R_a.r^2}-\frac{B_{\dot{\theta}_r}}{I} \\
  \end{bmatrix}, \\
  B_r & = \begin{bmatrix}
    0                                & 0                                             & 0                                             & \\
    0                                & 0                                             & 0                                             & \\
    0                                & 0                                             & 0                                             & \\
    0                                & \frac{l.K_t}{R_a.r}. \frac{\cos(30^\circ)}{M} & -\frac{l.K_t}{R_a.r}.\frac{\cos(30^\circ)}{M}   \\
    \frac{l.K_t}{R_a.r}.\frac{-1}{M} & \frac{l.K_t}{R_a.r}.\frac{\cos(60^\circ)}{M}  & \frac{l.K_t}{R_a.r}.\frac{\cos(60^\circ)}{M}    \\
    \frac{l.K_t}{R_a.r}.\frac{b}{I}  & \frac{l.K_t}{R_a.r}.\frac{b}{I}               & \frac{l.K_t}{R_a.r}.\frac{b}{I}                 \\
  \end{bmatrix},
  k = \begin{bmatrix}
    0 & 0 & 0 & 0                        & 0                        & 0                        & \\
    0 & 0 & 0 & 0                        & 0                        & 0                        & \\
    0 & 0 & 0 & 0                        & 0                        & 0                        & \\
    0 & 0 & 0 & -\frac{C_{\dot{x}_r}}{M} & 0                        & 0                          \\
    0 & 0 & 0 & 0                        & -\frac{C_{\dot{x}_r}}{M} & 0                          \\
    0 & 0 & 0 & 0                        & 0                        & -\frac{C_{\dot{x}_r}}{M}   \\
  \end{bmatrix},     \\
  C   & = \begin{bmatrix}
    1 & 0 & 0 & 0 & 0 & 0 & \\
    0 & 1 & 0 & 0 & 0 & 0 & \\
    0 & 0 & 1 & 0 & 0 & 0 & \\
    0 & 0 & 0 & 1 & 0 & 0 & \\
    0 & 0 & 0 & 0 & 1 & 0 & \\
    0 & 0 & 0 & 0 & 0 & 1 & \\
  \end{bmatrix}
\end{align*}