IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO.
4, AUGUST 2014
[21] H. K. Shin and B. K. Kim, “Energy-efficient reference gait generation
utilizing variable ZMP and vertical hip motion based on inverted pendulum
model for biped robots,” in Proc. IEEE Int. Conf. Control Auton. Syst.
(ICCAS), Oct. 27–30, 2010, pp. 1408–1413.
[22] F. M. Silva and J. A. T. Machado, “Energy analysis during biped walking,”
in Proc. IEEE Int. Conf. Robot. Auton. (ICRA), Detroit, MI, USA, May
1999, vol. 1, pp. 59–64.
[23] D. Goswami, V. Prahlad, and P. D. Kien, “Genetic algorithm-based opti-
mal bipedal walking gait synthesis considering tradeoff between stability
margin and speed,” Robotica, vol. 27, pp. 355–365, 2009.
[24] V. H. Dau, C. Chew, and A. Poo, “Achieving energy-efficient biped walk-
ing trajectory through GA-based optimization of key parameters,” Int. J.
Human. Robot., vol. 6, no. 4, pp. 609–629, 2009.
[25] Z. Liu, L. Wang, C. L. Philip Chen, X. Zeng, Y. Zhang, and Y. Wang,
“Energy-efficiency-based gait control system architecture and algorithm
for biped robots,” IEEE. Trans. Sys. Man. Cybernet. Part C: Appl. Rev.,
vol. 42, no. 6, pp. 926–933, Nov. 2012.
[26] L. C. Jain and N. M Marti, Fusion of Neural Networks, Fuzzy Systems
and Genetic Algorithms: Industrial Applications. Boca Raton, FL: CRC,
2011.
[27] R. Sellaouti, O. Stasse, S. Kajita, K. Yokoi, and A. Kheddar, “Faster and
smoother walking of humanoid HRP-2 with passive toe joints,” in Proc.
IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Beijing, China, Oct. 9–15,
2006, pp. 4909–4914.
[28] T. Ha and C. H. Choi, “An effective trajectory generation method for
bipedal walking,” Robot. Auton. Syst., vol. 55, no. 10, pp. 795–81, 2007.
[29] A. Goswami, “Postural stability of biped robots and the foot-rotation
indicator (FRI) point,” Int. J. Robot. Res., vol. 18, pp. 523–533, 1999.
[30] M. Nahon and J. Angeles, “Minimization of power losses in cooperating
manipulator,” J. Dyn. Syst. Meas. Control (Trans. ASME), vol. 114, no. 2,
pp. 213–219, 1992.
[31] S. K. M. Morisawa, K. Miura, S. Nakaoka, K. Harada, K. Kaneko,
F. Kanohiro, and K. Yokoi, “Biped walking stabilization based on linear
inverted pendulum tracking,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots
Syst., Taipei, Taiwan, Oct. 18–22, 2010, pp. 4489–4493.
[32] I. Ha, Y. Tamura, and H. Asama, “Gait pattern generation and stabilization
for humanoid robot based on coupled oscillators,” in Proc. IEEE/RSJ Int.
Conf. Intell. Robots Syst. (IROS), San Francisco, CA, USA, Sep. 25–30,
2011, pp. 3207–3212.
A Motion Planning Strategy for a Spherical Rolling Robot
Driven by Two Internal Rotors
Akihiro Morinaga, Student Member, IEEE,
Mikhail Svinin, Member, IEEE,
and Motoji Yamamoto, Member, IEEE
Abstract—This paper deals with a motion planning problem for a spher-
ical rolling robot actuated by two internal rotors that are placed on orthog-
onal axes. The key feature of the problem is that it can be stated only in
Manuscript received June 2, 2013; revised December 8, 2013; accepted Febru-
ary 14, 2014. Date of publication March 12, 2014; date of current version August
4, 2014. This paper was recommended for publication by Associate Editor M.
Vendittelli and Editor G. Oriolo upon evaluation of the reviewers’ comments.
The authors are with the Department of Mechanical Engineering, Faculty of
Engineering, Kyushu University, Fukuoka 819-0395, Japan (e-mail: morinaga@
ctrl.mech.kyushu-u.ac.jp; svinin@mech.kyushu-u.ac.jp; yama@mech.kyushu-
u.ac.jp).
This paper has supplementary downloadable material, available at
http://ieeexplore.ieee.org, provided by the authors. It has two files showing
motion of the spherical rolling robot in the simulation example in Section III.
The file animation_k 2.5.mp4 corresponds to the set of parameters k = 2.5 and
η = 0.6. The file animation_k2.5.mp4 corresponds to the set of parameters k =
10 and η = 0.5. Its size is 20.6 Mb.
Color versions of one or more of the figures in this paper are available online
at http://ieeexplore.ieee.org.
Digital Object Identifier 10.1109/TRO.2014.2307112
1552-3098 © 2014 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See http://www.ieee.org/publications standards/publications/rights/index.html for more information.
993
dynamic formulation. In addition, the problem features a singularity when
the contact trajectory goes along the equatorial line in the plane of the two
rotors. A motion planning strategy composed of two trivial and one non-
trivial maneuver is devised. The trivial maneuvers implement motion along
the geodesic line perpendicular to the singularity line. The construction of
the nontrivial maneuver employs the nilpotent approximation of the orig-
inally nonnilpotent robot dynamics, and is based on an iterative steering
algorithm. At each iteration, the control inputs are constructed with the
use of geometric phases. The motion planning strategy thus constructed is
verified under simulation.
Index Terms—Dynamics, motion planning, nonholonomic systems,
rolling constraints, spherical rolling robot.
I. INTRODUCTION
In recent years, there has been growing research interest in spherical
rolling robots. They are regarded as nonconventional single-wheeled
locomotion machines that can be helpful when the usage of traditional
vehicles is limited or undesirable. The motion of such machines is usu-
ally generated by creating gravity imbalance with a spherical pendulum
or sliders, or by changing the system inertia with the use of internal ro-
tors. Different designs of spherical mobile robotic systems are reported
in [1]–[7]. In this paper, we consider a rolling robot actuated by internal
rotors. Under proper placement of the rotors, the center of mass of the
robot is at the geometric center of the sphere and, as a result, gravity
terms do not enter the motion equations [2], [8].
One of the key problems in the control of spherical rolling robots
is the construction of motion planning algorithms. In the majority of
papers in the robotics literature, this problem is considered for the
so-called ball–plate system where the sphere is actuated by moving
two parallel plates. Under such an actuation, provided that the ball is
dynamically symmetric with respect to the plane axes, the spinning of
the ball around the vertical axis is canceled out and the ball moves in
the so-called pure rolling mode. As a result, motion planning for such
a system can be conducted in kinematic formulation, and a number of
motion planning algorithms have been developed so far [9]–[14]. In
general, however, the existence of the pure rolling mode in a rolling
system depends on the inertia distribution as well as on how the system
is actuated.
Addressing the rolling robots actuated by internal rotors, it should
be noted that when the number of actuators is more than two, the
motion planning problem can be decomposed into the kinematic and
dynamic levels and, in principle, its solution does not represent essential
difficulties. However, for the minimal number of two actuators, such
a decomposition is impossible because there appears a constraint on
the component of the angular velocity of the sphere and this constraint
depends on the inertia distribution [15]. For this reason, the assigned
trajectories for the angular velocity, including those corresponding to
pure rolling, are not necessarily dynamically feasible and realizable by
the robot actuators [15]. As a result, the motion planning problem must
be considered only in dynamic formulation.
Since the mathematical model of the rolling robot with two rotors
inherits the basic properties of that for the ball–plate system (not dif-
ferentially flat, not nilpotent, cannot be represented in a chained form),
it belongs to a special class of nonholonomic systems, the class for
which conventional planning techniques are not directly applicable. It
is worthwhile, therefore, to look at the two main motion planning ap-
proaches developed for the kinematic model of pure rolling. The first is
based on the optimal control theory [10], while the second deals with
geometric phases [9], [16].
Numerical implementation of the optimal control approach, apart
from the long computation time, can be sensitive to initial guess of the
994 IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO. 4, AUGUST 2014

