\section{Metode}

\subsection{Model Robot}

Berikut adalah model dari robot holonomic dalam bentuk \textit{state-space} \kutip{CORREIA20127}.
Dimana robot menggunakan tiga buah motor yang dihubungkan pada \textit{omniwheel} sehingga robot
dapat bergerak kesegala arah.
\begin{align}
  \dot{x}(t) & = A_r x(t) + B_r u(t) + K_rsgn(x(t)) \label{eq:ss1} \\
  y(t)       & = Cx(t) \label{eq:ss2}
\end{align}
Vector
$u(t) = \begin{bmatrix}
    u_1(t) & u_1(t) & u(2)(t)
  \end{bmatrix}^T$
adalah masukan model bersatuan $volt$ dengan batasan $-6 \leq u_i(t) \leq 6$ pada tegangan motor robot.
Vector $y(t) = x(t) = \begin{bmatrix}
    v(t) & v_n(t) & w(t)
  \end{bmatrix}^T$
adalah kecepatan robot yang akan diperoleh dari sensor percepatan,
dimana $v(t)$ adalah kecepatan pada sumbu $x$,
$v_n(t)$ adalah kecepatan pada sumbu $y$, dan
$w(t)$ adalah kecepatan rotasi dari frame robot.
Matrix $A_r \in \mathbb{R}^{3\times 3}$ dan $B_r \in \mathbb{R}^{3 \times 3}$ adalah parameter fisik robot berdasarkan
yang diperoleh dari identifikasi secara persamaan fisika.
Matrix $K_r \in \mathbb{R}^{3 \times 3}$ adalah parameter \textit{friction} dari robot yang diestimasi dari
hasil percobaan.

\subsection{Kendali Robot Holonomic}

Kendali dari robot akan menggunakan dua mode \textit{state-feedback}.
\textbf{Mode satu}, bertujuan untuk mencapai kecepatan robot yang diinginkan.
Untuk mencapai tujuan tersebut akan menggunakan persamaan kendali sebagai berikut
\begin{equation}
  \begin{split}
    u_{c1}(t) &= -K^c_1 x(t) + N^c_1 r^c_1 \\
    r^c_1 &= \begin{bmatrix}
      v^*(t) & v_n^*(t) & w^*(t)
    \end{bmatrix}^T
  \end{split}
  \label{eq:kendali_kecepatan}
\end{equation}
Dimana $r^c_1$ , $K^c_1 \in \mathbb{R} ^ {3 \times 3}$ dan $N^c_1 \in \mathbb{R} ^ {3 \times 3}$
adalah setpoint kendali mode satu, konstanta yang diperoleh dari optimasi
persamaan \textit{Riccati} terhadap matrix $A_r$ dan $B_r$,
dan kostanta yang diperoleh dari \textit{inverse state-space} pada keadaan \textit{steady state}.
\begin{equation*}
  N^c_1  =  -[ C(A_r - B_rK^c_1)^{-1}B_r ] ^{-1}
\end{equation*}
Dengan menggabungkan persamaan~\eqref{eq:ss1} dengan persamaan~\eqref{eq:kendali_kecepatan}
akan mendapatkan persamaan \textit{state-space} robot yang baru
\begin{align}
  \begin{split}
    \dot{x}(t) & = (A_r - B_r K^c_1) x(t)+B_r N^c_1 r^c_1  + K_r sgn(x(t)) \\
    & = A_c x(t) + B_c r^c_1 + K_r sgn(x(t))
  \end{split}
  \label{eq:ss_kendali_kecepatan}
\end{align}
\textbf{Mode dua}, bertujuan untuk mencapai titik koordinat tertentu
berdasarkan koordinat frame robot. Untuk mencapai tujuan tersebut
akan dimodifikasi persamaan~\eqref{eq:ss1} dengan menambah state koordinat frame robot
menjadi $y_{c2}(t) = x_{c2}(t) = \begin{bmatrix}
    x_r(t) & y_r(t) & \theta_r(t) & v(t) & v_n(t) & w(t)
  \end{bmatrix}^T$, $
  A_{c2} = \begin{bmatrix}
    0 & I   \\
    0 & A_r \\
  \end{bmatrix} \in \mathbb{R} ^ {6 \times 6}
