\chapter{Introduction}


%\epigraph{\emph{`` É fazendo que se aprende a fazer aquilo que se deve aprender a fazer.''}}%
%         {Aristóteles}
%
%\epigraph{\emph{`` O único homem que nunca comete erros é aquele que nunca faz coisa alguma. Não tenha medo de errar, pois aprenderá a não cometer duas vezes o mesmo erro.''}}%
%         {Theodore Roosevelt}
%
%\epigraph{\emph{`` É durante as tempestades que o verdadeiro marinheiro aprende a velejar.''}}
%         {Ricardo Jordão Magalhães}

%\epigraph{\emph{`` O único homem isento de erros, é aquele que não arrisca acertar.''}}
%         {Albert Einstein}

\section{The ATLAS project}

\subsection{Overview}

The ATLAS project was created to develop autonomous driving small scale robots for competition.
After the success achieved, the research group focused in applying the acquired knowledge in real scale vehicles, to start research an development in passive and active security systems.
Bound by several restrictions, there was the need to interface a conventional car with software commands, as well as monitor human driver actions for active security purposes.
\subsection{Framework}

The ATLAS team develops its code based in the CARMEN robot navigation toolkit, from Carnegie Mellon university.
All robot software modules are running simultaneously and communicate among them using IPC\cite{montemerlo_perspectivesstandardization_2003}.
Currently, the AtlasCar1 platform fig(\ref{fig:Atlascar}) is equipped with: one 200A alternator; one 3000W DC/AC inverter; one UPS; two desktop computers; various cameras; two LIDARs; and one GPS unit.
All the data acquired by the system is saved to log files for later off line analysis and can also be played in simulation mode to test new algorithms.

At this stage the main issues concern the ability to control the vehicle using software instructions, monitoring the driver actions on the vehicle controls and monitor the driver awareness about the surrounding environment.

\begin{figure}
  % Requires \usepackage{graphicx}
  \centering
  \includegraphics[width=0.7\textwidth]{Atlascar}\\
  \caption{AtlasCar1}\label{fig:Atlascar}
\end{figure}


\section{Challenges}

A servo-pilot as referred by its own name is a system responsible for maintaining an output condition specified by input parameters, using error sensing feedback to correct eventual errors between the input required condition and the actual output\cite{younkin_industrial_2003}. This system needs then to have feedback about the vehicle controls assigned to it, so it can compute the errors associated to each output.
Monitoring the vehicle controls such as pedals, gear lever, wheel and others, for this project will require that no modifications are preformed to the vehicle structure, and for driver monitoring functions, it has to be able to operate separately from the servo-pilot remaining systems, and be discrete enough to have no interference with the human driver.
The servo-pilot actuation unit has to be able to control the vehicle principal controls such as pedals and steering wheel in the first stage and later others like gear lever, hand brake and turn indicators. This unit needs to be suited to easy installation and removal, so the vehicle can be driven to the test local and back, and has to be installed without any vehicle structural modifications, that would revogue the vehicle circulation license according to Portuguese legislation.

%As mentioned before, this work tries to accomplish monitoring and control with simple interface at low cost, and without any vehicle structure modification as the car couldn't drive in the street according to Portuguese legislation, which defeats the purpose of this project.

\newpage

\section{Project proposal}

%\epigraph{\emph{`` O único homem que nunca comete erros é aquele que nunca faz coisa alguma. Não tenha medo de errar, pois aprenderá a não cometer duas vezes o mesmo erro.''}}%
%         {Theodore Roosevelt}

The requirements for this project are:
limited cost;
easy to assemble, install and remove from the vehicle;
should require no vehicle modifications.

For this project several approaches were considered:

For the perception solutions based in encoders, potentiometers and inertial sensors like accelerometers and gyroscopes were analyzed. As both encoders and potentiometers require anchorage points in the moving part and in the vehicle structure, the options were discarded.

Inertial modules offer less resolution than the other methods but this can be compensated with redundancy; they  can also be fixed to the vehicle without modifications. With that in consideration, an inertial module was developed based in ST microelectronics MEMS accelerometers and gyroscopes; taking advantage of its small size, the outcome was significantly smaller than all market available similar devices. For this application there is a particular interest in measuring the position of the vehicle pedals, wheel, gear lever, parking brake and among others. In first stage, and also due to the number of sensors available, only the pedals will be measured. The geometry of this vehicle allows to secure the sensors in the back of the pedals inside the grip rubber cover. For the following stages this setup will be extended to the gear lever, hand brake and the wheel. To decouple the components motion from the vehicle's, one sensor must be fixed to the vehicle mainframe to serve as a reference and all other sensors are attached to the moving parts. data is then compared with the fixed reference sensor, so differential measurement can filter out undesired global dynamic data.