optimal controls. Even in the case of pure rolling, the structure of the
optimal control exhibits several qualitatively different types of solutions
corresponding to Euler’s elastica [10]. One may expect that the solution
structure becomes more complex if rolling is not restricted to no-
spinning but constrained dynamically [15]. This makes a systematic
construction of the initial guess for the optimal control very difficult.
The geometric phase approach is based on tracing a closed path
on the sphere, which produces a nonclosed path in the contact plane.
For the kinematic model with pure rolling constraint, tracing geodesic
lines on the sphere results in geodesic lines in the contact plane and
vice versa, and many algorithms exploit this property explicitly [9],
[12], [13] or implicitly as a part of a more general procedure [14].
However, if the rolling is constrained dynamically, this property—
geodesic-to-geodesic mapping—does not, in general, hold true [15].
For this reason, the aforementioned algorithms cannot be used without Fig. 1. Rolling system with orthogonal placement of rotors.
essential modifications because the trajectories they produce are not,
in general, dynamically realizable.
To the best of our knowledge, not many results have been reported
so far on the development of motion planning algorithms for the actu-
ation by two rotors. In [2], the motion planning problem was posed in
the optimal control settings using an approximation by the Phillip Hall
basis [16]. However, since the robot dynamics are not nilpotent, this is
not an exact representation of the system and it can result to inaccu-
racies. An exact motion planning algorithm was described in [8], but
the trajectories generated by that algorithm are not always dynamically
realizable [15].
This paper continues our study undertaken in [15], where we devel-
oped a mathematical model of a rolling robot actuated by internal rotors
and analyzed the model’s properties. Here, we solve the motion plan-
ning problem in dynamic formulation by using the iterative steering
approach to control of general (not necessarily nilpotentizable or dif-
ferentially flat) driftless nonholonomic systems, which was formulated
and tested in [17]. The approach employs a nilpotent approximation of
the dynamic model for the synthesis of the control actions. Contrary
to the conventional tangent linearization, the nilpotent approximation
retains the controllability property of the original nonholonomic sys- Fig. 2. Basic frames and contact coordinates.
tem [18], [19]. For the construction of the control inputs, we resort to
the geometric phase approach. At each iteration step, it results, finite two contact frames, one on the object Σc o and one on the plane Σc b .
algebraic expressions for the change of the state variables of the approx- The contact coordinates are given by the angles ϑ and ϕ, describing the
imate nilpotent system, which essentially simplifies the computation contact point on the sphere, by the displacements x and y, describing
of the control inputs. the contact point on the plane, and by the contact angle ψ, which is
The paper is organized as follows. In Section II, a short summary of defined as the angle between the x-axis of Σc o and Σc b (the z-axes of
the mathematical model established in [15] is given. In Section III, a these frames are aligned, as depicted in Fig. 2). It is assumed that the
motion planning strategy, consisting of trivial and nontrivial maneuvers, frame Σc b is parallel to Σb , and in the zero configuration the axes of
is proposed. For the nontrivial maneuver, the nilpotent approximation Σo are parallel to those of Σb .
of the system dynamics is first constructed, and then an iterative steering The position of the contact point on the sphere is parameterized
algorithm is fabricated and verified under a simulation study. Finally, as c(ϑ, ϕ) = [−R sin ϑ cos ϕ, R sin ϕ, −R cos ϑ cos ϕ]T , where R is
conclusions are drawn in Section IV. the radius of the sphere. In this parameterization, the origin is placed
at the south pole of the sphere. In terms of the contact coordinates, the
orientation matrix of the sphere (the orientation of Σo relative to Σb )
II. MATHEMATICAL MODEL 
is defined as R(ψ, ϕ, ϑ) = RTz (ψ)RTx (ϕ)RTy (ϑ) = [n1 | n2 | n3 ] ,
Consider a rolling robot composed of a spherical shell (carrier) where Rx (ϕ), Ry (ϑ), and Rz (ψ) are the matrices of elementary ro-
actuated by internal rotors. It is assumed that the rotors have the same tations, and the columns of the orientation matrix are defined as
mass distribution and the center of mass of the system is located at
⎡ ⎤
the geometric center. The rotors are mounted symmetrically along cos ϑ cos ψ + sin ϑ sin ϕ sin ψ
orthogonal axes, as shown in Fig. 1. On each axis, the two diametrically ⎢ ⎥
n1 = ⎣ − cos ϑ sin ψ + sin ϑ sin ϕ cos ψ ⎦ (1)
opposite rotors are actuated in tandem. This scheme, with actuation
along two orthogonal axes, was first proposed in [2] and later on studied cos ϑ cos ϕ
in [8], [15]. ⎡ ⎤
cos ϕ sin ψ
Define the following coordinate frames (see Fig. 2): Σb is a base ⎢ ⎥
(inertial) frame fixed in the contact plane and Σo is a frame fixed at n2 = ⎣ cos ϕ cos ψ ⎦ (2)
the geometric center of the spherical object. In addition, we introduce − sin ϕ
IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO. 4, AUGUST 2014 995

⎡ ⎤
− sin ϑ cos ψ + cos ϑ sin ϕ sin ψ III. MOTION PLANNING
⎢ ⎥
n3 = ⎣ sin ϑ sin ψ + cos ϑ sin ϕ cos ψ ⎦ . (3) The motion planning problem we address in this paper consists of
finding a trajectory x(t), t ∈ [0, T ], given the start state x(0) = xs and
cos ϑ cos ϕ
the final state x(T ) = xf . It should be noted that for the robot under
T
Let ω = {ωx , ωy , ωz } be the angular velocity of the frame Σo , consideration, this problem cannot be solved exclusively at the level
defined in projections onto the axes of Σb . The evolution of the contact of kinematics1 , by dealing only with the system (4) and finding the
coordinates can be established from the Montana equations [16] and appropriate vector of the angular velocity ω(t). The reason is that the
represented as components of the vector ω are dynamically (or, better said, inertially)
ẋ = A(x) ω (4) constrained by the condition n3 · J ω = 0, which follows from (6).
where the state vector x = {ϑ, ϕ, ψ, x, y}T , and The physical meaning of this condition is that the angular momentum
⎡ ⎤ of the rolling carrier must lie in in the plane through the axes of the
− sin ψ/ cos ϕ − cos ψ/ cos ϕ 0
actuated rotors.
⎢ ⎥
⎢ − cos ψ sin ψ 0 ⎥ By checking the Lie algebra rank condition, one can find [15] that the
⎢ ⎥
⎢ ⎥ distribution formed by the vector fields h1 , h2 , h3 = [h1 , h2 ], h4 =
A = ⎢ − sin ψ tan ϕ − cos ψ tan ϕ −1 ⎥ . (5)
⎢ ⎥ [h1 , h3 ], h5 = [h2 , h3 ], where [·, ·] stands for the Lie brackets of two
⎢ ⎥
⎣ 0 R 0 ⎦ vector fields, has full rank unless ϑ = ±π/2, i.e., the contact point lies
−R 0 0 on the great circle in the equatorial plane of the rotors axes (red line in
Fig. 1). However, the distribution formed by h1 , h2 , h3 , h4 , h6 , where
Note that the no-spinning constraint ωz = 0, featuring the pure rolling
h6 = [h1 , h4 ], has full rank at ϑ = ±π/2 and, therefore, the system
mode, is not imposed here.
(7) is controllable.
The dynamic equations for the system under consideration are de-
The existence of the singular line contrasts the robot under consider-
veloped in [15]. Under the assumption that the motion does not start
ation to the ball–plate system. Mathematically, when the contact point
impulsively, the dynamic model admits the following integral
gets into the singular line the degree of nonholonomy of the system

