Biomorphic Hyper-Redundant Snake Robot: Locomotion Simulation, 3D Printed Prototype and Inertial-Measurement-Unit-Based Motion Tracking

University of Nevada, Reno

A thesis submitted in partial fulfillment of the requirements for the degree of Master of Science in Electrical Engineering

Weixin Yang

Dr. Yantao Shen/Thesis Advisor

December 2016

THE GRADUATE SCHOOL

We recommend that the thesis

prepared under our supervision by

WEIXIN YANG

Entitled

Biomorphic Hyper-Redundant Snake Robot: Locomotion Simulation, 3d Printed Prototype And Inertial-Measurement-Unit-Based Motion Tracking

be accepted in partial fulfillment of the

requirements for the degree of

MASTER OF SCIENCE

Yantao Shen, Ph.D, Advisor

M. Sami Fadali, Ph.D, Committee Member

Hung (Jim) La, Ph.D, Graduate School Representative

David W. Zeh, Ph.D., Dean, Graduate School

December, 2016

ABSTRACT

Snakes are one of the most successful species in the world due to the high adaptability in most environments of the earth. A snakebot is a biomorphic hyper- redundant robot that resembles a biological snake. Snakebots are most useful in situations where their unique characteristics give them an advantage in their locomotion environments. These environments tend to be long like pipes or highly cluttered like rubble. Thus, snake robots are currently being developed to assist search and rescue tasks in complex environments.

This thesis proposes a detailed architecture to develop a snakebot including building its mathematical model based on statistical geometry analysis and kinematic force analysis; verifying the mathematical model using the numerical simulation; mechanical structure design of the robot; electric system and data transmission system design, and its locomotion data analysis and performance evaluation.

In the thesis, we clearly demonstrate that the simulation results prove both the proposed mathematical model and the mechanical design of the robot; the IMU data analysis results agree with the simulation results. To increase the precision level of the IMU based motion tracking for the robot, the Kalman Filter and trapezoid integration algorithms were applied to the data processing and analysis. Finally, the achieved results validate the effectiveness of these applied algorithms.

The developed snakebot promises huge potentials in future applications. In our future work, we’ll integrate sensors like camera, force sensing units, and others with the robot, so as to facilitate its control capability with sensor feedback functions. In addition, we will employ and test more powerful microprocessors to improve the real-time computing performance of the robot. Our goal is to make the snakebot an advanced intelligent mechatronic system for fulfilling future search and rescue tasks in the complex and challenging environments.

Acknowledgments

I would like to thank my thesis advisor Dr. Shen for his supervision. The door to Prof. Shen office was always open whenever I ran into a trouble spot or had a question about my research or writing. He consistently allowed this project to be my own work, but steered me in the right direction whenever he thought I needed it. Without his support, expertise, guidance, and most of all, patience, the completion of this thesis wouldn’t be possible.

I would also like to thank Dr. Fadali and Dr. Hung for their support by serving on my thesis committee. Dr. Fadali helped me in fully understanding basic principles of random signal processing. With his great patience, Dr. Fadali also took his great efforts and his time to help revising my thesis. In addition, I would thank Dr. Hung for his help in the robotic class. The control algorithms taught in the class are essentially helpful to my research work in the thesis.

Next, I would like to thank the following graduate students in our lab, Mr. Yudong Luo and Ms. Na Zhao. Their selfless sharing of experience and thoughtful discussions help me to solve many technical issues in the project. Ms. Na Zhao helped to design the mechanical structure of the robot with her patience and expertise. In addition, I would like to acknowledge that the research work was partially supported by NASA CAN grant # NNX13AN15A.

Finally, I must express my very profound gratitude to my parents for providing me with unfailing support and continuous encouragement throughout my years of study and through the process of writing this thesis. This accomplishment would not be possible without their support.

iii

Dedication

To my mother, Huazhi, my father, Yunchuan and my fiancée Aijing

Contents

1 Introduction ...... 1

1.1 Goal, Objectives, and Contribution ...... 1

1.2 Motivation ...... 2

1.3 Organization ...... 3

2 Background ...... 4

2.1 Modeling and analysis of snake robot locomotion ...... 4

2.2 Implementation of physical snake robots ...... 5

2.2.1 Snake robots without contact force sensors ...... 5

2.2.2 Snake robots with contact force sensors ...... 11

2.2.3 Summary ...... 11

3 Snake Robot Mathematic Model ...... 12

3.1 Static Analysis...... 12

3.2 MATLAB Simulation ...... 13

3.3 Kinematic Analysis ...... 18

4 Snake Robot Mechanical Model and Simulation ...... 24

4.1 The Structure Model of Snake Robot ...... 24

4.2 Simulation Environment Setting ...... 26

4.3 2D Simulation ...... 27

4.3.1 Serpentine Locomotion Simulation ...... 27

4.3.2 Serpentine Locomotion under Different Friction ...... 34

4.4 3D Snake Robot Sidewinding Locomotion Simulation ...... 36

5 Snake Robot’s Mechanical Structure and Integrated Electronic System Design ...... 40

5.1 Snake Robot Mechanical Design ...... 40

5.2 Voltage Booster and Adapter Boards...... 43

5.2.1 5V Booster & Charger ...... 43

5.2.2 Parallel Charging ...... 45

5.2.3 Adapter Boards ...... 46

6 Remote Control and Locomotion Analysis...... 49

6.1 Wireless Communication ...... 49

6.1.1 Xbee module ...... 49

6.1.2 Wireless Communication Protocol ...... 50

6.2 Snake robot’s trajectory tracking model...... 54

6.2.1 Trajectory tracking algorithm ...... 54

6.2.2 Snake robot’s absolute orientation (Euler vector) and visualization ...... 58

6.3 Tracking system noise processing and software compensation algorithm ...... 60

6.3.1 System noise analysis and processing ...... 60

6.3.2 Noise processing algorithms ...... 62

6.3.3 Time domain and spectral analysis ...... 62

6.3.4 Low-pass filter ...... 66

6.3.5 IMU acceleration data calibration ...... 71

6.3.6 IMU baseline calibration ...... 73

6.3.7 Kalman Filter ...... 81

6.4 Snake robot locomotion analysis ...... 89

6.4.1 Snake robot locomotion analysis and calibration ...... 89

7 Conclusion ...... 96

Appendix ...... 100

A MATLAB Files ...... 100

A.1 Data processing algorithm program ...... 100

A.2 Butterworth Filter, Kalman Filter program ...... 104

A.3 Processing program ...... 106

vii

List of Tables

Table 3.1 The angle of 훽1, 훽2, 훽3 and 훽4 in the simulation...... 17

Table 3.2 The angle of 훽1, 훽2, 훽3 and 훽4 in the simulation corresponding to time...... 17

Table 4.1 Velocities of snake robot at different amplitudes...... 28

Table 4.2 Velocities of snake robot at different angular frequencies...... 30

Table 4.3 Velocity of Snake robot at different phases...... 32

Table 6.1 Statistic of oscillation distribution when stationary...... 77

Table 6.2 Statistic of oscillation distribution when moving...... 78

Table 6.3 Probability of range within [0,0.05] ...... 79

Table 6.4 Absolute displacement and average velocity...... 94

Table 6.5 Errors of IMU tracking system and simulation...... 94

Table 6.6 Comparison of IMU tracking and video tracking...... 94

viii

List of Figures

Figure 2.1 The world’s first snake robot developed by Prof. Shigeo Hirose in 1972 [1]. ... 6

Figure 2.2 The snake robot ACM R3 developed at Tokyo Institute of Technology [2]...... 7

Figure 2.3 The snake robot S5 developed by Dr. Gavin miller [3]...... 7

Figure 2.4 The snake robot ACM R5 developed at Tokyo Institute of Technology [4]...... 8

Figure 2.5 The snake robot Uncle Sam developed at Carnegie Mellon University [5]...... 9

Figure 2.6 A snake robot with a miniature joint mechanism developed at Tokyo Institute of Technology [6]...... 9

Figure 2.7 The OmniTread snake robot developed at the University of Michigan, the robot has pneumatic joints and is covered by motorized tracks [7]...... 10

Figure 2.8 A snake robot with a skin drive propulsion system developed at Carnegie

Mellon University [8]...... 10

Figure 2.9 The snake robot 퐾푢푙푘표 developed at the Norwegian University of Science and

Technology. Each joint module is covered by force sensors in order to measure contract forces from the environment [9]...... 11

Figure 3.1 Simplified 4-Connection-Rod System...... 12

Figure 3.2 Plot of 훽2 versus 훽1...... 14

Figure 3.3 Plot of 훽4 versus 훽1...... 15

Figure 3.4 Plot of 훽3 versus 훽1...... 16

Figure 3.5 Snake’s Locomotion Curve Based on Static Mathematical Analysis...... 17

Figure 3.6 Four types of Snake Locomotion [11]...... 18

Figure 3.7 Forces and Torque in Skeleton Model [12]...... 20

Figure 3.8 Normal force distribution along the snake. (adopted from [12])...... 21

Figure 3.9 Snake lift part of its body when moving [12]...... 22

Figure 4.1 Side view of schematic diagram of the transmission mechanism of the snake robot...... 24

Figure 4.2 3D printed model of gears and prototypes...... 25

Figure 4.3 Snake robot’s joint mechanical structure...... 26

Figure 4.4 Plan of snake robot...... 26

Figure 4.5 Simulation environment setting...... 27

Figure 4.6 Serpentine gaits of snake in different amplitudes...... 28

Figure 4.7 The snake robot’s velocities corresponding to different amplitudes...... 29

Figure 4.8 Serpentine gaits of a snake robot in different angular frequencies...... 30

Figure 4.9 The snake robot’s velocities corresponding to different angular frequencies. 31

Figure 4.10 Velocities of snake robot in different phases...... 32

Figure 4.11 The snake robot’s velocities corresponding to different phase angles...... 33

Figure 4.12 Simulation results with different friction...... 35

Figure 4.13 Combination curve [13]...... 37

Figure 4.14 Snake robot sidewinding locomotion...... 38

Figure 4.15 3D locomotion simulation results...... 39

Figure 5.1 The body of snake robot...... 40

Figure 5.2 Connection of two joints...... 41

Figure 5.3 Base of snake robot...... 42

Figure 5.4 5V booster and charger...... 44

Figure 5.5 Parallel charging setup for 2x 1s packs...... 45

Figure 5.6 8 Battery packs parallel charging...... 46

Figure 5.7 Connection board...... 47

Figure 5.8 Power source switch board...... 47

Figure 5.9 Battery enable board and switch...... 48

Figure 6.1 Wireless control diagram...... 49

Figure 6.2 Xbee module and adapter...... 50

Figure 6.3 Nine DOF sensor BNO055...... 54

Figure 6.4 Speed-time curve...... 55

Figure 6.5 Robot’s absolute orientation when steering...... 59

Figure 6.6 Robot’s absolute orientation when rolling...... 60

Figure 6.7 Data processing flow diagram...... 62

Figure 6.8 Acceleration for a stationary system...... 63

Figure 6.9 Signal properties in frequency domain...... 65

Figure 6.10 RC low-pass filter...... 66

Figure 6.11 Frequency responses of low pass filters (adopted from [14])...... 67

Figure 6.12 Butterworth low pass filter frequency response...... 68

Figure 6.13 Time-domain graph of acceleration value after Butterworth low pass filter when it keeps stationary (red = filtered acceleration)...... 69

Figure 6.14 Spectrogram of acceleration after Butterworth low pass filter when the snake moving...... 70

Figure 6.15 IMU data calibration...... 72

Figure 6.16 Baseline calibration test...... 74

Figure 6.17 Acceleration data between 5000 and 8000...... 74

Figure 6.18 Absolute difference of acceleration data when stationary...... 76

Figure 6.19 Absolute difference of acceleration data when moving...... 78

Figure 6.20 Acceleration data after baseline calibration...... 80

Figure 6.21 Baseline calibration results...... 81

Figure 6.22 Acceleration data when stationary...... 84

Figure 6.23 Kalman filter loop [15]...... 86

Figure 6.24 Acceleration data processing algorithm test results...... 87

Figure 6.25 snake displacement along each axis...... 88

Figure 6.26 IMU’s tracking trajectory in 3D space...... 89

Figure 6.27 Snake robot acceleration data...... 90

Figure 6.28 Snake robot velocity...... 91

Figure 6.29 Snake robot track distance in X, Y and Z axes...... 92

Figure 6.30 Comparison between video tracking and IMU tracking results...... 93

1 Introduction

This chapter focuses on the research goal, objectives, contribution, and motivation. The organization of this thesis will be discussed at the end of its chapter.

1.1 Goal, Objectives, and Contribution

The goal of this thesis is to use a mathematical model to characterize the snakes’ locomotion investigating biological snakes and then apply the mathematical model in a custom-built snake robot to imitate the biological snakes. Based on the snakes’ good adaptability in different terrains, snake-inspired robots are expected to be applied in many future applications. Specifically, the objectives to achieve this goal are:

1. Establish a mathematical model for a biological snake by analyzing the habits of the snake. 2. Build a mechanical snake model through observing the snake’s biological structure using the software Solid Work. 3. Test to find the optimal parameters of the snake mathematical model using the simulation software SimWise4D. 4. Create a snake-inspired robotic system capable of different snake locomotion with the potential of auto-control through propulsion of servomotors in the X- axis and the Y-axis. 5. Retrieve snake’s locomotion data wirelessly through the Inertial Measurement Units (IMU) mounted in different places in the snake to track snake’s motion curve.

The main contribution of this research is to provide a model to imitate biological snake, which can be used to develop a multi-terrain adaptive snake robot.

1.2 Motivation

Biological snakes have very simple cord-like body comprised of many links (bones). The simple cord-like body can form diverse locomotion modes with many functions. The snake is one of the most successfully evolved species; they have lived on the earth for 300 million years because they are highly adapted to their environment. Different locomotion modes also have different functions. Some snakes have the ability to change their movement modes immediately when entering a new environment. Snake have the function of a leg when moving from one place to another, an arm when coiling around something to hold it. Because of the snakes’ versatility, the study of snake locomotion modes promises many future applications such as space exploration, environmental monitoring, transportation and surveillance; a micro snake robot can ever be utilized as an active endoscope to go into intestines.