Concerning the actuation, several types were studied such as electric linear, electric rotational and pneumatic. For this application and given the space constraints, the only two option were the electric linear and pneumatic, as rotational types require structure modifications to attach to the vehicle. Given that pedals have spring elements responsible for their return to home position , force control is the simplest to implement, because the force applied by a spring is linearly related to its displacement and such control can be preformed by a regular PID controller which can act similarly to an human driver.

\section{Mathematical principles}\label{sec:math}
To obtain differential measures vector computing techniques are applied. Starting with a simplification in which equal acceleration modules are measured by the different measuring devices, the angle between them can be calculated by the equation \ref{eq:acos}, where $v_{1}$ and $v_{2}$ are two 3D vectors.

\begin{equation}
    \label{eq:dot_vs_cross}
    \vec{v_{1}}\cdot\vec{v_{2}}=|\vec{v_{1}}|\cdot|\vec{v_{2}}|\cdot\cos \alpha
\end{equation}

\begin{equation}
    \label{eq:dot_product}
    \vec{v_{1}}\cdot\vec{v_{2}}=v_{1x}\cdot v_{2x}+v_{1y}\cdot v_{2y}+v_{1z}\cdot v_{2z}
\end{equation}

\begin{equation}
    \label{eq:norm}
    |\vec{v_{1}}|=\sqrt{v_{1x}^{2}+v_{1y}^{2}+v_{1z}^{2}}
\end{equation}

\begin{equation}
    \label{eq:cos}
    \cos \alpha=\frac{\vec{v_{1}}\cdot\vec{v_{2}}}{|\vec{v_{1}}|\cdot|\vec{v_{2}}|}
\end{equation}

\begin{equation}
    \label{eq:acos}
    \alpha=\arccos\left(\frac{\vec{v_{1}}\cdot\vec{v_{2}}}{|\vec{v_{1}}|\cdot|\vec{v_{2}}|}\right)
\end{equation}

After motion tests it became clear that such simplification was not valid and more complex dynamic laws have to be considered. However if the units share one parallel axis, which is also parallel to the pedal rotation axis, there may be the possibility to keep some simplifications on the system. As the geometry of the system is known and a reference unit can be used to compute the differences in the acceleration applied to the other sensors\cite{foxlin2000head}\cite{1033718}.
Having one parallel axis between units, rotation movements are most likely to cause different modules in the acceleration applied to each unit. Knowing the vector that links each unit origin with the reference, the angular velocity data from the gyroscopes ($\omega$) can be used in equation \ref{eq:ac} to compute the expected difference in the acceleration between sensors, which will be used to correct the previous method.

\begin{equation}
    \label{eq:ac}
    \vec{a_{c}}=\omega^{2}r
\end{equation}

After computing the difference in the centripetal acceleration between the reference and the sensor it will be subtracted to the sensor global acceleration and the result should have approximately the same modulus as the reference unit. If this method is successfully applied for global dynamics, but still fail if the pedal in in motion relative to the vehicle, which happens when the angular velocity measured by the reference is different from the one measured by the pedal sensor, more variable have to be considered. For this situation, the difference between angular velocities can be used with the vector that links the pedal rotation axis to the pedal sensor origin, to compute the acceleration caused by the relative movement, that will also be subtracted from the global acceleration measured by the pedal sensor.
This methods only consider centripetal acceleration. If after testing, this is not enough, the angular velocity measured by the gyroscopes can be derived to obtain the angular acceleration and compute the tangential acceleration, and therefore the global acceleration eq(\ref{eq:at2}) caused by the rotation movement.
With this updates, the system should be able to preform in most real situations experienced by any urban cruising car.

\begin{equation}
    \label{eq:atan}
    \vec{a_{t}}=\frac
\end{equation}

\begin{equation}
    \label{eq:at}
    \vec{a_{total}}=\sqrt{a_{c}^{2}+a_{t}^{2}}
\end{equation}

\begin{equation}
    \label{eq:at2}
    \vec{a_{total}}=\sqrt{\omega^{4}r^{2}+\Delta\omega^{2}r^{2}}
\end{equation}

\section{State of Art}

\subsection{Inertial measurement systems}

A variety of inertial sensors and modules can be found in the market. Typically with three, six or nine degrees of freedom (DOF), as they contain accelerometer, gyroscope or magnetometer, as well as combination of these units up to three. However most of them are not compatible with your application due to size constraints.

There are some systems available in the market that can interface a standard car with software controls, but usually this devices have complex control protocol and require permanent vehicle modification.

Description for this systems can be found in the next section.

\subsubsection{Analog devices ADIS16405}