2
(7) changes from three to four. Note that when ϑ = ±π/2, the vec-
J ω + Jr q̇i ni = 0 (6) tor ω is always on the line through the vector n3 and the condition
i= 1
of dynamic realizability, n3 · J ω = 0, is not satisfied. The physical
reflecting the conservation of the total angular momentum of the system meaning of this singularity line, defined by ϑ = ±π/2, is evident—it is
about the contact point, which can be interpreted as the driving principle impossible to generate the rolling motion of the robot along this line
of the robot under consideration. Here, qi is the angle of rotation around when the rotors axes are placed in the equatorial plane.
the axis ni , Jr is the inertia moment of the single rotor around its axis While the system (7) is controllable, it can be shown that it is not
of rotation, and J = diag{Jx x , Jx x , Jz z } is the inertia tensor of the nilpotent, not differentially flat, and cannot be represented in chained
composite system (rolling carrier and the rotors) with respect to the form. The lack of special structure places it into the general class
contact point. Jx x = Jz z + M R2 , Jz z = 23 mo R2 + 2Jp + Jr , where of driftless nonholonomic systems and stipulates the use of general
M is the total mass of the system, mo is the mass of the spherical shell, steering techniques. To facilitate motion planning, one can devise a
and Jp is the inertia moment of the rotor about the plane orthogonal to motion strategy composed of maneuvers for which the construction
the axis of rotation. of movements is relatively easier than that conducted in the general
By combining (4) and (6), one obtains the following five states–two formulation.
inputs driftless affine control system In this paper, we fabricate a motion planning strategy consisting
ẋ = H (x)q̇ = h1 (x)q̇1 + h2 (x)q̇2 (7) of two trivial and one nontrivial maneuver, as depicted in Fig. 3. The
trivial maneuvers are executed right before and right after the nontrivial
where the rates of the rotors angles are considered as the control inputs, one. In the first trivial maneuver, the contact coordinates ϑ and ϕ are
and the columns of the matrix H = −Jr AJ −1 [n1 | n2 ] are defined nullified, while in the second one, they are brought from zeros to desired
as values. Thus, in the nontrivial maneuver, the motion planning problem
⎡ sin ϑ tan ϕ ⎤
⎡ ⎤ is solved under the assumption that at the start and end configurations,
⎢ ⎥ 1 the initial and final values of ϑ and ϕ are zero. The change of the
⎢ ⎥ ⎢ ⎥
⎢ cos ϑ ⎥ ⎢ 0 ⎥ contact coordinates x, y, and ψ corresponding to the trivial maneuvers
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ can be computed in advance, and the corresponding initial and desired
⎢ 2 2 ⎥
h1 = c⎢ sin ϑ sin ϕ+k cos ϕ ⎥ , h 2 = c⎢
⎢
(1−k) sin ϕ ⎥. (8)
⎥ values for the nontrivial maneuver can be easily established.
⎢ cos ϕ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ −Rn2 y ⎥ On the whole, this general reconfiguration strategy is comparable
⎢ ⎥ ⎣ ⎦
⎢ ⎥ with those considered in [9], [11]–[13] for the kinematic ball–plate
⎣ −Rn1 y ⎦ Rn2 x
system. Apart from the structural simplicity, the reason the strategy is
Rn1 x divided into trivial and nontrivial maneuvers has also to do with the
Here, n1 x , n1 y , n2 x , n2 y are the corresponding components of the vec- singularity line ϑ = ±π/2. This line is passed only during the trivial
tors n1 and n2 , and the dimensionless constants k = Jx x /Jz z and maneuvers where the singularity does not create any computational
c = Jr /Jx x , defined through the components of the inertia tensor J , problems. Thus, during the nontrivial maneuver, the contact point on
can be represented as the sphere does not leave either lower or upper hemisphere. The details
M R2 of the trivial maneuver are described in the Appendix, and from now
k =1+ 2 (9) on, we concentrate on the nontrivial maneuver.
3
mo R2 + 2Jp + Jr
and 1 This can be done only for some restricted formulations of the motion plan-
Jr k − 1 ning problem, for example, when the evolution of the angular state variables is
c= . (10)
M R2 k not important.
996 IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO. 4, AUGUST 2014

Fig. 3. Motion planning strategy.

A computational algorithm, solving the motion planning problem Having defined the local coordinates y, one then transforms them to
for the nontrivial maneuver, can be constructed with the use of the privileged coordinates z. In the component form, the transformation is
nilpotent approximation2 of the nonnilpotent system dynamics. The represented as
idea was first proposed in [20] and later developed in [17], [21]. Since w j −1

the control problem for the nilpotent system can be solved exactly, the zj = yj − hk (y1 , · · · , yj −1 ) (15)
control inputs found for the nilpotent system can be used for iterative k=2
steering of the original nonnilpotent system (7). In the remainder of this
where
paper, we compute the nilpotent approximation, construct an iterative
steering algorithm, and verify it under simulation. hk (y1 , . . . , yj −1 )
  j −1 α i
 α j −1

k −1 , yi
A. Nilpotent Approximation = g α1 1 · · · g j −1 yj + hq (xo )
αi !
Before approximating the original system (7), it is reasonable to |α |= k , w (α )< w j q=2 i= 1