Based on the research on the biological snake, the snake-inspired robot has extremely high redundancy with many degrees of freedom, and the robot’s body is on the ground and kinetically stable. This feature makes the snake easily adapt to irregular terrain. Based on this high adaptability, the snake robots can be sent where human beings cannot go to carry out tasks. The snake’s locomotion modes can be categorized into following four types: 1) serpentine movement, 2) rectilinear movement, 3) concertina movement, and 4) sidewinding movement. Based on Hirose’s (Hirose, 1993) real snake research, the serpentine movement is the most typical locomotion mode; it has been widely observed in almost all species of snakes. In this project, two locomotion modes are derived through the simulator. Therefore, the research starts with generating the serpentine curve for the snake robot using the simulation software SimWise4D. For the high adaptability of environments, sidewinding locomotion mode is formed; it is the fastest locomotion mode but not the most effective since double motors are needed to form sidewinding locomotion. We successfully built a snake robot through 3D printing.

Based on this custom-built snake robot, we investigated locomotion modes of the controlled robot to imitate biological snakes.

1.3 Organization

This thesis is organized as follows. Chapter 2 reviews previous researches. Chapter 3 discusses a mathematical model through static geometry and kinematics analysis and derives the basic locomotion equations of snake robot. Chapter 4 builds a mechanical model of the snake robots through simulation software SimWise4D. The implement equations derived in Chapter 3 are applied in the simulation to test the snake’s movement in a simulation environment. Different sets of parameters are applied to the implement equations to find out the optimal one. The speed comparison figures under different parameters are also derived. Chapter 5 presents a modified mechanical design of the snake robot based on the mechanical model of Chapter 4. Since two power systems, wireless control and a data transmission system and sensors have to be mounted on the snake’s body, the modified structure design distributes the space to each system and avoids interference. The snake-scale inspired surface allows the snake to move smoothly on the ground. The characterization results are also presented in chapter 5. In chapter 6, based on the snake’s locomotion features, a wireless control protocol is made to transfer parameters through the remote controller. The software is based on the Arduino. It gives the Analysis of snake’s motions through the data from the IMU in the snake’s head. The IMU is controlled by a single microchip to avoid interference from the PWM signals and motor noises. Because of the single control, the IMU acceleration and angular data are very precise and in real time, after the deviation calculation, the speed and displacement of the snake are derived precisely. In addition, the IMU is calibrated separately to make sure that the IMU works reliably. Concluding remarks and future work are in Chapter 7. Appendices are found at the end of the thesis.

2 Background

Inspired by the special locomotion methods of the biological snake, snake robots have the potential to satisfy the robotic mobility in different environments. The snake robots typically consist of serially connected joint modules capable of bending in one or more planes. Many degrees of freedom make snake robots difficult to control but give them the ability to travel in irregular and challenging environments that conventional wheeled robots cannot reach.

Research on snake robots has been conducted for several decades. Early snake robot’s locomotion analysis has been reported by Gray [10] in the 1940s. and Hirose developed the first snake robot in 1972 [1]. In the last 20 years, many types of research have focused on the analysis of the snake robot’s locomotion, many types of bionic locomotion have been developed base on the research which provides many suggestions for building a model to develop and control snake robots. The majority of literature on snake robots so far has focused on locomotion over flat surfaces. But there is a growing tended to let the snake robot move in the challenging environments. In the authors’ opinion, this is important to realize snake robots’ potential locomotion capabilities in the future.

2.1 Modeling and analysis of snake robot locomotion

Research on snake robots is inspired by the robust motion capabilities of biological snakes. These amazing creatures have emerged on earth through millions of years, and have evolved to adapt to most of its environments. Biomechanical studies of snakes are therefore relevant to research on snake robots.

One of the earliest analytical studies of snake locomotion methods was proposed by Gray in1940s [10], he proposed a mathematical description of the force acting when the snake is moving and provided properties of snake locomotion. One of the conclusions of Gray [10] was that forwarding motion on a flat surface requires the existence of external forces acting in the normal direction to the snake body.

Hirose [1] studied the biological snake and built a mathematical model as a continuous curve that cannot move sideways. A famous result proposed by Hirose [1] is the serpenoid curve (the most common form of snake locomotion) equation. Hirose discovered that a close approximation to the shape of a biological snake during lateral undulation is given by a planar curve whose curvature varies sinusoidally. Hirose also proposed mathematical descriptions of how external factors, such as the ground friction and temperature, affect the snake locomotion efficiency and the shape of the locomotion curve.

2.2 Implementation of physical snake robots

In this section, demonstrations of physical snake robots from the literature are presented. The snake robots’ locomotion in unknown and in cluttered environments relies heavily on the ability to sense their surroundings so consider snake robots with force sensor without detection sensors. With this partitioning, this section clearly illustrates that in earlier research, the ability to sense the environment is limited. The driving modes of snake robots can also be categorized into passive wheels and active propulsion. Snake robots with passive wheels have advantages during motion across a flat surface while snake robots equipped with active propulsion have move complicated propulsion system.

2.2.1 Snake robots without contact force sensors

The world’s first snake robot was developed by Hirose as early as 1972 [1] The snake robot is shown in Figure 2.1 and equipped with passive wheels. The friction between passive wheels and ground enables Hirose’s snake robot to move forward on flat surfaces. Several other snake robots with passive wheels have been proposed over the years. Illustrations are shown in Figure 2.2 [2], Figure 2.3 [3] and Figure 2.4 [4]. Some of the robots can only have locomotion on a flat surface while some others have the ability to move their link both horizontally and vertically. The two degrees of freedom of each modular link enable the snake robot to have 3D locomotion, which improves the robot’s adaptability to environments.

Some robots have shielded joint modules that enable motion in some special environments like mud and dust, and even enable locomotion under water. A common feature of these snake robots is that they are generally able to move across relatively flat surface since the passive wheels block the motion in the cluttered environment. Such mechanisms are therefore limited to run in the lab environment but not for practical application in the challenging environment [16].

Figure 2.1 The world’s first snake robot developed by Prof. Shigeo Hirose in 1972 [1].

Figure 2.2 The snake robot ACM R3 developed at Tokyo Institute of Technology [2].

Figure 2.3 The snake robot S5 developed by Dr. Gavin miller [3].

Figure 2.4 The snake robot ACM R5 developed at Tokyo Institute of Technology [4].

Snake robots without wheels, i.e. robots basically with straight links interconnected by motorized joints. Despite its lack of wheels, the snake robot maintains an anisotropic ground friction property since the underside of each link has edges, or grooves, that run parallel to the link. These robots can, therefore, move forward by lateral undulation through purely planar motion. Robots whose ground friction properties are isotropic, on the other hand, can move forward during lateral undulation by resorting to sinuslifting, i.e. by slightly lifting the peaks of the body wave curve from the ground. However, snake robots with isotropic friction are mostly used for studying gaits other than lateral undulation, such as gaits based on sidewinding, inchworm motion, or lateral rolling. Illustrations are shown in Figure 2.5 [5] and Figure 2.6 [6] focuses on the development of small, light-weight, and strong joint actuation mechanisms, which are important for many future applications of snake robots [16]. There are also some snake robots equipped with active propulsion along the body, such as these equipped with motorized wheels

9 or by employing two motors in each link to generate a screw drive motion as is shown in Figure 2.7 [7] and Figure 2.8 [8], respectively.

Figure 2.5 The snake robot Uncle Sam developed at Carnegie Mellon University [5].

Figure 2.6 A snake robot with a miniature joint mechanism developed at Tokyo Institute of Technology [6].

Figure 2.7 The OmniTread snake robot developed at the University of Michigan, the robot has pneumatic joints and is covered by motorized tracks [7].

Figure 2.8 A snake robot with a skin drive propulsion system developed at Carnegie Mellon University [8].

2.2.2 Snake robots with contact force sensors

The environment sensing for snake robots in previous research is limited. Early in 1972 [1], Hirose equipped snake robot with contact switches, which enable the snake robot to have basic obstacle avoidance ability. In later research, some snake robots with active wheels were equipped with a three-axial force sensor. The translational forces are measured by force sensor on the wheel axis based on optical range measurements. Also the snake robot shown in Figure 2.9 [9] is in ball-shape joint modules equipped with force sensors mounted underneath the shell of each module.

Figure 2.9 The snake robot 퐾푢푙푘표 developed at the Norwegian University of Science and Technology. Each joint module is covered by force sensors in order to measure contract forces from the environment [9].

In this thesis, I designed a snake robot with passive wheels equipped with IMU to track its trajectory which is presented in Chapter 5.

2.2.3 Summary

This chapter covered the fundamentals of modeling and analysis of snake robot locomotion, reviewed the current state-of-the-art implementation of physical snake robots. I developed my snake robot based on previous research. The static and kinematic analysis are applied to the snake robot force analysis which is presented in Chapter 3. And the mechanical structure of physical snake robot with passive wheels is presented in Chapter 5.

3 Snake Robot Mathematic Model

3.1 Static Analysis

Based on Hirose [1] studies, a simplified 4-connection-rod mathematical description of the snake robot is presented in section 3.1. A more complicated kinematic analysis of snake locomotion is presented in Section 3.3.

For static mathematic analysis, the snake robot can be regarded as a redundant connecting rod system and then simplified as a four-connection-rod system with 1 degree of freedom, which is shown in Figure 3.1.

Figure 3.1 Simplified 4-Connection-Rod System.

The length of each rod, 푃𝑖+2푃𝑖+1 = a; the across length, 푃𝑖−1푃𝑖+2 = 푑, the counter clockwise direction as the positive direction. 훼𝑖, 푖 = 1, 2, 3, 4 is the angle between

푖푡ℎ rod and the horizontal direction. 훽𝑖, i = 1, 2, 3, 4 is the angle between adjacent rods 푖 and 푖 − 1 . Therefore, we have the following equations through the geometrical relationship of quadrilateral:

훼1 = 훽1 훼 = 훽 + 훽 = −훽 − 훽 { 2 1 2 3 4 (3.1) 훼3 = 훽1 + 훽2 + 훽3 훼4 = 휋 − 훽4

According to the vector direction of the quadrilateral, we have:

a푒𝑖훼1 + a푒𝑖훼2 = 푑 + a푒𝑖훼4 (3.2)

The real part of the above equation:

푑 cos(훼 ) + cos(훼 ) = + cos(훼 ) (3.3) 1 2 푎 4

The imaginary part is:

sin(훼1) + sin(훼2) = sin (훼4) (3.4)

The relations between 훼1 and 훼2 , 훼1 and 훼4 , 훼1 and 훼3 , can be obtained from equations (1.1), (1.2) and (1.3), which are:

푑 푑 2 1 + 2 ∗ cos(훼2 − 훼1) − 2 ∗ ∗ (cos(훼1) + cos(훼2)) + ( ) = 0 푎 푎 푑 푑 2 (3.5) 1 + 2 ∗ cos(훼4 − 훼1) − 2 ∗ ∗ (cos(훼4) − cos(훼1)) + ( ) = 0 푎 푎 { 훼3 = 훼4 − 훼1 − 휋

Since the main parameters of motor control are the relative angle between two adjacent rods, replacing 훼1, 훼2, 훼3 and 훼4 with 훽1, 훽2,훽3 and 훽4 gives:

푑 푑 2 1 + 2 ∗ cos(훽2) − 2 ∗ ∗ [cos(훽1) + cos(훽1 + 훽2)] + ( ) = 0 푎 푎 푑 푑 2 (3.6) 1 + 2 ∗ cos(훽1 + 훽2) − 2 ∗ ∗ [cos(훽1) + cos(훽4)] + ( ) = 0 푎 푎 { 훽3 = −훽1 − 훽2 − 훽4

3.2 MATLAB Simulation

푑 푑 can be calculated by the geometry which is = 1 + 푐표푠훼 . Using equation (3.6), 푎 푎 1 we plot figure of 훽2 versus 훽1, which is shown in Figure 3.2.

Figure 3.2 Plot of 훽2 versus 훽1.

The coordinates (x, y) at turning points 퐴2, 퐵2, 퐶2 and 퐷2 are 퐴2(0,30), 퐵2(31, −29),

퐶2(36, −55) and 퐷2(30, −60).

Similarly, we use equation (3.6) to plot 훽4 versus 훽1 as shown in Figure 3.3.

Figure 3.3 Plot of 훽4 versus 훽1.

The coordinates (x, y) at turning points 퐴4, 퐵4, 퐶4, 퐷4 and 퐸4 are 퐴4(0,30), 퐵4(12,36), 퐶4(31,31), 퐷4(36,16) and 퐸4(30,0).

휋∗훽 2휋∗훽 Solving equation (3.6) for 훽 gives 훽 = − (180 ∗ ( 3 + 1))/휋. A plot of 훽 3 3 180 180 3 versus 훽1 is given in Figure 3.4.

Figure 3.4 Plot of 훽3 versus 훽1.

The corresponding turning points coordinates of 퐴3, 퐵3, 퐶3 , 퐷3 and 퐸3 are 퐴3(0, −60), 퐵3(18, −52), 퐶3(33, −22), 퐷3(36,6) and 퐸3(30,30).

Based on the value analysis, the corresponding value of 훽1, 훽2,훽3 and 훽4 is shown in Table 3.1:

Table 3.1 The angle of 훽1, 훽2,훽3 and 훽4 in the simulation. β1 0 12 18 31 33 36 30 β2 30 7 -4 -29 -39 -55 -60 β3 -60 -58 -52 -26 -22 6 30 β4 30 36 35 31 25 16 0 allocate time point to each beta value to form Table 3.2, and plot each point in Figure 3.5, the snake’s locomotion curve is approximately sinusoidal.

Table 3.2 The angle of 훽1, 훽2,훽3 and 훽4 in the simulation corresponding to time. Time 1 2 3 4 5 6 7 훽1 0 12 18 31 33 36 30 Time 8 9 10 11 12 13 14 훽2 30 7 -4 -29 -39 -55 -60 Time 15 16 17 18 19 20 21 훽3 -60 -58 -52 -26 -22 6 30 Time 22 23 24 25 26 27 28 훽4 30 36 35 31 25 16 0

Snake's Locomotion Curve 60

0 0 5 10 15 20 25 30 -20

-40

-60

-80

Figure 3.5 Snake’s Locomotion Curve Based on Static Mathematical Analysis.

This snake model is a simplified 4-connecting-rod mathematic model, which can be used to analyze the snake’s locomotion curve to find out the basic implement equation, which forms a sinusoidal curve. To avoid large errors and biases in the

18 programming, kinematic Analysis are necessary to the optimal parameters for sinusoidal wave function, which are discussed in the next section.

3.3 Kinematic Analysis

This section introduces the kinematics of snake motion. In the previous section, the snake’s locomotion curve is determined to be approximately sinusoidal through mathematic Analysis and MATLAB simulation. The snake robot is a kinematic motion system with many degrees of freedom to give the snake robot extremely high redundancy. In this section, the snake implement equation parameters are determined through kinematics Analysis.

Such motion can be categorized into the following four types: 1) serpentine movement, 2) rectilinear movement, 3) concertina movement, and 4) sidewinding movement. Of these, the serpentine movement is the most smooth and effective mode of locomotion, and is widely observed in almost all species of snakes. The four types of such motion are shown in Figure 3.6.

