section 3 dynamic systems & control - uab · 3, 2013 - birmingham, alabama usa . ... denote the...

SECTION 3

DYNAMIC SYSTEMS & CONTROL

ASME District F - ECTC 2013 Proceedings - Vol. 12 87

ASME District F - Early Career Technical Conference Proceedings ASME District F - Early Career Technical Conference, ASME District F – ECTC 2013

November 2 – 3, 2013 - Birmingham, Alabama USA

VIBRATION OF ATOMIC FORCE MICROSCOPY USING HAMILTON’S PRINCIPLE

Rachael McCarty, M. Haroon Sheik, Kyle Hodges, and Nima Mahmoodi University of Alabama Tuscaloosa, AL, USA

ABSTRACT

Atomic force microscopy (AFM) uses a scanning process

performed by a microcantilever beam to create a three

dimensional image of a physical surface that is accurate on a

nano-scale level. AFM includes a microcantilever beam with a

tip at the end that is controlled in order to keep the force

between the tip and the surface constant by changing the

distance of the microcantilever from the surface. An accurate

understanding of the microcantilever motion and tip-sample

force is needed to generate accurate imaging. In this paper,

Hamilton’s Principle and the Galerkin Method are employed to

investigate the vibration of the microcantilever probe used in

tapping mode AFM. The goal is to compare two different

methods of including contact and excitation force in the

equations of motion and boundary conditions. The first case

considers contact force at the tip to be a part of the boundary

conditions of the beam. The second case assumes that the force

is a concentrated force that is applied in the equations of

motion, and the boundary conditions are similar to the free end

of a cantilever beam. For the two cases, the equations of

motion, the modal shape functions including the natural

frequencies, the time response functions, and the complete

beam mechanics are obtained. The first natural frequencies of

the two models are also compared to each other and to the

experimental natural frequency of the Bruker Innova AFM with

MPP-11123-10 microcantilever. For both cases, results are

shown for neglecting and including the effects of the tip mass.

I. INTRODUCTION Atomic Force Microscopy (AFM) was originally invented

and used for nano-scale scanning to create a three dimensional

image of a physical surface. The scanning process is performed

by a microcantilever that contacts or taps the surface. More

recently, microcantilever probes have been used extensively for

Friction Force Microscopy (FFM), Lateral Force Microscopy

(LFM), Piezo-response Force Microscopy (PFM), biosensing,

and other applications [1-4]. Most AFMs operate by exciting

the microcantilever using a piezoelectric tube actuator at the

base of the probe. However, some microcantilevers have a layer

of piezoelectric material on one side for actuation purposes.

This layer is usually ZnO [5] or Lead Zirconate Titanate (PZT).

The application of the piezoelectric microcantilever is

widespread; it has been used for force microscopy, Scanning

Near-field Optical Microscopy (SNOM), biosensing, and

chemical sensing [6-9]. An accurate understanding of the

microcantilever motion and tip-sample force is needed to

generate accurate imaging.

The force between tip and sample consists of two main

forces: Van der Waals force and contact force [10]. In non-

contact mode, there is only Van der Waals force between the

AFM tip and sample. However, in a tapping contact AFM, both

forces are applied to the tip. In this work, only the linear

contact force is considered, since it is much larger than the Van

der Waals force.

The dynamics of the microcantilever have been

experimentally and analytically studied in some research

works. Experimental investigations have been performed in air

and liquid on dynamic AFMs, and the frequency response of

the systems were obtained [11-13]. Nonlinear dynamics of a

piezoelectric microcantilever have been studied considering the

nonlinearity due to curvature, and piezoelectric material [14,

15]. In other works, linear dynamic models have been

developed for contact AFM probes, and numerically solved

[16-20].

When including the force at the end of the beam during

mathematical analysis of beam mechanics and vibrations, two

methods are currently used in research works. One method is to

consider the force at the end of the beam to be a part of the

equation of motion using some type of step function such as the

Heaviside function [14, 15, 19, 21, 22]. The other method is to

consider the force in the boundary conditions [11, 16, 17, 23,

24, 25].

This research work investigates the vibration of dynamic

tapping mode AFM for these two different methods of analysis.

For the two different cases, the equations of motion are derived

using Hamilton’s Principle, the modal shape functions

including the natural frequency are obtained using separation of

variables, and the time response functions are obtained using

Galerkin Method. The microcantilever beam is modeled with a

spring on the free end to approximate the linear contact force

[26]. The first case considers contact and excitation force at the


tip to be a part of the boundary conditions of the beam. The

second case assumes that the force is a concentrated force that

is applied in the equations of motion, and the boundary

conditions are similar to the free end of a cantilever beam. The

results of these two cases will be compared. The natural

frequencies of the two models are also compared to each other

and to the experimental natural frequency of the Bruker Innova

AFM with MPP-11123-10 microcantilever.

Additionally, most research works neglect the effect of tip

mass completely from the equation of motion and boundary

conditions [11-25]. In this work, the tip mass is included and

the results will be compared for including and excluding the tip

mass to determine the validity of the commonly used approach

of neglecting the tip mass.

II. METHODS The governing equations of motion, natural frequencies,

mode shapes, and time response functions for the dynamics of a

microcantilever are mathematically derived in this Section for

the case of 1) a spring at the free end with the contact force at

the tip considered to be a part of the boundary conditions of the

beam and 2) a spring at the free end where the contact force is

considered to be a concentrated force that is applied in the

equations of motion and the boundary conditions.

Figure 1 shows a cantilever beam system with a spring

attached to the free end. The bending displacement of the beam

in the negative z direction at position x along the beam and time

t is w(x,t). The coordinate system (x, z) describes the dynamics

of the microcantilever, and t denotes time.

Figure 1. Cantilever beam with a spring attached to the free end.

2.1 Case 1: Force considered in Boundary Conditions

The relevant equation from Hamilton’s Principle is

1

0

0t

tdtWUT , (1)

where T is kinetic energy, U is potential energy, and W is the

work done by external loads on the beam. To derive the

equations of motion, expressions for kinetic energy, potential

energy, and external work will need to be derived. First, the

expression for kinetic energy is derived. The kinetic energy will

be the combined kinetic energy of the beam (Tb) and the tip

(Ttip). In order to be able to compare the results including and

excluding the tip mass, the kinetic energy of each must be

separate.

LL

tipbt

wdx

t

wTTT

0

2

2

2

1 ½m½m , (2)

where m1 is the mass per unit length of the beam, m2 is the tip

mass, and L is the length of the beam. Also, wL is the

displacement of the microcantilever at the free end and is a

function of time.

The expression for potential energy comes from two

sources. Ub is the potential energy due to the strain energy of

the beam, and Us is the potential energy due to the spring.

L

b dxx

wU

0

2

2

2

½EI , (3)

Lw

LLS dwkwU0

2L½kw , (4)

where E is the elastic modulus of the beam, I is the mass

moment of inertia of the beam, and k is the spring constant.

The external work is

LwtFW sin , (5)

where F and Ω are the amplitude and frequency of the

excitation force.

Substituting Equations (2)-(5) into Equation (1) and

simplifying results in the equation of motion and boundary

conditions for bending vibrations of the microcantilever of the

AFM shown in Figure 1 for Case 1:

