Body Levelling of a Hexapod Robot using the [627765]

Body Levelling of a Hexapod Robot using the
Concept of Sensor Fusion
Cosmin Copot, Clara M. Ionescuy, Robin De Keysery
University of Antwerp, Department of Electromechanics, Op3Mech
Groenenborgerlaan 171, 2020 Antwerp, Belgium
Email: [anonimizat]
yGhent University, Department of Electrical Energy, Metals, Mechanical Constructions and Systems (EEMMeCS),
Research group on Dynamical Systems and Control (DySC)
Technologiepark 914, 9052 Ghent, Belgium
Email: [anonimizat], [anonimizat]
Abstract —Contrary to their wheeled counterpart, multi-legged
robots are capable of easily changing their body posture through
reconfiguration of the legs (i.e. inclination or posture control).
This capability is exploited in the paper at hand to level out a
six-legged robot when placed on a plain slope with adjustable
inclination. By extension such inclination control could then be
used to let the legged robot mount plain slopes with its body
horizontally which greatly benefits the way the robot absorbs
the load posed on its mechanics. The implemented body levelling
algorithm uses the hexapod’s forward and inverse kinematic
model, basic geometry, and the Cartesian plane equation to
calculate the leg tip displacements able of counteracting the fused
orientation data obtained from an inertial measurement unit
(IMU). Experimental results show the approach to be successful,
although the speed of the levelling process leaves room for
improvement. To address the latter a more powerful processing
unit and/or, more importantly, a simpler redesign of the legs is
advised.
I. I NTRODUCTION
Spurred by enormous improvements in actuation, sensing
and computing capabilities, the field of robotics has flourished
tremendously over the past few decades. Besides the traditional
robot manipulator arms which have already reached a high
state of maturity, considerate attention is being spent to the
creation of mobile robots and gifting these with ever higher
degrees of autonomy. One such type of mobile robots being the
target of countless studies is the six-legged or hexapod robot.
This insect-like machine possesses great stability properties
and as such shows great promise for use in applications where
rough, unstructured terrain has to be traversed like search and
rescue operations in the wake of natural disaster [1], [2], vol-
canic and/or extraterrestrial exploration [3], mountain logging
[4], maintenance and repair of societal centric structures [5],
mine clearance [6] etc.
Contrary to their wheeled counterpart, multi-legged robots
have the desirable ability to adjust their body configuration
while moving (e.g. reposition their centre of gravity, change
body orientation) [7]. Just as the legendary mountain goat-
like animal Dahu which has legs of different lengths to fit the
mountain side, a hexapod robot should also be able to adjust
its legs in order to mount slopes more instinctively. It can
easily be shown profitable from a mechanical point of viewsince levelling the hexapod on a slope avoids the occurrence
of an extra moment onto the body of the bug-like creature.
Towards achieving this levelling, the robot should have an
actual sense of how its body is oriented on the slope. In
this paper the use of an inertial measurement unit (IMU)
is opted for which combines accelerometers, gyroscopes and
optionally digital compasses to get highly accurate estimates
on the body’s orientation. Therefore an IMU can be seen as
the embodiment of the concept of sensor fusion which is a
major and extensively examined research topic. As described
by Dudek and Jenkin [8] sensor fusion is about combining
measurement data from different sensors, positions and/or
times to obtain better measurement estimates than when these
sources were used individually. One other example in mobile
robotics where sensor fusion techniques prove particularly
useful is the process of maintaining an ongoing estimate
of the position of a robot relative to some external set of
landmarks or features [9]. Although various approaches have
been developed to guide the process of sensor fusion, two
distinct but related techniques have become pre-eminent in
mobile robotics, namely approaches based on the classic least
squares combination of estimates (Kalman filter, extended
Kalman filter) and approaches that utilize a less strong er-
ror model (Markov localisation, discrete grid representations,
Monte Carlo and condensation techniques) [8].
The paper at hand covers the implementation of a body
levelling algorithm for a hexapod robot using an inertial
measurement unit. In what follows, section II and III first
describe the six-legged robot and the sensor fusion used to
perform the body levelling experiments. Next, in section IV
the forward and inverse kinematic model are discussed to gain
insight into the relationship between joint actuation and actual
leg tip displacement. The algorithm to let the hexapod level
out its body horizontally on plain slopes of up to ten degrees
inclination is given in section V. The experimental results are
presented in section VI while the conclusions are given in
section VII.CONFIDENTIAL. Limited circulation. For review only.
Preprint submitted to 21st International Conference on System
Theory, Control and Computing. Received June 7, 2017.