convert it to strictly triangular form. This can be done with the use of (16)
the following input transformation n j −1
! ! ! w(α) = i = 1 wi αi , |α| = i = 1 αi  , and the expression in the paren-
q̇1
=
1 0 sec ϑ u1
(11) theses is the Lie derivative of order ji =−11 αi along the vector fields
q̇2 c 1 − tan ϑ tan ϕ u2 g 1 , g 2 , . . . , g j −1 evaluated at the point x0 .
Having constructed the transformation (15), one expresses the sys-
where u1 and u2 are the new control inputs. This results in the following
tem (12) in the privileged coordinates and expands it in Taylor series
representation
at the origin. Finally, the nilpotent approximation of system (12) is ob-
ẋ = G(x)u = g 1 (x)u1 + g 2 (x)u2 (12) tained by collecting in the Taylor series the terms of weighted degree
−1. The approximate system
where the columns of the matrix G are defined as
⎡ ⎤ ⎡ ⎤ ż = G̃(z)u = g̃ 1 (z)u1 + g̃ 2 (z)u2 (17)
1 0
⎢ ⎥ ⎢ ⎥ is nilpotent, controllable, composed of polynomial functions, and has
⎢ 0 ⎥ ⎢ 1 ⎥ the same degree of nonholonomy and the same growth vector as the
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ original system [17], [22].
g 1 = ⎢ (1 − k) sin ϕ ⎥, g 2 = ⎢ k sec ϕ tan ϑ ⎥. (13)
⎢ ⎥ ⎢ ⎥ In our implementation of the nontrivial maneuver, we need to con-
⎢ ⎥ ⎢ ⎥
⎣ −R cos ϕ cos ψ ⎦ ⎣ R sin ψ ⎦ struct the nilpotent approximation only at the south pole of the sphere
R cos ϕ sin ψ R cos ψ where ϑ0 = ϕ0 = 0. When the point the approximation is constructed
at is set as x0 = [0, 0, ψ0 , x0 , y0 ] the calculations are simplified con-
The nilpotent approximation of the system (12) in the neighborhood siderably. In particular, the matrix Γ takes the following form
of some point x0 is obtained as follows [17], [22]. First, one transforms
the original coordinates x to local coordinates y at the point x0 as Γ = [ g 1 (x0 ), g 2 (x0 ), g 3 (x0 ), g 4 (x0 ), g 5 (x0 ) ]−1
follows: ⎡ ⎤
1 0 0 0 0
y = Γ(x − x0 ) (14) ⎢ ⎥
⎢ 0 1 0 0 0 ⎥
where Γ is the inverse of a matrix with columns g 1 , g 2 , g 3 = ⎢ ⎥
⎢ ⎥
[g 1 , g 2 ], g 4 = [g 1 , g 3 ], g 5 = [g 2 , g 3 ]. Let Ls (x0 ) be the vector space =⎢ 0 0 γ3 3 0 0 ⎥ (18)
⎢ ⎥
generated at x0 by the Lie brackets of g 1 and g 2 of length ≤ s, s = ⎢ ⎥
⎣ 0 γ4 2 0 γ4 4 γ4 5 ⎦
1, 2, . . . and ns (x0 ) = dimLs (x0 ), s = 1, . . . , r, where r is smallest
integer such that dimLr (x0 ) = 5. The weight wi of the coordinates xi γ5 1 0 0 γ5 4 γ5 5
is defined by setting wj = s if ns −1 < j ≤ ns , with ns = ns (x0 ) and where the nonzero elements of Γ are obtained as
n0 = 0. Note that r defines the degree of nonholonomy, and the vec- 1 1
tor (n1 (x0 ), . . . , nr (x0 )) is the growth vector of the nonholonomic γ3 3 = , γ4 2 = γ5 1 = (19)
2k − 1 3k − 1
system (12).
sin ψ0 cos ψ0
γ4 4 = −γ5 5 = , γ4 5 = γ5 4 = .
2 The conventional tangent linearization cannot be used for our purpose be-
R(1 − 3k) R(1 − 3k)
cause it does not retain the controllability property. (20)
IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO. 4, AUGUST 2014 997

In addition, when ϑ0 = ϕ0 = 0 the second term of (15) vanishes and (15) the image of the subgoal in the privileged coordinates, z di ,
the privileged coordinates zi coincides with yi . Therefore, the trans- and construct a controller that steers the approximate nilpo-
formation from the original to the privileged coordinates is obtained tent system (17) from the origin to z di in the time interval
as t ∈ [ti , ti + 1 ]. This control problem can be solved exactly, and
the control law found for the nilpotent system is then applied to
z1 = ϑ (21) the original system (12).
z2 = ϕ (22) 3) If the state x(ti + 1 ) the system is brought to by the control action
is far from xf , set i = i + 1 and xi = x(ti + 1 ) and repeat the
z3 = γ3 3 ψ (23) iterative process.
z4 = γ4 2 ϕ + γ4 4 x + γ4 5 y (24) The particular structure of the control law to steer a nilpotent sys-
tem can be constructed in a variety of ways [20], [23]–[25]. In this
z5 = γ5 1 ϑ + γ5 4 x + γ5 5 y. (25) paper, we build it in the spirit of the geometric phase approach [9],
[12] because it results to simple calculations and has a clear geometric
The original system (12), (13), expressed in the privileged coordinates interpretation. Compared with steering the robot under consideration
with the use of (21), (25), becomes by sinusoids at integrally related frequencies [26], it results in well-
structured trajectories and requires less iterations to reach the goal
z˙1 = u1 (26)
state. As in [17], the nontrivial maneuver in our construction is divided
z˙2 = u2 (27) into two parts corresponding to attaining, respectively, the desired ori-
entation and translation. More specifically, reconfiguring the initial
z˙3 = {(1 − k)γ3 3 sin z2 } u1 + {k γ3 3 tan z1 sec z2 } u2 (28) state xs = [0, 0, ψs , xs , ys ] to the final one xf = [0, 0, ψf , xf , yf ] is
"  %
z3 z3 described as follows.
z˙4 = R cos z2 γ4 5 sin − γ4 4 cos u1 First Part: In the first part, we steer ϑ, ϕ, and ψ to the desired values
γ3 3 γ3 3
"  % 0, 0, and ψf regardless of the values of x and y. The computational
z3 z3 procedure can be summarized as follows.
+ γ4 2 + R γ4 4 sin + γ4 5 cos u2 (29)
γ3 3 γ3 3 
1) Set i = 0, ti = 0, xi = [ϑi , ϕi , ψi , xi , yi ] = [0, 0, ψs , xs , ys ].
"  %
z3 z3 2) Set ti + 1 = ti + ΔT , define the subgoal ψid = ψi + η(ψf −
z˙5 = γ5 1 − R cos z2 γ5 4 cos − γ5 5 sin u1 ψi ), where η ∈ [0, 1], and compute by (23) its image in the
γ3 3 γ3 3
"  % privileged coordinates
z3 z3
+ R γ5 4 sin + γ5 5 cos u2 . (30)
γ3 3 γ3 3 Ψdi = η(ψf − ψi )/(2k − 1) (32)

