:

 

.mzfimkzfiz...

.i :3

 

 

 

 

 

 

 

, , in» .

t 75.

‘71.: :....v. v . (:3. i. . ,
5 . a $313 mg»? . a; ,. -.

. . Eggfig ,

Sanka

fins-

3

m3

.:
i.
.z .1

 

THESIS

24

“"3 mmllllllllllll

llllllllllllllllllllllllll0

3 1293 01771

 

LIBRARY
Michigan State
University

 

 

 

This is to certify that the

thesis entitled

CONTROL OF ‘NONHOLONOMIC’SYSTEMS
WITH ROLLING CONSTRAINTS

presented by

Jay Tawee Pukrushpan

has been accepted towards fulfillment
of the requirements for

M. S . degree in Mechanical Engineering

. I / - ' - ‘
,0 r %/6/ I: 9:. yb-fl ¢fiyf>/’:&/ l
_/ ‘

7

Major professor

Date é3/:228/628)

0-7639 MS U is an Affirmative Action/Equal Opportunity Institution

 

PLACE IN REIURN BOX to remove this checkout from your record.
To AVOID FINES return on or before date due.
MAY BE RECALLED with earlier due date if requested.

 

DATE DUE DATE DUE DATE DUE

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

1/93 W14

Control of Nonholonomic Systems with Rolling

Constraints

By

Jay Tawee Pukrushpan

A THESIS

Submitted to
Michigan State University
in partial fulfillment of the requirements

for the degree of

MASTER OF SCIENCE

Department of Mechanical Engineering

1998

ABSTRACT

Control of Nonholonomic Systems with Rolling

Constraints
By

Jay Tawee Pukrushpan

Two control strategies for two nonholonomic systems with rolling constraints are
presented. These systems are the two-wheeled mobile robot and the rolling sphere.
For the mobile robot, the problem of point-to-point stabilization is addressed. It. is
well-known that there does not exist a smooth pure-state feedback that can stabilize
the mobile robot to an arbitrary fixed posture. In the past, smooth time-varying feed-
back, non-smooth time-invariant feedback and their hybrid combination have been
used for stabilization of mobile robots. In this report, a new feedback control strat-
egy is presented. Based on a nonlinear oscillator, it generates almost-smooth time-
invariant control action that can asymptotically converge the robot from practically
any posture to the desired posture. The only exception is an equilibrium manifold
of measure zero from where the robot cannot move. These points are however un-
stable equilibria and, therefore points arbitrary close to them can be asymptotically
converged to the desired posture. Simulation results are shown to demonstrate the

capability of the controller. Our controller can handle the situation where the robot

has to turn more than 360° between initial and final configurations. Such capability
will be useful for tethered mobile robots in factory automation.

We also present an open loop control strategy for the rolling sphere. The open
loop controller is based on the concept of geometric phase which is quite popular
in nonholonomic motion planning. It uses the idea that when a sphere rolls along
a spherical triangle on its surface, the sphere undergoes a net displacement in the
Cartesian coordinates with a pure rotation about the vertical axis. The magnitude
and direction of the displacement are related to the dimensions of the sphere and the
amount of rotation about the vertical axis is proportional to the surface area of the
spherical triangle. The relation between the amount of rotation and the surface area
is also shown to be true for any arbitrary closed curve on the surface of the sphere. For
a given desired location and orientation, a method for finding the spherical triangular
path is demonstrated. Simulations are presented to subsequently show convergence

of system variables for the motion of the sphere along the spherical triangle.

To my parents

iv

ACKNOWLEDGMENTS

I would like to express my deepest gratitude to my major advisor, Dr. Ranjan
Mukherjee, for his support, guidance and patience. It has always been a great pleasure
to work with him. I would also like to thank the other members of my committee, Dr.
Ronald Rosenberg and Dr. Steven W. Shaw for their helpful advice. My appreciation
also goes to all my friends in Dynamics Lab, especially Joga Setiawan and Mark
Minor for their promptly assistance with my computer difficulties.

1 also want to thank Thai government for giving me a scholarships for my graduate
study. I would finally like to give special thanks to my dearest family, my dad,
my mom and my sister, for their love, care and support. My last thank goes to

Chutamanee for her love and encouragement.

TABLE OF CONTENTS

List of Tables
List of Figures

1 Background and Introduction

2 Feedback Stabilization of a Two-Wheeled Mobile Robot
2.1 Motivation .................................
2.2 Kinematic Model of Mobile Robot ....................
2.3 Almost-Smooth Time-Invariant Feedback Control Strategy ......
2.3.1 Transformation of variables ...................
2.3.2 The feedback control strategy ..................
2.3.3 Convergence and stability ....................
2.4 Simulation Results ............................

3 Open-Loop Control of The Rolling Sphere
3.1 Motivation .................................
3.2 Review of Spherical Trigonometry ....................
3.3 Sphere rolling along the spherical triangle ...............
3.4 Sphere rolling along lune .........................
3.5 Sphere rolling along an arbitrary closed path ..............
3.6 Kinematic Model of Sphere .......................
3.7 Formulation of Open Loop strategy ...................
3.8 Additional simulations supporting open-loop control strategy .....

4 Conclusion

Appendix
A Program for calculating vector rotation ..................
B Spherical trigonometric identities ......................
C Simplification of rotation matrix ......................
D Fortran program for numerical root finding ................

Bibliography

vi

vii

viii

(010N030)

10
12
16

21
21
22
26
33
36
38
41
52

59

62
62
65
67
76

82

2.1
2.2
3.1

3.2

LIST OF TABLES

Initial configuration of mobile robot ................... 16
Controller parameter used in simulations of mobile robot ....... 17
Numerical values used in simulations 1, 2 and 3 for open 100p control
of the sphere rolling along spherical triangles. ............. 46
Results from simulations 4 and 5 of the open loop control of the sphere
rolling along arbitrary closed paths .................... 56

vii

2.1
2.2
2.3
2.4

2.5
2.6
2.7
3.1
3.2
3.3
3.4

3.5

3.6

3.7

3.8

3.9

3.10
3.11
3.12
3.13
3.14
3.15
3.16
3.17
3.18
3.19
3.20
3.21
3.22
3.23

LIST OF FIGURES

Diagram showing the state variables of the mobile robot ....... 8
Circle in the r — 06 plane defining the Lyapunov function V ...... l3
Trajectory of the mobile robot for Case A. ............... 17
Trajectories of the mobile robot for Case B with different set of control

parameter .................................. 18
Trajectory of the mobile robot for Case C. ............... 19
Trajectory of the mobile robot for Case D. ............... 19
Trajectory of the mobile robot for Case E. ............... 20
A spherical triangle ............................ 23
The spherical triangle that specify the path in figure 3.3 ....... 27
Motion of the sphere defined by the spherical triangle in figure 3.2 . . 27
Path generated by the spherical triangle similar to the one in figure 3.3

but with opposite direction. ....................... 31
The Lune ................................. 33
Cartesian path given by the Lune in figure 3.5 ............. 34
Arbitrary Spherical Polygon ....................... 36
Arbitrary closed path that crosses itself ................. 37
Figure defining the state variables of a sphere ............. 39
Open loop control strategy of spherical robot .............. 41
Possible values of d and a for one spherical triangle .......... 44
Possible values of d and (f) for one spherical triangle .......... 44
Possible values of a and ab for one spherical triangle .......... 45
System variables used in simulation ................... 45
Trajectory of the sphere in simulation 1. ................ 47

Variables d, (15 and a from simulation 1 for open loop control of sphere. 48
Variables 6,113,131,, y, 3],, from simulation 1 for open loop control of sphere. 48
Trajectory of the sphere in simulation 2. ................ 49
Variables d, ¢ and a from simulation 2 for open loop control of sphere. 49
Variables 6, x, asp, y, yp from simulation 2 for open loop control of sphere. 50
Trajectory of the sphere in simulation 3. ................ 50
Variables d, 45 and a from simulation 3 for open loop control of sphere. 51
Variables 0, 27,311,, g, yp from simulation 3 for open loop control of sphere. 51

viii

3.24
3.25
3.26

3.27
3.28

3.29

Parameters of closed curve ........................
Surface integral for arbitrary closed curve on sphere ..........
Trajectory of the sphere in simulation 4. Solid line represents trajectory
of center of the sphere, dashed line represents trajectory of point P. .
Variables 0 and a from simulation 4 for open loop control of sphere. .
Trajectory of the sphere in simulation 5. Solid line represents trajectory
of center of the sphere, dashed line represents trajectory of point P. .
Variables 0 and a from simulation 5 for open loop control of sphere. .

ix

53
55

56
57

57
58

CHAPTER 1

Background and Introduction

In this chapter, we provide a brief introduction and some background in motion
planning and control of nonholonomic system. Specifically, we define nonholonomic
systems, discuss their characteristics, and summarize the two main problems namely
motion planning and stabilization. A general introduction to nonholonomic systems
can be found in the book by Murray, Li, and Sastry [26], and a review of the recent
literature can be found in the review article by Kolmanovsky and McClamroch [18].

Nonholonomic systems are characterized by nonholonomic constraints of motion.
Such constraints are not integrable and therefore they can not be used to solve for
a set of independent generalized coordinates. Nonholonomic systems consequently
require more coordinates for their description than there are degrees of freedom.
Nonholonomic constraints usually arise in two kinds of situations. They appear in
rolling contacts where objects roll without slipping. Examples of such systems are
mobile robots, wheeled vehicles, the rolling disk and the rolling sphere. Nonholonomic
constraints also appear in systems that conserve angular momentum. A free-flying
space robot is an example of such a nonholonomic systems. A nonholonomic system
such as the wheeled snake board [22] is a result of both the rolling constraint and the
conservation of angular momentum.

There are some important properties of nonholonomic systems. First, a set of in-

dependent generalized coordinates does not exist since the non-integrable constraints
can not be used to eliminate the dependent generalized coordinates. Secondly, non-
holonomic control systems do not have isolated equilibrium points and hence the
origin cannot be asymptotically stabilized using pure state feedback [7]. Finally, the
nonholonomic characteristic of a given system is invariant with respect to diffeomor-
phic state transformations and with respect to smooth pure state feedback.

Generally, there are two problems that have been studied for nonholonomic sys-
tems, namely, motion planning and stabilization. Motion planning problems are
concerned with obtaining Open loop controls which will steer the system from an
initial state to a final state over a given finite time interval. The difficulty of mo-
tion planning for nonholonomic system originates from the non-existence of a set of
independent generalized coordinates, which means that not every motion is feasible.
Only motions that satisfy the instantaneous nonholonomic constraints are feasible.
Researchers have looked into efficient means for planning feasible trajectories and a
number of solutions have been proposed. Optimal-trajectory problems and problems
of path planning with obstacle avoidance have also been addressed.

Motion planning methodologies can be loosely classified into three methods,
namely, differential-geometric techniques, geometric phase based methods, and con-
trol parameterization approaches. A variety of motion planning techniques are de-
scribed in two books ([23] and [26]) and in a review article [18]. Most motion planning
tools in differential-geometric techniques are based on Lie-algebra. Tools have been
developed to generate motions in the directions of iterated Lie brackets that repre-
sent directions that violate the instantaneous constraints. The idea of using piecewise
constant inputs to generate motions in the directions of iterated Lie brackets has been
proposed by Lafferiere [19] and Lafferiere and Sussmann [20]. Methods based on aver-
aging theory has been developed by Gurvits and Li [12], Leonard and Krishnaprasad

[21], and Sussmann and Liu [33]. The fundamental idea of motion planning using ge-

ometric phase is that the change in some states or function of states, called geometric
phase, depends on the geometry of the path. The appropriate path that produces
the desired geometric phase can be usually solved as a function of path parameters.
Examples of publications of the techniques using geometric phase include [6], [12] and
[25]. Motion planning using parameterization of inputs are based on a finite dimen-
sional family of functions. Steering the system using a family of sinusoids by Murray
and Sastry [27] is an example of this method.

The control problem of tracking is defined as the task of finding a feedback control
law that asymptotically stabilizes the motion of the system about a reference trajec-
tory. Solutions to this problem have been developed using classical nonlinear control
approaches, as in [9, 15, 35, 30]. The other extensively studied control problem for
nonholonomic systems is feedback stabilization. These problems are concerned with
obtaining feedback laws, which guarantee asymptotic stability of the equilibrium of
the closed loop system. It follows from Brockett’s necessary condition for feedback
stabilization [7] that there exists no smooth pure state feedback that can asymp—
totically stabilize a nonholonomic system. The stabilization problems are therefore
inherently more difficult than the problem of tracking. In agreement with Brockett’s
result, primarily two types of stabilizing controllers have been developed, namely,
smooth time-varying controllers and non-smooth time-invariant controllers. Hybrid
combinations of the two controllers have also been pr0posed.

Explicit time-varying control laws for feedback stabilization of nonholonomic sys-
tems was first proposed by Samson [31, 32]. General existence results can be found
in [8] and explicit time periodic feedback laws can be found in [28]. The conver-
gence properties of time-varying feedback has been studied by Gurvits and Li [12]
and M’Closkey and Murray [24]. In general, the smooth time-varying controllers are
known to have slow convergence rates and faster convergence can be achieved through

the design of non-smooth controllers. Bloch [5] initiated the work on discontinuous

controllers for nonholonomic system. Piecewise continuous stabilizing controllers with
exponential convergence rates were subsequently developed by Canudas de Wit and
Sordalen [10], and Khennouf and Canudas de Wit [17]. In a somewhat different strat-
egy a non-smooth state transformation was used to design a smooth time-invariant
controller in the transformed coordinates [1, 2]. The feedback law in the original
coordinates for both cases is however discontinuous. Discontinuous controllers based
on sliding modes have also been proposed by a number of researchers, [11, 4], but
such controllers offen lead to chattering.

In this thesis, we concentrate on the control of two nonholonomic systems, namely
a two-wheeled mobile robot and the rolling sphere. We introduce a new design method
for the feedback control of a two-wheeled mobile robot based on a nonlinear oscillator.
Because the stiffness and damping can be functions of the robot states, nonlinear os-
cillators offer more flexibility than undamped linear oscillators that have been widely
used in time-varying control of nonholonomic systems. Our controller generates ”al-
most smooth” [13] time-invariant control action that can asymptotically converge the
robot from practically any posture to the desired posture. The only exception is an
equilibrium manifold of measure zero (containing the desired posture) from where the
robot cannot move. All points on this manifold (except the desired posture) are how-
ever rendered unstable and therefore any posture arbitrarily close to, but not exactly
on, the manifold can be asymptotically converged to the desired posture. This gap
in the posture space is not a limitation of the controller.