$,
$B_{c2}    = \begin{bmatrix}
    0 \\ B_r
  \end{bmatrix} \in \mathbb{R} ^ {6 \times 3} $,
$K_{c2}(x) = \begin{bmatrix}
    0 & 0         \\
    0 & K_rsgn(x) \\
  \end{bmatrix}  \in \mathbb{R} ^ {6 \times 3} $.
Berikut adalah state space model untuk kendali mode satu.
\begin{equation*}
  \begin{split}
    \dot{x}_{c2}(t) & = A_{c2} x_{c2}(t) + B_{c2} u_{c2}(t) + K_{c2}(x_{c2}(t)) \\
    y_{c2}(t) & = C_{c2} x_{c2}(t) \\
  \end{split}
\end{equation*}
Dimana $u_{c2}(t)$ adalah persamaan kendali mode dua.
\begin{equation}
  \begin{split}
    u_{c2} (t) &= -K^c_2 x_{c2}(t) + N^c_2 r^c_2                      \\
    r^c_2 &= \begin{bmatrix}
      x_r^*(t) & y_r^*(t) & \theta_r^*(t) & v^*(t) & v_n^*(t) & w^*(t)
    \end{bmatrix}^T
  \end{split}
\end{equation}
Dimana $r^c_2$, $K^c_2 \in \mathbb{R} ^ {3 \times 6}$ dan $N^c_2 \in \mathbb{R} ^ {3 \times 6}$
adalah setpoint kendali mode dua dan kostanta yang diperoleh dari cara yang sama pada mode satu
menggunakan matrix model yang dimodifikasi.

\subsection{Kendali Formasi Berdasarkan Jarak}
Dinotasikan $n \triangleq | \simpul |$ sebagai jumlah dari node
dan $m \triangleq | \sisi |$ sebagai jumlah dari sisinya.
Dinotasikan $p = \begin{bmatrix} x_1^T & \dots & x_n^T \end{bmatrix}^T \in \mathbb{R}^{3n}$,
dimana $x_i \in \mathbb{R}^3$ dan $x_i \neq x_j$ untuk semua $i \neq j$.
Dinotasikan vektor posisi relatif $ e_k \triangleq x_j - x_i$ dan semua vector sisi
$e=\begin{bmatrix}e_1^t & \dots & e_m^T\end{bmatrix} \in \mathbb{R}^{3m}$.
didefinisikan fungsi \textit{Jacobian} sisi \kutip{Rozenheck2015}),
\begin{align}
  R(p) & \triangleq diag(e_i^T)(E^T \otimes I_2) \in \mathbb{R}^{m\times 3n} \nonumber \\
\end{align}
Dimana $E \in \mathbb{R}^{n\times m}$, adalah matrik \textit{incidence} $\{0,\pm 1\}$ dimana
baris matrik mengindikasikan simpulnya dan kolomnya sebagai sisinya dan
$diag(A_i) \triangleq blkdiag\{A_1, \dots, A_n\} \in \mathbb{R}^{np \times nq}$
Orde kendali formasi yang digunakan adalah orde dua
Mengadopsi persamaan potensial, didefinisi persamaan \kutip{Oh2014}.
\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}
Lalu kendali formasi menggunakan persamaan~\eqref{eq:ss_kendali_kecepatan} sebagai modelnya
diberikan \textit{negative gradient} dari fungsi potensial dan
konstanta \textit{proportional} \kutip{Rozenheck2015}
\begin{equation}
  \begin{split}
    \dot{p} & = A_f p(t) + B_f\frac{\partial \Phi(e)}{\partial v}                                            \\
    & =  A_f p(t) + B_fv(t)                                                                           \\
    \dot{v} & = -C \Big( \frac{\partial \Phi(e)}{\partial v} + \frac{\partial \Phi(e)}{\partial p} \Big) \\
    & = -k_{p1}v(t) + R(p(t))^Tk_{p2}(R(p(t))p(t) - d ))                                                         \\
  \end{split}
  \label{eq:dynmState}
