SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

1

An Efficient Algorithm Involving the Canonical Switching Structure to Compute Minimum-length Trajectories for a Car Huifang Wang, Yangzhou Chen and Philippe Soue`res

Abstract—About fifteen years ago, the problem of determining minimum-length trajectories for a car with a lower bounded angle of steering received a complete analytical solution. Based on successive contributions, a partition of the configuration space was determined, each cell being associated to an optimal path type to drive the car to the origin. However, though this partition describes the optimal structure completely it does not provide directly the optimal solution to link any two configurations. Indeed, a preliminary computation phase is required to characterize the cell inside which the initial configuration is located. In this paper, we describe an optimal control algorithm that can analytically and directly compute an optimal trajectory between any two configurations. The reasoning is grounded on optimal control theory and involves an original geometrical reasoning based on the canonical switching structure of the system. This algorithm is complete, exact, and has a very low computational cost. Simulation results are presented to illustrate the efficiency of the method. Index Terms—Minimum-length Trajectories, nonholonomic constraints, car-like robots, optimal synthesis, geometric methods

I. I NTRODUCTION

T

HE problem of determining minimum length curves with upper bounded curvature and prescribed initial and terminal positions and orientations was first addressed by Dubins in 1957 [1]. This model also described the main two kinematic constraints of a car moving forward in the plane, without slipping, and with a bound on its angle of steering. Dubins proved that optimal solutions are made of two kinds of pieces straight line segments and arcs of circle with minimum turning radius - and characterized a sufficient family of trajectories. More than thirty years later, Reeds and Shepp addressed the same optimization problem but for a car moving forwards and backwards [2]. By using an elegant reasoning that involved geometry and differential calculus they characterized a sufficient family for this extended problem. Later, Sussmann and Tang [3] and Boissonnat et al. [4] proved that Reeds and Shepp’s result can be deduced and even improved by using a unified reasoning based on Pontryagin’s Maximum Principle (PMP) and Optimal Control Theory. Finally, on the basis of these different works completed by a geometric study, the This work was supported by Doctoral Fund of Ministry of Education of China, 20060005014 and by the National Natural Sciences Foundation of China 60774037 H. Wang and Y. Chen are with of Electrical Information and Control Engineering, Beijing University of Technology, 100022, China

[email protected]; [email protected] P. Sou` eres is

with the

[email protected]

LAAS-CNRS,

31077

Toulouse, France

This paper contains supplementary multimedia material provided by the authors. The material consists of a video file for demonstrating the computation and reasoning results of some examples and a Matlab program which can produce the optimal trajectory for any two specified configurations

shortest path synthesis was analytically solved in [5]. This last work provided a partition of the configuration space, each cell being associated to particular path type optimal to reach the origin. From this construction, it is then possible to determine a minimum-length trajectory to reach any two configurations. Identifying the final configuration with the origin, one needs first to determine the cell that contains the initial configuration in order to characterize the optimal sequence of arcs of circles and lines, and then to determine the length of each piece by constructing the trajectory. In this paper, an alternative algorithm is proposed to determine the optimal solution, analytically and directly. Instead of using the partition of the configuration space described in [5], [6], a representation of extremal curves is considered in an associated space whose basis is defined from the switching structure equations introduced in [3]. Compared to [7], this paper provides a grounded geometric reasoning and an alternative efficient procedure of the algorithm. The grounded geometric reasoning is organized in two steps: a local reasoning grounded on PMP to achieve the sufficient family of trajectory and a global reasoning to eliminate nonoptimal trajectories within the sufficient family. A vector ω(t) is introduced to describe the switching functions of optimal control and classify the extremal trajectories into three cases. In each cases, the rotation rules of ω(t) are obtained. They determine necessary optimality conditions from which a set of geometric rules can be deduced to characterize the structure of optimal trajectories. Finally, a global reasoning is used to refine the sufficient family, in order to eliminate extremal curve that are actually not optimal. This last step achieves the characterization of optimal solutions. Given any two configurations of the car, the algorithm allows to determine an optimal trajectory to link them exactly with a very low computational cost. The motivation for studying this problem is both on its theoretical interest [8] and for practical applications in nonholonomic motion planning [9]. From a theoretical point of view, the analysis of this problem is a significant example for the development of modern geometric optimal control theory [3]. For practical applications, the elaboration of such an efficient algorithm in free space is a key point for the design of more complex planning methods amidst obstacles. For instance, the two-step approach proposed in [9] and [10] requires the knowledge of minimum-length nonholonomic paths between successive points sampled from collision-free holonomic paths. Randomized kinodynamic motion planning was proposed in [11] and [12] to build a probabilistic roadmap of sampled points connected by shortest admissible trajectories. Moreover, the shortest paths between two points in flat terrain was used as effective initial guesses of vehicle controls

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

2

the car-like robot is described as:       x˙ cos θ 0  y˙  =  sin θ  u +  0  v 0 1 θ˙

Fig. 1.

The Car-like robot model

parameters for optimal trajectory generation in rough terrain in [13]. The integrated planning and control method described in [14] and [15] also requires the knowledge of the nonholonomic distance to obstacles [16]. Most motion planning methods presented in [17] and [18] can be extended to nonholonomic systems if the solution to the minimum-length problem is effectively achieved. In addition to cars, the minimum-length trajectories have been studied for other nonholonomic systems. PMP was used to demonstrate the time-optimal paths for a mobile robot with one trailer in [19] and for an underwater vehicle in [20]. Moreover, the geometric structure of optimal trajectories was derived from PMP and then a time optimal trajectory algorithm was designed for a differential drive vehicle in [21]. For this kind of vehicle, the optimal path planning problem was then extended in [22] to cope with further constrained related to sensing considerations. The problem was stated in a slightly different way in [23] where author tried to characterize the minimum wheel-rotation paths among piecewise obstacles. Finally, a recent review on the shortest paths for wheeled vehicles was given in chapter 15 of [24]. The paper is organized as follows: The model of a car-like robot and the statement of the optimal control problem are given in Section II. The structural properties of the switching functions and a categorization of optimal trajectories into three cases is proposed in section III. The rotation rules that allow to determine trajectories within the sufficient family are stated in section IV. A global reasoning that allow to eliminate nonoptimal trajectories within the sufficient family is then presented in section V. The description of the algorithm is detailed in Section VI. Finally, simulation results and analysis are then presented in section VII and a concluding remarks are given in section VIII. II. M ODEL AND PROBLEM A. The model of a car-like robot A configuration of the robot, in Fig. 1, is an oriented point z = (x, y, θ) ∈ M = R2 × S 1 , where (x, y) is the coordinate of a reference point describing the position of the vehicle and θ is the orientation with respect to the x-axis. We assume the distance between the front and rear wheel axles D to be 1 and the minimal turning radius to be 1. The kinematic model of

(1)