Figure 3.6 Four types of Snake Locomotion [11].

Therefore, the standard body shape of serpentine locomotion can be approximately by a sinusoidal function. The main feature of the curve is the curvature changes along the body axis. Force analysis is applied to the skeleton model of part of a snake’s body to find out how the serpentine curve propels a snake to move. Figure 3.7 shows a skeleton model, consisting of serially connected links with length 훿푠. The bending torque 푇𝑖 is generated by the muscle at the joint 푃𝑖 (not signed in the figure). The torque 푇𝑖 applies force 푓𝑖 to the ground from joints 푃𝑖. The resultant forces 푓푡𝑖 acting along the body’s tangential direction

(the direction that bisects 휃𝑖 ) and the normal force 푓푛𝑖 acting normal to the tangential force. The 푓푡𝑖 [12] and 푓푛𝑖 [12] are obtained as follows [12]:

휃푖 푇푖+1−푇푖−1 푓푡𝑖 = {(푓𝑖 − 푓𝑖−1) + (푓𝑖+1 − 푓𝑖)} sin ≈ 휃𝑖 (3.7) 2 2훿푠

휃푖 푇푖+1−푇푖 푇푖−푇푖−1 푓푛𝑖 = {(푓𝑖+1 − 푓𝑖) + (푓𝑖 − 푓𝑖−1)} cos ≈ − (3.8) 2 훿푠 훿푠 where

훿푠 is the snake’s link length.

푇𝑖 is a bending torque.

푓𝑖 is the force 푇𝑖 applies to the ground.

푓푡𝑖 is the resultant force acting along the body’s tangential direction.

푓푛𝑖 is the normal force acting normal to the tangential force.

Figure 3.7 Forces and Torque in Skeleton Model [12].

In the snake’s skeleton model, the snake’s link (bones) are short, so it is

휃푖 reasonable to assume that 훿푠 → 0. By substituting 휃𝑖 for curvature 푘(푠) = and 훿푠

푇𝑖 → 푇(푠), 푓푡𝑖 → 푓푡(푠)훿푠, and 푓푛𝑖 → 푓푛(푠)훿푠 . The basic principle equations of the snake’s force analysis are formed as [12]:

푑푇(푠) 푓 (푠) = 푘(푠) (3.9) 푡 푑푠

푑2푇(푠) 푓 (푠) = (3.10) 푛 푑푠2

In equation (3.9) and (3.10), T(s) indicates the bending torque, k(s) is the curvature, 푓푛(푠) is the force per unit length that acts on the ground in the normal direction, and 푓푡(푠) is the force per unit length that acts on the ground in the tangential direction. These two equations indicate the relationship between the torque, curvature and forces that are applied to the ground, and it show the basic principle of the mechanism of a snake’s locomotion.

To analyze the snake’s most efficient locomotion mode, serpentine locomotion, a standard movement curve must be defined. The shape of the curve is influenced by many factors. It is reasonable to assume that the snake muscle’s co-movement is the most dominant factor among these factors. Propulsion generated by each part of the body is different. The derived force distribution along the snake’s shape is illustrated in Figure 3.8. The dotted line in the figure is the normal force distribution of a real snake derived with electromyography sensor during the snake motion.

Figure 3.8 Normal force distribution along the snake. (adopted from [12]).

From this illustration, it is obviously that the snake shows a tendency to push against the ground most significantly around the point O. The forces acting around each point O are not parallel to the traveling direction of the snake, but since they oppose each other every half cycle, the net normal forces are directed in the traveling direction and these normal forces push the snake body forward. This analysis explains the snake lifts part of the body when moving fast. The snake lifts

22 the body around points P and contacts the ground around points O as shown in Figure 3.9. This is the result of the serpentine motion that maximizes locomotion efficiency. The torque and angular velocity of a joint reach its maximum at points O.

Figure 3.9 Snake lift part of its body when moving [12].

Through force Analysis, it is clear how a snake propels itself by generating the standard body-shape curve formed in a typical serpentine motion. The morphology of snake’s serpentine motion is a sinusoidal curve. The curve shape may be influenced by many factors; the main feature of this curve is that the curvature changes along the body axis in a sinusoidal manner [12].

2퐾 휋훼 2퐾 휋 푘(푠) = − 푛 sin ( 푛 푠) (3.11) 퐿 퐿 where 퐾푛 is the number of the S-shape; L is the wavelength of the curve; 훼 is the angle of the body at point O relative to the traveling direction (called the winding angle). So the serpentine curve is given by the curvature function. It is assumed that the snake robot holds its shape on the serpentine curve. The 푖-th joint variable of relative value 휃𝑖(= ∅𝑖 − ∅𝑖−1) is derived by integrating the curvature function given above. Assuming that 퐿 = 푛푙 (adopted from [12]).

1 1 푆+푆 + 푙 푆+𝑖푙+ 푙 푝푖 2 2 휃 = ∫ 푘(푢)푑푢 = ∫ 푘(푢)푑푢 𝑖 1 1 푆+푆 + 푙 푆+(𝑖−1)푙+ 푙 푝푖−1 2 2

퐾 휋 2퐾 휋 2퐾 휋 = −2훼푠푖푛 ( 푛 ) 푠푖푛 ( 푛 푠 + 푛 푖), 푖 = 1,2 … 푛 − 1 (3.12) 푛 푛푙 푛

2퐾 휋 Since the variables are changing with sequence of time, suppose b = 푛 , s = 푛푙 퐾 휋 ct, ct + bl, ct + 2bl, … , A = −2훼푠푖푛 ( 푛 ) , 휔 = 푏푐, 훽 = 푏푙, where 푐 is constant. 푛 Therefore, the serpentine locomotion equation is:

휃𝑖 = 퐴푠푖푛(휔푡 + 푖훽) (3.13)

In (3.13), the parameters 퐴, 휔 and 훽 are determined by the mechanical parameters of snake robot. However, even in the library laboratory environment, the snake’s locomotion efficiency may vary because of different parameters. In the application environment, the optimal parameters in the calculation cannot guarantee the high efficiency of snake’s locomotion, in other words, the calculated parameters cannot ensure the snake’s high-speed locomotion. In the next chapter, a mechanical snake model is built using the software SimWise4D to test the snake’s moving speed under different parameters. thus, the snake’s mechanical structure can be tested close to the a physical environment; and the snake’s serpentine locomotion gait can be observed.

4 Snake Robot Mechanical Model and Simulation

4.1 The Structure Model of Snake Robot

A joint structure of the snake robot uses a modular design, the overall mechanical structure of the snake robot is composed of seven identical modular units (1 more joint was added later). The joint model size is of diameter 50mm, and length 144mm. This robot is composed of two types of bending joints, having different bending axis connected alternately at 90°. This connection allows the robot 3D motion because each joint drives 2 degrees of freedom (there are 16 degrees of freedom). Moreover, each joint can be controlled separately. This driving mechanism can be simple but efficiently transmit the power to propel the snake robot.

Figure 4.1 Side view of schematic diagram of the transmission mechanism of the snake robot.

Figure 4.1 shows the internal mechanism of the joint of the snake robot. A radio- controlled (RC) servomotor generates the bending torque. The servomotor output torque is up to 3.22 kg.cm at 5V, which is enough to propel the snake robot to

25 move fast. A polymer lithium ion (3.7V, 1000mAH) battery is installed inside the snake robot, and it allows the robot to operate continuously for more than half an hour. In addition, a 3.7V to 5V booster and charger is installed inside the robot’s joint and is discussed. Considering the smoothness of the snake’s scale, passive wheels are installed outside the robot’s surface to reduce friction. This structure allows the robot to move on the high-friction ground. Once a satisfactory 3D design is reached, all parts of the snake robot joint are fabricated by a 3D printer and assembled into a functional prototype. The 3D model and the metal fabricated gears are shown in Figure 4.2.

Figure 4.2 3D printed model of gears and prototypes.

Because of the snake robot’s modular design, once a satisfactory 3D design of the snake’s body and connection gears is obtained, all parts are assembled in Solid Work to from the snake’s joint as shown in Figure 4.3. Seven identical joints are then imported into the simulation software SimWise4D to form the snake robot in Figure 4.4.

Figure 4.3 Snake robot’s joint mechanical structure.

Figure 4.4 Plan of snake robot.

4.2 Simulation Environment Setting

The implement equation derived in Chapter 2 is 휃𝑖 = 퐴 ∙ 푠푖푛 (휔푡 + 푖훽) where 휃𝑖 is the 푖 -th relative joint angle. The implement equation is written as the time function 휑(푡) = (−1)푚−1퐴 ∙ 푠푖푛 (휔푡 + 푛훽), where 푛 = 0,1, … ,6 is the joint number and 푚 is the direction parameter. The snake robot can move forward when 푚 is even and back up when odd. The ground material in the simulation environment is steel. If the coefficient of restitution is zero, then all rebound power is absorbed, if 1, then no rebound power is absorbed. Here the coefficient of restitution is set to 0.5. And, the coefficient of friction is set 0.5, which corresponds to a physical environment. Different friction coefficients are applied in later simulation. The simulations environment setting is shown in Figure 4.5.

Figure 4.5 Simulation environment setting.

4.3 2D Simulation

There are two servomotors to control the snake robot’s rotation in the X-Y plane and Y-Z plane. In 2D simulation, only one servomotor at X-Y plane is used to propel the robot. 3D locomotion is simulated later. There are three parameters to determine the shape of a sinusoidal curve and to determine the winding curve of the snake robot. If simulation continues for over 1000ms so that motion of model remains stable.

4.3.1 Serpentine Locomotion Simulation

The simulation includes the follow scenarios:

 Fix the value of 휔 and 훽, change the value of 퐴: in the implement equation, 1 set 휔 = 15푟푎푑/푠 and 훽 = ∙ 휋 degree. The serpentine gaits of the snake robot 3 are shown in Figure 4.6 when 퐴 = 5푚푚, 15푚푚, 20푚푚, 40푚푚 and 60푚푚 respectively. The velocities of the snake robot are shown in Table 4.1.

Figure 4.6 Serpentine gaits of snake in different amplitudes.

Table 4.1 Velocities of snake robot at different amplitudes.

Amplitude(mm) 5 15 20 40 60 Forward velocity(m/s) 0.051 0.092 0.183 0.21 0.254

Relationship of the velocities and amplitudes 0.3 0.254 0.25 0.21 0.2 0.183

0.15

0.092 Velocity/(m/s) 0.1 0.051 0.05

0 0 10 20 30 40 50 60 70 Amplitude/(mm)

Figure 4.7 The snake robot’s velocities corresponding to different amplitudes.

1  Fix the value of 퐴 and 훽, change the value of 휔: set 퐴 = 30푚푚, 훽 = 휋. The 3 serpentine gaits of the snake robot are shown in Figure 4.8 when 휔 = 5 푟푎푑/푠, 15 푟푎푑/푠, 20 푟푎푑/푠, 40푟푎푑/푠 and 60 푟푎푑/푠 respectively. The velocities of the snake robot are shown in Table 4.2.

Figure 4.8 Serpentine gaits of a snake robot in different angular frequencies.

Table 4.2 Velocities of snake robot at different angular frequencies.

Frequency(rad/s) 5 15 20 40 60 Forward velocity(m/s) 0.177 0.246 0.282 0.172 0.323

Relationship of the velocities and angular frequencies

0.35 0.323

0.3 0.282 0.246 0.25

0.2 0.177 0.172

0.15

0.1 Frequency/(rad/s)

0.05

0 0 10 20 30 40 50 60 70 Forward velocity/(m/s)

Figure 4.9 The snake robot’s velocities corresponding to different angular frequencies.

 Fix the value of 퐴 and 휔, change the value of 훽: in the implement equation, 퐴 = 30푚푚 and 휔 = 20푟푎푑/푠. The serpentine gaits of snake robot are shown in Figure 4.10 when 훽 = 1/6휋, 1/4휋, 1/3휋, 1/2휋 and 휋 respectively. The velocities of the snake robot are shown in Table 4.3.

Figure 4.10 Velocities of snake robot in different phases.

Table 4.3 Velocity of Snake robot at different phases.

Phase(degree) 1 1 1 1 휋 ⁄6 휋 ⁄4 휋 ⁄3 휋 ⁄2 휋 Forward velocity(m/s) -0.01 -0.018 0.253 0.224 0.185

Relationship of the velocities and phase angles 0.3 0.253 0.25 0.224 0.185 0.2

0.15

0.1

0.05 -0.01 -0.018 0

Forward velocity/(m/s) Forward β=1/6*pi β=1/4*pi β=1/3*pi β=1/2*pi β=pi -0.05 Phase angle/(degree)

Figure 4.11 The snake robot’s velocities corresponding to different phase angles.

The relationship of the velocities and amplitudes of Figure 4.7 shows that the velocity increases with step. Comparing those serpentine gaits at different amplitudes, we choose 퐴 = 20푚푚 as the optimal amplitude value. The relationship of the velocities and angular frequencies of Figure 4.9 shows peak value as the frequency increased. Hence, we choose 휔 = 20푟푎푑/푠 as the optimal frequency value. In addition, as in the case of angular frequency, there is peak 1 value at 훽 = 휋 degree. Thus, the three optimal parameters of the sinusoidal curve 3 1 to from a complete serpentine curve are 퐴 = 20푚푚, 휔 = 20푟푎푑/푠 and 훽 = 휋 3 degree.

