%
% UnicycleControl.tex
%

\documentclass{beamer}

\usepackage{beamerthemeshadow}
\usepackage{listings}
\usepackage{graphics}
\usepackage{times}
\usepackage{algorithm2e}

\usepackage{pgf,pgfarrows,pgfnodes,pgfautomata,pgfheaps,pgfshade}
% \usepackage{amsmath,amssymb} \usepackage[latin1]{inputenc}
% \usepackage{colortbl}
\usepackage[english]{babel}

\newcommand{\erlang}{
\lstset{language=Prolog, basicstyle=\ttfamily\footnotesize,frame=trBL,
  frameround=fttt}}

\newcommand{\erlangsmall}{
\lstset{language=Prolog, basicstyle=\ttfamily\scriptsize,frame=trBL,
  frameround=fttt}}

\newcommand{\clang}{
\lstset{language=Prolog, basicstyle=\ttfamily\footnotesize,frame=trBL,
  frameround=fttt}}

\newcommand{\clangsmall}{
\lstset{language=Prolog, basicstyle=\ttfamily\scriptsize,frame=trBL,
  frameround=fttt}}

\newcommand{\clangtiny}{
\lstset{language=Prolog, basicstyle=\ttfamily\tiny,frame=trBL,
  frameround=fttt}}

\erlang

\newcommand\blue{\color[rgb]{0,0,1}}
\newcommand\green{\color[rgb]{0,0.5,0}}
\newcommand\red{\color[rgb]{1,0,0}}
\newcommand\black{\color[rgb]{0,0,0}}