where (u, v) ∈ U = {−1, 1} × [−1, 1]. u and v describe the linear and angular robot velocities, respectively. This model was called Reeds and Shepp’s problem (RS). As the modulus of the robot’s linear velocity is always equal to one, minimumlength trajectories for this system are equivalent to minimumtime trajectories. Therefore, the determination of shortest paths for the RS problem can be deduced from the resolution of a minimum-time problem. The existence of minimum-time trajectories for RS was proven in [3]. This result was deduced, from the study of the convexified Reeds-Shepp model (CRS), ˜ = [−1, 1] × [−1, 1], for which the convexified control set, U was considered instead of U . In that case, existence theorems allow to conclude directly [25]. For this CRS model, the authors solved the optimization problem and determined a sufficient family of time-optimal trajectories that was also admissible for the RS problem. Following [3], [5], the following notations are introduced to describe the trajectories. The letters L, R, S refer to the robot’s turning directions : Left (for v = 1), Right (for v = −1) and Straight (for v = 0), respectively. The superscript + or − indicates that the motion is forward (u = 1) or backward (u = −1). The subscripts are positive real numbers giving the length of each piece. For instance, L+ a is a trajectory corresponding to u = 1, v = 1, defined on a time interval of length a. Also, the letter B will be used to denote either L or R. Moreover, when necessary, a letter “u” or “v” will be added to specify which control is switching at a particular point. For instance, a + + + Ba vBb trajectory can be of one of four types L+ a Rb , Ra Lb , − − − − La Rb or Ra Lb , where v changes but u does not change. In addition, the pair {(u, v), a} is used to denote a control vector (u, v), which is defined on a time-interval of length a. B. The optimal control problem and Necessary conditions Following the pioneering work by Reeds and Shepp [2], optimal control theory was first applied for this problem in [3] and [4]. The switching functions were achieved from the PMP ([25] and [26]) and their structural characteristics were obtained by introducing the Lie algebras of vector fields. The crucial role played by Lie algebras in analyzing the controllability properties of nonlinear control systems was explained in [3], [27] and [28]. In addition, the small-time locally controllability for the RS and the CRS problem was presented in [3] and [18]. The model (1) for RS can be rewritten in the form z˙ = uf (z) + vg(z),

(2)

where z = (x, y, θ)T ∈ M , f (z) = ( cos θ sin θ 0 )T , ¡ ¢T . g(z) = 0 0 1 The problem is to find an admissible control (u, v) which, subject to (2), minimizes the total travel time.

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

3

Introducing an adjoint vector λ(t) ∈ R3 , the Hamiltonian function H = H(z, (u, v), λ) is expressed by H = hλ, f (z)u + g(z)vi = ϕ(t)u(t) + ψ(t)v(t) = (λ1 (t) cos(θ) + λ2 (t) sin(θ))u(t) + λ3 (t)v(t)

(3)

where ϕ(t) = hλ(t), f (z)i , ψ(t) = hλ(t), g(z)i

(4)

The fundamental results from PMP are as follows: Let (u (t), v ∗ (t)) be an optimal control and z ∗ (t) the corresponding state trajectory. Also, let λ(t) be a nontrivial solution of the adjoint equation ∗

h iT ∂g λ˙ (t) = −∇z H = − u ∂f + v λ(t) ∂z ∂z

(5)

Then, for all t ∈ [0, T ], (u∗ (t), v ∗ (t)) = arg min H(z ∗ (t), (u(t), v(t)), λ(t)) (6) (u,v)∈U

Furthermore, there exists a constant λ0 ≥ 0 such that H(z ∗ (t), (u∗ (t), v ∗ (t)), λ(t)) = −λ0 , i.e. ϕ(t)u∗ (t) + ψ(t)v ∗ (t) = −λ0

(7)

Moreover, the 4-tuple (x∗ , u∗ , λ0 , λ) satisfying the necessary condition of PMP is called an extremal. Minimizing the Hamiltonian function (3), from (6), it comes that u∗ (t) = −sign(ϕ(t)); v∗ (t) = −sign(ψ(t))

(8)

where sign(s) is equal to +1 if s > 0, −1 if s < 0, and any element of [−1, 1] if s = 0. So ϕ(t), ψ(t) will be called the u-switching function and v-switching function, respectively. From (7) (8), it can be deduced λ0 = |ϕ(t)| + |ψ(t)|

(9)

Let h(z) = [g(z), f (z)] = (− sin θ, cos θ, 0)T , χ(t) = ∂g hλ(t), h(z)i, where [g, f ](z) = ∂f ∂z · g(z) − ∂z · f (z) denotes the Lie bracket of vector field g and f . Since [f (z), h(z)] = 0, [g(z), h(z)] = −f (z), from (4), (5), it comes that: ˙ ϕ(t) ˙ = v(t)χ(t); ψ(t) = −u(t)χ(t); χ(t) ˙ = −v(t)ϕ(t) (10) Finally, (5) being linear in λ, if λ(t) = 0 for some t then λ(t) = 0 for all t. Therefore, the nontrivial adjoint vector λ(t) never vanishes. Moreover, the vector fields f (z), g(z) and h(z) define a basis of the tangent space to M , for each z, so ϕ(t), ψ(t) and χ(t) can not have a common zero; i.e. for any t, |ϕ(t)| + |ψ(t)| + |χ(t)| 6= 0 (11) III. S TRUCTURAL PROPERTIES OF SWITCHING FUNCTIONS AND CATEGORY OF OPTIMAL TRAJECTORIES

The first part of this section recalls the results stated in [3], [4] and [5] that describe the structural properties of extremal trajectories. Then, a new geometric representation will be introduced to describe the optimal trajectories and classify them into three cases, depending on their structural properties.

Fig. 2. Nonzero vector ω(t) traveling in χ − ϕ coordinate system and its corresponding control values

A. Properties of optimal trajectories Extremals for which λ0 = 0 are usually called ”abnormal”. On any interval where one of the switching functions does not vanish (resp. has finitely many zeros) the corresponding control is bang (resp. regular bang-bang). At the other extreme, if the switching function ϕ(t) (resp. ψ(t)) vanishes identically on a nonempty interval, it is u-singular (resp. v-singular) on that interval. Property 1: Nontrivial optimal abnormal extremals do not exist. Furthermore, on a nontrivial optimal extremal, ϕ(t), ψ(t) cannot have a common zero. Proof: Assume λ0 = 0. Equation (9) implies that ϕ(t) ≡ ψ(t) ≡ 0. From (11), χ(t) 6= 0. But, (10) implies that u(t)χ(t) = v(t)χ(t) = 0. So u(t) ≡ v(t) ≡ 0. Thus the trajectory will always stay on the start configuration and it can only be timeoptimal if the trajectory is trivial. Obviously, at any t, that ϕ(t), ψ(t) have a common zero implies λ0 = 0. Therefore the only corresponding trajectory is trivial, i.e. of zero length in zero time. Property 2: κ = ϕ2 (t) + χ2 (t) is a constant. Proof: Since κ˙ = 2ϕ(t)ϕ(t) ˙ + 2χ(t)χ(t), ˙ ϕ(t) ˙ = v(t)χ(t), and χ(t) ˙ = −v(t)ϕ(t), we get κ˙ = 0. So κ is a constant. Property 3: κ = 0 if and only if ϕ(t) ≡ 0. Proof: If κ = 0, then obviously ϕ(t) ≡ 0. Conversely when ϕ(t) ≡ 0, ϕ(t) ˙ = vχ(t) ≡ 0. From property 1, we get ψ(t) 6= 0 that implies v 6= 0, so χ(t) ≡ 0. Thus, κ = 0. B. Categories of optimal trajectories Let us define [0, T ] as the whole time interval for an optimal trajectory. According to Property 2, κ is constant on [0, T ]. This property will be used to categorize the optimal trajectories on the whole time interval. In terms of Property 2, the optimal trajectories are categorized according to the value of the constant κ: the cases κ = 0 and κ > 0 are to be considered. κ = 0 corresponds to u-singular on [0, T ]. For κ > 0, three cases are considered, depending on ψ(t): 1. ψ(t) never vanishes on [0, T ]. 2. ψ(t) has isolated zeros on [0, T ] and 3. ψ(t) ≡ 0 on a nontrivial interval [t1 , t2 ] ⊂ [0, T ]. In the situation κ > 0, without loss of generality, the condition κ = 1 is assumed, thus ϕ2 (t) + χ2 (t) = 1

(12)

From (10), (12), the following notation is introduced ϕ(t) = sin(θ − θ0 ); χ(t) = cos(θ − θ0 )

(13)

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

4

TABLE I C ATEGORIES OF THE OPTIMAL TRAJECTORIES

