\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 are caused by this development. Solving these problems can bring great benefits to the world's economy if, for example, these problems are about industrial robotics. However, if we 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 found, 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 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 present under the \gls{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 \gls{lar} with a way to learn and practice in the areas of programming and robotics.

\gls{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 \gls{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 \gls{imu}, 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_nodate}.}
	\label{fig:phua}
\end{figure}

\section{Problem description}

The main problem with humanoid robots is associated with the ability to maintain balance during both standing and walking. 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 angular position and velocity of humanoid robot's joints at any given moment in time.

This problem has been approached in two ways by the \gls{phua} project 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 \gls{ptu} with a camera and 2 \gls{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 there was a great interest in further exploring the benefits of visual servoing.

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 dissertation 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 (\gls{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}

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.

\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 \gls{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 \gls{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 \gls{visp} \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 \gls{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 \gls{ptu} with the inertial data coming from two different \gls{imu}'s, which were also installed on the \gls{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 \gls{imu}'s installed on the \gls{ptu}, allowing for the estimation of the gravity vector, although these measurements only provide accurate results when taken without external disturbances.

Additionally, a \gls{ros} node to communicate with the robotic manipulators at \gls{lar} was also developed, which will prove useful when conducting experiments to evaluate this work's solutions.

\section{Balance in humans and humanoid robots}
\subsection{Human balance system}

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}

When it comes to sight influencing balance, it can be said that it is only an accessory to the vestibular system. However, since this work focuses mainly on estimating verticality (and therefore reaching equilibrium) through visual data, the vision system will also be explored.

In the eye, there are sensory receptors called rods and cones  (see Fig. \ref{fig:rods}) which are located in the retina. Rods detect only the presence of light, whereas cones detect colors. Rods are also proportionally more abundant toward the periphery of the retina. This is why our best vision in dim light or at night, where colors can't be seen, is periphery vision. When light strikes the rods and cones, they send impulses through the optic nerve to the brain that provide visual cues which identify how a person is oriented relative to the environment. \cite{scanlon_essentials_2007}\cite{noauthor_human_nodate}

\begin{figure}[htbp]
	\centering
	\includegraphics[scale=0.7]{./Imagens/retina.png}
	\caption{Microscopic structure of the retina. Taken from \cite{scanlon_essentials_2007}.}
	\label{fig:rods}
\end{figure}

After information is gathered from the vestibular and sight systems (and also the proprioceptive system which is not going to be explained), it is sent to the cerebellum, which is responsible for the maintenance of posture and equilibrium through an involuntary process. The cerebellum then sends nervous signals to muscles all over the body which maintain balance and also to muscles controlling the eyes, in what is known as the Vestibulo-Ocular Reflex (VOR). This reflex helps stabilize gaze during active head movements (e.g., running) and passive head movements (e.g., sitting on an accelerating car). \cite{noauthor_human_nodate}

As an extra note, it is useful to know that disorientation occurs when the vestibular organs, the eyes and the muscles and joints send conflicting information to the cerebellum. One example of this is when a person is sitting on a car which is accelerating and deccelerating and reading a book at the same time. Since the eyes do not see any difference between consecutive images but the vestibular system senses the movement of the car, the cerebellum doesn't know what signals to send to the body, since it doesn't know if it is standing still or moving. \cite{noauthor_human_nodate}



\clearpage
\subsection{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 large majority of humanoid robots us the concept of Zero Moment Point (ZMP) as the base of their balance mechanisms. 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. Most humanoid robots use 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 dissertation will focus on an approach different from what has been done before, making it difficult to find similar applications implemented, be it in the University of Aveiro, or all around the world. After extensive research, only one paper \cite{berthoz_benefits_2011} was found that also explored the benefits of head stabilization when trying to achieve a humanoid robot's balance. It states that stabilizing a humanoid head during locomotion brings benefits when estimating the gravitational vertical. In this paper, verticality was estimated using a damped inclinometer and an \gls{imu} and results were obtained through the use of simulations. As such, it is possible to conclude that there is room for further study in a real world application which could use other methods (e.g. using visual data) to estimate verticality.
