A Biomimetic Snake-Like Robot Using Bioloids

A Biomimetic Snake-Like Robot using BIOLOIDS

Mark P. Sena1 and Varad Kishore2

University of California Berkeley

1. Bioengineering 2. Electrical Engineering & Computer Sciences

Abstract

The simplicity of the snake body plan and the elegance of snake locomotion have served as a

source of inspiration for artists and engineers for centuries. Despite the lack of limbs, snakes are

capable of navigation in a variety of aquatic, terrestrial, and sub-terrestrial environments. The

redundancy, stability, and adaptability of snake-like motion are highly desirable for robotic

systems, which are typically limited wheeled and sometimes legged propulsion. The overall goal

of this project was to develop a passive-wheeled, autonomous snake-like robot capable of

navigating to a specified target or series of targets in a smooth and efficient manner. We were

successful in building such a robot using an educational Bioloid servo set, demonstrating the

capability of snake-like locomotion, and implementing both motion-planning and real-time

feedback control in order to guide the robot towards a target.

1
 A Biomimetic Snake-Like Robot Using Bioloids

1. Introduction

1.1 Motivation and Inspiration

Flexible robotic manipulators capable of operating in obstacle-filled workspaces, as well as

locomotive robots that can navigate efficiently through tight spaces are in great demand. The

development of a snake-like robot, albeit not a novel undertaking, is significant for its potential

applications in search and rescue, inspection of industrial pipes, and even surgery[1]. Such a

design might enable the robot to navigate through soil, water, the human body, or any region

where climatic or spatial accessibility are limited. Robotic locomotion with motor-controlled

wheels is often taken for granted due to energy efficiency and its straightforward

implementation. However, other means of locomotion warrant study since they surpass this

design in their adaptability to terrain and have implications at a biomechanical level[2]. This

project isolates undulatory serpentine locomotion as its focus in order to create a snake-like robot

using the BIOLOID servo kit.

The fluidic body motion of swimming and crawling creatures such as fish, octopuses,

worms, and snakes has long been a source of biological inspiration for designers of multi-degree-

of-freedom robotic systems. Many of these animals, regardless of their evolutionary diversity,

exhibit similar patterns of locomotion, characterized by an S-shaped body configuration that

undulates over time (serpentine motion)[3]. Indeed it has been found that the wave-like

propagation of muscle contractions during locomotion is governed by a core set of biological

motor-control principles [4, 5]. In many cases of serpentine motion in particular, the

neuromuscular mechanism responsible for locomotion is based on oscillatory firing of neuronal

cell clusters known as Central Pattern Generators (CPGs)[6]. In essence, CPGs generate a

primary control signal that drives undulating body motion. Meanwhile, secondary neuromuscular

2
 A Biomimetic Snake-Like Robot Using Bioloids

control signals (based on high-level volition or proprioceptive feedback[7], for example) produce

small adjustments in body configuration necessary for obstacle avoidance or trajectory steering.

Here, we aim to mimic this dual-level control structure in order to drive and steer a mechanical

snake robot (snakebot) constructed using modular servos from Bioloid.

1.2 Goals and Scope

The overall goal of this project was to develop an autonomous snake-like robot capable of

navigating to a specified target or series of targets in a smooth and efficient manner. In order to

achieve this goal, the problem was decomposed into three general tasks: 1) Assemble a

BIOLOIDS robot and supply a servo-command signal that results in straight snake-like

locomotion. 2) Design and model a motion-planning-based control system that sends the

snakebot along a specified trajectory. 3) Implement real-time image acquisition and feedback

control in order to continuously steer the snakebot towards a single target.

2. Biological Background

2.1 Snake Kinematics

Considering the simplicity of their body plan (essentially a flexible tube), the efficiency and level

of adaptability of snake motion is remarkable. On both rigid and deformable surfaces, in

obstacle-filled environments, within holes and crevasses, and even underwater, snakes employ

modes of locomotion that propel them swiftly for both predation and escape from predators.

Snakes are highly maneuverable owing to their flexible backbone—containing hundreds of

vertebrae connected in series by pivoting ball and socket joints—as well as their complex

musculature.

Snakes employ several modes of body motion for locomotion in different

environments[8, 9]. These are typically categorized as 1) lateral undulation—whereby all parts of

3
 A Biomimetic Snake-Like Robot Using Bioloids

the body move rhythmically as the snake steadily progress; 2) rectilinear crawling—where

ratcheting of the scales produces slow creeping locomotion; 3) concertina-like motion—during

which some portions of the snake body rest bunched up as the rest of the body elongates in one

direction; and 4) sidewiding—where the snake undulates and also lifts its body in order to push

sideways off several contact points. Lateral undulation is not well-suited for confined spaces, but

is efficient in the open and under water. Rectilinear crawling employs fine motor control useful

for moving through narrow spaces. Concertina motion allows snakes to exert high forces at a

stable push-point, and is useful for making incremental progress across slippery surfaces, up

trees, or through tubes.

Different species of snakes employ various (or combinations of) locomotive modes.

Jayne studied the kinematics of snake locomotion within the Colubrid family by digitizing video

of single points on snake body, and measuring the longitudinal, lateral, and average forward

velocities[8]. Nerodia fasciata exhibited the most rapid locomotion (in body lengths (BL) per

second) in both sidewinding (1.4 BL/s) and lateral undulation (1.9 BL/s) modes, in comparison

Elaphe obsolete, Acrochordus javinicus, Cererus rynchops, and Crotalus cerastes (< 1 BL/s).

During lateral undulation, a roughly linear relationship was found between undulation frequency

f (1-2 Hz) and overall velocity V (0.3-2 Hz), with a larger V/f ratio observed with increasing

density of push-points. Not surprisingly, for N. fasciata, concertina motion was found to be

markedly slower (0.05 BL/s) than lateral undulation (1.88 BL/s), sidewinding (1.3 BL/s), and

swimming (2.5 BL/s). The author concluded that there exists an important interplay between

locomotion mode, substrate, and maximum speed, and that it is difficult to generalize kinematic

efficiency based on observation alone.

4
 A Biomimetic Snake-Like Robot Using Bioloids

2.2 Snake Biomechanics

The locomotive mechanics of snakes are fundamentally different from the mechanics of bi- or

quadripeds, where swinging of a limb propels the animal forward. Animals with limbs can be

thought of as inverted pendulums. At low speeds propulsion is dominated by gravity, as the

center of mass periodically rises and falls over the ground contact point, and by friction, as the

limb pushes off against the ground. At high speeds, propulsion is dominated by inertia, as

momentum carries the animal forward. Snakes, on the other hand, are in complete or nearly

complete contact with the ground during locomotion—even at high speeds—so frictional forces

are prevalent.

A useful dimensionless metric* that quantifies the relative importance of inertial and

gravitational/frictional forces is the Froude number. Comparing linear inertia and gravity, Fr =

V2/gL, where V is the characteristic forward velocity, g is the acceleration due to gravity, and L is

the characteristic body length. Comparing undulatory inertia and friction, the Froude number can

be interpreted as Fr = Lf2/µg, where f is the body undulation frequency and µ is the coefficient of

friction between the body and the ground[3]. For bipedal walking, Froude numbers of Fr = 0.5