κ=0 κ>0

situations to be considered ϕ ≡ 0 on [0, T ] 1. ψ never vanishes on [0, T ] 2. all zeros of ψ are isolated on [0, T ] 3. ψ ≡ 0 on a nontrivial interval [t1 , t2 ] ⊂ [0, T ]

Cases κ=0 β ∈ (0, β = π2

π ) 2

with respect to a constant θ0 . A unit vector ω(t) = (χ(t), ϕ(t)) is rotating with the angular velocity v along the unit circle in the χ − ϕ coordinate system, shown in Fig. 2 and first introduced in [3]. The rotation of ω(t) is clockwise as long as v = −1 and counterclockwise when v = 1. v changes its value only when ψ(t) = 0. u = 1 if ω(t) is located below the χ axis, whereas u = −1 if ω(t) is located above the χ axis, and u changes its sign when ω(t) goes through the χ axis. Therefore the rotation of ω(t) uniquely determines an optimal trajectory. When κ > 0, two cases are to be distinguished: 1) ψ(t) never vanishes; 2) ψ(t) has zeros. If ψ(t) never vanishes, it implies v ≡ 1 or v ≡ −1. ω(t) rotates only in one direction such that the intervals between two consecutive switching of u, where ω(t) meets the χ axis, are of length π. Thus there is only one possible optimal trajectory which is Ba uBπ · · · uBπ uBb , where 0 ≤ a, b ≤ π. It has been proved in Lemma 11 of [3] that, a Ba uBπ trajectory with a > 0 cannot be optimal. Therefore the only possibility for optimality in the sufficient family is Ba uBb with 0 ≤ a, b ≤ π, but a = 0 if b = π or b = 0 if a = π. This two-parameter trajectory Ba uBb can be regarded as a special case for a three-parameter trajectory Ba uBb uBc , obtained in case κ = 0. In case κ = 0, since ϕ ≡ 0, ψ also never vanishes on [0, T ]. Differently, u is singular for case κ = 0, so u can switch at any time. If ψ(t) has ¡ ¤ a zero at time t, then λ0 = |ϕ(t)|. Let us define β ∈ 0, π2 such that sin(β) = λ0 . With this notation, when 0 0 or ψ− ψ(t) = 0, ω(t) is aligned with one of the two lines ψ+ making respectively an angle β or π − β with respect to the χ axis (See Fig. 3 (a)). In the sequel, the generic notation ψ 0 will 0 0 when necessary. From be used to describe either ψ+ or ψ− Property 1, ϕ(t) and ψ(t) cannot have a common zero. So, β 6= 0. Thus, ψ 0 is never identical to the χ axis. Depending on the ¡ value ¢ of β, the trajectories are classified in two cases: β ∈ 0, π2 and β = π2 . The categories of optimal trajectories are described in Table 1. As a conclusion of the above it is sufficient to ¡ analysis, ¢ consider three cases: Case β ∈ 0, π2 ; Case β = π2 and Case κ = 0. IV. G EOMETRIC RULES FOR THE SUFFICIENT FAMILY This section introduces a geometric reasoning associated to the rotation of the ω(t) vector in the canonical switching space. The types of trajectories that constitute a sufficient family and the rules for the algorithm to compute optimal trajectories will be deduced from the rotation of ω(t), for each of the three cases specified in the preceding section. Meanwhile, two important propositions, which provide a principle for the geometric algorithm, are obtained.

− + + − Fig. 3. If β ∈ (0, π2 ), for trajectory Rβ Rβ Lβ Lβ , (a) demonstrates the rotation trajectory of ω(t);(b) shows the time evolution of its ψ(t). Letters A-D clarify the corresponding positions between (a) and (b)