By expanding this system in Taylor series at the origin and retaining where k > 1 is the inertia ratio given by (9). Let ω = 2π/ΔT .
the terms of weighted degree −1, one obtains the representation (17) Define the control law by
with the vector fields g̃ 1 and g̃ 2 defined as
⎡ ⎤ u1 (t) = ri cos σi ωt (33)
1 ⎡ ⎤
0 u2 (t) = ri sin σi ωt (34)
⎢ ⎥ ⎢ ⎥
⎢ 0 ⎥ ⎢ ⎥
⎢ ⎥ ⎢
1
⎥
⎢ ⎥ ⎢ ⎥ where t ∈ [ti , ti + 1 ] and σi = sign(ψf − ψi ). Geometrically, in
⎢ γ3 3 (1 − k)z2 ⎥ ⎢ γ3 3 kz1 ⎥
⎢ ⎥ the space of the contact coordinates ϑ and ϕ, this control law
g̃ 1 = ⎢ ⎥ , g̃ 2 = ⎢
⎢ γ4 4
⎥.
⎥ (31)
⎢ γ4 5 1 ⎥ ⎢ ⎥ traces a circle of radius ri in the direction defined by σi
⎢ Rz3 + γ4 4 Rz22 ⎥ ⎢ γ3 3 Rz 3 ⎥
⎢ γ3 3 2 ⎥ ⎢ ⎥
⎢ ⎥ ⎣γ ⎦ ri
⎣ γ5 5 1 ⎦ 54 ϑ(t) = sin σi ωt (35)
Rz3 + γ5 4 Rz2 2 Rz3 σi ω
γ3 3 2 γ3 3
ri
ϕ(t) = (1 − cos σi ωt). (36)
The resulting approximate system is polynomial and in triangular form. σi ω
The latter property facilitates the integration of the approximate dynam- Therefore, by the end of the iteration, the contact coordinates ϑ
ics in closed form under some well-defined control inputs. and ϕ remain unchanged.
It should be finally noted that if one sets k = 0 in (31), the approx- By direct integration of the approximate system (17) and (31)
imate system (17) will coincide with that constructed in [17] for the with the control (33) and (34), it can be shown that
kinematic model of a ball–plate system with no-spinning constraint
ωz = 0. However, in our model, such a setting can be only purely z1 (ti + 1 ) = z2 (ti + 1 ) = 0, z3 (ti + 1 ) = σi πri2 /ω 2 . (37)
hypothetical because by definition (9) the inertia ratio k > 1.
The free parameter ri is defined from the condition z3 (ti + 1 ) =
B. Iterative Steering Ψdi , which results in
#
Having constructed the nilpotent approximation (17), one can use ri = ω |Ψdi |/π. (38)
it for the iterative steering of the original system (12) from the initial
xs to a desired state xf . The general idea of this approach can be Thus, the control law defined by (33) and (34) steers the privi-
formulated as follows [17]. leged coordinates z1 , z2 , and z3 from the origin to, respectively,
1) Set i = 0, ti = 0, and xi = xs . 0, 0, and Ψdi . By checking the first three equations of the system
2) Set ti + 1 = ti + ΔT , where ΔT is the movement time allocated (12), (13), one can see that in the original coordinates, we have
for one iteration. Define the subgoal xdi = xi + η(xf − xi ), ϑ(ti + 1 ) = ϕ(ti + 1 ) = 0, but ψ(ti + 1 ) does not necessarily reach
where η ∈ [0, 1] is a sufficiently small number. Compute from ψid .
998 IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO. 4, AUGUST 2014