are typical, while for running Fr > 1[10]. During snake motion, on the other hand, Hu et al.[3]

found frictional forces were an order of magnitude greater than inertial forces (Fr ~ 0.1). Even

for fast-moving species, Fr ~ 1 indicating that snakes “walk” rather than “run” (For N. fasciata,

Fr ~ (0.8 m)(1.5 Hz)2 / (0.2)(9.8m/s2) = 0.9, using body length and undulation frequency values

reported by Jayne and the coefficient of friction µ = 0.2 assumed in the analysis by Hu et al). A

key observation made by Hu et al.[3] was that frictional anisotropy plays a critical role in the

ability of snakes to locomote. Upon measuring the frictional properties of snake underbellies, the

*
Another metric for quantifying mechanical efficiency of motion is the distance traveled per amount of energy used.
Or equivalently, average forward velocity divided by power η=Vfwd/P, where power P is the amount of frictional
work done tangent to the length per unit time P = FfricVtan

5
 A Biomimetic Snake-Like Robot Using Bioloids

authors found large differences between the friction coefficients in the directions tangent (µt =

0.11) and normal (µn = 0.2) to the snake body. On smooth surfaces, however, friction was

approximately isotropic (µ = 0.15), which resulted in an inability of the snake to progress in any

one direction.

2.3 Motor Control Principles

The observation that coordinated motor patterns could be generated independently of

proprioceptive feedback from limbs was first made in arthropods in the 1960s[11, 12]. Since that

time, it has been shown that the complex modes of locomotion observed in many creatures

ranging from cats to crawfish are the result of low-level of small clusters of neuronal cells known

as Central Pattern Generators (CPGs)[13]. The result is often wave-like propagation of muscle

contractions throughout the body. Such behavior was observed in electromyographic studies of

Nerodia fasciata and Elaphe obsolete snake locomotion by Jayne[14, 15], during which the

semispinalis-spinalis, logissimus dorsi, and iliocostalis muscles showed synchronous, unilateral

activity which propogated down the length of the snake body during motion. Similar wave-like

muscle propagation has been observed and modeled by Yekutieli et al in the reaching

movements of octopuses[5, 16].

It was once thought that higher control centers and were necessary for performance of

complicated motor tasks. It is now well known that, in both vertebrates and invertebrates,

rhythmic behaviors such as breathing and locomotion are governed by ‘motor programs’ initiated

by CPGs[4]. A motor program can be defined as “a set of muscle commands which are

structured before a movement begins and which can be sent to the muscle with the correct timing

so that the entire sequence is carried out in the absence of peripheral feedback”[17]. In the

6
 A Biomimetic Snake-Like Robot Using Bioloids

development of a snake-like robot, these ‘motor programs’ are what we attempt to implement for

control of a series of linearly arranged servos.

3. Biomimetic concept and analysis

3.1 Robotic Central Pattern Generator

In general, it is extremely difficult to design a series of command signals that propel a highly

redundant wheeled robot[18]. Mimicking the undulating body motion of a snake, however, is

relatively straightforward. Our intuition initially suggested that undulating snakelike motion

might be generated by providing sinusoidal joint angle control signals to a series of servos—the

sinusoid being phase-lagged along the length of the robot. Indeed after reviewing some literature,

it was found that such a control signal is common among snake-like robot designs, and often

referred to as a “serpentine gait”[2, 19-21]. Actual serpentine body shape and motion has been

characterized by a curvature function κ(s,t) = αcoskπ(s+t) that varies sinusoidally over the snake

length s ϵ [0, L] of the snake and over time t ϵ [0, T][3, 22]. Discretizing the body length into a

finite number of rigid segments, a time-continuous set of signals that embodies serpentine shape

and motion can be represented as

(1)

where ϕ is the joint angle command sent to the servo, i is the index representing the joint

number, ϕmax is the amplitude of the joint angle oscillation, f is the temporal frequency of the

oscillation, and δ is the phase lag between joints. This type of joint angle command signal is

hereon referred to as a robotic Central Pattern Generator (rCPG) owing to its similarity to the

sinusoidal neuronal firing patterns described earlier. An example of outputs from a crayfish CPG

and an actual rCPG used to control our 6-joint snake-like robot is shown in Fig. 1. A phase lag

7
 A Biomimetic Snake-Like Robot Using Bioloids

between joint angles (s1 through s6) generates an instantaneous S-shaped body configuration,

while sinusoidal propagation of this shape propels the snakebot forward.

In the laboratory, the three parameters that define the command signal (ϕmax, f, and δ)

were adjusted until a combination was found that resulted in relatively smooth serpentine

motion. ϕmax was varied between 15o and 17o, f, between 0.1 and 0.5 Hz, and delta between π/10

and π. Quality of motion was evaluated subjectively, based on the apparent amount of lateral

slipping of the snakebot wheels, and on the speed at which the snakebot appeared to move

forward.

3.2 Simulating Snake-like Locomotion

An n-link snake robot has n+2 degrees of freedom. The body configuration is determined by n-1

joint angles, the position of the robot relative to a fixed inertial frame is given by a 2-component

position vector p=(px, py), and the average orientation of the robot is given by a single angle, θ,

measured from the horizontal. However, given a primary command signal ϕi(t), which specifies

the n-1 joint angles, only 3 degrees of freedom remain.

In a simplified simulation, which does not model the ground-contact forces generated at

each link, the system state can be represented by a single link and three variables x = (θ, px, py)

(Fig. 2, left). Forward kinematics can be employed in order to illustrate the position of an

arbitrary series of links controlled by ϕi(t) (Fig. 2, center). Contact forces between the passive

robot wheels and the ground can be neglected if it is assumed that each link precisely follows the

trajectory of link preceding it. This can be expressed as a velocity constraint

(2)

This ‘follow-the-leader’ type behavior is characteristic of snake motion, where frictional

anisotropy in the tangential and perpendicular directions between the snake underbelly and the

8
 A Biomimetic Snake-Like Robot Using Bioloids

ground results in the application of contact forces directed perpendicular to the snake body and

motion tangent to it[3] (Fig. 2, right). If there is sufficient perpendicular friction µt between the

snake belly and the surface, the snake progresses along its body path without lateral slipping.

In order implement this constraint in a ground contact model, evolution of the system state is

carried out in a stepwise manner x(k)
x(k+1),

(3)

where f [x(k)] and u(k) are nonlinear discrete-time state and control functions, respectively.

The state function,

(4)

maintains the previous link orientation θ(k)
θ(k+1), and generates a new link end position

p(k+1) by translating the previous position p(k) along the length of the link by a distance of L,

the link length. The nonlinearity in f [x(k)] arises from the sine and cosine elements within the

rotation matrix Rθ(k).

The input function,

(5)

is composed of a discrete-time version of the sinusoidal driving signal described in the previous

section, along with a signal ϕs(k) that introduces an offset in the link orientation θ(k+1). It is this

offset that allows for active steering of the simulation trajectory towards a desired direction.

Implementation of the simulation and forward kinematics for the animation is described in the

Appendix.

9
 A Biomimetic Snake-Like Robot Using Bioloids

3.3 Controlling Snake-like Locomotion