The control of the rolling sphere is inspired by the idea of the spherical robot. A
spherical robot is conceived to have a spherical exoskeleton and an internal mechanism
for self-propulsion. The robot will have a retractable camera that will be deployed
and retracted through an orifice in the exoskeleton. Therefore, the motion of the
sphere has to be controlled such that this point of deployment of the camera always

appear on the top when the robot comes to rest. In this thesis, we design an open loop

control strategy, one that borrows ideas from spherical trigonometry and is based on
the concept of geometric phase. The basic idea is that when a sphere rolls along a path
described by a spherical triangle on the surface of the sphere, the sphere undergoes a
net displacement in the Cartesian coordinates with a pure rotation about the vertical
axis. The amount of rotation and the direction and magnitude of displacement depend
on the dimensions of the spherical triangle. In this thesis, the idea of the spherical
triangle is later extended for motion of the sphere along any arbitrary closed curve
on its surface.

This thesis is organized as followed. The control strategy of the two-wheeled mo—
bile robot is presented in chapter 2. After a brief motivation, the kinematic model of
the mobile robot is presented, followed by the design of the closed loop controller. The
convergence and stability of closed loop system trajectories are demonstrated next.
Simulation results are then presented to demonstrate the capabilities of the controller.
Chapter 3 presents an open loop control strategy for the rolling sphere. This chapter
first provides some motivation and the fundamentals of spherical trigonometry. The
expression for the amount of rotation of the sphere rolling along a spherical triangle,
a two-sided curve are then derived. The kinematic model of the sphere, used in simu-
lation, is presented next. Simulation results are presented at the end of the chapter to
demonstrate the open loop control strategy. Chapter 4 provides concluding remarks

and makes suggestion about the scope of future research.

CHAPTER 2

Feedback Stabilization of a
Two-Wheeled Mobile Robot

2. 1 Motivation

During the past seven decades, the number of robots used in industries has grown
enormously. Most of these robots are designed to be rooted to the ground where they
perform their tasks. Because they are fixed, their workspace is limited to certain dis-
tance from their location. Larger size robots are needed to handle larger workspaces.
Mobile robots have the advantage of a large workspace compared to the size of the
robot. Such robots are routinely used to transport parts between machining centers
in automotive industries. If mobile robots can be made to be autonomous, and they
have maneuvering capability, they can be used in remote areas such as in space ex-
ploration. In order to have satisfactory performance, mobile robots need to possess
intelligence, which is normally associated with their control and sensing systems.
The mobile robot control problem belongs to the class of nonholonomic control
systems since the rolling contact at its wheels results in non-integrable velocity con-
straints. In this thesis, a new control strategy for the mobile robot is introduced.

The controller, based on a nonlinear oscillator can generate almost-smooth [13] time-

invariant control action that can asymtotically converge the mobile robot from prac-
tically any posture to the desired posture. The only exception is an equilibrium
manifold of measure zero (containing the desired posture) where the robot cannot
move. However, all points on the manifold (except the desired posture) are ren-
dered unstable and therefore any posture arbitrarily closed to the manifold can be
asymptotically converged to the desired posture.

The mathematical model of mobile robot and the state transformation are pre-
sented in section 2.2. In section 2.3, we propose the control strategy and establish the
stability and convergence properties of the closed loop system trajectories. Simula-
tion results are presented in section 2.4 to demonstate the capability of the controller

to converge the mobile robot to different desired postures.

2.2 Kinematic Model of Mobile Robot

The kinematic of a two-wheeled mobile robot, shown in Figure (2.1), is given as

:i: = cosQul (2.1a)
y' = sin6u1 (2.1b)
0.: 11.2 (2.1C)

where x and y denote the Cartesian coordinates of the robot and 6 denotes the
orientation of the robot with respect to the positive :1: axis. The control inputs ul
and uz are the linear and angular velocities of the robot, respectively.

In reference to Figure 2.1, it is assumed that 1:, y and 6 denote the current config-
uration of the robot. The desired configuration is assume to be zero. If they are not
zero, a coordinate tranformation can be used to set them equal to zero. Now consider

the following state and control transformation [31] of the kinematic equations given

 

 

x————_—

Figure 2.1. Diagram showing the state variables of the mobile robot

by Equation (2.1)

561:6

3:2 = xeosO+ysin6

1173 = xsinH — ycos6 (2.2)
v1 :u2

v2 2 ul — (:1: sinfi —— ycos 6)u2

It can be easily verified that the transformed kinematics takes the chained form [27]

1.71 2 ’U1 (2.33.)
(I12 = ’02 (2.3b)
i3 = 1:21)] (2.3c)

The task of converging the mobile robot from its present coordinates to the desired

coordinates is equivalent to the task of converging (1:1, :172, 173) to (0,0,0).

2.3 Almost-Smooth Time-Invariant Feedback

Control Strategy

2.3.1 Transformation of variables

For the purpose of controller design, we first transform the state and control variables
of the robot model in Equation (2.3). By treating the variables $1 and 2:2 in Equa-
tion (2.3) as Cartesian coordinates, we transform them into polar coordinates r and

1/2 as follows

271 2: roost/J
(2.4)

172 = rsinz/J, r20

The inverse transformation from (r, '27)) —> (131,122) given by Equation (2.4) is defined
everywhere. The forward transformation is undefined at the point ($1,232) 2 (0,0),

where r = 0. At all other points, the forward transformation is defined as

7‘ = (/$¥+r§ (2 5)

21) = arctan(:1:2/:r1), r750

By differentiating Equation (2.4) and using Equation (2.3) we get

I
l

v = :i: = rcost’; — TSlni/J it
1 1 . (2.6)
v2 2 1'72 2 rsini/J + rcosww
Substitution of Equations (2.4) and (2.6) into Equation (2.3c) results in
a, = 7210‘. me + 7720,1010 (2.7)
where 771 = 7‘ sin 1/2 cos d)
712 : —r2 sin2 l/J

We transform 11:3 in Equation (2.3) into a new variable, which we denote by 6, using
the relation

2

