arXiv:1102.1820v2  [cs.RO]  11 Feb 2011
Optimal Synthesis for Nonholonomic Vehicles With
Constrained Side Sensors
Paolo Salaris∗, Lucia Pallottino∗and Antonio Bicchi∗∗†
February 27, 2018
Abstract
We present a complete characterization of shortest paths
to a goal position for a vehicle with unicycle kinemat-
ics and a limited range sensor, constantly keeping a given
landmark in sight. Previous work on this subject stud-
ied the optimal paths in case of a frontal, symmetrically
limited Field–Of–View (FOV). In this paper we provide
a generalization to the case of arbitrary FOVs, including
the case that the direction of motion is not an axis of sym-
metry for the FOV, and even that it is not contained in the
FOV. The provided solution is of particular relevance to
applications using side-scanning, such as e.g. in under-
water sonar-based surveying and navigation.
1
Introduction
In several mobile robot applications, a vehicle with non-
holonomic kinematics of the unicycle type, equipped with
a limited range sensor systems, has to reach a target while
keeping some environment landmark in sight. For exam-
ple, in the Visual–Based control field the vehicle usually
has an on-board monocular camera with limited Field–
Of–View (FOV) and, subject to nonholonomic constraints
on its motion, must move maintaining in sight one or
more specified features of the environment. On the other
hand, in the field of underwater surveying and naviga-
tion, a common task for Autonomous Underwater Vehi-
∗This work was supported by E.C. contracts n.224428 CHAT,
n.224053 CONET (Cooperating Objects Network of Excellence), n.
257462 HYCON2 (Network of Excellence) and n.2577649 Planet.
†∗
The
Interdept.
Research
Center
“Enrico
Piaggio”,
University
of
Pisa,
via
Diotisalvi
2,
56100
Pisa,
Italy.
paolo.salaris,l.pallottino,bicchi@ing.unipi.it
cles (AUV) equipped with side sonar scanners is to de-
tect and recognize objects (mines, wrecks or archeologi-
cal find, etc.) on the sea bed (see e.g. [8, 10]). Side-scan
sonar is a category of sonar systems that is used to effi-
ciently create an image of large areas of the sea. There-
fore, in order to recognize objects AUVs must move keep-
ing them inside the limited range of the sensor.
Motivated by those application, in this paper we pro-
pose the study of optimal (shortest) paths for a nonholo-
nomic vehicle moving in a plane to reach a target position
while making so that a given landmark fixed in the plane
is kept inside a planar cone moving with the robot.
The literature of optimal (shortest) paths stems mainly
from the seminal work on unicycle vehicles with a
bounded turning radius by Dubins [9]. Dubins has char-
acterized the finite family of optimal paths for the particu-
lar vehicle while a complete optimal control synthesis for
this problem has been reported in [4]. Later on, a simi-
lar problem with the car moving both forward and back-
ward has been solved with different approaches in [11],
[15]. In particular, in [14] the optimal control synthesis
for the Reeds&Shepp car has been provided. Minimum
wheel rotation paths in for differential-drive robots have
been considered in [6]. More recently, also the problem
of determining minimum time trajectory has been taken
into account in [16], [1] and [7] for particular classes of
robots, e.g. latter is on underwater robots. Finally, pre-
vious works on the same subject of this paper ([13], [12],
[2]) have studied the optimal paths in case of a vehicle
with a limited on-board camera but only with a symmetric
FOV with respect to the forward direction of the robot. In
this paper, we present a more general synthesis of shortest
paths in case of side sensor systems, like side sonar scan-
ners on UAVs, where the forward direction is not neces-
sarily included inside the sensor range modeled as a cone
centered on the vehicle. The impracticability of paths that
point straight to the feature lead to a more complex anal-
ysis of the reduction to a finite and sufficient family of
optimal paths by excluding particular types of path.
In the rest of the paper, we provide a complete opti-
mal synthesis for the problem, i.e., a finite language of
optimal control words (at most 15 words, depending on
orientation of the sensor with respect to the forward direc-
tion), and a global partition of the motion plane induced
by shortest paths, such that a word in the optimal lan-
guage is univocally associated to a region and completely
describes the constrained shortest path from any starting
point in that region to the goal point.
2
Problem Definition
Consider a vehicle moving on a plane where a right-
handed reference frame ⟨W⟩is defined with origin in OW
and axes Xw,Zw. The configuration of the vehicle is de-
scribed by ξ(t) = (x(t),z(t),θ(t)), where (x(t),z(t)) is
the position in ⟨W⟩of a reference point in the vehicle,
and θ(t) is the vehicle heading with respect to the Xw axis
(see fig. 1). We assume that the dynamics of the vehicle
are negligible, and that the forward and angular veloci-
ties, ν(t) and ω(t) respectively, are the control inputs to
the kinematic model. Choosing polar coordinates for the
vehicle η = [ρ ψ β]T (see fig. 1), the kinematic model of
the unicycle-like robot is


˙ρ
˙ψ
˙β

=