Two levels of control were implemented for the snake robot (Fig. 3). The first level described

earlier consists of the RCPG that generates the driving command, ϕdrive. Specification of motion

parameters including the joint angle amplitude, ϕmax, oscillation frequency f, and inter-joint phase

lag, δ allows for control of the overall motion of the snakebot. The second level of control

generates the steering command, ϕsteer, which skews the sinusoidal driving command towards the

target though proportional-integral (PI) feedback based on error between the current and desired

snakebot direction.

Combined, these two levels of control generate the joint angle inputs, ϕinput, which are

sent into the ground contact model (and are also stored and sent as a list of commands to the

BIOLOID servos). Both the simulation described here (as well as the real-time feedback system)

utilize the same control structure: a position error vector ∆p is first calculated as the difference

between the target coordinates, p*, and snake head (or center of mass) coordinates p. The

orientation of this vector is defined as the desired direction, θ* (the magnitude ||∆p|| was

originally going to be used as a control variable to slow the snakebot when approaching the

target, however it was found that this was not necessary). The direction error ∆θ is then

calculated as the difference between θ* and the current snake head orientation θ, and is fed into a

PI controller that outputs the steering command ϕsteer.

4. Model for evaluation and manufacture.

4.1 Snake Robot Prototyping Using BIOLOIDS

Six servos were connected in series, the last being connected to a container to hold the controller.

Two independent wheels were attached to each of four locations along the length of the snake.

The wheels were attached by adding a bracket under the tail of the snake holding the controller

10
 A Biomimetic Snake-Like Robot Using Bioloids

and under 3 of the 6 servos. Only 4 pairs of wheels were finally decided to be used since adding

wheels under the other servos resulted in the wheels slipping given the configuration of the snake

and occasionally collisions with the other wheels. The head and tail of the snake were marked

with colored tape for image segmentation. A photograph of the assembled snakebot is shown in

Fig. 4. For image acquisition and registration of the robot position, a Logitech QuickCam was

mounted on a contraption of the retort stands, such that the overhead view of the camera was

parallel to the ground.

4.2 Motion Planning-Based Navigation

Motion planning for redundant nonholonomic systems involving rolling contact is, in general, an

extremely challenging problem and an active area of research which we will discuss briefly later.

In the case of the snakebot, “motion planning” consists of simulating trajectories to a virtual

target using the techniques described earlier. An experiment begins by initializing video capture

and commanding the snakebot (and simulation) to go to an initial configuration ϕ(0) (Fig. 5).

Color segmentation allows for matching of the robot and simulation position and orientations

(registration). If the target is not within range of the user-defined threshold, a simulation is run in

order to generate the time-series of joint angle commands that get the snakebot from its current

position to the target position. These commands are subsequently sent to the BIOLOIDS robot.

This process flow structure was initially created so that robot progress towards the target

could be monitored and motion re-planned at a frequency determined by the user. However, due

to the length of time needed to capture images and simulate motion from the current position to

the target, time-series joint angle commands were generated on a target by target basis in

practice. Using this approach, command sets could be used to smoothly steer the robot towards

11
 A Biomimetic Snake-Like Robot Using Bioloids

the specified target, with the robot stopping only at the end of the command string (indicating

either that the allotted time ran out or that the target was reached).

4.3 Video-Tracking for Real-Time Feedback Control

In order to develop a guidance system to steer the snake towards a target selected by the user, we

employed vision tracking to obtain the center of mass and direction of the snake at any particular

frame. Once obtained, the current direction of the snake is compared with the ideal direction

required for the snake to reach its target. This difference in direction is then scaled and the result

(steerAngle) is applied to the sinusoidal commands to the snake. In order to obtain the direction

of the snakebot we decided to resolve the position of the head and the tail of snake separately.

The direction of the snake would then be estimated to be the vector from the tail’s center of mass

to the head’s center of mass. In order to get each of these we decided to use a combination of

regular Background Subtraction with Color Segmentation.

First, the user is asked to select a region in the colored parts of the head and then the tail.

The average colors in each of these regions is computed in order to create color-markers for the

head and the tail of the snake. When the snake is in motion, a frame from the webcam is

captured. Note that every time a frame was captured and processed the snake would stall, which

comprised its smooth motion otherwise. To reduce this effect, image processing was used only in

an interval of about 3 to 5 steps. After background image subtraction the image is converted

from RGB to the L*a*b* Color Space. The L*a*b* Color Space disintegrates the color into a

measure of L, for luminosity, and ‘a’ and ‘b’ which are nonlinear CIE XYZ based coordinates.

This color space was chosen because if we discard Luminosity with comparing colors, we won’t

have to contemplate them not matching in the case where there is a shadow. Application of a

threshold to the a* and b* attributes of the frame generates 2 separate masks for the head and one

12
 A Biomimetic Snake-Like Robot Using Bioloids

for the tail. Lastly, the center of mass of each segmented head and tail mask is calculated. The

orientation is represented as the direction of the vector pointing from the tail to the head and the

position is represented as the midpoint between the head point and the tail point.

5. Simulation and Experimental Results

5.1 Snake-Like Locomotion

Before any sort of control scheme was implemented, a variety of snake motion parameters were

tested in order to determine which resulted in the most snake-like motion. Both the smoothness

of motion and the degree of lateral wheel slipping were evaluated subjectively by observation

and video.

The smoothness of motion was mostly influenced by the time step length ∆t, oscillation

frequency f, and BIOLOID servo speed. If the time steps were too long, oscillation frequency too

low, or servo speed too fast, the robot motion from step to step was jerky. On the other hand, the

reverse resulted in incomplete undulation cycles because commanded joint angles could not be

fully reached by the time the next set of commands was received.

It was found that the degree of lateral wheel slipping was most strongly influenced by the

joint angle amplitude ϕmax , and phase lag δ. This observation agrees with simulation results

reported by Saito et al. [2] which also suggest that the optimality of these parameters is highly

dependent on the amount of friction between the robot and the surface. The difference in quality

of robot locomotion with respect to δ is illustrated in Fig. 6.

A value of δ=π/4 resulted in the best forward snakebot motion (Fig. 7, left), while a value

of δ=π/8 did not propel the robot forward at all (Fig. 7, right). The set of parameters, found by

trial and error, that generated the ‘best’ qualitative snake-like motion were ∆t = 0.2s, δ = π/4,

13
 A Biomimetic Snake-Like Robot Using Bioloids

ϕmax = 55o, f =0.25 (Hz), servo speed = 200 (a.u.). These parameters were used (with slight

modification depending on the demonstration surface) for further experiments.

5.2 Simulation and Model Calibration

Employing a proportional control scheme, attainment of multiple, randomly placed virtual

targets was demonstrated. An example of the output following one of these simulations is shown

in Fig. 7. Spikes in the position error represent the addition of a new target (7 targets total) after

the previous one has been reached.

In order to account for difference in performance between the simulation and the actual

robot, two correction factors were introduced. A slip parameter was employed to artificially

hinder the motion along the length of the simulated snake body by a factor of s. Trajectories for

s-values of 0, 0.3, and 0.6 are shown below (Fig. 8, left). The respective times-to-target were 49,

