\textit{``The development of the automobile replaced the horse as a mobility tool, and the development of humanoid robots offers the promise to replace humans as the bearer of hard and dull tasks.''} - Shuuji Kajita, Hirohisa Hirukawa, Kensuke Harada, Kazuhito Yokoi. \cite{kajita_introduction_2014}



\section{Background/Motivation}                            
In a world where robotics is developing at an extraordinary rate, (be it on a more industrial or domestic environment) there are more and more reasons to explore and solve the problems that originate from this development. Solving these kinds of problems can bring great benefits to the world's economy if, for example, these problems are about industrial robotics. However, if you focus on robots to be used in a domestic environment, their main purpose would be to improve our day-to-day activities, and to do it while interacting with humans safely.

Humanoid robots (see Fig. \ref{fig:humanoids}) can be considered part of this domestic application, and have been a point of interest for many decades. Although new applications for this kind of robots are being discovered, interest in this topic began with the writing of sci-fi stories, where androids and the moral intricacies regarding their existence were the main plot. The first real application of humanoid robotics was a military one, at the \textit{DARPA Rescue Challenge} in 2013 \cite{noauthor_darpa_nodate}, where multiple teams of engineers built robots with the purpose of entering highly hazardous environments and performing some kind of complex task. However, if the cost of building a human-like robot goes down in the near future, mass commercialization of humanoids does not seem like an impossibility at all.

\begin{figure}[htbp]
	\centering
	\includegraphics[height=6cm]{./Imagens/humanoids.jpg}
	\caption{Humanoid robots. Taken from \cite{noauthor_humanoids.jpg_nodate}.}
	\label{fig:humanoids}
\end{figure}


At the University of Aveiro, research on humanoid robots is also present under the Humanoid Project of the University of Aveiro (PHUA). This project comes from a partnership between the Department of Mechanical Engineering (DEM) and the Department of Electronics, Telecommunications and Informatics (DETI) and its purpose is to provide students who join the  Automation and Robotics Laboratory (LAR) with a way to learn and practice in the areas of programming and robotics.

PHUA started in 2004 \cite{santos_mechatronic_2012} and since then students have worked to find new and better solutions, ending up with the structure shown in Fig. \ref{fig:phua}. This structure weighs about 6 kg, has a height of 667 mm and has 27 degrees of freedom (DoF), which allow it to perform a range of human-like motions. It also features a total of eight pressure sensors under its feet (four for each), two cameras on its head and an Inertial Measurement Unit (IMU) platform, consisting of accelerometers, gyroscopes and magnetometers \cite{rafeiro_rede_2013}. A haptic interface was also developed, allowing the robot to be controlled using force feedback \cite{barros_cooperative_2014}. Lately, students have tried to integrate visual and inertial data in order to assist humanoid balance \cite{peixoto_visual_2016}, and visual servoing using fixed targets \cite{sousa_visual_2016}.


\begin{figure}[htbp]
	\centering
	\includegraphics[height=8cm]{./Imagens/robo2.png}
	\caption{Last iteration of the PHUA project. Taken from \cite{barros_tele-kinesthetic_2014}.}
	\label{fig:phua}
\end{figure}

\section{Problem description}

The main problem with humanoid robots is the lack of a way to balance themselves properly. Since they have legs to emulate human motion, there is a lack of a fixed reference point in relation to their environment (as opposed to other types of robots). Therefore it is difficult to determine the position and velocity of humanoid robot's joints at any given moment in time.

This problem has been approached in two ways until now. The way which has received the most attention to date consists of finding the center of pressure (CoP) by using load cells under the robot's feet, thus allowing to know where the robot is falling towards (e.g., if more force is felt at the ``toes'' then the robot should be falling to the front) \cite{estrelinha_tele-operation_2013}.

This approach, however, has not been fully successful and therefore attention has switched to another way of solving the problem, where the objective is to first stabilize the head of the humanoid, so that afterwards it is easier to balance the whole robot. In 2016, this topic was approached by another author \cite{peixoto_visual_2016} but more work needed to be done in order to understand if it is a viable way to approach the problem, or if there is yet another way to achieve the humanoid's balance.