−cosβ
0
sinβ
ρ
0
sinβ
ρ
−1


ν
ω

.
(1)
We consider vehicles with bounded velocities which can
turn on the spot. In other words, we assume
(ν,ω) ∈U,
(2)
with U a compact and convex subset of IR2, containing
the origin in its interior.
The vehicle is equipped with a rigidly fixed sensor sys-
tem with a reference frame ⟨C⟩= {Oc,Xc,Yc,Zc} such
that the center Oc corresponds to the robot’s center
[x(t),z(t)]T and the forward sensor axis Zc forms an angle
Γ w.r.t the robot’s forward direction. Moreover, let δ be
Figure 1: Autonomous vehicle and systems coordinates.
The vehicle’s task is to reach P while keeping OW within
a limited sensor range modelled as a planar cone (high-
lighted in color).
the characteristic angle of the cone characterizing the lim-
ited Sensor Range (SR) and let us consider the most inter-
esting problem in which δ ≤π/2. Without loss of gener-
ality, we will consider 0 ≤Γ ≤π
2 , so that, when Γ = 0 the
Zc axis is aligned with the robot’s forward direction (i.e.,
the particular case solved in [13]), whereas, when Γ = π
2 ,
is aligned with the axle direction. Consider φ1 = Γ −δ
2
and φ2 = Γ+ δ
2 the angles between the robot’s forward di-
rection and the right or left sensor’s border w.r.t. Zc axis,
respectively. The restriction on 0 ≤Γ = φ1+φ2
2
≤π
2 will be
removed at the end of this paper, and an easy procedure to
obtain the subdivision for any value of Γ will be given.
Without loss of generality, we consider the position of
the robot target point P to lay on the XW axis, with coordi-
nates (ρ, ψ) = (ρP, 0). We also assume that the feature to
be kept within the SR is placed on the axis through the ori-
gin OW and perpendicular to the plane of motion. We con-
sider a planar SR with characteristic angle δ = |φ2 −φ1|,
which generates the constraints
β −φ1 ≥0,
(3)
β −φ2 ≤0.
(4)
Note that we place no restrictions on the vertical di-
mension of the sensor. Therefore, the height of the feature
on the motion plane, which corresponds to its Yc coordi-
nate in the sensor frame ⟨C⟩, is irrelevant to our problem.
Hence, for our purposes, it is necessary to know only the
projection of the feature on the motion plane, i.e., OW.
The goal of this paper is to determine, for any point Q ∈
IR2 in the robot space, the shortest path from Q to P such
that the feature is maintained in the SR. In other words,
we want to minimize the length of the path covered by the
center of the vehicle under the feasibility constraints (1),
(2), (3), and (4).
From the theory of optimal control with state and con-
trol constraints (see [3]) it is possible to show that, when
constraints (3) and (4) are not active, extremals curves,
i.e., curves that satisfy necessary conditions for optimal-
ity, are straight lines (denoted by symbol S) and rotation
on the spot (denoted by symbol ∗). On the other hand,
when constraints (3) and (4) are active, the correspond-
ing extremal maneuvers are two logarithmic spirals with
characteristic angles φ1 and φ2 denoted by T1 and T2, re-
spectively (see [13] for details).
Logarithmic spiral T with characteristic angle φ > 0
(φ < 0) rotates counterclockwise (clockwise) around the
feature. We refer to counterclockwise and clockwise spi-
rals as Left and Right, and by symbols T L and T R, respec-
tively. The adjectives “left” and “right” indicate the half–
plane where the spiral starts for an on–board observer
aiming at the feature.
Notice that, for φ2 = π/2 the left sensor border is
aligned with the axle direction and the spiral T2 becomes a
circle centered in OW (denoted by C), whereas for φ1 = 0
the right sensor border is aligned with the direction of mo-
tion and T1 becomes an half line through OW (denoted by
H).
Extremal arcs can be executed by the vehicle in either
forward or backward direction: we will hence use super-
scripts + and −to make this explicit (e.g., S−stands for
a straight line executed backward).
We will build extremal paths consisting of se-
quences of symbols, or words, in the alphabet A =
{∗, S+, S−, E+
1 , E−
1 , E+
2 , E−
2 }, where the actual meaning
of symbols depends on angles Γ and δ as in fig. 2. Rota-
tions on the spot (∗) have zero length, but may be used to
properly connect other maneuvers.
Let LΓ be the set of possible words generated by the
aforementioned symbols in A for each value of Γ. The
rest of the paper is dedicated to showing that, due to the
physical and geometrical constraints of the considered
problem, a sufficient optimal finite language LO ⊂LΓ
(a) Frontal: 0 ≤Γ < δ
2 , E1 =
T L
1 , E2 = T R
2 .
(b) Borderline Frontal: Γ =
δ
2 , E1 = H, E2 = T R
2 .
(c) Side: δ
2 < Γ < π−δ
2 , E1 =
T R
1 , E2 = T R
2 .
(d) Borderline Side:
Γ =
π−δ
2 , E1 = T R
1 , E2 = C.
(e) Lateral:
π−δ
2
< Γ < π
2 ,
E1 = T R
1 , E2 = T L
2 .
(f) Symmetric Lateral: Γ =
π
2 , E1 = T R
1 , E2 = T L
2 .
Figure 2: Sensor configuration depending on angles Γ and
δ.
can be built such that, for any initial condition, it contains
a word describing a path to the goal which is no longer
than any other feasible path. Correspondingly, a partition
of the plane in a finite number of regions is described, for
which the shortest path is one of the words in LO.
3
Shortest path synthesis
In this section, we introduce the basic tools that will allow
us to study the optimal synthesis of the whole state space
of the robot, beginning from points on a particular sub–
set of IR2 such that the optimal paths are in a sufficient
optimal finite language.
Definition 1. Given the target point P = (ρP, 0) in polar
coordinates, and Q ∈IR2\OW, Q = (ρQ,ψQ) with ρQ ̸= 0,
let fQ : IR2 →IR2 denotes the map
fQ (ρG,ψG) =