0)(

1 ivEIwwm , (6)

00 w , 00 w , 0Lw , tFwmkwwEI LLL sin2

. (7)

For simplification, apostrophes w denote the partial

derivative with respect to x, and dots w denote the partial

derivative with respect to time. The following substitution is

helpful for analyzing the full response of the beam:

),(,, txatxvtxw , (8)

where

3

32

210),( xtaxtaxtatatxa . (9)

To solve for a(x,t), the first three boundary conditions from

Equation (7) are implemented. This results in:

23 3),( LxxtBtxa . (10)

To solve for B(t), the fourth boundary condition from Equation

(7) is used. This results in the second order ordinary differential

equation:

t

Lm

FtBdtB sin

2 32

2 , (11)

where

32

32

2

26

Lm

kLEId

. (12)


Any solution of Equation (11) will work in Equation (10). The

following solution is chosen:

tCtB sin , (13)

where

2232

dL

FC . (14)

Now Equation (8) becomes

tLxxCtxvtxw sin3,, 23

. (15)

Substituting Equation (15) into Equations (6) and (7) yields

tLxxCmv

m

EIv iv sin3 232

1)(

1

, (16)

00 v , 00 v , 0Lv , LLL vmkvvEI

2 . (17)

In order to derive the mode shapes and natural frequencies

of the system, the force term is removed from Equation (16)

and separation of variables is implemented. The mode shapes

and natural frequencies can be found by solving the following

equations:

0

21)( EI

wmiv, (18)

00 , 00 , 0L , 2

2mkEI LL (19)

Solving Equation (18) and (19) results in

xxAx nnnn sinhsin

LL

LLxx

nn

nnnn

coshcos

sinhsincoscosh , (20)

where n = 1, 2,…∞ indicating the number of the mode, and λn

are the roots of the following frequency equation:

LL nn coshcos1

0sinhcoscoshsin3

LLLLEI

nnnn

n

(21)

where

1

4

4

2m

EI

Lmk n , (22)

4 1

2

EI

mnn

, (23)

where ωn are the natural frequencies. The value for An can be

found using the orthogonality condition.

Now the Galerkin method can be utilized. The Galerkin

method uses the following definition:

m

i

ii tqxtxw

1

, . (24)

After substituting Equation (24) into Equation (16), multiplying

by Φj, integrating on the limits zero to L, simplifying, and

putting the equation into matrix form, the two term Galerkin

solution becomes two coupled ordinary differential equations:

2

1

2

1

2221

1211

2

1

f

f

q

q

kk

kk

q

q

, (25)

where

L

ivdxEIk

01

)(111 , (26)

L

ivdxEIk

01

)(212 , (27)

L

ivdxEIk

02

)(121 , (28)

L

iv dxEIk0

2)(

222 , (29)

L

dxLxxtCmf0

231

211 3sin2 , (30)

L

dxLxxtCmf0

232

212 3sin2 , (31)

The full results for Case 1 are obtained mathematically.

Section 3 will discuss the computational results.

2.2 Case 2: Force considered in Equation of Motion

The second case to be examined, as stated previously, is a

system including a spring at the free end where the contact

force is considered to be a concentrated force that is applied in

the equations of motion and the boundary conditions are similar

to those of a free end cantilever. Hamilton’s principle is, again,

utilized to derive the equation of motion and boundary

conditions for this case. The equation for the kinetic energy of

the beam, Equation (2), and the equation for the potential

energy of the beam, Equation (3), are the same as in the first

case. Equation (4), the potential energy of the spring, will not

be included in this case. Instead, the work done by the spring

force is included in the external work term resulting in

L

Next wdxLxHfW0

, (32)

where

tFkwf LN sin , (33)

and H(ζ) is the Heaviside function and is defined as

01

00)(

H . (34)

Substituting Equations (2), (3), and (32) into Equation (1) and

simplifying results in the equations of motion and boundary

conditions for bending vibrations of the microcantilever of the

AFM shown in Figure 1 for Case 2:

LxHfEIwwm N

iv )(1 , (35)

00 w , 00 w , 0Lw , LL wmwEI

2 . (36)

In order to derive the mode shapes of the system, the force term

is removed from Equation (35). The derivations will be similar

to Case 1. Equation (35) becomes the same as Equation (16).

Solving the differential equation and applying boundary

conditions then results in the solution for our mode shapes.

Following the process used in Section 2.1, the resulting mode

shape equation can be solved and is the same as Equations (20),

(21), and (23). The only difference is Equation (22) defining μ,

which becomes


1

4

4

2m

EI

Lm n . (37)

With the mode shapes and natural frequencies determined, the

full response of the system can be found, again, using the

Galerkin method. Following the same procedure as in Section

2.1, the two term Galerkin solution becomes two coupled

ordinary differential equations of the same form as Equation

(27) where

)(2

10

1)(

111 LkdxEIkL

iv , (38)

)()( 21

01

)(212 LLkdxEIk

Liv , (39)

)()( 21

02

)(121 LLkdxEIk

Liv , (40)

)(2

20

2)(

222 LkdxEIkL

iv , (41)

L

dxtFf0

11 sin , (42)

L

dxtFf0

22 sin , (43)

The full results for both cases are obtained mathematically.

Section 3 will discuss and compare the computational results.

III. RESULTS The complete mechanics of a microcantilever beam have

been obtained mathematically for two possible cases most

commonly used in research works. The first case considers

contact force at the tip to be a part of the boundary conditions

of the beam. The second case assumes that the force is a

concentrated force that is applied in the equations of motion,

and the boundary conditions are similar to the free end of a

cantilever beam. First, the equations of motion were found

using Hamilton’s Principle. Next, the natural frequencies and

mode shapes were found. Finally, the time response was

calculated using the Galerkin Method.

In this section, numerical results are presented. See Chart 1

for the values of properties used in the numerical analysis.

Table 1. Microcantilever Properties

Constant Value

L (μm) 125±5 E (GPa) 160±5 I (μm

4) 154±5

m1 (mg/m) 34.8±2 m2 (pg) 2.19±0.5

k (x10-15

N/m) 75.4±3 F (μN) 1±0.1

Beam values are found on the Bruker AFM Probe web site

[27]. The spring constant, k, is actually somewhere in the range

of -15 to 40 N/m [28]. However, this value of the spring, which

represents the contact force only, acts over a very small range

of motion when the tip is in contact with the sample. Therefore,

a number with a similar magnitude as the effect of the tip mass

was selected to represent the small nature of the spring force.

Since the excitation force, F, can only be found experimentally,

a reasonable value of 1 μN is chosen.

First, the results of the two methods for calculating the

natural frequencies are compared. The natural frequencies are

found by numerically solving Equations (21), (22), and (23) for

Case 1 and Equations (21), (23), and (37) for Case 2. Chart 2

shows that the two methods resulted in very similar results.

Table 2. Natural Frequencies of the Microcantilever Beam for Case 1 and 2

Natural Frequency

Case 1 Case 2 Percent

Difference

ω1 (kHz) 301.25 301.26 0.003 ω2 (kHz) 1,887.9 1,888.0 0.005

The nominal value of natural frequency was found on the

Bruker AFM Probe web site [27]. The nominal frequency is

300 kHz with a possible range of 200 to 400 kHz. Also, the

experimental value of natural frequency was found using the

Bruker Innova software and Bruker Innova AFM with MPP-

11123-10 microcantilever. The experimental first natural

frequency was found to be 360.3 kHz. The experimental value

is within Bruker’s range. Since the other properties are set to

the nominal values from Bruker’s website, it is most reasonable

to compare the numerical natural frequency to the nominal

natural frequency from the website. This results in an error of

0.42%.

Next, the mode shapes for the two different cases are

compared. Both Cases use Equation (20) to find the mode

shapes. Figures (2) and (3) are the first two modes for Case 1

and Case 2, respectively.

Figure 2. The first and second mode shapes, ϕ1 and ϕ2, for Case 1.

0 20 40 60 80 100 120 140-0.4

-0.2

0

0.2

0.4

x (m)

n (

x10

6)

1

2


Figure 3. The first and second mode shapes, ϕ1 and ϕ2,

for Case 2.

The maximum difference between these two cases is

essentially zero. There is practically no difference between

mode shapes of the two cases.

Next, the time dependent functions, q1 and q2, are

compared. The Matlab solver ode23 was used to numerically

solve Equations (25) through (31) for Case 1 and Equations (25

and (38) through (43) for Case 2. See Figures 4 and 5 (Case 1)

and Figures 6 and 7 (Case 2) for graphical representations of q1

and q2.

Figure 4. The first and second time response functions, q1

and q2, for Case 1.

Figure 5. A zoomed in view of the first and second time

response functions, q1 and q2, for Case 1.

Figure 6. The first and second time response functions, q1

and q2, for Case 2.

Figure 7. A zoomed in view of the first and second time response functions, q1 and q2, for Case 2.

As can be seen in Figures 4 through 7, the results for the

two different methods are similar. The main difference is the

amplitude of the first time function, q1. Comparison of the

amplitudes after the initial settling time reveals that the

magnitude of Case 1 is approximately 20.3% larger than Case

2. The second time function, q2, is much smaller than q1 in both

cases and will not contribute as much to the final response.

Finally, the complete response can be found using

Equations (15) and (24) for Case 1 and Equation (24) for Case

2. The results are shown in Figures 8 and 9 for Cases 1 and 2,

respectively.

Figures 8 and 9 show the complete microcantilever beam

mechanics at the free end of the beam as derived in Section 2.

The two graphs are very similar with the most noticeable

difference, again, being the amplitude. The amplitude of the

complete response, w(L,t), after the settling period is

approximately 19.6% larger in Case 1.

All of the steps detailed so far in Section 3 are repeated for

Case 1 and 2 without tip mass. The complete response for the

tip of the beam, w(L,t), is shown in Figures 10 and 11 without

tip mass included.

0 20 40 60 80 100 120 140-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

x (m)

n (

x10

6)

1

2

0 1 2 3 4 5 6-50

0

50

time (ms)

qn (

x10

-12)

q2

q1

5 5.01 5.02 5.03 5.04 5.05-30

-20

-10

0

10

20

30

time (ms)

qn (

x10

-12)

q2

q1

0 1 2 3 4 5 6-50

0

50

time (ms)

qn (

x10

-12)

q2

q1

5 5.01 5.02 5.03 5.04 5.05-30

-20

-10

0

10

20

30

time (ms)

qn (

x10

-12)

q2

q1


Figure 8. The complete response at the free end of the

beam, w(L,t), for Case 1.


beam, w(L,t), for Case 2.


beam, w(L,t), for Case 1 with tip mass not included.

A comparison of Figure 8 to Figure 10 shows that there is

approximately a 20.1% increase in amplitude after the settling

period when tip mass is included in Case 1. Therefore, for this

specific beam under these conditions, tip mass is not negligible

for Case 1. A comparison of Figure 9 to Figure 11 shows that

there is approximately a 0.3% increase in amplitude when tip

mass is included in Case 2. Therefore, for Case 2, tip mass is

negligible.


beam, w(L,t), for Case 2 with tip mass not included.

CONCLUSIONS Two common ways of handling the external force applied

to the microcantilever of an AFM are examined. The first case

includes the external force in the boundary conditions. The

second case includes it in the equation of motion with boundary

conditions like that of a free end. In Section 2, the equations of

motion are derived using Hamilton’s Principle, the natural

frequencies and mode shapes are determined, and the time

response is found using the Galerkin Method.

Section 3 details the numerical analysis. The results show

very little to no difference when determining the natural

frequencies using the two different methods with a maximum

difference of 0.005% in the first two natural frequencies and a

practically no difference in the mode shapes.

However, the time response plots show more difference

between the two different methods, with the amplitude having a

difference of approximately 20.3% between the two methods.

Similarly, the complete response of the microcantilever beam at

the free end has a difference in amplitudes of approximately

19.6%.

Finally, the complete response is found again with tip mass

set to zero. Comparing the results shows that tip mass creates a

20.1% increase in amplitude for Case 1 and a 0.3% increase for

Case 2.

These results indicate that if a research work is only

interested in finding the natural frequencies and mode shapes,

using either method is equally reliable. Since the second

method (including the force in the equation of motion) is a

considerably easier method for deriving these natural

frequencies and mode shapes, it is reasonable to use the second

method. However, the time response presents more differences

between the two methods and requires further examination and

physical experimentation to validate the results and to

determine which method is more reliable. As for the tip mass,

the complete response was affected to a greater extent in Case 1

than in Case 2. These results indicate that tip mass may not

always be negligible.

0 1 2 3 4 5 6-15

-10

-5

0

5

10

15

time (ms)

w(L

,t)

(m

)

0 1 2 3 4 5 6-15

-10

-5

0

5

10

15

time (ms)

w(L

,t)

(m

)

0 1 2 3 4 5 6-15

-10

-5

0

5

10

15

time (ms)

w(L

,t)

(m

)

0 1 2 3 4 5 6-15

-10

-5

0

5

10

15

time (ms)

w(L

,t)

(m

)


ACKNOWLEGEMENT This material is based upon work supported by the

National Science Foundation Graduate Research Fellowship

under Grant No. 23478. Also, portions of this work were

funded by the National Science Foundation, GK-12 Grant No.

0742504

REFERENCES

[1] Salehi-Khojin, A., Bashash, S., and Jalili, N., Thompson,

G.L., and Vertegel, A., 2009, “Modeling Piezoresponse

Force Microscopy for Low-Dimensional Material

Characterization: Theory and Experiment”, Journal of

Dynamic Systems, Measurement, and Control 31(6), pp.

061107(1-8).

[2] Lekka, M., and Wiltowska-Zuber, J., 2009, “Biomedical

Applications of AFM”, Journal of Physics: Conference

Series, 146, 012023.

[3] Mahmoodi, S.N., and Jalili, N., 2008, “Coupled

Flexural-Torsional Nonlinear Vibrations of

Piezoelectrically-Actuated Microcantilevers”, ASME

Journal of Vibration and Acoustics, 130(6), 061003.

[4] Holscher, H., Schwarz, U.D., and Wiesendanger, R.,

1997, “Modeling of the Scan Process in Lateral Force

Microscopy”, Surface Science, 375, 395-402.

[5] Shibata, T., Unno, K., Makino, E., Ito, Y., and Shimada,

S., 2002, “Characterization of Sputtered ZnO Thin Film

as Sensor and Actuator for Diamond AFM Probe”,

Sensors and Actuators A, 102, 106-113.

[6] Itoh, T., Suga, T, 1993, “Development of a Force Sensor

for Atomic Force Microscopy Using Piezoelectric Thin

Films”, Nanotechnology, 4, 2l8-224.

[7] Yamada, H., Itoh, H., Watanabe, S., Kobayashi, K., and

Matsushige, K., “Scanning Near-field Optical

Microscopy Using Piezoelectric Cantilevers”, Surface

and Interface Analysis, 27, 503-506.

[8] Rollier, A-S., Jenkins, D., Dogheche, E., Legrand, B.,

Faucher, M., and Buchaillot, L., 2010, “Development of

a New Generation of Active AFM Tools for Applications

in Liquid”, Journal of Micromechanics and

Microengineering, 20, 085010.

[9] Rogers, B., Manning, L., Jones, M., Sulchek, T., Murray,

K., Beneschott, B., and Adams, J.D., 2003, “Mercury

Vapor Detection with A Self-Sensing, Resonating

Piezoelectric Cantilever”, Review of Scientific

Instruments, 74(11), 4899-4901.

[10] Seo, Y. and Jhe, W., 2008, “Atomic Force Microscopy

and Spectroscopy”, Reports on Progress in Physics, 71,

016101.

[11] Delnavaz, A. Mahmoodi, S.N., Jalili, N., Ahadian, M.M.

and Zohoor, H., 2009, “Nonlinear Vibrations of

Microcantilevers Subjected to Tip-Sample Interactions:

Theory and Experiment”, Journal of Applied Physics,

106, 113510.

[12] Chu, J., Maeda, R., Itoh, T., and Suga, T., 1999, “Tip-

Sample Dynamic Force Microscopy Using Piezoelectric

Cantilever for Full Wafer Inspection”, Japanese Journal

of Applied Physics, 38, 7155–7158.

[13] Asakawa, H., and Fukuma, T., 2009, “Spurious-Free

Cantilever Excitation in Liquid by Piezoactuator with

Flexure Drive Mechanism”, Review of Scientific

Instruments, 80, 103703.

[14] Mahmoodi, S.N., Daqaq, M., and Jalili, N., 2009, “On

the Nonlinear-Flexural Response of Piezoelectrically-

Driven Microcantilever Sensors”, Sensors and Actuators

A, 153(2), 171–179.

[15] Mahmoodi, S.N., Jalili, N. and Daqaq, M., 2008,

“Modeling, Nonlinear Dynamics and Identification of a

Piezoelectrically-Actuated Microcantilever Sensor”,

ASME/IEEE Transaction on Mechatronics, 13(1), 59-65.

[16] Kuo, C., Huy, V., Chiu, C., and Chiu, S., 2012,

“Dynamic Modeling and Control of an Atomic Force

Microscope Probe Measurement System”, Journal of

Vibration and Control, 18(1), 101-116.

[17] Ha, J., Fung, R., and Chen, Y., 2008, “Dynamic

Responses of An Atomic Force Microscope Interacting

with Sample”, Journal of Dynamic Systems,

Measurement, and Control, 127, 705-709.

[18] El Hami, K., and Gauthier-Manuel, B., 1998, “Selective

Excitation of the Vibration Modes of A Cantilever

Spring”, Sensors and Actuators A, 64, 151–155.

[19] Chang, W. and Chu, S., 2003, “Analytical Solution of

Flexural Vibration Responses on Taped Atomic Force

Microscope Cantilevers”, Physics Letters A, 309, 133-

137.

[20] Rabe, U., Janser, K., and Arnold, W., 1996, “Vibrations

of Free and Surface-coupled Atomic Force Microscope

Cantilevers: Theory and Experiment”, Rev. Sci. Instrum.,

67, 3281.

[21] Jovanovic, V., “A Fourier Series Solution for the

Transverse Vibration Response of a Beam with a viscous

boundary”, Journal of Sound and Vibration, 2010, doi:

10.1016/ j.jsv.2010.10.007

[22] Hilal, M. A. and Zibdeh, H. S., 2000, “Vibration

Analysis of Beams with General Boundary Conditions

Traversed by a Moving Force”, Journal of Sound and

Vibration, 229(2), 377-388.

[23] Bahrami, A. and Nayfeh, A. H., 2012, “On the Dynamics

of Tapping Mode Atomic Force Microscoe Probes”,

Nonlinear Dyn, 70, 1605-1617.

[24] Abdel-Rahman, E. M. and Nayfeh, A. H., 2005, “Contact

Force Identification Using the Subharmonic Resonance

of a Contact-Mode Atomic Force Microscopy”,

Nanotechnology, 16, 199-207.

[25] Bahrami, A. and Nayfeh, A. H., 2013, “Nonlinear

Dynamics of Tapping Mode Atomic Force Microscopy

in the Bistable Phase”, Commun Nonlinear Sci Numer

Simulat, 18, 799-810.

[26] Abdel-Rahman, E. M. and Nayfeh, A. H., 2005, “Contact

Force Identification Using the Subharmonic Resonance

of a Contact-Mode Atomic Force Microscopy”,

Nanotechnology, 16, 199-207.


http://scitation.aip.org/vsearch/servlet/VerityServlet?KEY=ASMEDL&possible1=Salehi-Khojin%2C+Amin&possible1zone=author&maxdisp=25&smode=strresults&pjournals=AMREAD%2CJAMCAV%2CJBENDY%2CJCNDDM%2CJCISB6%2CJDSMAA%2CJEPAE4%2CJERTD2%2CJETPEZ%2CJEMTA8%2CJFEGA4%2CJFCSAU%2CJHTRAO%2CJMSEFK%2CJMDEDB%2CJMDOA4%2CJMOEEX%2CJPVTAS%2CJSEEDO%2CJOTRE9%2CJOTUEI%2CJVACEK%2CJTSEBV&aqs=true

http://scitation.aip.org/vsearch/servlet/VerityServlet?KEY=ASMEDL&possible1=Jalili%2C+Nader&possible1zone=author&maxdisp=25&smode=strresults&pjournals=AMREAD%2CJAMCAV%2CJBENDY%2CJCNDDM%2CJCISB6%2CJDSMAA%2CJEPAE4%2CJERTD2%2CJETPEZ%2CJEMTA8%2CJFEGA4%2CJFCSAU%2CJHTRAO%2CJMSEFK%2CJMDEDB%2CJMDOA4%2CJMOEEX%2CJPVTAS%2CJSEEDO%2CJOTRE9%2CJOTUEI%2CJVACEK%2CJTSEBV&aqs=true

http://scitation.aip.org/vsearch/servlet/VerityServlet?KEY=ASMEDL&possible1=Thompson%2C+Gary+Lee&possible1zone=author&maxdisp=25&smode=strresults&pjournals=AMREAD%2CJAMCAV%2CJBENDY%2CJCNDDM%2CJCISB6%2CJDSMAA%2CJEPAE4%2CJERTD2%2CJETPEZ%2CJEMTA8%2CJFEGA4%2CJFCSAU%2CJHTRAO%2CJMSEFK%2CJMDEDB%2CJMDOA4%2CJMOEEX%2CJPVTAS%2CJSEEDO%2CJOTRE9%2CJOTUEI%2CJVACEK%2CJTSEBV&aqs=true

http://scitation.aip.org/vsearch/servlet/VerityServlet?KEY=ASMEDL&possible1=Vertegel%2C+Alexey&possible1zone=author&maxdisp=25&smode=strresults&pjournals=AMREAD%2CJAMCAV%2CJBENDY%2CJCNDDM%2CJCISB6%2CJDSMAA%2CJEPAE4%2CJERTD2%2CJETPEZ%2CJEMTA8%2CJFEGA4%2CJFCSAU%2CJHTRAO%2CJMSEFK%2CJMDEDB%2CJMDOA4%2CJMOEEX%2CJPVTAS%2CJSEEDO%2CJOTRE9%2CJOTUEI%2CJVACEK%2CJTSEBV&aqs=true

[27] Bruker AFM Probes. Web.

http://www.brukerafmprobes.com/

[28] Hölscher, H., 2006, “Quantitative Measurement of Tip-

Sample Interactions in Amplitude Modulation Atomic

Force Microscopy”, Applied Physics Letters, 89, 123109.




ROBOT AUTONOMOUS NAVIGATION CONTROL ALGORITHMS FOR VARIOUS TYPES OF TASKS PERFORMED IN THE X-Y PLANE

Stephen Armah, Sun Yi

North Carolina A&T State University Greensboro, NC, USA

ABSTRACT

Mobile robot control has attracted considerable attention of

researchers in the areas of robotics and autonomous systems

during the last decade. One of the goals in the field of mobile

robotics is the development of mobile platforms that robustly

operate in populated environments and offer various services to

humans. Autonomous mobile robots need to be equipped with

appropriate control systems to achieve the goal. Such control

systems are supposed to have navigation control algorithms that

will make mobile robots successfully ‘move to a point’, ‘move

to a pose’, ‘follow a path’, ‘follow a wall’ and ‘avoid obstacles

(stationary or moving)’. Also, robust visual tracking algorithms

to detect objects and obstacles in real-time have to be integrated

with the navigation control algorithms. The research uses a Simulink model of the kinematics of a

unicycle, kinematically equivalent to a differential drive

wheeled mobile robot, developed by modifying the bicycle

model in [3]. Control algorithms that enable these robot

operations are integrated into the Simulink model. Also, an

effective navigation architecture that combines the ‘go-to-goal’,

‘avoid-obstacle’, and ‘follow-wall’ controllers into a full

navigation system is presented. A MATLAB robot simulator is

used to implement this navigation control algorithm. The robot

in the simulator moves to a goal in the presence of convex and

non-convex obstacles. Finally, experiments are carried out

using a ground robot, Dr Robot X80SV, in a typical office

environment to verify successful implementation of the

navigation architecture algorithm by modifying a program

available in [9].

Keywords: Wheeled mobile robots, PID-feedback control,

Navigation control algorithm, Differential drive, Hybrid

automata

INTRODUCTION A mobile robot is an automatic machine that is capable of

movement in any given environment. Wheeled mobile robots

(WMRs) are increasingly present in industrial and service

robotics, particularly when flexible motion capabilities are

required on reasonably smooth grounds and surfaces. Several

mobility configurations (wheel number and type, their location

and actuation, and single- or multi-body vehicle structure) can

be found in different applications [4]. The most common for

single-body robots are differential drive and synchro drive

(both kinematically equivalent to a unicycle), tricycle or car-

like drive, and omnidirectional steering [4].

The presented research uses control algorithms that make

mobile robots ‘move to a point’, ‘move to a pose’, ‘follow a

line’, ‘follow a circle’ and ‘avoid obstacles’ taken from the

literature . The main focus of the research is the

navigation control algorithm that has been developed to enable

a differential drive wheeled mobile robot (DDWMR) to

accomplish its assigned task of moving to a goal free from any

risk of collision with obstacles. In order to develop this

navigation system, a low-level planning is used, based on a

simple model whose input can be calculated using a PID

controller or transformed into actual robot input.

Simulation results using Simulink models and MATLAB

and a robot simulator that implements these algorithms are

presented. The robot simulator is able to move to a goal in the

presence of convex and non-convex obstacles. Also, several

experiments are performed using a ground robot, Dr Robot

X80SV, in a typical office environment, to verify successful

implementation of the navigation architecture algorithm using a

modified program developed in C# by Dr Robot Inc [9].

Possible applications of WMR include security robots, land

mine detectors, planetary exploration missions, Google

autonomous car, autonomous vacuum cleaners, and lawn

mowers, etc. [6].

1.1 Organization of contents

In Section 2 the methodology used for the research is

presented. Section 2.1 introduces the kinematic model of a

DDWMR and the control algorithms applied. For the purposes

of implementation, the kinematic model of a unicycle is also

introduced in this section. In Section 2.2 controllers built for

the different WMR behaviors are presented, such as ‘move to a

point’, ‘move to a pose’, ‘follow a line’, ‘follow a circle’, and

‘avoid obstacles’. Also in Section 2.2 a control navigation

algorithm that enables ‘go-to-goal’, ‘follow-wall’ and ‘avoid

obstacles’ is presented. In Section 3 simulations and

experiments performed are explained. In Section 4 the

simulations and experimental results are summarized.

Concluding remarks and future work are presented in Section 5.


KINEMATIC MODELING AND CONTROL ALGORITHMS

2.1 Kinematic Modeling of DDWMR The DDWMR setup used for the presented study is shown

in Figure 1 (top view). The configuration of the vehicle is

represented by the generalized coordinates , where is the global position and is the

heading. The vehicle’s velocity is by definition in the

vehicle’s -direction, is the distance between the wheels, is

the radius of the wheels, is the right wheel angular velocity,

is the left wheel angular velocity and is the heading rate.

Let and be the world and vehicle frames respectively.

Figure 1. DDWMR Setup

The kinematic model of the DDWMR based on the coordinate

is given by

For the purpose of implementation, the kinematic model of a

unicycle is used; it corresponds to a single upright wheel rolling

on a plane, with the equation of motion given as

The inputs in and are and . These inputs

are related as

A Simulink model shown in Figure 2 has been developed;

it implements the kinematic model of the unicycle by

modifying the bicycle model in [3].

Figure 2. Simulink Model for the Unicycle

2.2 Control Algorithms: Building Behaviors Control of the unicycle model inputs is about selecting the

appropriate input, , and applying the traditional

PID-feedback controller, given by

where , defined for each task below, is the error between the

desired value and the output value, is the proportional gain,

is the integrator gain, is the derivative gain and is time.

The control gains used in this research are obtained by

tweaking the various values to obtain satisfactory responses. If

the vehicle is driven at a constant velocity, then the

control input will only vary with the angular velocity, , thus

2.2.1 Developing Individual Controllers This section presents control algorithms that make mobile

robots ‘move to a point’, ‘move to a pose’, ‘follow a line’,

‘follow a circle’ and ‘avoid obstacles’.

2.2.1.1 Moving to a Point Consider a robot moving toward a goal point, ,

from a current position, , in the -plane, as depicted in

Figure 3 below.

Figure 3. Setup for Moving to a Point

The desired heading (robot’s relative angle), , is

determined as

and

Due to the problem of angles, a corrected error, is used

instead of as shown below

Thus can be controlled using . If the robot’s velocity is

to be controlled, a proportional controller gain, , is applied to

the distance from the goal, shown below [2]

2.2.1.2 Moving to a Pose The above controller could drive the robot to a goal

position, but the final orientation depends on the starting

position. In order to control the final orientation is

rewritten in matrix form as 3

theta

2

y

1

xw

limit

v

limit

acceleration

limit1

acceleration

limit

sin

cos

Product

1

s

Integrator1

1

s

Integrator

handbrake

2

w

1

v


is then transformed into the polar coordinate form using

the notation shown in Figure 4.

Figure 4. Setup for Moving to a Pose

Applying a change of variables, we have

which results in

and assumes the goal is in front of the vehicle. The linear

control law

drives the robot to unique equilibrium at . The intuition behind this controller is that the terms and

drive the robot along a line toward , while the term

rotates the line so that . The closed-loop system

is stable so long as . For the

case where the goal is behind the robot, that is

the

robot is reversed by negating and in the control law. The

velocity always has a constant sign which depends on the

initial value of . 2.2.1.3 Obstacle Avoidance

In a real environment robots must avoid obstacles in order

to go to a goal. Depending on the positions of the goal and the

obstacle(s) relative to the robot, the robot must move to the

goal using from a ‘pure go-to-goal’ behavior or blending the

‘avoid obstacle’ and the ‘go-to-goal’ behaviors. In pure

obstacle avoidance the robot drives away from the obstacle and

moves in the opposite direction. The possible values of that

can be used in the control law discussed in 2.2.1.1 are shown in

Figure 5 below, where is the obstacle heading.

Figure 5. Setup for Avoiding Obstacle

2.2.1.4 Following a Line Another useful task for a mobile robot is to follow a line on

a plane defined by . This requires two

controllers to adjust the heading. One controller steers the robot

to minimize the robot’s normal distance from the line given by

The proportional controller

turns the robot toward the line. The second controller adjusts

the heading angle to be parallel to the line

using the proportional controller

The combined control law

turns the wheel so as to drive the robot toward the line and to

move along it .

2.2.1.5 Following a Circle Instead of a straight line, the robot can follow a defined

path on the -plane, and in this section the robot follows a

circle. This problem is very similar to the control problem

presented in 2.2.1.1, except that this time the point is moving.

The robot maintains a distance behind the pursuit point and

an error, , can be formulated as [2]

that will be regulated to zero by controlling the robot’s velocity

using a PI controller

- -


The integral term is required to provide a finite velocity

demand when the following error is zero. The second

controller steers the robot toward the target which is at the

relative angle given by , and a controller given by . 2.2.2 Developing Navigation Control Algorithm

This section introduces how the navigation architecture,

that consists of go-to-goal, follow-wall and avoid obstacle

behaviors, was developed. In order to develop the navigation

system a low-level planning was used, by starting with a simple

model whose input can be calculated by using a PID controller

or transformed into actual robot input, depicted in Figure 6 . For this simple planning a desired motion vector, , is picked

and set equal to the input, , shown in Figure 7.

Figure 6. Planning Model Input to Actual Robot Input

This selected system is controllable as compared to the

unicycle system which is non-linear and not controllable even

after it has been linearized. This layered architecture makes the

DDWMR act like the point mass model shown in .

2.2.2.1 Go-to-goal (GTG) Mode Consider the point mass moving toward a goal point, ,

with current position as in the -plane. The error,

, is controlled by the input , where is gain

matrix. Since the system is asymptotically stable if

. An appropriate is selected to obey the function

shown in Figure 8 above such that , where

and are constants to be selected; in this way the robot will not

go faster further away from the goal .

2.2.2.2 Obstacle Avoidance (AO) Mode Let the obstacle position be , then is

controlled by the input , and since the system

is desirably unstable if . An appropriate is selected to

obey the function shown in Figure 9 above such that , where and are constants to be selected . 2.2.2.3 Blending AO and GTG Modes

In a ‘pure GTG’ mode, , or ‘pure AO’ mode, , or

what is termed as hard switches, performance can be

guaranteed, but the ride can be bumpy and the robot can

encounter the zeno phenomenon . A control algorithm for

blending the and modes is given by . This

algorithm ensures a smooth ride but does not guarantee

performance .

where is a constant distance to the obstacle/boundary, and

is the blending function to be selected, giving appropriately as

an exponential function by

where is a constant to be selected.

2.2.2.4 Follow-wall (FW) Mode

As pointed out in Section 2.2.2.2, in a pure obstacle

avoidance mode the robot drives away from the obstacle and

move in the opposite direction, but this is overly cautious in a

real environment where the task is to go to a goal. The robot

should be able to avoid obstacles by going around its boundary,

and this situation leads to what is termed as the follow-wall or

an induced or sliding mode, , between the and

modes; this is needed for the robot to negotiate complex

environments . The FW mode maintains to the obstacle/boundary as if it

is following it, and the robot can clearly move in two different

directions, clockwise (c) and counter-clockwise (cc), along the

boundary, Figure 10 . This is achieved by rotating by

and to obtain and

respectively, and then

scaled by to obtain a suitable induced mode, as shown in

to .

The direction the robot selects to follow the boundary is

determined by the direction of , and it is determined using

the dot product of and , as shown in and

.

Another issue to be addressed is when the robot releases

, that is when to stop sliding. The robot stops sliding when

TRACK: PID Reference

Trajectory

Actual Trajectory

TRANSFORM

Figure 7. Point Mass Model

Figure 8. Suitable Graph for an Appropriate

Figure 9. Suitable Graph for an

Appropriate

Figure 10. Setup for Follow-wall Mode

PLAN


“enough progress” has been made and there is a “clear shot” to

the goal, as shown in and , where is the time of

last switch .

2.2.2.5 Implementation of the Navigation Algorithms The behaviors or modes discussed above are put together

to form the navigation architecture shown in Figure 11 below.

The robot started at the state and arrived at the goal ,

switching between the three different operation modes; this

system of navigation is termed the hybrid automata (HA) where

the navigation system has been described using both the

continuous dynamics and the discrete switch logic .

Figure 11. Setup for Navigation Architecture

An illustration of this navigation system is shown in Figure 12,

where the robot avoids a rectangular block as it moves to a

goal.

Figure 12. Illustration of the Navigation System

2.2.2.6 Tracking and Transformation of the ‘Simple’ Model Input

The simple planning model input, can be

tracked using a PID controller or clever transformation can be

used to transform it into the unicycle model input,

. These two approaches are discussed below.

Method 1: Tracking Using a PID Controller

Let the output from the planning model be

and the current position of the point mass be ,

Figure 13, then the input, , to the unicycle model

can be determined as shown below

From

Method 2: Transformation

In this clever approach a new point, , of interest is

selected on the robot at a distance from the center of mass,

, as shown in Figure 14 , where and

.

If the orientation is ignored then

Thus

Substituting into , and using and ,

we have

3. SIMULATIONS AND EXPERIMENTATION 3.1 Simulations Using MATLAB/Simulink

Simulink models that implement the control algorithms

discussed in Section 2 are presented in Figures 15 to 18. These

models are based on the unicycle model in Figure 2.

Possible robot path

Enough progress and clear shot position

Position where robot start to follow the wall

Figure 13. Driving the Point Mass in a Specific Direction

Figure 14. DDWMR Model Showing the New Point


Figure 15. Model that Drives the Robot to a Point

Figure 16. Model that Drives the Robot to a Pose

Figure 17. Model that Drives the Robot along a Line

Figure 18. Model that Moves the Robot in a Unit Circle

Figure 19 (a-d). Sequence Showing the MATLAB Robot Simulator Movement

A MATLAB robot simulator developed by [10] was used

to simulate the navigation architecture control algorithms

presented in Section 2; it combines the GTG, AO, and FW

controllers into a full navigation system. Figure 19(a-d) shows

a sequence of simulator movements. The robot navigates

around a cluttered, complex environment without colliding with

any obstacles and reaching its goal location successfully.

The algorithm controlling the simulator implements a finite

state machine (FSM) to solve the full navigation problem. The

FSM uses a set of if/elseif/else statements that first check which

state (or behavior) the robot is in, and then based on whether an

event (condition) is satisfied, the FSM switches to another state

or stays in the same state, until the robot reaches its goal . The robot simulator mimics the Khepera III (K3) mobile

robot, which has infrared (IR) range sensors nine are located

in a ring around it and two are located on the underside. The IR

sensors are complemented by a set of five ultrasonic sensors.

The K3 has a two-wheel differential drive with a wheel encoder

for each wheel, for measuring the robot position .

Figure 20. Dr Robot X80SV used for the Experiments

3.2 Experimentation Using Dr Robot X80SV

The control algorithm built for the navigation architecture

presented in Section 2 was experimented on Dr Robot X80SV,

using a modified program developed in C# by , in an office

environment. The Dr Robot X80SV can be made to move to a

goal, able to wander around the environment, and follow a

given path whiles avoiding obstacles in front of it.

The Dr Robot X80SV, shown in Figure 20, is a fully

wireless networked system that uses two quadrature encoders

on each wheel for measuring its position and seven IR and three

ultrasonic range sensors for collision detection. It has a 2.6x

high resolution Pan-Tilt-Zoom CCD camera with two-way

audio capability, two 12V motors with over 22kg.cm torque

each, and two pyroelectric human motion sensors.

Its dimension is 38cm (length) x 35cm (width) x 28cm

(height), maximum payload of 10kg (optional 40 kg) with robot

weight of 3kg; its 12V 3700mAh battery pack has three hours

nominal operation time for each recharging, and it can drive up

to a maximum speed of 1.0 m/s. The distance between the

wheels is 26cm and the radius of the wheels is 8.5cm.

Figure 21. Closed-loop Feedback System for Controlling

the DC Motor of the Dr Robot X80SV [9]

sqrt(u(1)^2+u(2)^2)

throttle

atan2(u(2), u(1))

steeringKh

gain1

Kv

gain

+

-dif f

angular

difference

XY

v

w

x

y

theta

Unicycle

xg

Goal

xy

theta

q

rho

alpha

beta

direction

to polar

Krh

gain2

Kbe

gain1

Kal

gain

[xg(1:2) 0]

desired pose

xg(3)

desired headingXY

v

w

x

y

theta

Unicycle

STOP

|u|

Interpreted

MATLAB Fcn

direction of motion: +1 or -1

slope of l ine

Kh

gain1

Kd

gain

f(u)

distance from line1

constant+

-dif f

angular

difference

XY

v

w

x

y

theta

Unicycle

atan2(-L(1),L(2))

d

sqrt(u(1)^2+u(2)^2)-0.1

throttle

atan2(u(2), u(1))

steeringKh

gain

+

-dif f

angular

difference

XY

v

w

x

y

theta

Unicycle

PID

PID Controller

xy

Circle

goal

xy

PID Controller DC Motor

Potentiometer


The feedback system depicted in Figure 21 shows how the

DC motor system of the robot is controlled. A screenshot of the

C# interface used for the Dr Robot X80SV control is shown in

Figure 22(a and b).

Figure 22 (a). C# Graphic Interface: Main Sensor

Information and Sensor Map

Figure 22 (b). C# Graphic Interface: Path Control

Figure 23(a-f) shows an experimental setup showing a

sequence of movement of the Dr Robot X80SV implementing

the navigation control algorithm.

Figure 23 (a-f). Experimental Setup Showing Sequence of the Dr Robot X80SV Movement

4. RESULTS AND DISCUSSION

4.1 Simulink Models Results The time domain Simulink simulations were carried out

over a seconds duration for each model (refer to the

simulation setups in Figures 15 to 18). The trajectories in

Figure 24 were obtained by using proportional gains of and

for and respectively; the final goal point was

, compared to the desired goal of for

the initial state.

Trajectories in Figure 25 were obtained using ,

and ; the final pose was

, compared to the desired pose of

for the initial state.

The trajectories in Figure 26 were obtained with and , driving the robot at a constant speed of .

The trajectory in Figure 27 was obtained using , PID

controller gains of and , and the

goal is a point generated to move around the unit circle with a

frequency of Hz.

-1.5 -1 -0.5 0 0.5 1 1.5-1.5

-1

-0.5

0

0.5

1

1.5

x

y

Robot path

Circle

Initial state

Figure 25. Move to a Pose Figure 24. Move to a Point

Figure 26. Follow a Line

0 1 2 3 4 5 6 7 8 9 100

1

2

3

4

5

6

7

8

9

10

x

y

Line

Robot path

Initial state

0 1 2 3 4 5 6 7 8 9 100

1

2

3

4

5

6

7

8

9

10

x

y

Initial state

Robot path

Desired goal

0 1 2 3 4 5 6 7 8 9 100

1

2

3

4

5

6

7

8

9

10

x

y

Desired pose

Robot path

Initial state

Figure 27. Follow a Circle


4.2 MATLAB Robot Simulator Results

The trajectory shown in Figure 28a (refer to the simulation

setup in Figure 19) was obtained by using PID controller gains

of and , , ,

, initial state of , and desired goal of (1,1).

The final goal point associated with the simulation was (1.0077,

0.9677). Even though there is steady-state error in both states,

and , the result is encouraging, taken into account that a

stopping distance, of was specified.

Figure 28 (a and b). Trajectory in -plane (a) Robot Simulator (b) Experiment

4.3 Experimental Results

The trajectory shown in Figure 28b (refer to a similar

experimental setup in Figure 23) was obtained by using PID

controller gains of and for the

position control and and for the

velocity control, , , initial state of , and desired pose of . The final pose associated with the

experiment was . Even though there

were steady-state errors in the state values, the result is

encouraging. Possible causes of the errors are friction between

the robot wheels and the floor, imperfection in the sensors

and/or unmodeled factors (e.g., friction and backlash) in the

mechanical parts of the DC motor.

Note that during the experiments the robot sometimes got

lost or wandered around before arriving at the desired pose.

This is because the navigation system is not robust. It was built

using a low-level planning based on a simple model of a point

mass and the application of a linear PID controller.

5. CONCLUSION In this paper, an effective navigation control algorithm was

presented for a DDWMR, simulated using a MATLAB robot

simulator that mimics the robot and implemented on the Dr

Robot X80SV platform using C#. The algorithm makes the

robot move to a pose, able to wander around an office

environment, and followed a given path whiles avoiding

obstacles in the way.

This research paper has also demonstrated how to make a

unicycle, kinematically equivalent to DDWMR, ‘move to a

point’, ‘move to a pose’, ‘follow a line’, ‘move in a circle’, and

‘avoid obstacles’. These were simulated using Simulink

models.

In future, the authors will integrate real-time vision-based

object detection and recognition to address the imperfection of

the IR and ultrasonic range sensors. In addition, one or more

parameters such as length of path, energy consumption or

journey time optimization will be considered. The authors also

aim to design robust control algorithms with respect to time

delays. Furthermore, application of a high-level planning such

as Dijkstra, A*, D* or RRT for the robot path planning, and the

application of a non-linear control law such as adaptive control

to address some of the deficiencies of the navigation system

will be studied.

Another possible future work is to make the WMR

collaborate with unmanned aerial vehicles (UAV) for a range of

potential applications that include search and rescue,

surveillance, hazardous site inspection, planetary exploration

missions and domestic policing.

ACKNOWLEDMENT This work was supported by the NC Space Grant.

REFERENCES

[1] Borenstein, J. & Koren, Y. (1989). Real-time obstacle

avoidance for fast mobile robots. Systems, Man and

Cybernetics, IEEE Transactions, 19, pp 1179-1187.

[2] Corke, P. (2011). Robotics, vision and control: Fundamental

algorithms in matlab. Berlin: Springer, pp. 61-78.

[3] Corke, P. (1993-2011). Robotics Toolbox for MATLAB

(Version 9.8) (Software). Retrieved from:

http://www.petercorke.com/RVC

[4] De Luca, A., Oriolo G. & Vendittelli M. (2001). Control of

wheeled mobile robots: An experimental overview. Berlin:

Springer, pp 181-226.

[5] Egerstedt, M. (2013). Control of mobile robots [PowerPoint

slides]. Retrieved from: https://class.coursera.org/conrob-

001/class/index

[6] Jones J. L., Flynn A. M. (1993). Mobile robots: Inspiration

to implementation. A K Peters, Wellesley, MA

[7] Katsuhiko, O. (2002). Modern control engineering. Fourth

Edition. New Jersey: Prentice-Hall, pp 681-700.

[8] Siegwart, R., Nourbrakhsh, I. & Scaramuzza, D. (2004).

Introduction to autonomous mobile robots. Second Edition.

Massachusetts: The MIT Press, pp. 91-98.

[9] C# Advance X80 Demo Program (Software). (2008). Dr

Robot. Retrieved from: http://www.drrobot.com

[10] MATLAB Robot Simulator (Software). (2013). Georgia

Institute of Technology, Georgia: Georgia Tech Research

Corporation. Retrieved from:

http://gritslab.gatech.edu/projects/robot-simulator/

-0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

x

y

Desired goal

Initial state

Final goal

Robot path

-0.5 0 0.5 1 1.5 2 2.5-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5

x

y

Desired pose

Initial state

Final pose

Robot path




ROBOTIC BEHAVIOR PREDICTION USING HIDDEN MARKOV MODELS

ABSTRACT

There are many situations in which it would be

beneficial for a robot to have predictive abilities similar to

those of rational humans. Some of these situations include

collaborative robots, robots in adversarial situations, and

for dynamic obstacle avoidance. This paper presents an

approach to modeling behaviors of dynamic agents in order

to empower robots with the ability to predict the agent's

actions and identify the behavior the agent is executing in

real time. The method of behavior modeling implemented

uses hidden Markov models (HMMs) to model the

unobservable states of the dynamic agents. The background

and theory of the behavior modeling is presented.

Experimental results of realistic simulations of a robot

predicting the behaviors and actions of a dynamic agent in

a static environment are presented.

INTRODUCTION Robots are becoming increasingly autonomous,

requiring them to operate in more complicated

environments and assess complex phenomena in the world

around them. The ability to determine the behavior of

humans, other robots, and obstacles efficiently would

benefit many fields of robotics. Some such fields include

rescue robots, collaborative or assistive robots, multi-robot

systems, military robots, and autonomous vehicles. The

process of a robot determining the behavior or intent of

other agents is inherently probabilistic since the robot has

no way of ‘knowing’ the intent of the other agent. The

robot must make a best guess based on its observations of

the agent’s states. Yet, the same behavior may manifest

itself as different sequences in the change of state of the

agent being observed. Additionally, robots observe the state

of the agent via noisy sensors. This results in a doubly

stochastic process where an agent’s behavior is

probabilistically dependent on the sequence of state

changes it undergoes, and the observations seen by the

robot depend probabilistically on the state the agent is in. A

good model for the behavior of an agent being observed,

therefore, would incorporate both of these types of

uncertainty. One such model is a hidden Markov model

(HMM). A HMM is a statistical model that is a Markov

process with states that cannot be directly observed, i.e.

they are ‘hidden’. The basic theory of hidden Markov

models was developed and published in the early 1970’s by

Baum and his colleagues [1]-[2]. In the following several

years HMMs started being applied to speech processing by

Jelinek and others [3],[8],[9]. Since the theories were

originally published in mathematics journals, HMM

techniques were mostly unknown to engineers until

Rabiner [13] published his popular tutorial in 1989

thoroughly describing HMMs and some applications in

speech processing. Research in other applications has

begun to emerge.

In some previous works, there has been a focus on

predicting the behaviors of human drivers. In [4] and [5],

the authors researched in-vehicle systems that predict the

operators’ driving intent, but these systems have direct

access to the driver’s inputs to the vehicle (i.e. steering

angle, accelerator depression). In [6] Han and Veloso use

hidden Markov models to predict the behaviors of soccer

playing robots and utilize the probability of being in certain

‘accept’ or ‘reject’ states as the condition for establishing

occurrence of a behavior. In another application to robotic

soccer, [11] uses ‘sequential pattern mining’ to look for

mappings between currently occurring patterns and stored

predicate patterns. In [7] the authors model human

trajectories based on position observations at certain time

intervals. A HMM approach is used in [10], where the

authors study a mobile robot observing interactions

between two humans using a camera.

In this paper, a behavior modeling approach using

hidden Markov models is introduced that utilizes event

based observations. A novel method of determining

whether any of a set of non-exclusive behaviors are

occurring is described. The performance of these models is

Alan J. Hamlet, Carl D. Crane Department of Mechanical and Aerospace Engineering, University of Florida

Gainesville, Florida, USA


then tested in realistic simulations involving two mobile

robots in an indoor environment. The observations for the

models are made by the robot using a common laser range

finder. The remainder of this paper is laid out as follows:

first, the background and theory of the HMM behavior

models is explained. Next, the method of behavior

prediction is introduced, followed by the experimental set-

up and simulation description. Finally, the results of the

simulations are presented and discussed.

BEHAVIOR MODELING In this paper, ‘behaviors’ are defined as a temporal

series of state changes of a dynamic agent. The states of the

dynamic agent are assumed to be not directly observable,

but having observations that are correlated to the agent’s

state in a probabilistic manner. This is a reasonable

assumption because when the higher level intent or goal of

a dynamic agent is to be inferred, the relevant states would

be the agent’s internal or mental states. These states are not

directly observable but will result in measurable physical

phenomena that are correlated to the agent’s state. The

behavior dictates how the agent changes from one state to

another. When behaviors are defined in this way, hidden

Markov models are ideal representations for them.

There are several elements used to describe a HMM.

The number of states, N, is the number of distinct, finite

states in the model. Individual states are represented as

𝑆 = {𝑆1, 𝑆2, … . . , 𝑆𝑁}

The current state is 𝑞𝑡. Although the observations in hidden

Markov models can be continuous random variables, in this

paper only behavior models that have a finite number of

discrete observation symbols are considered. Therefore, M

is the number of possible observation tokens in the model

where the individual observation tokens are denoted as

𝑉 = {𝑣1, 𝑣2, … . , 𝑣𝑀}.

The state transition matrix

𝐴 = {𝑎𝑖𝑗}

is the 𝑁 × 𝑁 matrix of probabilities of switching from one

state to another, i.e.

𝑎𝑖𝑗 = 𝑃(𝑞𝑡+1 = 𝑆𝑗| 𝑞𝑡 = 𝑆𝑖).

It should be noted that HMMs assume the system follows

the Markov, or memoryless property. The assumption of the

Markov property is that the probability of the next state of

the system depends only on the current state of the system

and is independent of the previous system states, i.e.

𝑃(𝑞𝑡 = 𝑆𝑗|𝑆1, 𝑆2 …𝑆𝑡−1) = 𝑃(𝑞𝑡 = 𝑆𝑗|𝑆𝑡−1)

The observation symbol probability distribution for

state j is

𝑏𝑗(𝑘) = 𝑃(𝑣𝑘 𝑎𝑡 𝑡𝑖𝑚𝑒 𝑡 | 𝑞𝑡 = 𝑆𝑗)

𝑓𝑜𝑟 1 ≤ 𝑗 ≤ 𝑁 𝑎𝑛𝑑 1 ≤ 𝑘 ≤ 𝑀

In other words, the probability of seeing observation

symbol 𝑣𝑘 at time t, given the state at time t is 𝑆𝑗. In the

discrete model, this yields an 𝑁 ×𝑀 matrix of observation

probabilities.

The final element of HMMs is the initial state

distribution,

𝜋𝑖 = 𝑃(𝑞1 = 𝑆𝑖) 𝑓𝑜𝑟 1 ≤ 𝑖 ≤ 𝑁.

The entire model can be specified by the three probability

measures A, B, and π so the concise notation for the model

𝜆 = (𝐴, 𝐵, 𝜋)

is used. The goal is to determine if a series of observations

𝑂 = 𝑂1𝑂2 …𝑂𝑇

was generated by a behavior modeled as λ = (A,B,π), where

each Ot is an observation symbol in V. Throughout this

paper the ‘agent’ is the dynamic object that is being

observed and whose behavior is to be determined. The

agent could be a human, robot, or moving obstacle. The

‘robot’ is the observer trying to determine the agent’s

intent.

In typical applications, observation symbols for HMMs

are measured at regular time intervals. To take advantage of

the discrete nature of the models used here and reduce the

length of the observation sequences (and therefore the

required computation), observations symbol generation is

event-based. Only when an event triggers the value of an

observation to change is a symbol generated. The subscript

t is used to denote the sequential time step, but the temporal

spacing between observations can be highly irregular.

BEHAVIOR PREDICTION Using the form of the behavior model described above,

models can be trained to match observations generated

from a behavior of interest. While there is no analytical

solution for optimizing the model parameters, λ = (A,B,π), to

maximize P(O|λ), iterative techniques can be used to find a

local maxima. A modified version of Expectation-

Maximization known as the Baum Welch algorithm is a

commonly used method of doing this and is the method

used in this study. Since the solution is only a local

maximum, care must be taken in selecting initial

conditions. Once the model is trained on a series of

observation sequences, the model can be used to calculate

the likelihood of a new sequence being generated by the

model. This is where the strength of HMMs lie, since by


using dynamic programming techniques this can be

calculated in just O(N2T) calculations. To do so, a variable

known as the forward variable is defined:

𝛼𝑡(𝑖) = 𝑃(𝑂1𝑂2 …𝑂𝑡 , 𝑞𝑡 = 𝑆𝑖|𝜆)

where 𝛼𝑡(𝑖) is the joint probability of the observation

sequence up to time t and the state being 𝑆𝑖 at time t, given

the model, λ. The probability of the entire sequence given

the model, 𝑃(𝑂|𝜆), can be found inductively in three steps

as follows

1) 𝛼1(𝑖) = 𝜋𝑖𝑏𝑖(𝑂1), 1 ≤ 𝑖 ≤ 𝑁

2) 𝛼𝑡+1(𝑗) = [∑𝛼𝑡(𝑖)𝑎𝑖𝑗

𝑁

𝑖=1

] 𝑏𝑗(𝑂𝑡+1),