60, and 91, indicating a longer travel time needed for larger slip factors. The number of

undulations for a given path length also increased with larger slip factors. A turning offset ϕoff

was also added to the joint angle command signal in order to correct for robot drift towards the

left or the right. Trajectories for values ranging from 0o to 3o are shown below (Fig. 10, right).

Close matching between simulation and robot motion was a crucial factor influencing motion

planning accuracy. In practice, a slip value of s=0.4 and a turning offset ϕoff = -1.5 were found to

best reconcile simulation and snakebot motion.

5.3 Motion Planning

After determining an appropriate control scheme and calibrating the simulation correction factors

in order to match true system behavior, the motion planning-based experiments following the

process flow diagram shown in figure 5 were demonstrated. In the example below, two targets

(red x’s) were defined by the user. The true, registered positions of the snake robot in its initial

14
 A Biomimetic Snake-Like Robot Using Bioloids

state, and after attainment of both targets, as well as an image frame of the final position of the

snakebot are shown below (Fig. 9). A video of the demonstration is located here

([Link]

Direction error (Fig. 10, left) and position error (Fig. 10, right) trajectories demonstrated

successful reaching of two subsequent target positions by simulation. The same joint angle

commands used to drive and steer the simulation were used to drive and steer the robot to the

final configurations shown in Fig. 9.

Motion planning-based control was demonstrated successfully several times, however, robot and

simulation parameters had to be carefully calibrated in order to ensure that the motion generated

between the two was equivalent. In order to eliminate this difficulty and to remove dependence

of control commands on a simulation, real-time feedback was employed next.

5.4 Feedback Control

For simulation of snake locomotion towards a target, pure integral control, proportional control,

and a combination of the two were evaluated. Factors that influenced which control strategy was

ultimately used included the time to direction set-point, time to target, and smoothness of

simulated motion.

Pure integral control (Fig. 11, top) resulted in rapid acquisition (~3 sec) of the desired

direction due to the large magnitude of the steering signal (green trace). However, as is

characteristic of pure integral control, significant target overshoot was observed (blue trace),

which resulted in undesirable overcorrection about the desired direction (red trace). Under pure

proportional control (Fig. 11, bottom) the simulation achieved the desired direction more slowly

(~7 sec) in comparison to integral control, however the result was much more stable with no

observed overshoot.

15
 A Biomimetic Snake-Like Robot Using Bioloids

A combination of proportional and integral control resulted in both rapid acquisition of

the desired direction, as well as minimal overshoot (result not shown). However, in practice it

was found that rapid correction of the direction did not actually produce the smoothest robot

motion. Snakebot locomotion was best under gradual steering, which better preserved the basal

sinusoidal driving signal. Therefore, a pure proportional control strategy was ultimately chosen.

Residual error was not a problem because directional drift was corrected for separately.

The first test documented here shows the snakebot being given a target within the frame

northwest of its initial configuration. Fig. 12, left shows the ‘position error’ which is the distance

from the snakebot’s center to its target. Here we see that it starts at a distance of about 50cm

from the target and after 11 time-steps it is in an acceptable range to be considered as having

reached its target. Fig. 12, right depicts the direction error, which is the difference in angle

between the snakebot’s current bearing and the ideal bearing from the snakebot’s tail directly to

the target. As we can see the difference in direction wavers in an almost sinusoidal fashion since

the snakebot bearing is not necessarily the same as the tail-to-head vector.

Fig. 13 demonstrates the effectiveness and accuracy of the targeting system. The left image

shows the target selected by the user, inicated by a red dot plotted at the selected target position.

The right figure plots out the path of the center of mass of the head and tail of the snakebot

relative to the origin of snakebot. When we are converting from pixels to centimeters, the origin

is taken as the tail of first frame captured. Following is a filmstrip of some of the frames captured

and processed whilst the snakebot was steering towards its target.

Looking at any one of the image frames in Fig. 14, one may note that on the head of the

snakebot a blue circle has been plotted and on the tail a green circle has been plotted. The

positions of these two points were obtained from the Color Segmentation for each the head and

16
 A Biomimetic Snake-Like Robot Using Bioloids

the tail. As we can see above, they coincide with the actual position of the head and tail of the

snakebot in the frames. This demonstrates the effectiveness of the method getImageDetails

which performs this vital function of image capture and processing. The function is called within

the control loop every nSteps, which nSteps is about 3-5. As mentioned earlier, real-time image

processing at every time step would be much too expensive such that it would hinder the motion

of the snakebot. Here even though it does stall every nSteps, in that period it has gathered enough

momentum to move forward, before it stops to correct its path.

For the second test, a target was chosen north-east of the initial snakebot configuration.

Fig. 15, right displays the trajectories of the center of mass of the head and the tail relative to the

origin which was initially taken. As we can note, the figure corroborates the intended motion of

the snakebot in reaching the target north-east of its origin. Below displayed are the direction

error and the steer-angle recorded over time (x-axis).

In Fig. 16 we see that the direction while oscillating is initially far from the ideal

direction (an error of about -50°). However, as time passes the controller corrects this and it

approaches 0 error. Note that there is a large fluctuation of direction when it is very near the end.

This is justified since it ‘withers about’ when its near the end so that the center of mass reaches

the target, hence causing the fluctuation in direction. The figure on the right is steering angle

applies to the motion of the snakebot, in order to correct for bad direction. We note that as the

robot reaches its target, the required steering angle drops from about 6 to 0, notwithstanding the

fluctuation. This oscillation also correspondes to that of the direction error, showing that when

the direction error increases (eg: t=1 to t=2, from -40 to -70), the steering angle increases as well

to compensate (t=1 to t=2, from 4 to 7). In this manner the steering of the snakebot attempts to

counter the directional error of the snakebot at every time step. Demo 2 to steer the robot towards

17
 A Biomimetic Snake-Like Robot Using Bioloids

a north east target was successful. The figure below shows the position error dropping from ~60

to 0 over the 13 image-captures and time-steps in between, taken for the robot to reach its target.

6. Benchmarks and Limitations

6.1 Robotic Central Pattern Generator

We were successful in constructing a BIOLOIDS robot that was capable of snake-like

locomotion. Undulating body motion was easily achieved by supplying the phase-lagged

sinusoidal joint angle control signals from the RCPG to each of the servos, however, without

wheels it was difficult to achieve any noticeable propulsion along either smooth or rough

surfaces. This makes sense since the basis of serpentine propulsion is anisotropic friction

between the normal and tangential directions.

After the addition of independent wheels it was immediately clear that snake-like

propulsion could be achieved. Interestingly, passive wheels act similarly to the underbelly of a

snake. Both allow for almost frictionless motion in the forward and direction and at the same

time, resist lateral slipping. It is worth noting that single-axel wheel pairs were not successful,

because they did not allow for pivoting of individual robot links. A potential addition that might

increase locomotion efficiency could be a ratcheting wheel that resists backwards motion much

like snake skin, although this is still very different from an active wheel that generates torque.

Trial and error allowed us to determine the oscillation amplitude, frequency and phase lag

parameters that resulted in the most efficient motion. This approach for achieving optimal

snakebot propulsion is a limitation, however. The parameters that result in the ‘best’ motion are

somewhat variable and are highly sensitive to factors such as the coefficient of friction between

the snakebot wheels and the ground, snakebot weight distribution, and desired speed. A strategy

to dynamically control these parameters for optimal propulsion would be desirable.

18
 A Biomimetic Snake-Like Robot Using Bioloids

6.2 Simulation and Motion Planning

We were successful in designing and modeling a motion-planning and control system that was

capable of steering the snake along a specified trajectory. Simulation of snake motion was

extremely useful for verifying appropriate behavior of the BIOLOIDS snakebot. Incorporation of

a ground contact model resulted in fairly realistic snake motion. Furthermore, the addition of

correction factors to the simulation made it possible to account for the degree of slip and turning

offset exhibited by the snakebot. Simulations also allowed for rapid evaluation of different

control strategies and parameters that would have been difficult to determine empirically.

Simulations were algorithmic in nature, only valid for step sizes equal to the length of a link, and

did not model frictional forces or joint torques. However, due to the simplicity of the single-link,

discrete-time model, simulations were extremely rapid and were sufficient for approximating

snakebot motion.

The problem of nonholonomic motion planning arises in determining whether or not a

path x(t) that connects two system states xo and xf and that satisfies a set of nonholonomic

constraints (e.g. eq. 2 for no sideways sliding) actually exists. As mentioned earlier, an n-link

snake robot has n+2 degrees of freedom, however only n-1 of them (joint angles) are directly

controllable. Some system states (position and orientation) may not be reachable from a given

initial configuration, so it may be difficult if not impossible to construct a path to a desired

target. Techniques used to approach this problem include optimal control, piecewise constant

inputs, and canonical paths[18]. The approach we took of employing a family of sinusoidal joint

angle control signals (RCPGs) as a primary (driving) control signal and adding a secondary

(steering) signal is most closely related to the canonical path method. A major limitation to the

motion-planning approach was the need to carefully calibrate the correction factors in order to

19
 A Biomimetic Snake-Like Robot Using Bioloids

match simulation and snakebot behavior. Mismatching typically resulted in poor target

acquisition. An improvement might incorporate both motion planning and real-time feedback

control.

6.3 Image segmentation and real-time feedback

Lastly, we were successful in achieving real-time image acquisition and feedback control in

order to dynamically steer the snakebot towards a specified target. The results obtained from the

feedback control system as outlined in the previous section were consistent and corresponded

with the predictions given the boundaries of our experimental setup. However, the limitations of

our feedback control system were equally significant in comparison to the operational success.

One of the first problems we faced was that the process time of grabbing an image from

the webcam, and performing the necessary background subtraction and color segmentation, was

long enough that it would stall the fluid motion of the robot, causing a loss of momentum. To

address this we first timed the various processes within the feedback control function

getImageDetails in order to bypass or optimize the most expensive of these processes. Despite

this, performing image processing at every time step entirely compromised the motion of the

snake. Therefore we chose to perform feedback control only every nSteps on continuous robotic,

a value which was chosen as 3. This improved the motion of the robot substantially.

Another issue commonplace to image processing was the situation where the frame had

noise comparable to the colors of either the head or tail. This resulted in erroneous estimations of

the center of masses of the head and tail of the snake. While unresolved we decided to confront

this issue by choosing a surface of a color in contract with those of the head and tail.

The most significant of limitations for the feedback control system was the limited

viewing range of webcam. This meant that the snake could be controlled in a very small frame.

20
 A Biomimetic Snake-Like Robot Using Bioloids

We initially attempted to solve this by attaching the webcam higher up with 2 retort stands

combined. However this greater areal view resulted in a loss of resolution from the camera,

causing the head and tail markers of the snake being eroded away during image processing. A

possible solution to this issue was proposed as having multiple overlapping webcams in order to

cover a greater area. If scope of the project were extended, this would be an appropriate choice in

order to effectively develop and test the navigation system of the snake.

7. Conclusions and future work

The simplicity of the snake body plan and the elegance of snake locomotion have served as a

source of inspiration for artists and engineers for centuries. Despite the lack of limbs, snakes are

capable of navigation in a variety of aquatic, terrestrial, and sub-terrestrial environments. The

redundancy, stability, and adaptability of snake-like motion are highly desirable for robotic

systems used in hazardous or difficult-to-navigate environments[9]. In the same way that a

redundant manipulator is capable of achieving the same task in many different configurations, a

redundant snake-like robot with many degrees of freedom can reach a prescribed endpoint by

taking a variety of different locomotive paths. Operating at ground level and in close body

contact with a surface, a snake-like robot’s configuration would be highly stable relative to that

of an elevated bipedal robot for example. Lastly, owing to an even body weight distribution, a

snake-like robot can exert traction along its entire length, rather than at limb contact points alone.

As a result, propulsion can be achieved on soft or deformable surfaces such as sand, where

wheels or a limb would get stuck or perform more soil work than locomotive work.

Future improvements to the current snakebot might be achieved by incorporating tactile

feedback into motor control algorithm. Such feedback could better regulate the timing and

rhythm of locomotive motor patterns[7] and serve as a means to steer the robot without

21
 A Biomimetic Snake-Like Robot Using Bioloids

information from an external camera system. Parameters of the rCPG could thus be modified

dynamically in order to adjust for differences in terrain type, or in order to switch locomotive

modes, for example. Another improvement might be achieved by cleverly redistributing the

weight of the snakebot in order to maximize forward-propelling surface contacts. In their study

of snake locomotion, Hu et al.[3] noticed that at high speeds, snakes lifted the curved portions of

their body off the ground, resulting in a more stable and efficient pushpoint. This might be

implemented using a more advanced snakebot design allows for bending about horizontal axes

(for body lifting) rather than vertical axes (for lateral undulation) alone.

Overall, this was a successful project accomplished through an introductory robotics

course and biomimetic engineering course at the University of California Berkeley.

Neuromuscular and biomechanical analyses were crucial in developing simple and easy-to-

implement locomotive strategies for the snakebot. Robotics concepts including forward

kinematics, feedback control, and object tracking were necessary for design and development of

the snakebot. We would like to thank Professor Bajcsy and Professor Dharan for organizing the

robotics and biomimetics courses, respectively, and would like to acknowledge the GSIs Ram,

Humberto, and Yasemin for their advice and support.

22
 A Biomimetic Snake-Like Robot Using Bioloids

8. References

1. Sensor Based Planning Lab, C.M.U. Snake Robots. 2007 [cited 2010 12/14/2010]; Available from:
[Link]
2. Saito, M., M. Fukaya, and T. Iwasaki, Serpentine locomotion with robotic snakes. Ieee Control Systems
Magazine, 2002. 22(1): p. 64-81.
3. Hu, D.L., et al., The mechanics of slithering locomotion. Proceedings of the National Academy of Sciences
of the United States of America, 2009. 106(25): p. 10081-10085.
4. Pearson, K.G., Common Principles of Motor Control in Vertebrates and Invertebrates. Annual Review of
Neuroscience, 1993. 16: p. 265-297.
5. Yekutieli, Y., et al., Dynamic model of the octopus arm. II. Control of reaching movements. Journal of
Neurophysiology, 2005. 94(2): p. 1459-1468.
6. Puhl, J.G. and K.A. Mesce, Keeping It Together: Mechanisms of Intersegmental Coordination for a
Flexible Locomotor Behavior. Journal of Neuroscience, 2010. 30(6): p. 2373-2383.
7. Pearson, K.G., Proprioceptive regulation of locomotion. Current Opinion in Neurobiology, 1995. 5(6): p.
786-791.
8. Jayne, B.C., Kinematics of Terrestrial Snake Locomotion. Copeia, 1986(4): p. 915-927.
9. Dowling, K., Limbless Locomotion: Learning to Crawl with a Snake Robot, in Robotics. 1997, Carnegie
Mellon University: Pittsburgh, PA.
10. Christopher, L.V. and J.O.M. Mark, Froude and the contribution of naval architecture to our
understanding of bipedal locomotion. Gait & posture, 2005. 21(3): p. 350-362.
11. Hughes, G.M. and C.A.G. Wiersma, The Co-Ordination of Swimmeret Movements in the Crayfish,
Procambarus-Clarkii (Girard). Journal of Experimental Biology, 1960. 37(4): p. 657-&.
12. Wilson, D.M., Central Nervous Control of Flight in a Locust. Journal of Experimental Biology, 1961.
38(2): p. 471-&.
13. Mulloney, B. and C. Smarandache, Fifty years of CPGs: two neuroethological papers that shaped the
course of neuroscience. Frontiers in Behavioral Neuroscience. 4: p. 12.
14. Jayne, B.C., Muscular Mechanisms of Snake Locomotion - an Electromyographic Study of the Sidewinding
and Concertina Modes of Crotalus-Cerastes, Nerodia-Fasciata and Elaphe-Obsoleta. Journal of
Experimental Biology, 1988. 140: p. 1-33.
15. Jayne, B.C., Muscular Mechanisms of Snake Locomotion - an Electromyographic Study of Lateral
Undulation of the Florida Banded Water Snake (Nerodia-Fasciata) and the Yellow Rat Snake (Elaphe-
Obsoleta). Journal of Morphology, 1988. 197(2): p. 159-181.
16. Yekutieli, Y., et al., Dynamic model of the octopus arm. I. Biomechanics of the octopus reaching
movement. Journal of Neurophysiology, 2005. 94(2): p. 1443-1458.
17. Marsden, C.D., J.C. Rothwell, and B.L. Day, The Use of Peripheral Feedback in the Control of Movement.
Trends in Neurosciences, 1984. 7(7): p. 253-257.
18. Murray, R.M., Z. Li, and S.S. Sastry, A Mathematical Introduction to Robotic Manipulation. 1994, Boca
Raton: CRC Press LLC.
19. Wu, X.D. and S.G. Ma, CPG-based control of serpentine locomotion of a snake-like robot. Mechatronics,
2010. 20(2): p. 326-334.
20. Mehta, V., S. Brennan, and F. Gandhi, Experimentally verified optimal serpentine gait and
hyperredundancy of a rigrid-link snake robot. Ieee Transactions on Robotics, 2008. 24(2): p. 348-360.
21. Hasanzadeh, S. and A.A. Tootoonchi, Ground adaptive and optimized locomotion of snake robot moving
with a novel gait. Autonomous Robots, 2010. 28(4): p. 457-470.
22. Hirose, S. and H. Yamada, Snake-Like Robots Machine Design of Biologically Inspired Robots. Ieee
Robotics & Automation Magazine, 2009. 16(1): p. 88-98.

23
 A Biomimetic Snake-Like Robot Using Bioloids

9. Appendices
SELECTED MATLAB CODES
SimpleSnake.m—Simplified one-link simulation for generating serpentine trajectories
This core of the simulation was implemented using the following few lines of code
for k = 1:nSteps
...
phi(k) = phiMax*sin(kLag*k) + steerAngle;
theta(k+1) = theta(k) + phi(k);
p(:,k+1) = R(theta(k+1))*[L*(1-slip) 0]' + p(:,k);
end

where the steering command steerAngle was generated separately by the snakeControl()
function.

snakeControl.m—generates the steering command for the simulation and snakebot

function [pos_err, direction_des, steerAngle] = snakeControl(target, CM, direction,
kSteer, steerMax)
% snakeControl calculates the position and direction error based on the
% current target position, snake CM position and snake direction.
% it outputs the steering angle control variable, as well as the po

% direction error
direction_des = 180/pi*atan2(target(2)-CM(2), target(1)-CM(1)); % desired
direction
temp = direction_des - direction;
dir_err = mod(temp+180,360)-180; % maps to [-180 180]

% position error
pos_err = norm(target - CM);

% proportional control signal

steerAngle = kSteer*dir_err;
% set cap on maximum allowable steerAngle
if steerAngle > steerMax, steerAngle = steerMax;
elseif steerAngle < -steerMax, steerAngle = -steerMax;
end

g0n.m—homogeneous transformation function for snake animation

Forward kinematics needed for animation of the snake body were implemented in a manner similar to the
pantograph animation, but for an arbitrary number of links and employing homogeneous coordinates
function x0 = g0n(xn, n, phi)
...
while n > 0,
g = [Rz(phi(n)), -Rz(phi(n))*[-L,0,0]’;...
0, 0, 0, 1];
xn = g*xn;
n = n-1;
end
x0 = xn;

where xn is the point of interest to be transformed expressed in the nth link’s coordinate frame, phi is an
nx1 vector of joint angles, Rz() is simply a rotation matrix function, and x0 is the point of interest
expressed in the reference coordinate frame.
Relevant parameters for the simulation were defined as follows

24
 A Biomimetic Snake-Like Robot Using Bioloids

% =============PARAMETERS THAT DEFINE SERPENTINE MOTION====================

kLag = pi/8; % phase lag between links (rad)
phiMax_deg = 25; % maximum joint angle (deg)
phiMax = phiMax_deg*pi/180;
f = 1/8; % oscillation frequency (Hz)

% for simulation
nSteps = 200; % number of simulation steps
L = 1; % fixed 1-unit link(step) size
slip = 0.; % slip factor
phi_off = 0; % 1*pi/180;

%=========DETERMINED PARAMETERS THAT DEFINE TIME EVOLUTION=================

dt = kLag/(2*pi*f); % time step
tanSpeed = L*(1-slip)/dt; % tangential snake speed (links/sec)
tspan = 0:dt:dt*nSteps; % time vector
% ====================CONTROL PARAMETERS ================================
global controlOn
controlOn = 1;
phiSteer = zeros(1,nSteps+1); % steering angle
kSteer = 0.05; % proportional control factor
steerMax_deg = 20; % maximum allowed steering angle
noise = 0*(rand(1,nSteps)-0.5*ones(1,nSteps));
% target = 50*(rand(2,1)-[0.5;0.5]);
target = [20; -40];
posThresh = 3; % position threshhold for goal reaching

drawSnake.m—implements forward kinematics to illustrate/animate snake motion

function [q1, CM, headCM, tailCM] = DrawSnake_final2(phi, theta1, p, drawOn, res, window)
% DrawSnake draws the n-jointed snake based on joint angles and outputs the
% position of the 1st joint and the snake center of mass
% MSena 11/28/10

% drawing parameters
a = 3/res; % aspect ratio of links
n = length(phi)-1; % number of joints (or links - 1)
q = zeros(3,n+1); % global origin and joint positions
L = (6)/res; % illustrated length of segement **input (physical dimension of
robot link)**
W = L/a; % illustrated Width of segment

% create x and y coordinates of rectangular link vertices

r = zeros(3,4,n+1);
r0 = zeros(size(r));
for j = 1:n+1
% the origin of each frame is located at the positive end
r(:,:,j) = [0 -L -L 0;... % x for vertices
-W/2 -W/2 W/2 W/2;... % y for vertices
0 0 0 0]; % z for vertices
q(:,j) = [-L;0;0]; % position of each joint
end

% rotate 1st segment (eliminate 1 DOF)

for i = 1:size(r,2) % for each vertex i
r0(:,i,1) = g0n(r(:,i,1), 1, theta1, q(:,1), 1);
end
% transform 2nd through (n+1)th segments according to phi
for j = 1:n
% Transform points to the inertial reference frame (might not work)
for i = 1:size(r,2) % for each vertex i
temp = g0n(r(:,i,j+1), j, phi, q(:,j+1), 1);
r0(:,i,j+1) = g0n(temp, 1, theta1, q(:,1),1);
end
end

%=======================SHIFT ALL COORDINATES===================

%(eliminate last 2 DOFs))
for j = 1:n+1

