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   O W  and axes   X w ,   Z w . 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   X w   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 IR 2 , containing the origin in its interior. The vehicle is equipped with a rigidly fixed sensor sys- tem with a reference frame   〈 C 〉   =   { O c ,   X c , Y c ,   Z c }   such that the center   O c   corresponds to the robot’s center  [ x ( t ) ,   z ( t )] T   and the forward sensor axis   Z c   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   O W   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  Z c   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.   Z c   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   X W   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   O W   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   Y c   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.,   O W   . The goal of this paper is to determine, for any point   Q   ∈  IR 2   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   T 1   and   T 2 , 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   T 2   becomes a circle centered in   O W   (denoted by   C ), whereas for  φ 1   =   0 the right sensor border is aligned with the direction of mo- tion and   T 1   becomes an half line through   O W   (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   L O   ⊂   L Γ  (a) Frontal: 0   ≤   Γ   <  δ  2   ,   E 1   =  T   L  1   ,   E 2   =   T   R  2   .  (b) Borderline Frontal:   Γ   =  δ  2   ,   E 1   =   H ,   E 2   =   T   R  2   .  (c) Side:  δ  2   <   Γ   <  π −  δ  2   ,   E 1   =  T   R  1   ,   E 2   =   T   R  2   .  (d)   Borderline   Side:   Γ   =  π −  δ  2   ,   E 1   =   T   R  1   ,   E 2   =   C .  (e) Lateral:  π −  δ  2   <   Γ   <  π  2   ,  E 1   =   T   R  1   ,   E 2   =   T   L  2   .  (f) Symmetric Lateral:   Γ   =  π  2   ,   E 1   =   T   R  1   ,   E 2   =   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   L O .  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 IR 2   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   ∈   IR 2   \   O W   , Q   = (  ρ Q ,  ψ Q )   with  ρ Q   6   =   0 , let f Q   : IR 2   →   IR 2   denotes the map f Q   (  ρ G ,  ψ G ) =       (  ρ G  ρ P  ρ Q  ,  ψ G   −  ψ Q  )  for  ρ G   6   =   0  ( 0 ,   0 )   otherwise.  (5) The map   f Q   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 f Q   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))   A Side   =  {∗ ,   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   / ∈   A Side , 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   P Q   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   6   =   0 , let the   path transform   function F Q  be defined as F Q   :   P Q   →   P   f Q ( P )  γ ( t )   7   →   f Q (  γ ( 1   −   t )) ,   ∀ t   ∈   I .   (6) Notice that    ̃  γ ( t ) =   F Q   (  γ ( 1   −   t ))   corresponds to  γ ( t )  transformed by   f Q   and followed in opposite direction. In- deed,  ̃  γ   is a path from  ̃  γ ( 0 ) =   f Q ( P ) =  (  ρ 2  P  ρ Q   ,   −  ψ Q  )  to  ̃  γ ( 1 ) =   f Q ( 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  f Q ( P ) =   Q , is the circle with center in   O W   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   F Q   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   ∀  γ   ∈   P Q ,   F Q (  γ )   ∈   P   f Q ( P )   with   f Q ( 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,   F Q   transforms an extremal in   A   in itself but followed in opposite direction. Hence,   F Q   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   =   F Q ( w ) .  Proposition 1.   Given Q   ∈   IR 2   and a path  γ   ∈   P Q   of length l, the length of the transformed path    ̃  γ   =   F Q (  γ )   is   ̃ l   =  ρ P  ρ Q   l.  The proof is easily obtained from a similar result in [13]. Based on the properties of   F Q , 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   P Q   with Q   ∈   C ( P )   there ex- ists a shorter or equal-length path in   P Q   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   F Z   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   O W   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 +  1 Q   ∗   T   R −  2 P   ,   of   length  (  ρ −  ρ N  cos  φ 1   +  ρ P −  ρ N  cos  φ 2  )  , where   N   is the intersection point between spirals   T   R  1 Q   and   T   R  2 P   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   C P ;  •   Lateral (  π −  δ  2   <   Γ   ≤  π  2   ):   T   L −  2 Q   ∗   T   R −  1 P   ,   of length  (  ρ −  ρ N  cos  φ 2   +  ρ P −  ρ N  cos  φ 1  )  , where   N   is the intersection point between spirals   T   L  2 Q   and   T   R  1 P . 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  ∂   SF 1 ( G )   and  ∂   SF 2 ( G )   (  ∂   SB 1 ( G )   and ∂   SB 2 ( G ) ) the borders of   SF ( G )   ( SB ( G ) ). Also, let   C i ( G )  denote the circular arcs from   G   to   O W   such that,   ∀ V   ∈  C i ( G ) ,   ̂   GVO W   =  π   − |  φ 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  ∂   SF 2 ( G ) =  C 2 ( G )   and  ∂   SF 1 ( G ) =   C 1 ( G ) . Let r 1 ( G )   (r 2 ( G ) ) denote the half–line from G forming an angle  ψ G   −  φ 1   (  ψ G   −  φ 2 ) with the X W   axis (cf. fig. 5). SB ( G )   is the cone delimited by  ∂   SB 1 ( G ) =   r 1 ( G )   and  ∂   SB 2 ( G ) =   r 2 ( G ) , outside cir- cle with center in O W   and radius  ρ G . Notice that, SF ( G )  lays completely in the circle with center in O W   and ra- dius  ρ G . Moreover, in the particular case in which   Γ   =  δ  2  (Borderline Frontal Case), E 1   =   H and  ∂   SF 1 ( G )   degener- ates in the chord ( GO W   ) between G and O W   , aligned with r 1 ( 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 S G F   be the chord between G and G F   = (  ρ G   sin  φ 1  sin  φ 2   ,  ψ G   + (  φ 2   −  φ 1 ))   ∈   C 2 ( G ) , i.e. such that   ̂   O W   GG F   =  φ 1   (cf.   fig. 4).   Naming with C G F   the  Figure 6: Forward and backward straight path Regions from   G   for  π −  δ  2   ≤   Γ   ≤  π  2   .  arc between G and G F   , SF ( G )   is the region between arc  ∂   SF 2 ( G ) =   C G F   and chord  ∂   SF 1 ( G ) =   S G F   . Consider the rotation and scale that maps G F   in G and G in G B : we have  ∂   SB 1 ( G ) =  ∂   SF 1 ( G B ) , i.e.  ∂   SB 2 ( G ) =  ∂   SF 2 ( G B ) . Moreover, for all point V on the circular arc C G B   from G B  to G, angle   ̂   G B VO W   =  π   − |  φ 2 | , and angle   ̂   O W   G B G   =  φ 1 . Notice that, in this case, SF ( G )   lays completely in the cir- cle with center in O W   and radius  ρ G . Notice that, in the particular case in which   Γ   =  π −  δ  2   (Borderline Side Case), E 2   =   C and  ∂   SF 2 ( G )   is an arc from G to G F   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   G F   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   O W  and radius  ρ G .  Remark 4.   Optimal forward (backward) straight arcs from any G ends on C G F   (C G B   ) (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 E i   and an extremal arc E   j   followed in the same direction is not optimal for any i ,   j   ∈ { 1 ,   2 }   with i   6   =   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   L O   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 ,   M 1   and   M 2   or  ̄ N ,    ̄ M 1  and  ̄ M 2   ≡   P , respectively, depending on the angular values  α M 1   or  α    ̄ M 1   . Moreover, in order to do the analysis, it is useful to parameterize the family by the angular value  α    ̄ M 1  of the switching point    ̄ M 1   along the arc   C 2 ( P )   between   P  and   Z   or the angular value  α M 1   of the switching point   M 1  along the extremal   E 1   between   P F   and   O W   .  Theorem 5.   For any point Q   ∈   CS, the length of a path  γ   ∈   P Q   of type E +  1   ∗   E −  2   S − E −  1   is:  •   for   0   ≤  α    ̄ M 1   ≤  φ 2   −  φ 1 , i.e. from P to Z (notice that the last arc has zero length):  L   =  ρ P  {   cos  α M 1  cos  φ 2  +   1  cos  φ 1  +  −   cos  φ 1   +   cos  φ 2  cos  φ 1   cos  φ 2  e (  ψ Q −  α M 1   )   t 1 t 2  t 2 − t 1  (   sin   (  φ 2   −  α M 1   )  sin  φ 2  ) −   t 1  t 2 − t 1  }  ,  (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  α M 1   ≥  φ 2   −  φ 1 , i.e. from Z to O W   : L   =  ρ P  {   2  cos  φ 1  +   e −  α M 1   t 1  [   cos (  φ 2   −  φ 1 )  cos  φ 2  −   1  cos  φ 1  +  −   cos  φ 1   +   cos  φ 2  cos  φ 1   cos  φ 2  e [  ψ Q − (  φ 2   −  φ 1   )]   t 1 t 2  t 2 − t 1  (   sin  φ 1  sin  φ 2  ) −   t 1  t 2 − t 1  ]}  ,  (8)  with t 1   =   1 /   tan  φ 1   and t 2   =   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  α M 1   or  α    ̄ M 1   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   ≤  ψ R 1   : =   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  ψ R 1   ≤  ψ Q   ≤  ψ R 2   with  ψ R 2   : = (  φ 2   −  φ 1 ) +  ψ R 1   +  tan  φ 2   ln  (   sin  φ 1  sin  φ 2  )  , optimal path is of type E +  1   ∗   E −  2   S − ;  •   for  ψ R 2   ≤  ψ Q   ≤  π   the optimal path is E +  1   ∗   E −  1  through O W   . Moreover, for  ψ Q   =  ψ R 2   , 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   =  ψ R 2   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   ≤  ψ R 1   , the   switching   locus   is   the   arc   of   E 2   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   =  ψ R 1   the intersection between   E +  1   and   E −  2   is   M .  Proposition 5.   For Q   ∈   CS with  ψ R 1   <  ψ Q   <  ψ R 2   , the loci of switching points M 2   and N are the  ∂   SF 2 ( P )   and ∂   SF 2 ( M ) . Proof.   For   Q   ∈   CS   with  ψ R 1   <  ψ Q   <  ψ R 2   , considering the values of  α M 2   obtained in the computations of Theorem 6 we obtain   M 2   ∈  ∂   SF 2 ( P ) . Furthermore, substituting those values in the equation of the intersection point   N   between  E 1   through   Q   and   E 2   through   M 2   we obtain   N   ∈  ∂   SF 2 ( M ) .  Finally, for   Q   ∈   CS   with  ψ R 2   ≤  ψ   <  π , the switching locus reduces to the origin   O W   since two extremal   E i   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   F Q   has been defined in 6 in order to transform paths starting from   Q   inside  C ( P )   in paths starting from   f Q ( P ) =  (  ρ 2  P  ρ Q   ,   −  ψ Q  )  outside  C ( P ) . From other properties of   F Q , 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   F Q   all borders of regions inside   C ( P ) .  Proposition 6.   Given a border   B   and Q   ∈   B   map F Q  transforms: 1.   B   =   C ( P )   into itself; 2.   B   =  ∂   SF 2 ( Q )   in  ∂   SB 1 (   f Q ( P ))  3.   B   =  ∂   SF 1 ( Q )   in  ∂   SB 2 (   f Q ( P ))
Figure 11: Partition of the motion plane for  δ  2   <   Γ   <  π −  δ  2  .  4.   B   =   E i   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,   E 1   =   T   R  1   of the Side case degenerates in a straight line   H   through   O W   for   Γ   =  δ  2   . Indeed, referring to fig. 10, points   M F   and   P F   degenerate on   O W   . As a con- sequence, Region IV, IV and   V I ′   while coordinates   Ψ R 1  and   Ψ R 2   of points   R 1   and   R 2   can be obtained from values in 6 replacing  φ 1   =   0. In   the   Frontal   case,   E 1   =   H   becomes   a   spiral  T   L  1   ,   straight   lines   from   P   and   R 2   split   in   straight line   and   a   spiral   arc   generating   the   partition   re- ported in fig. 13.   In this case,  φ 1   <   0 and points  R 1   and   R 2   do   not   lay   on   C ( P )   but   on   a   circle through   P   with center   ( 0 ,   −  ρ P   sin 2  φ 1 − sin 2  φ 2  2 sin  ξ   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.  t 1   + t 2  t 1   t 2   ln  (   cos  φ 1 + cos  φ 2  sin (  φ 2 −  φ 1 )  )  +   1  t 1   ln   ( −   sin  φ 1 ) −   1  t 2   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   ),   E 2   =   T   R  2   degenerates in   E 2   =   C . Points   R 1   ≡   M   and   R 2   lays on   C ( P )   with   Ψ R 1   =   1 + sin  φ 1  cos  φ 1  and   Ψ R 2   =  π  2   −  φ 1   +   Ψ R 1   +   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   E 2   =   C  becomes   E 2   =   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.   X W   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   =  max G ∈  γ   ρ 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   O W   is strictly in- creasing along backward extremal arcs (i.e.   S − ,   E −  1   ,   E −  2  with   E 2   6   =   C ) and strictly decreasing along forward ex- tremal arcs (i.e.   S + ,   E +  1   ,   E +  2   with   E 2   6   =   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   E 1 ) of length   ℓ   and an extremal   E 2   =   C   (in any order and direc- tion) is inscribed in two circumferences centered in   O W   . Hence, the shortest sequence is the one with   E 2   =   C   along
the circle of smaller radius necessarily preceded by a for- ward   S   (or   E 1 ) of same length   ℓ . Concluding, in an optimal path a forward arc cannot follow a backward arc.