ρGρP
ρQ
,ψG −ψQ

for ρG ̸= 0
(0,0)
otherwise.
(5)
The map fQ is the combination of a clockwise rotation
by angle ψG −ψQ, and a scaling by a factor ρP/ρQ that
maps Q in P.
Remark 1. The alphabet A is invariant w.r.t.
rota-
tion and scaling.
However, it is not invariant w.r.t.
axial symmetry, as it happened in the particular case
(i.e., the Frontal case with Γ = 0) considered in [13],
where the map fQ was defined as a combination of ro-
tation, scaling and axial symmetry. For example, loga-
rithmic spirals are self-similar and self-congruent (under
scaling and rotation they are mapped into themselves).
On the other hand, left (right) spirals are mapped into
right (left) spirals through an axial symmetry and alpha-
bet invariancy can be lost.
Indeed, for example, con-
sidering the Side case alphabet (see fig. 2(c)) ASide =
{∗, S+, S−, T R+
1
, T R−
1
, T R+
2
, T R−
2
}, and applying an axial
symmetry we have T R
1 →T L
1 /∈ASide, the same occurs for
the Frontal alphabet with Γ > 0.
Let γ be a path parameterized by t ∈[0,1] in the plane
of motion γ(t) = (ρ(t), ψ(t)). Denote with PQ the set of
all feasible extremal paths from γ(0) = Q to γ(1) = P.
Definition 2. Given the target point P = (ρP, 0) and Q =
(ρQ,ψQ) with ρQ ̸= 0, let the path transform function FQ
be defined as
FQ : PQ →PfQ(P)
γ(t) 7→fQ(γ(1 −t)), ∀t ∈I.
(6)
Notice that ˜γ(t) = FQ (γ(1 −t)) corresponds to γ(t)
transformed by fQ and followed in opposite direction. In-
deed, ˜γ is a path from ˜γ(0) = fQ(P) =
 ρ2
P
ρQ , −ψQ

to
˜γ(1) = fQ(Q) ≡P.
The F map has some properties that make it very use-
ful to the study of our problem in a way which is to some
extent similar to what described (for a different F map)
in [13].
In particular, the locus of points Q such that
fQ(P) = Q, is the circle with center in OW and radius ρP.
We will denote this circle by C(P) and the closed disk
within C(P) by D(P).
C(P) has an important role in the proposed approach
since properties of FQ will allow us to solve the synthesis
problem from points on C(P), and hence to extend the
synthesis to D(P) and to the whole motion plane. Indeed,
∀Q ∈C(P) and ∀γ ∈PQ, FQ(γ) ∈PfQ(P) with fQ(P) ∈
C(P), i.e., a path from a point on C(P) to P is mapped in
a path from C(P) to P.
Furthermore, FQ transforms an extremal in A in itself
but followed in opposite direction. Hence, FQ maps ex-
tremal paths in LΓ in extremal paths in LΓ. For example,
let w = S−∗H−∗S+ ∗T R+
2
be the word that character-
ize a path from Q to P, the transformed path is of type
z = T R−
2
∗S−∗H+ ∗S+. With a slight abuse of notation,
we will write z = FQ(w).
Proposition 1. Given Q ∈IR2 and a path γ ∈PQ of
length l, the length of the transformed path ˜γ = FQ(γ) is
˜l = ρP
ρQ l.
The proof is easily obtained from a similar result
in [13].
Based on the properties of FQ, optimal paths from
points on C(P) completely evolve inside C(P). To prove
this statement we first report the following result,
Theorem 1. Given two points A = (ρA, ψA) and B =
(ρB, ψB), with ψA > ψB and ρ = ρA = ρB, and an ex-
tremal path γ from A to B such that for each point G of γ,
ρG > ρ, there exists an extremal path ˜γ from A to B such
that for each point ˜G of ˜γ, ρ ˜G < ρ and ℓ( ˜γ) < ℓ(γ) (see
fig. 3).
The proof of this theorem can be found in section .1 in
the Appendix.
An important but straightforward consequence of the
theorem is the following
Corollary 1. For any path in PQ with Q ∈C(P) there ex-
ists a shorter or equal-length path in PQ that completely
evolves in D(P).
Figure 4: Forward and backward straight path regions from G for δ
2 < Γ ≤π−δ
2 .
Figure 3: An example for theorem 1: path γ = γ1γ2 (γ1
followed by γ2) of type T R−
2
S−∗T R+
1
from A to B is short-
ened by a path ˜γ = ˜γ1 ˜γ2 of type T R+
1
∗T R+
1
S−by applying
path transformation FZ to path γ.
4
Optimal paths for points on C(P)
Our study of the optimal synthesis begins in this section
addressing optimal paths from points on C(P). We first
need to establish an existence result of optimal paths.
Proposition 2. For any Q ∈C(P) there exists a feasible
shortest path to P.
Proof. Because of state constraints (3), and (4), and the
restriction of optimal paths in D(P) (Corollary 1) the state
set is compact.
Furthermore, it is possible to give an
upper-bound on the optimal path length for all Γ ∈[0, π
2 ].
Indeed, given a point Q at distance ρ from OW the optimal
path to P is shorter or equal to the following paths based
on the value of Γ and δ:
• Frontal (0 ≤Γ ≤δ
2 ): S+ ∗S−or H+ ∗H−of length
ρ + ρP;
• Side ( δ
2 < Γ <
π−δ
2 ):
T R+
1Q ∗T R−
2P ,
of length