1 ≤ 𝑡 ≤ 𝑇 − 1 1 ≤ 𝑗 ≤ 𝑁

3) 𝑃(𝑂|𝜆) = ∑𝛼𝑇(𝑖)

𝑁

𝑖=1

Here, the forward variables are first initialized as the joint

probabilities of the state 𝑆𝑖 and the observation 𝑂1. To

calculate the next forward variable, the previous forward

variables are multiplied by the transition probabilities and

summed up (to find the probability of transitioning to state

𝑆𝑗) before being multiplied by the next observation

probability. Finally, by summing up the forward variables

over all the states at the final time step, the state variable is

marginalized out and the probability of the entire

observation sequence is obtained. In the same manner, by

summing up the forward variables at the current time step,

the probability of the current partial observation sequence

can be found.

If the goal is to discriminate between a finite set of 𝑍

known independent behaviors, modeled as 𝜆𝑖 , 1 ≤ 𝑖 ≤ 𝑍, then the probability can be normalized as

P(𝑂|𝜆𝑖) = ∑ 𝛼𝑇(𝑖)

𝑁𝑖=1

∑ ∑ 𝛼𝑇𝑗(𝑖)𝑁

𝑖=1𝑍𝑗=1

so that ∑ 𝑃(𝑂|𝜆𝑖) = 1𝑍𝑖=1 and the most likely behavior is