Let us define a switching vector (ϕ(t), χ(t), ψ(t)), where (ϕ(t), χ(t)) refers to the coordinate of ω(t), and ψ(t) determines its direction. Also, let us define two kinds of behavior for ω(t). 1) ω(t) hits ψ 0 at isolated points. 2) ω(t) stays on ψ 0 over a nontrivial interval. ¢ ¡ A. Case β ∈ 0, π2 In this subsection, four lemmas that allow to characterize the rotation rules of ω(t) are first stated. Then, all the possible sequences of rotations of ω(t) are studied and the corresponding optimal trajectories are determined. Finally, a fundamental proposition claiming the uniqueness of optimal solutions is stated. The χ axis and ψ 0 divide the plane into four sectors that are numbered as illustrated ¡ ¢ in Fig. 3 (a). Lemma 1: If β ∈ 0, π2 , ω(t) cannot cross over ψ 0 . Proof: As the sign of u does not change, it comes from ¨ = sign(ψ(t)) |ϕ(t)|. Therefore, ψ(t) is convex (10) that ψ(t) when ψ(t) > 0 and concave when ψ(t) < 0. Fig. 3 (b) shows one possible time evolution of ψ(t) at period 4β in solid curve. Assume that ω(t) goes through ψ 0 at time tC . Then, at this point, the direction of rotation of ω(t) does not change, and correspondingly the sign of the angular velocity v is not modified, so the sign of ψ(t) does not change, as illustrated by the dashed line in Fig. 3 (b). Thus zero is an ˙ C ) = 0. But when ω(t) hits ψ 0 extremum of ψ(t), where ψ(t π at tC for β ∈ (0, 2 ), then χ(tC ) 6= 0 and |u(tC )| = 1, and ˙ C ) = −u(tC )χ(tC ) 6= 0. So, zero is not an extremum then ψ(t of ψ(t) and a contradiction ¡ ¢ is reached. Lemma 2: If β ∈ 0, π2 , ω(t) cannot stay on ψ 0 . ˙ Proof: If ω(t) stays on ψ 0 , then ψ(t) ≡ 0 on a ¡nontrivial ¢ interval. From the proof of Lemma 1, when β ∈ 0, π2 , if ˙ C ) 6= 0. Therefore ω(t) cannot stay ψ(t) = 0 at time tC , ψ(t on ψ 0 for a nontrivial¡interval. ¢ Lemma 3: If β ∈ 0, π2 , ω(t) cannot be located outside the four sectors. Proof: If a vector ω is located in a shadow area outside the four sectors represented in Fig. 3 (a), then ϕ > sin(β). But, from (9), it is known that ϕ ≤ λ0 = sin(β). So, ω(t) cannot be located outside the four sectors. Remark 1: From Lemma 1 and Lemma 2, it follows that, once ω(t) hits ψ 0 , it has to reverse its direction of rotation since it cannot pass through this line nor stay on it. If ω(t)

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

5

Fig. 6.

+ − − + Fig. 4. To link configurations (0, 0, 0) and (0, 0.2132, 0), Ra Rb Lb La + + − − is optimal, rather than Lβ Rβ Rβ Lβ

Fig. 5. All possible rotations of ω(t) starting from Sector 1 with all corresponding possible initial and final rotation states, and the corresponding optimal trajectory types for Case β ∈ (0, π2 )

starts from sector 1 or 4, it always rotates within these two sectors, and never reaches the areas outside the sectors 1 and 4. Similarly, ω(t) always rotates inside the sectors 2 and 3 if it starts from sectors 2 or¡ 3. ¢ Lemma 4: When β ∈ 0, π2 , there is no optimal trajectory that contains more than four B pieces. Proof: It is sufficient to¡prove¢ that there is no optimal fivepiece trajectory when β ∈ 0, π2 . The only two possibilities ¢ ¡ for the optimal trajectories with five B pieces when β ∈ 0, π2 are: Ba uBβ vBβ uBβ vBb and Ba vBβ uBβ vBβ uBb , where a, b ≤ β. In [3], the theory of envelopes was used to prove that a Ba uBβ vBβ uBβ trajectory with a < β cannot be optimal. That is to say that, as long as an optimal trajectory contains the piece Ba uBβ vBβ uBβ , it cannot be optimal. Obviously, the first possibility is ruled out for the sufficient family. Moreover, for the second one, if a trajectory Ba vBβ uBβ vBβ uBb reversed from the final configuration back to the initial one, a trajectory Bb uBβ vBβ uBβ vBa is obtained, which is not optimal. Therefore, this path-type is also excluded from the sufficient family.

Two ways in which ω(t) meets the ϕ axis twice

A general proof for Lemma 4 has been given in [3] [4]. Remark 2: It follows from Lemma 4 that the vector ω(t) hits ψ 0 no more than twice. Next, the ranges of length parameters a, b, for optimal trajectories with four B pieces, are discussed. Because a Ba uBβ vBβ uBβ , a < β, trajectory is not optimal, if an optimal trajectory is of the type Ba uBβ vBβ uBb , then 0 ≤ a, b < β. For type Ba vBβ uBβ vBb with 0 ≤ a, b ≤ β, a or b can be equal to β but not simultaneously. For instance, for + − − an L+ β Rβ Rβ Lβ trajectory (represented in red solid curves in + Fig.4) there always exists a shorter trajectory Ra+ Rb− L− b La (shown in blue dashed curves in Fig. 4.) that links the same initial and final configurations. The analysis used to exclude the path type Bβ vbβ uBβ vBβ from the sufficient family comes as a part of the global reasoning in Section V. Fig. 5 lists all possibilities for the rotation of ω(t) that starts from sector 1 with all corresponding possible initial and final rotation states, which are determined by the initial and final switching vectors, respectively. ∧ denotes the logical AND. A blue solid curve denotes the starting direction and position of ω(t), whereas a red dashed curve shows its ending direction and position. From Fig. 5, it follows that there is a one-toone mapping between the set of all possible rotation of ω(t) that starts from sector 1 and the corresponding time-optimal trajectories. Below each type of optimal trajectory, the range of the length parameters is given. The sequences of rotation of ω(t), starting from sectors 2, 3, 4, can be obtained similarly. Therefore, the Proposition 1 below ¡ can ¢ be stated: Proposition 1: When β ∈ 0, π2 , if the start switching vector (ϕs , χs , ψs ) and the final switching vector (ϕf , χf , ψf ) are specified, there exists only one possible rotation for ω(t) that uniquely determines an optimal control trajectory. So far, the following trajectory types have been obtained: Ba vBb , Ba uBb , Ba uBβ vBb , Ba vBβ uBb , with 0 ≤ a, b ≤ β; Ba uBβ vBβ uBb , with 0 ≤ a, b < β; Ba vBβ uBβ vBb with 0 ≤ a, b ≤ β, b 6= β if a = β or a 6= β if b = β. Moreover, as shown in Fig. 5, 4 × 4 = 16 three-parameter ¢ ¡ types of optimal trajectories are obtained when β ∈ 0, π2 . The number of three-parameter types of optimal trajectories needs to be regarded with attention. Indeed, such path types contain the maximum number of unknown parameters and will influence the computational cost of the algorithm. B. Case β = π2 0 0 β = π2 implies that ψ+ and ψ− are coincident and define a unique axis that will be denoted by ψ 0 . Also from β = π2

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

6

TABLE II A LL TYPES OF O PTIMAL T RAJECTORIES WITHIN THE SUFFICIENT FAMILY Cases β ∈ (0,

β=

π 2

κ=0

Fig. 7. In Case β = π2 , all possible rotations of ω(t) starting at Sector 1 with all corresponding possible initial and final switching states; the corresponding optimal trajectory types; and determination of the parameters

it can be deduced that the zeros of ψ(t) only occur on the ϕ axis, so ψ 0 is coincident with the ϕ axis, as shown in Fig. 6 and 7. Lemma 5: If ψ(t) ≡ 0 on the ϕ axis over a nontrivial interval [t1 , t2 ], then v = 0. Proof: If ψ(t) = 0 identically on the ϕ axis, we get χ(t) ≡ 0 and |ϕ(t)| = 1. As χ(t) ˙ = −vϕ(t) = 0, then v = 0. Another general proof stating that, if ψ(t) ≡ 0 on a nontrivial interval then v = 0 in any cases, can be found in [3]. Furthermore, it can be deduced from Lemma 2 that the condition v = 0 only happens when ψ(t) ≡ 0 on the ϕ axis. This implies that the S piece only occurs when β = π2 . Lemma 6 below recalls the statement of Lemma 3 in [5]. Lemma 6: It is sufficient to consider the trajectories starting from (xs , ys , θs ) and ending at (xf , yf , θf ), with |θs − θf | ≤ π. The following lemma can then be obtained: Lemma 7: In the sufficient family, ω(t) cannot cross twice the ϕ axis two times. Proof: There are only two ways in which ω(t) can cross the ϕ axis twice, as instantiated by Fig. 6. The first − − + L π , shown in Fig. 6 (a), is obviously not trajectory L− a Sσ L π 2 2 + optimal, since |θs − θf | > π. Because the trajectory L− π Lπ 2

2

π ) 2

Types Ba uBβ vBb Ba vBβ uBb Ba uBβ vBβ uBb Ba vBβ uBβ vBb Ba vSσ vBb Ba uB π vSσ vB π uBb 2 2 Ba uB π vSσ vBb 2 Ba vSσ vB π uBb 2 Ba uBb uBc

Ranges

Numbers

0 ≤ a, b ≤ β 0 ≤ a, b < β 0 ≤ a, b ≤ π2 For all same Bs : a + b ≤ π2 0 ≤ a, b, c ≤ π

16

28 2

+ is equivalent the trajectory R− π R π , so the second trajectory 2 2 + − − R π , shown by Fig. 6 (b), is equivalent the trajectory L− a Sσ R π 2 2 − − + L− L π and thus it is not optimal. These two trajectory a Sσ L π 2 2 types can be written as Ba Sσ B π2 B π2 . Therefore all trajectories that include the piece Ba Sσ B π2 B π2 are not optimal. So ω(t) cannot cross the ϕ axis twice. Remark 3: A consequence of Lemma 7 is that optimal trajectories in the sufficient family contain at most one S piece. From the above lemmas, Fig. 7 describes all possibilities for the rotation of ω(t) that starts from sector 1 with the given initial and final switching vectors. It is also true that any optimal trajectory with more than two pieces uniquely determines ω(t). Proposition 2 below can then be obtained. Proposition 2: In Case β = π2 , if the initial and final switching vectors, (ϕs , χs , ψs ) and (ϕf , χf , ψf ), are specified, there exists only one possible rotation for ω(t) that uniquely determines an optimal trajectory. All optimal trajectories with S can be described as let 0 ≤ a, b ≤ π2 , Ba vSσ vBb , Ba uB π2 vSσ vBb , Ba vSσ vB π2 uBb and Ba uB π2 vSσ vB π2 uBb with two different middle Bs. For types Ba uB π2 vSσ vBb and Ba vSσ vB π2 uBb , if all Bs are the same, then a + b < π2 . These results are consistent with the ones obtained by the different deductive methods in previous literature [3],[2]. Moreover there are 7 × 4 = 28 threeparameter types of optimal trajectories with straight lines.

C. Optimal trajectories for Case κ = 0 From Property 3, κ = 0 if and only if ϕ ≡ 0. From Property 1, ψ(t) never vanishes, i.e. along an optimal trajectory the only switching that may occur are on u. In this case, the search for optimal trajectories can be done by considering the LTV (Left Turning Velocity) when v ≡ 1, or the RTV (Right turning problem) corresponding to v ≡ −1. According to [3], a solution to these problem is given by the following lemma: Lemma 8: In Case κ = 0, the two trajectory types − + + − + L+ a Lb Lc and Ra Rb Rc , with 0 ≤ a, b, c ≤ π constitute a sufficient family for RS that correspond to v ≡ 1 and v ≡ −1 respectively. D. The sufficient family for RS Table 2 summarizes all types of optimal trajectories and the corresponding range of length parameters. The last column gives the number of the three-parameter trajectory types for

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

7

TABLE III S YMMETRY BETWEEN THE POINTS IN DIFFERENT QUADRANTS Q− 1 : (x1 , y1 , θ) w Q− 2 : (x2 , y2 , θ) −(reverse(w)) Q− 3 : (x3 , y3 , θ) −(w) Q− 4 : (x4 , y4 , θ) reverse(w)

Fig. 8.

Symmetry of the configurations and their corresponding trajectories

Fig. 9.

The domain of optimal trajectory types ending inside Q− 1

each case. The obtained 46 three-parameter trajectory types is consistent with the result in [3] evidences the correctness of our geometric reasoning. The family of all trajectory types described in Table 2 and satisfying the indicated conditions of length parameters will be denoted by F. Theorem 1: F is a sufficient family for the RS problem. V. G LOBAL R EASONING FOR THE COMPLETENESS OF THE ALGORITHM

According to the above geometric reasoning, any two configurations of the car can be linked by a minimum-length trajectory that belongs to the sufficient family F. However, the local information of PMP, upon which the construction of the family F is based, is not sufficient to conclude on the optimality of trajectories. Indeed, a global reasoning is necessary to complete the local information by comparing the cost of different extremal candidate. Such a global reasoning is proposed in this section. The global symmetry and reduction properties, that were used in [5] to refine the sufficient family, are recalled in a first part. On this basis, the characteristics of optimal trajectories that will be used in the algorithm are derived. They are presented in the second part of the section. Without loss of generality, it will be supposed in the sequel that the initial configuration of the car is the origin O = (0, 0, 0).

Q+ 1 : (x1 , y1 , −θ) lr(w) Q+ 2 : (x2 , y2 , −θ) −(reverse(lr(w))) Q+ 3 : (x3 , y3 , −θ) −(lr(w)) Q+ 4 : (x4 , y4 , −θ) (reverse(lr(w))

Symmetric axis between (xi , yi ) and (x1 , y1 ) ∆ θf O ∆⊥ θ

f

As shown in [5] and illustrated in Fig. 8, if a trajectory T1 starts from the origin and ends at (M1 , θf ), then four isometric trajectories T2 , T3 , T4 and T¯1 , starting from the origin and ending respectively at (M2 , θf ), (M3 , θf ), (M4 , θf ) and ¯ 1 , −θf ), can be drawn. M2 is the point symmetric to M1 (M with respect to ∆θf and M3 , M4 are the points symmetric to ¯ 1 is the M1 , M2 , respectively, with respect to the origin. M point symmetric to M1 with respect to the axis x. For i = 1, . . . , 4, when θf ∈ [−π, 0], let us denote by Q− i the ith quadrant delimited by ∆θf and ∆⊥ θf , whereas when θf ∈ [0, π], let us denote denote by Q+ i the quadrant which is symmetric to the ith quadrant delimited by ∆−θf and ∆⊥ −θf with respect to the x axis. If the word w is used to describe a trajectory, then the words describing the trajectories that end at the symmetric points in the other quadrants are described in Table 3. In this table, reverse(w) means “writing w in the reverse direction”; −(w) means “ writing w by permuting the superscripts + and −”; lr(w) means “writing w by permuting the R and the L”. Due to these symmetry properties, the characteristics of optimal trajectories can be deduced from the study of trajec⊥ tories ending inside the first quadrant Q− 1 of the (∆θf , ∆θf ) coordinate system, when θf ∈ [−π, 0]. Fig. 9 shows the partitions of domains corresponding to different types of π optimal trajectories ending inside Q− 1 for θf = − 4 and 3π θf = − 4 . Table 4 presents the trajectory types inside Q− 1 for θf in different intervals. It was proven in [5] that the + + − − + + + − + + − trajectory types L+ a Sσ R π Rb , La L π Sσ R π Rb , La Rβ Rb 2

2

2

+ + − and L− a Lβ Rβ Rb stop being optimal as soon as they cross the ∆θf axis. Note that the domains corresponding to types + − − π − + + − L+ a Rβ Rβ Lb and La Lβ Rβ Rb overlap when θf ∈ (− 3 , 0), π as illustrated in Fig. 9 for θf = − 4 .

B. Characteristic of trajectories for the case β =

π 2

Lemma 9: When β = π2 any trajectory of the sufficient family that ends inside Q− 1 with ψf ≥ 0 is optimal. Proof: The first column of Table 4 lists, when β = π2 , all optimal trajectories whose final configuration is in Q− 1. As described in Fig. 10, these trajectory end with the a piece R, which implies ψf ≥ 0. Only two trajectory types, + + − − + + + − L+ a Sσ R π Rb and La L π Sσ R π Rb , stop being optimal once 2

2

2

− − they cross ∆θf . Thus their symmetric types Rb+ R− π Sσ La

A. Symmetry and refinement of optimal trajectories θ

In the (x, y)-plane, let us denote by ∆θf the line y sin 2f + θ θf x cos 2f = 0, and by ∆⊥ θf the perpendicular line y cos 2 − θ x sin 2f = 0.

2

− − + and Rb+ R− π Sσ L π La with ∆θf , are the only two nonoptimal 2

2

trajectory types within the sufficient family that end inside Q− 1 . Moveover, these two nonoptimal trajectory types verify

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

8

TABLE IV T YPES OF OPTIMAL TRAJECTORY ENDING INSIDE Q− 1 Types + + L+ a Sσ Rb + + + R a Sσ R b + + − L+ a Sσ R π Rb

valid Intervals [− π2 , 0] [−π, 0] [−π, 0]

+ + + − Ra Sσ R π R b

[−π, − π2 ]

+ + + − L− a L π Sσ R π R b

[−2arccot(2), 0]

2

2

2

2

Types + − L+ a Rβ Rb + + − − La Rβ Rβ Lb + + − L− a Lβ Rβ Rb

valid Intervals [−π, 0] [− 2π , 0] 3 [− π3 , 0]

+ − + Ra Rb Rc

[−π, 0]

Fig. 11. Geometric representations of a group of symmetric optimal trajectory types ending inside 8 quadrants.

for ω(t) that uniquely determines an optimal trajectory. Let us first explain how to compute (ϕs , χs , ψs ) and (ϕf , χf , ψf ) from (xs , ys , θs ) and (xf , yf , θf ). From (5), it comes   λ˙ 1 (t) = 0 λ˙ (t) = 0  ˙2 λ3 (t) = λ1 (t) sin(θ)u(t) − λ2 (t) cos(θ)u(t) Fig. 10. Geometric representations of all optimal trajectory types ending π inside Q− 1 which verify ψf ≥ 0, for Case β = 2

ψf ≤ 0. Therefore, within the sufficient family, feasible trajectories ending inside Q− 1 and verifying the condition ψf ≥ 0 are necessarily optimal. Using the symmetry properties, a corollary can be stated to ensure the optimality of trajectories, depending on the sign of ψf and the ending quadrant. Fig. 11 gives geometric representations for a group of symmetric optimal trajectory in 8 different quadrants. The vectors ωf drawn in solid lines stand for ψf ≥ 0, whereas the dashed ones stand for ψf ≤ 0. Corollary 1: When β = π2 , optimal trajectories ending in − + + Q1 ∪ Q− 3 when θf ∈ [−π, 0], or in Q2 ∪ Q4 when θf ∈ [0, π] verify the condition ψf ≥ 0, whereas optimal trajectories ending in other terminal quadrants verify necessarily ψf ≤ 0. VI. D ESCRIPTION OF THE ALGORITHM This section presents the algorithm that allows to determine a minimum-length trajectory to link any two configurations. A simple change of coordinates is used to identify the initial configuration with (0, 0, 0). Then, the corresponding final configuration (xf , yf , θf ) constitutes the unique input of the algorithm. The computation steps of optimal trajectories are first described for each of the three cases. The integrated reasoning is then presented in the last part. A. Case β =

π 2

From Proposition 2, it is know that, when β = π2 if the initial switching vector (ϕs , χs , ψs ) and the final one (ϕf , χf , ψf ) are specified, there is only one possible rotation

and then:

  λ1 (t) = λ10 λ2 (t) = λ20  λ3 (t) = λ10 y(t) − λ20 x(t) + λ30

(14)

where (λ10 , λ20 , λ30 )T is the value of λ(t) at initial time. Expending ϕ(t) in (13), yields ϕ(t) = sin(θ − θ0 ) = sin(θ) cos(θ0 ) − cos(θ) sin(θ0 ) (15) According to (4) (15), λ10 = − sin(θ0 ) and λ20 = cos(θ0 ). From (4) and (14), it comes ψ(t) = λ3 (t) = λ10 y(t) − λ20 x(t) + λ30

(16)

The switching functions ϕ(t), ψ(t) satisfy (9) at both (xs , ys , θs ) and (xf , yf , θf ). So, ½

|sin(θs − θ0 )| + |− sin(θ0 )ys − cos(θ0 )xs + λ30 | = λ0 |sin(θf − θ0 )| + |− sin(θ0 )yf − cos(θ0 )xf + λ30 | = λ0 (17) If β = π2 , then λ0 = sin(β) = 1. Considering that (xs , ys , θs ) = (0, 0, 0) in (16), yields λ30 = ψs . In order to guarantee the completeness of the algorithm, according to Lemma 9 and Corollary 1, it is necessary to determine the sign of ψf . According to the definitions of the symmetry axes, the coordinate frame (∆θf , ∆⊥ θf ) can be deduced from θ the initial frame by a rotation of angle 2f . Therefore, with respect to (∆θf , ∆⊥ θf ), the coordinates of the trajectory end point (x∆ , y∆ ) is expressed by µ ¶µ ¶ µ ¶ θ θ x∆ xf sin( 2f ) cos( 2f ) = . y∆ yf sign(θf ) · sin( θ2f ) −sign(θf ) · cos( θ2f ) Then, the criterion for determining the sign of ψf is: −sign(θf ) · x∆ · y∆ ≥ 0 ⇒ ψf ≥ 0. Therefore, in this case,

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

(17) can be converted into ½ |sin(−θ0 )| + |ψs | = 1 |sin(θf − θ0 )| − sin(θ0 )yf − cos(θ0 )xf + ψs = 1

9

(18)

Similarly, it comes: −sign(θf ) · x∆ · y∆ ≤ 0 ⇒ ψf ≤ 0, which yields ½

|sin(−θ0 )| + |ψs | = 1 |sin(θf − θ0 )| + sin(θ0 )yf + cos(θ0 )xf − ψs = 1

(19)

The procedure for designing the optimal control law in case β = π2 can be described by the following steps: Step 1: Compute (θ0 , ψs ) from (18) or (19) according to x∆ , y∆ and θf ; Step 2: Determine (us , vs ), (uf , vf ) from (8) according to (ϕs , χs , ψs ) and (ϕf , χf , ψf ) determined by (13), (16). Step 3: Determine a, b and their corresponding control for the B pieces of the optimal trajectory. Step 4: Determine σ and the corresponding control for the S piece of the optimal trajectory. The following part focuses on the determination of control laws in step 3 and step 4. The notation ϕˆ will be used to indicate the half ϕ-axis that is encountered by ω(t). First, let us introduce θ1 , which is the abstract angle of ϕˆ with respect to the abstract angle of the positive χ axis θ0 . For instance, referring to Fig. 7, when the start position of ω(t) is within sector 1 or 4, if vs = −1, then θ1 = θ0 − π2 ; or else if vs = 1, then θ1 = θ0 + π2 . Next, a trajectory is split into three pieces in terms of the ϕˆ axis: C1 , a piece whose ω(t) has not met the ϕˆ axis; S, a piece whose ω(t) stays on the ϕˆ axis; and C2 , a piece whose ω(t) has met the ϕˆ axis. Let us denote by a ˆ and ˆb the length of C1 and C2 respectively. Since there is only one S piece, for C1 and C2 , ω(t) should meet the same ϕ half-axis, i.e. ϕˆ half-axis. Therefore the parameter a ˆ is the length of the trajectory whose ω(t) travels from the start position to the ϕˆ axis, and ˆb is the length of the trajectory whose ω(t) travels from the ϕˆ axis to the final position. These two parameters are determined by θ1 , vs , θs , vf , θf , described in the Column 3 of Fig.7 in detail, where the function mod : R → [0, 2π) has been introduced to map a real number to its value modulo ˆ − π2 and the control law for C1 is 2π. If a ˆ > π2 , then a = a π ˆ and {(us , vs ), a; (−us , vs ), 2 }. Or else, if a ≤ π2 , then a = a the control law is {(us , vs ), a}. A similar rule is used for the computation of b and its corresponding control law for C2 . For step 4, on the S piece the linear velocity u is the same as the preceding piece and the angular velocity v = 0. Assume that an S piece starts at the configuration S 1 and ends at the configuration S 2 . Obviously, σ represents the Euclidean distance between S 1 and S 2 . S 1 can be easily achieved from the origin according to the control at the piece C1 , whereas S 2 can be computed from the final configuration according to the reversing control at the piece C2 . For instance if there is a control law {(−uf , vf ), π2 ; (uf , vf ), b} for C2 , then its reversing control law {(−uf , −vf ), b; (uf , −vf ), π2 } drives the robot from the final configuration to S 2 . ¡ ¢ B. Case β ∈ 0, π2 From the global reasoning, in order to determine an optimal trajectory for any final configuration it is sufficient to

Fig. 12. Geometric representations of the optimal trajectory types ending π inside Q− 1 for Case β ∈ (0, 2 ) and the ranges of corresponding parameters.

characterize the optimal trajectory ending at its corresponding configuration in Q− 1 . Therefore in this case, the procedure is designed in three steps: Step 1: Convert (xf , yf , θf ) to the corresponding configu0 ration (x0f , yf0 , θf0 ) inside Q− 1 with θf ∈ [−π, 0]. Step 2: Search the optimal trajectory that links the origin to (x0f , yf0 , θf0 ). Step 3: Convert the obtained trajectory ending at (x0f , yf0 , θf0 ) to the trajectory ending at (xf , yf , θf ). The rules of conversions in both step 1 and step 3 are described in Table 3. Next, we focus on trajectories ending inside Q− 1 . In this case, we search for an optimal trajectory is + − + + − − limited to the following three types, L+ a Rβ Rb , La Rβ Rβ Lb , − + + − La Lβ Rβ Rb , as illustrated in Fig. 12. First, let us assume that the optimal trajectory is of the form + − L+ a Rβ Rb , whose a, β and b satisfy the below equations:  0 0  xf = sin(θf ) − 2 sin(a − β) + 2 sin(a) 0 y = − cos(θf0 ) + 2 cos(a − β) − 2 cos(a) + 1 (20)  f b = a − β − θf0 ¯ ¯ ¯ ¯ |θ0 | ¯ ¯ ¯ ¯ with β ∈ [ 2f , π2 ] and max(¯θf0 ¯ − β, 0) ≤ b ≤ min(¯θf0 ¯ , β). Fortunately, equation (20) can be solved analytically. If a + − trajectory L+ a Rβ Rb satisfying (20) and the parameter range condition can be obtained, then this trajectory is necessarily + − optimal, as the domain L+ a Rβ Rb does not intersect other domains. If θf0 ∈ [− 2π 3 , 0] and if there is no solution for (20), it can + − − be assumed that the trajectory type is L+ a Rβ Rβ Lb , with a, β and b satisfying:  0 0  xf = − sin(θf ) + 2 sin(a − 2β) − 2 sin(a − β) + 2 sin(a) 0 0 y = cos(θf ) − 2 cos(a − 2β) + 2 cos(a − β) − 2 cos(a) + 1  f b = 2β + θf0 − a ¯ ¯ (21) 2β−|θf0 | |θf0 | π ¯ ¯ with β ∈ [ 2 , 3 ] and ≤ a ≤ min(2β − ¯θf0 ¯ , β). If 2 π θf0 ∈ [− 2π 3 , − 3 ], the obtained trajectory is necessarily optimal. + + − However if θf0 ∈ [− π3 , 0], the trajectory type L− a Lβ Rβ Rb also needs to be considered. For this trajectory, the length parameter a, β and b satisfy:  0 0  xf = sin(θf ) + 2 sin(a + β) − 4 sin(a) 0 y = − cos(θf0 ) − 2 cos(a + β) + 4 cos(a) − 1 (22)  f b = a − θf0

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

Fig. 13. Example 1 for Case β = π2 : (a) the achieved optimal trajectory from (0, 0, 0) to (−4, 3, 1); (b) its geometric representation.

¯ ¯ ¯ ¯ ¯ ¯ ¯ ¯ where β ∈ [¯θf0 ¯ , π2 ] and ¯θf0 ¯ ≤ b ≤ |β|. If there are + + − + + − − two feasible trajectories L− a1 Lβ1 Rβ1 Rb1 and La2 Rβ2 Rβ2 Lb2 , choose the shorter one by comparing a1 + 2β1 + b1 and + − − a2 + 2β2 + b2 . If only one feasible trajectory L+ a Rβ Rβ Lb or + + − L− a Lβ Rβ Rb is obtained, then it is optimal since the domains + − − − + + − L+ a Rβ Rβ Lb and La Lβ Rβ Rb do not intersect with other domains. C. Case κ = 0 In this case, if θf ∈ [−π, 0), let us assume that the trajectory is Ra+ Rb− Rc+ , with a, b and c satisfying:   xf = sin(−θf ) − 2 sin(a + b) − 2 sin(a) yf = cos(−θf ) − 2 cos(a + b) + 2 cos(a) − 1 (23)  c = −a − b − θf with 0 < a, b, c < −θf0 . Similarly, if θf ∈ (0, π), let us assume that the trajectory is − + L+ a Lb Lc , with a, b and c satisfying:   xf = sin(θf ) − 2 sin(a + b) + 2 sin(a) yf = − cos(θf ) + 2 cos(a + b) − 2 cos(a) + 1 (24)  c = θf − a − b where 0 < a, b, c < θf . If a trajectory satisfying (23) or (24) can be obtained, then it is optimal. D. Practical implementation In the above three subsections it has been shown that ¡ three ¢ cases are to be considered: C1: β = π2 ; C2: β ∈ 0, π2 ; C3: κ = 0. To obtain a faster algorithm, a program has been developed to integrate these procedures in two ways, according to the final configuration (xf , yf , θf ). If xf > 2D or yf > 2D, the algorithm follows this reasoning: first, an optimal trajectory is selected for Case C1; if no solution is found, then search an optimal trajectory for Case C2; if still no optimal trajectory has been found, at last, determine the length parameters for a − + + − + trajectory type L+ a Lb Lc or Ra Rb Rc in Case C3. However, if xf < 2D and yf < 2D, the integrated order of these three cases is changed: first, select an optimal trajectory for Case 2; if no solution is found, then search the optimal trajectory for Case 3; if still no optimal trajectory has been obtained, then compute the optimal trajectory by the procedure for Case C1. A detail explanation for choosing 2D as a threshold of beginning conditions is in Section VII.B.

10

Fig. 14. Example 2 for Case β ∈ (0, π2 ): (a) the achieved optimal trajectory from (0, 0, 0) to (0.97, −1.4, 1.2); (b) its geometric representation where the orange dashed curves for the corresponding optimal trajectory ending inside Q− 1 and the blue solid curves for the optimal trajectory ending at the final configuration.

By the procedure for each case, we can obtain a trajectory which is guaranteed to be optimal, we can claim that our algorithm is complete, i.e. for two given configurations the algorithm can achieve an optimal trajectory to link them. VII. S IMULATION RESULTS AND ANALYSIS In the first part of this section, three simulation results are described to illustrate the three cases respectively. A discussion concerning the computational time is given in the second part. A. Simulation results Recall that the input of the algorithm is a final configuration, the initial configuration being the origin (0, 0, 0). Example 1: Final configuration:(−4, 3, 1). It gives (x∆ , y∆ ) = (−2.0721, −4.5504), then −sign(θf ) · x∆ · y∆ < 0, so ψf ≤ 0. From (19), it comes θ0 = −2.4285, ψs = 0.34582. We obtained (ϕs , χs , ψs ) = (0.6542, −0.7563, 0.3458) and (ϕf , χf , ψf ) = (−0.28299, −0.9591, −0.71701). The position and direction of the rotation of ωs and ωf , and θ1 are shown in Fig.13 (b), where θ1 = θ0 + π2 . From step 3 in the procedure for case C1, it comes a = mod(θs − θ1 ) = 0.8577 and b = mod(θf − θ1 ) − π2 = 0.28691. From step 4, σ = 3.0885 is computed as the length of straight line. Finally, − + the optimal trajectory is Ra− S− Lb , as drawn in Fig. 13 σ Lπ 2 (a). Example 2: Final configuration: (0.97, −1.4, 1.2), which − is inside Q+ 1 . Its corresponding configuration in Q1 is + + − (0.97, 1.4, −1.2). By solving (20), La Rβ Rb is obtained with a = 0.91905, β = 1.0757, b = 1.0434. Finally, the optimal − + + − trajectory Ra+ L+ β Lb is obtained by lr(La Rβ Rb ). Fig. 14 (a) shows the achieved optimal trajectory that links the origin to the final configuration. Fig. 14 (b) gives the geometric representations of the optimal trajectories with the orange dashed curves for the corresponding optimal trajectory ending inside Q− 1 and the blue solid curves for the optimal trajectory ending at the final configuration. Example 3: Final configuration: (−0.4, −0.4, −3). Since θf < 0, by solving (23), an optimal trajectory Ra+ Rb− Rc+ is obtained with a = 0.8094, b = 0.8666, c = 1.3239, as shown in Fig. 15.

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

Fig. 15. The optimal trajectory from (0, 0, 0) to (−0.4, −0.4, −3) for Example 3 in Case κ = 0

11

Fig. 16. Partitions of the configuration space for three cases with respect to θf

B. Analysis of the computation time Here we analyze the computational time of the proposed method. First, the computation time of the algorithm is investigated in each case. In Case C1 for β = π2 , the algorithm spends most time to find the numerical solution of (18) or (19), which can be fast achieved by the trust region dogleg method [29]. For Case C2, because as the analytic solutions of (20),(21),(22) can be obtained, the algorithm just needs to check whether the parameters a, β and b satisfy the range conditions at most three times. Similarly in Case C3, the algorithm just need to check whether the obtained a, β and b satisfy (23) or (24). Therefore, for each case, an optimal trajectory can be achieved in very low time. Let us consider now the computational time of the integrated algorithm and explain why 2D is taken as the threshold in the beginning condition of the program. Fig. 16 shows the partitions of the configuration space corresponding to the three cases. Because the restrictions of the partition of the configuration space to constant value of θf are different, three situations are represented in Fig. 16: θf = 0, θf = π2 and θf = π. The pentagon is the car. The areas included in the red curves are for Case C3 and the areas surrounded by the blue curves but outside of the red curves are for Case C2. All configurations located outside these areas are for case C1. The rectangle drawn by the dashed dot lines indicates the beginning condition for the algorithm, here 2D = 2. If the final configuration is outside the rectangle, the algorithm begins with Case C1, whereas inside the rectangle the algorithm begins with Case C2 or C3. The highest computational cost is paid only when the final configuration is in the dashed area, which is very small compared the whole configuration of the robot; and is no more than 3Tc , where Tc is the maximal time costed in three cases. So, in any case, the algorithm computes an optimal trajectory to the exact final configuration in real time in any modern computer. The low computation time of the algorithm is then the key point of the method. VIII. C ONCLUSION This paper describes the theoretical foundations of an effective algorithm that allows to determine a minimum-length trajectory between any two configurations of a car-like vehicle. This algorithm has three important features: first, it allows to compute exactly an optimal trajectory to drive the robot to a given terminal configuration, second it is complete, and

third it has a very low computational cost. The algorithm is based on the canonical representation of trajectories in the switching space. This original representation provides a simple geometric tool for describing the successive control switching along extremal curves. Such a canonical representation could be used to analyze the extremal trajectories of other optimal control problems for nonlinear systems having a similar lie algebra structure. From a practical point of view, this algorithm may be used as a basic tool for planning trajectories amidst obstacles. Indeed, as described in the introduction, the determination of optimal solutions in free space is at the basis of most part of nonholonomic motion planner. Using the proposed algorithm, this basis function can be solved exactly and at very low computational cost. ACKNOWLEDGMENT The authors would like to thank Drs. Rhoda E, and Edmund F. Perozzi for extensive editing and English language assistance. R EFERENCES [1] L. Dubins, On Curves of Minimal Length with a Constraint on Average Curvature and with Prescribed Initial and Terminal Positions and Tangents, American Journal of Mathematics, vol. 79 no.3, 1957, pp 497-516. [2] J. Reeds, L. Shepp, Optimal Paths for a Car that Goes Both Forwards and Backwards, Pacific Journal of Mathematics, vol. 145 no.2, 1990, pp 367-393. [3] H. Sussmann, G. Tang, Shortest Paths for the Reeds-Shepp Car: a Worked Out Example of the Use of Geometric Techniques in Nonlinear Optimal Control, Technical Report SYNCON 91-10, Dept˙of Mathematics, Rutgers University, Piscataway, NJ, 1991. [4] J.-D. Boissonnat, A. Cerezo, J. Leblond, ”Shortest paths of bounded curvature in the plane”, in Proceeding IEEE Int. Conf. on Robotics and Automation, Nice, France, 1992, vol.3, pp 2315-2320. [5] P. Sou`eres and J.-P. Laumond, Shortest Paths Synthesis for a Car-Like Robot. IEEE Transactions on Automatic Control, vol. 41, no. 5, 1996, pp 672-688. [6] P. Sou`eres, Minimum-Length Trajectories for a Car: An Example of the Use of Boltianskii’s Sufficient Conditions for Optimality. IEEE Transactions on Automatic Control, vol.52, no.2, 2007, pp 323-327. [7] H. Wang, Y. Chen and P. Sou`eres, ”An Efficient Geometric Algorithm to Compute Time-optimal trajectories for a Car-like Robot”, in Proceeding IEEE Int. Conf. on Decision and Control, New Orleans, LA, USA, 2007 (to be published). [8] S. Sastry, Nonlinear Systems: Analysis, Stability and Control, Springer Verlag; 1999.

SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS

[9] J-P. Laumond (Ed.), Robot Motion Planning and Control, Lecture Note in Control and Information Science No 229, Springer Verlag, Berlin 1998. [10] J.-P. Laumond, P. Jacobs, M. Taix, and R. Murray, A motion planner for nonholonomic mobile robots, IEEE Transactions on Robotics and Automation, vol. 10, no.5, 1994, pp 577-593. [11] S. LaValle, J. Kuffner, Randomized Kinodynamic Planning, The International Journal of Robotics Research, vol.20, no.5, 2001, pp 378-400. [12] D. Hsu, R. Kindel, J-C., Latombe and S. Rock, Randomized kinodynamic motion planning with moving obstacles, International Journal of Robotics Research, vol. 21, no. 3, 2002, pp 233-255. [13] T. M. Howard and A. Kelly, Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, The International Journal of Robotics Research vol. 26, no. 2, 2007, pp:141-166. [14] D. Conner, H.Choset and A. Rizzi, Integrated Planning and Control for Convex-bodied Nonholonomic systems using Local Feedback Control Policies, Tech. report CMU-RI-TR-06-34, Robotics Institute, Carnegie Mellon University, August, 2006 [15] D. Conner, A. Rizzi, and H. Choset, Towards Provable Navigation and Control of Nonholonomically Constrained Convex-Bodied Systems, in Proceedings of the 2006 IEEE International Conference on Robotics and Automation Orlando, USA, May, 2006 pp: 4416-4418. [16] P. Giordano, M. Vendittelli, J.-P. Laumond, P. Sou`eres, Nonholonomic Distance to Polygonal Obstacles for a Car-Like Robot Of Polygonal Shape, IEEE Transactions on Robotics, vol. 22, no. 10, 2006, pp 10401047. [17] J. C. Latombe, Robot Motion Planning, Springer, 1991. [18] H.Choset, K. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. Kavraki and S. Thurn, Principles of Robot Motion-Theory: Algorithms, Implementation, The MIT Press, 2005 [19] M. Chyba, S. Sekhavat, Time optimal paths for a mobile robot with one trailer, in Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, Kyongju, Korea, 1999 vol.3, pp 16691674. [20] M. Chyba, H. Sussmann, H. Maurer, G. Vossen, Underwater vehicles: the minimum time problem, in Proceedings of the 43rd IEEE Conference on Decision and Control, Atlantics, Bahamas, 2004, pp 1370-1375. [21] D. Balkcom, M. Mason, Time optimal trajectories for differential drive vehicles, The International Journal of Robotics Research, vol.21, no.3, 2002, pp 199-217. [22] S. Bhattacharya, R. Murrieta-Cid, S. Hutchinson, Optimal Paths for Landmark-Based Navigation by Differential-Drive Vehicles with Fieldof-View Constraints, IEEE Transactions on Robotics, vol. 23, no. 1, 2007, pp 47-59. [23] H. Chitsaz and S.LaValle, Minimum wheel-rotation paths for differential drive mobile robots among piecewise smooth obstacles, in Proceeding IEEE International Conference on Robotics and Automation, Rome, Itlay, 2007, pp 2718-2723. [24] S. LaValle, Planning Algorithms, Cambridge University Press, 2006. [25] L. Cesari, Optimization Theory and Applications, Springer-Verlag, New York, 1983. [26] D. Bertsekas, Dynamic Programming and Optimal Control, Volume I third Edition, Belmont, MA: Athena Scientific 2005. [27] A. Bressan, The generic local time-optimal stabilizing controls in dimension 3, SIAM J. Control and Opt., vol. 24, no.2, 1986, pp.177190. [28] B. Jakubczyk, and E. Sontag, Controllability of nonlinear discrete-time systems: a Lie-algebraic approach, SIAM J. Control and Opt., vol. 28 no.1, 1990, pp. 1-33. [29] J.J Mor´ e, B.S.Garbow, and K.E.Hillstrom, User Guide for MINPACK 1, Argonne National Laboratory, Rept.ANL-80-74, 1980

12