Improving low-cost inertial-measurement-unit (IMU)-based motion tracking accuracy for a biomorphic hyper-redundant snake robot
Yang et al. Robot. Biomim.
Improving low-cost inertial-measurement-unit (IMU)-based motion tracking accuracy for a biomorphic hyper-redundant snake robot
Weixin Yang 0
Alexandr Bajenov 0
Yantao Shen 0
0 Electrical and Biomedical Engineering, University of Nevada , Reno, N. Virginia St, Reno, NV 89557 , USA
This paper develops and experimentally validates a 3D-printed snake robot prototype. Its structure is designed to allocate limited room for each functional module (including an external power module, battery power module, the wireless control and transmission module and some detective sensors), so as to ensure the snake robot works in different environments. In order to control and track the snake robot, a low-cost MEMS-IMU (micro-electro-mechanical systems inertial measurement unit)-based snake robot motion tracking system is developed. Three algorithms (low-pass filter, baseline calibration, and Kalman filter) are used to eliminate noise from IMU's acceleration data, thus minimizing the noise influence to tracking accuracy. Through signal processing, the IMU acceleration data can be effectively used for motion tracking. The result from the video tracking software is employed as a reference for comparison, so as to evaluate the motion tracking algorithm efficiency. The comparison results demonstrate high efficiency of the proposed IMU-based motion tracking algorithm.
Snake robot; 3D printing; IMU; Motion tracking
Biological snake has a cord-like body with many links
(bones) which can form diverse locomotion modes with
many functions . Snake is one of the most
successfully evolved species, which has lived on the earth for 300
million years because of its high adaptation to the
environment. Different locomotion modes also have various
functions. Some snakes have the ability to change their
movement mode immediately when entering into a new
environment. Because of the snake’s versatility, the study
of snake locomotion mode promises many future
applications such as space exploration, environmental
monitoring, transportation, and surveillance.
According to the research on the biological snake,
the snake-inspired robot has extremely high
redundancy with many degrees of freedom, with its body on
the ground and kinetically stable. This feature makes
the snake easily adapt to irregular terrain. Based on this
high adaptability, the snake robot can be adopted where
human beings cannot go to carry out tasks. The snake’s
locomotion modes can be categorized into following four
) serpentine locomotion; (
) rolling locomotion; and (
locomotion. In this paper, two locomotion modes are developed.
The serpentine movement is the most typical locomotion
mode, which has been widely observed in almost all
species of snakes. According to Hirose’s  research on the
biological snake, the research starts with generating the
serpentine curve for the snake robot . To test
additional locomotion performance of the robot, sidewinding
is further investigated and performed on the developed
The snake robot is designed to collect information in
some challenging environments, such as forest, cave,
building ruins, or even underwater. Most of these
environments are GPS-denied, leading to the difficulties in
tracking and controlling snake robot. Hence, it is
necessary to consider and understand the efficiency of each
locomotion mode in different environments based on
collected terrain information. Many studies on object’s
attitude determination with MEMS-IMU have been
performed for many years. MEMS-IMU-based tracking
technology  is widely applied in many fields, such as
detection of unconstrained walking , pedestrian
tracking in indoor environments [6, 7].
However, IMU-based tracking system calculates the
position by double integration of measured acceleration,
and with double integration, calculation errors grow
rapidly, thus the system being not reliable for a long-time
tracking without the aid of GPS or another reference
system. Hence, this research focuses on accuracy of
IMUbased tracking system by using an improving acceleration
To eliminate the effects of accelerometer errors, most of
the papers adopt some sensor fusion algorithms, among
which the Kalman filter is the most commonly used. In
[8–10], standard Kalman filter, unscented Kalman filter
(UKF), and extended Kalman filter (EKF) are employed
in high data rate signal processing. EKF has lower
accuracy; however, UKF requires more computational time.
The paper is organized as follows. In "Custom-built
snake robot" section, a customized snake robot is built
through 3D printing. "Snake robot trajectory tracking"
section presents the motion tracking algorithm along
with the three signal processing algorithms including
Butterworth low-pass filter, baseline calibration, and
Kalman filter. After data processing, the acceleration data
can be used for motion tracking. The conclusion and
discussion are given in "Conclusion" section.
Custom‑built snake robot
The snake robot joint structure is modularly designed,
with the upper part and the lower part being designed
separately. Therefore, different chassis can be used based
on the environment. The snake robot is composed of
eight identical modular units. The joint model size is
50 mm in diameter and 144 mm in length. The volume of
the snake robot’s body is widened by increasing the body
width, and thus the integrated electronic system can be
installed for controlling. This snake robot has two
different bending axes connected alternately at 90 ◦C. This
allows the snake robot to have 3D motion because of the
2 degrees of freedom for each joint (total 16 degrees of
freedom). Moreover, each joint can be controlled
separately. The hollowed-out grids structure on robot’s body
increases the structure flexibility, thus enhancing its
coefficient of restitution and making the robot run efficiently.
The 3D-printed snake robot is shown in Fig. 1.
Snake robot trajectory tracking
Motion tracking algorithm
Snake robot displacement can be obtained by the double
integral of its acceleration. A low-cost IMU is installed in
the head of the snake robot, in order to collect its
realtime acceleration data at three axes with high frequency.
At the same time, all acceleration data are transferred to a
computer for calculation. Then the robot’s instant speed
curve and accumulated space trajectory are graphed on
the screen. Acceleration data are processed by MATLAB.
The basic algorithm is shown in Fig. 2.
Assume sampling starts at time t0. According to the
integration principle, the calculation relationship among
displacement s(t), velocity v(t), and acceleration a(t) in
the continuous-time domain is:
s(t0) is accumulative displacement from 0 to t0, v(t0) is the
system instant motion velocity at t0.
Since the outputs of the IMU are groups of discrete
data, the time–speed curve is decomposed as numbers
of a right-angled trapezoid. Initials(t0) = 0, the
displacement s(t) is presented as:
s[n] = n · v · [(n − 1) · a + (n − 2) · a
+ · · · + v[n − 1]] · t2 +
(a + a[n]) · t2
Through Eq. (
), it is easy to calculate the displacement
after obtaining the initial velocity and acceleration from
the IMU. However, based on this equation, the
calculation is very complicated for large n which leads to huge
burden to the system since the system needs to allocate
huge memory sources to store the acceleration data
from time number 0 to n. Furthermore, the system has
to repeat all the previous acceleration calculations to
update the object’s motion displacement. Thus, iteration
is applied to simplify the calculation.
From Eq. (
v[n] − v[n − 1] =
= v[n − 1] · t + 41 (a[n − 1] + a[n]) · t2
) and (
) are modified as:
v[n] = v[n − 1] +
a[n] + a[n − 1]
s[n] = s[n − 1] +
v[n] + v[n − 1]
= s[n − 1] + v[n − 1] · t + 41 (a[n − 1] + a[n]) · t2
From Eqs. (
) and (
), the object’s instant speed v[n]
and motion displacement s[n] can be calculated
recursively. The IMU has the acceleration outputs at X-, Y- and
Z- axis. Based on Eq. (
), the object’s instant speed at
X-, Y- and Z-axis can be presented as:
vx[t] = v¯x[t −
vy[t] = v¯y[t −
vz[t] = v¯z[t −
a¯x[t] + ax[t −
a¯y[t] + ay[t −
a¯z[t] + az[t −
The object’s instant speed in 3D space at time t can be
Similarly, the object’s motion displacement at X-, Y- and
Z-axis is presented as:
v[t] = v¯x[t] + v¯y[t] + v¯z[t]
sx[t] =sx[t −
t] + vx[t −
t] + ay[T ]) · t2
t] · t
t] + az[T ]) · t2
During time t −
t, the object’s space motion
displaces [t] = (sx[t] − sx[t − t])2 + (sy[t] − sy[t − t])2 + (sz[t] − sz[t − t])2
So at time t, the object’s space coordinate is
(sx[t], sy[t], sz[t]). In the space coordinate system, the
object’s trajectory can be plotted by connecting all the
space coordinate points from t0 to tn.
Noise processing of tracking system
Since the tracking system is based on the double
integral of acceleration, errors of IMU measurements cause
incorrect integration results and accumulated bias during
integration. Even minimal measurement errors will result
in huge calculation errors, leading to the low accuracy in
tracking results. This paper proposes several methods to
filter the acceleration data, so as to make the acceleration
data available to integrate. The flow diagram for data
processing is shown in Fig. 3.
IMU has noisy signals when stationary. To analyze the
impact of noise, 16,000 stationary data are collected, i.e.
4000 points for each axis plus 4000 starting points. Then,
calculate the average acceleration value of each axis,
finally, subtracting the average value from every single
data for each axis, thus getting the acceleration bias of
each axis which is shown in Fig. 4.
From Fig. 4, the acceleration vibrates randomly with a
maximum value of 0.05 m/s2. That means the object has
maximum 0.05 m shift when it keeps stationary.
Therefore, the errors have a great influence on the tracking
system accuracy. To eliminate noise, it is necessary to
analyze the signal properties in the frequency domain as
shown in Fig. 5.
After Fourier transform of the acceleration data, it is
clear in the above picture of Fig. 5 that the noise of each
axis is mainly around 50 Hz in stationary. From the lower
picture of Fig. 5, the valid acceleration data are mainly
The time-domain graph and spectrogram of
acceleration after Butterworth low-pass filtering of the stationary
snake robot are shown in Figs. 6 and 7 respectively.
It is clear in Fig. 7, there is noise only in the passband,
and its amplitude is very small.
After low-pass filter, the acceleration data can be used for
integration. Before integrating the data, the baseline shift
cannot be ignored.
The snake moves forward in a line and then stays
stationary. After low-pass filter, only the stationary
acceleration data of each axis are collected and plotted as shown
in Fig. 8.
The baseline shift in the X-axis is about − 0.3 m/s2, i.e.
the X-axis movement when tracking is − 0.3 m/s even if
the snake robot is stationary. Similarly, the baseline shift
in the Y-axis is about 0.3 m/s2, i.e. the Y-axis movement
is 0.3 m/s when stationary. When including the baseline
shift is in the integration, the cumulative errors will have
a significant influence on the system accuracy.
The absolute difference of acceleration presents the
relative change of acceleration data. When the snake robot
moves fast, the absolute difference becomes relatively
big. Also, the absolute difference can present the noise
oscillation when the snake robot maintains stationary.
MATLAB is used to plot the absolute difference
distribution of acceleration, which is shown in Fig. 9.
The above statistic shows that the data oscillation in
X-axis and Y-axis is within 0.05, while in the Z-axis
direction, the oscillation is in a wider range of 0.1. Table 1
shows the probability of absolute difference within the
range when the snake robot keeps stationary.
The results show the big probability of the absolute
difference is within 0.1 when the snake robot is stationary.
To compare with the moving acceleration data, the same
method is employed. The statistic of oscillation
distribution in moving state is shown in Table 2.
The distribution of absolute difference for motion data
is concentrated on a smaller interval comparing with
stationary data, which has been verified by repeated
Putting 1000 data from Y-axis in one group to
calculate the probability of range within [0, 0.05]. Five groups
are repeated, the results of which are shown in Table 3.
From Table 3, the data verify the previous conclusion that
the probability of the absolute difference within [0, 0.05]
in stationary is much higher than that of in moving state.
To calibrate the baseline, every 100 data (i.e. data in 0.2 s)
are taken for the statistic. The statistic shows whether the
probability of the absolute difference within [0, 0.05] is
higher than 0.95. If so, then the snake robot is stationary.
Then the average value of this group data is used as the new
average value to shift the acceleration data, i.e., adding or
subtracting the compensation value from acceleration data
(depending on the sign of acceleration data). At the same
time, once the data show that the snake robot is stationary,
the value of acceleration data is set to zero.
The baseline calibration results are shown in Fig. 10.
The bias after baseline calibration is zero when the snake
robot is stationary.
To eliminate random errors, the Kalman filter is applied
to the tracking system, thus filtering the acceleration
data. The Kalman filter is a set of mathematical equations
that provide an efficient computational (recursive) state
estimation, so as to minimize the mean squared error
The random process to be estimated can be modeled in
xk+1 = φk xk + wk + Bk uk
when the process state vector is:
xk = [Acce_x, Acce_y, Acce_z]
The initial process state vector is the initial set of
acceleration data. The observation (measurement) of the
process occurs at discrete points in time by the linear
relationship. φk is the state transition matrix. Hk is the
measurement matrix at time tk. The initial estimation
error covariance is manually selected as Pk−, where
zk = Hk xk + vk
Now all the data and equations are ready for the Kalman
filter loop. Once entering the loop, it will run to ad
infinitum. The data are transferred by the wireless chip to the
computer serial port, and then MATLAB will read the
data for Kalman filter algorithm.
Snake robot locomotion analysis
In the laboratory environment, IMU chip is installed
in the head of the snake robot which moves along a
straight line. The filtered acceleration data are shown
in Fig. 11. It clearly shows in the picture that the
highfrequency component of the acceleration is eliminated
and the random noise is minimized. The signal curve
becomes smooth and available to integrate. The IMU
tracking result of snake robot’s Serpentine
locomotion is shown in Fig. 12. It indicates that the serpentine
locomotion has the highest efficiency because the snake
robot doesn’t have slide-back either at X- or Y-axis.
Figure 13 gives the snake robot trajectory, showing the
forward locomotion in the X direction and sinusoidal
swing in the Y direction. The calculated trajectory
follows the real locomotion which proves the accuracy of
IMU tracking system.
To improve snake robot adaptivity in different
environments, the sidewinding locomotion mode is developed in
this paper. For better locomotion analysis, it also employs
a video tracking software. The 6 points video tracking
results are shown in the bottom picture of Fig. 14. In
the above picture, IMU tracking is used to replace video
tracking of point 1 which is shown by the green line.
Compared with video tracking result, IMU tracking still
has some glitches, thus leading to errors. The error
Let the snake robot move along a straight line. The
video recorder is set to be perpendicular to the snake
robot, in order to make the video tracking work well. The
snake robot’s locomotion in XY-plane is shown in Fig. 15.
Snake robot’s locomotion in Y-axis is used to calculate
IMU tracking which is shown in Fig. 16. Comparing with
the video tracking results, the IMU tracking still has
small errors. The results show that the maximum error
The absolute displacement and average velocity of the
two tracking systems are listed in Table 4.
Since the video tracking software has higher accuracy,
results of video tracking are used as the reference to
calculate the error of IMU. Repeating the experiment to find
out the average error of IMU tracking system, the results
are shown in Table 5.
Based on the experiments, the average displacement
error is 12.01%, and the average velocity error is 9.25%.
The error may come from
• Accelerometer accuracy.
• The measurement error due to mechanical
characteristics of MEMS under the influence of the
• Random noise.
This paper presents a solution to build a snake robot
with the tracking system. The tracking system/algorithm
can be used for locomotion analysis based on IMU data.
Experiments on the algorithm validate its effectiveness.
The algorithm can be further improved as the advanced
data processing methods.
YTS supervised the project. WXYdeveloped the motion tracking algorithm and
data processing algorithm, and also designed the experiments and
comparative experiments. AB modified the custom-built snake robot. All authors read
and approved the final manuscript.
The authors thank Yudong Luo and Na Zhao for helping simulation and
implementation of the project.
Weixin Yang, Bajenov Alexandr and Yantao Shen have no competing interests.
Availability of data and materials
The data sets supporting the conclusions of this article are
available in the Google Drive repository https://drive.google.com/
The project is partially supported by NASA CAN grant #NNX13AN15A and
NASA RID SEED grant NNX15AI02H.
Springer Nature remains neutral with regard to jurisdictional claims in
published maps and institutional affiliations.
1. Gray J. The mechanism of locomotion in snakes . J Exp Biol . 1946 ; 23 ( 2 ): 101 - 120 . http://jeb.biologists.org/content/23/2/101.full.pdf
2. Hirose S. Biologically inspired robots: serpentile locomotors and manipulators . Oxford: Oxford University Press; 1993 .
3. Ma S , Tadokoro N. Analysis of creeping locomotion of a snake-like robot on a slope . Auton. Robots . 2006 ; 20 ( 1 ): 15 - 23 . https://doi.org/10.1007/ s10514-006-5204-6.
4. Sabatelli S , Galgani M , Fanucci L , Rocchi A . A double-stage kalman filter for orientation tracking with an integrated processor in 9-d imu . IEEE Trans Instrum Measur . 2013 ; 62 ( 3 ): 590 - 8 . https://doi.org/10.1109/ TIM. 2012 . 2218692 .
5. Lee JK , Park EJ . Quasi real-time gait event detection using shank-attached gyroscopes . Med Biol Eng Comput . 2011 ; 49 ( 6 ): 707 - 12 . https://doi. org/10.1007/s11517-011-0736-0.
6. Faulkner WT , Alwood R , Taylor DWA , Bohlin J , David D , Taylor WA. Gpsdenied pedestrian tracking in indoor environments using an imu and magnetic compass . Proc Int Tech Meet Inst Navig Itm . 2010 ; 1 ( 2 ): 198 - 204 .
7. Foxlin E. Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter . In: Proceedings of the IEEE 1996 virtual reality annual international symposium; 1996 . pp. 185 - 194 . https://doi. org/10.1109/VRAIS. 1996 .490527
8. Corrales JA , Candelas FA , Torres F . Hybrid tracking of human operators using IMU/UWB data fusion by a kalman filter . In: 2008 3rd ACM/IEEE international conference on human-robot interaction (HRI) ; 2008 . pp. 193 - 200 . https://doi.org/10.1145/1349822.1349848
9. Zhang P , Gu J , Milios EE , Huynh P. Navigation with IMU/GPS/digital compass with unscented Kalman filter . In: IEEE international conference mechatronics and automation , vol. 3 . 2005 ; pp. 1497 - 15023 ( 2005 ). https://doi.org/10.1109/ICMA. 2005 .1626777
10. Sabatini AM . Quaternion-based extended kalman filter for determining orientation by inertial and magnetic sensing . IEEE Trans Biomed Eng . 2006 ; 53 ( 7 ): 1346 - 56 . https://doi.org/10.1109/TBME. 2006 . 875664 .
11. Speed Integration Principle. http://www.diangon.com/m22602.html
12. Discrete Kalman Filter. http://wolfweb.unr.edu/fadali/EE782/DiscreteKF. pdf