ρ−ρN
cosφ1 + ρP−ρN
cosφ2

, where N is the intersection point
between spirals T R
1Q and T R
2P through Q and P respec-
tively;
• Borderline Side (Γ = π−δ
2 :
T R+
1
∗C−
P ) of length

ρ−ρP
cosφ1 + (ψN −ψP)ρP

, where N is the intersection
point between spirals T R
1 and CP;
• Lateral ( π−δ
2
< Γ ≤π
2 ):
T L−
2Q ∗T R−
1P , of length

ρ−ρN
cosφ2 + ρP−ρN
cosφ1

, where N is the intersection point
between spirals T L
2Q and T R
1P.
The system is also controllable because there always ex-
ists an intersection point between two spirals (even if de-
generated in half–lines or circumferences) with different
characteristic angle even if both clockwise or counter-
clockwise around the feature. Hence, Filippov existence
theorem for Lagrange problems can be invoked [5].
In the following we provide a set of propositions that
completely describe a sufficient optimal finite language
for all values of Γ ∈[0, π
2 ].
Figure 5: Forward and backward straight path Regions
from G for 0 ≤Γ ≤δ
2 .
Definition 3. For any starting point G = (ρG, ψG), let
SF(G) (SB(G)) be the set of all points reachable from G
with a forward (backward) straight line without violating
the SR constraints.
We denote with ∂SF1(G) and ∂SF2(G) (∂SB1(G) and
∂SB2(G)) the borders of SF(G) (SB(G)). Also, let Ci(G)
denote the circular arcs from G to OW such that, ∀V ∈
Ci(G), \
GVOW = π −|φi|.
Remark 2. Based on simple geometric considerations,
for any starting point G = (ρG, ψG), for 0 ≤Γ ≤δ
2
(Frontal Case), SF(G) is the region between ∂SF2(G) =
C2(G) and ∂SF1(G) = C1(G). Let r1(G) (r2(G)) denote
the half–line from G forming an angle ψG −φ1 (ψG −φ2)
with the XW axis (cf. fig. 5). SB(G) is the cone delimited
by ∂SB1(G) = r1(G) and ∂SB2(G) = r2(G), outside cir-
cle with center in OW and radius ρG. Notice that, SF(G)
lays completely in the circle with center in OW and ra-
dius ρG. Moreover, in the particular case in which Γ = δ
2
(Borderline Frontal Case), E1 = H and ∂SF1(G) degener-
ates in the chord (GOW) between G and OW, aligned with
r1(G).
As a consequence of Remark 2, both SF(G) and SB(G)
are tangent in G to T L
1 or H and T R
2 .
Remark 3. For any starting point G = (ρG, ψG), and for
δ
2 < Γ ≤π−δ
2
(Side case), let SGF be the chord between G
and GF = (ρG
sinφ1
sinφ2 , ψG + (φ2 −φ1)) ∈C2(G), i.e. such
that
\
OWGGF = φ1 (cf.
fig. 4).
Naming with CGF the
Figure 6: Forward and backward straight path Regions
from G for π−δ
2
≤Γ ≤π
2 .
arc between G and GF, SF(G) is the region between arc
∂SF2(G) = CGF and chord ∂SF1(G) = SGF . Consider the
rotation and scale that maps GF in G and G in GB: we
have ∂SB1(G) = ∂SF1(GB), i.e. ∂SB2(G) = ∂SF2(GB).
Moreover, for all point V on the circular arc CGB from GB
to G, angle \
GBVOW = π −|φ2|, and angle \
OWGBG = φ1.
Notice that, in this case, SF(G) lays completely in the cir-
cle with center in OW and radius ρG. Notice that, in the
particular case in which Γ = π−δ
2
(Borderline Side Case),
E2 = C and ∂SF2(G) is an arc from G to GF on a semi-
circle with diameter ρG.
As a consequence of Remark 3, SF(G) is tangent in G
to T R
1 and T R
2 or C. Moreover, SF(G) is tangent in GF to
T R
1 and T R
2 or C, see fig. 4.
Fig. 6 shows the SF(G) and SB(G) regions described
in 3 for the Lateral case. Notice that, in this case, SF(G)
does not lay completely in the circle with center in OW
and radius ρG.
Remark 4. Optimal forward (backward) straight arcs
from any G ends on CGF (CGB) (see also [13] for details).
Based on all the above properties, we are now able to
obtain a sufficient family of optimal paths by excluding
particular sequences of extremals.
Theorem 2. Any path consisting in a sequence of a back-
ward extremal arc followed by a forward extremal arc is
not optimal.
The proof of this theorem, whose details can be found
in section .2 of the Appendix, is based on the fact that
for continuity of paths, for any sequence of a backward
extremal followed by a forward one, there exist points A
and B that verify hypothesis of Theorem 1.
Theorem 3. Any path consisting in a sequence of an ex-
tremal arc Ei and an extremal arc E j followed in the same
direction is not optimal for any i, j ∈{1, 2} with i ̸= j.
Notice that the feasible sequences consisting of two
extremals that we still need to discuss are those starting
or ending with S followed in any direction (E+E−and
E−E+ are obviously not optimal).
Proposition 3. From any starting point A, any path γ of
type S+ ∗E+
2 and S+ ∗E−
1 to B can be shortened by a path
of type S+E+
2 or E+
2 ∗E−
1 . Moreover, any path γ of type
S+ ∗E+
1 or S+ ∗E−
2 can be shortened by a path of type
E+
1 S+ or E+
1 ∗E−
2 .
Proposition 3 implies that paths of type S−∗E−
1 and
S−∗E−
2 are not optimal. Indeed, they can be shortened
by S−E−
1 and E−
2 S−, respectively (see fig. 7 for the Side
case).
By using all previous results, a sufficient family of opti-
mal paths is obtained in the following important theorem.
Theorem 4. For δ
2 < Γ ≤π
2 , i.e. Side and Lateral cases,
and for any Q ∈D(P) to P there exists a shortest path of
type E+
1 ∗E−
2 S−E−
1 or of type E+
1 S+E+
2 ∗E−
1 . For 0 ≤Γ ≤
δ
2 , i.e. Frontal case, and for any Q ∈D(P) to P there exists
a shortest path of type S+E+
1 ∗E−
2 S−or of type S+E+
2 ∗
E−
1 S−.
Proof. According to all propositions above several con-
catenations of extremal have been proved to be non op-
timal. Considering extremals as node and, possibly opti-
mal, concatenations of extremal as edges of a graph, the
sufficient optimal languages LO from Q in D(P), for dif-
ferent values of Γ and δ, are described in fig. 8. Indeed, it
is straightforward to observe that the number of switches
a)
b)
Figure 8: Feasible extremals and sequence of extremals
from point in D(P): a) in Side and Lateral cases ( δ
2 < Γ ≤
π
2 ). b) in Frontal case (0 ≤Γ ≤δ
2).
between extremals is finite and less or equal to 3, for any
value of Γ and δ. Hence, the thesis.
We now study the length of extremal paths from C(P)
to P in the sufficient family above.
Without loss of generality, it is sufficient to study the
length of extremal paths of type E+
1 ∗E−
2 S−E−
1 only from
points Q on the semicircle of C(P) in the upper-half plane
(denoted by CS). Indeed, up to a rotation, optimal paths
of type E+
1 S+E+
2 ∗E−
1 from the rest of C(P) can be easily
obtained. Referring to fig. 9, let the switching points of
the optimal path be denoted by N, M1 and M2 or ¯N, ¯M1
and ¯M2 ≡P, respectively, depending on the angular values
αM1 or α ¯M1. Moreover, in order to do the analysis, it is
useful to parameterize the family by the angular value α ¯M1
of the switching point ¯M1 along the arc C2(P) between P
and Z or the angular value αM1 of the switching point M1
along the extremal E1 between PF and OW.
Theorem 5. For any point Q ∈CS, the length of a path
γ ∈PQ of type E+
1 ∗E−
2 S−E−
1 is:
• for 0 ≤α ¯M1 ≤φ2 −φ1, i.e. from P to Z (notice that
the last arc has zero length):
L = ρP
cosαM1
cosφ2
+
1
cosφ1
+
−cosφ1 +cosφ2
cosφ1 cosφ2
e(ψQ−αM1)
t1t2
t2−t1
sin(φ2 −αM1)
sinφ2
−
t1
t2−t1
)
,
(7)
(a) From A to B, path S+ ∗T R+
2
through z and v can
be shortened by S+T R+
2
through v, where S arc is
tangent to T R
2 .
(b) From A to B′, path S+∗T R−
1
through z can be short-
ened by a path of type S+T R+
2
through v, whereas from
A to B′′ by a path of type T R+
2
∗T R−
1
through g.
(c) From A to B, path S+ ∗T R+
1
through z can
be shortened by a path of type T R+
1
S+ through v,
where S arc is tangent to T R
1 .
(d) From A to B′, path S+ ∗T R−
2
through z can be shortened
by a path of type T R+
1
S+ through v, whereas from A to B′′
by a path of type T R+
1
∗T R−
2
through g.
Figure 7: Examples of paths shortened in proposition 3 for the Side case.
• for αM1 ≥φ2 −φ1, i.e. from Z to OW :
L = ρP