II. S YSTEM DESCRIPTION
The experiments are done using a six-legged robot with
a traditional inline design (Fig. 1). Each of the six legs is
equipped with two Standard Futaba S-3111 servos enabling
a forward/backward hip swing and an up/down leg lift (two
degrees of freedom per leg). These servos only use position
sensing via a potentiometer as an elementary part of their
working principle; the position measurement itself is not made
available to the user ruling out the implementation of any
form of feedback control. It should also be noted that using
only two degrees of freedom (DOFs) per leg evidently poses
constraints on the maximally attainable manoeuvrability (e.g.
no sideways walking possible, less flexibility in overcoming
certain obstacles).
Fig. 1. The hexapod robot used to perform the body levelling experiments.
Furthermore, the robot is equipped with an Invensense
MPU-6050 inertial measurement unit (IMU) which combines
a three-axis accelerometer (yielding acceleration along three
perpendicular axes) and three-axis gyroscope (furnishing the
angular speed about each of the axes) together with an on-
board digital motion processor (DMP). This sensor is dis-
cussed more thoroughly in section III.
As for the computational unit of the robot, a single-board
embedded computer with an AM335x 1GHz ARM Cortex-
A8 processor and 512MB DDR3 RAM, called BeagleBone
Black (BBB), was chosen. This BBB is light, low in energy
consumption and should be powerful enough to do all of the
necessary computations. Unfortunately, the BBB is not capable
of delivering twelve PWM signals at a time. This implies that
the servo-actuated leg joints cannot be controlled individually
which would decrease the degree of flexibility. To cope with
this the Mini Maestro 12-channel USB servo controller has
been used. A full schematic representation of the hardware
components is given in figure 2. Note that the hexapod robot
was part of a larger project on developing autonomous robot
for environmental exploration [10] and therefore the hardware
contain sensors (e.g. stereo vision, ultrasonic) which are not
relevant for the scope of this paper.
III. IMU S ENSOR FUSION
The procurement of highly accurate estimates of the body’s
orientation is an absolute prerequisite for the success of the
levelling algorithm. To achieve this, accelero and gyrocsopedata (and optionally digital compass data) – which are quite
complementary – are often used together in sensor fusion.
As mentioned before the IMU used here combines a triple
axis accelerometer and gyroscope and is mounted centrally
on the hexapod robot By placing the IMU like this, the roll or
longitudinal axis is equal to the y-axis, the pitch or transverse
axis corresponds to the x-axis and the yaw or normal axis is
equivalent to the z-axis.
The gyroscope delivers angular velocity about a particular
axis (x, y and/or z) and thus needs to be integrated once to
deliver the changes in rotational attributes of a body like pitch,
roll and yaw. Using the accelero data, it is also possible to
calculate the Euler angles pitch and rollin rather static
applications by looking at the decomposition of the gravity
vector along the three axes (x,y,z) and applying the following
trigonometric relations:
=arctanaYp
a2
Z+a2
X(1)
=arctanaXp
a2
Y+a2
Z(2)
The yaw on the other hand, cannot be computed using the
accelerometer data due to the fact that turning around the yaw
axis does not cause any change in the accelerations, e.g. a strict
rotation around the yaw axis will cause accelerations aXand
aYto remain zero, while aZstays equal to the gravitational
acceleration g.
As becomes clear from the discussion above both ac-
celerometers and gyroscopes can, after some processing, grant
information on the orientation of the robot’s body. As it
happens, both sensors have got their own distinct drawbacks
and advantages making them perfectly suited for applying the
concept of sensor fusion. On the one hand accelerometers
are very noisy (e.g. most of the time a low-pass filter is
applied causing a delayed response) but remain stable over
time. Gyroscopes on the other hand are very smooth (as a
result of the single integration of the angular velocity over
time) with a fast response, but unfortunately the integration
also instigates an inevitable drift over the long run. Moreover
the gyro does not measure the gravity vector while it is
desired to perceive the tilt w.r.t. this vector as is luckily done
by the accelerometers. Consequently, the accelero and gyro
data are indeed often combined to get enhanced results. The
accelerometer corrects the gyro for drift and maintains the
orientation w.r.t. gravity and the gyro ensures a fast, more
smoothed response. The fusion itself is often done by a
Kalman or a complementary filter. However, not being able
to calculate the yaw with the accelerometers makes the drift
correction for this angle not as well as for the pitch and roll
values.
The MPU-6050 has an on-board DMP which is able to
do a nine-axis sensor fusion between three-axis gyroscopes,
accelerometers and optionally also compasses. In theory, if
one really wants to be able to correct the long term drift of
the yaw as well, this latter three-axis compass should be able
to provide the solution by delivering the magnetic north to theCONFIDENTIAL. Limited circulation. For review only.
Preprint submitted to 21st International Conference on System
Theory, Control and Computing. Received June 7, 2017.