simply

argmax1≤𝑖≤𝑍

𝑃(𝑂|𝜆𝑖).

This same process can also be done online on partial

observation sequences so that the most likely occurring

behavior can be updated after every time step.

While the above procedure is optimal when models for

all possibly occurring behaviors are available (and are

independent of one another), this is rarely the case. Also, it

would be beneficial to be able to add or remove behaviors

without modifying software and to be able to determine if

none, or multiple, of the modeled behaviors are occurring.

Therefore, a novel method of obtaining the likelihood of

behaviors is introduced.

In itself, the probability of a partial observation

sequence for a given model does not convey very much

information. This is because, depending on the number of

possible observation sequences, even the most likely

sequence of observations can have a very low probability.

Therefore, in order to determine the likelihood of a

behavior model given a partial observation sequence, the

probability of the current observation sequence is

normalized with respect to the probability of the most

likely observation sequence of equal length. Then, that

likelihood is scaled by the predicted fraction of the

behavior that has been observed.

L(𝜆𝑖|𝑂1𝑂2 …𝑂𝑡) = ∑ 𝛼𝑇(𝑖)

𝑁𝑖=1

max𝑂

P(𝑂1 𝑡|𝜆𝑖)

𝑡

𝑇𝑖

Behaviors are not assumed to be of equal length;

therefore, the length of an observation sequence

corresponding to behavior i is denoted as 𝑇𝑖 . For each

behavior, the normalizers were pre-computed as

max𝑂 𝑃(𝑂1 𝑡|𝜆𝑖) , 𝑓𝑜𝑟 1 ≤ 𝑡 ≤ 𝑇𝑖 .

Figure 1. Aerial view of the simulated office environment.


EXPERIMENTAL SET UP The behavior models described above were tested in

realistic simulations using the open source software ROS

(Robot Operating System) and Gazebo, an open source

multi-robot simulator. The simulations were conducted

using two Turtlebots in a simulated office environment,

shown in Figure 1 with the observing robot toward the

center of the figure and the robot representing the dynamic

agent toward the top of the figure. The following sections

describe the simulation environment and the software

implemented in order to test the behavior models presented

in the previous section.

Simulation Environment

The software used for the experimental simulations,

Gazebo, accurately simulates rigid-body physics in a 3D

environment. It also generates realistic sensor feedback and

interactions between three-dimensional objects. An aerial

view of the office environment used for the simulations is

shown in Figure 1 above. The observations for the behavior

models were generated using the robot’s planar laser range

finder. A visualization of the laser range finder’s output is

shown in Figure 2. The robot is facing the direction

corresponding to up in Figures 1 and 2, and the dynamic

agent as seen by the robot can be seen in Figure 2. During

the simulations, the sensor data generated by the simulator

was sent to ROS for processing and running the behavior

recognition algorithms, all in real-time. Both Turtlebots

were equipped with an inertial measurement unit and

encoders for measuring wheel odometry. All sensor data

(laser, IMU, and wheel encoders) had added Gaussian noise

to mimic real-world uncertainty.

ROS Software

While some software for operating Turtlebots is

available open source, all the software for the behavior

recognition algorithms was developed, along with the

necessary peripheral programs, in C++. The observing

robot tracked the dynamic agent by looking for the known

robot geometry in each laser range scan. The partial view of

the circular robot in the office environment can be seen in

the visualization of the laser range scan in Figure 2. The

maximum range of the laser range finder was 10 meters and

the tracker could identify the robot at a range of up to 7.5

meters. If the robot was identified in a scan, the robot's

absolute x-y coordinates were calculated and used as input

to a Kalman filter which estimated the velocity. Using the

position and velocity information as input to the behavior

models, the likelihood of each models occurrence was

calculated. The dynamic agent executed behaviors using a

PID controller using odometry data as feedback. The

odometry data was obtained via an Extended Kalman Filter

that fused wheel encoder data with IMU data. A program

for generating observations read in the position and

velocity of the dynamic agent and published observations

when events triggering them occurred. Then behavior

recognizers for each of the modeled behaviors determined

the likelihood of their respective behavior and output the

data in real time.

RESULTS To test the behavior models, six behaviors based on

polygonal trajectories were used. The observations used in

the models for these trajectories consisted of the change in

velocity direction of the dynamic agent. Examples of the

trajectories are shown in Figure 3. Going clockwise from

Figure 2. Visualization of a laser scan of the environment containing a dynamic agent.

Figure 3. Example trajectories from the six tested behaviors.


top-left in Figure 3, the behaviors are referred to as

rectangle, triangle, convex box, concave box, trapezoid,

and hourglass.

The tests performed consisted of having the dynamic

agent execute each of the six behaviors 10 times while

behavior recognizers for all six of the models were running.

Each one of the behaviors was randomized in three ways.

First, the dynamic agent executed the trajectories in

different orientations every time by randomly selecting an

initial direction. Second, the side lengths of the polygonal

trajectories were varied by randomly selecting a scaling

factor between 0.5 and 1.5. Finally, the trajectories were

executed in both clockwise and counter-clockwise

directions. In addition, the dynamic agent executed the

behaviors based on a PID controller receiving feedback

from noisy sensors, increasing the variance in trajectories

for the same behavior.

The average outputs of the behavior recognizers for

each of the six behaviors are graphed in Figures 4-9 as

functions of the percent of the behavior executed. The

percent of a behavior executed was determined by average

distance traveled at the time of the output divided by the

total trajectory length. For clarity, only the data for

behavior models having non-zero likelihood are shown. In

half the cases, the true behavior has the highest likelihood

starting from the first observation. In every case, the true

behavior has the highest likelihood by the time 40% of the

behavior has been executed.

Figure 4. Experimental results for the trapezoid behavior.

Figure 5. Experimental results for the rectangle behavior.

.

Figure 6. Experimental results for the triangle behavior.


CONCLUSIONS AND FUTURE WORK This paper presented a method of autonomous