This unit produced by Analog Devices fig(\ref{fig:analog_devices}) has nine DOF being three accelerations, three angular rates and three magnetic readings. The main features include: SPI interface and embedded temperature sensor for auto compensation, accelerometer scale up to 18 g, angular rate up to 300°/sec and magnetometer up to 2.5 Gauss\cite{Analog_devices_ADIS16405}. This device is presented in 23mm cubic housing\cite{Analog_devices_ADIS16405} and costs around 450\euro\cite{Analog_devices_price}.

\begin{figure}[!h]
  % Requires \usepackage{graphicx}
  \centering
  \includegraphics[width=0.5\textwidth]{ADIS16405BMLZ}\\
  \caption{Analog devices ADIS16405}\label{fig:analog_devices}
\end{figure}


\subsubsection{Xsens MTi}

This unit produced by Xsens fig(\ref{fig:xesns_mti}) is similar to to the previously referred, being the main difference the use of embedded DSP capable of processing the sensor data and generating 3D orientation output as well as raw data from each sensor. The main features include: RS-232, RS-485 and RS-422 interface, accelerometer scale up to 5 g, angular rate up to 300°/sec and magnetometer up to 0.75 Gauss\cite{Xsens_MTi}.
This device is presented in 58x58x22mm housing\cite{Xsens_MTi} and costs around 3000\euro.

\begin{figure}[!h]
  % Requires \usepackage{graphicx}
  \centering
  \includegraphics[width=0.5\textwidth]{Xsens_MTi}\\
  \caption{Xsens MTi}\label{fig:xesns_mti}
\end{figure}

\subsubsection{MEMSense nIMU}

MEMSense produces one of the smallest IMUs available in the market fig(\ref{fig:memsense_nimu}). It has 9 DOF, similarly to the previous devices and comes with asynchronous serial interface and I$^{2}$C\textsuperscript{\scriptsize{\texttrademark}} as well. It can read accelerations up to 10 g, angular rate up to 1200°/sec and magnetometer up to 1.9 Gauss\cite{MEMSense_nIMU}.
This device is presented in 46x23x12mm housing\cite{MEMSense_nIMU} and costs around 1100\euro\cite{MEMSENS_price}.

\begin{figure}
  % Requires \usepackage{graphicx}
  \centering
  \includegraphics[width=0.5\textwidth]{memsense_nimu}\\
  \caption{MEMSense nIMU}\label{fig:memsense_nimu}
\end{figure}

\subsubsection{MicroStrain\textsuperscript{\scriptsize{\textregistered}} 3DM-GX3\textsuperscript{\scriptsize{\texttrademark}}-25}

This device fig(\ref{fig:microstrain_3dm_gx3}) combines most of the features from the Xsens MTi in a package almost as small as the MEMSense nIMU. Main features include: RS-232, USB 2.0 and TTL serial interface, temperature compensated over entire operational range, data rate up to 1 kHz, outputs Euler angles, rotation matrix, delta angle, delta velocity, acceleration up to 5 g, angular rate  up to 300°/sec and magnetic field up to 1.9 Gauss\cite{MicroStrain_3DM_GX3}.
This device is presented in 38x24x12mm housing\cite{MicroStrain_3DM_GX3} and costs around 1500\euro\cite{Microstrain_price}.

\begin{figure}
  % Requires \usepackage{graphicx}
  \centering
  \includegraphics[width=0.5\textwidth]{microstrain}\\
  \caption{MicroStrain\textsuperscript{\scriptsize{\textregistered}} 3DM-GX3\textsuperscript{\scriptsize{\texttrademark}}-25}\label{fig:microstrain_3dm_gx3}
\end{figure}

\subsubsection{Inertial measurement systems summary}

Table \ref{tab:summary} shows comparison between the systems referred before.

\begin{table}[!h]
    \begin{center}
    \caption{Specification comparison between different IMUs}
    \label{tab:summary}
    %\begin{tabularx}{\textwidth}{|c|*{6}{x{1cm}|}}
    \begin{tabularx}{\textwidth}{|c|*{6}{s|}}
      \hline
      % after \\: \hline or \cline{col1-col2} \cline{col3-col4} ...
      %Iteração\textbackslash Acção  & 0 & 1 & 2 & 3 & 4 & 5 \tn
      \backslashbox{Spec}{Device}  & ADIS16405 & Xsens MTi & MEMSense & 3DM-GX3 & Unit \tn
      \hline
      Gyro scale & $\pm350$ & $\pm300$ & $\pm300$ & $\pm300$ & °/sec \tnhl
      Gyro linearity & $0.1$ & $0.1$ & $0.1$ & $0.2$ & \% of FS \tnhl
      Accelerometer scale & $\pm18$ & $\pm5$ & $\pm5$ & $\pm5$ & g \tnhl
      Accelerometer linearity & $0.1$ & $0.2$ & $0.4$ & $0.2$ & \% of FS \tnhl
      Magnetometer scale & $\pm3.5$ & $\pm0.75$ & $\pm1.9$ & $\pm2.5$ & Gauss \tnhl
      Magnetometer linearity & $0.5$ & $0.2$ & $0.5$ & $0.4$ & \% of FS \tnhl
      Interfaces & SPI & RS-232 -485 -422 & I2C USART & USB 2.0 RS-232 &   \tn
      \hline
    \end{tabularx}
    \end{center}