Although these 3 parameters tested in the simulation software yield the best performance, a physical mechanical snake in a physical environment may have different optimal parameters. The implement equation with the optimal parameters can be applied in an experiment and the test. These three values may be changed. Never the less, the simulation gives parameter ranges for the further testing.

4.3.2 Serpentine Locomotion under Different Friction

Once the three parameters of the implement equation are determined, the next simulation investigates the snake robot’s performance in different environments by changing the coefficient of friction of the surface on which snake robot moves. In the previous simulation, the ground friction fixed at 0.5. Here, we change the coefficient of friction from 0.25 to 5. let the simulation run for 3000ms until the motion of the model is stable. If the snake robot’s speed changes in a small range, the robot’s adaptability in different terrains demonstrated.

We set the implement equation’s parameters to be 퐴 = 20푚푚, 휔 = 20푟푎푑/푠 1 and 훽 = 휋 degree, and the coefficient of friction to be 0.25, 0.5, 1, 2, 3, 4 and 5. 3 The simulation results are shown in Figure 4.12.

(a) Coeff. Friction = 0.25 (b) Coeff. Friction = 0.5

(e) Coeff. Friction = 3 (f) Coeff. Friction = 4

(g) Coeff. Friction = 5

Figure 4.12 Simulation results with different friction.

The curves in the figures are the displacements in the X, Y, and Z directions respectively. The blue line is the displacement in the Y direction, the red line is the displacement in the X direction and the orange line is the displacement in the Z direction. Comparing plots, we observe that when the coefficient of friction is very small (0.25), there is nearly no displacement in the X direction, but a relatively big displacement in the Y direction. This implies that the robot does not move forward and only shifts sideways. When the coefficient of friction equals 0.5, 1, 2, 3 and 4, the slopes of the displacement curves in the simulations are similar. We conclude that environment friction has little influence on the robot’s locomotion when the coefficient of friction is within a range which is from 0.5 to 4. That is why in Figure

3.12 (g), there is little displacement even in the X-direction. The snake robot has sinusoidal motion in the Y direction, but no displacement.

Base on the simulation results, serpentine locomotion can be applied in most natural environments, and the results show the high efficiency of the snake robot’s serpentine locomotion. The snake robot has high adaptability to different terrains. However, the simulation also shows the weakness of serpentine locomotion. When the coefficient of friction is very low or very high, the efficiency of serpentine locomotion decreases sharply as shown in Figure 3.12(a) and Figure 3.12(g). The serpentine locomotion cannot be approached. We therefore consider a second locomotion mode sidewinding locomotion. Sidewinding locomotion is a type of locomotion unique to caenophidian snakes, used to move across the loose or slippery surface. It gives the best solution when friction is too low or too high. In the next section, the implement equation for sidewinding locomotion is derived and simulated to test its adaptability.

4.4 3D Snake Robot Sidewinding Locomotion Simulation

Since serpentine locomotion is not applicable in some special environments, it is necessary to consider another locomotion mode to improve the snake robot’s adaptability to different terrains. Sidewinding locomotion is a good solution when friction is very small or very high. The movement of caenophidian snakes has shown its adaptability on loose or slippery surface. In serpentine locomotion, 7 out of 14 servomotors are used to propel the snake robot. A sinusoidal function controls seven servo motors to run in the X-Y plane. By applying sidewinding locomotion, all servomotors under control of two perpendicular sinusoidal functions that one applies in the X direction, the other is shifted 90 degrees to apply in the Y direction. The shifted sinusoidal function becomes a cosine function. The implement equations for servomotors at X direction and Y direction are shown below respectively:

휑(푡) = (−1)푚−1퐴 ∙ 푠푖푛(휔푡 + 푛훽) (4.1)

휑(푡) = (−1)푚−1퐴 ∙ 푐표푠(휔푡 + 푛훽) (4.2) where n = 0, 1 … ,6 is the joint number from 0-6 and m is the direction parameter.

When applying both implement equations to the snake robot simultaneously, the curve is shown in Figure 4.13.

Figure 4.13 Combination curve [13].

Since the servomotors in the X and Y directions propel the snake robot to move together, the robot’s locomotion gaits are different from 2D serpentine locomotion. However, in each plane, the robot’s locomotion gaits should be the same with 2D locomotion because the individual sinusoidal implement equation is applied to each servomotor. The plan view and front view of snake robot’s locomotion gaits are shown in Figure 4.14.

(a) Plan view.

(b) Front view.

Figure 4.14 Snake robot sidewinding locomotion.

In the simulation, the robot’s movement speed increases. Figure 4.15 shows the displacement of the snake in the X, Y, and Z directions respectively, where the red line and the blue line is the displacement at X-Y plane, the orange one is the displacement at Y-Z plane. Obviously, the displacement in the X direction is huge within 1550ms; the speed has great improvement compares with 2D locomotion. In addition, the snake has a huge shift at Y-axis. The displacement at Z direction is small enough to be ignored.

Figure 4.15 3D locomotion simulation results.

The simulation shows the advantages of sidewinding locomotion; the speed of robot increases. However, serpentine locomotion is still the most efficient movement mode because it uses half of the servomotors. The snake robot is expected to use a battery and a power cable to provide electricity for the servomotors. If all 14 servomotors (16 in a later design) are used to propel the snake robot, the instant current could reach 5 Amps. The power limit may be an issue when using a battery to provide power. The power system is discussed in Chapter 4.

Serpentine locomotion is the most efficient locomotion mode; the sidewinding locomotion mode can be used in cluttered environments such as sand or slippery surface.

5 Snake Robot’s Mechanical Structure and Integrated Electronic System Design

5.1 Snake Robot Mechanical Design

The mechanical structure of the snake robot is modified from the mechanical model presented in Chapter 3. The volume of the snake robot’s body is limited, and the integrated electronic system has to be installed in the limited space. The integrated electronic system includes two power systems, the battery power system, and the external power system; the wireless control and data transmission system and some detective sensors. The modified structure design distributes the space to each system and avoids interference. The two mechanical designs are shown in Figure 5.1.

(a) The First design (b) Modified design

Figure 5.1 The body of snake robot.

The connection joint is the same as the previous design as shown in Figure 5.2 so that the simulation results are satisfactory with new mechanical structure.

Figure 5.2 Connection of two joints.

Comparing the two designs shown in Figure 5.1 (a) and Figure 5.1 (b), the middle part of the body is widened to provide more space for electric components. Since the printing material for the snake robot’s body is metal-plastic, grids are added to the body to dissipate heat and reduce weight. These hollowed-out grids increase the whole structure’s flexibility, which increases its coefficient of restitution to make the robot run more efficiently. Reduced weight saves power, and the heat from electrics and servomotors can be dissipated quickly. Two separate boards were added to fix batteries. A connection board and 5V booster and charger are introduced in Section 5.2. The biggest improvement is to separate the body into two parts. The upper part is to install electrics and the base is snake scale-inspired design. In addition, two designs are developed to adapt to different terrains. Two designs of the base of the snake robot are shown in Figure 5.3.

(a) Design 1.

(b) Design 2.

Figure 5.3 Base of snake robot.

In Design 1, passive wheels are equipped to make the snake robot move fast on the smooth surface. In Design 2, ball bearings are installed instead of passive wheels. By using this chassis, serpentine locomotion can be applied on the solid ground. The upper part and the lower part are installed separately, so different chassis can be installed based on the environment. This modular design ensures the snake robot’s high adaptability to different terrains and reduces cost; different locomotion modes can be tested using one snake with different chassis instead of building different snake robot for each locomotion mode. The motion data is discussed in Chapter 5.

5.2 Voltage Booster and Adapter Boards

There are two systems to supply power for all servo motors and electronic components. Fourteen 1000mAh polymer lithium ion batteries are in parallel as a power source to ensure the same voltage for 14 servomotors, 2 microcontrollers, Xbee wireless transmission chips, 2 IMU chips and several sensors. All the components require the same voltage at 5V. Considering the weight of 14 batteries and limited space inside the robot’s body, higher voltage and capacity nickel- hydrogen battery inappropriate and a lightweight polymer lithium ion battery is a better choice. The battery has built-in protection against over voltage, over current and minimum voltage. Benefit from the high-energy density, each cell outputs a nominal 3.7 V at 1000mAh. These features make the polymer lithium ion battery work efficiently and safely.

5.2.1 5V Booster & Charger

The battery voltage output is 3.7V. Although servo motor and other electronic components are able to work at 3.7V, the servo motor may lose torque and speed, the crystal oscillators in the microcontrollers and IMUs will be hard to oscillate which makes unfunctional, the Xbee wireless transmission may lose transmission power, the other terminator may loss data package. The control circuit inside the servos and electronic components may take partial voltage, which makes the situation worse. So a 5V booster and charger board is selected to solve this issue. The booster is shown in Figure 5.4.

Figure 5.4 5V booster and charger.

This DC/DC booster converter module is with a built-in battery charger circuit, it can be powered by a 3.7 LiPoly battery, and converts the battery output to 5.2V to run all the components in the snake robot. The booster converter raises the output to 5.2V, instead of a 5.0V, to allow for voltage drop along long cables. In addition, a diode on the output avoids reversed wire connection. The heart of the booster is TPS61090 boost converter from TI. This chip has some nice extras. Such as low battery detection. A low battery red indicator LED lights up when the voltage dips below 3.2V. Synchronous conversion allows the chip to disconnect the output completely by connecting the Enable pin to ground. The Enable pin is connected to the connection board as a switch to control the power supply. The chip also has an excellent efficiency, of up to 96%, 700 kHz high-frequency operation and a low quiescent current of 5mA when enabling and the power LED is on, and 20μA when disabled. A MicroLipo charger is mounted on the other side. The charger circuitry is powered from the Chrg Pin, and will recharge the 3.7V LiPoly battery at a maximum rate of 500mA. There are two LEDs for monitoring the charge rate, the yellow one indicates that it’s working and the green one lights up when it is done. Considering 14 batteries are in parallel connection, seven booster converter modules are installed inside the snake robot’s body to boost the voltage to 5.2V and parallel charging technology is applied to charge the 14 batteries simultaneously.

5.2.2 Parallel Charging

Parallel charging allows multiple batteries to be connected in parallel to create a single larger battery. There are fourteen 1000mAh batteries; every two batteries are in a battery pack as shown in Figure 5.5, which forms seven battery packs with 2000 mAh capacity each. Seven battery packs are wired in parallel as shown in Figure 5.6. If the charge rate is set to be 500mA, a battery pack’s charging is done in 4 hours and seven battery packs’ charging is done at a charge rate of 3.5A. The principle of parallel charging is simple. When placing all the packs in parallel, they all have the same voltage. Current flows between the packs, and they all adjust their voltages until they perfectly match. The charger only “sees” one large pack and charges it as if it were a single pack. The charger will balance all the individual packs if a parallel balance adapter is used. Using this also ties all the like cells of all the packs together (this battery just has one cell). Not only do the packs all have the same voltage but each set of cells does also. The charger then balances all the packs as if it was charging a single pack [17].

Figure 5.5 Parallel charging setup for 2x 1s packs.

Figure 5.6 8 Battery packs parallel charging.

Parallel charging allows using one power cable to charge all 16 batteries at the same time. For better electric performance, three adapter boards were designed to connect all electrical components, which makes the system work stable and easy to maintain.

5.2.3 Adapter Boards

 Connection board

The system consists of several connected subsystems. The power system is the most important part because of the big current required by servo motors. Considering the limited space in the snake robot’s body, a connection board was designed. Wires include a common GND, a servomotor power cable, a PWM signal line for each servo, a battery 5V power cable, a battery Enable pin, an external 5V power cable and a charge pin. All the wires are connected to the board for electric stability and easy maintenance. Since the current through the board is up to 5A, the power cable is widening. The common GND was connected to the copper overlay of the board. Ground capacitance is also on the back of the board. Screw holes are on the board allowing to fix the connection board to the snake robot’s body. The connection board is shown in Figure 5.7.

Figure 5.7 Connection board.

 Power Source Switch Board

There are two ways to provide power to the snake robot, external power and batteries. Therefore, the power source switchboard was designed to switch the power source. Also, all lines in the board are widened to accommodate a large current. A power slide switch, which uses 6V and up to 5A capacity is used to switch the power source. Ground capacitance is also used at the back of the board to avoid damage from the large instantaneous current. The power source switch cable and power slide switch are shown in Figure 5.8 (a) and (b).

(a) Power source switch. (b) Power slide switch.

Figure 5.8 Power source switch board.

 Battery Enable Board

The 5V battery power source is boosted from 3.7V by the booster board. The current quiescent of the booster is 5mA when enabling and power LED is on, 20휇퐴 when disabled (power and low battery LED are off). Therefore, Power Enable was designed as shown in Figure 5.9 (a). Since the current in the battery enable board is not large, 3 pins 2 slide switch is used to control the internal switch of booster as shown in Figure 5.9(b).

(a) Battery enable board. (b) Two-slide switch.

Figure 5.9 Battery enable board and switch.

This chapter gives solutions for the snake robot’s integrated electrical system and power system. A modified mechanical structure was developed to allocate the space to each system and avoid crowdy. A booster and charger board is used to boost 3.7V battery voltage to 5V to provide power to the whole system, and an external power cable is also used. A power source switchboard controls the power supply. Parallel charging technology is applied to charge all 14 batteries simultaneously. Three adapter boards were designed to make the whole electrical system work stably.

6 Remote Control and Locomotion Analysis.

6.1 Wireless Communication

6.1.1 Xbee module

After building the snake robot mechanical structure and electronic system, the snake robot is ready to move on the ground. Therefore, the control system must be built. Because the robot has an internal battery to supply power, wireless control is the only choice. Xbee module is a good choice for wireless communication. All Xbee modules work at 2.4GHz in a different channel. The Xbee module is highly integrated, the setting can be done in the computer terminal. By setting the user’s Xbee modules in the same channel, they can communicate with each other. There are three Xbee modules in a mesh network as shown in Figure 6.1.