25
 A Biomimetic Snake-Like Robot Using Bioloids

for i = 1:size(r0,2) % for each vertex i

r0(:,i,j) = g0n(r0(:,i,j), 1, 0, -p, 1);
end
end

% =============CENTERS OF MASS=================
% function that finds the center of mass for a link or set of links
% size(ri,3) is the number of segments to center
CMfun = @(rr) 1/(size(rr,3)*size(rr,2))*...
[sum(sum(rr(1,:,:))), sum(sum(rr(2,:,:)))];

% CENTER OF MASS coordinates (including tail)

CM = CMfun(r0);

% head and tail CM coordinates

headCM = CMfun(r0(:,:,n+1));
tailCM = CMfun(r0(:,:,1));

% 1st joint coordinates after transformation

q1 = [(r0(1,1,1) + r0(1,4,1))/2,...
(r0(2,1,1) + r0(2,4,1))/2,...
0]';

%==========DRAWING====================
...

centerOfMass.m—locates the center of mass of a binary mask

function cmass = centerOfMass(I)
[X, Y] = find(I);
cmass = [round(mean(X)), round(mean(Y))];

getImageDetails.m—segments the head and tail positions from an image

function [headCM, tailCM, origin, frame] = getImageDetails( videoObj, background, cform,
color_markers, tail0, pixelsPerRobotLength)
%GETIMAGEDETAILS gets the center of mass of the head and tail
%from a single fram grab. The video object passed in should be
%be set to 'inf' frames-per-trigger, and should be 'started'.
%cform and color_markers are outputs of initializeVideoCapture
% THE headCM and tailCM returned are in centimeters from the origin.