\section{Objectives}\label{sec:obj}

In order to achieve head stabilization, one must find the direction of the gravity vector (verticality). To do so, an existing Pan-Tilt Unit (PTU) with a camera and 2 IMU's installed \cite{sousa_visual_2016} will be used to obtain visual and inertial data, as was done before \cite{peixoto_visual_2016}. However, less focus will be given to inertial data since the measurements taken from the IMU's are influenced by vibrations from the robotic manipulator which is holding the PTU when performing experiments.

Regarding the visual data, it will be analyzed in a different way from what was done before, since the other approach based on visual features proved to be of a limited use.

In summary, the objectives proposed for this thesis are the following:

\begin{itemize}
	\item Estimate the direction of the gravity vector using both visual and inertial data.
	\item Develop computational tools which will act on the camera support (PTU), based on the previously estimated verticality, allowing it to remain stable.
	\item Explore and demonstrate the benefits of head stabilization when trying to achieve a humanoid robot's balance.  
\end{itemize}           

\section{Related work and state of the art}

In this section, a short summary of the theses that support this work will be made and there will also be a brief explanation of how their results will be used.
Furthermore, a concise state of the art regarding humanoid balance will be presented.

\subsection{Visual Servoing of a Humanoid Head, by \textit{C. Sousa}}