Figure 6.1 Wireless control diagram.

There is a wireless communication protocol that is presented in the next section to send a serial of comments to the Xbee receiver in the snake robot to control the robot’s locomotion. The collected sensor data will be sent to a computer for analysis since the calculation are too complicated to be done by the microcontroller. These three Xbee modules consist of a simple mesh network. Xbee modules work at 3.3V, the Xbee adapter is used to convert 5V to 3.3V and configure the Xbee module. The Xbee module and Xbee adapter are shown in Figure 6.2.

Figure 6.2 Xbee module and adapter.

6.1.2 Wireless Communication Protocol

For better communication between controller and robot, a remote-control protocol was developed. The remote-control unit packets serials of commands through the XBee link. The packets are twelve bytes long, consisting of ASCII-encoded hexadecimal digits. The snake robot decodes the packets as follows:

Packet = AADDDDDDDDCC where AA = address byte

DDDDDDDD = 32 bits of data, which is four data bytes

CC = checksum byte

The address points to a 32-bit word in an array of floating-point numbers in the Arduino’s SRAM. The checksum is valid if the sum of the values of the six bytes of data is a multiple of 256. If the checksum is valid, then the packet is accepted and the 32 bits of data are written to the corresponding cell in the array. Each cell of the array represents some parameters of the robot snake’s movement model. For example, steering, rolling, the phase difference between X-Y joints, the speed of sinusoidal prorogation, the ratio of vertical to horizontal magnitudes of sinusoids

(sidewinding factor), or a similarly-defined sinusoidal-lifting factor. This would allow the snake robot to change between modes of locomotion in response to wireless commands, eliminating the need to reprogram its Arduino every time a new set of parameters needs to be tested. The Arduino sketch that demonstrates some of the basic functionality described above. Other improvements include an algorithm that allows the robot to squiggle at precisely defined frequencies. The parametric data array is 10 numbers long (but can be expanded in later versions), and its address and motion parameters are defined as follows:

 00 = X Steering. Offsets the angle of horizontal servos in one direction or another to allow steering. Valid values between -1 and 1.  01 = Y Steering. Same as X Steering but applies to vertical servos. Valid values between -1 and 1.  02 = Rolling angle. This allows the robot to map the motion parameters of a virtual coordinate system (left/right and up/down) to the physical X-Y joints (even-numbered servos and odd-numbered servos) which allows a wheeled robot to roll onto its back and test a different mode of locomotion that do need wheels, then roll back onto its wheels as needed. Valid values between -π and +π.  03 = Power Consumption Factor. It allows the robot to adjust its motion for reduced power consumption at the price of slower speed. Valid values are between 0 and 1.  04 = Traction Factor. This is proportional to the amplitude of the robot's sinusoidal squiggles. Valid values between 0 and 1.  05 = Propagation Speed Factor. The higher this is, the faster the robot's squiggles propagate along the length of its body. The default value is 1. There are no hard limits on this value, but setting it too high causes performance and efficiency to deteriorate.  06 = Global Amplitude. This is meant to gradually shape the robot from a straight-line shape into a sinusoid shape shortly after power-up and to gradually return the robot from a sinusoid shape into a straight line at the end

of testing before it is powered down. This significantly reduces the power surge encountered at power-up.  07 = Delta Phase. The phase difference between joints. Instead of defining the phase for each X-Y joint, this value defines the difference between phases. It can control the wavelength of the robot's sinusoidal shape.  08 = Sinusoidal Lifting Factor. The ratio of sinusoidal lifting amplitude to the horizontal serpentine amplitude. A reasonable range is between -0.2 and 0.2. Zero by default, which means no sinusoidal lifting. Positive values make the robot use sinusoidal lifting in its motion. Negative values make it use the opposite of sinusoidal lifting - sinusoidal rolling.  09 = Sidewinding Factor. A reasonable range of the ratio of vertical sinusoidal amplitude to horizontal sinusoidal amplitude is between -0.3 and 0.3. The negative/positive sign of the side-winding factor determines whether the robot's body takes the shape of a right-handed versus a left-handed helix.

The remote-control protocol is written into remote controller and robot’s receiver. In the Arduino sketch, the program runs over 125ms. The running loop has little influence on the snake’s locomotion, but a huge influence on data transmission. For precise data analysis, IMU data is sent to the computer every 10ms. The data transmission program cannot use one microcontroller together with the control program. So an individual microcontroller is used to process to sensor’s data for the real time transmission. Parts of the program are demonstrated below including the “SendPacket” subprogram and the “Checksum” subprogram. void SendPacket(byte address, float data)

{

byte packet[12];

unsigned long int_data = floatToLong(data);

byte checksum = GenerateChecksum(address, int_data);

packet[0] = address>>4; // Get high nibble of address

packet[1] = address%16; // Get low nibble of address

packet[2] = (int_data>>28)%16;

packet[3] = (int_data>>24)%16;

packet[4] = (int_data>>20)%16;

packet[5] = (int_data>>16)%16;

packet[6] = (int_data>>12)%16;

packet[7] = (int_data>> 8)%16;

packet[8] = (int_data>> 4)%16;

packet[9] = int_data %16;

packet[10] = checksum>>4; // Get high nibble of checksum

packet[11] = checksum%16; // Get low nibble of checksum

int i;

for (i=0; i<12; i++)

{

Serial.print(packet[i],HEX);

}

} byte GenerateChecksum(byte address, unsigned long data){

byte checksum = address + ((data>>24)%256) + ((data>>16)%256) + ((data>>8)%256) + (data%256);

checksum = 0 - checksum;

return checksum;}

6.2 Snake robot’s trajectory tracking model

6.2.1 Trajectory tracking algorithm

IMU data is very important for the robot’s locomotion analysis. However, the robot’s orientation measurement is a hard problem. Sensor fusion algorithms are difficult to cover the implements on a low-cost real-time system. To save time and simplify the design, BNO055 – a smart 9-DOF sensor is selected to measure the IMU data. BNO055 is a microelectromechanical system with 3 axes accelerometer, 3 axes magnetometer and 3 axes gyroscope with a high-speed ARM based processor to process all the sensor data. The BNO055 outputs are quaternions, Euler angles or vectors. Figure 6.3 shows the sensor.

Figure 6.3 Nine DOF sensor BNO055.

Then, object’s displacement can be obtained by double integral of its acceleration. Using this model, a high precision acceleration sensor is installed in the head of the robot to detect its real-time 3D motion acceleration at a high frequency. At the same time, all acceleration data is transferred to a computer for integration. Then the robot’s instant speed curve and accumulated space trajectory are graphed on the screen. Acceleration data is processed by MATLAB.

As shown in Figure 6.4, the horizontal axis is the time axis, the vertical axis is the instant speed, a(t) is the acceleration output of BNO055, the MEMS acceleration

55 sensor. The acceleration at time t in the curve is the curvature of the corresponding point.

Figure 6.4 Speed-time curve.

Assume sampling starts at time 푡0 . 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(t) = ∫ 푣(푡)푑푡 + 푠(푡0) (6.1) 푡0

푡 v(t) = ∫ 푎(푡)푑푡 + 푣(푡0) (6.2) 푡0 s(푡0) is accumulative displacement from 0 to 푡0, v(푡0) is the system instant motion velocity at 푡0.

Since the outputs of the IMU are groups of discrete data, the time-speed curve is decomposed as numbers of a right-angled trapezoid. Initial s(푡0) = 0 , the displacement s(t) is presented as:

푡 s(t) = ∫ 푣(푡)푑푡 푡0

푣(푡 )+푣(푡 ) 푣(푡 )+푣(푡 ) 푣(푡 )+푣(푡 ) = 0 1 (푡 − 푡 ) + 2 1 (푡 − 푡 ) + ⋯ + 푛 푛−1 (푡 − 푡 ) 2 1 0 2 2 1 2 푛 푛−1 (6.3)

푡1 − 푡0 = 푡2 − 푡1 = ⋯ = 푡푛 − 푡푛−1 = ∆푡, ∆푡 is sampling time interval, when n > 1,

푣(푡 )+푣(푡 −1) s(t) = ∑푛 푘 푘 ∆푡 (6.4) 푘=1 2

Equation (6.4) is modified in the discrete domain as:

푣[푘]+푣[푘−1] S[n] = ∑푛 ∆푡 (6.5) 푘=1 2 when 푛 > 1, in the discrete domain

푎[푘]+푎[푘−1] 1 v[n] = ∑푛 ∆푡 = 푣[0] + (푎[0] + 푎[푛]) ∙ ∆푡 + (푎[1] + 푎[2] + 푘=1 2 2 … + 푎[푛 − 1]) ∙ ∆푡 (6.6)

푛 푣[푘] + 푣[푘 − 1] 1 s[n] = ∑ ∆푡 = (푣[0] + 푣[푛]) ∙ ∆푡 + (푣[1] + 푣[2] + 2 2 푘=1

… + 푣[푛 − 1]) ∙ ∆푡 (6.7)

Then the object’s displacement in one axis is presented as:

1 푠[푛] = 푛 ∙ 푣[0] ∙ [(푛 − 1) ∙ 푎[1] + (푛 − 2) ∙ 푎[2] + ⋯ + 푣[푛 − 1]] ∙ ∆푡2 + (푎[0] + 4 푎[푛]) ∙ ∆푡2 (6.8)

Though equation (6.8), it is easy to calculate the displacement after obtaining the initial velocity and the acceleration data from the IMU. However, based on this equation, the calculation is very complicated for large 푛, and the system will need

57 to allocate huge memory sources to store the acceleration data from time 0 to 푛. Furthermore, the system needs to repeat all the previous acceleration calculation to update the object’s motion displacement. To simplify the calculation, iteration is applied.

From equation (6.6)

푎[푛]+푎[푛−1] 푣[푛] − 푣[푛 − 1] = ∙ ∆푡 (6.9) 2

푣[푛] + 푣[푛 − 1] 푠[푛] − 푠[푛 − 1] = ∙ ∆푡 2

1 = 푣[푛 − 1] ∙ ∆푡 + (푎[푛 − 1] + 푎[푛]) ∙ ∆푡2 (6.10) 4

Equations (6.9) and (6.10) are modified as:

푎[푛]+푎[푛−1] 푣[푛] = 푣[푛 − 1] + ∙ ∆푡 (6.11) 2

푣[푛]+푣[푛−1] 푠[푛] = 푠[푛 − 1] + ∙ ∆푡 2

1 = 푠[푛 − 1] + 푣[푛 − 1] ∙ ∆푡 + (푎[푛 − 1] + 푎[푛]) ∙ ∆푡2 (6.12) 4

From equations (6.11) and (6.12), the object’s instant speed 푣[푛] and motion displacement 푠[푛] can be calculated recursively.

The outputs of IMU BNO055 has are the object’s accelerations at 3 axes. Based on equation (6.11), the object’s instant speed at X, Y and Z-axis can be presented as:

푎 [푡]+푎 [푡−∆푡] 푣 [푡] = 푣 [푡 − ∆푡] + 푥 푥 ∙ ∆푡 (6.13) 푥 푥 2

푎 [푡]+푎 [푡−∆푡] 푣 [푡] = 푣 [푡 − ∆푡] + 푦 푦 ∙ ∆푡 (6.14) 푦 푦 2

푎 [푡]+푎 [푡−∆푡] 푣 [푡] = 푣 [푡 − ∆푡] + 푧 푧 ∙ ∆푡 (6.15) 푧 푧 2

The system’s instant motion speed in 3D space at time 푡 can be calculated as:

푣[푡] = 푣푥[푡] + 푣푦[푡] + 푣푧[푡] (6.16)

Similarly, the object’s motion displacements at X, Y, and Z-axis are presented as:

1 푠 [푡] = 푠 [푡 − ∆푡] + 푣 [푡 − ∆푡] ∙ ∆푡 + (푎 [푡 − ∆푡] + 푎 [푇]) ∙ ∆푡2 (6.17) 푥 푥 푥 4 푥 푥

1 푠 [푡] = 푠 [푡 − ∆푡] + 푣 [푡 − ∆푡] ∙ ∆푡 + (푎 [푡 − ∆푡] + 푎 [푇]) ∙ ∆푡2 (6.18) 푦 푦 푦 4 푦 푦

1 푠 [푡] = 푠 [푡 − ∆푡] + 푣 [푡 − ∆푡] ∙ ∆푡 + (푎 [푡 − ∆푡] + 푎 [푇]) ∙ ∆푡2 (6.19) 푧 푧 푧 4 푧 푧

During time 푡 − ∆푡, object’s space motion displacement is:

2 2 2 푠∆[푡] = √(푠푥[푡] − 푠푥[푡 − ∆푡]) + (푠푦[푡] − 푠푦[푡 − ∆푡]) + (푠푦[푡] − 푠푦[푡 − ∆푡])

(6.20)

So at time 푡, the object’s space coordinates are (푠푥[푡], 푠푦[푡], 푠푧[푡]). In the three- dimensional system of coordinates, the object’s trajectory can be plotted by connecting all space coordinates at time 푡0, 푡1, 푡2, ⋯ ⋯ , 푡푛.

6.2.2 Snake robot’s absolute orientation (Euler vector) and visualization

Since the IMU chip includes MEMS accelerometer, magnetometer, and gyroscope, the orientation of the Euler vectors is also very useful to determine the robot’s locomotion. Unlike acceleration data, has to be processed in the desktop since the algorithm is too complex to load in the Arduino. The output of absolute orientation is simple digital numbers which are easy to plot and display.

An IDE, Processing is used to visualize the absolute orientation in Euler angle. Processing is an open source computer programming language built into the visual design. By using Processing, the absolute orientation data can be displayed on the screen in real time and the robot’s attitude can be obtained intuitively.

The absolute orientation data is also transmitted wirelessly, after receiving the data, Processing will read it from the serial port, then run the program. The orientation curve will be a plot on the screen in the form programmed.

Figure 6.5 shows an example of a locomotion test involving forward/reverse motion and a left/right steering test.

Figure 6.5 Robot’s absolute orientation when steering.