frame = getsnapshot(videoObj);
%% Background Subraction
threshold = 5;
mask = uint8(abs(rgb2gray(frame)-rgb2gray(background))<threshold);
overlay = zeros(size(frame));
overlay(:,:,1) = mask.*frame(:,:,1);
overlay(:,:,2) = mask.*frame(:,:,2);
overlay(:,:,3) = mask.*frame(:,:,3);
frame = uint8(overlay);

%% Color Segmentation in L*a*b* Color-Space

lab_frame = applycform(frame,cform);
L = double(lab_frame(:,:,1));
a = double(lab_frame(:,:,2));
b = double(lab_frame(:,:,3));

distance = repmat(0,[size(a), 2]);

for count = 1:2
distance(:,:,count) = ( (a - color_markers(count,2)).^2 + ...
(b - color_markers(count,3)).^2 ).^0.5;
end

headMask = imdilate(imerode(distance(:,:,1)<threshold, ones(2,2)), ones(7,7));

tailMask = imdilate(imerode(distance(:,:,2)<threshold, ones(2,2)), ones(7,7));

26
 A Biomimetic Snake-Like Robot Using Bioloids

headCM = centerOfMass(headMask);
tailCM = centerOfMass(tailMask);

if isempty(tail0)
origin = tailCM;
tailCM = [0, 0];
headCM = [headCM(1)-origin(1), headCM(2)-origin(2)].*(51.4/pixelsPerRobotLength);
else
origin = tail0;
tailCM = [tailCM(1)-origin(1), tailCM(2)-origin(2)].*(51.4/pixelsPerRobotLength);
headCM = [headCM(1)-origin(1), headCM(2)-origin(2)].*(51.4/pixelsPerRobotLength);
end