2
cosφ1
+e−αM1t1
cos(φ2 −φ1)
cosφ2
−
1
cosφ1
+
−cosφ1 +cosφ2
cosφ1 cosφ2
e[ψQ−(φ2−φ1)] t1t2
t2−t1
sinφ1
sinφ2
−
t1
t2−t1
#)
,
(8)
with t1 = 1/tanφ1 and t2 = 1/tanφ2.
The analytical expression for the length L is based on
a direct computation. Having the path’s length as a func-
tion of two parameters αM1 or α ¯M1 and ψQ, we are now
in a position to minimize the length within the sufficient
family.
Theorem 6. Given a point Q ∈CS,
• for 0 ≤ψQ ≤ψR1 := sin(φ2−φ1)
cosφ1 cosφ2 ln

cosφ1+cosφ2
sinφ2 sin(φ2−φ1)

,
optimal path is of type E+
1 ∗E−
2 ;
• for ψR1 ≤ψQ ≤ψR2 with ψR2 := (φ2 −φ1) + ψR1 +
tanφ2 ln

sinφ1
sinφ2

, optimal path is of type E+
1 ∗E−
2 S−;
• for ψR2 ≤ψQ ≤π the optimal path is E+
1 ∗E−
1
through OW.
Moreover, for ψQ = ψR2, any optimal path of type E+
1 ∗
E−
2 S−E−
1 turns out to have the same length ℓof optimal
path E+
1 ∗E−
1 . Hence, for ψQ = ψR2 also E+
1 ∗E−
2 S−E−
1
is optimal.
Figure 9: Path of type E+
1 ∗E−
2 S−E−
1 or the degenerate
case of type E+
1 ∗E−
2 S−from Q ∈CS.
Previous results have been obtained computing first and
second derivatives of L and nonlinear minimization tech-
niques.
We are now interested in determining the locus of
switching points between extremals in optimal paths.
Proposition 4. For Q ∈CS with 0 < ψQ ≤ψR1,
the switching locus is the arc of E2
between P
M = (ρP
sinφ2 sin(φ2−φ1)
cosφ1+cosφ2 , ψM) (included), where ψM =
tanφ2 ln