Figure 6.6 shows an example of snake locomotion when rolling. The information in the figure shows the angle variation in the X, Y, and Z-axis. It is obvious that coordinates of Z flip twice while X maintains the same.

Figure 6.6 Robot’s absolute orientation when rolling.

By the visualization of the snake robot’s absolute orientation, it is easy to get its real-time motion attitude. The information is very important for robot control. The Processing program can be found in the Appendix.

6.3 Tracking system noise processing and software compensation algorithm

6.3.1 System noise analysis and processing

Since the tracking system is based on double integral of accelerations, so errors in IMU data cause errors in the integration results, and the errors accumulate during integration. Even minimal measurement errors result in huge calculation errors, which leads to low accuracy in the tracking results. In the tracking system, the errors sources are:

1. Acceleration sensor errors. These IMU errors include: 1) Random noise: The silicon made capacitive accelerometer has mechanical random noise and electrical random noise. Mechanical random noise is

mainly the accelerometer’s elastic system’s Brownian noise. Electrical random noise is mainly thermal noise. 2) Accelerometer fabrication error. 3) Accelerometer measurement error. 4) Errors from environmental variation including temperature variation and magnetic field variation. 2. Wireless transmission errors caused by data loss, data retransmission, etc.

Once the error sources are known, error-processing methods are proposed to correct the bias based on hardware and software data processing.

During wireless transmission, the ZigBee communication protocol is applied to improve transmission reliability. ZigBee is an IEEE 802.14.4-based specification for a suite of high-level communication protocols used to create personal area networks with small, low-power digital radios. ZigBee’s advantages include reliability of transmission and negligible data loss rate in a small area.

6.3.2 Noise processing algorithms

I proposed several methods to filter the acceleration data to make the acceleration data available to integrate. The flow diagram for data processing is shown in Figure 6.7.

Figure 6.7 Data processing flow diagram.

6.3.3 Time domain and spectral analysis

The IMU has noisy data when stationary. To analysis the impact of noise, we collect 16000 stationary data, i.e. 4000 for each axis plus 4000 starting coordinates data. Then, we calculate the average acceleration value for each axis. Finally, we subtract the average acceleration value from each axis acceleration to get the baseline of each axis acceleration. Since the data is too long to plot, only the first 400 are shown in Figure 6.8. The acceleration of the X, Y and Z axis is designated as Ax, Ay, and Az respectively.

Figure 6.8 Acceleration for a stationary system.

From Figure 6.8, it is clear that the acceleration vibrates randomly with a maximum value of 0.05푚/푠2. That is the system noise generated after acceleration voltage signal quantification, i.e. the object’s trajectory has maximum 0.05m shift when it keeps stationary. The errors have a big influence on the tracking system accuracy.

This is why it is necessary to eliminate noise from the input acceleration data. To eliminate noise, it is necessary to Analysis the signal properties in the frequency domain.

(a) Spectral of acceleration at each axis when stationary.

(b) Spectral of acceleration at each axis when moving. Figure 6.9 Signal properties in frequency domain.

After Fourier transforming the stationary acceleration data with MATLAB, it is clear from Figure 6.9 (a) that the noise of each axis is mainly around 50Hz. From the Figure 6.9 (b), the valid acceleration data is mainly around 10Hz for the X-axis and 15Hz for the Y-axis. Hence, the sampling rate must be larger than rate Nyquist frequency higher than 30Hz to restore the acceleration signal.

6.3.4 Low-pass filter

A low pass filter is applied to process collected acceleration data to eliminate noise. A low-pass filter is a filter that passes signals with a frequency lower than a certain cutoff frequency and attenuates signals with frequencies higher than the cutoff frequency.

By programming the IMU chip BNO055, the data output frequency is 50Hz. A hardware low-pass filter is installed between the IMU output terminal and wireless transmission chip to eliminate high-frequency noise. Since the environment is not the obvious noise source, the low-pass filter is designed as a simple RC circuit which has a cutoff frequency of 50Hz consisting of two standard resistors in serials of resistance 2.4kΩ and 750Ω, and a 1μF capacitor. The cutoff frequency is:

1 1 푓 = = ≈ 50퐻푧 (6.21) 푐 2휋푅퐶 2×3.14×(2400+750)×1×10−6

The circuit is shown in Figure 6.10:

Figure 6.10 RC low-pass filter.

Based on the acceleration data spectrum analysis, a software low pass filter is applied. The following low pass filters were considered. A Butterworth filter, a Chebyshev type 1 filter, a Chebyshev type 2 filter and an Elliptic filter. Figure 6.11 shows the frequency responses of the four types of low pass filters.

Figure 6.11 Frequency responses of low pass filters (adopted from [14]).

Compared with other filters, the signal decay of the Butterworth low-pass filter is much slower than another filter, but it is smoother and has no oscillations. Since the tracking algorithm is based on double integral of acceleration, we prefer to use Butterworth to reduce cumulative errors.

The MATLAB function “buttord” and the function “butter” are used to design the Butterworth digital filter. The function “buttord” calculates the minimum order of a digital or analog Butterworth filter required to meet a set of filter design specifications. [푛, 푊푛] = 푏푢푡푡표푟푑(푊푝, 푊푠, 푅푝, 푅푠) returns the lowest order, 푛, of the digital Butterworth filter with no more than 푅푝 dB of passband ripple and at

68 least 푅푠 dB of attenuation in the stopband. The scalar (or vector) of corresponding cutoff frequencies, 푊푛 is also returned. The output arguments of the function “butter” are 푛 and 푊푛 [18].

If the sampling rate is 푓푆, the passband corner frequency is 푓퐿, the stopband 푓퐿 푓퐻 corner frequency is 푓퐻, then 푊푝 = 푓푆 , 푊푠 = 푓푆 . 2 2

The function “butter” is used to design a Butterworth filter. The function is [푏, 푎] = 푏푢푡푡푒푟(푛, 푊푛), which returns the transfer function coefficients of an nth-order low pass digital Butterworth filter with normalized cutoff frequency 푊푛. In this thesis, the sampling rate is 500Hz, the passband corner frequency is 15Hz, the stopband corner frequency is 50Hz, the maximum passband ripple is 3dB, and the minimum attenuation is 60dB. The frequency response of the 4th order low pass digital Butterworth filter obtained using “butter” are shown in Figure 6.12.

Figure 6.12 Butterworth low pass filter frequency response.

The time-domain graph and spectrogram of acceleration after Butterworth low pass filtering for stationary snake are shown in Figure 6.13.

Figure 6.13 Time-domain graph of acceleration value after Butterworth low pass filter when it keeps stationary (red = filtered acceleration).

Figure 6.14 Spectrogram of acceleration after Butterworth low pass filter when the snake moving.

It is clear from Figure 6.14, there is the noise only in the pass band and its amplitude is very small.

6.3.5 IMU acceleration data calibration

The IMU data is crucial for robot locomotion analysis, but before the Analysis, it is necessary to calibrate the data to ensure the validity of the acceleration values. The BNO055 has an internal calibration program, the calibration level is rated from 0 to 3 where 0 is no calibration and 3 is fully calibrated. However, even though it has the calibration function; the precision cannot be guaranteed. The solution is to compare BNO055’s data with other commercial IMU products. Although we cannot completely trust the commercial product, the probability of inaccuracy of both IMUs is very low. Flight control requires high precision IMU data to control the aircraft’s flying attitude. So the IMU data from flight controller is used to compare with the IMU data from the snake robot to complete IMU calibration.

The first step is to run the BNO055’s internal program to calibrate the IMU until 3 and run the flying controller to collect data until stable. The next step is to fix them together with screws so that the BNO055 and flying can move together. Intuitively, the IMU data read from both of them should be the same when neglecting system noise. Finally, we power them up and begin to collect IMU data from BNO055 and from the flying controller for over 2 minutes as the IMU sensor needs above 10 seconds to calibrate itself. During the test period planned, let both devices move irregularly. The IMU BNO055 demonstrates the similar output as the flying controller. The test results are illustrated in Figure 6.15 (a) and Figure 6.15 (b).

(a) Acceleration data at X-axis.

(b) Acceleration data at Y-axis.

Figure 6.15 IMU data calibration.

Comparing in the figure, intuitively, whatever in X-axis or -axis, two curves from BNO055 and flying controller are almost matched. The oscillations and amplitude are almost the same indicating that the measured data from BNO055 and the flying

73 controller are almost the same. The collected curves have a small right shift because the measurement starting time is not the same (BNO055 started first). Considering the influences of system noise and the gravity component, the absolute errors of two curves are acceptable. We compare the two sets of data from BNO055 and flying controller using excel. We first deleting initial collected zero data to eliminate the right shit, then use Excel to calculate the covariance of the two sets X-axis acceleration data and the two sets Y-axis acceleration data. The results are 0.2296 for the X data and 0.5932 for Y data, which shows a good match. Thus, the BNO055’s data is acceptable for snake robot locomotion analysis. In the next section, IMU data wireless transmission and robot locomotion tracking model based on acceleration integration are discussed.

6.3.6 IMU baseline calibration

After low-pass filtering, part of the noise is eliminated from the original IMU acceleration data. The acceleration data becomes smooth and available to be integrated. Before integrating the data, the baseline shift cannot be ignored.

The snake moves forward in a line and lifts its head, then maintains stationary. The collected acceleration data of each axis is plotted after low-pass filtering as shown in Figure 6.16:

Figure 6.16 Baseline calibration test.

From the test results, the red line is the data after the low-pass filtering, which basically follows the original acceleration with less oscillation. Zoom in between 5000 and 8000, it shows that the baseline has shifted from zero, especially in the x-axis and y-axis direction. The shifts are shown in Figure 6.17

Figure 6.17 Acceleration data between 5000 and 8000.

The baseline shift in the x-axis direction is about −0.3푚/푠2 , i.e. the X-axis displacement in the tracking system is -0.3푚/푠 even if the snake-robot is stationary. Similarily the baseline shift in the y-axis direction is about 0.3푚/푠2, i.e. the Y-axis movement is 0.3푚/푠 when the snake robot is stationary. If the baseline shitf is included in the integration, the cumulative errors will have big a influence on the accuracy of final the claculations.

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. Using MATLAB to plot the absolute difference distribution of acceleration data between 5000 to 8000 which is shown in Figure 6.18.

Absolute Difference Distribution at X-Axis 70.00%

60.00% 63.57% 50.00% 40.00% 30.00% 20.00% 20.36% 10.00% 11.29% 4.03% 0.68% 0.07% 0.00% 0 0.01 0.02 0.03 0.04 0.05

Absolute Difference Distribution at Y-Axis 70.00% 60.00% 61.00% 50.00% 40.00% 30.00% 20.00% 10.00% 13.53% 13.04% 4.94% 1.33% 0.33% 0.03% 0.00% 0 0.01 0.02 0.03 0.04 0.05 0.06

Absolute Difference Distribution at Z-Axis 60.00%

50.00% 53.03% 40.00%

30.00%

20.00% 5.40% 4.39% 4.03% 2.90% 1.59% 1.11% 0.42% 0.20% 0.03% 10.00% 4.72% 5.69% 4.55% 3.32% 5.86% 1.56% 0.55% 0.46% 0.13% 0.00% 0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 0.1 0.11 0.12 0.13 0.14 0.15 0.16 0.17 0.18

Figure 6.18 Absolute difference of acceleration data when stationary.

The above statistic shows that the data oscillation in the X-axis and Y-axis direction are within 0.05, while in the Z-axis direction, the oscillation is in a wider range of 0.1. Table 6.1 shows the probability of absolute difference within the range when the snake robot keeps stationary. The statistic is shown in Table 6.1.

Table 6.1 Statistic of oscillation distribution when stationary.

X-axis “<0.05” Y-axis “<0.05” Z-axis “<0.1” 1 0.9418 0.9548 The probability of absolute difference within 0.05 is 1 at X-axis, 0.9418 at Y-axis. The probability of absolute difference within 0.1 is 0.9548 at Z-axis. The results show that there is the big probability that the absolute difference is within 0.1 when the snake is stationary. To compare with acceleration data when the snake robot is moving, we use MATLAB to plot the absolute difference distributions between 5000 to 8000 which are shown in Figure 6.19.

Absolute Difference Distribution at X-Axis 2.00% 1.50% 1.00% 0.50%

0.00%

0.3 0.6 0.9

0.18 0.72 0.03 0.06 0.09 0.12 0.15 0.21 0.24 0.27 0.33 0.36 0.39 0.42 0.45 0.48 0.51 0.54 0.57 0.63 0.66 0.69 0.75 0.78 0.81 0.84 0.87 0.93 0.96 0.99

Absolute Difference Distribution at Y-Axis 2.00% 1.50% 1.00% 0.50%

0.00%

0.3 0.6 0.9

0.18 0.72 0.03 0.06 0.09 0.12 0.15 0.21 0.24 0.27 0.33 0.36 0.39 0.42 0.45 0.48 0.51 0.54 0.57 0.63 0.66 0.69 0.75 0.78 0.81 0.84 0.87 0.93 0.96 0.99

Absolute Difference Distribution at Z-Axis 1.50% 1.00% 0.50%

0.00%

0.3 0.6 0.9

0.18 0.72 0.06 0.09 0.12 0.15 0.21 0.24 0.27 0.33 0.36 0.39 0.42 0.45 0.48 0.51 0.54 0.57 0.63 0.66 0.69 0.75 0.78 0.81 0.84 0.87 0.93 0.96 0.99 0.03

Figure 6.19 Absolute difference of acceleration data when moving.

Figure 6.19 shows that when the snake robot is moving, the acceleration values fluctuate in a wider range than when stationary. Table 6.2 shows the probability of absolute difference within the range when the snake robot is moving.

Table 6.2 Statistic of oscillation distribution when moving.

X-axis “<0.05” Y-axis “<0.05” Z-axis “<0.1” 0.0839 0.0704 0.0124 Comparing with the Table 6.1, the distribution of absolute difference when stationary is concentrated in [0,0.05] for the X-axis and Y-axis, and [0,0.1] for the

Z-axis. The distribution of absolute difference for motion data is concentrated in a smaller interval. More experiments verified this conclusion.

Every 1000 acceleration data is one group to calculate the probability of range within [0,0.05] at Y-axis only. 10 groups of data repeat the calculation. The results are shown in Table 6.3.