behavior prediction based on hidden Markov models. A

procedure for performing the online classification problem

of selecting the most likely behavior from a set of mutually

exclusive behaviors was introduced. Next, the procedure

was extended in order to determine the individual

likelihood of behaviors for the problem of having an

incomplete set of potentially dependent behaviors. The

method uses event-based observation models and measures

the similarity between the current observations and the

‘ideal’ observations for a given behavior. The models were

tested in simulation using two robots in a static

environment. The results showed very good prediction of

the simple behaviors tested. All models output the highest

likelihood for the true behavior by the time 40% of the

behavior was executed, and in five out of 6 behaviors,

before 25% of the behavior was executed.

In future work, more complex behaviors with realistic

applications will be modeled. The models will be expanded

to include continuous observation densities, and the models

will be tested on physical hardware as well as in

simulation.

REFERENCES [1] Baum, L. E. (1972). An equality and associated

maximization technique in statistical estimation for

probabilistic functions of Markov processes.

Figure 7. Experimental results for the hourglass behavior.

Figure 8. Experimental results for the convex box behavior.

Figure 9. Experimental results for the concave box behavior.


Inequalities, 3, 1-8.

[2] Baum, L. E., Petrie, T., Soules, G., & Weiss, N. (1970).

A maximization technique occurring in the statistical

analysis of probabilistic functions of Markov chains. The

annals of mathematical statistics, 164-171.

[3] Bakis, R. (1976). Continuous speech recognition via

centisecond acoustic states. The Journal of the Acoustical

Society of America, 59(S1), S97-S97.

[4] Salvucci, Dario D. "Inferring driver intent: A case study

in lane-change detection." Proceedings of the Human

Factors and Ergonomics Society Annual Meeting. Vol. 48.

No. 19. SAGE Publications, 2004.

[5] Pentland, A., & Liu, A. (1999). Modeling and prediction

of human behavior.Neural computation, 11(1), 229-242.

[6] Han, Kwun, and Manuela Veloso. "Automated robot

behavior recognition." ROBOTICS RESEARCH-

INTERNATIONAL SYMPOSIUM-. Vol. 9. 2000.

[7] Bennewitz, Maren, Wolfram Burgard, and Sebastian

Thrun. "Using EM to learn motion behaviors of persons

with mobile robots." Intelligent Robots and Systems, 2002.

IEEE/RSJ International Conference on. Vol. 1. IEEE, 2002.

[8] Jelinek, F., Bahl, L., & Mercer, R. (1975). Design of a

linguistic statistical decoder for the recognition of

continuous speech. Information Theory, IEEE Transactions

on, 21(3), 250-256.

[9] Jelinek, F. (1976). Continuous speech recognition by

statistical methods. Proceedings of the IEEE, 64(4), 532-

556.

[10] Kelley, R.; Nicolescu, M.; Tavakkoli, A.; King, C.;

Bebis, G., "Understanding human intentions via Hidden

Markov Models in autonomous mobile robots," Human-

Robot Interaction (HRI), 2008 3rd ACM/IEEE

International Conference on , vol., no., pp.367,374, 12-15

March 2008

[11] Lattner, Andreas D., et al. "Sequential pattern mining

for situation and behavior prediction in simulated robotic

soccer." RoboCup 2005: Robot Soccer World Cup IX.

Springer Berlin Heidelberg, 2006. 118-129.

[12] Thrun, S., Burgard, W., & Fox, D.

(2005). Probabilistic robotics (Vol. 1). Cambridge, MA:

MIT press.

[13] Rabiner, Lawrence R. “A tutorial on hidden Markov

models and selected applications in speech recognition.”

Proceedings of the IEEE. 1989, 257–286.

[14] Fox, M., Ghallab, M., Infantes, G., & Long, D. (2006).

Robot introspection through learned hidden markov

models. Artificial Intelligence, 170(2), 59-113.

[15] Liu, Yanming, and Karlheinz Hohm. "Discrete hidden

Markov model based learning controller for robotic

disassembly." Proceedings of the international ICSC/IFAC

symposium on neural computation (NC’98), Vienna. 1998.

[16] Yang J and et al. Hidden markov model approach to

skill learning and its applications to telerobotics. IEEE

Transactions on Robotics and Automation, 10(5):621–631,

October 1994.




NEAR REAL-TIME TELEOPERATION CONTROL OF THE STÄUBLI TX40 ROBOT

Tom Trask University of North Florida Jacksonville, Florida USA

Arnold Cassell Seagate

Minneapolis, Minnesota USA

Dang Huynh University of North Florida Jacksonville, Florida USA

Daniel Cox University of North Florida Jacksonville, Florida USA

ABSTRACT The Stäubli TX40 robot is an industrial robot with motion

control coordinated and accomplished through a programming console and a pendant interfaced to the robot. Neither of these interfaces is suitable for teleoperation control, which is a transparent intuitive human user interface to seamlessly control the motion of the robot’s end effector. User control of the robot motion is enabled through a custom-developed, network-based communications system architecture. The system architecture is demonstrated through implementation of two commercial off-the-shelf user input devices used for teleoperation.

The custom-developed architecture binds the movements actuated from the user to the robot. The architecture allowing the user to control the data speed and motion sensitivity for the six motion axes is contained in the implementation of the architecture. Commands of the architecture are sent via the internet to the robot controller, and read through the communications features of the robot. The commands are developed to provide the motion control including input filtering, communications protocol, and motion calculations for the coordinated six degree-of-freedom teleoperated motion control of the robot. The telerobotic motion commands are decoded and translated into motion commands customizing the software of the commercial industrial robot to accomplish user-transparent spatial motion control.

INTRODUCTION Teleoperation involves the physical operation of machinery

using a remotely controlled interface. Telerobotics involves the teleoperation of robot manipulators. Teleoperation is important for multiple reasons, and has received much attention. Early attention has been motivated by space exploration. In [1], salient problems in the supervisory control using telerobotics are outlined, while hardware implementations are addressed in [2,3]. A telerobotic system may also have force reflection in which the operator kinesthetically feels the force exerted at the end effector of the robotic manipulator [4,5,6]. Stability in

band-limited behavior is described in [7]. A well known contributing stability issue for telerobotics is time delay [5,8,9] while this problem is pervasive due to communication time lag [9]. Mobile robots present another application domain for telerobotics [10] as well as telerobotics in ocean environments [7]. Strategies to accomplish force reflection with time delay consideration and issues are also studied [6,11,12].

Applications in telerobotics continues, as the maturation of robotic technology allows for systems to be applied to hazardous environments and material handling [13,14], nuclear disasters using the internet [15], and evolving space systems [16], to be operated remotely and efficiently. Teleoperation is also being pursued in the health industry in an effort to increase the mobility of handicapped patients using assistive robotics.

Available teleoperation technologies are not intuitively easy to use; a goal of this research is to develop an easier-to-use interface architecture for robotic teleoperation using simple available off-the-shelf input devices. This paper describes a teleoperation interface architecture making use of off-the-shelf user input devices and integrated with an industrial robot. Development of the architecture to capture user input and the introduction of the custom-developed graphical user interface is described in the next section. Section 3 describes the communication structure used to transmit the commands using the architecture. Section 4 describes the architecture of the software written with the robot controller, including the working modes developed for teleoperation. An outline of future work using additional off-the-shelf components associated with this effort is presented in Section 5, followed by a conclusion.

CUSTOM-DEVELOPED GRAPHICAL INTERFACE A custom developed, ease-of-use, interface for robotic

teleoperation architecture binds the off-the-shelf user input devices, with easily settable features, to the robot controller [17]. The custom-developed user interface interfaces to a commercial industrial Stäubli TX40 robot (also referred to as


the TX40). The architecture is more fully described below with later sections discussing extensions using additional off-the-shelf devices. The custom graphical user interface is created in an architecture for use with the Windows operating system in order control the stream of data being collected and sent to and from the robotic controller. This custom-developed interface, is referred to as the Stäubli Control Panel (SCP) because it was developed for the Stäubli TX40 robot [18]. The TX40 industrial manipulator is controlled by the Stäubli model CS8C robot controller [18], referred to as the robot controller, which is a component of the robot system supplied with the robot. This system also contains a teach pendant component useful for operation and programming, but not suitable for teleoperation. The robot is programmed using VAL3 [18]. VAL3 handles many robot functions including forward and reverse kinematics which are controlled automatically by the robot controller. The normal operational mode robot controller is to guide the arm through a preset or trained path, using a FIFO motion stack to queue movement commands in an interpretive fashion.

While useful for predetermined tasks, this preprogrammed sequencing mode of operation poses difficulty for teleoperation of the robot. The stack of queued commands awaiting execution is not suitable for the continuously varying immediate needs of a user in a telerobotic mode of operation. Most importantly, the user intent is not necessarily predetermined under teleoperation conditions as it is in a structured industrial robotic application.

The SCP architecture is developed to be compatible with off-the-shelf devices. Two off-the-shelf user input devices, the SpaceNavigator and the P5 Extended Reality Glove [19, 20] are implemented with the SCP. An example of the custom-developed SCP display is shown in Figure 1. The development features of the SCP are further described below.

Figure 1. Custom Developed SCP The unique customization of VAL3 commands used in the

robot controller to work effectively with the SCP is also more fully described in later sections of the paper. Positional data is captured from the user input devices, as well as rotation information of rotation about three axes, also from the user input devices. The SCP has four panels. The position data is displayed continuously to the user in the Motion Control Panel of the SCP. The Server Control and Client Control Panels of the

SCP monitor the communications to and from the robot controller. All panels as well as interfaces to and from the SPC are custom developed for the SPC. Informational messages are also displayed in the respective panels of the SCP.

The Diagnostics Panel of the SCP allows the operator to independently activate motion axes. The operator is also able to control the frequency that command messages are sent, as well as scaling the values of the movement commands being sent through the features of the SCP.

The SCP accepts input from one of two off-the-shelf input devices. The Essential Realities P5 extended reality glove is referred to as the P5 [19, 20]. The P5 consists of a plastic, artificial glove which the user slips onto their hand, and a tower that receives six degree-of-freedom (DOF) trajectory data from infrared LEDs mounted onto the glove. Using 2 banks of infrared receivers as depicted in Figure 2, the tower can calculate the positional coordinates of the glove at any given time. Once the user ensures that the glove is on their hand securely, the glove is powered on, is confirmed to be within range of the tower, and then the user simply needs to move their gloved hand naturally to have its motion mimicked by the TX40 via the SCP.

Figure 2. P5 Glove-Tower Interaction

The SCP also interfaces with another off-the-shelf input

device, the 3DConnexion’s SpaceNavigator six DOF mouse, referred to as the SpaceNavigator [21]. The SpaceNavigator is a puck-shaped motion input device that rests on top of a base. The SpaceNavigator can be pressed left, right, forward, backward, up, and down along translational axes, and has the ability to be twisted/rotated about three rotational axes. To operate the TX40 with this device via the SCP, the user should move/rotate the SpaceNavigator in a desired direction to have the TX40 move in a similar manner. The more forcefully that the SpaceNavigator is actuated, the faster the robot manipulator will respond. Both the P5 and SpaceNavigator communicate with the host PC via a standard USB connection which is used by the SCP.

When the SCP is initialized, it automatically begins looking for either the P5 or the SpaceNavigator [21,22-25]. When found, motion commands from either connected device are collected and displayed in real time in the Motion Control Panel of the SCP. Once the TX40 has connected to the SCP’s server, the SCP sends encoded versions the motion commands to the robot controller via tcp/ip for processing by the robot


control software. This is discussed in the next section. The custom-developed SCP program flow is shown in Figure 3.

Figure 3. SCP Operational Flow Diagram

The SCPs Diagnostics Panel can be used to meter how often and to what scale the collected motion commands are sent to the robot controller. This panel also is used for enabling and disabling the sending of motion data for any of the six available axes (X, Y, Z, pitch, roll, yaw). The SCP also supports sending the robot controller VAL3 commands directly, via the Client Control Panel.

COMMAND STRUCTURE OF THE SCP Commands consist of two types, control type and motion

type. Control commands are used to toggle working modes in the robot controller or to activate the end effector. These command words are sent in plain text through tcp/ip communications.

The command to close the end effector is simply “grip,” while the command to open the end effector is simply “release.” Move Modes and Step Modes may also be toggled with plain text command words to generate movement. The movement modes are discussed in the next section.

Motion commands are sent as relative positional data, whereby each motion command is the relative change in position from the last motion command sent. These commands are formatted in a vector and resent in plain text. Translational motion along the X, Y, and Z axes are sent first, followed by standard respective rotations about the X, Y, and Z axes (roll, pitch, yaw). A motion command may take on the following form of a string of numbers, for example:

-1.3 7.12 0.01 2.1 0.0 0.101

This would be read in as “-1.3 along the X axis, +7.12 along the Y axis,” and so on. The translational units are millimeters and angles are in degrees as is standard with the TX40. As mentioned above, these are relative to the last known position.

Commands of all types are appended with a carriage return, which signifies the end of the message as in standard messaging structures. This carriage return character is discarded

before processing motion commands. The robot controller also sends messages back to the Control Panel of the SCP for verification and acknowledgement. These standard messages contain the absolute position of the end effector relative to the origin of the workspace. Six values are sent from the robot controller to the SCP in the same order and format as the motion commands shown above, for example:

210 15.1 -1.2 0.5 179.8 2.2

The X, Y, and Z values are millimeters of absolute displacement from the origin, whereas the roll, pitch, and yaw, Rx, Ry, and Rz values are provided in degrees.

VAL3 PROGRAM STRUCTURE The robot controller program to accept commands from the

SCP is written in the VAL3 programming language, which is standard with the TX40 robot [18]. The programs are designed and customized to work effectively with the SCP. VAL3 is a multitasking language. This allows for easy creation of multiple threads, or processes, to handle asynchronous events simultaneously, one of the very useful programming features of VAL3 [26].

The main VAL3 programs designed for and used by the SCP are the “tcpLink”, which receives and reformats all incoming commands, “catch,” which handles any events generated by those commands other than movement, “calculate,” which processes the movement vectors, and “move” which constantly adjusts the position of the arm. These commands are implemented in VAL3 and customized to work with the custom-developed SCP. The “user” program in the robot controller monitors the performance of the system and provides message feedback displayed on the SP1 Manual robot Control Pendant (MCP) of the TX40 [18]. The VAL3 program flow is shown in Figure 4.

Figure 4. VAL3 Program Information Flow

In one thread of control, the VAL3 program in the robot controller listens for a relative motion command from tcp/ip. The thread verifies that the new command when added to the current destination, checks that the command will allow the manipulator to remain in the workspace, and then adds that new command to the current destination if acceptable.


In a separate, simultaneous thread of control the VAL3 program in the robot controller compares the current position to the destination, and takes an incremental step towards the destination. Thus the move is parsed into intermediate movements of the manipulator.

These are kept as separate threads of control so that they execute only when needed. The “move” program executes continuously during operation, and constantly checks the robot manipulator position relative to the goal destination. The other commands in the controller are always running, but wait for user actions for triggering before any required calculations or actions take place.

MANUAL CONTROL PENDANT (MCP) The Stäubli SP1 Manual robot Control Pendant (MCP) is shown in Figure 5 [18]. The MCP is a component of the robot system. The pendant is programmable, and to some extent can be used to operate the robot in a remote mode albeit in a crude fashion. The MCP is not designed for teleoperation per se. The use of the pendant is most suitable to be used by a factory floor robot operator, and for teaching coordinate locations to the robot controller by the robot programmer for factory floor applications.

Figure 5. SP1 Manual Control Pendant (MCP) [18]

As dictated by the design of the robot system, the MCP is the only device capable of operating the manipulator outside of the programming environment for the robot system provided in its standard configuration. It is used to load programs from within the robot controller, and to control their execution. The MCP also provides maintenance and supervision functions. For this application, once the application has been started, only the screen is used to state message feedback to the user. An example of an MCP screen is given in Figure 6.

Figure 6. MCP Message Feedback Example

The MCP is not suitable for teleoperation, thus the motivation to create the custom-developed SCP containing interfaces to off-the-shelf user input devices. Note that the Motion Control Panel of the SPC displays information that is possible to send to the display of the MCP, however, the SCP also simultaneously captures user input commands for teleoperation among other features.

MOVE MODES USING VAL3 The movement modes are performed using the VAL3

software in the architecture of the robot controller. The “calculate” function receives the transform from “tcpLink” and tests this new destination before any changes in actual motion are processed. There are three methods of applying the transform, two of the Move Modes correspond to the functionality of VAL3.. The first is Tool Mode, in which rotation and associated motion are processed about the current position of the end effector. Alternately, in Origin Mode, rotation and displacement are relative to the origin of the workspace, which is located at the center of the TX40. These modes are inherent to the robot and VAL3 controller and adapted to SCP teleoperation interface.

Lastly, Mixed Mode allows displacement to be relative to the origin, but rotation to be measure relative to the tooling point of the end effector. This last custom-designed mode best matches user expectations of motion and thus is best suited to the teleoperation interface. This is a hybrid of modes inherent to the normal use of the robot and VAL3 and custom-developed for the teleoperation of the TX40 and for use by the SCP.