ρP
ρM

.
Proof. From Theorem 6, the optimal path from Q ∈CS
to P is of type E+
1 ∗E−
2 . For ψQ = ψR1 the intersection
between E+
1 and E−
2 is M.
Proposition 5. For Q ∈CS with ψR1 < ψQ < ψR2, the
loci of switching points M2 and N are the ∂SF2(P) and
∂SF2(M).
Proof. For Q ∈CS with ψR1 < ψQ < ψR2, considering the
values of αM2 obtained in the computations of Theorem 6
we obtain M2 ∈∂SF2(P). Furthermore, substituting those
values in the equation of the intersection point N between
E1 through Q and E2 through M2 we obtain N ∈∂SF2(M).
Finally, for Q ∈CS with ψR2 ≤ψ < π, the switching
locus reduces to the origin OW since two extremal Ei in-
tersect only in the origin for i = 1,2.
Region
Optimal Path
I
S−
II
E+
1 ∗E−
2
II′
E+
2 ∗E−
1
III
E+
1 ∗E−
1
IV
E−
2 S−E−
1
V
E+
1 ∗E−
2 S−
V′
S+E+
2 ∗E−
1
VI
S−E−
1
Figure 10: Optimal synthesis inside D(P).
5
Shortest paths from any point in
the motion plane
The synthesis on C(P) induce a partition in regions of
D(P).
Indeed, for any Q ∈D(P), there exists a point
V ∈C(P) such that the optimal path γ from V to P goes
through Q. The Bellmann’s optimality principle ensure
the optimality of the sub–path from Q to P. Based on this
construction the partition of C(P) is reported in fig. 10.
For points outside C(P), function FQ has been defined
in 6 in order to transform paths starting from Q inside
C(P) in paths starting from fQ(P) =
 ρ2
P
ρQ ,−ψQ