3) Set xi = xi + 1 , increase the counter i = i + 1, and repeat the Next, since one circle is traced in clockwise direction and the
calculations until ψ(ti + 1 ) reaches a given vicinity of ψf . other in the counterclockwise direction, by the end of the itera-
It should be noted that, since the control law (33) and (34) defines a tion, the angle ψ remains unchanged.
circle in the space of ϑ and ϕ, one has |ϑ(t)| ≤ ri /ω and |ϕ(t)| ≤ ri /ω. By direct integration of the approximate system (17), (31) with
To guarantee that the contact point does not leave the lower hemisphere, the control (43), (44) and (45), (46), it can be shown that
one must have
- .
z1 (ti + 1 ) = z2 (ti + 1 ) = z3 (ti + 1 ) = 0 (51)
ri η |ψ f − ψi | η
= ≤ < π/2 (39) z4 (ti + 1 ) = 2πri3 sin (ψf − θi )/ω 3
(52)
ω (2k − 1)π 2k − 1
z5 (ti + 1 ) = 2πri3 cos (ψf − θi )/ω . 3
(53)
which gives the following estimate
The free parameters ri and θi are defined from the conditions
η < (2k − 1)π 2 /4. (40) z4 (ti + 1 ) = Xid and z5 (ti + 1 ) = Yid , which results in
.
Thus, since the inertia ratio k > 1, for any η ∈ [0, 1] the control law d 2
6 (X i ) + (Yi )
d 2
(33), (34) keeps the contact point away from the singularity line ϑ = ri = ω 2
(54)
4π
±π/2.
1) Second Part: In the second part of the maneuver, we steer x, y θi = ψf − arctan (Xid /Yid ). (55)
to the desired values xf , yf without changing (in the final configura-
Thus, the control law defined by (43) and (44), as well as (45)
tion) ϑ = ϕ = 0 and ψ = ψf . The computational procedure can be
and (46), steers the state variables of the approximate nilpotent
summarized as follows.
system (17) from the origin to, respectively, [0, 0, 0, Xid , Yid ]. By
1) Set i = 0, ti = 0, xi = [0, 0, ψf , x̄s , ȳs ], where x̄s , ȳs are the
the geometric construction, for the original state variables, we
coordinates of the contact point in the plane attained by the end
have ϑ(ti + 1 ) = ϕ(ti + 1 ) = 0, and ψ(ti + 1 ) = ψf , but x(ti + 1 )
of the first part of the maneuver.
and y(ti + 1 ) do not necessarily reach xdi and yid .
2) Set ti + 1 = ti + 2ΔT . Define the subgoal in the contact plane
3) Set xi = xi + 1 , increase the counter i = i + 1, and repeat the
xdi = xi + η(xf − xi ) and yid = yi + η(yf − yi ), where η ∈
calculations until the coordinates of the contact point in the plane
[0, 1], and compute by (24) and (25) the image of the subgoal in
[x(ti + 1 ), y(ti + 1 ) reach a given vicinity of [xf , yf ].
the privileged coordinates
Under the control law (43) and (44), as well as (45) and (46), the
η contact point in the space of the contact coordinates ϑ and ϕ is always
Xid = {sin ψf (xf −xi ) + cos ψf (yf −yi )}
R(1−3k) on the circle of radius ri /ω, with |ϑ(t)| ≤ ri /ω and |ϕ(t)| ≤ ri /ω. To
(41) ensure that the contact point does not leave the lower hemisphere, one
η
needs to have .
Yid = {cos ψf (xf −xi ) − sin ψf (yf −yi )} . ri d 2 d 2
6 (X i ) + (Yi ) π
R(1−3k) = 2
< . (56)
ω 4π 2
(42) By transforming this inequality with the use of (41), (42), one obtains
the following estimate
Let ω = 2π/ΔT . Define the control law by
η π 4 (3k − 1)
u1 (t) = ri cos (θi + ωt) (43) <  . (57)
R 32 (xf − xi )2 + (yf − yi )2
u2 (t) = ri sin (θi + ωt) (44)
Thus, if η is selected in accordance with (57), the trajectory of the
for t ∈ [2(i − 1)ΔT , (2i − 1)ΔT ] and contact point on the sphere is kept below the singularity line ϑ = ±π/2.
Two comments are in order.
u1 (t) = ri cos (θi − ωt) (45) 1) For the brevity of presentation, in the iterative scheme described
in this section, the parameter η is a constant that must be tuned
u2 (t) = ri sin (θi − ωt) (46)
to ensure the convergence of the iterative process. A globally
for t ∈ [(2i − 1)ΔT , 2iΔT ]. Geometrically, this control law de- convergent iterative scheme with automatic adjustment of η at
fines two symmetric circles in the space of the contact coordinates every iteration is reported in [27], [28]. In principle, the pre-
ϑ and ϕ sented scheme can be easily adjusted to the settings of [27], [28]
without modification of the control laws. Note, however, that the
ri
ϑ(t) = {sin(θ+ωt)−sin θ} (47) convergence rate for the schemes with constant and nonconstant
ω η can be different.
ri 2) The control laws constructed in this section define a path con-
ϕ(t) = {cos θ−sin(θ+ωt)} (48)
ω necting the initial and final configurations. The timing of trac-
for t ∈ [2(i − 1)ΔT , (2i−1)ΔT ] and ing this path can be readjusted as t = t(τ ), τ ∈ [0, T ], with
uj (τ ) = uj (t)dt/dτ, j = 1, 2. This can also be interpreted as
ri breaking motion planning into path planning and time plan-
ϑ(t) = {sin θ − sin(θ + ωt)} (49)
ω ning [29]. To guarantee that the motion at each iteration does
ri not start and end impulsively, one can set the timing con-
ϕ(t) = {sin(θ + ωt) − cos θ} (50)
ω trol law as t(τ ) = ti + ΔT {10s3 (τ ) − 15s4 (τ ) + 6s5 (τ )},
where s(τ ) = (τ − ti )/ΔT so that t(τi ) = τi = iΔT and
for t ∈ [(2i−1)ΔT , 2iΔT ]. Therefore, the contact variables ϑ
[dt/dτ ]τ = τ i = [dt/dτ ]τ = τ i + 1 = 0.
and ϕ are zero at t = ti + (2i − 1)ΔT , and t = ti + 2iΔT .
IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO. 4, AUGUST 2014
Fig. 4. Time histories of x (red line), y (blue line) (top), ψ (blue line), ϑ (red
line), and ϕ (green line) (bottom) during the first part of the maneuver.
C. Simulation
The performance and effectiveness of the motion planning algorithm
are tested under simulation for the nontrivial maneuver. Here, we drive
the system from the initial state x0 = {0, 0, 3π/4, 2.0, 2.0} to the
final state xf = {0, 0, 0, 0, 0}. In the simulation, we set R = 1, ΔT =
1, k = 2.5, and η = 0.6. The units of all the dimensional quantities are
specified in the SI system.
In the first part of the maneuver, where we drive ψ to zero regardless
of the attained x and y, we set η = 0.6. Time histories of all the state
variables are shown in Fig. 4. After five iterations the error between
the current and target values of ψ becomes less than 0.001, while ϑ
and ϕ keep zero values at the end of each iteration (integer moments
of time). By the end of the first part of the maneuver, the values of x
and y change, respectively, from 2.0 to 1.51 and from 2.0 to 2.85.
In the second part of the maneuver, where x and y are driven to
the origin, we set η = 0.6. Time histories of the state variables are
shown in Fig. 5. After ten iterations the error between the current
and target position of the contact point on the plane, defined by the
Euclidean distance (x(t) − xf )2 + (y(t) − yf )2 , becomes less than
0.001, while ϑ, ϕ, and ψ keep zero values at the end of each iteration
(integer moments of time).
The evolution of the contact point trajectory on the sphere and on
the plane is shown in Fig. 6. Here, each iteration in the first part of the
maneuver is shown in green color. For the second part of the maneuver,
the first half of each iteration, corresponding to the control action (43),
(44), is shown in red color while the second half, corresponding to (45)
and (46), is shown in blue color. As can be seen from this figure3 , the
magnitude of the change of the state variables is gradually decreasing
as the robot approaches the target state.
While the results presented demonstrate the computational feasibil-
ity of the proposed motion planning strategy, it would be also interesting
to analyze the performance of the steering algorithm when the inertia
parameter k of the mathematical model is changed. Although a de-
tailed theoretical analysis is beyond the scope of this paper, we provide
some simulation results illustrating this issue. The main problem here
3 The animation of the simulation results, provided in supplementary down-
loadable files, are available at http://ieeexplore.ieee.org.
999
Fig. 5. Time histories of x (red line), y (blue line) (top), ψ (red line), ϑ (blue
line), and ϕ (green line) (bottom) during the second part of the maneuver.
is the selection of the parameter η which defines the convergence and
the number of iterations to reach the goal state. In this connection, it
should be noted that the estimates (40), (57) provide the guidance for
not crossing the singularity line but have no relation to the convergence
of the algorithm.
Let us revisit the simulation example considered before and change
the inertia coefficient from k = 2.5 to k = 10. It is found under
simulation trials that to preserve the convergence of the steering al-
gorithm, one needs to decrease the parameter η from 0.6 to 0.5. The
simulation results, presenting the evolution of the contact point trajec-
tory on the sphere and on the plane, are shown in Fig. 7. Note that the
magnitude of the control inputs is directly proportional to the parameter
η. Therefore, the decrease of η results to smaller circles traced in the
contact coordinates ϑ and ϕ, which in turn leads to the increase in the
number of iterations necessary to reach the goal state.
The number of iterations as a function of η for different values
of the inertia ration k can be estimated under simulation runs. For the
example considered in this section, the simulation results, including the
hypothetical case of k = 0, which corresponds to the kinematic model
of pure rolling, are shown in Fig. 8. As can be seen from this figure,
the interval of η on which the iterative steering algorithm converges is
gradually decreasing with the increase of the inertia ratio k. This means
that the accuracy of the nilpotent approximation at a neighborhood of
the south pole of the sphere is decreasing with the increase of k.
A clear interpretation of this fact is not available at the moment as
the thorough estimation of the accuracy of the nilpotent approximation
goes beyond the scope of this paper. It is interesting to note, however,
that in the limiting case of k → ∞, the approximate system (17) and
1000 IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO. 4, AUGUST 2014

Fig. 6. Trajectory of the contact point on the plane (top) and on the sphere
(bottom) during the nontrivial maneuver for k = 2.5. The first part of the
maneuver is shown in green color, while the second part is shown in red [as
resulted from (43) and (44)] and blue [as resulted from (45) and (46)] colors.
Fig. 7. Trajectory of the contact point on the plane (top) and on the sphere
(bottom) during the nontrivial maneuver for k = 10. The first part of the ma-
neuver is shown in green color, while the second part is shown in red [as resulted
from (43) and (44)] and blue [as resulted from (45) and (46)] colors.

(31) remains controllable but the matrix Γ in (18) becomes singular.

Thus, the transformation from the original to privileged coordinates
tends to singularity with the increase of k. Note also that from the
physical point of view, roughly speaking, the dynamic coefficient k
becomes larger with the increase of the ratio of the inertia of the
spherical shell to that of the rotors. Therefore, it is realistic that driving
a heavier spherical shell with lighter rotors would result in the control
inputs of smaller magnitude.
It should be noted that in our algorithm, constructed in the settings
of [17], η is constant for every iteration. The iterative algorithm [27],
[28] with automatic adjustment of η converges for any value of k. In our
Fig. 8. Number of iterations in the second part of the nontrivial maneuver as
numerical experiments, not reported in this paper because of the page a function of η for different values of the inertia ratio k.
limitation4 , the latter algorithm often converges faster than the former
one but not always because the actual convergence rate depends also
on the initial and final configurations. Still, the pattern of the decrease IV. CONCLUSION
of the convergence rate with the increase of k can be witnessed also in
Motion planning for a spherical rolling robot, actuated by two in-
the globally convergent algorithm [27], [28].
ternal rotors that are placed on orthogonal axes, has been studied in
this paper. The problem of finding trajectories of the state variables
given the initial and final configuration of the robot has been consid-
4 However, Mathematica software scripts, implementing the algorithms with ered. One of the characteristic features of this problem is that it can be
constant and nonconstant η, are available from the authors upon request. addressed only in dynamic formulation. The mathematical model of
IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO. 4, AUGUST 2014 1001