WORKSPACE BOUNDARIES OF THE TX40 In order to limit the potential for the arm to overreach its

inherent motion limitations, an algorithm ensures the test destination is allowable. The “pTest” command provides insurance that the motion command exists within set boundaries of the robot’s workspace to allow for safe operation. The boundaries of the allowable workspace are illustrated in Figure 7.


Figure 7. Workspace Boundaries

The program architecture will not allow a destination point that is below the safe working floor. This is called the “zFloor”. The program will also not allow command of a point that is outside the reach of the manipulator, or too close to the main torso of the manipulator which contains the first joint of the manipulator. Any such movement commands are instantly discarded by the SCP and not attempted.

STEP MODES USING VAL3 The Step Mode is also performed using the VAL3 software

in the architecture of the robot controller. The “move” program is responsible for bringing the current position of the manipulator closer to the destination provided by the “calculate” program. The standard method of controlling the manipulator position is asynchronous in the robot controller. Commands enter a stack, where they are executed sequentially, but they are not interruptible under normal operation, nor are they responsive to the immediate needs of an operator. The motion stack is typically used in a repetitive motion sequence as found in an industrial robot application.

Therefore, the motions in the program that integrates to the SCP are segmented into smaller goals. The function of the "move" program is to compare the manipulator's current position with the goal destination, and then calculate an intermediate closer goal. The closer goal is sent to the robot controller, and the "move" function waits until the manipulator is a certain distance away from that goal before calculating a new goal. That distance (known internally in VAL3 as the blendSize) is adjustable and is referred to as a Blend Mode. When set to zero, the arm will make a full and complete stop at every interconnected goal along the path. A graphical representation of the Step Mode and the Blend Mode are illustrated in Figures 8 and 9.

Figure 8. Step Mode

Figure 9. Blend Mode.

The parameters of these calculations are fully adjustable from within the program and have been tested through the use of the SCP. They are currently not visible to the teleoperation user but may be made available to the operator in the future. The total delay in the system can be up to nearly 350 milliseconds in the worst cases, but the response often faster with much less noticeable. It is well-known that delay is an implementation issue in telerobotics, and the SCP also must overcome this issue to provide truly seamless teleoperation. Although the worst case of delay in the current implementation is beyond the expectation for real-time control, the system provides a reasonable near real-time teleoperated system using low-cost, off-the-shelf (OTS) devices under many conditions. Regardless, the features of the architecture using the custom-developed SCP are far superior to the pendant supplied with the robot system which is unsuitable for telerobotics.

It is found that the SpaceNavigator input device is less problematic in complex manipulator configurations compared to the P5, although execution speed is comparable using either device. Adjusting the parameters for blending to smooth some of the jitter in the manipulator’s motion is likely to improve performance, although full real-time issues with delay still exist in the current implementation. Also, motion commands which


are purely rotational may present a problem due to kinematic closure and workspace constraints as it is handled currently. As long as the destination is distant from the current position, meaning that the delay before adding another command to the stack does trigger, then this is less problematic. When that delay is not triggered, many commands fill the stack and delay responsiveness. A method for calculation of the robot motion needed to complete a move command, rather than the distance the end effector will travel, is also being developed to help overcome delay issues. Another issue with one of the devices, the P5 glove, is that it does not effectively convey rotational data as a relativistic measure of its current position. Efforts are being made to compensate for this issue mentioned above. The SpaceNavigator more effectively measures affective change in rotation but is significantly less intuitive for teleoperation use when compared to the P5 glove.

ALTERNATIVE OTS APPROACHES Alternative off-the-shelf (OTS) input devices are being

explored as well. Incorporation of additional alternative OTS input devices, either to move the arm, or to provide insight into the operator's state, are being pursued. Emerging new technology, such as the LeapMotion controller [27], which is able to see objects in its path in 3D space, may allow the TX40 to be operated without needing any type of wearable apparatus, much like the Microsoft Kinect. The use of an Inertial Measurement Unit (IMU) such as the InvenSense IMU-3000 [28], which records both gyroscopic and accelerometer data in real-time, could be used to create a much more user-friendly means of gathering 3D positional and acceleration data [29].

Using both the IMU-3000 and the Arduino Lilypad [30], it is believed that an apparatus can be fashioned directly onto a normal glove, resulting in a more-natural end-user interface. Low-profile “bend sensors”, flexible resistance-based sensors can readily be fashioned out of conductive thread and fabric [31]. Communication between the glove and a host device can be facilitated using the readily-available wireless communication framework. Figure 10 depicts such an interface.

Figure 10. Prototype IMU-Arduino Glove-PC [32]

It is postulated that both the use of the LeapMotion and IMU-based glove will alleviate the issue the TX40 currently has with effectively calculating rotational motion in all cases compared to the P5. Due to the workspace constraints, one problem in the current implementation of the SCP is motion

halting due to internal workspace boundaries. This tends to be more pervasive using the P5. Also, an application of this technology would be to allow a user to more easily operate the robot remotely via the internet. Due to the simplicity of the commands being sent by the SCP, the user should experience minimum performance degradation when using the SCP.

CONCLUSION The TX40 industrial robot can be controlled in a relatively

synchronous fashion by constantly monitoring where the manipulator is, and where the operator expects the it to be, and sending intermediate adjustments at regular intervals using a custom-developed SCP. With this communication architecture, simple but effective off-the-shelf devices are capable of use as teleoperated input devices for the industrial manipulator. Several operating modes are made available for different tasks, and the ability to switch modes or isolate motion along a particular axis is provided and makes use of VAL3 programming features. A user interface is developed that features the custom-developed SCP to aid the teleoperation user of the robot. The system functions well for telerobotics with two user input devices, however some implementation issues remain. Regardless, the SCP provides teleoperation of the manipulator well beyond the standard configuration of the robot and its pendant.

Several features of the arm control calculation could be modified based on user factors introduce many interesting and exciting prospects. Future releases of VAL3 and feature add-ons are also planned to be used to advance the teleoperation features of the system. Additional OTS devices are also under consideration for development and integration in the SCP architecture.

REFERENCES [1] Sheridan, T. 1986, “Human Supervisory Control of Robot

Systems,” IEEE International Conference on Robotics and Automation, pp. 808-812.

[2] Bejczy, A. and Szakaly, Z., 1987, “Universal Computer Control System (UCCS) for Space Telerobotics,” IEEE International Conference on Robotics and Automation, pp. 443-449.

[3] Hirzinger, G., 1987, “The Space Telerobotic Concepts of DFVLR Rotex,” IEEE International Conference on Robotics and Automation.

[4] Hannaford, B. 1989, “A Design Framework for Teleoperators with Kinesthetic Feedback, 1989, IEEE Transactions on Robotics and Automation, 5(4), pp. 426–434.

[5] Anderson, R., and Spong, M., 1989, “Bilateral Control of Teleoperators with Time Delay,” IEEE Transactions on Automatic Control, 34(5), pp. 494–501.

[6] Chopra,N., Spong, M., Ortega, R. and Barabanov, N., 2004, “ On position tracking in Bilateral Teleoperation,” Proceedings American Controls Conference, pp. 5244–5249.


[7] G. Niemeyer and Slotine, J. 1991, “Stable Adaptive Teleoperation,” IEEE Journal of Oceanic Engineering, 16(1):152–162, January 1991.

[8] Munir, S. and Book, W., 2003, “Control Techniques and Programming Issues for Time Delayed Internet Based Teleoperation,”ASME Journal of Dynamic Systems, Measurement, and Control, 125(2):205–214.

[9] Kosuge, K., Itoh, T., and Fukuda, T., 1996, “Scaled Telemanipulation with Communication Time Delay,” Proceedings International Conference on Robotics and Automation, pp. 2019–2024.

[10] Schilling, K, and Roth, H., 1999, “Control Interfaces for Teleoperated Mobile Robots,” Proceedings IEEE International Conference on Emerging Technologies and Factory Automation, pp.1399–1403, October, 1999.

[11] Eusebi, A. and Melchiorri, C., 1998, “Force Reflecting Telemanipulators with Time-delay; Stability, Analysis and Control Design,” IEEE Transactions on Robotics and Automation, 14(4) pp. 635–640.

[12] Alise, M., Roberts, R., and Repperger, D., 2006, “The Wave Variable Method for Multiple Degree of Freedom Teleoperation Systems with Time Delay,” Proceedings IEEE International Conference on Robots and Automation.

[13] Cox, D., Legault, J., Turner, C., and James, C., 1999, “Requirements for Small Automation Systems in Glovebox Environments,” Amarillo National Resource Center for Plutonium Researchers’ Conference, pp. 49-51, Amarillo National Resource Center.

[14] Cox, D., Cetin, M., Pryor, M., and Tesar, D., 1999, “Requirements for Modular Dual-Arm Robot Architecture for Use in Deactivation and Decommissioning Tasks,” American Nuclear Society’s 8th International Topical Meeting on Robotics and Remote Systems, Pittsburgh, pp. 0904 1-14, American Nuclear Society Press, La Grange Park, 1999. ISBN 0-89448-647-0.

[15] Cragg, L., Hu, H., 2003, "Application of Mobile Agents to Robust Teleoperation of Internet Robots in Nuclear Decommissioning", 2003 IEEE International Conference on Industrial Technology, pp. 1214-1219.

[16] Robonaut, 2013, available: http://robonaut.jsc.nasa.gov/ [17] Cassell, A., Trask, T., Huynh, D., and Cox, D.,

2012,“Teleoperated Interface for the Staubli TX40 Robot,” Proceedings of 25th Annual Florida Conference on Recent Advances in Robotics.

[18] Stäubli , 2012, available: http://www.Stäubli.com/ [19] Cyberworld, 2012, “ Virtual Reality Solutions - P5 Glove -

Virtual Reality Data Glove," available: http://www.cwonline.com/store/view_product.asp?Product=1179

[20] P5, 2012, "P5 Glove Community - Yahoo Groups," Yahoo, 2009, available: http://tech.groups.yahoo.com/group/p5glove/ [Accessed 2012].

[21] 3Dconnexions-TDxInput, 2012, “3Dconnexions - Using TDxInput," 3DConnexions, available: http://www.3dconnexion.com/forum/viewtopic.php?p=221

3#2213. [22] Parkhurst, P., 2012, "Getting Data from the P5 data glove,"

available: http://thirtysixthspan.com/P5/index.cgi. [23] Robotgroup, 2012, "Robot Group - P5 Glove," available:

http://www.robotgroup.org/moin.cgi/P5Glove. [24] 3Dconnexions-SpaceNavigator, 2012, "3Dconnexions:

SpaceNavigator," 3Dconnexions, available: http://www.3dconnexion.com/products/spacenavigator.html

[25] "C# Marshalling Data with Platform Invoke," available: http://msdn.microsoft.com/en-us/library/fzhhdwae.aspx.

[26] Stäubli, 2010, "VAL3 Reference Manual Version 6.1," Stäubli.

[27] LeapMotion, 2012, "LeapMotion Controller", available: https://www.leapmotion.com/

[28] InvenSense, 2012, "InvenSense ITG-3200 Product Specifications,"Sparkfun Inc., https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf

[29] Madgwick, S., Harrison, A., Vaidyanathan, R., 2011, "Estimation of IMU and MARG Orientation using a Gradient Descent Algorithm,", 2011 IEEE International Conference on Rehabilitation Robotics (ICORR).

[30] Buechley, L., 2012, "The LilyPad Arduino: Using Computational Textiles to Investigate Engagement, Aesthetics, and Diversity in Computer Science Education", Proceedings of the SIGCHI Conference on Human Factors in Computing Systems, pp. 423-432.

[31] Mellis, D., Jacoby, S., Buechley, L., Perner-Wilson, H., and Qi, J., 2013, “Microcontrollers as Material: Crafting Circuits with Paper, Conductive Ink, Electronic Components, and an ‘Untoolkit’” (forthcoming), TEI 2013.

[32] Kaan, B. “Laptop Computer Silhouette Image”, 2010. available: http://www.vectorstock.com/royalty-free-vector/laptop-computer-silhouette-vector-698220




UAV PERFORMING AUTONOMOUS LANDING ON USV UTILIZING THE ROBOT OPERATING SYSTEM

Joshua N. Weaver University of Florida

Gainesville, Florida, U.S.A.

Daniel Z. Frank University of Florida


Dr. Eric M. Schwartz University of Florida


Dr. A. Antonio Arroyo University of Florida


ABSTRACT This paper outlines the design and implementation of the

launching and landing of an unmanned aerial vehicle (UAV) onto an unmanned surface vehicle (USV). The US Navy and Northrop Grumman’s X-47B drone have just recently achieved the first autonomous landing of an aircraft onto an aircraft carrier, but it came at the price tag of over one billion U.S. dollars. The authors of this paper will create a scaled-down model of this system that will cost in the order of thousands of dollars. The aerial vehicle utilized in this study is a quadrotor helicopter, which greatly simplifies the launching and landing process. The surface vehicle used is an autonomous boat that features a catamaran-style hull. Both vehicles will use the Robotic Operation System (ROS) as a framework for their software. A successful mission for this system would be one in which the quadrotor launches from the launch pad of the boat, navigates to a series of GPS waypoints, lands at a predetermined location to pick up an object, navigates back to the boat, and then lands successfully to the USV.

INTRODUCTION Since World War II, the ability for an aircraft carrier to

deploy and recover aerial vehicles thousands of miles from the closest air base has proven to be a great tactical advantage to those forces capable of utilizing them. Despite the advantage that they provide, the success of carriers is largely dependent on the skill and experience of the pilots flying the aircraft. The use of UAVs makes it possible to reduce failure due to human error, while also preventing the loss of human lives in the process.

Landing a rotary wing aircraft onto an aircraft carrier has several challenges associated with it. Unlike descent over land, the aircraft pilot has to compensate for the pitching of the ship to make sure the boat doesn’t accidentally slam into the landing gear. This could damage the aircraft as well as cause it to become unstable. As the aircraft approaches the surface of the

ship, it becomes susceptible to the ground effect which may also cause it to become difficult to control.

A number of researchers have investigated methods to autonomously land UAVs using vision guidance. The simplest method, shown in [1], uses a combination of GPS waypoints and vision to get the UAV in the vicinity of a landing pad. The UAV then uses vision to search for the location of the landing pad. Once the UAV has located the pad using an attached camera, the UAV then begins its descent. In [2,3], more sophisticated methods were developed for UAVs to use vision to identify hazardous features to determine whether or not a surface was safe to land.

Also pertinent to this study is the work already done on using UAVs in conjunction with unmanned ground vehicles (UGVs) to accomplish a mission. The research shown in [4] uses a manually flown AUV and an autonomous UGV. The AUV shared the information gathered by its camera to assist the UGV with path planning and obstacle avoidance. A group in Switzerland has also done extensive work in using UAVs magnetically mounted onto the ceiling to help a swarm of UGVs navigate between two targets in a foraging task system that closely mimics ant colony movement [5]. Other focuses of the research show how eye-bots assist foot-bots in traversing a gap clearing environment [6] and using a single eye-bot to assist foot-bots in traveling over a hill in terms of angle steepness and calculating group formation in order to traverse an angle too steep for a single vehicle to climb [7].

Wenzel [8] was able to use vision to autonomously take off and land a quadrotor onto a small ground vehicle with a 90% success rate. However, since their vision system relies on an infrared (IR) camera, their system only works in indoor environments with controlled lighting. Additionally, by landing on a land vehicle in an indoor environment, one can make the assumption that the orientation of the landing surface is constant. Landing on an USV presents an additional challenge,


since the orientation of the vehicle’s landing surface is also free to roll and pitch.

In this paper, the design and implementation of a heterogeneous robotic system composed of a UAV and an USV will be detailed. ROS is used to handle high-level behaviors, perform controls, and facilitate communications between the USV and UAV. Using vision, the UAV will be able to safely land on both a stationary target as well as a landing pad mounted on top of the USV in both indoor and outdoor lighting conditions.

UNMANNED SURFACE VEHICLE DESIGN The unmanned surface vehicle used in this study was

designed and built by undergraduate and graduate students at the Machine Intelligence Laboratory (MIL) at the University of Florida. It was initially designed for the purpose of competing in the Association for Unmanned Vehicles Systems (AUVSI) and Office of Naval Research’s (ONR) 6th Annual International RoboBoat Competition. The USV earned first place and has since been devoted to robotics research.

Figure 1. CAD rendering of the USV and UAV system

The USV used in this study features a catamaran-style hull to provide stability as well as a shallow draft. The two pontoons are held together with a frame composed of 8020 aluminum extrusions. For propulsion, the USV has four trolling motors. The front pair is pointed inwards at an angle of 30° while the back pair is pointed 30° outwards. This allows the USV to translate in any direction as well as rotate in place. Combined, the four trolling motors are capable of providing up to 22 lbs of forward thrust. It has a water displacement of 115 lbs and overall dimensions of 72” x 30” x 36” (length x width x height) [9].

A complete computer-aided design (CAD) model of the USV and UAV system was created using SolidWorks and can be seen in Figure 1. There are several benefits to creating a CAD model for a vehicle. First, the model allows the designer to visualize how components fit together. Second, it facilitates the use of engineering analysis tools. For example, airflow simulations conducted on the model helped determine the