r 1
5 = 3:3 -/ 711d?” = (173 — ‘g'Sin’l/J COS 22>) : (333 — 55171132) (2-8)
0

Two important ideas behind the definition of [3 are that, first, 333 —> 0 when (r, [3) —> 0,
and, second, 6 is a function only of if, not i. In the transformed coordinates, the

kinematics of the robot takes the form

7' : U11 (2.93)
a} = w2 (2.9b)
[3 = ——(7‘2/2)w2 (2.9C)

where Equation (2.9c) was obtained by differentiating Equation (2.8) and substituting
Equation (2.7). The convergence of the mobile robot to its desired posture now refers
to the convergence of r and [3 to zero. Indeed, from Equations (2.5) and (2.8),

(m3) = (0.0) implies ($1,362,563) = (0.0.0)-

2.3.2 The feedback control strategy

The feedback strategy for the kinematic model of Equation (2.9) is proposed as

2111 = ar(c72,82 —— r2) (2.10)
102 = ”#3 (2.11)

where it is assumed that
r(0) 7‘- 0 (2.12)

10

and where a, 7 and a are positive controller parameters, chosen according to the
relation

7 > 2002 (2.13)

The controls for the robot, 1.21 and 212 defined in Equation (2.3), are related to the
control variables ml and 102 in Equation (2.9) by Equation (2.6). The control inputs

for the asymptotic convergence of the states of the robot are therefore proposed as

7‘ a 02/32 - 7'2 cos 'i/) — 7/3 sin 11'; if r > 0

,1 = { [ ( ) ] (2.14a)
0 if r = 0
Ta 02132—7“2 sin'tb+7[3cosi,/J ifr>0

v2 = { [ ( ) ] (2.14b)
0 if r = 0

The controller proposed in Equation (2.14) cannot stabilize the states of the system
to the origin from points where r(0) = 0, or 171(0) = 172(0) 2 0. Nevertheless, system
states arbitrarily close to, but not exactly on, the manifold of equilibria r = 0 can
be asymptotically converged to the equilibrium state. This is explained in the next

remark, and proved in section 2.3.3.
Remark:

The assumption in Equation (2.12) is reasonable since r(0) cannot be exactly equal
to zero. For the system defined by Equations (2.9a) and (2.10), it is apparent that
r = 0 is an unstable equilibrium point for )3 aé 0. Therefore, the assumption in
Equation (2.12) ensures that the system trajectories do not remain stuck at this

unstable equilibrium point.
Remark:

The controller in Equation (2.14) is (i) defined everywhere on R3, (ii) smooth on the

open (and dense) subset R3\{r = 0} of R3, and (iii) continuous at r = 0. Therefore,

11

they are almost-smooth [13] functions of the states of the mobile robot. Other con-
trollers [1, 2], using non-smooth transformations do not satisfy the third condition.
The controller in Equation (2.14) is also time invariant, as seen from Equations (2.5)

and (2.8).

2.3.3 Convergence and stability

In this section, we establish that the mobile robot converges to its desired posture
(231,352,123) 2 (0,0,0) from every posture satisfying Equation (2.12) for the control
inputs defined by Equation (2.14). To this end, we establish that (136) —> (0,0)
for the control inputs defined by Equations (2.10) and (2.11). We also show in this

section that the desired posture (331,272, :53) = (0, 0,0) is stable.
Theorem: (Asymptotic convergence to the desired posture)

Under the control law given by Equation (2.10) and (2.11), the mobile robot
described by Equation (2.9) will converge asymptotically from any initial posture

satisfying r(0) 76 0, to the desired posture. Specifically (r, [3) -—> (0, 0) as t —> 00.
Proof:

Substituting Equation (2.10) and (2.11) into Equation (2.9), the dynamics of (73,3)

are seen to be

7" = 07(0262 — 7‘2) (2.15a)
5 = —(%)r2[3 (2.15b)

It is apparent from Equation (2.15a) that r = 0 represents a set of unstable equilibria

for any fixed 6 75 0. Therefore, unless ('r, B) = (0,0), the robot can not practically

12

stay at r = 0. Let us therefore assume that r(0) ¢ 0 and show that (7‘, B) —> (0, 0) as
t —> 00.

Consider the state space X in (73 3) to be X = XOUX+, X0 = (0, 0), X+ : {(73 [3) :
r > 0}. In this space, define the Lyapunov function V in the following way. Pick
0 Z 0 and let (0,0) be a point on the r axis in the (73013) half plane, as shown in
figure 2.2. Draw a circle that is symmetric about the 7" axis and passes through (0, 0)
and (c, 0). Let V be defined such that its level set V = c is the circle, 7'.e.

V 2 V 2 . .
(3) :07) ”“32 (2'16)

Clearly, V _>_ 0 in X and V = 0 implies (736) = (0,0). Furthermore, if ||(73fl)|| ——>

 

(0.0)

 

Figure 2.2. Circle in the 7‘ — 0/3 plane defining the Lyapunov function V.

00, V —+ 00. Therefore, V is positive definite and radially unbounded. Note however
that, when 7' = 0, V is only defined for 6 2 0 since Equation (2.16) becomes (V / 2)2 2

(V/2)2 + 02732. This is not a problem since V is defined in the set X.

13

Before we calculate the derivative of V, we rewrite Equation (2.16) in the following

two forms

7'2 — 77V 2 —a2/32 (2.17a)

272 — TV 2 7‘2 — 0252 (2.17b)

Differentiating Equation (2.17a) with respect to time, substituting Equations (2.15)

and (2.17b), and rearranging terms yields

TV 2 277'“ — 7"V + 202fiB
2 07(27‘2 — TV)(02fl2 — 7‘2) — 70272/32

:- —a(a2fi2 — 7‘2) — 70272132 (2-18)

Hence 7V is negative definite in X and is equal to zero at (7‘, [3) = (0,0). Since V
is positive definite and radially unbounded in X, and V is negative definite in X,
we conclude that for all (7'(O),fi(0)) e X, V —+ 0 as t —+ 00. This implies that

(nfi) —+ (0,0) as t —> 00.
Remark:

The above theorem establishes asymptotic convergence of the mobile robot to
its desired posture (r,/3) = (0,0) or (2:1,:r2,:r3) = (0,0, 0). Even though V in
Equation(2.18) is negative definite, it does not claim stability of the desired pos-
ture because V was defined in the subset X,X = XOUX+, X0 = (0, 0), X+ = {(7, fl) :
7" > 0}, of the posture space. In the complete space of robot postures, we claim

stability of the desired posture with the help of the corollary below.

Corollary: (Stability of the desired posture)

14

Consider the dynamics of the mobile robot described by Equation (2.3) and (2.14).

The robot posture ($1,172, 273) = (0, 0, 0) is a uniformly stable equilibrium point.
Proof:

Define the Lyapunov function candidate

1 .- . . .3 2
V’ = § (If + .175 + (173 — 1:12”) ) (2.19)

Clearly V’ 2 0 and V’ = 0 implies (231,332,333) 2 (0,0,0). Therefore, V’ is globally

 

positive definite. Using Equation (2.3c), (2.4), (2.8), (2.13) and (2.14), the derivative

of V’ can be computed as

 

. . O 1 ’2 ' . 1 . 2 C
7! 1! 1! t 1!
V = 231:1:1 + 1721'2 + ($3 — ) (1:3 — -J:2 — —:I:1

: ($1721 + 17217-2) + -2-(:132'U1 - 1171112)

2 072 (02/3.2 — 7‘2) — (7/2) 72(32 (2.20)
= —07 (T2 + (l — a2) [32)
2a
3 0
The uniform stability of (1:1,:L'2,:r3) = (0,0,0) follows from the negative semi-

definiteness of V’ .
Remark:

The control strategy in this section uses the nonlinear oscillator in polar coordinates

7" = m“ (711V — 72) (2.21a)
2.17 = w (2.21b)

15

where W Z 0 is a function of the states of the robot. The oscillator frequency, denoted
by if), is a variable that converges to zero. This is evident from Equation (2.21) and
(2.11) and the fact that B converges to zero. As a result, the phase of the oscillator,

71), converges to some value, which is not of importance.

2.4 Simulation Results

Simulation results are presented here to demonstrate the capability of the controller
to converge the mobile robot from different initial configurations to the origin. The
initial configuration of the robot are given in table 2.1 and simulation results are shown
in figures 2.3 -2.7. Controller parameters for each simulation are given in table 2.2.
Of particular interest is Case B, where 7(0) 2 0. This violates the assumption in
equation (2.12). This problem is remedied by assuming the initial orientation to be

0.01 degrees instead of exactly zero.

Table 2.1. Initial configuration of mobile robot

 

 

 

 

 

 

a: y 0
Case A 1 1 0
Case B 0 l 0
Case C 0 1 180
Case D -1 0 180
Case E 2 2 0,90,180,-90

 

 

16

Table 2.2. Controller parameter used in simulations of mobile robot

 

 

a 7 a
Case A 0.02 1 1
Case B (a) 0.02 1 3.8
Case B (b) 0.02 1 11
Case C 0.02 1 1
Case D 0.02 1 8
Case E 0.02 1 1

 

 

 

 

 

 

 

0.5 - /

-—0.5 '-

 

 

 

X

Figure 2.3. Trajectory of the mobile robot for Case A.

17

 

 

 

 

 

 

 

Fig!“

Dinah

 

0.5 - -

 

 

 

 

 

 

 

—0.5 ~ ~
_1 u— -4
_15 1 1 1 L 1
—1.5 -1 ~05 0 0.5 1 1.5
X
(a)
15 I I I I I
1 *- _,
0.5 ~ «
> O 7- .1
-0.5 _ d
-1 - .1
_15 1 1 1 r 1
-1.5 -1 -05 O 05 1 1 5

Figure 2.4. Trajectories of the mobile robot for Case B with different set of control
parameter.

18

 

 

 

 

15 I f u r I
12 a
05*- a
> 0>- a
—0.5* ..
-17. .
—1.5 ‘ 1 1 1 l
—1.5 —1 -05 0 05 1 15

Figure 2.5. Trajectory of the mobile robot for Case C.

 

 

 

 

15 1 , T r .
1- 4
05~ -
-05» a
-1~ 1
-15 1 1 1 1 1
-1.5 -1 -05 0 05 1 15

Figure 2.6. Trajectory of the mobile robot for Case D.

19

 

 

 

 

 

 

A;

 

 

(a) (b)

 

 

 

 

 

 

 

 

(C) (0)

Figure 2.7. Trajectory of the mobile robot for Case E.

20

CHAPTER 3

Open-Loop Control of The

Rolling Sphere

3.1 Motivation

This chapter is motivated by the idea of the spherical robot. The spherical robot
has been conceived as a mobile robot with a spherical exoskeleton and an internal
mechanism for self-propulsion. It will also have retractable manipulators and/or
limbs that can be deployed through ports in the exoskeleton after the robot comes
to rest and can be retracted before the robot resumes its motion. It will also have
a retractable camera that can be deployed and turned to record an image around
the robot. With its unique structure, this robot will have a number of advantages
over conventional mobile robots and have a wide range of applications. For example,
with a fire-resistant exoskeleton, the robot can be useful in fire—fighting and rescue
missions. It can be used for inspection in hazardous environments, such as in chemical
factories and nuclear power plants. In every application, the superlative locomotion
capabilities of the robot will be an added advantage. The spherical exoskeleton will
provide the robot with the maximum stability. It will be able to move over rough

terrain with relative ease. This robot will be an ideal candidate for space exploration.

21

It can also find significant military applications. In addition to reconnaissance, it can
be used for terrorist countermeasures or as a robotic soldier in the line of fire.

In this thesis, an open-loop controller is presented for the spherical robot. We
will focus on the robot with a retractable camera, which, to be effective, must be
deployed from the top. Given an arbitrary point on the sphere, the basic idea behind
the open-loop strategy is to design a trajectory such that the point comes to the top
of the sphere when the sphere is commanded to a certain coordinate on the x-y plane.
Our open loop control strategy is based on the concept of geometric phase applied to
spherical triangular paths on the surface of the sphere. Later, the trajectories will be

generalized to arbitrary closed curves from triangular paths.

3.2 Review of Spherical Trigonometry

When a plane intersects with a sphere, it produces a circle. If the intersecting plane
passes through the center of the sphere, the circle is called a great circle. Any angle
on the sphere formed by two intersecting arcs of great circles is called a spherical
angle. . The portion of the sphere bounded by the arcs of three great circles is called
a spherical triangle. Three bounding arcs are called the sides and three vertices of
each spherical angle are the vertices of the spherical triangle. We shall designate
the vertices by RC), R, spherical angles by A, B, C and the corresponding opposite
sides by a, b, c respectively. Each angle of the spherical triangle is measured by the
dihedral angles made by the plane defining its sides. The spherical triangle is shown
in figure 3.1. In general, we shall consider only those spherical triangles of which each
side and angle is less than 7r. The important relations between the sides and angles

of the spherical triangle are

1. The sum of the angles of a spherical triangle is greater than 180° and less than

540°; that is, 180° < A + B + C < 540°.

22

 

 

Figure 3.1. A spherical triangle

2. If two angles of a spherical triangle are equal, the Opposite sides are equal; and

conversely.

3. If two angles of a spherical triangle are unequal, the opposite sides are unequal,

and the greater side lies opposite the greater angle; and conversely.
4. The sum of any two sides of a spherical triangle is greater than the third side.
Some basic formulas that relate sides and angles of a spherical triangle are

Law of Sines: In any spherical triangle ABC,

sin a sin b sin c

 

 

 

= .— 3.1
sin A sin B sin C ( )
Law of cosines for sides: In any spherical triangle ABC,
cos a = cos bcos c + sin b sin ccos A
cosb = cosccosa + sin csinacosB (3.2)

cos c = cosacosb + sin asinbcos C

23

Law of cosines for angles: In any spherical triangle ABC,

cos A = — cosBcosC + sinBsinCcosa
cos B = — cos C cos A + sin C sin A cos b (3.3)
cos C : — cos A cos B + sin A sin B cos 6

Some useful relations that can be derived from the law of cosines of sides, are

sinacosB = sinccosb — sinbcosccosA
sin bcos C = sin acos c — sin ccos a cos B (3.4)

sinccosA = sinbcosa — sinacosbcosC

sinAcosb = sinCcosB +sinBcochosa
sin B cos c = sin A cos C + sin CcosA cos b (3.5)

sinCcosa = sinBcosA + sin A cosBcosc

sinacosC = sinbcosc — sinccosbcosA
sinbcosA = sinccosa — sin acosccosB (3.6)

sinccosB = sinacosb — sinbcosacosC

sinAcosc : sinBcosC + sinCcosBcosa.
sin B cos a = sin C cos A + sin A cos C cos b (3.7)

sinCcosb = sinAcosB +sinBcosAcosc

24

 

 

 

The (191

 

em: sm
Ilw

angles 1

Gix

The derivation of these formulas can be found in most books on spherical trigonom-
etry, such as [3, 16, 34].
The area of a spherical triangle is proportional to the excess of the sum of its three

angles over the sum of two right angles, or

Area of Spherical Triangle ABC 2 (A + B + C — 7r)r2 2 E7“2 (3.8)

where E = (A + B + C — 7r) is the spherical excess and r is the radius of the sphere.
In general, if any three of the sides and angles a, b, c, A, B, C are given, the remaining
three can be found. In this thesis, we will consider solving the spherical triangle with
the knowledge of two sides (a, b) and the included angle (C). Deriving from Napier’s

analogies [16], the formulas for solving the spherical triangle are

 

 

 

cos%(A—B) : tan%(a+b) (3 9)
cos %(A + B) tan éc '
sin %(a -— b) = tan £15041 -— B) (3.10)
sm §(a + b) cot 5C

cos%(a—b) = tan%(A+B) (311)
cos %(a + b) cot 5C '

Given two sides and the included angle, a spherical triangle is uniquely determined.
The sum and the difference of the other two angles can be found using formulas (3.10)
and (3.11). Then the two angles can be found from the sum and the difference. The
third side can be found from formula (3.9). In other words, given a, b and C, the other

two angles A, B and the third side c can be obtained from the sequence of equations

 

sinl(a—b)
A—B =2 t 2
( ) arc an(sin %(a + b) tan éC)
cos%(a—b)

(A + B) = 2 arctan(

 

)

cos %(a + b) tan 5C

25

(A — B) + (.4 + B)
2
(.4 + B) — B
tan %(a + b) cos %(A + B)
cos %(A — B)

A = (3.12)

 

B

 

)

c = 2 arctan(

3.3 Sphere rolling along the spherical triangle

In this section, basic trigonometric identities and coordinate transformations are used
to compute the final location and orientation of a sphere resulting from following a
path specified by its own spherical triangle. Since a spherical triangle is a closed
path on the surface of the sphere, the point of contact with the ground after rolling
is the same point as the one before rolling. It implies that the sphere will undergo
some amount of rotation about the vertical axis passing through this point of contact
and the center of the sphere. Although the spherical triangle is a closed path on the
sphere, it does not generate a closed path on the ground. Our goal is to find the final
location, and the rotation of the sphere in terms of the dimensions of the spherical
triangle. This will enable us to plan the open-loop control strategy for the spherical
robot, using the concept of geometric phase.

Let us suppose that the center of the sphere in figure 3.2 is at the origin (point 1)
of the :ryz coordinate frame in figure 3.3. Also suppose that the point Q on the sphere
(figure 3.2) is at the bottom of the sphere. To roll along the spherical triangle PQR
in figure 3.2, the sphere is first rolled by the distance of a in the negative y direction.
This brings the sphere to position 2 in figure 3.3 and the point B in figure 3.2 is now
at the bottom of the sphere. The sphere is then rolled by the distance of b in the
direction angled C from the previous direction. This brings the sphere to point 3 in
figure 3.3. Finally, the sphere is rolled by the distance of c in the direction angled A

from the previous direction. This bring the sphere to point 4 in figure 3.3. The final

26

 

Figure 3.2. The spherical triangle that specify the path in figure 3.3

 

71y

‘ >

«b d 2
4

‘4/ c

a
A 3

2C b

 

Figure 3.3. Motion of the sphere defined by the spherical triangle in figure 3.2

27

pOSlllUli

o. The!

 

0111/2.
and 0 ('1

figure 3

Tim

1 1'

 

 

(our

position of the sphere in the x-y plane is represented by the distance d and the angle
(b. The distance d is measured from the starting position, or, in our case, the origin
of xyz. The angle (15 is measured clockwise from the first direction of rolling. Both d
and gb can be found in terms of the sides and angles of the spherical triangle PQR in

figure 3.3, using basic trigonometry. Following the path in figure 3.3, we get

dx : bsinC — csin(A + C)

(1,, = a — bcosC + ccos(A + C)

Therefore,

 

d2 = a2 + b2 + C2 + 2ac cos(A + C) —- 2abcosC — 2abcos A (3.13)
bsinC - csin(A + C)
(b are an (a—bcosC+ccos(A+C)) (3 )

Now, consider amount of rotation of the sphere. Since rolling along the spherical
triangle is equivalent to a pure rotation about the vertical axis, the rotation matrix
R, which relate the original coordinate and final coordinate of the sphere, is expected

to be in the form of

 

 

 

 

 

 

 

 

 

 

l -, ' ' 1 i . ‘ ' . ' ' . '
z z cosa sma 0 z
3’ : R j = — sin (1 cos a 0 3‘ (3.15)
12' is 0 0 1 is

l. _l L. .l .. 4 - .1 - .l

where a is the amount of rotation, i, 3“,]; are unit vectors along the fixed reference
system, and i’, j’,l}’ are unit vectors along the sphere fixed coordinate system. At
the initial time i’, j’, ic’ coincide with i, j, I}, respectively. We use vector rotation to
calculate the rotation matrix R. Recall that each row of matrix R is the rotated

coordinate represented in the original coordinate system. For example, the first row

28

AAA

of matrix R is the i” coordinate represented in the original coordinate system ijk.
Hence, we can use vector rotation to find each individual 3,3" or 12' in terms of f, j
and I} and, then, determine the entries of the rotation matrix R.

From vector analysis, rotation of a vector 7 about a vector 1 by an angle of 07

results in the vector 7’, defined by [14]

7’: (1 —cosa)(l-7)l+cosa-7+sina(l X7) (3.16)

For convenience, we assume the sphere to have unit radius. The results can
however be extended to a sphere of radius other than unity. We have assumed that
the sphere-fixed coordinates described by the unit vectors 5’, j’, 12’ are initially coincides
with the reference coordinates 5,5,1}. Therefore, rolling of the sphere along side a of
the spherical triangle, or from point 1 to point 2, in figure 3.3, on the ground is equal
to a rotation of the sphere fixed axis about 2' by the angle of a. Rolling from point
2 to point 3 on the ground, as seen in Figure 3.3, is equal to a rotation about the
axis [cos Ci — sin C j] by the angle of —b. Finally, rolling from point 3 to point 4 is
equal to the rotation about the axis [cos(A + C)i — sin(A + C) j] by the angle of c.
The calculation of vector rotation is done using Mathematica. The code is shown in
Appendix A. Using spherical triangle identities (Appendix B) to simplify the results

from Mathematica, the rotation matrix becomes (Appendix C)

r- -

—cos(A+B+C) sin(A+B+C) 0
R: ——sin(A+B+C) —cos(A+B+C) 0 (3-17)
0 0 1

n d

 

 

29

01‘

cos(7r—(A+B+C)) sin(7r—(A+B+C)) O
R: —-sin(7r—(A+B+C)) cos(7r—(A+B+C)) 0 (3-18)

 

 

0 0 1

d

Comparing the matrix R in equations (3.15) and (3.18), the amount of rotation of

the sphere about the vertical axis is determined to be
a=7r—(A+B+C) (3.19)

From equation (3.8), we can now say that by following the spherical triangle path
in figure 3.3, the coordinate of the sphere rotates about 2 axis by an angle equal to
negative of the area of the spherical triangle, assuming that the radius of the sphere
is unity.

a = 7r — (A + B + C) = — (Area of the Spherical Triangle) (3.20)

If we consider a sphere of radius 7, 7 7é 1, the analysis presented above changes
marginally. When the sphere of radius 7 rolls from point 1 to point 2 on the ground,
the coordinates of the sphere rotate about the 2' axis by the angle (a/r). Rolling
from point 2 to point 3 is equal to a rotation of the coordinates about the axis
[cos Ci — sin C j] by the angle (—b/7). Rolling from point 3 to point 4 is equivalent
to rotation of the coordinates about the axis [cos(A + C )i — sin(A + C) j] by an
angle (c/7). In summary, for a sphere of radius 7, the set of angles used in our
analysis of coordinate rotation (Appendix A and C) is changed from a, b, c, A, B, C to
(a/7), (b/7), (c/ 7), A, B, C, respectively. Note that angles A, B and C are not changed
and the spherical triangle identities in Appendix B still hold good. Since A, B and
C are unchanged, the only change in Appendix C is affected by replacing a, b and c
with ((1/7), (b/7) and (c/r), respectively. At the end, the rotation matrix resulting

from simplification is the same as the matrix in equation (3.17) since it depends only

30

on A, B and C. The amount of rotation can still be expressed as in equation (3.19).
However, by comparing with the area of the spherical triangle in equation (3.8), we

can state that for a sphere of radius 7, the rotation about the z axis is equal to

 

(3.21)

a : 7r _ (A + B + C) = _ ( Area of the Spherical Triangle)

7‘2

One may question the amount of rotation if the sphere were to roll to the right

instead of rolling to the left, as the path in figure 3.4. Considering figures 3.3 and

 

XV

 

 

Figure 3.4. Path generated by the spherical triangle similar to the one in figure 3.3
but with opposite direction.

3.4, it is not hard to see that the analysis of the path in figure 3.4 is similar to the
one in figure 3.3 except that the spherical angles are negative of each other. In other

words, we have to use —A, —B and —C as the spherical angles in the rotation matrix

31

for the path in figure 3.4. The rotation matrix R in equation (3.17) would become

)- -I

— cos(—A — B — C) sin(—A — B — C) 0
R = — SiII(—A — B — C) — cos(—A — B — C) 0 (3-22)

 

 

0 0 1

h- d

01'

n

[ cos((A+B+C)—7r) sin((A+B+C)—7r) 0
R: —sin((A+B+C)—7r) cos((A+B+C)—7r) 0 (3.23)

 

 

0 0 1

I-

For the path in figure 3.4, the rotation of the sphere is equal to the area of the

spherical triangle divided by square of the radius, or

Area of the Spherical Triangle

 

a=A+B+C—W= QM)

7-2

The path in figure 3.4 will be refered to as a right-handed path, since, to an observer
within the sphere, the sphere always turns to the right. The path in figure 3.3 is a
left-handed path.

In summary, when a sphere rolls along the path specified by its own spherical
triangle, the final location of the sphere can be calculated from equation (3.13) and
(3.14). If the path is right-handed, the amount of rotation of the sphere about the
vertical axis is equal to the area of the spherical triangle divided by the square of the
radius of the sphere. If the path is left-handed, the amount of rotation is negative of

the area of spherical triangle divided by the square of the radius of the sphere.

32

3.4 Sphere rolling along lune

In this section, we consider the effect of rotation of the sphere along a closed path
given by the two sides of a lune. A lune [34], as shown in figure 3.5, is the portion of

the surface of a sphere, which is comprised between two great circles. Two sides of a

E

Figure 3.5. The Lune

lune make a closed path on the surface of a sphere. Both sides have equal length of
77. Both angles ,which are formed by the two great circles, are equal. The area of a

lune is proportional to its angle and is given as
Area of a Lune = 2A72 (3.25)

where A is the angle of the lune. The cartesian path generated by a sphere rolling
along a lune is shown in figure 3.6. First the sphere rolls by the distance of 77.
Then it changes the direction by the angle of A and rolls for the distance of 77.
The coordinate of the sphere at position 2 in figure 3.6 can be found easily. Assume

that the coordinates of the sphere at location 1, 51 jlfcl, coincide with the reference

33

coordina

 

cwrdina

10 11111:

l'fi‘f‘to

 

 

coordinate, zjk. Since the coordinates rotate by an angle 7r about the :2: axis, the

coordinates of the sphere at point 2 in figure 3.6 are

K;
N
H
l
E.)
||
|
K)

To find the coordinates of the sphere at final position (3 in figure 3.6) we again use

 

A y
l N.
x
3
a nr"
\ A m
2

 

Figure 3.6. Cartesian path given by the Lune in figure 3.5

vector rotation in equation (3.16). The axis of rotation is

l = — cos A? + sin Aj (3.26)

34

Therefore,

i3 = cos(2A)i — sin(2A)j

jg = sin(2A)i + cos(2A)j (3.27)

>

- I}

' 3

The relation between the coordinates i3j3k3 and ijk can be written in matrix form as

 

 

 

 

 

 

 

 

 

33 = R 3“ (3.28)
_ ’“J . l l .’“ l
where the rotation matrix
[ . , ' r . -
cos 2A -sm 2A 0 cos(-—2A) s1n(—2A) 0
R = sin 2A cos 2A 0 = —sin(—2A) cos(—2A) 0 (329)
_ 0 0 1 J . 0 0 1 ]

 

Comparing equations (3.15) and (3.25), the amount of rotation of the sphere is given

bv

I/

(3.30)

 

a ___ _2‘4 :. _ (Area of a lune)

7.2

Note that this amount of rotation is derived from the left-handed path in figure 3.6.
Similar to the spherical triangle, if the path is right-handed, the amount of rotation
about the vertical axis is equal to the area of the lune divided by the square of radius.

For a right-handed path, we have

0:2442Area ofalune (3.31)

7‘2

 

35

3.5 Sphere rolling along an arbitrary closed path

The results of section 3.3 indicate that when a sphere of unit radius rolls along a
spherical triangle, the net rotation of the sphere is equal to the area of the spherical
triangle. The direction of rotation is positive for a right—handed path and negative
for a left-handed path. The result for a two-sided curve in section 3.4 is similar, the
amount of rotation is equal to the surface area enclosed by the two sided curve. In this
section, we attempt to generalize this result to arbitrary closed paths on the sphere.
First consider the simple spherical polygon in figure 3.7. This polygon 12345 can be

divided into three spherical triangles by the arcs 13 and 14. The final orientation

 

Figure 3.7. Arbitrary Spherical Polygon

of the sphere rolling along the path 123451 is the same as rolling along the path
1231341451 since paths 313 and 414 do not change the orientation of the sphere.
Path 1231341451 is composed of three spherical triangular paths. When following
each spherical triangle, the sphere rotates by an amount equal to the area of that

spherical triangle. All rotations are positive since the spherical triangles are right—

36

 

 

ha

[0

handed according to the definition in section 3.3. Clearly, the effect of the sphere
rolling along the spherical polygon is a rotation by an amount equal to the surface
area of the spherical polygon. If the path is right-handed, the rotation is positive and

if it is left-handed, the rotation is negative.

 

Figure 3.8. Arbitrary closed path that crosses itself

Now, consider the path in figure 3.8 that crosses itself. We can represent such
paths by two consecutive spherical triangular paths. First, the sphere rolls along
the spherical triangle path 1231 and then path 1341. The right-handed path 1231
results in the rotation of the sphere by an amount equal to the sum of area 11 and
III. The left-handed path 1341 cause the sphere to rotate by an amount equal to the
sum of area I + III in the negative direction. Thus, the sphere rolling along either
path 12341 or 1231341 has the amount of rotation equal to the difference of areas
II and I. For an arbitrary path that crosses itself, the surface area bounded by the
path can be divided into two areas at the cross point. The amount of rotation of the
sphere rolling along the arbitrary path equals the difference of the two areas. The

direction of rotation depends on the direction of the path. If the area is bounded by

37

the right-handed path is larger than the one which is left-handed, the total rotation
is positive. If area bounded by left-handed path is larger, the net rotation is negative.

Any closed curve on the surface of a sphere can be always represented by an n-
sided polygon, where n is a large number. Therefore, it can be deduced that the
amount of rotation of the sphere after rolling along any closed curve is equal to the

surface area enclosed by the curve on the sphere.

3.6 Kinematic Model of Sphere

In view of the specific control problem we have defined for the sphere, namely
control of the sphere to a certain :r-y coordinate such that a certain point on the
surface of the sphere appears on the top, we propose the kinematic model in terms
of four state variables. Two of these states include the two angles (0 and a) that
represent a certain point (p) on the surface of the sphere. The other two states
denote the coordinates of the center of the sphere (2:0 and yo). The variables are
shown in figure 3.9. The angle a is measured from the :1: axis on the 7-3; plane as
shown in figure 3.9(a) and 0 is measured from the vertical axis in the :r’-z plane as
shown in figure 3.9(b). The two control inputs are the angular velocities in .7: and y

direction, namely (w, and toy). The kinematic equations for .70 and yo are

:ito 2 may (3.32)

y'o : ~7w$ (3.33)

where 7 is the radius of the sphere. The derivative of 6’ with respect to time is the

angular velocity in the y’ direction. Therefore, we have

9 = —wx sin oz + wy cos a (3.34)

38

 

 

 

 

 

 

//7////////

(b)

Figure 3.9. Figure defining the state variables of a sphere

39

The differential equation for 07 can be found from the velocity of point p, namely
(i = — cot 6w; 2 — cot 9(a); cos a + my sin a) (3.35)

The kinematic equation for a in equation (3.35) has the problem of singularity at 6 =
0. This is unavoidable since 07 is undefined at 6 = 0. However, with equation (3.35),
the range of a is not limited within —7r to 7r. Therefore, to find a, we track point p
and, from relative position of point p with the center of the sphere, we compute 0.
Consider figure 3.9(b), since velocity of point q is zero, the velocity of point p can be

written in vector form as

_o

v. = x a.
where (13 is the angular velocity of the sphere
L3 = taxi + wyj
and RPM is the position vector from point q to point 1)
RPM = (7 sin 0 cos (1)2 + (7 sin 0 sin a)j + 7(1+ cos 0)k (3.36)
Thus,
VP 2 wy7(1+ cos 0)i — wx7(1+ cos 9)j + ((1217 sin 03in a — wyr sin 0 cos a) k (3.37)
Therefore, the velocity of point p in m and y directions are
17,, = wy7(1 + cos 9) (3.38)

7],, = —wx7(1 + cos 6) (3.39)

40

At any instant of time, the angle a can be found from the relation

07 = arctan (M) (3.40)

3.7 Formulation of Open Loop strategy

In this section, we formulate an open loop control strategy that can lead the sphere
to the desired final position and orientation. This contributes positioning the sphere

at the origin of the 57-3) coordinate, in figure 3.10, and locating point P at the top of

 

 

Y A)
5 —-
/ I” \‘\\
\
”—-/“ \\
l’ ’ x \
4 I, I \ \
/ I \\ P(2)\
I, I, \ l’fl \‘
7 [ P(6) ‘ ” l a
l r x
‘7 I P(3) 2,6 I
' ‘ 3 i I’
\\ \\ I I,
\ \\ [I I,
\ \\‘ ” ——-‘
\\ :/--’ ” “\
I
I \\
’ \
’ \
l 1
] I
\ 1 ,’
\ I
\ I
\ /
\ I
\ /
\ x
\ /
\\ I

Figure 3.10. Open loop control strategy of spherical robot

the sphere. It can also include an arbitrary orientation of the sphere with point P on

41

the top. At the initial configuration of the sphere, shown by point 1 in figure 3.10,
point P is arbitrary located on the sphere, P( 1). The control strategy consists of
three main steps. First, the sphere rolls from the initial configuration to the origin of
the 7—31 coordinate frame, denoted by the straight line path from point 1 to point 2
in figure 3.10. Point P is then located at a position other than the top of the sphere,
P(2). Second, the sphere rolls away from origin 33-31 in order to move point P to the
top. This is shown by the straight line trajectory from point 2 to point 3. Finally, a
spherical triangle is used to transfer the sphere to the desired location, point 6, while
providing the proper amount of rotation. At this location, point P is on the top and
center of the sphere is back at the origin of 23-3; frame.

The first two processes are trivial to implement. Therefore, in this section, we
concentrate only on the third step which is the motion along the spherical triangle.
The task is to find a spherical triangle that can lead the sphere from stage 3 to stage
6, in figure 3.10, and give the desired amount of rotation to the sphere. We know from
section 3.3 that when the sphere roll along the spherical triangle PQR, in figure 3.2,
the final position and orientation of the sphere are given by three variables (1, (b and
a. Distance (1 and angle (b represent the final position of the sphere and can be found

by solving equations

 

d2 = a2 + b2 + c2 + 2accos(A + C) — 2abcosC — 2abcosA (3.41)
bsinC—csin(A+C)

= . t 3.42

(b arc an (a-bcosC+cos(A+C)) ( )

Distance d is measured from the initial position and, with our control strategy, is
limited to the maximum value of 7r for unit radius sphere. d) is measured from the
direction in which the sphere begin to roll.

The amount of rotation about the vertical axis a can be found by equation (3.21)

or (3.24) depending on the direction of rolling. In our simulation, for convenience, we

42

will assume the sphere to have unit radius. Therefore, the amount of rotation of the

sphere rolling along the spherical triangle ABC is

a = i(A + B + C — 77) = iArea of the spherical triangle (3.43)

The rotation is positive for the right-handed path and negative for left-handed path.

Our task is to find the spherical triangular path that results in the desired final
position and orientation of the sphere. In other words, given the value of d, qb and
07, we need to find sides a and b and angle C of the spherical triangle that satisfy
equations (3.41)-(3.43). Along with these equations, the set of equations (3.12) in
section 3.2 are also used to find the other three remaining angles A, B and side 0 of
the spherical triangle. To get an idea of achievable values of (1, ¢ and a, we compute
them by inputting value of a, b and C within their range, 0 to 180°, into equations
(3.12) and (3.41)—(3.43). Figure 3.11 show the possible value of d and a. If the desired
value of d and a lie outside the dark band, we are not able to find a spherical triangle
that satisfy those desired value. A set of spherical triangles will then be needed to
satisfy the desired value of d and a. The d and gb plot and the 07 and (b plot are shown
in figures 3.12 and 3.13, respectively. Recall that 9b is measured from the direction
the sphere begins to roll and we have the freedom to choose this direction, denoted by
7/) in figure 3.14. The desired value of (b can then lie outside the band in figures 3.12

and 3.13 since it can be corrected by changing the direction the sphere start to roll.

The set of equations:, (3.12) and (3.41)-(3.43), are nonlinear and difficult to solve
for a closed form solution of a, b and C in terms of d, (b and (1. Therefore, we use
numerical methods to solve for a, b and C when the desired value of d, qb and a are
given. The F ortran programs are given in Appendix D.

In the simulations, the magnitude of the angular velocity of the sphere is set

43

 

400 1 , , , , T

350

250

alpha (600)
N
8

150

 

 

 

 

Figure 3.11. Possible values of d and a for one spherical triangle

 

Phi (090)

 

 

 

 

Figure 3.12. Possible values of d and (,b for one spherical triangle

44

 

 

 

 

 

803 in

400

350

200 250

alpha

150

 

Figure 3.13. Possible values of a and d) for one spherical triangle

 

 

Figure 3.14. System variables used in simulation

45

to 0.1 rad/sec. We present simulation results of three cases with different desired

configurations. The configurations of the sphere for these simulations are shown in

table 3.1.

Table 3.1. Numerical values used in simulations 1, 2 and 3 for open loop control of

the sphere rolling along spherical triangles.

 

 

 

 

 

 

Parameter Simulation 1 Simulation 2 Simulation 3
yd 0.5 -1.5 0.5
ad —45° 90° 50°
dd 0.51 1.8 0.7071
aid 78.69° —56.3099° 45°
()6 100° 80° 90°
2,”; —21.31° 23.6901° 135°
a 1.81022 1.8618 2.27049
b 2.30807 1.19845 2.93917
C 30.1449° 95.4131° 26.1455°
Direction left-handed right-handed right-handed

 

 

 

 

 

 

In the first simulation, the desired location is set at .1") = 0.1, yd = 0.51 and ad =
—45 degree. This desired location is equivalent to the desired value of d = 0.5, a) =
78.69° and a = —45°. From figure 3.11, the desired location lie inside the band. Thus,
we can use one spherical triangle to accomplish the task. However, from figure 3.12
and 3.13, the desired location lies outside the dark band. We then used the value d)
of 100° to solve for the spherical triangle. This modification in (15 can be corrected
by using 7/2 = 78.69° — 100° = —21.31°. Since the desired amount of rotation (a) is
negative we use a left-handed path. Two sides and the included angle of the spherical
triangle were numerically solved and were found to be a = 1.81022, b = 2.30807 and
C = 30.1449°. The trajectory of the sphere is shown in figure 3.15 and the system
variables are shown in figures 3.16 and 3.17. In figure 3.16, all variables reach their

desired values as expected. Figure 3.17 shows that the point on the top of the sphere

46

comes back to the top after rolling along the spherical triangle path.

 

-1»-

-2—

 

 

 

Figure 3.15. Trajectory of the sphere in simulation 1.

In the second simulation, the desired configuration is chosen as 23,) = 1, yd = —1.5
and ad 2 90° or, equivalently, d = 1.8, gb = ——56.3099° and a = 90°. The spherical
triangle is computed by setting ()6 = 80°. Therefore, the start direction is if) =
23.6901°. Two sides and the included angle of the spherical triangle are computed to
be a = 1.8618, b = 1.19845 and C = 95.4131°. The trajectory is shown in figure 3.18.
Figures 3.19 and 3.20 show that all system variables converge to their desired value.
The path is right-handed since the desired amount of rotation is positive.

In the third simulation, the desired configuration is set at $4 = 0.5, yd = 0.5 and
ad 2 50° or, equivalently, d = 0.7071, 9b = 45° and a = 50°. The spherical triangle is
computed by setting ()5 = 90°. Therefore, the start direction, 21) is set at 135°. The

three computed parameters of the spherical triangle are a = 2.27049, b = 2.93917 and

47

 

 

 

 

 

 

 

 

 

2 r l I r r r r f I
15" "
U ‘1- -1
0.5" ..
o l l I l I l L L l
0 5 10 15 20 25 30 35 40 45 50
100 r I r r r r I r r
50" .,
E
Q
0" ..
-50 l l i l L l l l l

 

 

 

 

 

time

 

 

 

 

 

 

 

 

 

 

 

 

 

150 I I I I I I I I I
100» -
a
'6
'5
50" ‘1
o l l I l l l 1 l 1
0 5 10 15 20 25 30 35 4O 45 50
A3 I I I T I T r I I
8 ’ A ‘ \
.C / ‘
§2L ’z’ ’ \‘\\ "
K I] \ \
X. , \
551*- III ‘\\ ..
K / \ ‘ ‘
o l L 1 L L l l 1
0 5 10 15 20 25 30 35 40 45 50
3
Z 4
GD
0
E
g- -1
£5
‘5 -1
GD
‘5'.
_2 1 L 1 L 1 1 1 1 1
O 5 10 15 20 25 30 35 4O 45 50

Figure 3.17. Variables 6, :73, slip, y, 3),, from simulation 1 for Open loop control of sphere.

48

 

 

 

 

>. o»- 1
-1 — -
-2 F ..
-3 1 1 1 1 1

-3 -2 —1 0 1 2 3

Figure 3.18. Trajectory of the sphere in simulation 2.

 

 

 

 

3 l I I I I I I I I
2 r — -
O
1 *' _
0 l l l l l I l I l
0 5 10 15 20 25 30 35 40 45 50
50 I r T r r r I r 1

 

 

 

 

 

 

 

 

 

0 5 10 15 20 25 30 35 40 45 50
200 I I I I I I r I I
1m >- q
a
5 o r
a
_1 w '- u
~200 l l 1 l l l l l l
0 5 10 15 20 25 30 35 40 45 50

Figure 3.19. Variables d, d) and a from simulation 2 for open loop control of sphere.

49

theta

 

‘50 I I r I I I I I I

 

 

 

 

 

 

 

 

100~ 1
507
0 1 1 1 1 1 1 1 1 1
0 5 1O 15 20 25 30 35 40 45 50
4 r r r 1 r v r T r
6‘
2
e3" ——————— \~ ‘
E ,/” \‘\
92’- // ‘\ "
.6: ”/ \\\
61v ,” 2
m I
‘5? z’
o I 1 l l l l l l l
O 5 1O 15 20 25 30 35 4O 45 50
2 I I I T I I 7 I I
.1
fl
.1

y(soud).yp(dashed)

 

 

 

 

 

-2)-

p-
p-
h-

 

 

 

-3 l 1
-3 -2 -1 0 1 2

Figure 3.21. Trajectory of the sphere in simulation 3.

50

 

 

 

 

 

 

 

 

 

3 I I I I I
2 ’- ..
‘D
1 ’- _
o 1 1 l l 1
0 10 20 30 40 50 60
150 1 I I T I
100 ~ '1
E
Q
50 - "
0 l 1 l l l
0 1O 20 30 40 50 60

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

150 r r r I r
100'" '4
S
0
E.
507 T
o 1 1 1 I 1
0 10 20 30 4O 50 60
2 I I I I I,\
E /’ ‘x
(n 1’
.8 0"\‘ ’x "
a K“ (II
x. ”~\‘ ”’
§-2— ‘ ~~~~~~~~ " .
‘6
‘D
Y
-4 l I l l l
0 10 20 30 4O 50 60
A3 I I I I I
U
0
5 ..-_
‘02- z’ ‘‘‘‘‘ '1
3 ’7 ~~-‘
& ’/’ ‘\‘
§1_ // \\\‘ -1
25 / ~L‘
In I
‘S ,’
o l I l l l
0 10 20 30 40 50 60

Figure 3.23. Variables 6,113,:rp,y,yp from simulation 3 for open loop control of sphere.

51

C = 26.1455°. The trajectory is shown in figure 3.21. Figures 3.22 and 3.23 show
the plot of all variables with respect to time. Figure 3.22 shows that all variables
converge to their desired value. Figure 3.23 shows that point p converges to the
top of the sphere. The path is right-handed since the desired amount of rotation is

positive.

3.8 Additional simulations supporting open-loop
control strategy

In the open loop strategy, we used spherical triangles to plan the trajectory. However,
any closed path on the sphere can be used to achieve the same task. While we have
proved in section 3.5 that the amount of rotation of the sphere rolling along any
closed curve equals to the surface area on the sphere bounded by the closed curve,
we present additional simulations to support this claim. We show that the amount
of rotation of the sphere rolling along any closed curve on its surface equals to the
surface area bounded by the closed curve, To this end, we characterize an arbitrary

closed curve by the relation of two angle, aa(t) and 90(t) in figure 3.24 where
0.10) = 6.1m 1344)

where t f is the final time.

To find the angular velocity that drives the sphere along the given closed curve, we
first assume that there is a pendulum attach at the center of the sphere and always
hanging downward. When the sphere rolls along the closed curve, the end of this
pendulum tracks the closed curve. From an observer inside the sphere, an angular

velocity is needed for the pendulum to track the closed curve. This angular velocity

52

 

Figure 3.24. Parameters of closed curve

can be written as
(Iipend/sphere = —éa sin aai’ + 00 cos aaj’ + dalt’ (3.45)

In the expression above,'we use primed coordinates since these coordinates are ro-
tating with the sphere and not fixed inertially. For the sphere to roll such that the
pendulum stays hanging down, the angular velocity of the sphere must be negative

of that of the pendulum.
Qaphere : _wpend/sphere : 9a Sin 001’ _ 0a COS aaj’ _ dak, (346)

However, the input angular velocity of the sphere should be based on the reference
coordinate system ijic. This coordinate system is different from the coordinate i’j’ic’
since when the sphere rolls, the coordinate i’j’l’t’ rotates with the sphere. We transform
the angular velocity in equation (3.46) into the coordinate system ijl} in the following

manner. We keep track of coordinates It’, with angles 07 and 9, and i’, with angles a2

53

and 62. Thus,

A

k' = sin6cos of + sin6sinaj + cos 6i: (3.47a)
i' = sin 62 cos 02% + sin 62 sin Ofgj + cos 62]} (3.47b)
j’ = ic' x i’

2 (sin 6 sin a cos 62 — sin 62 sin 02 cos 6)i

+ (sin 62 cos (12 cos 6 — sin 6 cos (1 cos 62)j

A

+ (sin 6 cos ()1 sin 6;; sin a2 — sin 62 cos 012 sin 6 sin a)k (3.47c)
From equation (3.46), the angular velocity of the sphere in the world coordinates is

433phere = [6,, sin an sin 62 cos 02 —— (2,, sin 6 cos a
-6(, cos aa(sin 6 sin 01 cos 62 -- sin 6;; sin (12 cos 6)]2
+[6a sin an sin 62 sin (12 — a, sin 6 sin a (3.48)
—6a cos aa(sin 62 cos (12 cos 6 — sin 6 cos 0 cos 62)]j
+[6a sin 00 cos 62 -— ('10 cos 6

A

—6a cos aa (sin 6 cos a sin 62 sin (12 — sin 62 cos (12 sin 6 sin 0)]k

Since the sphere cannot turn while standing, the angular velocity in .h’ direction is
set to zero. The area enclosed by an arbitrary closed curve can also be obtained
analytically. In reference to figure 3.25 the area bounded by the closed curve 60 =

f (ad) is found by the double integral

0a(tf) f(0a)
A = f / sin a, do, daa (3.49)
6

00(0) 0(0)

54

 

 

 

 

Figure 3.25. Surface integral for arbitrary closed curve on sphere

We present two simulations, 4 and 5. The closed curve in simulation 4 is given by
equation

6a(t) =7r—sinaa(t), 01(0) =O,a(tf) =7r

In simulation 5, the closed curved satisfy
6a(t) = 7r — sin aa(t) + 0.3 sin 500(t), 0(0) 2 0, a(tf) = 7r

The simulation results are shown in table 3.2 and figures 3.26-3.29. Figures 3.26 and
3.28 show trajectories of center point and the point initially at the top of the sphere.

Figures 3.27 and 3.29 shown the orientation of the sphere.

55

Table 3.2. Results from simulations 4 and 5 of the open loop control of the sphere

rolling along arbitrary closed paths.

 

Surface Area

Amount of rotation

 

 

 

 

 

 

 

 

 

60(aa) from obtained through
Integration numerical integration
(radian)
7r - sinaa 0.7376 0.7376
7r — sinaa + 0.3 sin 50,, 0.7912 0.7912
2 fl T
1 8 ’ .- F F \ ‘ x \
\\
\
16 \ a
\
\

1.4 \\

\
1 2 “ a

l
>. 1 a
/

0 8 /

/
06 — /
04 ’1” ~
0.2 , x ”

0 L 1 4

—08 0 6 0.8 1 1.2

 

 

 

Figure 3.26. Trajectory of the sphere in simulation 4. Solid line represents trajectory
of center of the sphere, dashed line represents trajectory of point P.

 

 

 

 

 

 

 

 

 

 

1 r r r 1 r r r T r
08" "
206'- "
i3
04» ~
0.2- ..
0 1 r r l 1 r L 1 r
0 2 4 6 8 10 12 14 16 18 20
0
-02..
2
9 —04*
(U
-06»-
-03 r r r 1 L r r i 1
2 4 6 8 10 12 14 16 18 20
time

Figure 3.27. Variables 6 and a from simulation 4 for open loop control of sphere.

 

2.5 r r r I
["\
/ \\

2" l/ \ ..

I, \

I \

/ \

I \ ’,/“-‘\\
15" t \\ ’r” \ -4

 

 

 

 

)~ 1!- l/ -
/
I
,l
I
[I
05- 1
Or- ..
_05 1 1 1 1
-1 ~05 0 05 1 15

Figure 3.28. Trajectory of the sphere in simulation 5. Solid line represents trajectory
of center of the sphere, dashed line represents trajectory of point P.

57

 

14 1 T l 7 I I 1 1 I

12» -

0.8 - -*

heta

«oer .
04 p- -<
0.2 "' ..

 

 

p—
p-
p-
p.
..

 

 

 

 

 

 

Figure 3.29. Variables 6 and a from simulation 5 for open loop control of sphere.

58

CHAPTER 4

Conclusion

Rolling without slipping is a nonholonomic constraint that is quite common in me-
chanical systems. These constraints are not integrable, and can not be used to solve
for a set of independent generalized coordinates. Motion planning for nonholonomic
system is more difficult than that for holonomic systems, since independent general-
ized coordinates do not exist. Therefore only motions that satisfy the instantaneous
nonholonomic constraints are feasible. Stability problems for nonholonomic systems
are also interesting since there exists no smooth time-invariant static-state feedback
that can asymptotically stabilize nonholonomic systems [7].

In this thesis, we present two control strategies for two nonholonomic systems,
namely, the two—wheeled mobile robot and the rolling sphere. For the mobile robot, we
present an almost-smooth time-invariant state feedback law that can asymptotically
stabilize the mobile robot from practically any initial posture to the desired posture.
The only exception is an equilibrium manifold of measure zero (containing the desired
posture) from where the robot cannot move. All points on the manifold (except the
desired posture) are however unstable and therefore any posture arbitrary close to,
but not exactly on, the manifold can be asymptotically converged to the desired
posture. This gap in the posture space is not a limitation of the controller. Our

controller has an ability to turn the robot more than 360 degrees between initial

59

and final configurations, which will be useful for tethered mobile robots in factory
automation. Our controller design, which is based on the kinematic model of mobile
robot in chained form, provides a means of generalization to other nonholonomic
systems. Our controller is motivated by the idea of a nonlinear oscillator, which
has not been previously used for nonholonomic systems. The results presented here
show that the nonlinear oscillator can be creatively used to control nonholonomic
systems. The extension of the controller to higher order nonholonomic systems does
not seem trivial and further investigation is required in this direction. The effect
of control parameters to the trajectories of the system is needed to be investigated.
Selection of the control parameters to optimize trajectories of the system still requires
further studies. The implementation of the closed loop strategy to the mobile robot
experiment is needed to be done.

For the case of the rolling sphere, which has higher order than the two-wheeled
mobile robot, we proposed a motion planning scheme using spherical triangles. The
goal is to control the sphere to the desired location while bringing a certain point
on the surface of the sphere to the top. The basic idea is that when the sphere rolls
along its spherical triangle, it moves to another location with a pure rotation about
the vertical axis. This location, which can be found from plane trigonometry, is a
function of the sides and angles of the spherical triangle. The amount of rotation
is equal to the surface area of the sphere bounded by the spherical triangle divided
by the radius of the sphere squared. This equality is also shown to be true for
arbitrary closed curves on the sphere. The formulation along with simulation results
is presented to demonstrate the control strategy. However, further studies need to
be done in order to use closed curve on the sphere in open loop control. Closed
loop control for rolling sphere has been studied but the effective control strategy has
not been found. Control strategies for the different final configuration have to be

investigated. The improvement of locomodation dynamics of rolling sphere is also

60

needed. The design and development of a spherical robot and its internal mechanism

is required before an implementation of the control strategies.

61

APPENDICES

APPENDIX A

APPENDIX A

Program for Calculating Vector Rotation

The Mathematica program used to calculate the vector rotation in equation (3.16)
is presented in this section. The final set of coordinates are 2'3, jg, and k3. They
correspond to coordinates i’, j’ and It' in section 3.3, respectively.

Inll]:= i={1, 0, 0};
j = {0, 1, O};
k: {0 OI 1};
ll=i
11 s (l- Cos[a]) (11. i) 11+Cos[a] 1+Sin[a] (Cross[ll, i])
jl = (1- Cos[a]) (11. j) 11+Cos[a] j +Sin[a] (Cross[11,j])

k1 = (1- Cos[a]) (11. k) 11+Cos[a] k+Sin[a] (Cross[ll, k])

12 = Cos[CC] i - Sin[CC] j

12 = (1 - Cos [b]) (12 . il) 12 +Cos [b] i1 - Sin[b] (Cross[12, 11])
= (1 - Cos [b]) (12 .j1)12 +Cos [b] jl - Sin[b] (Cross[12, j1])

k2 = (1 - Cos[b]) (12 . k1) 12 + Cos [b] kl - Sin[b] (Cross[12, kl])

13 = Tringpand[Cos [AA + CC] 1 - Sin[AA + CC] j]

13 = (1 -Cos[c]) (13 . 12) 13 +Cos[c]12 +Sin[c] (Cross[13, 12])

j3 = (l -Cos[c]) (13 . j2) 13 +Cos[a] j2 +Sin[c] (Cross[13, j2))

k3 = (1 - Cos[c]) (13 . k2) 13 +Cos[c] k2 + Sin[c] (Cross[13, k2])

Out[1]= {1, 0, O}

Outl2l= {1, 0, O}

Outl3l= {0, Cos[a], Sin[a]}
Out[4l= {0, -Sin[a], Cos[a]}
OutlSl= {Cos[CC], ~Sin[CC], 0}

Out[6]= {Cos[b] + (1 — Cos[b]) Cos[CC]2, — (1 -COS[b]) Cos[CC] Sin[CC],
—Sin[b]Sin[CC]}

Out[7]= {—Cos[a] (1 - Cos[b]) Cos[CC] Sin[CC] + Sin[a] Sin[b] Sin[CC] ,
Cos[a] Cos[b] +Cos[CC] Sin[a] Sin[b] +Cos[a] (1 — Cos[b]) Sin[CC]2,
Cos[b] Sin[a] - Cos[a] Cos[CC] Sin[b]}

Out[8]= {(1 — Cos[b]) Cos[CC] Sin[a] Sin[CC] +Cos[a] Sin[b] Sin[CC] ,
-Cos[b] Sin[a] +Cos[a] Cos[CC] Sin[b] — (1 — Cos[b]) Sin[a] Sin[CC]2 ,
Cos[a] Cos[b] +Cos[CC] Sin[a] Sin[b]}

Out[9]= {Cos[AA] Cos[CC] — Sin[AA] Sin[CC] , —Cos[CC] Sin[AA] -Cos[AA] Sin[CC] , O}

62

cum]: {Cos[c] (Cos[b] + (1 -COS[b]) Cos[CC]2) +
Sin[c] (Cos[CC] Sin[AA] Sin[b] Sin[CC] +Cos[AA] Sin[b] Sin[CC]2) +
(1 - Cos[c]) (Cos[AA] Cos [CC] - Sin[AA] Sin[CC])
(- (1 —Cos[b]) Cos[CC] Sin[CC] (-Cos[CC] Sin[AA] -Cos[AA] Sin[CC]) +
(Cos[b] + (1 —COS[b]) Cos[CC]2) (Cos[AA] Cos[CC] — Sin[AA] Sin[CC] ) ),
- (l - Cos[b]) Cos[c] Cos[CC] Sin[CC] +
Sin[c] (Cos[AA) Cos[CC] Sin[b] Sin[CC] - Sin[AA] Sin[b] Sin[CC]2) +
(l -Cos[c]) (—Cos[CC] Sin[AA] - Cos[AA] Sin[CC])
(— (1 ~Cos[b]) Cos[CC] Sin[CC] (—Cos[CC] Sin[AA] -Cos[AA] Sin[CC]) +
(Cos[b] + (1 -COS[b]) Cos[CC]2) (Cos[AA] Cos[CC] - Sin[AA] Sin[CC] ) ),
-Cos[c] Sin[b] Sin[CC] + Sin[c] (Cos[b] Cos[CC] Sin[AA] +
Cos[CC]3 Sin[AA] - Cos[b] Cos[CC]3 Sin[AA] +Cos[AA] Cos[b] Sin[CC] +
Cos[CC] Sin[AA] Sin[CC]2 - Cos[b] Cos[CC] Sin[AA] Sin[CC]2)}

Out[11]= {Sin[c] (-Cos[b] Cos[CC] Sin[a] Sin[AA] +Cos[a] Cos[CC]2 Sin[AA] Sin[b] —
Cos[AA] Cos[b] Sin[a] Sin[CC] + Cos[a] Cos[AA] Cos[CC] Sin[b] Sin[CC]) +
Cos[c] (—Cos[a] (1 —Cos[b]) Cos[CC] Sin[CC] + Sin[a] Sin[b] Sin[CC]) +
(1 — Cos [c])
(Cos[AA] Cos[CC] — Sin[AA] Sin[CC]) ((Cos[AA] Cos[CC] - Sin[AA] Sin[CC])
(~Cos[a] (1 - Cos[b]) Cos[CC] Sin[CC] + Sin[a] Sin[b] Sin[CC]) +
(-Cos[CC]S'1n[AA]—Cos[AA]Sin[CC]) (Cos[a] Cos[b] +
Cos[CC] Sin[a] Sin[b] +Cos[a] (1 —COS[b]) Sin[CC]2)),
Sin[c] (-Cos[AA] Cos[b] Cos[CC] Sin[a] +Cos[a] Cos[AA] Cos[CC]2 Sin[b] +
Cos[b] Sin[a] Sin[AA] Sin[CC] ~Cos[a] Cos[CC] Sin[AA] Sin[b] Sin[CC]) +
Cos[c]
(Cos[a] Cos [b] +Cos[CC] Sin[a] Sin[b] +Cos[a] (1 —-Cos[b]) Sin[CC]2) +
(1 — Cos[c]) (~Cos [CC] Sin[AA] — Cos[AA] Sin[CC])
((Cos[AA] Cos[CC] — Sin[AA] Sin[CC])
(—Cos[a] (1 - Cos[b]) Cos[CC] Sin[CC] + Sin[a] S1n[b] Sin[CC]) +
(—Cos[CC] Sin[AA] - Cos[AA] Sin[CC]) (Cos[a] Cos[b] +
Cos[CC] Sin[a] Sin[b] +Cos[a] (1 - Cos[b]) Sin[CC]2)),
Cos[c] (Cos[b] Sin[a] -Cos[a] Cos[CC] Sin[b]) +
Sin[c] (Cos[a] Cos[AA] Cos[b] Cos[CC] + Cos[AA] Cos[CC]2 Sin[a] Sin[b] —
Cos[a] Cos[b] Sin[AA] Sin[CC] — Cos[a] Cos[CC]2 Sin[AA] Sin[CC] +
Cos[a] Cos[b] Cos[CC]2 Sin[AA] Sin[CC] +
Cos[AA]Sir1[a]Sin[b]Sin[CC]2 — Cos[a] Sin[11\.11>lJSir1[C‘.C]3 +
Cos[a] Cos[b] Sin[AA] Sin[CC]3 )}

63

Out[12]= {Cos[c] ((1 - Cos[b]) Cos[CC] Sin[a] Sin[CC] +Cos[a] Sin[b] Sin[CC]) +
Sin[c] (—Cos[a] Cos[b] Cos[CC] Sin[AA] -Cos[CC]2 Sin[a] Sin[AA] Sin[b] -
Cos[a] Cos[AA] Cos[b] Sin[CC] -Cos[AA] Cos[CC] Sin[a] Sin[b] Sin[CC]) +
(1 — Cos [c])
(Cos[AA] Cos[CC] — Sin[AA] Sin[CC]) ((Cos[AA] Cos[CC] - Sin[AA] Sin[CC])
((1 —Cos[b]) Cos[CC] Sin[a] Sin[CC] +Cos[a] Sin[b] Sin[CC]) +
(-Cos[CC] Sin[AA] -Cos[AA] Sin[CC]) (~Cos[b] Sin[a] +
Cos[a] Cos[CC] Sin[b] — (l—COS[b]) Sin[a] Sin[CC]2)),
Sin[c] (—COS[a] Cos[AA] Cos[b] Cos[CC] - Cos[AA] Cos[CC]2 Sin[a] Sin[b] +
Cos[a] Cos[b] Sin[AA] Sin[CC] +Cos[CC] Sin[a] Sin[AA] Sin[b] Sin[CC]) +
Cos[c]
(—Cos[b] Sin[a] +Cos[a] Cos[CC] Sin[b] — (l-Cos[b]) Sin[a] Sin[CC]2) +
(1 — Cos[c]) (-Cos [CC] Sin[AA] — Cos[AA] Sin[CC])
((Cos[AA] Cos [CC] - Sin[AA] Sin[CC])
((1 — Cos[b]) Cos[CC] Sin[a] Sin[CC] +Cos[a] Sin[b] Sin[CC]) +
(—Cos[CC] Sin[AA] -Cos[AA] Sin[CC]) (—Cos[b] Sin[a] +
Cos[a] Cos[CC] Sin[b] — (1 — Cos[b]) Sin[a] Sin[CC]2)),
Cos[c] (Cos[a] Cos[b] +Cos[CC] Sin[a] Sin[b]) +
Sin[c] (—Cos[AA] Cos[b] Cos[CC] Sin[a] +Cos[a] Cos[AA] Cos[CC]2 Sin[b] +
Cos[b] Sin[a] Sin[AA] Sin[CC] +Cos[CC]2 Sin[a] Sin[AA] Sin[CC] —
Cos[b] Cos[CC]2 Sin[a] Sin[AA] Sin[CC] +
Cos[a] Cos[AA] Sin[b] Sin[CC]2 + Sin[a] Sin[AA] Sin[CC]3 -
Cos[b] Sin[a] Sin[AA] Sin[CC]3)}

64

APPENDIX B

APPENDIX B

Spherical Trigonometric Identities

We summarize spherical trigonometry identities from section 3.2 to be used for sim-

plification of the rotation matrix in equation (3.17) in section 3.3.

Law of Sines: In any spherical triangle ABC,

sin a sin b sin 0

sin A sin B sin C

Law of cosines for sides: In any spherical triangle ABC,

 

 

 

cosa = cosbcosc + sinbsinccos A
cosb = cosccosa + sincsinacosB

cosc = cosacosb+ sinasinbcosC

Law of cosines for angles: In any spherical triangle ABC,

cosA = —cosBcosC+sinBsinCcosa
cosB = —cochosA+sinCsinAcosb
cosC = —cosAcosB+sinAsinBcosc

And useful formulas deriving from Law of Sines and Law of cosines,

sinacosB = sinccosb — sinbcosccos A
sin bcosC = sinacosc — sinccosacosB

sinccos A = sin bcosa — sinacosbcosC

sinAcosb = sinCcosB+sinBcochosa
sinBcosc= sinAcosC+sinCcosAcosb

sinCcosa= sinBcosA+sinAcosBcosc

sinacosC = sinbcosc - sin ccosbcosA
sinbcosA = sinccosa — sinacosccosB

sin ccosB = sinacosb — sinbcosacosC

65

(B2)
(8.3)
(B.4)

(B.11)
(8.12)
(B.13)

(8.14)
(B.15)
(B16)

sinAcosc : sinBcosC +SinCcosBcosa
sinBcosa= sinCcosA+sinAcochosb

sin C cos b = sin A cos B + sin B cos A cos c
And the basic trigonometric identity

sin2 A + cos2 A = 1

66

(8.17)
(8.18)
(B.19)

(B20)

APPENDIX C

APPENDIX C

Simplification of Rotation Matrix

In this section, coordinate rotation is used to find the amount of rotation in equa-
tion (3.17) resulting from the sphere rolling along a spherical triangle. For simplicity,
we use symbol 5,, and Ca to represent sina and cos a, respectively. The number of
formula used which are expressed in brackets is refered to Appendix B.

Consider the first element in 2'3, from Mathematica

13(1) = C,(Cb + (1 — (mag) + SC(CCSAS,,SC + CAsbsg) + (1 — Cc)
(CACC — SASC)[—(1 — Cb)CCSC(—CCSA — CASC) + (C, +
(1 — C,)Cg)(CACC — 5.50)]
= 0.02. + 0.0ng + 5,0,33,45,50 + SCCAsbsg +
(1 — CC)(CACC — SASC)[CgsCsA + 00530,, —
Q@&&—QQ%Q+QQQ—Q&&+
02:0,, — CgsAsC — 0,030,, + 0,050,, + 0,035.50]

Cancel and rearrange terms

13(1) 2 060% + SCCCSASbSC + C2055?) + Sac/1358?; +
(1 — Cc)(CAC'C — sAanCngCA + 02.0.. - 0,5450 -
05005210,; — CngwCA + CbCACC]

Since C3; + SE"; = 1 and use identity (B2),

23(1) 2 CECE. + SCCCSASbSC + C053; + (I — CC)(CACC - SASC)
[CCCA - Obs/{SC - CbCCCA + CbC‘qcc]

Cancel terms and use identity (86),
13(1) 2 CcCgv + SCCCSASbSC + CaSg+(1— CC)(CACC — SASC)[—CB]
Expand all terms,

13(1) 2 Coca» + SCCCSASbSC + 005(2) —- CACCCB + SASCCB ‘1‘
CCCACCCB _ CCSASCCB

67

Use identities (B2), (B7), and (8.19),

13(1) 2 CCCCZJ + SCCCSASbSC + SEbeCC + 527SbSCCA '-
CACCCB + SASCCB + CECCSASB — (:ch — CCCbSE +
CESCSBCA

Cancel and rearrange terms,

13(1) = SZ.SCS(C4 + CESCSBC4 + SCCCSASbSC + CECCSASB —
CACCCB '1' 535003

Use identity (B. 1),

13(1) 2 SCSBCA + CCSASB — CACCCB + SASCCB

= -CA+B+C

Consider the second element in 23, from Mathematica

M32%L%MQQ&%SKM%W%—&&%H'
(I - CC)(-CCSA — CASC)[—(I — Cb)Ccsc(-CCSA — 0,450) + (Cb +
(1 — Cb)C(2_7)(CACC — SASC)l

Expand 13‘ two terms and all terms in square bracket

13(2) = _CCCCSC + CchCCSC + SCCACCSbSC — SOS/1555(2) +
(h0%€firflfifl%%a+Q%Q—Q%&fi—
CbC'ngC'A + CbCACC — CbSASC + C270,] — C'éSASC -
Q$Q+Q$a&]

Cancel terms and use identity (B2) in square bracket

23(2) = —CCCCSC + CCSCCC - $05,45ng + (1 — CC)(—CCSA — CASC)
[CCSgCA + C30,; — CbSASC — CbCngCA — CBC/€04 +
CbCACCl

Use (B20) and (B6) and cancel terms, then expand terms
13(2) 1’ —CCCCSC + CCSCCa — SCSASbSEv + Cos/(CB + CASCCB -

68

CCCCSACB — CcCASCCB

Use (B.13) with second and third terms, substitute (B.1) for third term, substitue

(B.7) for last term, then cancel terms
13(2) 2 CCSBCA — SgSASBSC + CCSACB + CASCCB — CESCSASB
Use (8.20) with second and fifth terms

23(2) = 00580.4 + CCSACB + CASCCB — SASBSC

= SA+B+C
Consider the third element in 2'3, from Mathematica

23(3) = —CCSbSC + SC[C(,CCSA + 0278A -- CngSA + 0,405.30 ‘1'
@afi—QQ&%]

Use (B20) and cancel terms
i3(3) = *CCSbSC + SC[CCSA + CACbSC]

Use (B.12)
13(3) 2 “CCSCSC + SCSBCC

Use (31)

Consider the first element in 33, from Mathematica

j3(1) = S,(-CbCCSaSA + CanSASb - CACbSaSC +
CaCACCSbSC) + Cc(_Ca(1 " Cb)CCSC + SaSbSC) ‘1' (1 — Cc)
(CACC — SASC)[(CACC — SASC)(—Ca(1 — Cb)CCSC + SaSbSC) +
6008.4 — CAscxcacb + 005.5. + C.(1 — 00532)]

Use (8.16) and then expand terms,

13(1) = Sc(—SCCCSACB — CASCSCCB) + Cc(—CaCCSC + CaCbCCSC +
308530) + (I — CC) (CACC - SASC)[—CAC(2JCQSC + CACECaCbSC '1'
CACCSaSbSC '1' 5148230000 — 51453000be — SASEvSaSb '-

69

CCSACaCb — 037544505), '1' Gas/100527 + CCSACaCb-SZC —
CASCCaCb — CASCCCSaSb — 0.45500 + CASg'CaCb)

Cancel and rearrange terms

j3(1) = SC(—-SCC(;SAC3 — CASCSCCB) + CC(—C,,CCSC + CaCbCCSC +
SaSbSC) + (1 — Cc)(CACC — SASC)[—CACC2;CGSC — c.4330, —
CCSACCC), — SASéSaSb — 03844503,, + CACE‘CCCCSC +
CASE/COG, — CASCCaCb]

Use (B20)

13(1) = Sc(-SCCCSACB — CASCSCCB) + Cc(_CACCSC + CaCbCCSC +
SaSbSC) + (1 - Cc)(CACC - SASC)[_CACaSC - CCSACaCb — SASaSb +
CACaCbSC —— CASCCaCb]

Inside square bracket, cancel terms, use (B.18), (3.1), and (8.20), we left with [—SB].
Then expand all

j3(1) = —SC2CCSACB — SECASCCB — CCCCCCSC + CCCaCbCCSC +
CcSaSbSC — CACCSB + 5.45053 + OCCACCSB — CcSASCSB

Use (B20) with the first and second terms, use (B.4) for fourth term, and then

rearrange terms

13(1) = —CCSACB + CCSAC'BCC2 - CASCCB + CASCCBCE —
0.0.0050 + CgCCSC — Cccgscsasb + CCSaSbSC —
CACCSB '1' 3,1505}; '1" CCCACCSB — CCSASCSB

Use (8.13) for the second and third terms, use (B20) in the 8“ term,

j3(1) = —CCSAC,, + CCCCSCCO. — CASCCB + CASCCBCE ‘—
CCCaCCSC + CECCSC -' CCSaSbSC ‘1' 005235035 ‘1'
CCSaSbSC — CACCSB + SASCSB — CCSASCSB

70

Use (B1) in 8th, cancel 2nd and 5th, cancel 7th and 9th,

j3(1) :- —CCSACb — CASCCB + CqSCCBC: + C30030 '1'
CCSASBSESC — CACCSB '1' 5,1305}; — CCS‘4Sng

Use (820) in 8th, then cancel terms

.78“) : "CCSACb — CASCCB + CASCCBCE + CECCSC —
CCSASBSCCE — CACCSB + SASCSB

Use (7) with 4th and 8th, then cancel with 6th,

j3(1) = —CCSACb - CASCCB — CACCSB + 5,150.33

2 —SA+B+C

Consider the second element in )3, from Mathematica

313(3) : S,(—CAC,,CCSO + CaCACCSb + CbSaSASC —
C.CCSAS.SC) + 040.0. + 003.5. + C.(1 — CBS?» +
(1 — C,)(-—CCSA — CASC)[(CACC — SASC)(—C,,(1 — Cb)CCSC +
505,50) + (405,, — CASC)(C.Cb + 00803:. + Ca(1 — 0052)]

Consider terms in square bracket, when expand all terms,

[...] = [4.53.0.5C + 63,050,635C + (1,005,550 +
5.,5écacC — 5,,5(,~(J,,C,,CCSC — 5.52.5.5, - CCSACaCb —
CCSASaSb — CCSACGSZ. + CCSACaCbSE; - CASCCCCC —
CASCCCSaSb - 0.1350,. + CASCCaCng‘l

Cancel terms, 3rd and 12th, 4th and 9th, 5th and 10th. Pairs 15t-13th, 2nd-14th,
and 6th-8th can be combined.

l--l = l—CaSCCa + CACaCbSC — SASaSb — CCSACaCb — CASCCaCbl

The second and fifth terms are canceled. The first and forth terms can be combined

using (18), therefore,
[...] = [—C§SB — SASaSb]

71

Return to j3(3), use (8.16) in the first and (B20) in the second brackets, then expand

terms.

323(3) .—. SECACCCB — 535,45CCB — 0.0.0ng — 0,005.5, —
0.0.53. - 005.0353 — 0053505,, — CASCcfSB —
(1,505,505,, — 0.0054035,9 + 0.0053, 5.5,, +
am&@%+ammaaa

Expand the 5th terms using (313), 6th using (B.11). Combine 10th and 11th, 8th
and 9th.

j3(3) = SECACCCB — SEVSASCCB — CCCaCng — CCCCSaSb —
0650530,, — CgSCsACB — 0,530,, + CaSASCCB —. 0053505,, —
CASCSB — CCCCSASB + (ICC/15000233 + COG/1505,4505),

The 5th term combine with the 13th equal to 12th. Combine 2nd and 6th using
(B.20), 7th and 9th using (4), and expand 8th using (17).

j3(3) = SECACCCB - SASCCB — CcCaCbCEv - CCCCSaSb — 5,240.; +
531C; — SASBCC — CASCSB -- CCCCSASB

The fifth term cancel with sixth term. The third, fourth, and nineth add up with

first terms.

13(3) = CACCCB — SASCCB - SASBCC - SCSBCA

= —CA+B+C

Consider the third element in jg, from Mathematica

j3(3) = Cc(CbSa - 000055) + SC(C0CACbCC + CACgSaSb -
CaCbSASC - CanSASC + CaCbchASc + CASaSbSCZ; —
005,152: + CaCbSAsg)

Use (320), cancel terms, and expand

j3(3) = CchSa _ CVCC'aC'CSb + ScCaCACbCC + SCCASaSb _
ScCaSASC

72

Use (BM) in the third term

33(3) 2 CCCbSa — CCCQCCSI, + CaCCSbCC — 0003350 +
SCCASaSb — ScCaSASC

Use (B20) in 4th term

333(3) 2 CchSa — CcCaCch + CaCCSbCC — CaS'a + CaSaSg +
Soc/1505b — ScCaSASC

Use (B2) in 1st and 6th, then cancel with 4th. Use (B.1) in 5th. Cancel 2nd and
3rd.
j3(3) : C'aSAScSC _ ScCaSASC = 0

Consider the first element in k3, from Mathematica

k3(1) = CC((1 — Cb)CCSaSC + CaSbSC) + Sc(—CaCbCCSA — (12.5.5.5, —
000,4ch0 — CACCSaSbSC) + (1 — Cc)(CACC -— SASC)[(CACC — SASC)
((1 — Cb)CCSaSC + CaSbSC) + (—CCSA — C'AS'C,~)(—C',,Sa + CaCCSb —
(1 - 00353)]

Use (B.10) in the first bracket, (B.4) in second. Use (B.10) (B.16) and (B20) in
square bracket.

k3(1) = CC(C(;SaSC + SCSCCA) + SC(— VCSACC — CASCCC) +
(1 — CC)(CACC — SASC)[(CACC — SASC)(CCSaSC + SCSCCA) +
(—CCSA — CASC)(CCSCCA - 50533)]

Expand terms,

an) = 0.005050 + 0,505.0... -— 5.00540. — 5.0.1500. +
(1 — axe/,0C — 5A50)[CA03,5,50 + 03,00505c —
5.152.065. — 5,535.5, -— 055.50,, + 005,555 —
Cgsccc5. + 0.1535,}

The first and third terms can be canceled using (31). Second and fourth are canceled.

73

In square bracket, use (BI) and (B20), all terms can be canceled. Therefore,
Consider the second element in k3, from l\/‘Iathematica

153(2) 2 SC(—COCAC(,CC - CACZ'SOS), + CaCbSASC +
CCSaSASbSC) + CC(—CbSa + CaCCSb — (1 — CHEESE.) +
(1 — Cox—005A — CASC)i(CACC — SASC)((1’ Cb)CCSaSC +
CaSbSC) + (—CCSA — CASC)(_CbSa + CaCCSb — (1 —' Cb)(SaSg')l

Use (B.4) in the first bracket, (B.10) and (B20) in second bracket. Use (B.10) and
(B20) in square bracket.

1M?) = 5c(—CACCCC + SASCCC) + Cc(CCScCA — SaSg‘) +
(1 — CC)(—CCSA — CASC)[(CACC — SASC)(CCSGS(; + SCCASC) +
(_ccs. — CASCMCCSCC. — 5052.)]

Expand terms,

k3(2) = —SCCACCCC + 5.5.500. + (3,005.0, — 6.5053; +
(1 — CC)(—CCSA - CA50)[CACg5050 + 5.035000 —
5.453008. — 5.4535634 — CéSCCASA + CCSASGSE; —
0350005. + 54525,]

The first and third terms can be canceled using (B.1). Second and fourth are canceled.
In square bracket, use (BI) and (B20), all terms can be canceled. Therefore,

Consider the third element in kg, from Mathematica

[413(3) = Cc(CaCb + CasaSb) + Sc(—CACbCCSa + €00,1ch1, +
01,303,430 + CéSaSASC — CngSaSASC + COCASbSE; +
SaSAS'g — 01,305,432.)

74

Use (B.4) in the first bracket. Use (BIG) and (B20) in the second bracket.
163(3) 2 CE + SC(SGSASC + CASCCA)

By using (B.1),
This completes the simplification of all entries of rotation matrix in equa-

tion (3.17). After simplification we can write the final coordinates of the sphere

i’,j’, k’, or i3,j3, k3 in this appendix, as

y:_amA+B+mammM+B+Cfi
j’ = —sin(A+B+C)-i—cos(A+B+C)j

ic’ : is
or, in matrix form
? —aMA+B+Q $MA+B+Q 0 i
f = —$MA+B+C)—aMA+B+C)0 j
I? 0 0 1 is

75

APPENDIX D

APPENDIX D

Fortran program for numerical root finding

This section presents the fortran routine used for numerically root-finding of equa-
tion (3.41), (3.42) and (3.43) to find two sides and the included angle of spherical
triangle. We follow the instruction of root-finding in chapter 9 of the book by W. H.
Press, S. A. Teukolsky, W. T. Vetterling and B. P. Flannery [29]. Some necessary
modification is made in order to apply to our set of equations.

Main program : sphtri.f

INTEGER 11

REAL x(3),t(3)

PARAMETER (pi=3. 1415926535897)
COMMON Idesire/ dd,phid,alphad
n=3

write(",") 'guess a,b,CC(rad)'
read?!) X(1),X(2).X(3)

Wfitd‘.‘) 'desired d.Phi(md),alpha(r8d)'
read?) dd.phid.alphad

call newt(x,n,check)

write(“,") 'a,b,CC(deg),check'
Wfl'td'.‘)X(1).X(2).X(3)‘180/Pi.0h60k
call fimCWmLO

Wfitd‘f) 'fll).f12),t13)'

WNW?) ‘1 04121113)

END

Subroutine : newtf

SUBROUTINE newt(x,n,check)
INTEGER nJm,NP,MAXITS
LOGICAL check
REAL x(n),fvec,TOLF,TOLMIN,TOLX,STPMX
PARAMETER (NP=40,MAXITS=200,TOLF=1.e-6,TOLMIN=1 .e-8,TOLX=1 .e-8,
" STPMX= 100.)
COMMON lnewtv/ fvec(NP),nn
SAVE Inewtv/
INTEGER i,its,j,indx(NP)
REAL d,den,f,fold,stpmm<,smn,temp,test,tjac(NP,NP),g(NP),p(NP),
" xold(NP),fmin
EXTERNAL fmin
nn=n
f=fmin(x)
test=0.
do i=1 ,n
if(abs(fveo(i)).gt.test)tst=abs(fvec(i))
enddo
if(test.1t..01‘TOLF)then
check=.false.
return

76

endif
sum=0.
do i=l,n
sum=sum+x(i)"2
enddo
WST'PW‘IDNSQIKM),float(nD
do its= l ,MAXIT S
call fdjadmvaecNPflaC)
do i=1,n
sum=0.
do j=l ,n
smn=smn+tjac(j,i)‘fvec(j)
enddo
g(i)=sum
enddo
do i=1,n
xold(i)=x(i)
enddo
fold=f
do i=1,n
p(i)=-fvec(i)
enddo
call ludcmp(tjac,n,NP,indx,d)
call lukaNfiacmNPJndxm)
call lnsrch(n,xold,fold,g,p,x,f,stpmax,check,finin)
test=0.
do i=l,n
if(abs(fvec(i)).gttest)test=abs(fvec(i))
enddo
ifltestlLTOLFMhen
check=.false.
return
endif
if(check)then
test=0.
den=max(f,.5‘n)
do i=1 ,n
temp=ab8(g(i))‘maX(abS(X(i)),l.)/den
if(temp.gt.test)test=temp
enddo
ifltestltTOLMIN)then
check=.tme.
else
check=.false.
endif
return
endif
test=0.
do i=l,n
t6111944134X(i)-Xold(i)))’IIIIIX(flbs(X(i»,l-)
if(temp.gt.test)test=temp
enddo
ifltestlLTOLXhetum
enddo
pause 'MAXITS exceeded in newt'
END

Subroutine : ludcmp.f
SUBROUTINE ludcmp(a,n,np,indx,d)

INTEGER n,np,indx(n),NMAX
REAL d.a(np.np),TINY

77

PARAMETER (NMAX=500,T11~IY=I.0e-20)
INTEGER i,imax,j,k
dRElAL aamax,dum,sum,vv(NMAX)
do i= 1 ,n
aamax=0.
do j= l ,n
mommigtammx) aamax=abs<a<u>>
enddo

if(aamax.eq.0.) pause 'singular matrix in ludcmp'
vv(i)=l./aa.max
enddo
do j= I ,n
do i=1 J-l
sum=a(i.i)
do k=l,i-l
=Sum-a(i.k)‘a(k.i)
o
a(i‘.i)=sum
enddo
aamax=0.
do i=j,n
sum=a(i.i)
do k=l,j-l
m=smn-a(i.k)’a(kd)
enddo
804an
dum=vv( i)‘abs( sum)
if(dum.ge.aamax)then
imax=i
aamax=dum
endif
enddo
if(j.ne.imax)then
do k=l,n
dum=a(imax,k)
8(imax,k)=8(.i,k)
86.ka
enddo
=-d
w(imax)=vv(j)

indx(j)=imax
iflaOJIOQ-OJBUJFTWY
ifG.ne.n)then
dum= 1 ./a(j ,j)
do i=j+1,n
8(iJ)=a(iJ)‘dmn
enddo
endif
enddo
return
END

Subroutine : lubksbf

SUBROUTINE lubksb(a,n,np,indx,b)
INTEGER n,np,indx(n)

REAL a(np,np).b(n)

INTEGER i,ii,i,ll

REAL sum

ii=0

78

do i=1,n
ll=indx(i)
sum=b(ll)
b(llFbG)
if(ii.ne.0)then
do j=ii,i-l
sum=81m—a(iJ)‘b(i)
enddo
else if (sumne.0.)then
ii=i
endif
b(i)=sum
enddo
do i=n,l ,-1
:1,0)
do j=i+l,n
sum=mn-a(i.i)’b(i)
enddo

b(i)=sum/80.0
enddo
rCtum
END

Subroutine : lnsrch.f

SUBROUTINE lnsrch(n,xold,fold,g,p,x,f,stpmax,check,func)
INTEGER n
LOGICAL check
REAL f,fold,stpmax,g(n),p(n),x(n),xold(n),fimc,ALF,TOLX
PARAMETER (ALF=] .e-4,TOLX= l .e-7)
EXTERNAL func
INTEGER i
REAL a,alarn,alam2,alamin,b,disc,t2,rhsl,rh52,slope,smn,temp,
" testtmplam
check=.false.
sum=0.
do i=1,n
m=sum+p(i)‘9(i)
enddo
SUIFSQIKSWH)
iflsumgtstpmaxfihen
do i=1,n
p(i)=p(i)’stpmax/sum
enddo
endif
slope=0.
do i=1,n
slope=slope+g(i)‘p(i)
enddo
iflslope.ge.0.) pause 'roundoff problem in lnsrch'
test=0.
do i=1,n
temp=abs(p(i))/max(abs(xold(i)), 1 . )
if(temp.gttest)test=temp
enddo
alamin=TOLXltest
alam=l.
1 continue
do i=1 ,n
x(i)=xold(i)+alam‘p(i)
enddo
f=func(x)

79

iflalamltalaminfihen
do i=1 ,n
x(i)=xold(i)
enddo
check=.true.
return
else if(f.le.fold+ALF ‘alam'slopefihen
return
else
if(alam.eq. 1 .)then
tmplam=-slope/(2.‘(f-fold-slope»
else
rhsl=f-fold-alam‘slope
rh52=12-fold-alam2‘slope
a=(rhsl/alam"2-fl152/alam2“2)l(alam~alam2)
b=(-alam2 ‘rhs 1 Ialam‘ ‘2+alam’rth/alam2“ ‘2 )l
‘ (alam-alamz)
if(a.eq.0.)then
tmplam=-slope/(2. ‘b)
else
disc=b"b-3. ‘a‘slope
if(disc.lt.0.)then
trnplam=.5'alam
else iftb.le.0.)then
tmplam=(-b+sqrt(disc))/(3.‘a)
else
tmplam=-slope/(b45qrt(disc))
endif
endif
if(tmplam.gt.5‘alam)tmplam=.5‘alam
endif
endif
alarn2=alam
f2=f
alam=rnax(tmp1am,. l "alam)
goto 1
END

Subroutine: fdjac.f

SUBROUTME fdjaqmX,fvec,np,dt)
INTEGER n,np,NMAX
REAL df(np,np),fvec(n),x(n),EPS
PARAMETER (NMAX=40,EPS=l.e-4)
INTEGER ij
REAL h,temp,f(NMAX)
do j=1,n
temp=x(j)
h=EPS‘abs(temp)
if(h.eq.0.)h=EPS
x0)=temp-+h
h=xo)-temp
call funcv(n,x,t)
x0')=temp
do i=1,n
dfliJHflivaediWh
enddo
enddo
return
END

80

Subroutine : fminf

FUNCTION fmin(x)

INTEGER n,NP

REAL fmin,x(‘),fvec

PARAMETER (NP=40)

COMMON Inewtv/ fvec(NP),n

SAVE lnewtv/

INTEGER i

REAL sum

call funcv(n,x,fvec)

sum=0.

do i=1,n
sum=sum+fvec(i)“2

enddo

fmin=0.5“sum

return

END

81

BIBLIOGRAPHY

BIBLIOGRAPHY

[1] M. Aicardi, G. Casalino, A. Balestrino, and A. Bicchi. Closed-loop smooth
steering of unicycle-like vehicles. IEEE Conference on Decision and Control,
pages 2455—2458, 1994.

[2] A. Astolfi. On the stabilization of nonholonomic systems. IEEE Conference on
Decision and Control, pages 3481~3486, 1994.

[3] Frank Ayres, Jr. Schaum’s Outline of Theory and Problems of Plane and Spher-
ical Trigonometry. McGraw Hill, New York, NY, 1954.

[4] A. M. Bloch and S. Drakunov. Stabilization of a nonholonomic system via sliding
modes. IEEE Conference on Decision and Control, pages 2961-2963, 1994.

[5] A. M. Bloch, N. H. McClamroch, and M. Reyhanoglu. Controllability and sta-
bilizability properties of a nonholonomic control system. Proceedings of 29th
IEEE Internation Conference on Decision and Control, Honolulu, Hawaii, pages
1312—1314, 1990.

[6] A. M. Bloch, M. Reyhanoglu, and N. H. McClamroch. Control and stabilization
of nonholonomic dynamical systems. IEEE Transactions on Automatic Control,
37(11):1746-1757, 1992.

[7] R. W. Brockett. Asymptotic stability and feedback stabilization. Differential
Geometric Control Theory, Editors. R. W. Brockett, R. S. Millman, and H. F.
Sussman, Birkhauser, pages 181—191, 1983.

[8] J. M. Coron. Global asymptotic stabilization for controllable systems withour
drift. Mathematics of Control, Signals, and Systems, 5:295—312, 1992.

[9] C. Canudas de Wit and C. Samson. Path following of a 2-dof wheeled mobile
robot under kinematic constraints. Proceedings of First European Control Con-
ference, Grenoble, France, pages 2084—2088, 1991.

[10] C. Canudas de Wit and O. J. Sordalen. Exponential stabilization of mobile
robots with nonholonomic constraint. IEEE Transactions on Automatic Control,
37(1):1791—1797, 1992.

82

[11] J. Guldner and V. I. Utkin. Stabilization of nonholonomic mobile robots using
lyapunov functions for navigation and sliding mode control. IEEE Conference
on Decision and Control, pages 2967—2972, 1994.

[12] L. Gurvits and Z. X. Li. Smooth time-periodic feedback solutions for nonholo-
nomic motion planning. Pregress in Nonholonomic Motion Planning, Z. X. Li

and J. Canny, editors, Kluwer Academic Press, 1992.

[13] A. Isidori. Nonlinear Control Systems. Springer Verlag, New York, NY, third
edition, 1995.

[14] J. L. Junkins and J. D. Turner. Optimal Spacecraft Rotational Maneuvers. El-
sevier, New York, NY, 1986.

[15] Y. Kanayama, Y. Kimura, F. Miyazaki, and T. N oguchi. A stable tracking control
method for an autonomous mobile robot. Proceedings of IEEE International
Conference on Robotic and Automation, Cincinnati, Ohio, pages 384—389, 1990.

[16] L. M. Kells, W. F. Kern, and J. R. Bland. Plane and Spherical Trigonometry.
McGraw Hill, New York, NY, second edition, 1940.

[17] H. Khennous and C. Canudas de Wit. On the construction of stabilizing discon-
tinuous controllers for nonholonomic systems. Nonlinear Control Systems Design
Symposium (NOLCOS), IFAC Preprints, Tahoe City, pages 747—752, 1995.

[18] I. Kolmanovsky and N. H. McClamroch. Developments in nonholonomic control
problems. IEEE Control Systems, 15(6):20—-36, 1995.

[19] G. A. Lafferriere. A general strategy for computing steering controls of systems
without drift. Proceedings of the 30th IEEE Conference on Decision and Control,
pages 1115—1120, 1991.

[20] G. A. Lafferriere and H. Sussmann. A differential geometric approach to mo-
tion planning. Nonholonomic Motion Planning, Z. Li and J. F. Canny, editors,
Kluwer, pages 235—270, 1993.

[21] N. E. Leonard and P. S. Krishnaprasad. Averaging for attitude control and
motion planning. Proceedings of the 32nd Conference on Decision and Control,
pages 3098—3104, 1993.

[22] A. Lewis, J. P. Ostrowski, R. Murray, and J. Burdick. Nonholonomic mechan-
ics and locomotion: The snakeboard example. Proceedings of the International
Conference on Robotics and Automation, pages 2391—2397, 1994.

83

[23] Z. Li and J. F. Canny. Nonholonomic Motion Planning. Kluwer, 1993.

[24] R. T. M’Closkey and R. M. Murray. Convergence rates for nonholonomic systems

in power form. American Control Conference, San Francisco, CA, 1993.

[25] R. Mukherjee and D. P. Anderson. A surface integral approach to the motion
planning of nonholonomic systems. Journal of Dynamic Systems, Measurement
and Control, 1162315—324, 1994.

[26] R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to Robotic
Manipulation. CRC Press, Ann Arbor, 1993.

[27] R. M. Murray and S. S. Sastry. Nonholonomic motion planning: Steering using
sinusoids. IEEE Transactions on Automatic control, 38(5):700—716, 1993.

[28] J. B. Pomet. Explicit design of time-varying stabilizing control laws for a class
of controllable systems withour drift. System and Control Letters, 18:147—158,
1992.

[29] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery. Numerical
Recipes in Fortran 77: The Art of Scientific Computing. Cambridge University

Press, second edition, 1992.

[30] C. Rui and N. H. McClamroch. Stabilization and asymptotic path tracking of a
rolling disk. Proceedings of the 34th IEEE Conference on Decision and Control,
pages 4294—4299, 1995.

[31] C. Samson. Velocity and torque feedback control of a nonholonomic cart. In-
ternational Workshop in Adaptive and Nonlinear Control: Issues in Robotics,
Proceedings in Advanced Robot Control, Springer Verlag, 162, 1990.

[32] C. Samson. Time-varying feedback stabilization of a car-like wheeled mobile
robots. International Journal of Robotics Research, 12(1):55—66, 1993.

[33] H. J. Sussmann and W. Liu. Limits of highly oscillatory controls and approxi-
mation of general paths by admissible trajectories. Proceedings of the 30th IEEE
Conference on Decision and Control, pages 437—442, 1991.

[34] I. Todhunter. Spherical Trigonometry. Macmillan, London, fourth edition, 1878.

[35] G. C. Walsh, D. Tilbury, S. Sastry, and R. Murray adn J. P. Laumond. Stabi-
lization of trajectories for systems with nonholonomic constraints. IEEE Trans-
actions on Automatic Control, 39:216—222, 1994.

84

"I[1111111111[111111111