\end{table}

\subsection{Actuation systems}
\subsubsection{SEA, Ltd. ASC}

The SEA, Ltd. automated steering controller as seen in fig(\ref{fig:SEA_ASC1}) and fig(\ref{fig:SEA_ASC2}) is a robot that can be programmed to execute a variety of steering maneuvers\cite{SEA_VDS_2006}, and also record human input for later reproduction.
It is only capable of open-loop actuation \cite{heydinger_portable_2008}. It is mounted in the vehicle by replacing it's original steering wheel and fixing two vacuum pads to the windshield to prevent the entire system rotation.

It has the advantage of allowing the operator to keep hands on the wheel while operating, but is not suitable for diary assembly and disassembly, and once again can't preform close-loop operation.

\begin{figure}[!h]
  % Requires \usepackage{graphicx}
  \centering
  \subfloat[]{
    \includegraphics[width=0.4\textwidth]{SEA_ASC_00}
    \label{fig:SEA_ASC1}}
  \hfill
  \subfloat[]{
    \includegraphics[width=0.4\textwidth]{SEA_ASC_01}
    \label{fig:SEA_ASC2}}
  \caption{SEA, Ltd. Automated Steering Controller system}
\end{figure}

\subsubsection{ATI/Heitz Sprint 3}

This robot fig(\ref{fig:ATI_HEITZ}) is very similar to SEA ASC, being the main diferences the maximum torque it can achieve and its programming is done by downloading the data from the computer to one special EEPROM which is then inserted into the unit.

\begin{figure}[!h]
  % Requires \usepackage{graphicx}
  \centering
  \includegraphics[width=0.6\textwidth]{ATI_HEITZ}\\
  \caption{ATI/Heitz Sprint 3 steering controller}\label{fig:ATI_HEITZ}
\end{figure}

\subsubsection{Stähle robot systems}
Stähle GmbH has some driving robots that are mounted in the driver seat and actuate both the steering wheel, the pedals and the gear lever fig(\ref{fig:stahle_spf2000ff}). It can operate in open-loop and close-loop control, has TCP/IP, CANbus and RS-232 communication protocols, and various analog and digital inputs and outputs.
This would be the ideal system for this project, but the cost is around 135.000\euro.

\begin{figure}[!h]
  % Requires \usepackage{graphicx}
  \centering
  \includegraphics[width=0.6\textwidth]{prod_sfp2000ff}\\
  \caption{Stähle SPF2000FF robot driver}\label{fig:stahle_spf2000ff}
\end{figure}

\subsubsection{Kairos Autonomi\textsuperscript{\scriptsize{\textregistered}} Pronto4\textsuperscript{\scriptsize{\texttrademark}}}

Kairos Autonomi\textsuperscript{\scriptsize{\textregistered}} has developed Pronto4\textsuperscript{\scriptsize{\texttrademark}} fig(\ref{fig:pronto4}), an agnostic autonomy system which means that it can convert almost every ground vehicle or surface vessel that uses a steering wheel or even skid steering into an tele-operated or semi-autonomous vehicle\cite{Kairos_pronto4}.
This system was used by Team Juggernaut and by University of Utah in the DARPA Urban Challenge.
While having some advantages, this system is not suitable for this project, since it requires vehicle modification and takes about four hours to install.

\begin{figure}[!h]
  % Requires \usepackage{graphicx}
  \centering
  \includegraphics[width=0.6\textwidth]{Series_3_Small}\\
  \caption{Kairos Autonomi\textsuperscript{\scriptsize{\textregistered}} Pronto4\textsuperscript{\scriptsize{\texttrademark}} agnostic autonomy system}\label{fig:pronto4}
\end{figure}
\newpage

\subsection{Other systems}

Other market solutions can be found, like the Froude Hofmann Robot Driver fig(\ref{fig:Froude}) or the Horiba ADS-7000 - Automatic Driving System fig(\ref{fig:horiba}). But these systems are only suitable for dynamometer tests.

\begin{figure}[!h]
  % Requires \usepackage{graphicx}
  \centering
  \subfloat[]{
    \includegraphics[width=0.5\textwidth]{froude_hofmann}
    \label{fig:Froude}}
  \hfill
  \subfloat[]{
    \includegraphics[width=0.4\textwidth]{horiba}
    \label{fig:horiba}}
  \caption{Froude Hofmann Robot Driver (a) and Horiba ADS-7000 (b) systems}
\end{figure}

%\newpage 