geometry of the vents used for the USV’s cooling system. Third, the individual components that make up the model can easily be fabricated using the lab’s Dimension SST 1200es 3D printer or Roland MDX-540 SRP 3D milling machine as shown in Figure 2.

Figure 2. Fabricating parts using the 3D milling machine

UNMANNED AERIAL VEHICLE DESIGN The UAV used in this study is a quadrotor helicopter with a

commercially available DJI Flame Wheel F450 frame. Off-the-shelf parts were selected for the UAV to minimize the cost of the overall system, as well as to promote open-source collaboration. The ABS landing gear on the UAV is designed so that in the event the UAV crashes over land, the landing gear will absorb most of the impact and break off. This will leave the rest of the frame with minimum damage. Figure 3 shows the UAV used in this study.

Figure 3. UAV used in this study

All of the electrical components on the UAV are powered by a single 6000 mAh 3-cell lithium polymer (LiPo) battery. With a full charge, the UAV has an approximate flight time of 15 minutes. The weight of the UAV is 3.8 lbs, and it is lifted by four brushless DC motors, which can each provide 1.2 lbs of thrust.


Basic stabilization and navigation is handled with an ArduPilot Mega (APM) 2.5+ autopilot control board. Control and sensor processing abilities are provided to the UAV via a quad-core ARM ODROID-U2 installed with Linux-Ubuntu and ROS. The ODROID can retrieve information such as current orientation and GPS location from the APM. The ODROID is also capable of sending the APM information such as a desired attitude or GPS waypoint. The main sensor of the UAV is a Logitech HD Pro Webcam C920 camera which is used for computer vision as well as obstacle and target identification. Communication between the two vehicles is handled by XBee RF devices which have a range of approximately of 300 feet.

ROS ENVIRONMENT Both the USV and the UAV operate within a ROS

environment. ROS was chosen due to the open source nature of the software. This allows for various sources of code to be used or for code developed in this research to be easily shared. ROS also allows an easy method for all code, in the form of nodes, to be duplicated and moved to multiple vehicles. This is used throughout the various levels in the heterogeneous cooperative system. Some of the groundwork code for the UAV was completed in previous studies [10,11].

Since all nodes created are started with and must have access to a ROS Master Core, a loss of connection to the ROS Master would result in all nodes and controls shutting down, effectively stopping the vehicle from functioning. Each vehicle is therefore set up as a ROS Master to avoid this failure. Though each vehicle is independent and is the master of its own instance of ROS, vehicles are able to communicate via RF so that they can cooperatively plan their desired tasks to complete the mission.

Figure 4. ROS_Bridge communication between vehicles

Communication, control, and data are all handled within the ROS environment through multiple nodes. The ROS_Bridge node runs separately on each ROS Master, handling communication between each vehicle within the overall ROS environment (Figure 4). ROS_Bridge is configured using specific parameters which define desired topics and services to be shared between multiple ROS Masters. Once a topic or service is modified with new data within a ROS Master, ROS_Bridge acknowledges the change, and broadcasts the resulting data via RF communications. Other RF devices then receive this data, and subsequently update their own ROS Masters. Within this research, ROS_Bridge primarily handles mission objects in the form of desired GPS points as well as mission status information.

Direct UAV control and stabilization is left to the APM autopilot. However, mission navigation, as well as direct control for visual servoing, is handled via the APM_Driver node. The APM allows external devices to communicate with the onboard autopilot software via a standardized Micro Air Vehicle Communication Protocol called MAVLink. Specifically, the APM_Driver uses MAVLink to receive attitude and status information from the APM which is then published to ROS. Other nodes may specify mission objectives in the form of GPS waypoints which are sent to the APM for waypoint navigation. Commands may also be sent that result in the APM arming, disarming, launching, landing, or modifying stabilization modes. During visual servoing, the APM_Driver node is used to send roll, pitch, yaw, and altitude adjustments to the controller.

Figure 5. Data flow with Agent_Control

ROS Core

ROS Bridge

Topic Service Update

RF

USV

Topic Service Publish

Topic Service Update

UAV

TopicService Publish

ROS Core

ROS Bridge

Agent Control

ROS Bridge ROS Core/

CVBridge

APM Driver

CAM Driver

Vision Driver

ROS CORE

ROS CORE

ROS CO

RE


Mission planning, control, and communication with a device is managed through the Agent_Control node. Agent_Control acts as an interpreter and mission controller for vehicle driver nodes (Figure 5). Sensor data such as IMU, battery voltage, and GPS information are retrieved from a driver, such as APM_Driver. The data may be modified and then published back to ROS as desired. Subsequently, the node performs as the mission planner and control system.

The mission state system uses data retrieved from the APM, external sources such as the USV, waypoint lists, and tracking data from vision. Based on the state of the mission planner, waypoints and commands may be sent to the APM_Driver. Through a combination of the APM_Driver, Agent_Control, and ROS, many types of multirotor aerial vehicles may be controlled.

Visual processing is handled within the Vision_Driver node. Images are collected from the camera through another node, CAM_Driver, and are then published to ROS using standard image messages. Images are transferred to a format usable in OpenCV, an open-source library of programming functions for real time computer vision, by CVBridge, a ROS library which acts as an interface between ROS and OpenCV. The Vision_Driver node processes the incoming image, extracts the required data, and then publishes target data of the item being tracked to ROS. There are various other smaller nodes that are used to assist in the control or sharing of information as needed.

As previously mentioned, the USV also uses ROS for control, however, for the purposes of this research, only two components were in primary use when communicating with the UAV: Mission_Planner and ROS_Bridge. During the launch and recovery of the UAV, the USV loitered in a general position, using the Mission_Planner node to start the USV mission as well as send and receive status updates during the mission. When the mission was complete and the UAV was returning for recovery, GPS positions of desired targets were sent to the UAV as well as the USV’s location. Mission_Planner sends this information from the ROS Master on the USV to the ROS Master on the UAV through ROS_Bridge.

VISION PROCESSING All of the vision processing performed by the UAV is

computed on-board by the ODROID. The UAV is able to tell when it has found desired targets, such as a landing pad, by running an application that uses adaptive thresholding and blob detection algorithms. The application is written using OpenCV.

As each image is received, an initial Gaussian blur filter is performed to reduce image noise. The resulting image is split into individual R, G, and B channels as well as converted to HSV and Lab color spaces. Adaptive thresholding and modification of each color space channels is used to create various color filters tasked to detect colors of the desired targets to be tracked. Dilation and erosion morphological functions are

used to further reduce noise and fill in gaps of the thresholded image.

The resulting filtered image is processed further to detect blobs and contours in the image. Each blob found results in a calculated area, perimeter, centroid, and shape declaration. Blobs are reviewed to meet certain criteria in terms of minimum and maximum sizes as well as closeness to square or circular objects. Depending on the desired target, certain aspects of the image are extracted.

To allow for robustness, a Kalman Filter is used on the final image to allow for accurate tracking in case of obstructions or short loss of the desired target. The filter allows the UAV to return to an expected area if it potentially overshot the desired target.

Figure 6. Processed image of landing pad

Three red squares forming an isosceles triangle are used to designate the desired landing place on the USV. The centroid of each square is located using vision. If only one or two squares were located, the centroid of either an individual square or both squares combined were used as the desired target for the UAV. Once all three squares were located within the image, the length between each square is calculated to understand the shape of the triangle. The centroid of all three squares combined is used as the desired target location, while the shape of the triangle is used to designate the desired heading for the UAV to land. Figure 6 shows an example of this procedure.


Figure 7. Filtered and extracted image of landing dock

Figure 8. Processed image showing landing dock border and circular target

Another type of target, in the form of a landing dock, holds

a small circular object. The landing dock is also designated by a colored border around the dock. When attempting to locate the landing dock, the colored border is first recognized and used to extract a region of interest (ROI) from the image, as shown in Figure 7. It is also possible to ignore the colored border and extract the shape of the dock against the foreground of the water. The new ROI is processed to look for the small object (Figure 8), in which the centroid is used as the desired target location. Once the centroid of the object is located, a nearest neighbor between the centroid and the edge of the dock is found, resulting in a desired heading for the UAV to land.

VISUAL SERVOING Once a target is located through vision processing and the

resulting position of the target is published in ROS, the Agent_Control node handles visual servoing to align and land on the target. This is handled by a two stage PID controller that takes in target data and attitude information and results in a desired roll, pitch, and yaw adjustments.

One stage of the PID controller handles alignment of heading. Once a desired heading, depending on the type of target, is published to ROS, the yaw controller handles aligning the vehicle to the desired target. This is to handle offsets in camera placement or aligning the vehicle to a desired landing location on the landing pad.

If a desired heading is not published to ROS or if the yaw controller has accomplished its current goal, stage two of the PID controller handles roll and pitch control. A directional vector is chosen between the current vehicle’s position and heading in reference to the desired target’s position. The vector is broken down into roll and pitch errors which are used within the stage two PID controller to adjust the UAV position. Once the UAV is above the desired target it begins to decrease in altitude. Adjustments may be made in roll, pitch, and yaw during the descent.

RESULTS The mission planner on the USV initiated the launch and

recovery mission for the UAV. The USV sent a command to the UAV to launch via ROS_Bridge. Once airborne, the UAV began its own mission by following a series of waypoints until it located the landing dock with its camera. The UAV then began visual servoing so that it was properly aligned over the circular Velcro-covered object to be recovered. The UAV landed on the object and picked it up using a Velcro net.

The UAV then launched with the object from the landing dock and began making its way back to the USV using GPS waypoints. Once it was close enough for the camera to locate the landing pad on the boat, the UAV’s vision processing began visual servoing to orient and position itself before it began its descent. Once the boat successfully landed, the UAV sent a mission accomplished confirmation to the USV. The USV was then able to continue with the rest of its own planned missions.

CONCLUSION In this paper the implementation of a USV and UAV

system used within a launch and recovery scenario has been described. The design of each vehicle was given, including an overview of the software environment used to control each vehicle during the mission. Also outlined are specific functions of the computer vision algorithms created to detect both the landing pad and dock which are needed to complete the mission objectives. The method for control once a target has been acquired has also been detailed. A description of the launch and landing mission is given as well as the resulting ability for the UAV and USV to accomplish described goals.


In future developments, target identification for the landing pad will be modified to use AR tags in place of the triangle formed by squares. This will allow for single object detection as well as an easier method for detecting orientation. Another modification will include attitude information for the USV to be transmitted to the UAV during recovery procedures. This information can assist in anticipating movements the USV may make in order to decrease the difficulty for the UAV to land on the USV’s landing platform.

ACKNOWLEDGEMENTS The authors would like to thank Dr. A. Antonio Arroyo and

Dr. Eric M. Schwartz who continue to challenge and encourage students to design intelligent machines. Thanks to Dr. Carl D. Crane and everyone at CIMAR for their support and for the usage of their manufacturing machinery.

REFERENCES [1] Saripalli, S., Montgomery, J.F., and Sukhatme, G., 2002, "Vision-based Autonomous Landing of an Unmanned Aerial Vehicle," Proc. Robotics and Automation, pp. 2799-2804. [2] Cesetti, A., Frontoni, E., Mancini, A., and Zingaretti, P., 2010, “Autonomous Safe Landing of a Vision Guided Helicopter,” Proc. Mechatronics and Embedded Systems and Applications, pp. 125-130. [3] Johnson, A., Montgomery, J., and Matthies, L., 2005, “Vision Guided Landing of an Autonomous Helicopter in Hazardous Terrain,” Proc. IEEE International Conference of Robotics and Automation, pp. 3966-3971. [4] Garzón, M., Valente, J., Zapata, D., and Barrientos, A., 2013, “An Aerial-Ground Robotic System for Navigation and Obstacle Mapping in Large Outdoor Areas,” Sensors, 13(1), pp. 1247-1267.

[5] Ducatelle, F., Di Caro, G.A., and Gambardella, L.M., 2010, “Cooperative Self-Organization in a Heterogeneous Swarm Robotic System,” Proc. 12th Annual Conference on Genetic and Evolutionary Computation, pp. 87-94. [6] Mathews, N., Christensen, A.L., O’Grady, R., and Dorigo, M., 2010, “Cooperation in a Heterogeneous Robot Swarm through Spatially Targeted Communication,” Proc. Swarm Intelligence: 7th International Conference, pp. 400-407. [7] Mathews, N., Stranieri, A., Scheidler, A., and Dorigo, M., 2012, “Supervised Morphogenesis – Morphology Control of Ground-based Self-Assembling Robots by Aerial Robots,” Proc. The 11th International Conference on Autonomous Agents and Multiagent Systems, pp. 97-104. [8] Wenzel, K. E., Masselli, A., and Zell, A., 2011, “Automatic Take Off, Tracking and Landing of a Miniature UAV on a Moving Carrier Vehicle,” Journal of Intelligent and Robotic Systems, 61(1-4), pp. 221-238. [9] Gray, A., Shahrestani, N., Frank, D., and Schwartz, Dr. E., 2013, “PropaGator 2013: UF Autonomous Surface Vehicle,” http://mil.ufl.edu/propagator/wp-content/uploads/2012/10/PropaGator-2013-RoboBoat.pdf. [10] Weaver, J.N., Dash, G., Thompson, M., Arroyo, Dr. A. A., and Schwartz, Dr. E.M., 2013, “Control of UAV Through the Robot Operating System and Android,” Proc. Florida Conference on Recent Advances in Robotics. [11] Weaver, J.N., Arroyo, Dr. A.A., and Schwartz, Dr. E. M., 2013, “Collaborative Coordination and Control for an Implemented Heterogeneous Swarm of UAVs and UGVs,” Proc. Florida Conference on Recent Advances in Robotics.


ASME District F - Early Career Technical Conference ProceedingsASME District F - Early Career Technical Conference, ASME District F – ECTC 2013

November 2-3, 2013, Birmingham, Alabama, USA

SIMULATION AND VALIDATION OF A LOW POWER REGIME BASED ATTITUDEDETERMINATION AND CONTROL SYSTEM FOR CUBESATS

William D. McGinnis, Dr. David G. BealeDepartment of Mechanical Engineering

Auburn UniversityAuburn, Alabama 36849

Email: [email protected]

Dr. J-M WersingerDepartment of Physics

Auburn UniversityAuburn, Alabama 36849

Email: [email protected]

ABSTRACTThe small form factor of CubeSats has led to their rapid

acceptance into academic and commercial applications. Theirrelatively small size not only lowers manufacturing and launchcosts, but also shortens the design and development time. Thesmaller size, however, limits power available to often missioncritical subsystems such as attitude determination and control(ADACS). By using rate gyroscopes to identify 3 regimes (de-tumbling, pointing and steady state), the power consumed by theADACS can be minimized by selectively turning off sensors. It isshown that not only does this practice lower the power consumedby the subsystem, but also the regime based control system cansuccessfully detumble and control the CubeSat comparably tothe much more power intensive fully active system.

1 BackgroundWhat is referred to as attitude is simply the relationship be-

tween the orientation of any two coordinate frames. For example,take two arbitrary reference frames A and B. A single vector, r,can be represented relative to either reference frame:

Ar = (xa,ya,za) = (1,0,0) (1)

or

Br = (xb,yb,zb) = (0,1,0) (2)

The vector itself, r, is the exact same in each case, it is sim-ply being described relative to two different frames, which givesrise to the change in the numerical value between Ar and Br. Ascan be seen, the notation for a vector relative to a certain frameis to put the frame’s name in a superscript before the vector. SoAr is the vector r, relative to the A reference frame.

A rotation matrix is a matrix of values which rotate a vectorfrom one position to another. It can also be seen as a transformbetween one reference frame and another reference frame whichis oriented differently. Rotation matrices are represented withcapital E, preceded by a superscript describing the rotation. Forexample, the rotation matrix which rotates a vector from the ref-erence frame A to the reference frame B would be written asA→BE. Rotation matrices are used by multiplying them with thestarting vector to find the result. For example, take the arbitraryvector, Ac. The corresponding vector, Bc can be found by:

Bc =A→B EBc (3)

It is clear that this rotation matrix, E, defines how the Bframe is oriented relative to the A frame.

Though the notation can be cumbersome, it is important tokeep track of which vectors and rotation matrices go with whichreference frames.


2 Attitude Determination2.1 Definition of Attitude

In this report there will be 3 relevant reference frames. Allreference frames are right handed and orthogonal. The three sys-tems are shown in Figure 1.

FIGURE 1. The three reference frames used in attitude kinematics

The body frame is a Cartesian coordinate system centered atthe center of mass of the satellite, with the axes aligned with theprinciple moments of inertia. The components of any vector ineach axis of the body frame will be denoted with:

B = (X,Y,Z) (4)

The Z axis is chosen to be along the largest principle axis,and the X is chosen to be along the smallest.

The orbital frame is also centered at the center of mass ofthe system, where the x axis points along the velocity vector ofthe system, z points along the Nadir vector, and y completes theframe. The components of the orbital frame will be denoted with:

O = (x,y,z) (5)

The terrestrial frame is an earth centered earth fixed Carte-sian coordinate system, with the X axis pointing through 0◦ lat-itude and longitude, the Z axis going due north, and Y com-pleting the frame. It is denoted by:

T = (X ,Y ,Z ) (6)