Let α be the angle of rotation around the unit vector ns . The matrix
of rotation around ns can be defined by the Rodriguez formula

Rs = I + sin αΩ(ns ) + (1 − cos α)Ω2 (ns ) (58)

where Ω(·) is the skew-symmetric matrix used for the matrix repre-

sentation of the vector product (Ω(a)b = a × b), which gives
⎡ ⎤
1−sin2 β(1−cos α) 12 sin 2β(1−cos α) sin β sin α
⎢ ⎥
Rs =⎣ 12 sin 2β(1−cos α) 1−cos2 β(1−cos α) − cos β sin α⎦.
sin β sin α cos β sin α cos α
(59)
The orientation of the sphere at the final configuration can be repre-
sented as
R(ψ0 , 0, 0)Rs (β, α) = R(ψf , ϑf , ϕf ) (60)
where the orientation matrix of the sphere R(ψ, ϑ, ϕ) is defined in
Section II. By solving this equation with respect to the unknown angles
α, β, and ψ0 , one obtains

α = arccos (cos ϑf cos ϕf ) (61)

 
Fig. 9. Trivial maneuver. cos ϕf sin ϑf
β = arctan (62)
sin ϕf
 
the robot, represented by a driftless control system, contains a physical sin ϑf cos ψf −cos ϑf sin ϕf sin ψf
ψ0 = β −arctan . (63)
singularity corresponding to the motion of the contact point along the sin ϑf sin ψf +cos ϑf sin ϕf cos ψf
equatorial line in the plane of the two rotors. To solve the state-to-state
Having defined the angles α, β and ψ0 , one finally obtains
transfer problem, a motion planning strategy composed of two trivial
and one nontrivial maneuver has been proposed. The trivial maneuvers x0 = xf − R α cos (β − ψ0 ) (64)
implement motion along the geodesic line perpendicular to the singu-
larity line. The nontrivial maneuver is based on an iterative steering y0 = yf − R α sin (β − ψ0 ). (65)
algorithm and employs the nilpotent approximation of the originally
nonnilpotent robot dynamics. The feasibility of the motion planning It remains to be shown that the motion along the selected geodesic
strategy has been verified under simulation. line is dynamically realizable. When α is changed as a function of time
In future research, apart from the experimental verification of the the orientation matrix of the sphere R is defined by the left hand side
motion planning strategy, it would be interesting to try out different, of (60). Therefore,
more systematic techniques to pass through the singularity line. Theo-
n3 = [sin α(t) sin (β −ψ0 ), − sin α(t) cos (β −ψ0 ), cos α(t)]T .
retical approaches to steering nonholonomic systems with singularities
(66)
are reported in [22], [28]. In principle, these approaches are applicable
Computing ω from Ω(ω) = ṘRT yields
to the rolling robot with two rotors. However, the practical construction
of computationally feasible algorithms based on these approaches is ω = Rα̇(t)[cos (β −ψ0 ), sin (β −ψ0 ), 0]T . (67)
not trivial and requires a special investigation.
Since the last component of ω is zero, the motion is pure rolling and the
contact point on the plane moves along the straight line6 connecting
APPENDIX
(x0 , y0 ) with (xf , yf ). Since J = diag{Jx x , Jx x , Jz z }, it is easily
Here, we consider the trivial maneuver executed in the third step5 of verified that n3 · J ω = 0, and, therefore, the motion is dynamically
the motion strategy and show how to modify the x, y, and ψ components realizable. The velocities of the rotors, realizing this motion, are defined
of the vector x0 so that rolling along a geodesic line on the sphere will from (6) and obtained as
guarantee arrival at the given target configuration xf . The vector x0 is
then set as the target configuration for the nontrivial maneuver executed q̇1 (t) = −(n1 · J ω)/Jr = −(Jx x /Jr )α̇(t) cos β (68)
in the second step of the motion strategy.
q̇2 (t) = −(n2 · J ω)/Jr = −(Jx x /Jr )α̇(t) sin β. (69)
Let ϑf and ϕf be the corresponding components of the vector xf .
The geodesic line connecting ϑf and ϕf with the origin is defined
REFERENCES
uniquely and is perpendicular to the singularity line ϑ = ±π/2. Let
β + π/2 be the angle of orientation of the great circle, formed by this [1] A. Bicchi, A. Balluchi, D. Prattichizzo, and A. Gorelli, “Introducing the
geodesic line, with respect to the frame Σo as shown in Fig. 9. When “Sphericle”: An experimental testbed for research and teaching in non-
the sphere rolls along the geodesic line it rotates around the unit vector holonomy,” in Proc. IEEE Int. Conf. Robot. Autom., Albuquerque, NM,
USA, 1997, vol. 3, pp. 2620–2625.
ns whose components in the frame Σo are [cos β, sin β, 0]T .

