186 lines
8.2 KiB
TeX
186 lines
8.2 KiB
TeX
|
|
\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} |