outside
C(P).
From other properties of FQ, such as Proposition 1, we
have also that an optimal path is mapped into an optimal
path. Hence, the optimal synthesis from points outside
C(P) can be easily obtained mapping through map FQ all
borders of regions inside C(P).
Proposition 6. Given a border B and Q ∈B map FQ
transforms:
1. B = C(P) into itself;
2. B = ∂SF2(Q) in ∂SB1(fQ(P))
3. B = ∂SF1(Q) in ∂SB2(fQ(P))
Figure 11: Partition of the motion plane for δ
2 < Γ < π−δ
2
.
4. B = Ei in arcs of the same type (i = 1,2)
Proof. The proof of this proposition can be found in [13].
Based on Proposition 6, the optimal synthesis of the
entire motion plane is reported in fig. 11.
6
Optimal synthesis for generic Γ
We first obtain the synthesis of the Borderline Frontal
case, i.e. Γ = δ
2 , reported in fig. 12 from the one obtained
in the previous section.
Notice that, E1 = T R
1 of the Side case degenerates in a
straight line H through OW for Γ = δ
2 . Indeed, referring
to fig. 10, points MF and PF degenerate on OW. As a con-
sequence, Region IV, IV and VI′ while coordinates ΨR1
and ΨR2 of points R1 and R2 can be obtained from values
in 6 replacing φ1 = 0.
In the Frontal case,
E1 = H
becomes a spiral
T L
1 , straight lines from P and R2 split in straight
line and a spiral arc generating the partition re-
ported in fig. 13.
In this case, φ1 < 0 and points
R1 and R2 do not lay on C(P) but on a circle
through P with center (0, −ρP
sin2 φ1−sin2 φ2
2sinξ sinφ1 sinφ2 ), where ξ =
Figure 12: Partition of the motion plane for Γ = δ/2 (i.e.
a SR border is aligned with the robot motion direction,
Borderline Frontal).
Figure 13: Partition of the motion plane for 0 ≤Γ < δ
2 ,
i.e. Frontal case.
t1+t2
t1t2 ln

cosφ1+cosφ2
sin(φ2−φ1)