Table 6.3 Probability of range within [0,0.05]

Probability Group 1 Group 2 Group 3 Group 4 Group 5 Stationary 0.9418 0.9854 0.9712 0.9755 0.9587 Moving 0.0704 0.0839 0.0655 0.0814 0.0716 Probability Group 6 Group 7 Group 8 Group 9 Group 10 Stationary 0.9358 0.9456 0.9718 0.9466 0.9810 Moving 0.0767 0.0802 0.0835 0.0924 0.0749 From Table 6.3, the data verifies the previous conclusion which the probability of an absolute difference in the range [0,0.05] when stationary is much higher than the probability when moving. To calibrate the baseline, take every 100 data (i.e. data in 0.2s) to have a statistic. The statistic data shows whether the probability of the absolute difference in the range [0,0.05] is higher than 0.95. If yes, indicating the snake robot is stationary. Then take the average value of this group of data 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 shows the snake robot is stationary, the value of acceleration data is set to zero.

The baseline calibration results are shown in Figure 6.20 with zooming in on the figure from 2000 to 3500 in Figure 6.21. The results after baseline calibration are zero when the snake robot is stationary.

Figure 6.20 Acceleration data after baseline calibration.

Figure 6.21 Baseline calibration results.

6.3.7 Kalman Filter

To eliminate random errors, the Kalman Filter is applied to the tracking system to filter the acceleration data. The Kalman Filter is a set of mathematical equations that provides an efficient computational (recursive) state estimate that minimizes the mean squared error.

The Kalman filter is a recursive least-squared estimation that uses a series of noisy measurements observed over time indicating Kalman Filters have numerous applications in guidance, navigation, and control. Kalman filters are also used in the robotic motion planning and control, and trajectory optimization.

A Kalman Filter is applied to the tracking system to filter random noise in the acceleration signal. A measurement model is proposed to estimate the accelerometer’s output data and future states based on the IMU data. The optimization criterion used here is the minimization of the mean-square estimation error of the acceleration data.

The random process to be estimated can be modeled in the form [15]

푥푘+1 = 휙푘푥푘 + 푤푘 + 퐵푘푢푘 (6.22)

Here: 푥푘 = (n×1) process state vector at time 푡푘;

푤푘 = (푛×1). It is the zero-mean white noise for the time interval (푡푘+1, 푡푘).

휙푘 = (푛×푛) is the state-transition matrix.

퐵푘 = (n×m) is the input matrix. In this system, there is no control input

during data transmission, so 푢푘 = 0. where the process state vector is

푥푘 = [퐴푐푐푒푙푒푟푎푡푖표푛_푥, 퐴푐푐푒푙푒푟푎푡푖표푛_푦, 퐴푐푐푒푙푒푟푎푡푖표푛_푧] (6.23)

The initial process state vector is the initial set of acceleration data.

The state transition matrix is

1 0 0 휙푘 = [0 1 0] (6.24) 0 0 1

The observation (measurement) of the process occurs at discrete points in time in accordance with the linear relationship [15]

푧푘 = 퐻푘푥푘 + 푣푘 (6.25)

Here: 푣푘 = (푚×1) is the measurement noise. It is assumed to be a zero-mean white sequence with known covariance structure and having zero crosscorrelation with the 푤푘 sequence.

퐻푘 = (푚×푛) is the measurement matrix at time 푡푘 where the measurement matrix is:

1 0 0 퐻푘 = [0 1 0] (6.26) 0 0 1

The mean of noise 푤푘 and 푣푘 is 0 respectively, the covariance matrices for the 푤푘 and 푣푘 vectors are given by [15]

푄 , 푖 = 푘 E[푤 푤푇] = { 푘 (6.27) 푘 𝑖 0, 푖 ≠ 푘

푅 , 푖 = 푘 E[푣 푣푇] = { 푘 (6.28) 푘 𝑖 0, 푖 ≠ 푘

푇 E[푤푘푣𝑖 ] = 0, for all 푘 and 푖 (6.29)

For calculating measurement noise covariance value 푅푘 and system noise covariance value 푄 푘, the IMU chip was split from snake robot to collect data when it maintains stable. Since the IMU does not move, the acceleration data it collected should be unchanged. However, the data oscillates as shown in Figure 6.22 (a), Figure 6.22(b) and Figure 6.22(c).

(a) Acceleration data at X-axis

(b) Acceleration data at Y-axis (c) Acceleration data at Z-axis

Figure 6.22 Acceleration data when stationary.

Because the IMU maintains stable, the oscillations are considered to the input white noise 푤푘. The measurement noise covariance matrixes 푄푘 can be calculated by equation (6.27). The measurement noise covariance matrixes 푅푘 can be calculated from measurement data by equation (6.28.)

The initial estimate of the process in time 푡푘 is based on our knowledge. This prior − estimate will be denoted as 푥̂푘 . The estimation error is [15]

− − 푒푘 = 푥푘 − 푥̂푘 (6.30)

And the associated error covariance matrix is [15]

− − −푇 푃푘 = 퐸[푒푘 푒푘 ] (6.31)

In a tracking system, we begin the estimation problem with no prior measurements. Thus, the initial estimate is zero and the associated error covariance matrix is just the covariance matrix of 푥 itseft.

For easy calculation, the initial estimation error covariance is set manually and a little bit big so that the Kalman Filter will think the estimations at the first few steps are not optimal, the filter will keep going to process until the real optimization.

The initial estimation error covariance is manually selected as:

. 05 0 0 − 푃푘 = [ 0 . 05 0 ] (6.32) 0 0 . 05

− With the assumption of a prior estimate ̂푥푘 , the measurement 푧푘 is used to improve the prior estimate [15].

− − 푥̂푘 = 푥̂푘 + 퐾푘(푧푘 − 퐻푘푥̂푘 ) (6.33) where

푥̂푘 = updated estimate

퐾푘 = is the optimal Kalman gain, the one that minimizes the mean-square estimation error [15].

− 푇 − 푇 −1 퐾푘 = 푃푘 퐻푘 (퐻푘푃푘 퐻푘 + 푅푘) (6.34)

The covariance matrix associated with the optimal estimate is [15]

− 푃푘 = (I − 퐾푘퐻푘)푃푘 (6.35)

− To accomplish the Kalman Filter loop, the state 푥̂푘+1 are easily projected ahead [15]

− 푥̂푘+1 = 휙푘푥̂푘 (6.36)

− The error covariance matrix associated with 푥̂푘+1 is expressed as [15]

− 푇 푃푘+1 = 휙푘푃푘휙푘 + 푄푘 (6.37)

Equations (6.33), (6.34), (6.35), (6.36) and (6.37) comprise the Kalman filter recursive equations [15].

Now all the data and equations are ready to enter into the Kalman Filter loop. Once the loop is entered it can be continued ad infinitum. The data is transferred by the wireless chip to the computer serial port, then MATLAB will read it and take the data to enter Kalman Filter loop. These steps ensure the data are processed in real time and do not influence the snake robot’s Arduino control.

Kalman Filter loop is shown in Figure 6.23

Figure 6.23 Kalman filter loop [15].

The Kalman filter MATLAB function is presented below, and whole program is given in the appendix. function s = Kalman_Filter_Function(s) % Prediction for state vector and covariance: s.x = s.A*s.x + s.B*s.u; s.P = s.A * s.P * s.A' + s.Q; % Compute Kalman gain: K = (s.P*s.H')/(s.H*s.P*s.H'+s.R); % Correction based on measurements: s.x = s.x + K*(s.z-s.H*s.x); s.P = s.P - K*s.H*s.P; return end

For testing the availability and efficiency of the acceleration data processing algorithm, the IMU chip is split from the snake robot to collect several sets of acceleration data. All the data are processed and plotted by MATLAB. The example shows below is for the IMU moving in an S-sharp. The acceleration data is plotted in Figure 6.24 (a), Figure 6.24 (b) and Figure 6.24 (c). The red line is the acceleration data after Kalman filtering.

(a) Acceleration data at X-axis.

(b) Acceleration data at Y-axis. (c) Acceleration data at Z-axis. Figure 6.24 Acceleration data processing algorithm test results.

After double integral, the displacements along each axis are also shown in Figure 6.25 (a), Figure 6.25 (b) and Figure 6.25(c).

(a) Displacement at X-axis.

(b) Displacement at Y-axis. (c) Displacement at Z-axis. Figure 6.25 snake displacement along each axis.

The IMU’s trajectory in 3D space in shown in Figure 6.26.

Figure 6.26 IMU’s tracking trajectory in 3D space.

From the figures, the acceleration curve after Kalman filtering become smooth especially when the IMU moves quickly. The displacement curve after double integral is smooth. The IMU’s trajectory almost follows the S-shape. Therefore, the tracking results match the object’s actual trajectory and the errors are acceptable.

6.4 Snake robot locomotion analysis

6.4.1 Snake robot locomotion analysis and calibration

In the laboratory environment, the IMU chip is installed in the head of the snake robot and the data is transmitted by the Xbee wireless module. The snake robot moves along a straight line. The filtered acceleration data are shown in Figure 6.27, including snake robot’s acceleration, velocity, displacement and trajectory along each axis. The brown line is the raw acceleration data, the blue line is the data after the low-pass filter and baseline calibration, and the red line is the Kalman filtered data after. The snake robot’s velocity tracking results are shown in Figure 6.28. The snake robot’s locomotion is shown in Figure 6.29.

Figure 6.27 Snake robot acceleration data.

Figure 6.28 Snake robot velocity.

Figure 6.29 Snake robot track distance in X, Y and Z axes.

To verify the IMU tracking system’s efficiency and verify the simulation presented in Chapter 3, a video tracking software is used. Let the snake robot move along a

93 straight line. The video recorder is set to be perpendicular to the snake robot to make the video tracking software work well. The video tracking software outputs a mass point displacement and velocity. The results of the comparison of the two tracking systems and the displacements along the Y-axis are much bigger than other two axes. The data of the y component are used to verify the IMU tracking system efficiency.

The two position curves for the Y-axis are shown in Figure 6.30.

Figure 6.30 Comparison between video tracking and IMU tracking results.

The absolute displacement and average velocity of the two tracking systems are listed in Table 6.4.

Table 6.4 Absolute displacement and average velocity.

Displacement(cm) Velocity(cm/s) IMU Tracking 106.85 28.59 Video Tracking 95.25 25.34 Simulation 100 24.3 Since the video tracking software has higher accuracy, results of video tracking are used as the reference to calculate the error of IMU tracking and simulation. In the simulation presented in Chapter 3, if the snake moves forward 100cm the average velocity is 0.243푚/푠. Thus, the simulation matches actual locomotion.

Table 6.5 Errors of IMU tracking system and simulation.

Error Displacement Velocity IMU Tracking 12.17% 12.8% Simulation * 4.1% For no measurements in the Z-direction. The X-axis displacement in cm and velocity in cm/s are shown in Table 6.6.

Table 6.6 Comparison of IMU tracking and video tracking.

Video IMU Average Video Video IMU IMU Dis. Num. Ave Ave Velocity Dis-X Dis-Y Dis-X Dis-Y error Speed Speed Error 1 34.37 95.85 38.65 106.85 24.90 27.12 8.93% 11.59% 2 31.56 93.48 36.14 105.61 24.28 26.80 10.40% 13.14% 3 32.14 96.54 36.54 107.36 25.08 27.25 8.67% 11.48% 4 33.19 101.3 39.45 109.74 26.31 27.85 5.88% 9.87% 5 36.21 98.65 42.19 109.22 25.62 27.72 8.19% 11.56% 6 34.95 97.51 38.47 110.35 25.33 28.01 10.58% 12.85% 7 35.41 101.3 39.1 113.84 26.31 28.89 9.81% 12.18% 8 34.87 95.11 40.3 107.97 24.70 27.40 10.93% 13.78% 9 39.12 105.9 45.85 118.54 27.50 30.09 9.41% 12.71% 10 34.84 96.34 36.54 108.21 25.02 27.46 9.76% 11.70% Based on the experiments the average displacement error is 12.01%, the average velocity error is 9.25%. Compared to other tracking methods, the tracking error is acceptable. The error may come from:

1) Accelerometer accuracy. 2) The measurement error due to mechanical characteristics of MEMS under the influence of the environment. 3) Gravity. 4) Random noise.

7 Conclusion

This thesis developed a new snake robot link structure. A snake robot model was built by software to simulate the snake robot motion based on the proposed mathematical model. Considering the friction effects of snake scales, we proposed the solution of forward and inverse kinematics. This thesis proposes a solution to build a snake robot starting with a mathematical model of the snake robot locomotion and including locomotion simulation, the solution of the mechanical model, power source, wireless data transmission and locomotion analysis based on IMU data.

In the future, we will improve the mechanical structure of the snake robot to make it lighter to improve its mobility especially on non-smooth surfaces such as sand and grass. We will also build a special version for underwater exploration. Based on the improvement of the mechanical structure, there will be more space inside the snake for sensors which gives the snake robot more advantages in practical application.

Bibliographic

[1] S. Hirose, Biologically Inspired Robots: Snake-Like Locomotors and Manipulators, Oxford University Press, Oxford, 1993. [2] M. Mori, S. Hirose, Three-dimensional serpentine motion and lateral rolling by active cord mechanism ACM-R3, in: Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 2002, pp. 829–834. [3] G. Miller, Snake robots for search and rescue, in: Neurotechnology for Biomimetic Robots, MIT Press, Cambridge, MA, USA, London, 2002, pp. 271–284. (Chapter).