BIOLOID_guidanceSystem.m—performs real-time feedback control for snakebot

clear all;
delete(instrfind)
imaqreset
s = serial('COM1','BaudRate',57600);
fopen(s);

ids1 = [2 17 10];
ids2 = [15 6 8];
posIni = [0 0 0];
speed = 150*[1 1 1 1 1];
% l1 = 1;
% l2 = 1;
% l3 = 1;

% First we move the robot to an initial position

cmd_SyncMove(s,ids1,posIni,speed);
cmd_SyncMove(s,ids2,posIni,speed); pause(2);

% sinusoidal motion
% linspace(0,20,100) original tspan
nSteps = 5;
tEnd = 20;
t0 = linspace(0,tEnd,100); %time
dt = t0(2) - t0(1);
t = 0:nSteps*dt:tEnd;

% snake parameters
N = 6; % no. segments
a = 5; % aspect ratio (length/width)
% movement parameters
p = pi/4; % phase lag
A = 40; % amplitude
f = .25; % frequency
of = 0; % offset

%=================VIDEO SETUP====================
% setup video capture of background
[color_markers, cform, vidobj, background] = initializeVideoCapture;
robotLength = 51.4; %centimeters
cmd_SyncMove(s,ids1,[0 0 0],speed);
cmd_SyncMove(s,ids2,[0 0 0],speed);
disp('Place the straight Snake in the frame, and Hit enter')
preview(vidobj)
pause
closepreview
disp('Please select a line along the length of the straight Snake (Left-click to start
line; Right-click to end line)')
flushdata(vidobj);
img = getdata(vidobj,1);
imagesc(img)
[iIm,jIm] = getline;
pixelLength = ((iIm(2)-iIm(1))^2 + (jIm(2)-jIm(1))^2)^0.5;
tail0 = [];

27
 A Biomimetic Snake-Like Robot Using Bioloids

%%
%============DEFINE TARGET====================
targetPixels = [];
targetCm = [];

disp('Place snake in Starting position & Enter to select a target.')

preview(vidobj)
pause
closepreview
flushdata(vidobj);
img = getdata(vidobj,1);
figure(1), imshow(img);
targetPixels = ginput;
targetPixels = [targetPixels(2), targetPixels(1)];
disp('click on target')

%% ============================================

%===========DEFINE CONTROL PARAMETERS=======