\end{equation}
Dimana $A_f \in \mathbb{R}^{3n \times 3n}$ dan $B_f \in \mathbb{R}^{3n \times 3n}$ adalah 
matrix diagonal dari $A_c$ dan $B_c$.

\subsection{Algoritma Cosinus}
Robot $B_i \in \tetangga_A$, adalah tetangga dari robot $A$ .
$d_i[k]$ adalah jarak yang diperoleh dari sensor.
Dalam strategi (Gambar~\ref{fig:strategiPenentuanKoordinat}) ini diperlukan perpindahan robot $A$ ke $ A' = (0, l_a)$.
Perpindahan tersebut akan menghasilkan jarak $d_i[k+1]$.
Dari perbedaan tersebut akan didapatkan sudut $\alpha_i^\circ$.
\begin{align}
  \alpha_i^\circ & = 180^\circ \pm cos^{-1}\Bigg( \frac{l_a^2 + d_i[k+1]^2 -d_i[k]^2}{2 d_i[k+1] l_a} \Bigg).\nonumber \\
                 & = 180^\circ \pm \zeta_i^a
  \label{eq:algo_getAngle}
\end{align}
Dari persamaan~\eqref{eq:algo_getAngle} akan didapat koordinat tetangga.
\begin{align}
  x_{B_i}^A = \begin{bmatrix}
    x_{B_i} = d_i[k+1]\cos \alpha_i^\circ \\
    y_{B_i} = d_i[k+1]\sin \alpha_i^\circ
  \end{bmatrix}
  \label{eq:algo_koordinat_tetangga}
\end{align}
Untuk memvalidasi apakah koordinat telah sesuai dapat menggunakan nilai jarak pada sensor
dibandingkan dengan jarak dari hasil koordinat persamaan~\eqref{eq:algo_koordinat_tetangga}.
Akan tetapi hasil validasi tersebut akan mengalami bias dikarenakan
sudut $\zeta_i^a$ adalah sudut segitiga  $\angle{AA'B_1}$ atau $\angle{AA'B_2}$.
Oleh karena itu pada persamaan~\eqref{eq:algo_getAngle} terdapat operasi $\pm$
dimana operasi tersebut akan dilakukan berdasarkan letak kuadran $B_i$.
\begin{align}
  \alpha_i^\circ & =
  \begin{cases}
    180^\circ - \zeta_i^a, & \text{Kuadran \RN{1},\RN{2}} \\
    180^\circ + \zeta_i^a, & \text{Kuadran \RN{3},\RN{4}} \\
  \end{cases}.
  \label{eq:algo_getAngle2}
\end{align}
Diperlukan satu langkah lagi untuk menentukan kejadian pada
persamaan~\eqref{eq:algo_getAngle2}(Gambar~\ref{fig:strategiPenentuanKoordinat2}).
Langkah 2 adalah langkah pengujian dari hasil koordinat yang telah dikalkulasi
dan membandingkannya dengan nilai yang didapat dari sensor.
Apabila terdapat perbedaan maka kejadian pada persamaan~\eqref{eq:algo_getAngle2}
diubah ke kejadian selanjutnya.

\begin{figure}[ht]
  \begin{subfigure}[t]{.5\textwidth}
    \centering
    \includegraphics[scale=.2]{BAB3/img/estimate_coordinate.png}
    \caption{}
    \label{fig:strategiPenentuanKoordinat}
  \end{subfigure}
  \begin{subfigure}[t]{.5\textwidth}
    \centering
    \includegraphics[scale=.2]{BAB3/img/estimate_coordinate2.png}
    \caption{}
    \label{fig:strategiPenentuanKoordinat2}
  \end{subfigure}
  \caption{Strategi penentuan koordinat (a) langkah 1 dan (b) Langkah 2}
\end{figure}