The relationship of concern, in the attitude determinationproblem specifically, is the attitude of the body frame relative

to the orbital frame. By taking the body frame relative to orbitalinstead of the body relative to the terrestrial frame, the attitudedetermination problem is complicated slightly, but the controlsproblem is simplified. It can be seen that what we refer to as ”at-titude” and what we are interested in, and want to find, is O→BE.

Strictly speaking, this alone could be used to denote attitude,this notation method is referred to as DCM (Direction CosineMatrix). For reasons that will become apparent later on in thecontrols problem, it is preferable to use fewer numbers than the9 needed for a DCM. The two common ways to do this are withEuler angles (3 numbers) and quaternions (4 numbers). Due tonumerical stability concerns, quaternions are used [7].

So E (any E) must be written in terms of quaternions. Thisis done by:

E =

q21−q2

2−q23 +q2

4 2(q1q2 +q3q4) 2(q1q3−q2q4)2(q1q2−q3q4) −q2

1 +q22−q2

3 +q24 2(q2q3 +q1q4)

2(q1q3 +q2q4) 2(q2q3−q1q4) −q21−q2

2 +q23 +q2

4

(7)

2.2 Attitude CalculationIt is immediately apparent that to calculate the value of

O→BE, one first needs to find some vector, r, that is know rel-ative to both frames, because it is known that:

Br =O→B EOr (8)

If one knows Br and Or, O→BE can be estimated. This, how-ever, is not as simple as is seems. In 1965, Wahba restructuredthe problem as an optimization problem [4]. She declared a lossfunction:

J(E) =12

N

∑k=1

ak||wk−Evk||2 (9)

Where wk is the kth element of a set of vectors in the orbitalframe, vk is the corresponding member of a set of correspondingvectors in the body frame, and E is the rotation matrix betweenthe two (O→BE in our notation). ak is a weighting factor for thecorresponding pair of vectors.

Applied to the case of the CubeSat, there are two sets ofvectors to be used. A triaxial magnetometer and GPS can beused to calculate the Earth’s magnetic field vector in two frames,Bβββ and Oβββ , which will have a relatively high weighting. Second,solar panels and a GPS can be used to calculate the angle to thesun in each frame (Bs and Os), which will have a low weighting.


The low weighting for the sun vector measurement is due to therelative inaccuracy of the measurement of the solar panels versusthe magnetometer.

With these two sets of vectors, one can use Wahba’s lossfunction to calculate O→BE. This is done by selecting valuesfor ak, using the measured and calculated vectors, and settingthe rotation matrix (which is defined by q1,q2,q3,andq4) as afree parameter. Any optimization method can then be used tominimize the value of J(E). Whatever q values correspond to thisminimum J value define the rotation matrix between the orbitaland body frame.

This method, generally, is how the attitude between the or-bital and body frames is calculated. The optimization routineitself is an open problem, with speed of calculation and accuracybeing the main concerns. There are a number of codes available,including the QUEST algorithm, which will be used in this pa-per [2].

The QUEST algorithm requires the two vector pairs whichare measured and calculated in similar ways. The methodologyof calculating the magnetic field vectors will be discussed in de-tail, but the process for calculating the sun vectors is nearly thesame.

2.3 Magnetic Field in the Body FrameDetermining the direction of the magnetic field in the body

frame is relatively simple. A magnetometer which is aligned withthe satellites principle moments of inertia can measure field di-rectly, so after some signal conditioning, it can be found.

2.4 Magnetic Field in the Orbital FrameFinding the magnetic field with respect to the orbital frame

is somewhat more complex. There are a number of steps in theprocess, which include:

1. Calculate position with GPS2. Calculate T βββ using dipole model and position3. Calculate T→OE4. Transform T βββ into Oβββ

2.4.1 Calculate Position with GPS A space ratedGPS can report position relative to the earth in some format, gen-erally longitude and latitude, with an altitude. These noisy mea-surements are then filtered and conditioned to prepare for use inthe dipole model.

2.4.2 Calculate T βββ using dipole model and posi-tion There are a number available models, of varying complex-ity, for Earth’s magnetic field.

Rauschenbakh et al. models the magnetic flux density of theEarth in the terrestrial frame, as [3] [6]:

Tβx = µ03Heqsin(i)cos(i)sin2(u) (10)

Tβy =−µ03Heqsin(i)sin(u)cos(u) (11)

Tβz = µ0Heq(1−3sin2(i)sin2(u)) (12)

Where Heq is the equatorial magnetic field strength at the al-titude of interest, i is the orbit inclination, µ0 is the permeabilityof a vacuum, and u is the argument of latitude. These are the val-ues which must be known either ahead of time, or be calculablefrom the GPS data.

2.4.3 Calculate T→OE The dipole model in Equations10, 11, and 12 gives the direction of the magnetic field relative tothe terrestrial frame, but it is needed relative to the orbital frameto calculate the satellite’s attitude, therefore T βββ must be trans-formed into Oβββ . As demonstrated earlier, this is done with arotation matrix. This will take the form of:

Oβββ =T→O ET

βββ (13)

This can be seen as a similar problem to the original attitudedetermination problem, and as will be seen, many of the sameprocesses can be applied to solve it. In this case,T→OE is to becalculated. Again, this is done by finding some vector that isknown in both frames (orbital and terrestrial). The simplest caseis the Nadir vector. The Nadir vector always goes from the centerof the satellite to the center of the earth. The Nadir vector will bedenoted with N (T N and ON).

Finding the Nadir vector relative to the orbital frame (ON)is quite simple. As mentioned in a previous section, the orbitalframe itself is defined by the Nadir vector, such that the positivez axis is always along it. So the unit vector in the positive zdirection of the orbital frame will always, by definition, pointalong the Nadir vector.

ON ≡ (0,0,1) (14)

The Nadir vector relative to the terrestrial frame is also rel-atively simple. Assume the Cartesian position of the satellite inthe terrestrial frame is known from the GPS reading as T rpos. Ifthis is the case, then the Nadir vector, by it’s own definition mustbe:

T N =−T rpos

||T rpos||(15)


Which is simply due to the fact that the Nadir vector goesfrom the center of the satellite (T rpos) to the center of the earth,or (0,0,0) in the terrestrial frame. It is then divided by it’s mag-nitude to form a unit vector.

An identical process can be used to calculate the velocityvector in each frame as well. In the orbital frame it will alwaysbe along the x axis, while in the terrestrial frame the vector canbe estimated with successive GPS position measurements, or re-ported directly from the GPS.

Now with two vectors relative to both the orbital and terres-trial frames, the rotation between the two frames can be calcu-lated using the QUEST algorithm [2].

2.4.4 Transform T βββ into Oβββ With T→OE estimatedfrom the previous section, and T βββ calculated in the section be-fore that, Oβββ can be found quite simply, by

Oβββ =T→O ET

βββ (16)

2.5 Final Attitude CalculationIn past sections, Bβββ has been measured and Oβββ has been cal-

culated. Again, these values are used with the QUEST algorithmto give the rotation matrix between the body and orbital frame.The output will be the quaternions which define the attitude ofthe satellites body frame relative to the orbital. That in whole,defines the entire attitude determination system.

3 Attitude ControlStrictly speaking, with perfect attitude determination, differ-

ent control algorithms for detumbling and arbitrary pointing arenot required. Detumbling is simply the type of control that takesplace when the initial conditions include very high angular rates.In these cases, especially with a magnetometer and magnetorquerbased control system (where the actuator and sensor cannot oper-ate simultaneously), it is very difficult to cycle the determinationand control systems fast enough to have accurate measurementsand signals.

In order to account for this, at each time step, the maximumangular body rate will be found from rate gyroscope measure-ments:

ωmax = max(ωx,ωy,ωz) (17)

This value is then checked to see what regime it falls into:

Regime =

T. ωmax > upperP. lower > ωmax > upperS.S. ωmax < lower

(18)

The three states are tumbling, where the angular rates arevery high (such that full attitude determination is overly diffi-cult), pointing (where the angular rates are low enough to deter-mine attitude, the body is still moving and not settled), and steadystate (where the body rates are negligibly low, and the attitude iscorrect).

All together, the flow chart of the attitude determination andcontrol system in the pointing regime is shown in Figure 2. Intumbling, the model simplifies, as the GPS and all associatedcalculations are not present and not required to detumble. Insteady state, only the rate gyroscopes are on, simply checking toensure that the regime has not changed.

3.1 DetumblingFor detumbling, it is assumed that full attitude is not known,

but the angular rates in the body frame are known. In this case,what is known as a D controller is used to stabilize the satellitebefore beginning the arbitrary pointing control algorithm. A Dcontroller works by trying to minimize the angular rates of thebody frame, rather than by trying to point to some specific atti-tude. The control equation for a D controller is:

MMMd =−KKKdω̇ωω (19)

So measurements are taken from the rate gyroscopes on eachaxis, and the value is used to give the control torque.

With this vector of desired torques, the current signal can becomputed with the relationships:

Md,true = µµµ×Bβββ (20)

µi = µ0nIiAcx (21)Imax ≡ 300mA (22)Imin ≡−300mA (23)

Isignal = f (Vpin) (24)

Further, at each time-step the angular rates are checked tosee if they are below the upper threshold. Once they are belowthis threshold, the regime changes to pointing.


FIGURE 2. Flow Chart of Attitude Determination and Control Sys-tem in Arbitrary Pointing Regime

3.2 PointingOnce the satellite is detumbled, full attitude is calculable.

With this information, the satellite can be controlled to point toany desired attitude (given in quaterinon form). There are a num-ber of ways to do this, the simplest is a PD controller. In this casethe equation would take the form [5]:

MMMdesired =−(KKK pq̄qqe +KKKdω̄ωω) (25)

Where Kp and Kd are the proportional and derivative con-trol gains respectively and qe is the error quaternion. The errorquaternion is defined by [1]:

q̄qqe =

qd4 qd3 −qd2 −qd1−qd3 qd4 qd1 −qd2qd2 −qd1 qd4 −qd3qd1 qd2 qd3 qd4

q̄qqm (26)

Where qd is the desired quaternion and qm is the measuredquaternion, from the attitude determination system.

The controller gains are selected as 3 component vectors,giving 6 gains. Because the disturbance magnitudes and mo-ments of inertia about each axis are different in a general satel-lite, it is advisable to use 6 separate gains.

Again, this desired torque is combined with these equationsto generate a usable signal:

MMMpd,true = µµµ×Bβββ (27)

µi = µ0nIiAcx (28)Imax ≡ 300mA (29)Imin ≡−300mA (30)

Isignal = f (Vpin) (31)

Unlike the detumbling phase, where the magnetorquers andmagnetometer cycle as quickly as possible to detumble the fastmoving satellite, the wait time, n is variable in arbitrary point-ing. The reasoning behind this decision is that when the satelliteis moving relatively fast, the attitude needs to be updated morefrequently than when it is moving relatively slowly. So once anew signal is generated, the magnetorquers are activated at thatlevel for n seconds, where n is:

n = c1− c2eωmax (32)

One can easily see that this will result in a wait time that ex-ponentially decays to some standard minimum as the maximumangular rate increases. The constants are selected depending onthe limitations of the system itself.

Once the measured attitude is within a pre-defined errorbound of the desired attitude, and the maximum angular rateis below the lower threshold, the system is determined to be insteady state.

3.3 Steady StateSteady state is characterized as a power conservation state.

Because the satellite is at the correct attitude, the power hun-gry GPS and the magnetometer can be turned off. Unless themaximum angular rate rises above the lower threshold again, theADACS system generates no torque.

4 SimulationA MATLAB script was written to simulate this control sys-

tem. Disturbance torques due to both gravity gradient and aero-dynamic forces were taken into account. The satellite simulated


was a 3U CubeSat with no deployables and an terrestrial tensorof:

I =

0.5946 0 00 0.6654 00 0 0.062

(33)

The initial conditions were a 3.25 rad/sec initial angular ve-locity about each body axis and initial quaternion of [0.1913,0.4619, 0.1913, 0.8446]. The upper threshold was set to be 2rad/sec and the lower threshold was 0.1 rad/sec. The sensorsmodeled were 3 noisy rate gyroscopes, a noisy space GPS, anda noisy 3 axis magnetometer. Discrete sampling from the GPSwas simlated, using the relationship in Equation 32. These sensormodels were used to calculate the total ADACS power consump-tion at each time step.

The same initial conditions were also used to simulate a con-trol system which operates in the arbitrary pointing phase at alltimes. In this case, all of the sensors were on all of the time.While the single regime system has full attitude data in the de-tumbling phase, the magnetorquers cannot be active at all times,because they must be turned off to acquire that attitude data. Cer-tainly it is expected that the single regime system will use muchmore power to continue running the sensors once the satellitehas reached steady state. The same controller gains were usedfor each simulation.

Both cases were simulated for 60 seconds, so that the powerconsumption of each system could be compared. Both casesproperly detumbled and pointed the satellite as expected. Fig-ure 3 shows the power consumption of the single regime overthe simulation. The switching nature of the magnetometer andmagnetorquer system are clear in both Figures 3 and 4. This is aresult of having to turn off the magnetorquers when the magne-tometer is measuring the local magnetic field.

The satellite quickly detumbles and orients itself, beforereaching a steady state. The average power consumption overthe entire 90 second simulation was 2.67 W. This is, therefore,the benchmark for the multi-regime based control system. Fig-ure 4, shows the power consumption of the multi-regime ADACSover the course of it’s simulation.

Clearly, a difference can be seen from the single regime sys-tem, with 3 discrete regimes discernible in the power consump-tion and a much lower power steady state. Over the course ofthe entire orbit, the multi-regime ADACS averaged just 1.19Wof power consumption.

Both systems detumble in a similar time-scale (as shown inFigures 5 and 6 respectively), but in the multi-regime system’ssteady state, the disturbance forces are allowed to induce a verysmall angular velocity before triggering the control system backinto action.

FIGURE 3. Power Consumption of Single Regime ADACS

FIGURE 4. Power Consumption of Multi-Regime ADACS

This means that over very long periods of time, the satellitecan gradually drift out of alignment. Depending on the pointingaccuracy requirements of the particular mission, the control sys-tem should therefore be turned back on after a fixed period oftime in the steady state regime to provide correctional torquing.

The end goal of the attitude determination and control sys-tem is to point along a certain vector. With this in mind, Figures7 and 8 show the pointing error in degrees of the multi regimeand single regime systems respectively. In both cases, the errorquickly drops to an acceptable level. In the multi-regime case,the CubeSat quickly settles to within 3 degrees of the desiredorientation.


FIGURE 5. Quaternions of Single-Regime ADACS

FIGURE 6. Quaternions of Multi-Regime ADACS

5 Conclusion

It has been shown that a full attitude determination and con-trol system for a CubeSat can be designed with a GPS, rate gy-roscopes, magnetometers and coarse sun sensors for determina-tion, coupled with magnetorquers for control. A 3-state regimeswitching routine is used to differentiate between detumbling,arbitrary pointing, and steady state, which conserves power bymore than a factor of 2 relative to a fully active control system.The multi-regime system functions comparably to the single-regime system, despite the lower power consumption.

FIGURE 7. Pointing Error of Multi-Regime ADACS

FIGURE 8. Pointing Error of Single-Regime ADACS

6 Future WorkThe most apparent improvement of this work to be done in

the future is the consideration of time-to-lock in space rated GPSreceivers. In this paper, the time-to-lock is assumed to be 0, forsimplicity in simulation, but in reality it could be as much as50 seconds. In order to fill the data hole between turning theGPS on and getting new data, it is suggested that the most recentdata point be simply integrated forward to produce near contin-uous position estimates. This process may also help to improvethe control algorithm in between mangetometer measurementswhile in the pointing regime, allowing for more infrequent sen-sor checks.


7 AcknowledgmentsThe authors would like to thank the Auburn University Stu-

dent Space Program (AUSSP) for their continued guidance andsupport. Funding for this project was generously provided by theAlabama Space Grant Consortium.

REFERENCES[1] H. LEE B.J. KIM and S.D. CHOI. Three-axis reaction wheel

attitude control system for kitsat-3 microsatellite. Thesis,2011.

[2] Hall C. D. Spacecraft dynamics and control. Online, March2003.

[3] D. T Gerhardt and S. E. Palo. Passive magnetic attitude con-trol for cubesat spacecraft. In 24th Annual AIAA/USU Con-ference on Small Satellites, 2010.

[4] F.L. Markley. Attitude determination using vector observa-tions and the sinuglar value decomposition. Journal of theAstronautical Sciences, 38:245–258, 1988.

[5] Boudjemai S. Mohammed, A.M. and S. Chouraqui. Mag-netorquer control for orbit manouvre of low earth orbit mi-crosatellite. In Proceedings of the 5th WSEAS InternationalConference on Applied Computer Science, 2006.

[6] Ovchinnikov M. Y. Rauschenbakh, B. V. and S. McKenna-Lawlor. Essential Spaceflight Dynamics and Magnetospher-ics. Miscrocosm Press and Kluwer Academic Publishers,2004.

[7] Ahmed A. Shabana. Computational Dynamics. John Wileyand Sons, Inc., 2nd edition, 2001.


section 3 dynamic systems & control - uab · 3, 2013 - birmingham, alabama usa . ... denote the...

Documents