+ 1
t1 ln(−sinφ1)−1
t2 ln(sinφ2). No-
tice that for φ2 = −φ1, this circle coincide with C(P) and
the synthesis proposed in [13] is obtained.
Referring again to fig. 10, in the Borderline Side case
(Γ = π−δ
2 , i.e. the SR border is aligned with the axle
direction and φ2 = π
2 ), E2 = T R
2 degenerates in E2 = C.
Points R1 ≡M and R2 lays on C(P) with ΨR1 = 1+sinφ1
cosφ1
and ΨR2 = π
2 −φ1 +ΨR1 +tanφ1 ln(sinφ1). The obtained
Figure 14: Partition of the motion plane for Γ = π−δ
2
(i.e.
a SR border is aligned with the axle direction).
Figure 15: Partition of the motion plane for π−δ
2
≤Γ < π
2
(i.e. axle direction is included inside the SR).
synthesis is reported in fig. 14. For the Lateral case E2 =C
becomes E2 = T L
2 and the synthesis of the Lateral case, re-
ported in fig. 15, can be obtained from the one in fig. 14.
The subdivision of the motion plane in case of π
2 < Γ ≤
π can be easy obtained by using that one for 0 ≤Γ ≤π
2
considering optimal path followed in reverse order, i.e.
forward arc in backward arc and viceversa. Finally, a
symmetry w.r.t. XW axis of each subdivision of the motion
plane for each Γ ∈[0, π] allows to obtain the correspond-
ing subdivision for Γ ∈[−π, 0].
7
Conclusions and future work
A complete characterization of shortest paths for unicy-
cle nonholonomic mobile robots equipped with a limited
range side sensor systems has been proposed. A finite suf-
ficient family of optimal paths has been determined based
on geometrical properties of the considered problem. Fi-
nally, a complete shortest path synthesis to reach a point
keeping a feature in sight has been provided. A possible
extension of this work is to consider a bounded 3D SR
pointing to any direction with respect to the direction of
motion. A more challenging extension would be consid-
ering a different minimization problem such as the mini-
mum time.
References
[1] D. Balkcom and M. Mason. Time-optimal trajec-
tories for an omnidirectional vehicle. The Interna-
tional Journal of Robotics Research, 25(10):985–
999, 2006.
[2] S. Bhattacharya, R. Murrieta-Cid, and S. Hutchin-
son.
Optimal paths for landmark–based naviga-
tion by differential–drive vehicles with field–of–
view constraints. IEEE Transactions on Robotics,
23(1):47–59, February 2007.
[3] A.E. Bryson and Y.C. Ho. Applied optimal control.
Wiley New York, 1975.
[4] X.N. Bui, P. Sou`eres, J-D. Boissonnat, and J-P. Lau-
mond.
Shortest path synthesis for Dubins non–
holonomic robots. In IEEE International Confer-
ence on Robotics and Automation, pages 2–7, 1994.
[5] L. Cesari.
Optimization-theory and applica-
tions: problems with ordinary differential equations.
Springer-Verlag, New York, 1983.
[6] H. Chitsaz, S. M. LaValle, D. J. Balkcom, and M.T.
Mason.
Minimum wheel-rotation for differential-
drive mobile robots. The International Journal of
Robotics Research, pages 66–80, 2009.
[7] M. Chyba, N.E. Leonard, and E.D. Sontag. Opti-
mality for underwater vehicle’s. In 40th IEEE Con-
ference on Decision and Control, volume 5, pages
4204–4209, 2001.
[8] N. Dias, C. Almeida, H. Ferreira, J. Almeida,
A. Martins, A. Dias, and E. Silva. Manoeuvre based
mission control system for autonomos surface vehi-
cle. in Proceeedings of OCEANS ’09, May 2009.
[9] L. E. Dubins. On curves of minimal length with a
constraint on average curvature, and with prescribed
initial and terminal positions and tangents. Ameri-
can Journal of Mathematics, pages 457–516, 1957.
[10] F. Langner, C. Knauer, W. Jans, and A. Ebert. Side
scan sonar image resolution and automatic object
detection, classification and identification. in Pro-
ceeedings of OCEANS ’09, May 2009.
[11] J. A. Reeds and L. A. Shepp. Optimal paths for a
car that goes both forwards and backwards. Pacific
Journal of Mathematics, pages 367–393, 1990.
[12] P. Salaris, D. Fontanelli, L. Pallottino, and A. Bicchi.
Optimal paths for mobile robots with limited field of
view camera. In 48th IEEE Conference on Decision
and Control, pages 8434–8439, Dec 2009.
[13] P. Salaris, D. Fontanelli, L. Pallottino, and A. Bic-
chi. Shortest paths for a robot with nonholonomic
and field-of-view constraints. IEEE Transactions on
Robotics, 26(2):269–281, April 2010.
[14] H. Sou`eres and J. P. Laumond. Shortest paths syn-
thesis for a car-like robot. IEEE Transaction on Au-
tomatic Control, pages 672–688, 1996.
[15] H. Sussmann and G. Tang. Shortest paths for the
reeds-shepp car: A worked out example of the use
of geometric techniques in nonlinear optimal con-
trol`o. Technical report, Department of Mathematics,
Rutgers University, 1991.
[16] H. Wang, Y. Chan, and P. Sou`eres.
A geometric
algorithm to compute time-optimal trajectories for
a bidirectional steered robot. IEEE Transaction on
Robotics, pages –, 2009.
.1
Proof of Theorem 1
Theorem 1. Given two points A = (ρA, ψA) and B =
(ρB, ψB), with ψA > ψB and ρ = ρA = ρB, and an ex-
tremal path γ from A to B such that for each point G of γ,
ρG > ρ, there exists an extremal path ˜γ from A to B such
that for each point ˜G of ˜γ, ρ ˜G < ρ and ℓ( ˜γ) < ℓ(γ).
Proof. Consider a point Z = (ρZ, ψZ) such that ρZ =
maxG∈γ ρG > ρ. Let γ1 and γ2 the sub–paths of γ from
Z to B and from Z to A.
The sub–path γ1, is rotated and scaled (contracted of
factor
ρ
ρZ < 1) such that Z is transformed in A obtaining
a path ˜γ1 from A to ˜Z = ( ρ2
ρZ , ψA + ψB −ψZ). Similarly,
γ2, can be rotated and scaled with the same scale factor but
different rotation angle w.r.t. γ1 such that Z is transformed
in B, see fig. 3. After geometrical considerations, it is easy
to notice that the obtained path ˜γ2 starts in B and ends in
˜Z.
The obtained paths are a contraction of γ1 and γ2 re-
spectively and hence shorter. Moreover, any point G of
γ1 or γ2 has ρG > ρ hence is scaled in ˜G of ˜γ1 or ˜γ2 with
ρ ˜G = ρρG
ρZ < ρ.
Concluding, we have obtained a shorter path from A to
B that evolves completely in the disk of radius ρ.
.2
Proof of Theorem 2
Theorem 2. Any path consisting in a sequence of a back-
ward extremal arc followed by a forward extremal arc is
not optimal.
Proof. Observe that the distance from OW is strictly in-
creasing along backward extremal arcs (i.e. S−, E−
1 , E−
2
with E2 ̸= C) and strictly decreasing along forward ex-
tremal arcs (i.e. S+, E+
1 , E+
2 with E2 ̸= C). For continuity
of paths, for any sequence of a backward extremal fol-
lowed by a forward one, there exist points A and B that
verify hypothesis of Theorem 1, hence it is not optimal.
Any sequence consisting in an extremal S (or E1) of
length ℓand an extremal E2 = C (in any order and direc-
tion) is inscribed in two circumferences centered in OW.
Hence, the shortest sequence is the one with E2 = C along
the circle of smaller radius necessarily preceded by a for-
ward S (or E1) of same length ℓ.
Concluding, in an optimal path a forward arc cannot
follow a backward arc.