6 Note that if a different geodesic line, not orthogonal to the rotors plane and
therefore not passing through the south pole is selected, the trajectory of the
5 The calculations in the first step are similar and can be conducted straight- contact point on the plane will not be a straight line [15] and the motion will
forwardly in much the same way. not necessarily be pure rolling.
1002
[2] S. Bhattacharya and S. Agrawal, “Spherical rolling robot: A design and
motion planning studies,” IEEE Trans. Robot. Autom., vol. 16, no. 6,
pp. 835–839, Dec. 2000.
[3] A. Javadi and P. Mojabi, “Introducing glory: A novel strategy for an om-
nidirectional spherical rolling robot,” ASME J. Dyn. Syst., Meas., Contr.,
vol. 126, no. 3, pp. 678–683, 2004.
[4] R. Balasubramanian, A. Rizzi, and M. Mason, “Legless locomotion: A
novel locomotion technique for legged robots,” Int. J. Robot. Res., vol. 27,
no. 5, pp. 575–594, 2008.
[5] M. Ishikawa, Y. Kobayashi, R. Kitayoshi, and T. Sugie, “The surface
walker: A hemispherical mobile robot with rolling contact constraints,”
in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., St. Louis, MO, USA,
2009, pp. 2446–2451.
[6] V. Joshi, R. Banavar, and R. Hippalgaonkar, “Design and analysis of a
spherical mobile robot,” Mech. Mach. Theory, vol. 45, no. 2, pp. 130–136,
2010.
[7] A. Borisov, A. Kilin, and I. Mamaev, “How to control Chaplygin’s sphere
using rotors: Part II,” Regular Chaotic Dyn., vol. 18, no. 1–2, pp. 144–158,
2013.
[8] V. Joshi and R. Banavar, “Motion analysis of a spherical mobile robot,”
Robotica, vol. 27, no. 3, pp. 343–353, 2009.
[9] Z. Li and J. Canny, “Motion of two rigid bodies with rolling constraint,”
IEEE Trans. Robot. Autom., vol. 6, no. 1, pp. 62–72, Feb. 1990.
[10] V. Jurdjevic, “The geometry of the plate-ball problem,” Arch. Rational
Mech. Anal., vol. 124, no. 4, pp. 305–328, 1993.
[11] A. Marigo and A. Bicchi, “Rolling bodies with regular surface: Control-
lability theory and applications,” IEEE Trans. Autom. Control, vol. 45,
no. 9, pp. 1586–1599, Sep. 2000.
[12] R. Mukherjee, M. Minor, and J. Pukrushpan, “Motion planning for a
spherical mobile robot: Revisiting the classical ball-plate problem,” ASME
J. Dyn. Syst., Meas. Contr., vol. 124, no. 4, pp. 502–511, 2002.
[13] M. Svinin and S. Hosoe, “Motion planning algorithms for a rolling sphere
with limited contact area,” IEEE Trans. Robot., vol. 24, no. 3, pp. 612–625,
Jun. 2008.
[14] F. Alouges, Y. Chitour, and R. Long, “A motion-planning algorithm for the
rolling-body problem,” IEEE Trans. Robot., vol. 26, no. 5, pp. 827–836,
Oct. 2010.
[15] M. Svinin, A. Morinaga, and M. Yamamoto, “On the dynamic model
and motion planning for a spherical rolling robot actuated by orthogonal
internal rotors,” Regular Chaotic Dyn., vol. 18, no. 1–2, pp. 126–143,
2013.
[16] R. Murray, Z. Li, and S. Sastry, A Mathematical Introduction to Robotic
Manipulation. Boca Raton, FL, USA: CRC, 1994.
[17] G. Oriolo and M. Vendittelli, “A framework for the stabilization of general
nonholonomic systems with an application to the plate-ball mechanism,”
IEEE Trans. Robot., vol. 21, no. 2, pp. 162–175, Apr. 2005.
[18] H. Hermes, “Nilpotent and high-order approximations of vector field sys-
tems,” SIAM Rev., vol. 33, no. 2, pp. 238–264, 1991.
[19] A. Bellaı̈che, “The tangent space in sub-riemannian geometry,” in Sub-
Riemannian Geometry, A. Bellaı̈che and J.-J. Risler, Eds. Cambridge,
MA, USA: Birkhäuser, 1996, pp. 1–78.
[20] G. Lafferriere and H. Sussmann, “A differential geometric approach to
motion planning,” in Nonholonomic Motion Planning, Z. Li and J. Canny,
Eds. Norwell, MA, USA: Kluwer, 1993, pp. 235–270.
[21] E. Sachkova, “Approximate solution of a control problem on the basis of
the nilpotent approximation,” Different. Equat., vol. 45, no. 9, pp. 1386–
1364, 2009.
[22] M. Vendittelli, G. Oriolo, F. Jean, and J.-P. Laumond, “Nonhomoge-
neous nilpotent approximations for nonholonomic systems with singu-
larities,” IEEE Trans. Automat. Contr., vol. 49, no. 2, pp. 261–266, Feb.
2004.
[23] R. Murray and S. Sastry, “Nonholonomic motion planning: Steering using
sinusoids,” IEEE Trans. Autom. Control, vol. 38, no. 5, pp. 700–716, May
1993.
[24] S. Agrawal and S. Bhattacharya, “Optimal control of driftless nilpoten
systems: Some new results,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots
Syst., Kyongju, Korea, 1999, vol. 1, pp. 356–361.
[25] E. Sachkova, “Solution of a control problem for a nilpotent system,”
Different. Equat., vol. 44, no. 12, pp. 1768–1772, 2008.
[26] A. Morinaga, M. Svinin, and M. Yamamoto, “On the motion planning
problem for a spherical rolling robot driven by two rotors,” in Proc.
IEEE/SICE Int. Symp. Syst. Integrat., Fukuoka, Japan, 2012, pp. 704–
709.
[27] F. Jean, G. Oriolo, and M. Vendittelli, “A global convergent steering
algorithm for regular nonholonomic systems,” in Proc. IEEE Int. Conf.
Decis. Control, Seville, Spain, 2005, pp. 7514–7519.
1552-3098 © 2014 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See http://www.ieee.org/publications standards/publications/rights/index.html for more information.
IEEE TRANSACTIONS ON ROBOTICS, VOL. 30, NO. 4, AUGUST 2014
[28] Y. Chitour, F. Jean, and R. Long, “A global steering method for non-
holonomic systems,” J. Different. Equat., vol. 254, pp. 1903–1956,
2013.
[29] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: Modelling,
Planning and Control, (ser. Advanced Textbooks in Control and Signal
Processing). London, U.K.: Springer-Verlag, 2009.
A Selective Retraction-Based RRT Planner for
Various Environments
Junghwan Lee, OSung Kwon, Liangjun Zhang, and Sung-Eui Yoon
Abstract—We present a novel randomized path planner for rigid robots
to efficiently handle various environments that have different character-
istics. We first present a bridge line test that can identify narrow passage
regions and then selectively performs an optimization-based retraction only
at those regions. We also propose a noncolliding line test, which is a dual
operator to the bridge line test, as a culling method to avoid generating
samples near wide-open free spaces. These two line tests are performed
with a small computational overhead. We have tested our method with
different benchmarks that have varying amounts of narrow passages. Our
method achieves up to several times improvements over prior RRT-based
planners and consistently shows the best performance across all the tested
benchmarks.
Index Terms—Motion and path planning, sampling-based motion plan-
ning, various environments.
I. INTRODUCTION
Robot motion planning involves computing collision-free paths for a
robot and has many applications, such as part disassembly simulation,
tolerance verification, protein folding, and computer graphics [1], [2].
The motion planning has been actively studied since the 1970s to
develop a complete solution.
Sampling-based motion planning algorithms have been designed and
successfully used to compute a probabilistic complete solution for a
variety of environments. Two of the most successful algorithms include
the Probabilistic Roadmap Method (PRM) [3] and Rapidly-exploring
Random Tree (RRT) [4]. At a high level, as we randomly generate more
samples, these techniques provide collision-free paths by capturing a
larger portion of the connectivity of the free space.
One of the most challenging cases for sampling-based techniques is
to efficiently handle narrow passages. In many practical motion plan-
ning applications, a robot often needs to pass through narrow passages
Manuscript received November 22, 2013; accepted February 28, 2014. Date
of publication May 12, 2014; date of current version August 4, 2014. This paper
was recommended for publication by Associate Editor M. Vendittelli and Editor
D. Fox upon evaluation of the reviewers’ comments. This work was supported in
part by NRF-2013R1A1A2058052, DAPA/ADD (UD110006MD), MEST/NRF
(2013-067321), and the IT R&D program of MOTIE/KEIT [10044970].
J. Lee, O. Kwon, and S.-E. Yoon are with the Department of Computer
Science, Korea Advanced Institute of Science and Technology, Daejeon, Korea
(e-mail: goolbee@gmail.com; miruce78@gmail.com; sungeui@gmail.com).
L. Zhang is with Samsung Research America, San Jose, CA 95134 USA
(e-mail: zlj@cs.unc.edu).
Color versions of one or more of the figures in this paper are available online
at http://ieeexplore.ieee.org.
Digital Object Identifier 10.1109/TRO.2014.2309836