5V USB5V7.4V
TTL7.4V
PWM
AIN 5V1.8V
AIN
I2C
3.3V6V
Processing unitPower supply
SensorsActuatorsFig. 2. Full schematic of the various hexapod robot’s components and their interfacing.
gyroscopes. Although, even without the implementation of the
compasses, the data processed by the DMP gives quite accu-
rate yaw values due to the Invensense motion determination
[11]. This algorithm is used to determine whether a sensor,
that can identify motion (e.g. gyroscopes and accelerometers),
has moved or not. Basically, when a signal is received from
the sensor, the moments of this signal are calculated and
compared to determine whether the signal is Gaussian. If so,
it is recognised as coming from a non-moving sensor while a
non-Gaussian one implies an event of motion.
IV. K INEMATIC MODEL
In order to level out the hexapod’s body when placed on a
slope, the six-legged creature should be able to actuate its legs
such that they counteract the body orientation measured by the
IMU. Such leg movement requires a proper understanding of
the relationship between the servo-actuated joint rotations and
the corresponding leg tip displacement. To this end, a forward
and inverse kinematic model is derived.
A. Forward kinematics
Fig. 3 shows the rod mechanism of the legs in a sketch of
the rear side of the hexapod robot. A simplified leg model
(of the left rear leg) is also depicted in Fig. 4 serving as a
guide for the derivation of the forward kinematics and inverse
kinematics. The origin of coordinate frame (v,w) is placed at
the shaft of the knee servo and the origin of coordinate frame
(x,y,z) is located on top of the frame at the hip joint.
Firstly, the forward kinematics are calculated in order to find
out the end effector position H when the servo angles (i.e. the
knee angle and the hip angle ) are given. By means of
example the derivation is done for the left rear leg. Starting
from the knee servo motor a basic rotational movement determines the coordinates of point B with respect to the (v,w)-
coordinate frame:(
vB =jABjcos
wB=jABjsin(3)
In order to obtain the (v C;wC)-coordinates and the angles
and, the following equation are employed:
8
>>><
>>>:vC =jABjcosjBCjcos
wC=jABjsinjBCjsin
vC =vD+jDCjcos
wC=wD+jDCjsin (4)
To determine the coordinates of the end effector point H
with respect to the (v,w)-reference frame, we made use of the
next trigonometric equations:
 = tanjICj
jIDj(5)
(
vF =vD+jFDjcos
wF=wD+jFDjsin(6)
Fig. 3. Rear view of the hexapod robot.CONFIDENTIAL. Limited circulation. For review only.
Preprint submitted to 21st International Conference on System
Theory, Control and Computing. Received June 7, 2017.

Fig. 4. Rod mechanism representation of a left leg.
(
vG =vE+jEGjcos
wG=wE+jEGjsin(7)
(
vH =vGjGHjsin5
wH =wGjGHjcos5(8)
In order to know the 3D position of the foot, the hip swing
effect needs to be taken into account as well. Therefore, the
(vH;wH)-coordinates are described with respect to the (x,y,z)-
coordinate frame:8
><
>:xH = (vH30)cos
yH =wH40
zH =xHtan(9)
This merely implies a translation over the fixed distances
(30 mm in x-direction and 40 mm in y-direction) between the
two coordinate frames and a rotation depending on the hip
angleas in Fig. 5.
B. Inverse kinematics
After obtaining the forward kinematics, the inverse kinemat-
ics are determined based on Fig. 4 and 5 such that a wanted
end effector position can be translated into actual actuator
angles (knee angle and hip angle ). Notice that there are
only two degrees of freedom; as such the end effector ( yH,zH)-
coordinates form sufficient input for the inverse kinematic
model to find the corresponding servo angles. First of all, the
angleis calculated starting with the known y-coordinate of
the end effector H:
8
>>><
>>>:wH =yH+ 40 mm
wG =wH+jGHjcos5
 = 180arcsinwGwE
jEGj(10)
The value of is then used to determine vHand using:
Fig. 5. Top view of the hexapod robot.
vH =yE+jEGjcosjGHjsin5(11)
=+arctanjICj
jIDj(12)
Once is known, we can calculate the coordinates of the
point C with respect to the (v,w)-frame using next equations:
(
vC =vD+jDCjcos
wC =wD+jDCjsin (13)
Then the angles and
(Fig. 4) are calculated using the
coordinates ( vC,wC) and trigonometric relationships:
8
>>>>>><
>>>>>>:jACj=p
v2
C+w2
C
 =arctanwC
vC

=arccosjBCj2jABj2jACj2
2jABjjACj(14)
At last, all of the acquired information is combined in order
to compute the corresponding knee and hip servo angles (i.e.
and):
8
>><
>>:L =vH30mm
 =arcsinzH
L
 = 180+
+(15)
Note that the forward and inverse kinematics of the right legs
can be acquired by similar reasoning.
V. B ODY LEVELLING
Depending on the situation at hand multiple different leg
configurations could be applied to counteract the roll and pitch
values measured by the IMU. Therefore, some constraints
need to be imposed. First of all the levelling procedure
starts with the current hip angles as a fixed given. Although
this decreases the flexibility of overcoming a steeper slope,
it reduces the levelling options by a certain amount alongCONFIDENTIAL. Limited circulation. For review only.
Preprint submitted to 21st International Conference on System
Theory, Control and Computing. Received June 7, 2017.

with the complexity of the algorithm. Secondly, the algorithm
begins by checking which leg is positioned lowest onto the
slope (i.e. evaluating the signs of the measured pitch and roll).
Once the robot knows which leg is situated most downhill,
it starts with extending this leg to its maximum limit, thus
choosing the minimal reachable y-coordinate known from the
kinematic study in section IV. Together with the constraints
imposed on the hip angles mentioned earlier, the downhill
leg’s actuators are completely determined. For the further
explanation of the algorithm let us consider that LF (left front
leg) is fully extended.
The next steps in the algorithm define the plane in which
the feet of the hexapod should be put in order to counteract the
measured pitch and roll. Since a plane can be defined with only
three points, besides LF only two other leg tip positions need
to be identified. RF (right front leg) is chosen such that the
height difference between the RF and LF leg tips compensates
the measured roll (Fig. 6(a)), and similarly, LR (left rear leg)
such that the LF and LR height difference compensates pitch
(Fig. 6(b)). Following this reasoning we have:
tan =yLF;HyRF;H
xRF;H + 177xLF;H(16)
xRF;H = (vRF;H + 30)cos =:::=f(yRF;H ) (17)
tan =yLR;HyLF;H
zLF;H + 272 +zLR;H
(18)
=yLR;HyLF;H
zLF;H + 272 +f(yLR;H )
wherexRF;H refers to the x-position of the right front end
effector (i.e. leg tip) with respect to the fixed hip coordinate
frame,vRF;H is the same but with respect to the rotatable knee
frame, andis the known hip angle.
Finally these three leg tip positions are transformed to
one common coordinate frame enabling the expression of the
plane in its Cartesian equation (ax+by+cz=d). Now the robot
is able to redirect the remaining legs to fit the calculated
plane. For example, for RM (right middle leg) this is done by
solving a set of three equations, namely the Cartesian equation,
xRM;H =f(zRM;H )andxRM;H =f(yRM;H )(obtained from
the inverse kinematics).
VI. R ESULTS
To provide proof of concept the six-legged robot is placed
randomly (i.e. random yaw value) on a plain slope of which
the inclination can be adjusted. Due to physical limitations
imposed by the robot design the maximum inclination that
can be dealt with is about 10. When placed on the slope
the deviation from the desired horizontal body orientation
(zero pitch and roll) is detected using the IMU and the body
levelling algorithm starts. Figures 7-8 show two measurements
obtained for a slope with an inclination of 7.6. During the
first few seconds (about 4 s), the system computes the servo
(a) Front view: Level roll principle.
(b) Right side view: Level pitch principle.
Fig. 6. Body levelling principles.
positions necessary to counteract the observed inclination.
Once these are obtained, the servo commands are sent out
and the hexapod readjusts its configuration. This process is
repeated until the measured pitch and roll values lie within
a predefined region around the desired 0-orientation (0:3
for Fig. 7 and0:035for Fig. 8).
In Fig. 7 the robot is placed on the slope such that the initial
pitch and roll deviation are about 3.8and6respectively.
It takes two iterations over a time of 6 s to reach the desired
horizontal body orientation. The small offset that remains is
due to the chosen boundary width of the region around the 0-
orientation. In Fig. 8 the hexapod is placed differently on the
slope and the predefined convergence boundaries are chosen
to be stricter (0:035instead of0:3) such that the offset
is minimised. Now it takes about 20 seconds and several steps
to reach the horizontal body pose, but the steady-state error
has been reduced significantly.
The results show that the algorithm is successful in levelling
the hexapod’s body horizontally, however it is rather slow
in doing so. This lack of speed is caused by the fact that
the computations needed to solve the kinematic model cor-
responding to the given hexapod design are quite demanding
(e.g. nonlinear solvers) in light of the limited computational
capabilities of the single-board embedded computer (BBB).
Moreover in all the cases it takes multiple steps to reach
the horizontal orientation. Besides by the influence of the
predefined convergence boundaries and wrongly measuredCONFIDENTIAL. Limited circulation. For review only.
Preprint submitted to 21st International Conference on System
Theory, Control and Computing. Received June 7, 2017.

0 5 10 15 20 25
Time t (s)-6-5-4-3-2-101234Pitch θ (°)
0 5 10 15 20 25
Time t (s)-6-5-4-3-2-101234Roll φ (°)Fig. 7. Body levelling when placed on a slope with 7.6inclination. Initial
pitch and roll deviation of about 3.8and -6respectively. Convergence
boundaries set equal to 0:3.
0 5 10 15 20 25
Time t (s)-2-1012345678Pitch θ (°)
0 5 10 15 20 25
Time t (s)-2-1012345678Roll φ (°)
Fig. 8. Body levelling when placed on a slope with 7.6inclination. High
initial pitch deviation. Stricter predefined region of convergence, namely
0:035instead of0:3.
model parameters, this can be explained by the absence of
a local controller for each servo. As the higher level control
calculates the wanted servo angles there is no lower level
control which ensures that these are really obtained. Therefore,
a more powerful processing unit and the addition of local
controllers for each joint servo will probably deliver even more
satisfying results.
VII. C ONCLUSIONS
In this paper an algorithm to level a hexapod robot placed
on a plain slope is proposed. The algorithm involves the use of
the robot’s kinematic model and some geometric relationships
to calculate how the leg tips should be redirected in a plane
able of counteracting the orientation angles (i.e. pitch and roll)
measured by an inertial measurement unit. Experiments proveto be successful but the computations needed to solve the
kinematic model of the hexapod robot are quite demanding
for the single-board embedded computer, resulting in a rather
slow response. Besides the obvious use of a more powerful
processing unit, a simpler redesign of the hexapod’s leg con-
figuration (i.e. abandoning the leg lift mechanism implemented
in the current design and using three degrees of freedom) could
greatly speed up the levelling process. This conclusion stresses
the importance of the design phase in the realisation of any
application.
In a next step, the implemented inclination control could
now be used to let the robot mount slopes with its body
horizontally which is highly profitable from a mechanical point
of view. However, the suggested approach is limited to fairly
even slopes due to its reliance on the Cartesian plane equation
so when rougher, more irregular slopes have to be overcome
another method should be adopted (e.g. extend the sensory
system with 3D force sensors in each of the legs).
ACKNOWLEDGMENT
The authors would like to thank Simon Cherlet and Pieter
Maelegheer for their assistance related to the robot develop-
ment.
REFERENCES
[1] M. Billah, M. Ahmed, and S. Farhana, “Walking hexapod robot in
disaster recovery: developing algorithm for terrain negotiation and
navigation,” in Proc. of World Academy of Science, Engineering and
Technology , vol. 42, 2008, pp. 328–333.
[2] E. Karalarli, A. M. Erkmen, and I. Erkmen, “Intelligent gait synthesizer
for hexapod walking rescue robots,” in Proc. IEEE Robotics and
Automation (ICRA’04) , vol. 3, 2004, pp. 2177–2182.
[3] M. Massari, P. Massioni, S. Nebuloni, G. Sangiovanni, and F. Bernelli-
Zazzera, “Realization and control of a prototype of legged rover for
planetary exploration,” in Proc. IEEE Advanced Intelligent Mechatronics
(ASME’05) , 2005, pp. 863–868.
[4] Ash, “Pulstech Oy Timberjack Walking Harvester,”
http://www.treeblog.co.uk/viewpost.php?id=177, 2008, accessed
May 3, 2015.
[5] M. Agheli, L. Qu, and S. S. Nestinger, “Shero: Scalable hexapod
robot for maintenance, repair, and operations,” Robotics and Computer-
Integrated Manufacturing , vol. 30, no. 5, pp. 478–488, 2014.
[6] K. Nonami, Q. Huang, D. Komizo, Y . Fukao, Y . Asai, Y . Shiraishi,
M. Fujimoto, and Y . Ikedo, “Development and control of mine detection
robot COMET-II and COMET-III,” JSME International Journal Series
C, vol. 46, no. 3, pp. 881–890, 2003.
[7] T. Fukuda, Y . Adachi, H. Hoshino, K. Kosuge, I. Matsunaga, and F. Arai,
“Posture control of 6-leg walking robot,” in Proc. IEEE Robotics and
Automation , vol. 1, 1995, pp. 1006–1011.
[8] G. Dudek and M. Jenkin, Computational principles of mobile robotics .
Cambridge university press, 2010.
[9] R. Sharma, H. Daniel, and F. Du ˇsek, “Sensor fusion: An application to
localization and obstacle avoidance in robotics using multiple ir sensors,”
inNostradamus 2014: Prediction, Modeling and Analysis of Complex
Systems . Springer, 2014, pp. 385–392.
[10] S. Cherlet and P. Maelegheer, “Developing an autonomous hexapod
robot for environmental exploration,” Master’s thesis, Ghent University,
Belgium, 2015.
[11] W. Keal, “Motion determination,” U.S. Patent
13/164,136, Dec. 20, 2012. [Online]. Available:
https://www.google.be/patents/US20120323520CONFIDENTIAL. Limited circulation. For review only.
Preprint submitted to 21st International Conference on System
Theory, Control and Computing. Received June 7, 2017.