The first thesis that this work will be based on was written by C{\'e}sar Sousa in 2016 \cite{sousa_visual_2016}and its main subject is the visual control of a humanoid head. This thesis' main objectives are twofold: to develop image analysis techniques for detection and following of a static visual cue and to then try to control the camera so that even if exterior disturbances were provoked, it would still fixate on the target.

A PTU with a mounted camera was developed and built during the writing of that thesis and it will also be the one used for the current work. This PTU was meant to allow the camera to track the target and its joints were controlled so that when an external disturbance was provoked (via a robotic manipulator) the camera would always point in the direction of the target.

It was also in this thesis that the first interaction with ViSP (Visual Servoing Platform) \cite{marchand_visp_2005} was made, a modular C++ library which allows fast development of visual servoing applications. Since it was first released to public in 2005, this library has evolved immensely and its integration with the ROS environment is now easier than ever. Therefore, this library will also be explored when trying to estimate verticality whilst using visual data. 

\subsection{Visual and Inertial Data Integration to Assist Humanoid Balance, by \textit{J.Peixoto}}
The second thesis that this work will be based on was also written in 2016 by Jo{\~a}o Peixoto \cite{peixoto_visual_2016} and its main subject is the integration of visual and inertial data to assist with humanoid balance. That thesis' main objective was to combine the visual data coming from the camera mounted in the previously mentioned PTU with the inertial data coming from two different IMU's, which were also installed on the PTU. The merged data would then allow for an estimation of the humanoid head's orientation, making it possible to stabilize and achieve the humanoid's balance in an easier way.

During that thesis' writing, the author arrived at the conclusion that the method he used to estimate verticality using local visual features couldn't provide great accuracy, although being well implemented. As such, an alternative to estimating verticality using visual data must be found during this work. Multiple experiments were also made with the IMU's installed on the PTU, allowing for the estimation of the gravity vector, although these measurements only provide accurate results when taken without external disturbances.

Additionally, a ROS (Robot Operating System) node to communicate with the robotic manipulators at LAR was also developed, which will prove useful when conducting experiments to evaluate this work's solutions.

\subsection{State of the Art}
\subsubsection{Human balance}

In order to understand the basics of balance and how to reach equilibrium on humanoid robots, it is paramount to understand how humans can balance themselves.

The sense of balance is a result of three major groups of sensory information being sent to the brain. These are sight, the vestibular system, and proprioception (body awareness). Of these, the vestibular system is the one of most importance, and therefore will be explained in greater detail. The vestibular system is a region of the inner ear where three semicircular canals filled with a fluid called endolymph are located (see Fig. \ref{fig:vestibular}). These canals are all perpendicular to each other and at the base of each canal there is an enlarged portion called ampulla , which contains hair cells (the crista) that are affected by movement. These hair cells are bent (sending a nervous impulse) when our body starts or stops moving, and when it accelerates or decelerates, or when it changes direction. These canals function just as a modern day accelerometer does, helping us to maintain equilibrium while moving. \cite{scanlon_essentials_2007}

Also in the inner ear, there exist two membranous sacs called utricle and saccule (see Fig. \ref{fig:vestibular}). Within them exist the same kind of hair cells embedded in a gelatinous membrane with tiny crystals of calcium carbonate called otoliths. These otoliths are pulled by gravity and bend the hair cells, allowing them to send nervous signals to the cerebellum and giving us information about the orientation of our heads while standing still. Another analogy can be made here with gyroscopes, which give us the orientation of the gravity vector. \cite{scanlon_essentials_2007}

In short, the otolith organs provide us information about the position of the body at rest, while the semicircular canals give us informations about the body in motion. \cite{scanlon_essentials_2007}

\begin{figure}[htbp]
	\centering
	\includegraphics[scale=0.7]{./Imagens/vestibular_system_crop.png}
	\caption{Physiology of equilibrium. (A) Utricle and saccule. (B) Semicircular canals. Taken from \cite{scanlon_essentials_2007}.}
	\label{fig:vestibular}
\end{figure}

Although the role of sight and body awareness in obtaining equilibrium isn't to be discarded, this thesis will not go into detail of how those systems function.

\clearpage
\subsubsection{Achieving balance in humanoid robots}

Now that the basics of how humans perceive balance have been explained, this section will try to describe previous works on the topic of balancing humanoid robots.

The first attempt at building a humanoid robot was made by Ichiro Kato et al. from Waseda University in 1973. \cite{kajita_introduction_2014} Although it had biped legs, it could not walk with sufficient stability. Kato's group also developed WABOT-2 in 1984, although this was just a "specialist robot" who was built for entertainment purposes.  \cite{kato_development_2003}

It was only in 1996 that Honda released the P2 humanoid robot, shocking everyone in the field. After that, Honda released the P3 in 1997 and ASIMO in 2000. Before the P2, the majority of the robotics community were pessimistic about the development of a biped humanoid robot who could walk stably. This was only possible since Honda could manufacture mechanical links with high rigidity and make them still be lightweight by using advanced mechanical CAD (Computer Aided Design), whilst the previous projects did not have the required funding to make this possible.

The Honda robots truly made a mark in the field, and as such, the next robots to be developed followed the configuration of Honda robots closely, including the balancing mechanisms. These robots had gyroscopes and accelerometers, and also force/torque sensors on its feet, allowing the measurement of the contact forces/torques between the feet and the ground. \cite{kajita_introduction_2014}

This is where it is needed to introduce the concept of Zero-Moment Point (ZMP). ZMP was first defined by Vukobratovi\'{c} and it is the point where the resultant of the forces inflicted on the robot's foot passes the surface of the foot (see Fig. \ref{fig:ZMP}) \cite{vukobratovic_stability_1972}. It is therefore a criterion to judge if the contact between the sole of the foot and the ground can be kept. The contact is kept if the ZMP is an internal point on the sole. \cite{kajita_introduction_2014}

\begin{figure}[htbp]
	\centering
	\includegraphics[scale=1]{./Imagens/ZMP.png}
	\caption{Zero-moment point. Taken from \cite{vukobratovic_stability_1972}.}
	\label{fig:ZMP}
\end{figure}

If the robot is not moving, then the ZMP coincides with the projection of the Center of Mass (CoM) of the robot onto the ground. The majority of humanoid robots developed after P2 used the concept of ZMP to plan motion patterns that can make the robot walk while keeping the contact between the sole of the supporting foot and the ground. \cite{kajita_introduction_2014} However, as was said before, this thesis will focus on an approach different from what has been done before, be it in the University of Aveiro, or all around the world.