kStr = 0.1; % steering proportional control coefficient
%nSteps = 1; % number of snake steps between vid capture/control commands
% factor of 100
steerMax = 5;
pos_thresh = 5; % threshhold for target reaching
%============================================
disp('Processing will now begin. Please hold on.')
preview(vidobj)
% Now we record the motion to a final position
% initialize storage variables
frame_save = zeros(size(background, 1), size(background, 2), size(background, 3),
length(t));
dir_err_save = zeros(length(t),1);
pos_err_save = dir_err_save;
steerAngle_save = dir_err_save;
headCM_save = zeros(length(t),2);
tailCM_save = zeros(length(t),2);

for i = 1:length(t)
% outer control loop
[headCM, tailCM, origin, frame] = getImageDetails( vidobj, background, cform,
color_markers, tail0, pixelLength);
frame_save(:,:,:,i) = frame;
headCM_save(i,:) = headCM;
tailCM_save(i,:) = tailCM;
tail0 = origin;

% convert image details to inputs for snakeControl

CM = mean([headCM;tailCM],1);
direction = 180/pi*atan2(headCM(2)-tailCM(2), headCM(1)-tailCM(1));
% check for authenticity of target
if isempty(targetCm)
targetCm = [targetPixels(1)-origin(1), targetPixels(2)-
origin(2)].*(51.4/pixelLength);
end
% plug inputs into control function
[dir_err, pos_err, steerAngle] = snakeControl(targetCm, CM, direction, kStr);
dir_err_save(i) = dir_err;
pos_err_save(i) = pos_err;
steerAngle_save(i) = steerAngle;

% set cap on maximum allowable steerAngle

if steerAngle > steerMax, steerAngle = steerMax;
elseif steerAngle < -steerMax, steerAngle = -steerMax;
end

if pos_err < pos_thresh

disp('got it')
break
end

28
 A Biomimetic Snake-Like Robot Using Bioloids

for k = 1:nSteps
% generate joint angle command
phi = moveSnake2(t(i)+(k-1)*dt, N, a, p, A, f, of);
% add (or subtract) control signal
phi = phi + steerAngle*ones(size(phi));

% send commands to robot

cmd_SyncMove(s,ids1,phi(1:3),speed);
cmd_SyncMove(s,ids2,phi(4:6),speed); %pause(dt/2);
end
end

%% Closing serial object

closepreview
stop(vidobj);
fclose(s);

%% plotting
figure(10), clf, plot(t, dir_err_save);
figure(20), clf, plot(t, pos_err_save);
figure(30), clf, plot(t, steerAngle_save);
figure(40), clf, plot(t, headCM_save), hold on
plot(t, tailCM_save), hold off
%%
figure(50), clf, plot(headCM_save(1:11,1), headCM_save(1:11,2),'ob-'), hold on,
plot(tailCM_save(1:11,1), tailCM_save(1:11,2),'og-')
legend('head', 'tail')

29
 A Biomimetic Snake-Like Robot Using Bioloids

10. Figures

400
s1

servo angle commands

300 s2
s3
200 s4
s5
100 s6

-100
0 1 2 3 4 5
time (sec)
Figure 1: Coordinated motor output in swimmeret motor nerves recorded from isolated abdominal nerve cords of
crayfish[13] (left). Powerstroke muscles are labeled PS2-PS5. Actual command signal used to drive a 6-servo
BIOLOIDS snakebot (right). Servos are labeled S1-S6.

θ(k+1)
p(k+1)

p(k) θ(k)

Figure 2: Single-link model of the snakebot used for simulations (left). p and θ are the position and orientation
system states, respectively. Example frame from the animated snakebot simulation (center), where the registered
head, tail, and target positions are labeled by blue, green, and red circles, respectively. Schematic of a typical
serpentine body plan and path coordinates (right) [3]. µ represents the frictional coefficient in the normal n and
tangential s directions.

φ1(1), φ2(1) … φN(1)

φ1(2), φ2(2) … φN(2) BIOLOIDS
… commands
φ1(M), φ2(M) … φN(M)
target position
Snakebot Model pi*
φdrive Ground Contact Model
Central Pattern
Generator φinput x(k+1) = f[x(k)] + u(k) p(k+1) -/+
φj = φmaxsin(2πf kΔt + δj) x = (p, θ) u=(0, φ1)

φsteer Δp
θ(k+1)

Motion pattern
PI
parameters Δθ -/+ θ*(k+1) tan-1(Δpy /Δpx)
controller
(φmax, f, δ)

Figure 3: Feedback control schematic for the BIOLOIDS snakebot. Motion pattern parameters and target position
(green) are the inputs for control, the central pattern generator, ground contact model and controller operate on the
system variables, and a list of ϕ1..N(1..M) joint angles serves as the output command.

30
 A Biomimetic Snake-Like Robot Using Bioloids

Head

Tail
Controller

Dynamixel

Independent-Axis

Figure 4: Image of the BIOLOIDS snakebot with head, tail, controller, Dynamixel servo and axes labeled

Figure 5: Process flow diagram for snakebot motion-planning.

Figure 6: Example of efficient snake-like motion (left) and poor locomotion (right).

31
 A Biomimetic Snake-Like Robot Using Bioloids

200
current direc tion
20 steerAngle
100
des ired direction

direction (o )
15 0

10
-100

5
-200
0 10 20 30 40 50 60 70 80
0

-5 20

distance from target

-10 15

-15 10

-20 5

-20 -15 -10 -5 0 5 10 15 20 25 30

0
0 10 20 30 40 50 60 70 80
time (a.u.)

Figure 7: Multiple target acquisition simulation. Snake trajectory (left), control trajectories (right)

40 60

30 50
φ off = 0
20 40
y (link units)

10 s=0 30 φ off = 1
y (link units)

0
s=0.3 20
-10 s=0.6
10 φ off = 2
link trajectory
-20
initial position 0
φ off = 3
-30 final position
-10
-40 target position
-20
-40 -20 0 20 40 -60 -40 -20 0 20
x (link units) x (link units)

Figure 8: Example trajectories with different slip factors (left) and turning offsets (right)

100

80 50

60
y (cm)

100
40
150
20

200
0

-100 -50 0 50
50 100 150 200 250 300
x (cm)

Figure 9: Registered snakebot positions after reaching two targets (left) and final snakebot position (right)

32
 A Biomimetic Snake-Like Robot Using Bioloids

100
0
t a rg e t 1
80 t a rg e t 2
direction error (o)

position error (cm)

-50
60
-100
target 1 40
target 2
-150 20

-200 0
0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 70

Figure 10: Example direction and position error trajectories for the simulation used in the demonstration

Figure 11: Integral control (top), and proportional control (bottom). Left: Simulation trajectories towards a target
located at (30, 30). Right: current direction (blue), desired direction (red), steering angle (green)

Figure 13: Position Error Direction Error

33
 A Biomimetic Snake-Like Robot Using Bioloids

Figure 14: Target Position Chosen By User Center of Mass Trajectories

Figure 15: Example of color segmentation during real-time feedback control

Figure 16: Initial snakebot configuration (left) and example trajectory of head and tail (right)

34
 A Biomimetic Snake-Like Robot Using Bioloids

Figure 17: Direction Error Steer Angle

Figure 18: Position Error

35