[4] H. Yamada, S. Chigisaki, M. Mori, K. Takita, K. Ogami, S. Hirose, Development of amphibious snake-like robot ACM-R5, in: Proc. 36th Int. Symp. Robotics, 2005. [5] C. Wright, A. Johnson, A. Peck, Z. McCord, A. Naaktgeboren, P. Gianfortoni, M. Gonzalez-Rivero, R. Hatton, H. Choset, Design of a modular snake robot, in: Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 2007, pp. 2609–2614. [6] H. Yamada, S. Hirose, Study of a 2-DOF joint for the small active cord mechanism, in: Proc. IEEE Int. Conf. Robotics and Automation, 2009, pp. 3827–3832. [7] G. Granosik, J. Borenstein, M.G. Hansen, Industrial Robotics: Programming, Simulation and Applications, Pro Literatur Verlag, Germany, ARS, Austria, 2006 (Chapter 33), pp. 633–662. [8] J.C. McKenna, D.J. Anhalt, F.M. Bronson, H.B. Brown, M. Schwerin, E. Shammas, H. Choset, Toroidal skin drive for snake robot locomotion, in: Proc. IEEE Int. Conf. Robotics and Automation, May 2008, pp. 1150–1155. [9] P Liljebäck, K.Y. Pettersen, Ø Stavdahl, A snake robot with a contact force measurement system for obstacle-aided locomotion, in: Proc. IEEE Int. Conf. Robotics and Automation, Anchorage, AK, USA, 2010, pp. 683–690. [10] J. Gray, The mechanism of locomotion in snakes, Journal of Experimental Biology 23 (2) (1946) 101–120.

[11] Animating reptiles and snakes. http://www.rubberbug.com/reptiles.htm. [12] S. Hirose, H. Yamada, Snake-like robots: Machine design of biologically inspired robots, in: Proc. IEEE Robotics and Automation Magazine, 2009, vol. 19, issue 1, pp. 88-98.

[13] 7 Animated FIGs that will make you instantly understand trigonometry, http://www.businessinsider.com/7-gifs-trigonometry-sine-cosine-2013-5. [14] Linear filter in Wikipedia, https://en.wikipedia.org/wiki/Linear_filter. [15] PATRICK Y C HWANG R B. Introduction to Random Signals and Applied Kalman Filtering with MATLAB Exercises, 4th edition, 2012, pp: 153-162.

[16] P. Liljebäck, K. Y. Pettersen, Ø. Stavdah, & J. T. Gravdahl. (2012). A review on modelling, implementation, and control of snake robots. Robotics & Autonomous Systems, 60(1), 29–40. [17] How to: Parallel Charging, http://www.tjinguytech.com/charging-how-tos/parallel- charging. [18] Description of “buttord” function, https:// www.mathworks.com/ help/ signal/ ref/ buttord.html [19] T. Sato, W. Watanabe, A. Ishiguro, An adaptive decentralized control of a serpentine robot based on the discrepancy between body, brain and environment, in: Proc. IEEE Int. Conf. Robotics and Automation, May 2010, pp. 709–714. [20] H. Date, Y. Hoshi, M. Sampei, Locomotion control of a snake-like robot based on dynamic manipulability, in: Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 2000. [21] F. Matsuno, K. Suenaga, Control of redundant 3D snake robot based on kinematic model, in: Proc. IEEE Int. Conf. Robotics and Automation, 2003, pp. 2061–2066. [22] M. Ishikawa, K. Owaki, M. Shinagawa, T. Sugie, Control of snake-like robot based on nonlinear controllability analysis, in: IEEE Int. Conf. Control Applications, 2010, pp. 1134–1139.

[23] R. Linnemann, K.L. Paap, B. Klaassen, J. Vollmer, Motion control of a snakelike robot, in: Proc. Third European Workshop on Advanced Mobile Robots, 1999, pp. 1–8. [24] F. Chernousko, Snake-like locomotions of multilink mechanisms, Journal of Vibration and Control 9 (1–2) (2003) 235–256. [25] A.A. Transeth, N. van de Wouw, A. Pavlov, J.P. Hespanha, K.Y. Pettersen, Tracking control for snake robot joints, in: Proc. IEEE/RSJ Int. Conf. Intelligent

Robots and Systems, San Diego, CA, USA, October–November 2007, pp. 3539– 3546. [26] J.W. Burdick, J. Radford, G. Chirikjian, A sidewinding locomotion gait for hyper- redundant robots, Advanced Robotics 9 (3) (1995) 195–216. [27] J. Gonzalez-Gomez, H. Zhang, E. Boemo, Locomotion principles of 1D topology pitch and pitch-yaw-connecting modular robots. Advanced Robotics Systems International and I-Tech Education and Publishing, September 2007, pp. 403–428 (Chapter 24). [28] D. Rincon, J. Sotelo, Ver-vite: dynamic and experimental analysis for inchwormlike biomimetic robots, IEEE Robotics and Automation Magazine 10 (4) (2003) 53–57. [29] L. Chen, Y. Wang, S. Ma, B. Li, Studies on lateral rolling locomotion of a snake robot, in: Proc. IEEE Int. Conf. Robotics and Automation, 2004, pp. 5070–5074. [30] G. Hicks, K. Ito, A method for determination of optimal gaits with application to a snake-like serial-link structure, IEEE Transactions on Automatic Control 50 (9) (2005) 1291–1306. [31] K.A. McIsaac, J.P. Ostrowski, A framework for steering dynamic robotic locomotion systems, International Journal of Robotics Research 22 (2) (2003) 83–97. [32] K. Lipkin, I. Brown, H. Choset, J. Rembisz, P. Gianfortoni, A. Naaktgeboren, Differentiable and piecewise differentiable gaits for snake robots, in: Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, San Diego, CA, USA, October–November 2007, pp. 1864–1869. [33] F. Chernousko, Modelling of snake-like locomotion, Applied Mathematics and Computation 164 (2) (2005) 415–434.

[34] A. Shapiro, A. Greenfield, H. Choset, Frictional compliance model development and experiments for snake robot climbing, in: Proc. IEEE Int. Conf. Robotics and Automation, 2007, pp. 574–579. [35] Z. Zuo, Z. Wang, B. Li, S. Ma, Serpentine locomotion of a snake-like robot in water environment, in: IEEE Int. Conf. Robotics and Biomimetics, 2008, pp. 25–30.

100

Appendix

A MATLAB Files

A.1 Data processing algorithm program close all; clear;clc; [time, data] = rtpload('922mass-A-IMU.txt'); %data retrived by IMU Acem_x_raw = data.x; Acem_y_raw = data.y; Acem_z_raw = data.z; Cov_x = data.Cov_x; Cov_y = data.Cov_y; Cov_z = data.Cov_z;

%set time step delta_t = 1; %total = 1:delta_t:legth(Ace,_x) total = 1:length(Acem_x_raw); %Error covariance of measurement model e_cov_x = mean(Cov_x); e_cov_y = mean(Cov_y); e_cov_z = mean(Cov_z);

%Z-axis to zero for j=1:length(total) Acem_z_raw(j)=Acem_z_raw(j)+9.8; end

101

%Differential value for i=1:length(total)-1 d_Acem_x(i)=abs(Acem_x_raw(i+1)-Acem_x_raw(i)); if(d_Acem_x(i)<0.04) Acem_x_raw(i)=0; end

d_Acem_y(i)=abs(Acem_y_raw(i+1)-Acem_y_raw(i)); if(d_Acem_y(i)<0.04) Acem_y_raw(i)=0; end

d_Acem_z(i)=abs(Acem_z_raw(i+1)-Acem_z_raw(i)); if(d_Acem_z(i)<0.1) Acem_z_raw(i)=0; end end

%Butterworth Low-pass Filter Wp = 30/500; Ws = 60/500; [n,Wn] = buttord(Wp,Ws,3,60);

[b,a] = butter(n,Wn); [z,p,k] = butter(n,Wn); sos = zp2sos(z,p,k);

Acem_x=filter(b,a,Acem_x_raw); Acem_y=filter(b,a,Acem_y_raw); Acem_z=filter(b,a,Acem_z_raw);

102

%Initialize state s.x = [Acem_x(1);Acem_y(1);Acem_z(1)];

%Enter State(1*3) %Define a process noise of state; %Define the measurement matrix H; %Enter measurement matrix H(3*3) for measurement model z s.H = eye(3); %Define transition matrix A s.A = eye(3); %Define a measurement error s.B = eye(3); %Initial value of u s.u = [0; 0; 0]; %Enter covariance matrix Q for state x s.Q = [1.00e-4 0 0; 0 1.37e-4 0; 0 0 2.22e-3]; %Enter covariance matrix R for measurement model z s.R=[e_cov_x 0 0; 0 e_cov_y 0; 0 0 e_cov_z]; %Initial covariance matrix P; s.P = [.5 0 0; 0 .5 0; 0 0 .5]; for t=1:length(total)

%Enter covariance matrix Q for state x

103

s(t).Q = [1.00e-3 0 0; 0 1.37e-3 0; 0 0 2.22e-2]; s(t).A = eye(3); s(t).R = [Cov_x(t) 0 0; 0 Cov_y(t) 0; 0 0 Cov_z(t)]; % Enter System State s(t).z = [Acem_x(t); Acem_y(t); Acem_z(t)]; %Perform a Kalman Filter iteration s(t+1) = Kalman_Filter_Function(s(t)); a_x_p(t) = s(t+1).x(1); a_y_p(t) = s(t+1).x(2); a_z_p(t) = s(t+1).x(3); Cov_x_p(t) = s(t+1).P(1,1); Cov_y_p(t) = s(t+1).P(2,2); Cov_z_p(t) = s(t+1).P(3,3); end v_x(1) = 0; s_x(1) = 0; %Initialize velocity and position. v_y(1) = 0; s_y(1) = 0; v_z(1) = 0; s_z(1) = 0; for t = 2:length(total) v_x(t) = v_x(1)+((a_x_p(t)+a_x_p(t-delta_t))*delta_t)/2; v_y(t) = v_y(1)+((a_y_p(t)+a_y_p(t-delta_t))*delta_t)/2; v_z(t) = v_z(1)+((a_z_p(t)+a_z_p(t-delta_t))*delta_t)/2;

s_x(t) = s_x(1)+v_x(t-delta_t)*delta_t+1/4*(a_x_p(t- delta_t)+a_x_p(t)*delta_t^2);

104

s_y(t) = s_y(1)+v_y(t-delta_t)*delta_t+1/4*(a_y_p(t- delta_t)+a_y_p(t)*delta_t^2); s_z(t) = (s_z(1)+v_z(t-delta_t)*delta_t+1/4*(a_z_p(t- delta_t)+a_z_p(t)*delta_t^2))/1000;

s_delta(t) = sqrt((s_x(t)-s_x(t-delta_t))^2+(s_y(t)-s_y(t-delta_t))^2+(s_z(t)-s_z(t- delta_t)^2)); end

dis_x=cumtrapz(total,s_x); dis_y=cumtrapz(total,s_y); dis_z=cumtrapz(total,s_z);

A.2 Butterworth Filter, Kalman Filter program

%Butterworth Low-pass Filter Wp = 15/250; Ws = 50/250; [n,Wn] = buttord(Wp,Ws,3,60); [b,a] = butter(n,Wn); [z,p,k] = butter(n,Wn); sos = zp2sos(z,p,k); freqz(sos,512,1000) title(sprintf('n = %d Butterworth Lowpass Filter',n))

%Initialize state s.x = [Acem_x(1);Acem_y(1);Acem_z(1)];

%Enter State(1*3) %Define a process noise of state;

105

%Define the measurement matrix H; %Enter measurement matrix H(3*3) for measurement model z s.H = eye(3); %Define transition matrix A s.A = eye(3); %Define a measurement error s.B = eye(3); %Initial value of u s.u = [0; 0; 0]; %Enter covariance matrix Q for state x s.Q = [1.00e-4 0 0; 0 1.37e-4 0; 0 0 2.22e-3]; %Enter covariance matrix R for measurement model z s.R=[e_cov_x 0 0; 0 e_cov_y 0; 0 0 e_cov_z]; %Initial covariance matrix P; s.P = [.5 0 0; 0 .5 0; 0 0 .5]; for t=1:length(total)

%Enter covariance matrix Q for state x s(t).Q = [1.00e-1 0 0; 0 1.37e-1 0; 0 0 2.22e0]; s(t).A = eye(3); s(t).R = [Cov_x(t) 0 0; 0 Cov_y(t) 0;

106

0 0 Cov_z(t)]; % Enter System State s(t).z = [Acem_x(t); Acem_y(t); Acem_z(t)]; %Perform a Kalman Filter iteration s(t+1) = Kalman_Filter_Function(s(t)); a_x_p_raw(t) = s(t+1).x(1); a_y_p_raw(t) = s(t+1).x(2); a_z_p_raw(t) = s(t+1).x(3); Cov_x_p(t) = s(t+1).P(1,1); Cov_y_p(t) = s(t+1).P(2,2); Cov_z_p(t) = s(t+1).P(3,3); End

A.3 Processing program

#include #include #include #include const int reset = 6; //the number of reset pin /* Set the delay between fresh samples */ #define BNO055_SAMPLERATE_DELAY_MS (100) Adafruit_BNO055 bno = Adafruit_BNO055(55); void displaySensorDetails(void) { sensor_t sensor; bno.getSensor(&sensor); Serial.println("------"); Serial.print ("Sensor: "); Serial.println(sensor.name); Serial.print ("Driver Ver: "); Serial.println(sensor.version); Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id);

107

Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" xxx"); Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" xxx"); Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" xxx"); Serial.println("------"); Serial.println(""); delay(500); } void setup(void) { Serial.begin(115200); Serial.println("Orientation Sensor Test"); Serial.println(""); /* Initialise the sensor */ if(!bno.begin()) { /* There was a problem detecting the BNO055 ... check your connections */ Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); while(1); }

delay(1000);

/* Display some basic information on this sensor */ displaySensorDetails(); } void loop(void) { /* Get a new sensor event */ sensors_event_t event; bno.getEvent(&event); /* The processing sketch expects data as roll, pitch, heading */ Serial.print(F("Orientation: "));

108

Serial.print((float)event.orientation.x); Serial.print(F(" ")); Serial.print((float)event.orientation.y); Serial.print(F(" ")); Serial.print((float)event.orientation.z); Serial.println(F("")); /* Also send calibration data for each sensor. */ /*uint8_t sys, gyro, accel, mag = 0; bno.getCalibration(&sys, &gyro, &accel, &mag); Serial.print(F("Calibration: ")); Serial.print(sys, DEC); Serial.print(F(" ")); Serial.print(gyro, DEC); Serial.print(F(" ")); Serial.print(accel, DEC); Serial.print(F(" ")); Serial.println(mag, DEC);*/ delay(BNO055_SAMPLERATE_DELAY_MS); }