\newcommand\tape[9]{
\framebox{#1}
\framebox{#2}
\framebox{#3}
\framebox{#4}
\framebox{#5}
\framebox{#6}
\framebox{#7}
\framebox{#8}
\framebox{#9}}

\newcommand\two[2]{
\framebox{#1}
\framebox{#2}}

\newcommand\six[6]{
\framebox{#1}
\framebox{#2}
\framebox{#3}
\framebox{#4}
\framebox{#5}
\framebox{#6}}

\newcommand\three[3]{
\framebox{#1}
\framebox{#2}
\framebox{#3}}

\title[Cinematica Robot Mobile]{Cinematica e Controllo di un robot mobile}
%%

\author[Corrado Santoro]
       {Corrado Santoro}

\date{ }

\institute[University of Catania]
  {
    \begin{center}
      \textbf{\red{}ARSLAB - Autonomous and Robotic Systems Laboratory}\\
      Dipartimento di Matematica e Informatica - Universit\`{a} di Catania, Italy\\
      \texttt{santoro@dmi.unict.it}\\
      \includegraphics[width=60pt]{robotEscher_Eura_blur.eps}\\%%~~~~\includegraphics[width=80pt]{logo_dmi.png}
      Programmazione Sistemi Robotici\\
    \end{center}
  }



\begin{document}


%%
\frame{\titlepage}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica di Robot Mobile su Ruote}
%%
\begin{center}
\includegraphics[scale=0.3]{reference_system.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}La \textbf{\red{}posa} istantanea di un robot su un
  ambiente \textbf{bidimensionale} \`{e} caratterizzata da:
%%
  \begin{itemize}
    %%
  \item\red\footnotesize{}Posizione $x$
  \item Posizione $y$
  \item Orientamento $theta$ (rispetto all'asse $x$)
    %%
  \end{itemize}
%%
\end{itemize}
%%
\end{frame}
%%




%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica di Robot Mobile su Ruote}
%%
\begin{center}
\includegraphics[scale=0.3]{reference_system_2.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Si considerano due sistemi di riferimento:
  \begin{itemize}
    %%
  \item\footnotesize\textbf{\red{}Body system}, sistema di riferimento
    \emph{locale} del robot $(x',y')$
  \item\textbf{\red{}Earth system}, sistema di riferimento
    \emph{globale} dell'ambiente $(x,y)$
    %%
  \end{itemize}
%%
\item La trasformazione tra un sistema e l'altro avviene usando le normali
  formule di roto-traslazione
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Modello di Robot Mobile a due ruote indipendenti}
%%
\begin{center}
\includegraphics[scale=0.3]{robot_model.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Due ruote indipendent poste ai lati del robot
\item Il punto {\red$C$} \`{e} detto \textbf{centro di istantanea
  rotazione} ed \`{e} il riferimento per la posizione globale del robot
\item I parametri di geometria per il modello cinematico sono:
  \begin{itemize}
    %%
  \item\red\footnotesize{}$r_L$, raggio della ruota sinistra
  \item\red$r_R$, raggio della ruota destra
  \item\red$n_L$, numero di tick per giro dell'encoder sinistro
  \item\red$n_R$, numero di tick per giro dell'encoder destro
  \item\red$L$, distanza tra i punti di contatto delle due ruote
    (\textbf{wheelbase})
    %%
  \end{itemize}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
\begin{itemize}
%%
\item\footnotesize{}Parametri:
  \begin{itemize}
    %%
  \item\footnotesize{}$r_L$, raggio della ruota sinistra
  \item$r_R$, raggio della ruota destra
  \item$n_L$, numero di tick per giro dell'encoder sinistro
  \item$n_R$, numero di tick per giro dell'encoder destro
  \item$L$, distanza tra i punti di contatto delle due ruote
    (\textbf{wheelbase})
    %%
  \end{itemize}
%%
\item Detti $\Delta tick_L$ e $\Delta tick_R$ il numero di \textbf{tick
  contati}, su ruota destra e sinistra, nell'intervallo di campionamento
  $\Delta T$, abbiamo:
{\red{}
\begin{eqnarray*}
\Delta d_L  =  \frac{2 \pi r_L}{n_L}\Delta tick_L & ~ &
\Delta d_R  =  \frac{2 \pi r_R}{n_R}\Delta tick_R \\
\Delta theta & = & \frac{\Delta d_R - \Delta d_L}{L} \\
v_L  =  \frac{\Delta d_L}{\Delta T} & ~ &
v_R  = \frac{\Delta d_R}{\Delta T}
\end{eqnarray*}}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
\begin{itemize}
%%
\item\footnotesize{}Detti $\Delta tick_L$ e $\Delta tick_R$ il numero di \textbf{tick
  contati}, su ruota destra e sinistra, nell'intervallo di campionamento
  $\Delta T$, abbiamo:
{\red{}
\begin{eqnarray*}
\Delta d_L  =  \frac{2 \pi r_L}{n_L}\Delta tick_L & ~ &
\Delta d_R  =  \frac{2 \pi r_R}{n_R}\Delta tick_R \\
\Delta \theta & = & \frac{\Delta d_R - \Delta d_L}{L} \\
v_L  =  \frac{\Delta d_L}{\Delta T} & ~ &
v_R  = \frac{\Delta d_R}{\Delta T}
\end{eqnarray*}}
%%
\item Dove:
  \begin{itemize}
    %%
  \item\footnotesize{}$\Delta d_L$, $\Delta d_R$, distanza percorsa (in
    $\Delta T$) dalla ruota sinistra/destra
  \item$\Delta theta$, variazione dell'orientamento (in radianti)
  \item$v_L, v_R$, velocit\`{a} istantanea della routa sinistra/destra
    %%
  \end{itemize}
\end{itemize}
%%
\end{frame}
%%




%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
\begin{eqnarray*}
\Delta d_L  =  \frac{2 \pi r_L}{n_L}\Delta tick_L & ~ &
\Delta d_R  =  \frac{2 \pi r_R}{n_R}\Delta tick_R \\
\Delta \theta & = & \frac{\Delta d_R - \Delta d_L}{L} \\
v_L  =  \frac{\Delta d_L}{\Delta T} & ~ &
v_R  = \frac{\Delta d_R}{\Delta T}
\end{eqnarray*}
\begin{itemize}
%%
\item\footnotesize{}Sulla base delle relazioni precedenti, l'aggiornamento
  della posa si ottiene con:
{\red{}
\begin{eqnarray*}
 x  & = & x + \frac{\Delta d_L + \Delta d_R}{2}\cos(\theta + \frac{\Delta \theta}{2}) \\
y  & = &y + \frac{\Delta d_L + \Delta d_R}{2}\sin(\theta + \frac{\Delta \theta}{2}) \\
\theta & =& \theta + \Delta \theta
\end{eqnarray*}}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
\begin{itemize}
%%
\item\footnotesize{}Sulla base delle relazioni precedenti, l'aggiornamento
  della posa si ottiene con:
{\red{}
\begin{eqnarray*}
 x  & = & x + \frac{\Delta d_L + \Delta d_R}{2}\cos(\theta + \frac{\Delta \theta}{2}) \\
y  & = &y + \frac{\Delta d_L + \Delta d_R}{2}\sin(\theta + \frac{\Delta \theta}{2}) \\
\theta & =& \theta + \Delta \theta
\end{eqnarray*}}
%%
\item Le relazioni precedenti, note come \textbf{formule per l'odometria},
  si determinano tramite l'\textbf{\red{}integrazione dell'equazione del
    moto}
%%
\item Tale integrazione (che non pu\`{o} essere effettuata analiticamente)
  \`{e} effettuata \textbf{numericamente} e pertanto contiene un
  \textbf{errore che si propaga {\red{}in modo additivo}} nella stima della
  posa $(x,y,\theta)$
%%
\item La posa stimata $(x,y,\theta)$ \`{e} pertanto soggetta ad un errore
  non nullo e non eliminabile che cresce con l'andare del tempo/distanza
  percorsa
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
%%
\begin{center}
\includegraphics[scale=0.3]{robot_model_speed.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}La \textbf{velocit\`{a}} \`{e} invece rappresentata
  secondo due possibili sistemi di riferimento
  \begin{itemize}
    %%
  \item\footnotesize{}$(v_L, v_R)$, velocit\`{a} istantanea delle
    routa sinistra e destra
  \item$(v, \omega)$, velocit\`{a} istantanea di \textbf{traslazione} e
    \textbf{rotazione} del punto $C$
  \end{itemize}
%%
\item Tra i due sistemi di riferimento esiste la seguente trasformazione:
%%
{\red\begin{eqnarray*}
v = \frac{v_R+v_L}{2}& ~ & \omega = \frac{v_R-v_L}{L}\\
v_L = v - \frac{L\omega}{2} & ~ & v_R = v + \frac{L\omega}{2}
\end{eqnarray*}}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Modello di Controllo in Posizione}
%%
%%
\begin{itemize}
%%
\item\footnotesize{}Per il \textbf{controllo del moto} \`{e} opportuno
  utilizzare il  sistema di riferimento {\red{}$(v, \omega)$}, e trasformare nel
  sistema {\red{}$(v_L, v_R)$} i valori ottenuti.
%%
\item Stabiliti i valori target di {\red{}$(v_L, v_R)$}, ogni ruota
  implementa poi il controllo in velocit\`{a} classico.
%%
\end{itemize}
%%
%%
\begin{center}
\includegraphics[scale=0.2]{position_control_robot.eps}
\end{center}
%%
\end{frame}
%%




%%
\begin{frame}[fragile]
%%
  \frametitle{Moto su traiettoria rettilinea}
%%
%%
\begin{itemize}
%%
\item\scriptsize{}Desideriamo un controllo del moto in cui il
  robot percorre una certa distanza $\red{}TargetDistance$ su una \textbf{traiettoria rettilinea}.
%%
\item Calcoliamo, a ogni iterazione, la distanza percorsa dall'inizio del
  moto come:
\[\red{}%%
dist(t) = dist(t) + \frac{\Delta d_L + \Delta d_R}{2}
\]
%%
\item Utilizziamo due controllori:
%%
\begin{itemize}
%%
\item\scriptsize{}Su $\red{}v$, applichiamo il profilo trapezoidale di
  velocit\`{a}, basato sull'\textbf{errore lineare} $\red{}targetDistance -
  dist(t)$
%%
\item Su $\red{}\omega$, applichiamo un controllore
  proporzionale, basato sull'\textbf{errore angolare} sull'orientamento
  iniziale $\red{}\theta(t)   -  \theta(0)$
%%
\end{itemize}
%%
\end{itemize}
%%
%%
\begin{center}
\includegraphics[scale=0.18]{forward_to_distance.eps}
\end{center}
%%
\end{frame}
%%







%%
\begin{frame}[fragile]
%%
  \frametitle{Rotazione sul posto}
%%
%%
\begin{itemize}
%%
\item\scriptsize{}Desideriamo ruotare (sul posto) in modo da raggiungere
  un'orientamento assoluto  $\red{}\theta_{target}$.
%%
%%
\item Utilizziamo due controllori:
%%
\begin{itemize}
%%
\item\scriptsize{}Su $\red{}\omega$, applichiamo il profilo trapezoidale di
  velocit\`{a} (angolare),  basato sull'\textbf{errore angolare} sull'orientamento
  $\red{}\theta(t) - \theta_{target}$
%%
%%
\item\scriptsize{}Poich\`{e} occorre non spostarsi dal punto di partenza,
applichiamo un controllore
  proporzionale basato sull'\textbf{errore lineare} $\red{}0 -
  dist(t)$ ($0$ perch\`{e} non vogliamo spostarci dal posto)
\end{itemize}
%%
\end{itemize}
%%
%%
\begin{center}
\includegraphics[scale=0.18]{rotate.eps}
\end{center}
%%
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Rotazioni, angoli e differenze}
%%
%%
\begin{itemize}
%%
\item\footnotesize{}Nell'usare gli angoli, occorre considerare che la loro
  artimetica \`{e} \textbf{modulare}
%%
\item Ad esempio, i valori (in gradi) $\red{}350$ e $\red{}-10$ si riferiscono allo
  \textbf{stesso angolo}
%%
\item Tuttavia, il \textbf{differente segno} implica un comportamento
  diverso del controllore che generer\`{a} velocit\`{a} {\red{}positive} o {\red{}negative}
%%
\item Nel calcolo degli angoli, \`{e} pertanto opportuno
  \textbf{normalizzare} sempre il risultato, riconducendolo all'intervallo
  $\red{}[-\pi, \pi]$ (risp. $\red{}[-180, 180]$)
%%
\end{itemize}
%%
%%
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Raggiungimento di un punto}
%%
%%
\begin{itemize}
%%
\item\scriptsize{}Desideriamo raggiungere un punto specifico del piano di
  coordinate $\red{}(x_{target},y_{target})$.
%%
%%
\item In ogni istante dobbiamo:
%%
\begin{itemize}
%%
\item\scriptsize{}Percorrere la distanza (lineare) che ci separa dal punto
  target:
\[\red{}%%
dist = \sqrt{(x_{robot} - x_{target})^2 + (y_{robot} - y_{target})^2}
\]
%%
\item\scriptsize{}Assicurare che l'orientamento del robot sia coerente con
  l'orientamento della traiettoria:
\[\red{}%%
\theta_{trajectory} = \arctan{\frac{y_{target} - y_{robot}}{x_{target} - x_{robot}}}
\]
%%
\end{itemize}
%%
\end{itemize}
%%
%%
\begin{center}
\includegraphics[scale=0.2]{goto_point.eps}
\end{center}
%%
\end{frame}
%%






%%
\begin{frame}[fragile]
%%
  \frametitle{Raggiungimento di un punto}
%%
%%
\begin{itemize}
%%
\item\footnotesize{}Usiamo due controllori, in entrambi i casi applichiamo il profilo
  trapezoidale della velocit\`{a}
%%
\item Il controllore su $\red{}v$ utilizza, come errore, proprio la
  \textbf{distanza} dal punto target
%%
\item Il controllore su $\red{}\omega$ utilizza, come errore, la
  \textbf{differenza} tra orientamento della traiettoria e orientamento del
  robot $\red{}\theta_{trajectory} - \theta_{robot}$
%%
\end{itemize}
%%
%%
\begin{center}
\includegraphics[scale=0.22]{goto_point_2.eps}
\end{center}
%%
\end{frame}
%%











%%
\frame{\titlepage}
%%




%%
%%
\end{document}
%%
%%