# Autonomous Path Planning for Road Vehicles in Narrow Environments: An Efficient Continuous Curvature Approach

Authors: Domokos Kiss; Gábor Tevesz
Journal: Journal of Advanced Transportation
 (2017)

---

## Abstract

In this paper we introduce a novel method for obtaining good quality paths for autonomous road vehicles (e.g., cars or buses) in narrow environments. There are many traffic situations in urban scenarios where nontrivial maneuvering in narrow places is necessary. Navigating in cluttered parking lots or having to avoid obstacles blocking the way and finding a detour even in narrow streets are challenging, especially if the vehicle has large dimensions like a bus. We present a combined approximation-based approach to solve the path planning problem in such situations. Our approach consists of a global planner which generates a preliminary path consisting of straight and turning-in-place primitives and a local planner which is used to make the preliminary path feasible to car-like vehicles. The approximation methodology is well known in the literature; however, both components proposed in this paper differ from existing similar planning methods. The approximation process with the proposed local planner is proven to be convergent for any preliminary global paths. The resulting path has continuous curvature which renders our method well suited for application on real vehicles. Simulation experiments show that the proposed method outperforms similar approaches in terms of path quality in complicated planning tasks.


---

## Body

## 1. Introduction

Looking around in the automotive industry, we can see a great amount of automation and an accelerating pursuit to develop intelligent self-driving vehicles. According to the popularity of the field, research is considerably active in the last decades [1–5]. The navigation task of autonomous robotic vehicles consists of many subproblems, involving sensing, perception and recognition of situations, localization, motion planning, and execution. Amongst these, we treat here the problem of generating appropriate paths for robotic cars in the presence of obstacles. As usual in case of cars, we assume the workspace of interest being a planar surface. From a control theoretic view, the rolling without slipping constraint of wheels induces nonholonomic constraints, which do not prohibit the controllability of a system but cause remarkable difficulties (it is enough to think of parallel parking a car). Although controlling a nonholonomic vehicle is a control theoretic challenge, it is worth incorporating knowledge about the specific system in the path planning task as well.This paper describes a path planning algorithm which is intended to solve navigation problems where the vehicle has to cross narrow or cluttered areas in order to reach the goal. Since cars have a nonzero minimum turning radius, such environments require in many cases nontrivial maneuvering (including more reversals) between narrowing. Our solution is appropriate for the continuous steering car, which means that the finite steering rate is taken into account as well beyond the turning radius. In other words, steering at standstill is avoided by forcing the path to have a continuous curvature profile. This is achieved by using clothoid curves between straight and circular path segments. The proposed algorithm is an approximation-based method consisting of a local and a global planner. The local planner generates feasible paths in the absence of obstacles. The global planner delivers a preliminary collision-free path by omitting the minimum turning radius constraint of the vehicle. It is shown that the proposed local and global planners are appropriate to be applied together and guarantee finding a feasible solution in the presence of obstacles if the planning task is solvable. Moreover, the local paths are designed to be similar to those a human driver would find.Although different human drivers and passengers prefer different types of paths, we try to summarize the main characteristics of a “human-like” path as follows. Existing optimal planners strive to return the shortest possible solution which results in sharp (mostly maximal curvature) turns and many reversals in most cases. In our opinion optimal length is not as important for a passenger as feeling comfortable. If a path is longer than optimal (to a meaningful extent, of course) but contains less sharp turns and reversals, then we treat it better from the passenger’s perspective. Our proposed approach is designed with these preferences in mind: the preliminary global path consists only of straight segments (with in-place turning between them), and the local planner has the task of connecting these segments with smooth and not necessarily maximal curvature turns.The remaining part of the paper is organized as follows. The next section summarizes related work in path planning for car-like vehicles. Section3 introduces the main characteristics of the proposed approach and the previous work of the authors in this field. In Section 4 we describe the local planning component (TTS-planner) in detail, in which we discuss the applied path elements, the properties of local paths, and a topological admissible steering method based on these. In Section 5 the global planning tool (RTR-planner) is described which generates the collision-free preliminary path. Finally, simulation examples are presented which confirm the advantageous characteristics of the proposed approach.


## 2. Related Work

The path planning task for nonholonomic systems is difficult, caused by the concurrent presence of global geometric (environmental) and local kinematic constraints. For this reason most available approaches decouple the treatment of these in two separate phases of the planning process. Basically we can distinguish between global and local path planning methods, which account for the two subproblems. In some approaches the configuration space of the vehicle is sampled and a graph-based description is used to grasp the topological structure of the environment [6]. For example, the Probabilistic Roadmap Method [7] uses general graphs with an offline sampling; the Rapidly Exploring Random Tree approach [8–10] builds one or more trees online to cover the free space. These sampling-based frameworks make use of a vehicle-specific local planner module (also denoted as asteering method), which accounts for connecting two arbitrary configurations of the nonholonomic car. Graph vertices represent configurations, while edges in the graphs correspond to local paths in these approaches. The global admissibility of these is checked by an appropriate collision detector module.Other approaches are based on the Holonomic Path Approximation Algorithm [11], which recommends planning a preliminary holonomic (kinematically unconstrained) path first and approximating it using a steering method in a second step. In this framework, the holonomic global path is iteratively subdivided and the steering method together with a collision detector is applied to replace the parts. To ensure the convergence of this algorithm, the steering method has to verify thetopologically property, which is defined in [12].Steering nonholonomic systems is a difficult task even in the absence of obstacles. Exact algorithms are not available in general but only for special classes, for example, nilpotentizable [13], chain-formed [14–16], and differentially flat systems [17]. Common examples for the above-mentioned system classes include wheeled vehicles with or without trailers. There exist elegant solutions of path planning and control especially for differentially driven and car-like robots [18–21] and mobile manipulators [22–24]. Most of the available work investigates differentially driven robots with one trailer [19] or more trailers [18, 20]. The steering methods presented in [19, 20] are shown to verify the topological property as well. Since the kinematics of a car is equivalent to a differentially driven robot towing a trailer hitched on top of the wheel axis, these flatness-based methods were shown to be applicable for cars [21] as well. In these approaches a sufficiently smooth planar curve is planned for the flat output of the system, which is the axle midpoint of the last trailer, or simply the rear axle midpoint in case of a single car. Although the method proposed in [21] is a simple and elegant way of generating local paths for cars with continuous curvature, it does not take into account the upper-bounded curvature constraint (except at the start and end configurations).An important and widely used family of steering approaches is based on optimal control. For general systems only approximate methods exist [25]. However, exact methods for computing optimal (e.g., shortest length) local paths are available for cars moving only forward (Dubins-car [26]) or both forward and backward (Reeds–Shepp-car [27–30]). For the Reeds–Shepp-car, it is shown in [27] that for any pair of configurations the shortest path can be chosen from 48 possible sets of paths, each of which consisting of maximum five circular or straight segments and having maximum two cusps. The number of sets was reduced to  46 in [28] and to  26 in [30]. Because these optimal paths consist of linear and circular segments, the resulting curvature profile contains abrupt changes, which can be traversed precisely only when the car is stopped at points where the curvature changes, in order to reorient its wheels. To overcome this limitation, a steering method is introduced in [31] which approximates Reeds–Shepp paths using continuous curvature turns, which contain clothoid curves to connect linear and circular parts. It was shown that although these paths are not optimal, they converge to the optimal Reeds–Shepp paths when the allowed sharpness of the clothoids tends to infinity. Both the Reeds–Shepp planner and its continuous curvature generalization (let us denote it as CCRS planner in the sequel) is proved to be topological admissible.Other continuous curvature planning approaches fall into the “path smoothing” class, where a sequence of configurations or a preliminary polygonal route is given and the goal is turning it to a smooth curve. These make use of clothoids [32] and higher order polynomial curves [33–35], or curves with closed form expressions such as Bézier curves [36, 37] or B-splines [38]. Unfortunately these are not treating issues such as topological admissibility or convergence guarantees in general planning tasks.Beyond these approaches, there are specialized algorithms, which were designed to solve a specific planning task, for example, parallel or garage parking. These are not suited to solve more general planning problems in case of arbitrary obstacle distributions. For example, [39, 40] use circle segments and straight lines to generate trajectories for car parking. In [40] these are denoted as translation (TM) and rotation movements (RM), and a number of TM–RM–TM combinations are linked together, depending on the maneuvering complexity of the parking task. An exhaustive search is performed in every step; thus the computational complexity increases heavily with the number of necessary maneuvers. The method of Müller et al. [41] proposes solving the parking task by the general two-step path approximation approach, where they use the CCRS local planner [31] and the global planner described in [42]. Although a general framework is applied, they exploit some simplifications and assumptions (e.g., regarding the shape of obstacles and symmetry of their distribution) in order to make computations tractable for real-time applications.A recent publication [43] introduces a planning approach which generalizes a parking planner to handle more difficult obstacle distributions, for example, navigation in a narrow parking lot containing some additional obstacles. It is shown that the algorithm can be applied in real time thanks to the fast optimization strategy it is based on. However, completeness or guaranteed convergence is not proven.


## 3. The RTR + TTS-Planner

Many of the mentioned planning methods were seminal for the development of our approach presented in this paper. Our goal was to design an algorithm with the following properties:(i)
It should solve the general planning task; that is, no assumptions are made regarding the obstacle distribution or the starting and goal configurations of the vehicle. At the same time, it should perform well also in the specialized parking situations.(ii)
The resulting paths should have continuous curvature profile.(iii)
The aim is to generate “human-like” paths, which are simple and contain a “meaningful” number of maneuvers needed by the task at hand.We chose the following methodology during the design of this planning method. A two-step approximation approach similar to [11, 41] was chosen; however, our global planner already takes some nonholonomic properties of the vehicle into account. The main contributions of this paper are the introduction of our novel topological admissible continuous curvature local planning method (TTS-planner) which facilitates the construction of “human-like” paths, as well as the detailed description of the global RTR-planner.This paper extends our previous work [45] giving a thorough theoretical treatment, detailed derivation, and proofs regarding the local planning method, as well as a more detailed description of the global planner.Previous versions of the proposed two-step planning approach can be found in [46–49], where the local planner used only straight and circular segments, resulting in a discontinuous curvature profile. The currently proposed generalized version uses clothoids as well (similarly to [31]). In the derivation we rely on the results of Wilde [50] regarding the parametrization of clothoids. Our approach is mostly comparable to the CCRS planner [31] or the two-step planning method [41] based on it, because they have similar theoretical properties. However, the simulation examples in Section 6 show that our local planner generates better quality paths and has a higher success rate in narrow environments than a CCRS-based approach.In the sequel we introduce the local TTS-planner first, by deriving it from the kinematic properties of the car and choosing path primitives with clothoids as building blocks. The global RTR-planner is introduced after that where we explain the reason of using only straight movement and in-place turning primitives in the preliminary planning phase.


## 4. Local Planning: The TTS-Planner

Before going into details of the local path planning method, we give mathematical definitions of the solvable problem. First the vehicle model is introduced, followed by basic properties of the feasible paths, which will serve as a basis for choosing the geometric building elements of the local planner module.


### 4.1. Vehicle Model

The geometrical model of a car can be seen in Figure1. In this approach we use the one-track (or bicycle) motion model of a continuous steering kinematic car [41]: (1)x˙y˙θ˙ϕ˙=cos⁡θsin⁡θ1Ltan⁡ϕ0v+0001Ω,where x and y are the position coordinates of the back axle midpoint (treated as the reference point of the model), θ is the orientation of the car, ϕ denotes the steering angle, and L is the distance of the front and rear wheel axles. The signed velocity of the reference point is v (positive means forward motion), and Ω stands for the steering rate. The absolute values of the steering angle and its rate of change are limited to (2)ϕ≤ϕmax<π2Ω≤Ωmax<∞.The instantaneous turning radius (the reciprocal of the path curvature κ) is given by (3)1κ=Ltan⁡ϕ.The steering angle limit implies an upper bound on the path curvature (4)κ≤κmax=tan⁡ϕmaxL.Figure 1
Kinematic model of a car.The more widespread form of the motion equation is the following [51]: (5)x˙y˙θ˙κ˙=cos⁡θsin⁡θκ0v+0001σ,where σ denotes the curvature change rate. The vector q=x,y,θ,κ is called the configuration (or state) of the vehicle and u=v,σ is the input vector of the system. The connection between (1) and (5) is given by (6)κ=tan⁡ϕLσ=κ˙=ϕ˙Lcos2⁡ϕ=ΩLcos2⁡ϕ.An upper bound on the curvature change rate can be expressed as (7)σ≤σmax=ΩmaxL≤ΩmaxLcos2⁡ϕ.


### 4.2. Feasible Paths

To elaborate the properties of feasible local paths of our car model, let the translational velocity of the car be written asv=γv, where v stands for the magnitude of the velocity and γ represents the local direction of motion: (8)γ=1,ifv≥0(forward  motion),-1,ifv<0(backward  motion).Furthermore, let us introduce the translational displacement of the car along its path (9)st=∫0tvτdτand express the translational velocity as (10)v=γdsdt.Using (10) and the chain rule of differentiation, we reformulate the motion equation (5) as follows: (11)ddsxyθκ·dsdt=cos⁡θsin⁡θκ0γdsdt+0001σ.After dividing by (10) and defining (12)αs=σv=γdκdss=d2θds2s,thesharpness of the path, we get the spatial description of feasible paths: (13)γddsxyθκ=cos⁡θsin⁡θκ0+0001α.We can establish the following properties of these paths for our car model:(i)
The orientationθ at s is constrained by the local displacements along the x- and y-axes and by the motion direction γ. The path curvature κ is given by the first (spatial) derivative of θ and the motion direction. Thus every feasible path can be fully described by a planar curve labeled by the motion direction at every point. This fact actually arises from the differential flatness property of the car with the rear axle midpoint position as flat output [17, 21].(ii)
The sharpness of the path is the second derivative ofθ (the spatial change rate of the curvature along the path) and determines the maximal allowed velocity at every point on the path: (14)vs≤σmaxαs.If a nonzero minimal traveling speedvmin is required, then the sharpness has an upper bound:(15)αs≤αmax=σmaxvmin.In other words, if we would like to obtain a path which can be followed by the continuous steering car (1) and we allow the car to stop only if the motion direction is reversed, then the path should be a planar curve with(i)
bounded curvature and(ii)
bounded sharpness (continuous curvature).In Sections4.3, 4.4, and 4.5 we are about to present specific paths fulfilling these requirements and the local path planner for generating such paths.


### 4.3. Path Elements

Our aim is to choose geometric primitives which can serve as building elements in the process of path construction. We assume that the motion direction is the same along a primitive, but it can change between the primitives (which means a cusp in the path). The most simple planar curves with bounded curvature are straight lines (κ=0, called S segments in the sequel) and curvature-limited circular arcs (κs=const≤κmax, called C segments). Paths constructed by these primitives have piecewise constant curvature with abrupt curvature changes (i.e., infinite sharpness) between the pieces. To obtain continuous curvature, we are about to useclothoids, which are the most simple parametric curves with changing curvature and bounded sharpness.


#### 4.3.1. Clothoids

A clothoid is a curve with constant sharpness(16)αs=α≤αmax<∞,∀s.According to (13), the equations describing a clothoid path can be obtained by integration. If we assume that q(0)=0, we get (17a)xs=γπαCFαπs,(17b)ys=γsgn⁡α⁡παSFαπs, where sgn⁡α denotes the sign of α and CF and SF stand for the Fresnel cosine and sine integrals: (18a)CFr=∫0rcos⁡π2μ2dμ,(18b)SFr=∫0rsin⁡π2μ2dμ.


#### 4.3.2. Clothoid Types

From the point of view of path planning, eight different types of clothoids can be distinguished, based on the following properties:(i)
Sign of the sharpness:sgn⁡α=γsgn⁡dκ/ds, it can be positive (P,sgn⁡α=1) or negative (N,sgn⁡α=-1). Zero sharpness is omitted because it results in a straight or circular segment instead a clothoid.(ii)
Direction of motion:γ, it can be forward (F) or backward (B)(iii)
Sign of the absolute curvature change:sgn⁡dκ/ds. The absolute curvature grows if the vehicle is entering a turn (calledin-type clothoid) and vanishes if it returns to a straight segment (out-type clothoid).Equations (17a) and (17b) describe in-type clothoids parametrized by the constants α, γ and the running variable s∈0,sCC, starting from qin0=0 and arriving at some qinsCC, where sCC denotes the whole curve length. From every in-type clothoid we can derive an out-type counterpart by preserving the same geometry but reversing the motion direction: (19a)xouts=-γπαCFαπsCC-s,(19b)youts=-γsgn⁡απαSFαπsCC-s. Note that these out-type clothoids start from some qout0=qinsCC and arrive at qoutsCC=0. The eight different types of clothoids are illustrated in Figure 2.Figure 2
Clothoid types.Note that we deal here only with such clothoid path segments which have either their starting or their final configuration at the origin with zero orientation and curvature (q(0)=0 for in-type and q(sCC)=0 for out-type clothoids). However, this causes no serious limitations. On the one hand, any q=(x,y,θ,0) can be transformed to q′=(0,0,0,0) by translation and rotation. On the other hand, κ(0)=0 or κ(sCC)=0 means that our final concatenated paths can consist of S segments, CCin-C-CCout triplets, and CCin-CCout pairs. Practically, the only limitation is that transitions between two C segments of different curvatures are allowed only with a curvature profile crossing or touching zero, that is, by a C-CCout-CCin-C sequence. As we will see later, this does not prohibit the creation of a complete path planner.From now on, according to the nomenclature of [31], we will denote CCin-C-CCout triplets asCC-turns or T segments, and CCin-CCout pairs aselementary paths or E segments. Note that an elementary path is a special case of a CC-turn.


#### 4.3.3. Reparametrization of Clothoids

By looking at (17a) and (17b), we can state that any point on an in-type clothoid starting from q0=0 can be described by the pair α,γs, the sharpness and the signed displacement along the curve. It was shown by Wilde [50] that an equivalent representation is available using the pair κ,δ, the curvature and the deflection of the curve at a certain point: (20a)xκ,δ=sgn⁡δ2πδκCF2δπ,(20b)yκ,δ=2πδκSF2δπ.As it can be seen, κ appears only as a multiplier in the expressions; thus it can be interpreted as a scaling factor. In this representation δ is responsible for the “shape” of the clothoid segment, and κ determines its “size.”From a computational perspective, thisκ,δ parametrization is less convenient than the parametrization by α,γs. The reason is that α and γ are constant values and only s is variable along the path segment; thus the curve is described by some fixed parameters and exactly one free parameter, as usual. This makes the explicit computation of these curves simple, for example, for visualization or collision checking. Opposed to this, both κ and δ are varying along the curve, and coherency has to be maintained between them to obtain proper results. The coherency is ensured by the constant sharpness, which is related to κ and δ as (21)α=κ22δ.For simplicity, let us choose fixed curvature and deflection values κCC and δCC at the endpoint of curve. This is the point with maximal absolute curvature and deflection: (22a)κCC=arg⁡maxκ⁡κ,(22b)δCC=arg⁡maxδ⁡δ.Based on (20a), (20b), (22a), and (22b), in-type clothoids can be fully described by (23a)xinδ=sgn⁡δCC2πδCCκCCCF2δπ,(23b)yinδ=2πδCCκCCSF2δπ,(23c)θinδ=δ,(23d)κinδ=κCCδδCC,where δ∈0,δCC is the variable parameter and δCC and κCC are constant values. A shorter form for xinδ and yinδ can be obtained by defining the following functions: (24)Xβ=sgn⁡βπβ·CFβπ(25)Yβ=πβ·SFβπ.Using these, (23a), (23b), (23c), and (23d) will look like (26a)xinδ=X2δκCCδCCδ=X2δκinδ,(26b)yinδ=Y2δκCCδCCδ=Y2δκinδ,(26c)θinδ=δ,(26d)κinδ=κCCδδCC.Note that if we look at the endpoint, X2δCC and Y2δCC can be treated as the end position of and “unscaled” in-type clothoid of δCC full deflection. The “scaling factor” 1/κCC can be used to obtain the real endpoint coordinates xinδCC and yinδCC.Now, let us consider a pair of in-type and out-type clothoids having the same geometry (such pairs were shown in Figure2, e.g., CCinPF and CCoutPB). In order to ensure the above-mentioned coherency between κCC and δCC, we define them according to (22a) and (22b) for out-type clothoids as well. However, in this case κCC belongs to the starting point, while δCC to the endpoint of the curve. Since an out-type clothoid has the same geometry as its in-type pair but opposite motion direction, we can obtain its equation as (27a)xoutδ=-sgn⁡δCC2πδCCκCCCF2δ-δCCπ,(27b)youtδ=2πδCCκCCSF2δ-δCCπ,(27c)θoutδ=δ-δCC,(27d)κoutδ=κCCδCC-δδCC.


#### 4.3.4. Elementary Paths and CC-Turns

As already mentioned, elementary paths (E segments) are symmetric curves, consisting of a pair of compatible CCin and CCout clothoids. The curvature of the first curve changes from 0 to κCC, while the second from κCC back to 0. Both curves have the same deflection δCC. Compatibility is assured by the same motion direction γ and the same curvature κCC at the end of the in-type part and at the beginning of the out-type part. The compatible pairs are(i)
CCinPF and CCoutNF,(ii)
CCinNF and CCoutPF,(iii)
CCinPB and CCoutNB,(iv)
CCinNB and CCoutPB.A CC-turn consists of a pair of compatibleCCin and CCout curves, with an additional circular arc between them, which has κC=κCC curvature and δC deflection. As mentioned before, an elementary path is a special case of a CC-turn with δC=0 (see Figure 3).Figure 3
Structure of (a) elementary paths and (b) CC-turns.
(a)
(b)Let us characterize these path components. They can be described by a parametric configuration curveqδ=xδ,yδ,θδ,κδ with δ∈0,δend. The curve is fully determined by the following parameters:(i)
A starting configurationq0=qs=xs,ys,θs,0.(ii)
The maximal curvatureκCC (in an absolute sense: κCC≥κδ,∀δ).(iii)
Deflections of the clothoid (δCC) and circular (δC) parts.We use the following building elements to synthesize elementary paths and CC-turns:(i)
An in-type clothoidqinδ of deflection δCC, starting from qin(0)=(0,0,0,0) and arriving at qinδCC=xinδCC,yinδCC,δCC,κCC, as described by (23a), (23b), (23c), and (23d).(ii)
A circular arcqcδ of deflection δC and curvature κCC, starting from qc0=0,0,0,0, described by (28a)xcδ=sin⁡δκCC(28b)ycδ=1-cos⁡δκCC(28c)θcδ=δ(28d)κcδ=κCC.(iii)
A compatible out-type clothoidqoutδ of deflection δCC, starting from qout0=xout0,yout0,-δCC,κCC and arriving at qoutδCC=0,0,0,0, as described by (27a), (27b), (27c), and (27d).The starting and end configurations of these parts have to be aligned in order to obtain the resulting elementary path or CC-turn. The alignment is performed by rotation and translation, described by homogeneous transformation matrices. Let us assign a transformation matrix to any configurationq0 as follows: (29)q0=x0y0θ0κ0⟶Tq0=cos⁡θ0-sin⁡θ000x0sin⁡θ0cos⁡θ000y00010θ00001000001.This transformation matrix represents the pose of the local coordinate frame attached to q0. The transformation of another configuration q by Tq0 can be performed as usual: (30)q′1=Tq0q1.This means that we interpret the coordinates of q as local, relative to q0. Multiplying by Tq0 we get q′, which represents the global coordinates of the transformed q. Notice that the transformation does not affect the curvature part of the configuration. It follows that the original configuration q0 can be obtained by applying the transformation Tq0 to q=0,0,0,κ0: (31)q01=Tq0000κ01.The inverse transformation is given by(32)T-1q0=cos⁡θ0sin⁡θ000-x0cos⁡θ0-y0sin⁡θ0-sin⁡θ0cos⁡θ000x0sin⁡θ0-y0cos⁡θ00010-θ00001000001.These transformation matrices have the following chaining property: (33)q′H=Tq0·qH⟺Tq′=Tq0·Tq,where the upper “H” index stands for the homogenous coordinate representation qH=q,1T.Now, we have all the tools to assemble CC-turns and elementary paths. A general description of these is obtained as follows:(34a)qHδ=Tqs·qHinδ,ifδ∈0,δCC,(34b)qHδ=TqδCC·qHcδ-δCC,ifδ∈δCC,δCC+δC.(34c)qHδ=TqδCC+δC·T-1qout0·qHoutδ-δCC-δC,ifδ∈δCC+δC,2δCC+δC,where qs stands for a given arbitrary starting configuration with zero curvature and qinδ and qoutδ are given by (23a), (23b), (23c), (23d), (27a), (27b), (27c), and (27d), respectively. This can be treated as a “direct” approach for generating a CC-turn or elementary path. One specifies the starting configuration qs, the maximal curvature κCC, and the deflections δCC and δC, and the end configuration qe=q2δCC+δC is obtained directly using (34a), (34b), and (34c). Without loss of generality we can assume that qs=0,0,0,0. In this case we get (35a)qHδCC=qHinδCC=TqinδCC·0H,(35b)qHδCC+δC=TqinδCC·qHcδC=TqinδCC·TqcδC︸TqδCC+δC·0H,(35c)qeH=qH2δCC+δC=TqδCC+δC·T-1qout0︸Tqe·qHoutδCC=Tqe·0H,because the endpoint of a standard out-type clothoid is the origin, according to (27a), (27b), (27c), and (27d). The components of (36)Tqe=TqinδCC·TqcδC·T-1qout0have the following form: (37)TqinδCC=cos⁡δCC-sin⁡δCC00X2δCCκCCsin⁡δCCcos⁡δCC00Y2δCCκCC0010δCC0001000001TqcδC=cos⁡δC-sin⁡δC00sin⁡δCκCCsin⁡δCcos⁡δC001-cos⁡δCκCC0010δC0001000001.In order to obtain T-1qout0, we express qout0 first: (38)qout0=-xinδCCyinδCC-δCCκCC=-X2δCCκCCY2δCCκCC-δCCκCC,and it follows from (32) that (39)T-1qout0=cos⁡δCC-sin⁡δCC00t1,5sin⁡δCCcos⁡δCC00t2,50010δCC0001000001,where(40)t1,5=X2δCCκCCcos⁡δCC+Y2δCCκCCsin⁡δCC,t2,5=X2δCCκCCsin⁡δCC-Y2δCCκCCcos⁡δCC.After multiplying the three matrices we get (41)Tqe=cos⁡δend-sin⁡δend00A2δCC,δCκCCsin⁡δendcos⁡δend00B2δCC,δCκCC0010δend0001000001,where(42)δend=2δCC+δC,(43)A2δCC,δC=X2δCC1+cos⁡2δCC+δC+Y2δCCsin⁡2δCC+δC+sin⁡δCC+δC-sin⁡δCC,(44)B2δCC,δC=X2δCCsin⁡2δCC+δC+Y2δCC1-cos⁡2δCC+δC-cosδCC+δC+cosδCC.The end configuration of the CC-turn is given by the elements in the last column of T(qe): (45a)xe=A2δCC,δCκCC.(45b)ye=B2δCC,δCκCC(45c)θe=2δCC+δC(45d)κe=0It is important to note that, in order to fully define a CC-turn, two of the deflectionsδCC, δC, and δend should be given, together with the maximal curvature κCC. The third deflection parameter is determined by (42).The equations of an elementary path are similar to (45a), (45b), (45c), and (45d); we only need to substitute δC=0: (46a)xe=A2δCC,0κCC=A2δCCκCC,(46b)ye=B2δCC,0κCC=B2δCCκCC,(46c)θe=2δCC,(46d)κe=0,where(47)A2δCC=X2δCC1+cos⁡2δCC+Y2δCCsin⁡2δCC,B2δCC=X2δCCsin⁡2δCC+Y2δCC1-cos⁡2δCC.


### 4.4.TTS Paths for Local Planning

Algorithms that generate feasible local paths for a wheeled system—such as our car model (5)—in the absence of obstacles are called local planners or steering methods. The exact definition of these is the following.Definition 1 (steering method).
Let us denote byΛf the set of all feasible local paths λf regarding system equation q˙=fq,u. A steering method (or local planner module) is a function(48)Steer:C×C⟶Λf,i.e.,λf·=Steerq0,q1such that(49)λf0=Steerq0,q10=q0,λfSλ=Steerq0,q1Sλ=q1.As already mentioned in Section2, the most widely used local planners for the simple kinematic car model (without the requirement of curvature continuity) are based on the optimal length Reeds–Shepp (RS) paths [27]. These local paths consist of maximum five circular or straight segments and have at most two cusps, where the curvature of circular arcs is exactly ±κmax. If the continuous steering car model (5) is used, the continuous curvature generalization proposed in [31] can be applied. This approximates Reeds–Shepp paths by replacing circular segments with CC-turns. It was shown that although the resulting CCRS paths are not optimal, they converge to the optimal RS paths when the allowed sharpness of the clothoids tends to infinity. The CC-turns in this approach consist of clothoids having the maximal allowed sharpness and circle segments of maximal curvature.In the sequel we are doing something similar: introduce a new set of local paths for the continuous steering car. The main ideas which motivated the invention of our approach instead using the CCRS paths are the following:(i)
The main goal is global planning in the presence of obstacles. Most existing planning methods use a concatenation of local paths, which has the consequence that the result will be not optimal, even if the parts were generated by an optimal planner. Even the CCRS paths are suboptimal. Thus, having the final application in our mind, we lifted the requirement of optimality of our local paths.(ii)
Instead minimizing the global path length, we had the goal to achieve “natural” or “human-like” paths. What this means is informally described in the following points.(iii)
The RS (resp., CCRS) paths consist of up to five circular (resp., CC-turn) and straight segments. We show in the sequel that three path segments are sufficient to connect any two configurations.(iv)
The RS (resp., CCRS) paths use circular segments (resp., CC-turns) having the maximal allowed curvature (the steering wheel is turned to the end position in every curve). Our planner is not restricted to this. Only the limit is taken into account, but smaller curvatures are allowed.(v)
The last segment of RS (resp., CCRS) paths is always circular (resp., CC-turn). We argue that this contradicts the behavior of human drivers. Because the car can only move forward or backward, in most cases the more natural behavior is to arrive at a given goal configuration by a straight movement. There are a few exceptions, for example, the case of parallel parking, but these can usually be transformed to the mentioned case by reversing the roles of initial and goal configurations. Consequently, we choose a straight segment as the last section of our local paths.To simplify notation in the sequel, we will denote local paths as a concatenation ofS, T, and E letters, corresponding straight segments, CC-turns, and elementary paths, respectively. In our approach we use TTS (as well as EES) paths (note that in our previous work [45] we used the notations T∗TS and E∗ES where T∗ and E∗ represented “CC-turn or straight segment” and “elementary path or straight segment,” although we omit here the possibility of having a straight segment as the first element because our experiments showed that STS and SES paths are chosen very rarely by the implemented planning algorithm). In the sequel we present the construction of EES paths first (where we use only elementary paths in place of CC-turns) and show that this class of paths is sufficient to connect an arbitrary pair of configurations. After that we generalize E segments to CC-turns.


#### 4.4.1. Construction ofEES Paths

Let us assume without loss of generality that the path starts from a given initial configurationqI=xI,yI,θI,0 and arrives at the origin as goal configuration qG=0,0,0,0. Any query pair qI,qG can be transformed to this form by translation and rotation, and the designed path can be transformed back. Let us denote the intermediate configurations between the path segments by q~I and q~G. For every segment, i denotes its index (1, 2 or 3) inside the path. A straight segment is parametrized by its absolute length si and direction γi and an elementary path segment by its curvature κi,CC and deflection δi,CC at the middle point between the CCin and CCout parts. To simplify the notation, let us neglect the “CC” subscript from now on: let κi=κi,CC and δi=δi,CC.The scheme of anEES path (illustrated in Figure 4) is the following: (50)qI⟶δ1,κ1Eq~I⟶δ2,κ2Eq~G⟶s3,γ3SqG.Figure 4
EES path.The coordinates ofq~I can be obtained from qI by a transformation belonging to the first elementary path (51)q~IH=TqI·Tqeδ1,κ1·0H,where(52)Tqeδ1,κ1=cos⁡2δ1-sin⁡2δ100A2δ1κ1sin⁡2δ1cos⁡2δ100B2δ1κ100102δ10001000001,TqI=cos⁡θI-sin⁡θI00xIsin⁡θIcos⁡θI00yI0010θI0001000001.By applying the matrix product we obtain(53a)x~I=A2δ1κ1cos⁡θI-B2δ1κ1sin⁡θI+xI(53b)y~I=A2δ1κ1sin⁡θI+B2δ1κ1cos⁡θI+yI(53c)θ~I=2δ1+θI(53d)κ~I=0.Let us define the auxiliary functions (54)Cβ,ψ=Aβcos⁡ψ-Bβsin⁡ψDβ,ψ=Aβsin⁡ψ+Bβcos⁡ψand write (53a), (53b), (53c), and (53d) in a shorter form: (55a)x~I=C2δ1,θIκ1+xI(55b)y~I=D2δ1,θIκ1+yI(55c)θ~I=2δ1+θI(55d)κ~I=0.Alternatively, we can getq~I from q~G=x~G,0,0,0 by “traveling backward” along the elementary path. This corresponds to a normal E segment with inverse deflection. We can obtain q~I this way by applying the transformation (56)q~IH=Tq~G·Tqe-δ2,κ2·0H,where Tqe-δ2,κ2 is the same as (41) with parameters δCC=-δ2 and δC=0: (57)Tqe-δ2,κ2=cos⁡2δ2sin⁡2δ200A-2δ2κ2-sin⁡2δ2cos⁡2δ200B-2δ2κ20010-2δ20001000001and Tq~G looks like (58)Tq~G=1000x~G01000001000001000001.The result of the transformation is (59a)x~I=A-2δ2κ2+x~G(59b)y~I=B-2δ2κ2(59c)θ~I=-2δ2(59d)κ~I=0.By comparing (55a), (55b), (55c), (55d), (59a), (59b), (59c), and (59d) we can conclude that the path parameters δ1, κ1, δ2, κ2, and x~G=-γ3s3 of an EES path fulfill the following constraints: (60a)C2δ1,θIκ1+xI=A-2δ2κ2-γ3s3(60b)D2δ1,θIκ1+yI=B-2δ2κ2(60c)2δ1+θI=-2δ2.One can conclude that in anEES planning problem only one path segment can be parametrized freely; the others are determined. In the sequel we will use δ1 and κ1 as free parameters.Let us pay some attention to the ranges of angle parameters in the above equations. The orientation parameterθI is treated as being in the interval -π,π/~, where “/~” means that the two boundary values are identified. Similarly to the geometric derivation of CC-turns, we introduce an upper bound δend,i≤π on the whole deflection of both elementary paths, which results in δi∈-π/2,π/2. In order to ensure that δ1, δ2, and θI being in the given intervals fulfill the constraint (60c), we have to choose the set carefully from which δ1 and δ2 can be taken. Let us examine two cases:(i)
θI∈[0,π]: choosing δ1∈-π/2,π/2-θI/2⊆-π/2,π/2 results in (61)δ1+θI2=-δ2∈-π2+θI2,π2⊆-π2,π2.(ii)
θI∈[-π,0]: choosing δ1∈-θI/2-π/2,π/2⊆-π/2,π/2 results in (62)δ1+θI2=-δ2∈-π2,π2+θI2⊆-π2,π2.These choices ensure both δ1 and δ2 being in -π/2,π/2 for any values of θI. We can write these conditions in a single compact form:(63)δ1,δ2∈IδθI=-sgnp⁡θIπ2,sgnp⁡θIπ2-θI2,where sgnp⁡· is a special sign function with sgnp⁡0=1: (64)sgnp⁡x=sgn⁡x,ifx≠01,ifx=0.An illustration of this set can be seen in Figure 5.Figure 5
Set of valid(δ1,δ2) pairs forEES paths.
(a)
(b)Up to now we have not taken the curvature bound into account. It can be proven that an arbitrary configuration pair can be connected by anEES path even if an upper bound on the absolute curvature is given. The following Lemma states this fact; the proof is elaborated in Appendix A.Lemma 2.
In the absence of obstacles, anyqI,qG pair of initial and goal configurations of a continuous steering car can be connected by an EES path.Furthermore, it turns out that infinitely many solutions can be found even with bounded curvature, due to the infinitely many choices ofδ1 and κ1 parameters. This means that q~I can be chosen in infinitely many ways, as illustrated in Figure 6.Figure 6
There is an infinite number ofTTS solutions between two configurations.


#### 4.4.2. FromEES toTTS Paths

As we have already seen in Section4.3.4, elementary paths are special CC-turns with δC=0. It follows that the results for elementary paths used in EES planning can be generalized to CC-turns as well. The equations of an elementary path starting from any q~I and arriving at the x-axis—the middle segment of an EES path—are given by (59a), (59b), (59c), and (59d). This can easily be generalized to the case when the deflection of the circular part is nonzero: (65a)x~I=A-2δ2,CC,-δ2,Cκ2+x~G(65b)y~I=B-2δ2,CC,-δ2,Cκ2(65c)θ~I=-2δ2,CC-δ2,C(65d)κ~I=0,where δ2,CC and δ2,C stand for the deflections of the clothoid and the circular parts, respectively. The full deflection of the CC-turn is (66)δ2,end=2δ2,CC+δ2,C=-θ~I.Let us introduce the relative amount of the circular part of a CC-turn as (67)Rδ=δCδend.Using this, we can express (65b) as (68)y~I=B1-Rδ,2δ2,end,Rδ,2δ2,endκ2.The maximal curvature and the sharpness of the CC-turn are given by (69a)κ2=1y~IB1-Rδ,2δ2,end,Rδ,2δ2,end(69b)α2=κ222δ2,CC=B21-Rδ,2δ2,end,Rδ,2δ2,endy~I21-Rδ,2δ2,end.Note that if we fix the full deflection δ2,end and change the ratio Rδ,2 only, then for a given q~I the end configuration q~G on the x-axis remains the same; only κ2 and α2 are affected. In other words, we can tune the shape of a CC-turn with Rδ without affecting the starting and final configurations. This is illustrated in Figure 7, where clothoids are drawn with solid black lines and the red dashed arcs represent circular segments.Figure 7
Effect ofRδ on the shape of a CC-turn (Rδ is modified in steps of 0.1).WhenRδ grows, the curvature is decreasing but the sharpness is increasing at the same time, independently from the full deflection of the path segment. This property can be examined in Figure 8, which shows surface and contour plots of the (absolute) curvature and sharpness of a CC-turn (with y~I=1), depending on the full deflection and the relative amount of the circular part.Figure 8
Dependence of (a)-(b) maximal curvature and (c)-(d) sharpness of CC-turns on the full detection and relative amount of the circular part (absolute values are depicted).
(a)
(b)
(c)
(d)It follows that if we already have obtained anE segment between any two configurations, we can reshape it to a CC-turn having smaller curvature and higher sharpness by tuning the Rδ parameter. With this, we can define a maximal sharpness αmax beyond the already treated κmax constraint. If the sharpness of the initial E segment is less than αmax, then there exists an Rδ value such that α=αmax. This is beneficial because the length of the path segment is decreasing, as can be seen in Figure 7.If we have anEES path, we can do this reshaping for every E segment to obtain a TTS path. However, if any of the E segments violates the αmax constraint, then it is not so straightforward. Fortunately it can be shown that a TTS solution obeying the αmax constraint exists in all cases. If the second segment in the EES solution violates the sharpness constraint, we can write using (59c) and (69b) and (A.3) in Appendix A: (70)κmax′=αmaxθ~I(71)y~I≥Bθ~Iκmax′,where κmax′ is the minimal value of the curvature upper bound ensuring that the second elementary path will have α≤αmax. If we would like to obtain a TTS path from an EES solution, then by recalling (55b) and (A.6) in Appendix A we can state that κ1 can be decreased arbitrarily in order to fulfill condition (71) for the second segment and the maximal sharpness constraint for the first segment at the same time.The only drawback of usingTTS paths instead EES solutions is that the sharpness tuning requires an iterative search of Rδ in the interval [0,1]. This cannot be avoided, because Rδ is in the argument of the B(·,·) function in (69a) and (69b), which does not have an inverse because of the contained Fresnel integrals.


### 4.5. A Steering Method Based onEES Paths

ATTS path planner based on the conditions in proof of Lemma 2 and their generalization in Section 4.4.2 is not a steering method itself (in the sense of Definition 1), because it delivers not only one solution for a query pair. Instead of this, it delivers a parametrized class of solutions, which contains an infinite number of EES (and TTS) paths from qI to qG, fulfilling the maximum curvature (and sharpness) constraint. This property is useful in global planning problems (i.e., in the presence of obstacles), because one can more likely find a collision-free path from a solution class than from the one and only solution of an exact local planner.This does not mean that in the presence of obstacles the existence of any collision-free (local) solution would be guaranteed. For successful global planning explicit reasoning about free space connectivity is necessary. As mentioned in Section3, we apply the framework of the Holonomic Path Approximation Algorithm [11] for this purpose. A distinct global planner delivers a preliminary geometric path, which is iteratively subdivided and approximated by an appropriate steering method. To ensure the convergence of this algorithm, the local planner has to generate such paths that get closer to the original path as the local endpoints get closer to each other. This is called thetopological property [12] which is formulated more precisely in Appendix B.Therefore, if a collision-free geometric path is given and we have an appropriate steering method that verifies the topological property, then a complete (completeness means that the algorithm does not fail to return a solution if the problem is solvable) approximation algorithm can be constructed (which was a footnote in the manuscript). Hence, in order to design such a complete algorithm based onTTS paths, we need a steering method which returns exactly one from the class of TTS paths and verifies the topological property.


#### 4.5.1. Definition ofee-S Paths

In the sequel we take onlyEES paths into account for the purpose of designing the exact steering method. This choice will make possible proving the topological property, because EES paths have closed form expressions. In contrast to that, TTS paths with a given sharpness can only be obtained by iterative deriving from EES solutions, as seen in Section 4.4.2.More possibilities can be found to restrict the class ofEES paths to exactly one specific solution for every query pair (qI,qG). For example, we may choose the sign of κ2 and the equality in condition (A.4) in Appendix A and fix δ1 to any chosen value except 0 and -θI±π. In this case the remaining parameter κ1 is determined unequivocally. This would result in an EES path with κ2=κmax and having the intermediate orientation θ~I=θI+2δ1 at the end of the first E segment. This approach has the problem that it is hard to formulate any reasonable direct rule for choosing δ1 and the sign of κ2 for a given query. Instead of this we introduce the following approach. Let us choose κ1=-κ2 such that κ1=κ2 is maximal. In this case we do not have to specify any other parameters. The resulting EES path has two E segments with same maximal absolute curvature, but having opposite signs (the steering wheel is turned from right to left or reversely). This type of paths is denoted as ee-S paths in the sequel to emphasize that the resulting turning radii (reciprocal of the curvature) are opposite, equal, and minimal. The ee-S planning problem is formulated as follows: (72)qI⟶δ1,κ1Eq~I⟶δ2,κ2Eq~G⟶s3,γ3SqGsuch that (73)qI=xI,yI,θI,0,qG=0,0,0,0,(74)κ≔κ1=-κ2,(75)κ≤κmax,(76)κ⟶max.Using (74) we can reformulate (60a), (60b), and (60c) as (77a)xI+γ3s3+A2δ1+θI+C2δ1,θIκ=0(77b)yI+B2δ1+θI+D2δ1,θI︷G2δ1,θIκ=0(77c)δ2=-δ1-θI2.The necessary and sufficient condition for existence of this kind ofEES paths is obtained by (75) and (77b): (78)G2δ1,θI=yI·κ≤yI·κmax.In the sequel we use some properties of the G(·,·) function, which are established in Appendix C.Lemma 3.
For anyyI,θI there exists δ1 such that G2δ1,θI≤yIκmax.Proof.
According to PropertiesC.6 and C.7 of the G function we can state that for all θI there exist δ1 values for both G2δ1,θI≠0 and G2δ1,θI=0. Since G is a continuous function, this means that, for all θI, G2δ1,θI can take an arbitrarily small value.


#### 4.5.2. Theee-S Steering Method

Based on the existence condition (78) and the optimization criterion (76) we can construct a steering method which generates a single ee-S path for any local planning query. We denote it as the ee-S-planner.Let us examine the special caseyI=0 first, where the existence condition (78) takes the form (79)G2δ1,θI=0.According to the Proof of Property C.7 in Appendix, we can state that G2δ1,θI has a zero for all θI in the interval δ1∈-θI/2,0. Let us denote this δ1 solution as δ1,z: (80)δ1,zθI:δ1∈-θI2,0∣G2δ1,θI=0In this case κ could be chosen arbitrarily, but the condition (76) suggests the choice of κ=±κmax.IfyI≠0 then it follows that (81)κ=-G2δ1,θIyI,and δ1 which maximizes κ is defined by (82)δ1∗θI=arg⁡maxδ1∈IδθI⁡G2δ1,θI.Let us define κ∗ as (83)κ∗yI,θI=-G2δ1∗θI,θIyI,which can be treated as the unbounded optimum of κ. Since G has a maximum at δ1∗ and zero at δ1,z, an arbitrary κ∈0,κ∗ can be achieved by δ1∈δ1,z,δ1∗. It follows that if κ∗yI,θI>κmax, then a δ1,maxyI,θI can be found between δ1∗θI and δ1,zθI such that κ=κmax: (84)δ1,maxyI,θI:δ1∈δ1,z,δ1∗∣G2δ1,θIyI=κmaxWe can summarize theee-S steering method as follows. For a given qI we have to determine δ1 and κ=κ1 first: (85)Conditionδ1κ1yI=0δ1,zθI±κmaxyI≠0,κ∗≤κmaxδ1∗θIκ∗yI,θIyI≠0,κ∗>κmaxδ1,maxyI,θIsgn⁡κ∗κmaxThe remaining path parameters δ2, κ2, and γ3s3 can be determined using (77c), (74), and (77a), respectively.We have proven that the above definedee-S steering method verifies the topological property which renders it suitable for application in any approximation-based planning approach. The topological property theoretically guarantees convergence of the approximation process in any cases. This advantageous fact is stated by Theorem B.2 in Appendix B and the proof is given there in detail as well.


#### 4.5.3. Implementation Issues

It turns out by numerical examination ofG2δ1,θI that for any θI there is exactly one δ1,z and δ1∗ value; hence δ1,max must be unique as well. Figure 9 shows a contour plot of the G function, where the thick red lines show the zeros and the thick black lines the maxima of G. It can be seen that for every θI there is another zero δ^1,z outside the interval -θI/2,0, but δ1,z inside the interval is unique.Figure 9
Contour plot of theG function.Unfortunately there are no closed form expressions for calculatingδ1,z, δ1∗, and δ1,max values, because G contains Fresnel integrals with δ1 in the argument. Thus a numerical evaluation is needed which can be done by approximating the Fresnel integrals, for example, by using precalculated lookup tables for Xβ and Yβ. It is important to note that no two-dimensional lookup table for G is needed because we can decompose it to one-dimensional factors and terms of Xβ, Yβ and trigonometric functions.


#### 4.5.4. Remarks on Bounded Sharpness

In the above described local planner we payed attention only to the upper bound of the curvature. One could argue that the sharpness should have an upper bound as well to achieve paths which can be traversed by a given minimal velocity. Indeed, as already seen in (15), when a minimal traveling speed is required, then the sharpness of the path should have a given upper bound. The E segments in the EES or ee-S have a sharpness of α=κCC2/2δCC, which has the worst-case value α=κmax2/2δCC. It follows that the sharpness is unbounded if δCC approaches zero, and we can only state that it is less than infinity, because δCC=0 means no motion. Hence the curvature is continuous and the maximal allowed speed is greater than zero everywhere.Let us determine the traveling time along anE segment in case of κCC=κmax. For this purpose we can use the velocity constraint (14) along the path and the following identities related to the α, γs, δ, and κ parameters of a clothoid [50]: (86)γs=2δκ,α=2δs2.Based on these we obtain the length of an E segment (twice as long as a clothoid), the maximal allowed absolute velocity and the minimal travel time along the segment: (87)sδCC=4δCCκmax,vmaxδCC=σmaxαδCC,tminδCC=sδCCvmaxδCC=4δCCκmax·κmax22δCCσmax=2κmaxσmax.It turns out that the traveling time along such an E segment is finite and lower bounded by 2κmax/σmax, independently from δCC. Thus we can state that the car can go along the planned path in finite time if the number of path segments is finite.


### 4.6. TheTTS Planning Algorithm

At the end of this section, let us summarize how we can useTTS paths and the ee-S steering method in an approximation framework for the purpose of generating collision-free local paths in the presence of obstacles. A mixed algorithm was implemented, which consists of two parts: the first building block is a sampling-based method relying on general TTS paths; the second one is the exact ee-S steering method, as described in Section 4.5.2. We will denote this mixed algorithm as theTTS-planner in the sequel. It takes a number of samples from the set of TTS paths connecting qI and qG and fulfilling the maximal curvature and sharpness constraints. Additionally, it computes the unique ee-S solution between qI and qG as well. From the resulting local path candidates the colliding ones are excluded and the shortest is returned from the remaining set. If every candidate is in collision, then the TTS-planner tries to find new candidates by switching the roles of initial and goal configurations. When no collision-free local path candidate could be found, the TTS-planner reports failure, and the approximation algorithm proceeds with subdividing the global path. The reason of using this mixed local planner approach is twofold. At one hand the ee-S-planner part guarantees convergence of the approximation process, according to its topological property. On the other hand, the sampling part gives a good chance of generating good quality paths by reducing the number of necessary subdivision steps during approximation. In fact, the use of ee-S steering method alone would be sufficient to ensure convergence, but the sampling part gives a number of alternatives which contribute to path quality and are beneficial when the ee-S solution collides.


## 4.1. Vehicle Model

The geometrical model of a car can be seen in Figure1. In this approach we use the one-track (or bicycle) motion model of a continuous steering kinematic car [41]: (1)x˙y˙θ˙ϕ˙=cos⁡θsin⁡θ1Ltan⁡ϕ0v+0001Ω,where x and y are the position coordinates of the back axle midpoint (treated as the reference point of the model), θ is the orientation of the car, ϕ denotes the steering angle, and L is the distance of the front and rear wheel axles. The signed velocity of the reference point is v (positive means forward motion), and Ω stands for the steering rate. The absolute values of the steering angle and its rate of change are limited to (2)ϕ≤ϕmax<π2Ω≤Ωmax<∞.The instantaneous turning radius (the reciprocal of the path curvature κ) is given by (3)1κ=Ltan⁡ϕ.The steering angle limit implies an upper bound on the path curvature (4)κ≤κmax=tan⁡ϕmaxL.Figure 1
Kinematic model of a car.The more widespread form of the motion equation is the following [51]: (5)x˙y˙θ˙κ˙=cos⁡θsin⁡θκ0v+0001σ,where σ denotes the curvature change rate. The vector q=x,y,θ,κ is called the configuration (or state) of the vehicle and u=v,σ is the input vector of the system. The connection between (1) and (5) is given by (6)κ=tan⁡ϕLσ=κ˙=ϕ˙Lcos2⁡ϕ=ΩLcos2⁡ϕ.An upper bound on the curvature change rate can be expressed as (7)σ≤σmax=ΩmaxL≤ΩmaxLcos2⁡ϕ.


## 4.2. Feasible Paths

To elaborate the properties of feasible local paths of our car model, let the translational velocity of the car be written asv=γv, where v stands for the magnitude of the velocity and γ represents the local direction of motion: (8)γ=1,ifv≥0(forward  motion),-1,ifv<0(backward  motion).Furthermore, let us introduce the translational displacement of the car along its path (9)st=∫0tvτdτand express the translational velocity as (10)v=γdsdt.Using (10) and the chain rule of differentiation, we reformulate the motion equation (5) as follows: (11)ddsxyθκ·dsdt=cos⁡θsin⁡θκ0γdsdt+0001σ.After dividing by (10) and defining (12)αs=σv=γdκdss=d2θds2s,thesharpness of the path, we get the spatial description of feasible paths: (13)γddsxyθκ=cos⁡θsin⁡θκ0+0001α.We can establish the following properties of these paths for our car model:(i)
The orientationθ at s is constrained by the local displacements along the x- and y-axes and by the motion direction γ. The path curvature κ is given by the first (spatial) derivative of θ and the motion direction. Thus every feasible path can be fully described by a planar curve labeled by the motion direction at every point. This fact actually arises from the differential flatness property of the car with the rear axle midpoint position as flat output [17, 21].(ii)
The sharpness of the path is the second derivative ofθ (the spatial change rate of the curvature along the path) and determines the maximal allowed velocity at every point on the path: (14)vs≤σmaxαs.If a nonzero minimal traveling speedvmin is required, then the sharpness has an upper bound:(15)αs≤αmax=σmaxvmin.In other words, if we would like to obtain a path which can be followed by the continuous steering car (1) and we allow the car to stop only if the motion direction is reversed, then the path should be a planar curve with(i)
bounded curvature and(ii)
bounded sharpness (continuous curvature).In Sections4.3, 4.4, and 4.5 we are about to present specific paths fulfilling these requirements and the local path planner for generating such paths.


## 4.3. Path Elements

Our aim is to choose geometric primitives which can serve as building elements in the process of path construction. We assume that the motion direction is the same along a primitive, but it can change between the primitives (which means a cusp in the path). The most simple planar curves with bounded curvature are straight lines (κ=0, called S segments in the sequel) and curvature-limited circular arcs (κs=const≤κmax, called C segments). Paths constructed by these primitives have piecewise constant curvature with abrupt curvature changes (i.e., infinite sharpness) between the pieces. To obtain continuous curvature, we are about to useclothoids, which are the most simple parametric curves with changing curvature and bounded sharpness.


### 4.3.1. Clothoids

A clothoid is a curve with constant sharpness(16)αs=α≤αmax<∞,∀s.According to (13), the equations describing a clothoid path can be obtained by integration. If we assume that q(0)=0, we get (17a)xs=γπαCFαπs,(17b)ys=γsgn⁡α⁡παSFαπs, where sgn⁡α denotes the sign of α and CF and SF stand for the Fresnel cosine and sine integrals: (18a)CFr=∫0rcos⁡π2μ2dμ,(18b)SFr=∫0rsin⁡π2μ2dμ.


### 4.3.2. Clothoid Types

From the point of view of path planning, eight different types of clothoids can be distinguished, based on the following properties:(i)
Sign of the sharpness:sgn⁡α=γsgn⁡dκ/ds, it can be positive (P,sgn⁡α=1) or negative (N,sgn⁡α=-1). Zero sharpness is omitted because it results in a straight or circular segment instead a clothoid.(ii)
Direction of motion:γ, it can be forward (F) or backward (B)(iii)
Sign of the absolute curvature change:sgn⁡dκ/ds. The absolute curvature grows if the vehicle is entering a turn (calledin-type clothoid) and vanishes if it returns to a straight segment (out-type clothoid).Equations (17a) and (17b) describe in-type clothoids parametrized by the constants α, γ and the running variable s∈0,sCC, starting from qin0=0 and arriving at some qinsCC, where sCC denotes the whole curve length. From every in-type clothoid we can derive an out-type counterpart by preserving the same geometry but reversing the motion direction: (19a)xouts=-γπαCFαπsCC-s,(19b)youts=-γsgn⁡απαSFαπsCC-s. Note that these out-type clothoids start from some qout0=qinsCC and arrive at qoutsCC=0. The eight different types of clothoids are illustrated in Figure 2.Figure 2
Clothoid types.Note that we deal here only with such clothoid path segments which have either their starting or their final configuration at the origin with zero orientation and curvature (q(0)=0 for in-type and q(sCC)=0 for out-type clothoids). However, this causes no serious limitations. On the one hand, any q=(x,y,θ,0) can be transformed to q′=(0,0,0,0) by translation and rotation. On the other hand, κ(0)=0 or κ(sCC)=0 means that our final concatenated paths can consist of S segments, CCin-C-CCout triplets, and CCin-CCout pairs. Practically, the only limitation is that transitions between two C segments of different curvatures are allowed only with a curvature profile crossing or touching zero, that is, by a C-CCout-CCin-C sequence. As we will see later, this does not prohibit the creation of a complete path planner.From now on, according to the nomenclature of [31], we will denote CCin-C-CCout triplets asCC-turns or T segments, and CCin-CCout pairs aselementary paths or E segments. Note that an elementary path is a special case of a CC-turn.


### 4.3.3. Reparametrization of Clothoids

By looking at (17a) and (17b), we can state that any point on an in-type clothoid starting from q0=0 can be described by the pair α,γs, the sharpness and the signed displacement along the curve. It was shown by Wilde [50] that an equivalent representation is available using the pair κ,δ, the curvature and the deflection of the curve at a certain point: (20a)xκ,δ=sgn⁡δ2πδκCF2δπ,(20b)yκ,δ=2πδκSF2δπ.As it can be seen, κ appears only as a multiplier in the expressions; thus it can be interpreted as a scaling factor. In this representation δ is responsible for the “shape” of the clothoid segment, and κ determines its “size.”From a computational perspective, thisκ,δ parametrization is less convenient than the parametrization by α,γs. The reason is that α and γ are constant values and only s is variable along the path segment; thus the curve is described by some fixed parameters and exactly one free parameter, as usual. This makes the explicit computation of these curves simple, for example, for visualization or collision checking. Opposed to this, both κ and δ are varying along the curve, and coherency has to be maintained between them to obtain proper results. The coherency is ensured by the constant sharpness, which is related to κ and δ as (21)α=κ22δ.For simplicity, let us choose fixed curvature and deflection values κCC and δCC at the endpoint of curve. This is the point with maximal absolute curvature and deflection: (22a)κCC=arg⁡maxκ⁡κ,(22b)δCC=arg⁡maxδ⁡δ.Based on (20a), (20b), (22a), and (22b), in-type clothoids can be fully described by (23a)xinδ=sgn⁡δCC2πδCCκCCCF2δπ,(23b)yinδ=2πδCCκCCSF2δπ,(23c)θinδ=δ,(23d)κinδ=κCCδδCC,where δ∈0,δCC is the variable parameter and δCC and κCC are constant values. A shorter form for xinδ and yinδ can be obtained by defining the following functions: (24)Xβ=sgn⁡βπβ·CFβπ(25)Yβ=πβ·SFβπ.Using these, (23a), (23b), (23c), and (23d) will look like (26a)xinδ=X2δκCCδCCδ=X2δκinδ,(26b)yinδ=Y2δκCCδCCδ=Y2δκinδ,(26c)θinδ=δ,(26d)κinδ=κCCδδCC.Note that if we look at the endpoint, X2δCC and Y2δCC can be treated as the end position of and “unscaled” in-type clothoid of δCC full deflection. The “scaling factor” 1/κCC can be used to obtain the real endpoint coordinates xinδCC and yinδCC.Now, let us consider a pair of in-type and out-type clothoids having the same geometry (such pairs were shown in Figure2, e.g., CCinPF and CCoutPB). In order to ensure the above-mentioned coherency between κCC and δCC, we define them according to (22a) and (22b) for out-type clothoids as well. However, in this case κCC belongs to the starting point, while δCC to the endpoint of the curve. Since an out-type clothoid has the same geometry as its in-type pair but opposite motion direction, we can obtain its equation as (27a)xoutδ=-sgn⁡δCC2πδCCκCCCF2δ-δCCπ,(27b)youtδ=2πδCCκCCSF2δ-δCCπ,(27c)θoutδ=δ-δCC,(27d)κoutδ=κCCδCC-δδCC.


### 4.3.4. Elementary Paths and CC-Turns

As already mentioned, elementary paths (E segments) are symmetric curves, consisting of a pair of compatible CCin and CCout clothoids. The curvature of the first curve changes from 0 to κCC, while the second from κCC back to 0. Both curves have the same deflection δCC. Compatibility is assured by the same motion direction γ and the same curvature κCC at the end of the in-type part and at the beginning of the out-type part. The compatible pairs are(i)
CCinPF and CCoutNF,(ii)
CCinNF and CCoutPF,(iii)
CCinPB and CCoutNB,(iv)
CCinNB and CCoutPB.A CC-turn consists of a pair of compatibleCCin and CCout curves, with an additional circular arc between them, which has κC=κCC curvature and δC deflection. As mentioned before, an elementary path is a special case of a CC-turn with δC=0 (see Figure 3).Figure 3
Structure of (a) elementary paths and (b) CC-turns.
(a)
(b)Let us characterize these path components. They can be described by a parametric configuration curveqδ=xδ,yδ,θδ,κδ with δ∈0,δend. The curve is fully determined by the following parameters:(i)
A starting configurationq0=qs=xs,ys,θs,0.(ii)
The maximal curvatureκCC (in an absolute sense: κCC≥κδ,∀δ).(iii)
Deflections of the clothoid (δCC) and circular (δC) parts.We use the following building elements to synthesize elementary paths and CC-turns:(i)
An in-type clothoidqinδ of deflection δCC, starting from qin(0)=(0,0,0,0) and arriving at qinδCC=xinδCC,yinδCC,δCC,κCC, as described by (23a), (23b), (23c), and (23d).(ii)
A circular arcqcδ of deflection δC and curvature κCC, starting from qc0=0,0,0,0, described by (28a)xcδ=sin⁡δκCC(28b)ycδ=1-cos⁡δκCC(28c)θcδ=δ(28d)κcδ=κCC.(iii)
A compatible out-type clothoidqoutδ of deflection δCC, starting from qout0=xout0,yout0,-δCC,κCC and arriving at qoutδCC=0,0,0,0, as described by (27a), (27b), (27c), and (27d).The starting and end configurations of these parts have to be aligned in order to obtain the resulting elementary path or CC-turn. The alignment is performed by rotation and translation, described by homogeneous transformation matrices. Let us assign a transformation matrix to any configurationq0 as follows: (29)q0=x0y0θ0κ0⟶Tq0=cos⁡θ0-sin⁡θ000x0sin⁡θ0cos⁡θ000y00010θ00001000001.This transformation matrix represents the pose of the local coordinate frame attached to q0. The transformation of another configuration q by Tq0 can be performed as usual: (30)q′1=Tq0q1.This means that we interpret the coordinates of q as local, relative to q0. Multiplying by Tq0 we get q′, which represents the global coordinates of the transformed q. Notice that the transformation does not affect the curvature part of the configuration. It follows that the original configuration q0 can be obtained by applying the transformation Tq0 to q=0,0,0,κ0: (31)q01=Tq0000κ01.The inverse transformation is given by(32)T-1q0=cos⁡θ0sin⁡θ000-x0cos⁡θ0-y0sin⁡θ0-sin⁡θ0cos⁡θ000x0sin⁡θ0-y0cos⁡θ00010-θ00001000001.These transformation matrices have the following chaining property: (33)q′H=Tq0·qH⟺Tq′=Tq0·Tq,where the upper “H” index stands for the homogenous coordinate representation qH=q,1T.Now, we have all the tools to assemble CC-turns and elementary paths. A general description of these is obtained as follows:(34a)qHδ=Tqs·qHinδ,ifδ∈0,δCC,(34b)qHδ=TqδCC·qHcδ-δCC,ifδ∈δCC,δCC+δC.(34c)qHδ=TqδCC+δC·T-1qout0·qHoutδ-δCC-δC,ifδ∈δCC+δC,2δCC+δC,where qs stands for a given arbitrary starting configuration with zero curvature and qinδ and qoutδ are given by (23a), (23b), (23c), (23d), (27a), (27b), (27c), and (27d), respectively. This can be treated as a “direct” approach for generating a CC-turn or elementary path. One specifies the starting configuration qs, the maximal curvature κCC, and the deflections δCC and δC, and the end configuration qe=q2δCC+δC is obtained directly using (34a), (34b), and (34c). Without loss of generality we can assume that qs=0,0,0,0. In this case we get (35a)qHδCC=qHinδCC=TqinδCC·0H,(35b)qHδCC+δC=TqinδCC·qHcδC=TqinδCC·TqcδC︸TqδCC+δC·0H,(35c)qeH=qH2δCC+δC=TqδCC+δC·T-1qout0︸Tqe·qHoutδCC=Tqe·0H,because the endpoint of a standard out-type clothoid is the origin, according to (27a), (27b), (27c), and (27d). The components of (36)Tqe=TqinδCC·TqcδC·T-1qout0have the following form: (37)TqinδCC=cos⁡δCC-sin⁡δCC00X2δCCκCCsin⁡δCCcos⁡δCC00Y2δCCκCC0010δCC0001000001TqcδC=cos⁡δC-sin⁡δC00sin⁡δCκCCsin⁡δCcos⁡δC001-cos⁡δCκCC0010δC0001000001.In order to obtain T-1qout0, we express qout0 first: (38)qout0=-xinδCCyinδCC-δCCκCC=-X2δCCκCCY2δCCκCC-δCCκCC,and it follows from (32) that (39)T-1qout0=cos⁡δCC-sin⁡δCC00t1,5sin⁡δCCcos⁡δCC00t2,50010δCC0001000001,where(40)t1,5=X2δCCκCCcos⁡δCC+Y2δCCκCCsin⁡δCC,t2,5=X2δCCκCCsin⁡δCC-Y2δCCκCCcos⁡δCC.After multiplying the three matrices we get (41)Tqe=cos⁡δend-sin⁡δend00A2δCC,δCκCCsin⁡δendcos⁡δend00B2δCC,δCκCC0010δend0001000001,where(42)δend=2δCC+δC,(43)A2δCC,δC=X2δCC1+cos⁡2δCC+δC+Y2δCCsin⁡2δCC+δC+sin⁡δCC+δC-sin⁡δCC,(44)B2δCC,δC=X2δCCsin⁡2δCC+δC+Y2δCC1-cos⁡2δCC+δC-cosδCC+δC+cosδCC.The end configuration of the CC-turn is given by the elements in the last column of T(qe): (45a)xe=A2δCC,δCκCC.(45b)ye=B2δCC,δCκCC(45c)θe=2δCC+δC(45d)κe=0It is important to note that, in order to fully define a CC-turn, two of the deflectionsδCC, δC, and δend should be given, together with the maximal curvature κCC. The third deflection parameter is determined by (42).The equations of an elementary path are similar to (45a), (45b), (45c), and (45d); we only need to substitute δC=0: (46a)xe=A2δCC,0κCC=A2δCCκCC,(46b)ye=B2δCC,0κCC=B2δCCκCC,(46c)θe=2δCC,(46d)κe=0,where(47)A2δCC=X2δCC1+cos⁡2δCC+Y2δCCsin⁡2δCC,B2δCC=X2δCCsin⁡2δCC+Y2δCC1-cos⁡2δCC.


## 4.3.1. Clothoids

A clothoid is a curve with constant sharpness(16)αs=α≤αmax<∞,∀s.According to (13), the equations describing a clothoid path can be obtained by integration. If we assume that q(0)=0, we get (17a)xs=γπαCFαπs,(17b)ys=γsgn⁡α⁡παSFαπs, where sgn⁡α denotes the sign of α and CF and SF stand for the Fresnel cosine and sine integrals: (18a)CFr=∫0rcos⁡π2μ2dμ,(18b)SFr=∫0rsin⁡π2μ2dμ.


## 4.3.2. Clothoid Types

From the point of view of path planning, eight different types of clothoids can be distinguished, based on the following properties:(i)
Sign of the sharpness:sgn⁡α=γsgn⁡dκ/ds, it can be positive (P,sgn⁡α=1) or negative (N,sgn⁡α=-1). Zero sharpness is omitted because it results in a straight or circular segment instead a clothoid.(ii)
Direction of motion:γ, it can be forward (F) or backward (B)(iii)
Sign of the absolute curvature change:sgn⁡dκ/ds. The absolute curvature grows if the vehicle is entering a turn (calledin-type clothoid) and vanishes if it returns to a straight segment (out-type clothoid).Equations (17a) and (17b) describe in-type clothoids parametrized by the constants α, γ and the running variable s∈0,sCC, starting from qin0=0 and arriving at some qinsCC, where sCC denotes the whole curve length. From every in-type clothoid we can derive an out-type counterpart by preserving the same geometry but reversing the motion direction: (19a)xouts=-γπαCFαπsCC-s,(19b)youts=-γsgn⁡απαSFαπsCC-s. Note that these out-type clothoids start from some qout0=qinsCC and arrive at qoutsCC=0. The eight different types of clothoids are illustrated in Figure 2.Figure 2
Clothoid types.Note that we deal here only with such clothoid path segments which have either their starting or their final configuration at the origin with zero orientation and curvature (q(0)=0 for in-type and q(sCC)=0 for out-type clothoids). However, this causes no serious limitations. On the one hand, any q=(x,y,θ,0) can be transformed to q′=(0,0,0,0) by translation and rotation. On the other hand, κ(0)=0 or κ(sCC)=0 means that our final concatenated paths can consist of S segments, CCin-C-CCout triplets, and CCin-CCout pairs. Practically, the only limitation is that transitions between two C segments of different curvatures are allowed only with a curvature profile crossing or touching zero, that is, by a C-CCout-CCin-C sequence. As we will see later, this does not prohibit the creation of a complete path planner.From now on, according to the nomenclature of [31], we will denote CCin-C-CCout triplets asCC-turns or T segments, and CCin-CCout pairs aselementary paths or E segments. Note that an elementary path is a special case of a CC-turn.


## 4.3.3. Reparametrization of Clothoids

By looking at (17a) and (17b), we can state that any point on an in-type clothoid starting from q0=0 can be described by the pair α,γs, the sharpness and the signed displacement along the curve. It was shown by Wilde [50] that an equivalent representation is available using the pair κ,δ, the curvature and the deflection of the curve at a certain point: (20a)xκ,δ=sgn⁡δ2πδκCF2δπ,(20b)yκ,δ=2πδκSF2δπ.As it can be seen, κ appears only as a multiplier in the expressions; thus it can be interpreted as a scaling factor. In this representation δ is responsible for the “shape” of the clothoid segment, and κ determines its “size.”From a computational perspective, thisκ,δ parametrization is less convenient than the parametrization by α,γs. The reason is that α and γ are constant values and only s is variable along the path segment; thus the curve is described by some fixed parameters and exactly one free parameter, as usual. This makes the explicit computation of these curves simple, for example, for visualization or collision checking. Opposed to this, both κ and δ are varying along the curve, and coherency has to be maintained between them to obtain proper results. The coherency is ensured by the constant sharpness, which is related to κ and δ as (21)α=κ22δ.For simplicity, let us choose fixed curvature and deflection values κCC and δCC at the endpoint of curve. This is the point with maximal absolute curvature and deflection: (22a)κCC=arg⁡maxκ⁡κ,(22b)δCC=arg⁡maxδ⁡δ.Based on (20a), (20b), (22a), and (22b), in-type clothoids can be fully described by (23a)xinδ=sgn⁡δCC2πδCCκCCCF2δπ,(23b)yinδ=2πδCCκCCSF2δπ,(23c)θinδ=δ,(23d)κinδ=κCCδδCC,where δ∈0,δCC is the variable parameter and δCC and κCC are constant values. A shorter form for xinδ and yinδ can be obtained by defining the following functions: (24)Xβ=sgn⁡βπβ·CFβπ(25)Yβ=πβ·SFβπ.Using these, (23a), (23b), (23c), and (23d) will look like (26a)xinδ=X2δκCCδCCδ=X2δκinδ,(26b)yinδ=Y2δκCCδCCδ=Y2δκinδ,(26c)θinδ=δ,(26d)κinδ=κCCδδCC.Note that if we look at the endpoint, X2δCC and Y2δCC can be treated as the end position of and “unscaled” in-type clothoid of δCC full deflection. The “scaling factor” 1/κCC can be used to obtain the real endpoint coordinates xinδCC and yinδCC.Now, let us consider a pair of in-type and out-type clothoids having the same geometry (such pairs were shown in Figure2, e.g., CCinPF and CCoutPB). In order to ensure the above-mentioned coherency between κCC and δCC, we define them according to (22a) and (22b) for out-type clothoids as well. However, in this case κCC belongs to the starting point, while δCC to the endpoint of the curve. Since an out-type clothoid has the same geometry as its in-type pair but opposite motion direction, we can obtain its equation as (27a)xoutδ=-sgn⁡δCC2πδCCκCCCF2δ-δCCπ,(27b)youtδ=2πδCCκCCSF2δ-δCCπ,(27c)θoutδ=δ-δCC,(27d)κoutδ=κCCδCC-δδCC.


## 4.3.4. Elementary Paths and CC-Turns

As already mentioned, elementary paths (E segments) are symmetric curves, consisting of a pair of compatible CCin and CCout clothoids. The curvature of the first curve changes from 0 to κCC, while the second from κCC back to 0. Both curves have the same deflection δCC. Compatibility is assured by the same motion direction γ and the same curvature κCC at the end of the in-type part and at the beginning of the out-type part. The compatible pairs are(i)
CCinPF and CCoutNF,(ii)
CCinNF and CCoutPF,(iii)
CCinPB and CCoutNB,(iv)
CCinNB and CCoutPB.A CC-turn consists of a pair of compatibleCCin and CCout curves, with an additional circular arc between them, which has κC=κCC curvature and δC deflection. As mentioned before, an elementary path is a special case of a CC-turn with δC=0 (see Figure 3).Figure 3
Structure of (a) elementary paths and (b) CC-turns.
(a)
(b)Let us characterize these path components. They can be described by a parametric configuration curveqδ=xδ,yδ,θδ,κδ with δ∈0,δend. The curve is fully determined by the following parameters:(i)
A starting configurationq0=qs=xs,ys,θs,0.(ii)
The maximal curvatureκCC (in an absolute sense: κCC≥κδ,∀δ).(iii)
Deflections of the clothoid (δCC) and circular (δC) parts.We use the following building elements to synthesize elementary paths and CC-turns:(i)
An in-type clothoidqinδ of deflection δCC, starting from qin(0)=(0,0,0,0) and arriving at qinδCC=xinδCC,yinδCC,δCC,κCC, as described by (23a), (23b), (23c), and (23d).(ii)
A circular arcqcδ of deflection δC and curvature κCC, starting from qc0=0,0,0,0, described by (28a)xcδ=sin⁡δκCC(28b)ycδ=1-cos⁡δκCC(28c)θcδ=δ(28d)κcδ=κCC.(iii)
A compatible out-type clothoidqoutδ of deflection δCC, starting from qout0=xout0,yout0,-δCC,κCC and arriving at qoutδCC=0,0,0,0, as described by (27a), (27b), (27c), and (27d).The starting and end configurations of these parts have to be aligned in order to obtain the resulting elementary path or CC-turn. The alignment is performed by rotation and translation, described by homogeneous transformation matrices. Let us assign a transformation matrix to any configurationq0 as follows: (29)q0=x0y0θ0κ0⟶Tq0=cos⁡θ0-sin⁡θ000x0sin⁡θ0cos⁡θ000y00010θ00001000001.This transformation matrix represents the pose of the local coordinate frame attached to q0. The transformation of another configuration q by Tq0 can be performed as usual: (30)q′1=Tq0q1.This means that we interpret the coordinates of q as local, relative to q0. Multiplying by Tq0 we get q′, which represents the global coordinates of the transformed q. Notice that the transformation does not affect the curvature part of the configuration. It follows that the original configuration q0 can be obtained by applying the transformation Tq0 to q=0,0,0,κ0: (31)q01=Tq0000κ01.The inverse transformation is given by(32)T-1q0=cos⁡θ0sin⁡θ000-x0cos⁡θ0-y0sin⁡θ0-sin⁡θ0cos⁡θ000x0sin⁡θ0-y0cos⁡θ00010-θ00001000001.These transformation matrices have the following chaining property: (33)q′H=Tq0·qH⟺Tq′=Tq0·Tq,where the upper “H” index stands for the homogenous coordinate representation qH=q,1T.Now, we have all the tools to assemble CC-turns and elementary paths. A general description of these is obtained as follows:(34a)qHδ=Tqs·qHinδ,ifδ∈0,δCC,(34b)qHδ=TqδCC·qHcδ-δCC,ifδ∈δCC,δCC+δC.(34c)qHδ=TqδCC+δC·T-1qout0·qHoutδ-δCC-δC,ifδ∈δCC+δC,2δCC+δC,where qs stands for a given arbitrary starting configuration with zero curvature and qinδ and qoutδ are given by (23a), (23b), (23c), (23d), (27a), (27b), (27c), and (27d), respectively. This can be treated as a “direct” approach for generating a CC-turn or elementary path. One specifies the starting configuration qs, the maximal curvature κCC, and the deflections δCC and δC, and the end configuration qe=q2δCC+δC is obtained directly using (34a), (34b), and (34c). Without loss of generality we can assume that qs=0,0,0,0. In this case we get (35a)qHδCC=qHinδCC=TqinδCC·0H,(35b)qHδCC+δC=TqinδCC·qHcδC=TqinδCC·TqcδC︸TqδCC+δC·0H,(35c)qeH=qH2δCC+δC=TqδCC+δC·T-1qout0︸Tqe·qHoutδCC=Tqe·0H,because the endpoint of a standard out-type clothoid is the origin, according to (27a), (27b), (27c), and (27d). The components of (36)Tqe=TqinδCC·TqcδC·T-1qout0have the following form: (37)TqinδCC=cos⁡δCC-sin⁡δCC00X2δCCκCCsin⁡δCCcos⁡δCC00Y2δCCκCC0010δCC0001000001TqcδC=cos⁡δC-sin⁡δC00sin⁡δCκCCsin⁡δCcos⁡δC001-cos⁡δCκCC0010δC0001000001.In order to obtain T-1qout0, we express qout0 first: (38)qout0=-xinδCCyinδCC-δCCκCC=-X2δCCκCCY2δCCκCC-δCCκCC,and it follows from (32) that (39)T-1qout0=cos⁡δCC-sin⁡δCC00t1,5sin⁡δCCcos⁡δCC00t2,50010δCC0001000001,where(40)t1,5=X2δCCκCCcos⁡δCC+Y2δCCκCCsin⁡δCC,t2,5=X2δCCκCCsin⁡δCC-Y2δCCκCCcos⁡δCC.After multiplying the three matrices we get (41)Tqe=cos⁡δend-sin⁡δend00A2δCC,δCκCCsin⁡δendcos⁡δend00B2δCC,δCκCC0010δend0001000001,where(42)δend=2δCC+δC,(43)A2δCC,δC=X2δCC1+cos⁡2δCC+δC+Y2δCCsin⁡2δCC+δC+sin⁡δCC+δC-sin⁡δCC,(44)B2δCC,δC=X2δCCsin⁡2δCC+δC+Y2δCC1-cos⁡2δCC+δC-cosδCC+δC+cosδCC.The end configuration of the CC-turn is given by the elements in the last column of T(qe): (45a)xe=A2δCC,δCκCC.(45b)ye=B2δCC,δCκCC(45c)θe=2δCC+δC(45d)κe=0It is important to note that, in order to fully define a CC-turn, two of the deflectionsδCC, δC, and δend should be given, together with the maximal curvature κCC. The third deflection parameter is determined by (42).The equations of an elementary path are similar to (45a), (45b), (45c), and (45d); we only need to substitute δC=0: (46a)xe=A2δCC,0κCC=A2δCCκCC,(46b)ye=B2δCC,0κCC=B2δCCκCC,(46c)θe=2δCC,(46d)κe=0,where(47)A2δCC=X2δCC1+cos⁡2δCC+Y2δCCsin⁡2δCC,B2δCC=X2δCCsin⁡2δCC+Y2δCC1-cos⁡2δCC.


## 4.4.TTS Paths for Local Planning

Algorithms that generate feasible local paths for a wheeled system—such as our car model (5)—in the absence of obstacles are called local planners or steering methods. The exact definition of these is the following.Definition 1 (steering method).
Let us denote byΛf the set of all feasible local paths λf regarding system equation q˙=fq,u. A steering method (or local planner module) is a function(48)Steer:C×C⟶Λf,i.e.,λf·=Steerq0,q1such that(49)λf0=Steerq0,q10=q0,λfSλ=Steerq0,q1Sλ=q1.As already mentioned in Section2, the most widely used local planners for the simple kinematic car model (without the requirement of curvature continuity) are based on the optimal length Reeds–Shepp (RS) paths [27]. These local paths consist of maximum five circular or straight segments and have at most two cusps, where the curvature of circular arcs is exactly ±κmax. If the continuous steering car model (5) is used, the continuous curvature generalization proposed in [31] can be applied. This approximates Reeds–Shepp paths by replacing circular segments with CC-turns. It was shown that although the resulting CCRS paths are not optimal, they converge to the optimal RS paths when the allowed sharpness of the clothoids tends to infinity. The CC-turns in this approach consist of clothoids having the maximal allowed sharpness and circle segments of maximal curvature.In the sequel we are doing something similar: introduce a new set of local paths for the continuous steering car. The main ideas which motivated the invention of our approach instead using the CCRS paths are the following:(i)
The main goal is global planning in the presence of obstacles. Most existing planning methods use a concatenation of local paths, which has the consequence that the result will be not optimal, even if the parts were generated by an optimal planner. Even the CCRS paths are suboptimal. Thus, having the final application in our mind, we lifted the requirement of optimality of our local paths.(ii)
Instead minimizing the global path length, we had the goal to achieve “natural” or “human-like” paths. What this means is informally described in the following points.(iii)
The RS (resp., CCRS) paths consist of up to five circular (resp., CC-turn) and straight segments. We show in the sequel that three path segments are sufficient to connect any two configurations.(iv)
The RS (resp., CCRS) paths use circular segments (resp., CC-turns) having the maximal allowed curvature (the steering wheel is turned to the end position in every curve). Our planner is not restricted to this. Only the limit is taken into account, but smaller curvatures are allowed.(v)
The last segment of RS (resp., CCRS) paths is always circular (resp., CC-turn). We argue that this contradicts the behavior of human drivers. Because the car can only move forward or backward, in most cases the more natural behavior is to arrive at a given goal configuration by a straight movement. There are a few exceptions, for example, the case of parallel parking, but these can usually be transformed to the mentioned case by reversing the roles of initial and goal configurations. Consequently, we choose a straight segment as the last section of our local paths.To simplify notation in the sequel, we will denote local paths as a concatenation ofS, T, and E letters, corresponding straight segments, CC-turns, and elementary paths, respectively. In our approach we use TTS (as well as EES) paths (note that in our previous work [45] we used the notations T∗TS and E∗ES where T∗ and E∗ represented “CC-turn or straight segment” and “elementary path or straight segment,” although we omit here the possibility of having a straight segment as the first element because our experiments showed that STS and SES paths are chosen very rarely by the implemented planning algorithm). In the sequel we present the construction of EES paths first (where we use only elementary paths in place of CC-turns) and show that this class of paths is sufficient to connect an arbitrary pair of configurations. After that we generalize E segments to CC-turns.


### 4.4.1. Construction ofEES Paths

Let us assume without loss of generality that the path starts from a given initial configurationqI=xI,yI,θI,0 and arrives at the origin as goal configuration qG=0,0,0,0. Any query pair qI,qG can be transformed to this form by translation and rotation, and the designed path can be transformed back. Let us denote the intermediate configurations between the path segments by q~I and q~G. For every segment, i denotes its index (1, 2 or 3) inside the path. A straight segment is parametrized by its absolute length si and direction γi and an elementary path segment by its curvature κi,CC and deflection δi,CC at the middle point between the CCin and CCout parts. To simplify the notation, let us neglect the “CC” subscript from now on: let κi=κi,CC and δi=δi,CC.The scheme of anEES path (illustrated in Figure 4) is the following: (50)qI⟶δ1,κ1Eq~I⟶δ2,κ2Eq~G⟶s3,γ3SqG.Figure 4
EES path.The coordinates ofq~I can be obtained from qI by a transformation belonging to the first elementary path (51)q~IH=TqI·Tqeδ1,κ1·0H,where(52)Tqeδ1,κ1=cos⁡2δ1-sin⁡2δ100A2δ1κ1sin⁡2δ1cos⁡2δ100B2δ1κ100102δ10001000001,TqI=cos⁡θI-sin⁡θI00xIsin⁡θIcos⁡θI00yI0010θI0001000001.By applying the matrix product we obtain(53a)x~I=A2δ1κ1cos⁡θI-B2δ1κ1sin⁡θI+xI(53b)y~I=A2δ1κ1sin⁡θI+B2δ1κ1cos⁡θI+yI(53c)θ~I=2δ1+θI(53d)κ~I=0.Let us define the auxiliary functions (54)Cβ,ψ=Aβcos⁡ψ-Bβsin⁡ψDβ,ψ=Aβsin⁡ψ+Bβcos⁡ψand write (53a), (53b), (53c), and (53d) in a shorter form: (55a)x~I=C2δ1,θIκ1+xI(55b)y~I=D2δ1,θIκ1+yI(55c)θ~I=2δ1+θI(55d)κ~I=0.Alternatively, we can getq~I from q~G=x~G,0,0,0 by “traveling backward” along the elementary path. This corresponds to a normal E segment with inverse deflection. We can obtain q~I this way by applying the transformation (56)q~IH=Tq~G·Tqe-δ2,κ2·0H,where Tqe-δ2,κ2 is the same as (41) with parameters δCC=-δ2 and δC=0: (57)Tqe-δ2,κ2=cos⁡2δ2sin⁡2δ200A-2δ2κ2-sin⁡2δ2cos⁡2δ200B-2δ2κ20010-2δ20001000001and Tq~G looks like (58)Tq~G=1000x~G01000001000001000001.The result of the transformation is (59a)x~I=A-2δ2κ2+x~G(59b)y~I=B-2δ2κ2(59c)θ~I=-2δ2(59d)κ~I=0.By comparing (55a), (55b), (55c), (55d), (59a), (59b), (59c), and (59d) we can conclude that the path parameters δ1, κ1, δ2, κ2, and x~G=-γ3s3 of an EES path fulfill the following constraints: (60a)C2δ1,θIκ1+xI=A-2δ2κ2-γ3s3(60b)D2δ1,θIκ1+yI=B-2δ2κ2(60c)2δ1+θI=-2δ2.One can conclude that in anEES planning problem only one path segment can be parametrized freely; the others are determined. In the sequel we will use δ1 and κ1 as free parameters.Let us pay some attention to the ranges of angle parameters in the above equations. The orientation parameterθI is treated as being in the interval -π,π/~, where “/~” means that the two boundary values are identified. Similarly to the geometric derivation of CC-turns, we introduce an upper bound δend,i≤π on the whole deflection of both elementary paths, which results in δi∈-π/2,π/2. In order to ensure that δ1, δ2, and θI being in the given intervals fulfill the constraint (60c), we have to choose the set carefully from which δ1 and δ2 can be taken. Let us examine two cases:(i)
θI∈[0,π]: choosing δ1∈-π/2,π/2-θI/2⊆-π/2,π/2 results in (61)δ1+θI2=-δ2∈-π2+θI2,π2⊆-π2,π2.(ii)
θI∈[-π,0]: choosing δ1∈-θI/2-π/2,π/2⊆-π/2,π/2 results in (62)δ1+θI2=-δ2∈-π2,π2+θI2⊆-π2,π2.These choices ensure both δ1 and δ2 being in -π/2,π/2 for any values of θI. We can write these conditions in a single compact form:(63)δ1,δ2∈IδθI=-sgnp⁡θIπ2,sgnp⁡θIπ2-θI2,where sgnp⁡· is a special sign function with sgnp⁡0=1: (64)sgnp⁡x=sgn⁡x,ifx≠01,ifx=0.An illustration of this set can be seen in Figure 5.Figure 5
Set of valid(δ1,δ2) pairs forEES paths.
(a)
(b)Up to now we have not taken the curvature bound into account. It can be proven that an arbitrary configuration pair can be connected by anEES path even if an upper bound on the absolute curvature is given. The following Lemma states this fact; the proof is elaborated in Appendix A.Lemma 2.
In the absence of obstacles, anyqI,qG pair of initial and goal configurations of a continuous steering car can be connected by an EES path.Furthermore, it turns out that infinitely many solutions can be found even with bounded curvature, due to the infinitely many choices ofδ1 and κ1 parameters. This means that q~I can be chosen in infinitely many ways, as illustrated in Figure 6.Figure 6
There is an infinite number ofTTS solutions between two configurations.


### 4.4.2. FromEES toTTS Paths

As we have already seen in Section4.3.4, elementary paths are special CC-turns with δC=0. It follows that the results for elementary paths used in EES planning can be generalized to CC-turns as well. The equations of an elementary path starting from any q~I and arriving at the x-axis—the middle segment of an EES path—are given by (59a), (59b), (59c), and (59d). This can easily be generalized to the case when the deflection of the circular part is nonzero: (65a)x~I=A-2δ2,CC,-δ2,Cκ2+x~G(65b)y~I=B-2δ2,CC,-δ2,Cκ2(65c)θ~I=-2δ2,CC-δ2,C(65d)κ~I=0,where δ2,CC and δ2,C stand for the deflections of the clothoid and the circular parts, respectively. The full deflection of the CC-turn is (66)δ2,end=2δ2,CC+δ2,C=-θ~I.Let us introduce the relative amount of the circular part of a CC-turn as (67)Rδ=δCδend.Using this, we can express (65b) as (68)y~I=B1-Rδ,2δ2,end,Rδ,2δ2,endκ2.The maximal curvature and the sharpness of the CC-turn are given by (69a)κ2=1y~IB1-Rδ,2δ2,end,Rδ,2δ2,end(69b)α2=κ222δ2,CC=B21-Rδ,2δ2,end,Rδ,2δ2,endy~I21-Rδ,2δ2,end.Note that if we fix the full deflection δ2,end and change the ratio Rδ,2 only, then for a given q~I the end configuration q~G on the x-axis remains the same; only κ2 and α2 are affected. In other words, we can tune the shape of a CC-turn with Rδ without affecting the starting and final configurations. This is illustrated in Figure 7, where clothoids are drawn with solid black lines and the red dashed arcs represent circular segments.Figure 7
Effect ofRδ on the shape of a CC-turn (Rδ is modified in steps of 0.1).WhenRδ grows, the curvature is decreasing but the sharpness is increasing at the same time, independently from the full deflection of the path segment. This property can be examined in Figure 8, which shows surface and contour plots of the (absolute) curvature and sharpness of a CC-turn (with y~I=1), depending on the full deflection and the relative amount of the circular part.Figure 8
Dependence of (a)-(b) maximal curvature and (c)-(d) sharpness of CC-turns on the full detection and relative amount of the circular part (absolute values are depicted).
(a)
(b)
(c)
(d)It follows that if we already have obtained anE segment between any two configurations, we can reshape it to a CC-turn having smaller curvature and higher sharpness by tuning the Rδ parameter. With this, we can define a maximal sharpness αmax beyond the already treated κmax constraint. If the sharpness of the initial E segment is less than αmax, then there exists an Rδ value such that α=αmax. This is beneficial because the length of the path segment is decreasing, as can be seen in Figure 7.If we have anEES path, we can do this reshaping for every E segment to obtain a TTS path. However, if any of the E segments violates the αmax constraint, then it is not so straightforward. Fortunately it can be shown that a TTS solution obeying the αmax constraint exists in all cases. If the second segment in the EES solution violates the sharpness constraint, we can write using (59c) and (69b) and (A.3) in Appendix A: (70)κmax′=αmaxθ~I(71)y~I≥Bθ~Iκmax′,where κmax′ is the minimal value of the curvature upper bound ensuring that the second elementary path will have α≤αmax. If we would like to obtain a TTS path from an EES solution, then by recalling (55b) and (A.6) in Appendix A we can state that κ1 can be decreased arbitrarily in order to fulfill condition (71) for the second segment and the maximal sharpness constraint for the first segment at the same time.The only drawback of usingTTS paths instead EES solutions is that the sharpness tuning requires an iterative search of Rδ in the interval [0,1]. This cannot be avoided, because Rδ is in the argument of the B(·,·) function in (69a) and (69b), which does not have an inverse because of the contained Fresnel integrals.


## 4.4.1. Construction ofEES Paths

Let us assume without loss of generality that the path starts from a given initial configurationqI=xI,yI,θI,0 and arrives at the origin as goal configuration qG=0,0,0,0. Any query pair qI,qG can be transformed to this form by translation and rotation, and the designed path can be transformed back. Let us denote the intermediate configurations between the path segments by q~I and q~G. For every segment, i denotes its index (1, 2 or 3) inside the path. A straight segment is parametrized by its absolute length si and direction γi and an elementary path segment by its curvature κi,CC and deflection δi,CC at the middle point between the CCin and CCout parts. To simplify the notation, let us neglect the “CC” subscript from now on: let κi=κi,CC and δi=δi,CC.The scheme of anEES path (illustrated in Figure 4) is the following: (50)qI⟶δ1,κ1Eq~I⟶δ2,κ2Eq~G⟶s3,γ3SqG.Figure 4
EES path.The coordinates ofq~I can be obtained from qI by a transformation belonging to the first elementary path (51)q~IH=TqI·Tqeδ1,κ1·0H,where(52)Tqeδ1,κ1=cos⁡2δ1-sin⁡2δ100A2δ1κ1sin⁡2δ1cos⁡2δ100B2δ1κ100102δ10001000001,TqI=cos⁡θI-sin⁡θI00xIsin⁡θIcos⁡θI00yI0010θI0001000001.By applying the matrix product we obtain(53a)x~I=A2δ1κ1cos⁡θI-B2δ1κ1sin⁡θI+xI(53b)y~I=A2δ1κ1sin⁡θI+B2δ1κ1cos⁡θI+yI(53c)θ~I=2δ1+θI(53d)κ~I=0.Let us define the auxiliary functions (54)Cβ,ψ=Aβcos⁡ψ-Bβsin⁡ψDβ,ψ=Aβsin⁡ψ+Bβcos⁡ψand write (53a), (53b), (53c), and (53d) in a shorter form: (55a)x~I=C2δ1,θIκ1+xI(55b)y~I=D2δ1,θIκ1+yI(55c)θ~I=2δ1+θI(55d)κ~I=0.Alternatively, we can getq~I from q~G=x~G,0,0,0 by “traveling backward” along the elementary path. This corresponds to a normal E segment with inverse deflection. We can obtain q~I this way by applying the transformation (56)q~IH=Tq~G·Tqe-δ2,κ2·0H,where Tqe-δ2,κ2 is the same as (41) with parameters δCC=-δ2 and δC=0: (57)Tqe-δ2,κ2=cos⁡2δ2sin⁡2δ200A-2δ2κ2-sin⁡2δ2cos⁡2δ200B-2δ2κ20010-2δ20001000001and Tq~G looks like (58)Tq~G=1000x~G01000001000001000001.The result of the transformation is (59a)x~I=A-2δ2κ2+x~G(59b)y~I=B-2δ2κ2(59c)θ~I=-2δ2(59d)κ~I=0.By comparing (55a), (55b), (55c), (55d), (59a), (59b), (59c), and (59d) we can conclude that the path parameters δ1, κ1, δ2, κ2, and x~G=-γ3s3 of an EES path fulfill the following constraints: (60a)C2δ1,θIκ1+xI=A-2δ2κ2-γ3s3(60b)D2δ1,θIκ1+yI=B-2δ2κ2(60c)2δ1+θI=-2δ2.One can conclude that in anEES planning problem only one path segment can be parametrized freely; the others are determined. In the sequel we will use δ1 and κ1 as free parameters.Let us pay some attention to the ranges of angle parameters in the above equations. The orientation parameterθI is treated as being in the interval -π,π/~, where “/~” means that the two boundary values are identified. Similarly to the geometric derivation of CC-turns, we introduce an upper bound δend,i≤π on the whole deflection of both elementary paths, which results in δi∈-π/2,π/2. In order to ensure that δ1, δ2, and θI being in the given intervals fulfill the constraint (60c), we have to choose the set carefully from which δ1 and δ2 can be taken. Let us examine two cases:(i)
θI∈[0,π]: choosing δ1∈-π/2,π/2-θI/2⊆-π/2,π/2 results in (61)δ1+θI2=-δ2∈-π2+θI2,π2⊆-π2,π2.(ii)
θI∈[-π,0]: choosing δ1∈-θI/2-π/2,π/2⊆-π/2,π/2 results in (62)δ1+θI2=-δ2∈-π2,π2+θI2⊆-π2,π2.These choices ensure both δ1 and δ2 being in -π/2,π/2 for any values of θI. We can write these conditions in a single compact form:(63)δ1,δ2∈IδθI=-sgnp⁡θIπ2,sgnp⁡θIπ2-θI2,where sgnp⁡· is a special sign function with sgnp⁡0=1: (64)sgnp⁡x=sgn⁡x,ifx≠01,ifx=0.An illustration of this set can be seen in Figure 5.Figure 5
Set of valid(δ1,δ2) pairs forEES paths.
(a)
(b)Up to now we have not taken the curvature bound into account. It can be proven that an arbitrary configuration pair can be connected by anEES path even if an upper bound on the absolute curvature is given. The following Lemma states this fact; the proof is elaborated in Appendix A.Lemma 2.
In the absence of obstacles, anyqI,qG pair of initial and goal configurations of a continuous steering car can be connected by an EES path.Furthermore, it turns out that infinitely many solutions can be found even with bounded curvature, due to the infinitely many choices ofδ1 and κ1 parameters. This means that q~I can be chosen in infinitely many ways, as illustrated in Figure 6.Figure 6
There is an infinite number ofTTS solutions between two configurations.


## 4.4.2. FromEES toTTS Paths

As we have already seen in Section4.3.4, elementary paths are special CC-turns with δC=0. It follows that the results for elementary paths used in EES planning can be generalized to CC-turns as well. The equations of an elementary path starting from any q~I and arriving at the x-axis—the middle segment of an EES path—are given by (59a), (59b), (59c), and (59d). This can easily be generalized to the case when the deflection of the circular part is nonzero: (65a)x~I=A-2δ2,CC,-δ2,Cκ2+x~G(65b)y~I=B-2δ2,CC,-δ2,Cκ2(65c)θ~I=-2δ2,CC-δ2,C(65d)κ~I=0,where δ2,CC and δ2,C stand for the deflections of the clothoid and the circular parts, respectively. The full deflection of the CC-turn is (66)δ2,end=2δ2,CC+δ2,C=-θ~I.Let us introduce the relative amount of the circular part of a CC-turn as (67)Rδ=δCδend.Using this, we can express (65b) as (68)y~I=B1-Rδ,2δ2,end,Rδ,2δ2,endκ2.The maximal curvature and the sharpness of the CC-turn are given by (69a)κ2=1y~IB1-Rδ,2δ2,end,Rδ,2δ2,end(69b)α2=κ222δ2,CC=B21-Rδ,2δ2,end,Rδ,2δ2,endy~I21-Rδ,2δ2,end.Note that if we fix the full deflection δ2,end and change the ratio Rδ,2 only, then for a given q~I the end configuration q~G on the x-axis remains the same; only κ2 and α2 are affected. In other words, we can tune the shape of a CC-turn with Rδ without affecting the starting and final configurations. This is illustrated in Figure 7, where clothoids are drawn with solid black lines and the red dashed arcs represent circular segments.Figure 7
Effect ofRδ on the shape of a CC-turn (Rδ is modified in steps of 0.1).WhenRδ grows, the curvature is decreasing but the sharpness is increasing at the same time, independently from the full deflection of the path segment. This property can be examined in Figure 8, which shows surface and contour plots of the (absolute) curvature and sharpness of a CC-turn (with y~I=1), depending on the full deflection and the relative amount of the circular part.Figure 8
Dependence of (a)-(b) maximal curvature and (c)-(d) sharpness of CC-turns on the full detection and relative amount of the circular part (absolute values are depicted).
(a)
(b)
(c)
(d)It follows that if we already have obtained anE segment between any two configurations, we can reshape it to a CC-turn having smaller curvature and higher sharpness by tuning the Rδ parameter. With this, we can define a maximal sharpness αmax beyond the already treated κmax constraint. If the sharpness of the initial E segment is less than αmax, then there exists an Rδ value such that α=αmax. This is beneficial because the length of the path segment is decreasing, as can be seen in Figure 7.If we have anEES path, we can do this reshaping for every E segment to obtain a TTS path. However, if any of the E segments violates the αmax constraint, then it is not so straightforward. Fortunately it can be shown that a TTS solution obeying the αmax constraint exists in all cases. If the second segment in the EES solution violates the sharpness constraint, we can write using (59c) and (69b) and (A.3) in Appendix A: (70)κmax′=αmaxθ~I(71)y~I≥Bθ~Iκmax′,where κmax′ is the minimal value of the curvature upper bound ensuring that the second elementary path will have α≤αmax. If we would like to obtain a TTS path from an EES solution, then by recalling (55b) and (A.6) in Appendix A we can state that κ1 can be decreased arbitrarily in order to fulfill condition (71) for the second segment and the maximal sharpness constraint for the first segment at the same time.The only drawback of usingTTS paths instead EES solutions is that the sharpness tuning requires an iterative search of Rδ in the interval [0,1]. This cannot be avoided, because Rδ is in the argument of the B(·,·) function in (69a) and (69b), which does not have an inverse because of the contained Fresnel integrals.


## 4.5. A Steering Method Based onEES Paths

ATTS path planner based on the conditions in proof of Lemma 2 and their generalization in Section 4.4.2 is not a steering method itself (in the sense of Definition 1), because it delivers not only one solution for a query pair. Instead of this, it delivers a parametrized class of solutions, which contains an infinite number of EES (and TTS) paths from qI to qG, fulfilling the maximum curvature (and sharpness) constraint. This property is useful in global planning problems (i.e., in the presence of obstacles), because one can more likely find a collision-free path from a solution class than from the one and only solution of an exact local planner.This does not mean that in the presence of obstacles the existence of any collision-free (local) solution would be guaranteed. For successful global planning explicit reasoning about free space connectivity is necessary. As mentioned in Section3, we apply the framework of the Holonomic Path Approximation Algorithm [11] for this purpose. A distinct global planner delivers a preliminary geometric path, which is iteratively subdivided and approximated by an appropriate steering method. To ensure the convergence of this algorithm, the local planner has to generate such paths that get closer to the original path as the local endpoints get closer to each other. This is called thetopological property [12] which is formulated more precisely in Appendix B.Therefore, if a collision-free geometric path is given and we have an appropriate steering method that verifies the topological property, then a complete (completeness means that the algorithm does not fail to return a solution if the problem is solvable) approximation algorithm can be constructed (which was a footnote in the manuscript). Hence, in order to design such a complete algorithm based onTTS paths, we need a steering method which returns exactly one from the class of TTS paths and verifies the topological property.


### 4.5.1. Definition ofee-S Paths

In the sequel we take onlyEES paths into account for the purpose of designing the exact steering method. This choice will make possible proving the topological property, because EES paths have closed form expressions. In contrast to that, TTS paths with a given sharpness can only be obtained by iterative deriving from EES solutions, as seen in Section 4.4.2.More possibilities can be found to restrict the class ofEES paths to exactly one specific solution for every query pair (qI,qG). For example, we may choose the sign of κ2 and the equality in condition (A.4) in Appendix A and fix δ1 to any chosen value except 0 and -θI±π. In this case the remaining parameter κ1 is determined unequivocally. This would result in an EES path with κ2=κmax and having the intermediate orientation θ~I=θI+2δ1 at the end of the first E segment. This approach has the problem that it is hard to formulate any reasonable direct rule for choosing δ1 and the sign of κ2 for a given query. Instead of this we introduce the following approach. Let us choose κ1=-κ2 such that κ1=κ2 is maximal. In this case we do not have to specify any other parameters. The resulting EES path has two E segments with same maximal absolute curvature, but having opposite signs (the steering wheel is turned from right to left or reversely). This type of paths is denoted as ee-S paths in the sequel to emphasize that the resulting turning radii (reciprocal of the curvature) are opposite, equal, and minimal. The ee-S planning problem is formulated as follows: (72)qI⟶δ1,κ1Eq~I⟶δ2,κ2Eq~G⟶s3,γ3SqGsuch that (73)qI=xI,yI,θI,0,qG=0,0,0,0,(74)κ≔κ1=-κ2,(75)κ≤κmax,(76)κ⟶max.Using (74) we can reformulate (60a), (60b), and (60c) as (77a)xI+γ3s3+A2δ1+θI+C2δ1,θIκ=0(77b)yI+B2δ1+θI+D2δ1,θI︷G2δ1,θIκ=0(77c)δ2=-δ1-θI2.The necessary and sufficient condition for existence of this kind ofEES paths is obtained by (75) and (77b): (78)G2δ1,θI=yI·κ≤yI·κmax.In the sequel we use some properties of the G(·,·) function, which are established in Appendix C.Lemma 3.
For anyyI,θI there exists δ1 such that G2δ1,θI≤yIκmax.Proof.
According to PropertiesC.6 and C.7 of the G function we can state that for all θI there exist δ1 values for both G2δ1,θI≠0 and G2δ1,θI=0. Since G is a continuous function, this means that, for all θI, G2δ1,θI can take an arbitrarily small value.


### 4.5.2. Theee-S Steering Method

Based on the existence condition (78) and the optimization criterion (76) we can construct a steering method which generates a single ee-S path for any local planning query. We denote it as the ee-S-planner.Let us examine the special caseyI=0 first, where the existence condition (78) takes the form (79)G2δ1,θI=0.According to the Proof of Property C.7 in Appendix, we can state that G2δ1,θI has a zero for all θI in the interval δ1∈-θI/2,0. Let us denote this δ1 solution as δ1,z: (80)δ1,zθI:δ1∈-θI2,0∣G2δ1,θI=0In this case κ could be chosen arbitrarily, but the condition (76) suggests the choice of κ=±κmax.IfyI≠0 then it follows that (81)κ=-G2δ1,θIyI,and δ1 which maximizes κ is defined by (82)δ1∗θI=arg⁡maxδ1∈IδθI⁡G2δ1,θI.Let us define κ∗ as (83)κ∗yI,θI=-G2δ1∗θI,θIyI,which can be treated as the unbounded optimum of κ. Since G has a maximum at δ1∗ and zero at δ1,z, an arbitrary κ∈0,κ∗ can be achieved by δ1∈δ1,z,δ1∗. It follows that if κ∗yI,θI>κmax, then a δ1,maxyI,θI can be found between δ1∗θI and δ1,zθI such that κ=κmax: (84)δ1,maxyI,θI:δ1∈δ1,z,δ1∗∣G2δ1,θIyI=κmaxWe can summarize theee-S steering method as follows. For a given qI we have to determine δ1 and κ=κ1 first: (85)Conditionδ1κ1yI=0δ1,zθI±κmaxyI≠0,κ∗≤κmaxδ1∗θIκ∗yI,θIyI≠0,κ∗>κmaxδ1,maxyI,θIsgn⁡κ∗κmaxThe remaining path parameters δ2, κ2, and γ3s3 can be determined using (77c), (74), and (77a), respectively.We have proven that the above definedee-S steering method verifies the topological property which renders it suitable for application in any approximation-based planning approach. The topological property theoretically guarantees convergence of the approximation process in any cases. This advantageous fact is stated by Theorem B.2 in Appendix B and the proof is given there in detail as well.


### 4.5.3. Implementation Issues

It turns out by numerical examination ofG2δ1,θI that for any θI there is exactly one δ1,z and δ1∗ value; hence δ1,max must be unique as well. Figure 9 shows a contour plot of the G function, where the thick red lines show the zeros and the thick black lines the maxima of G. It can be seen that for every θI there is another zero δ^1,z outside the interval -θI/2,0, but δ1,z inside the interval is unique.Figure 9
Contour plot of theG function.Unfortunately there are no closed form expressions for calculatingδ1,z, δ1∗, and δ1,max values, because G contains Fresnel integrals with δ1 in the argument. Thus a numerical evaluation is needed which can be done by approximating the Fresnel integrals, for example, by using precalculated lookup tables for Xβ and Yβ. It is important to note that no two-dimensional lookup table for G is needed because we can decompose it to one-dimensional factors and terms of Xβ, Yβ and trigonometric functions.


### 4.5.4. Remarks on Bounded Sharpness

In the above described local planner we payed attention only to the upper bound of the curvature. One could argue that the sharpness should have an upper bound as well to achieve paths which can be traversed by a given minimal velocity. Indeed, as already seen in (15), when a minimal traveling speed is required, then the sharpness of the path should have a given upper bound. The E segments in the EES or ee-S have a sharpness of α=κCC2/2δCC, which has the worst-case value α=κmax2/2δCC. It follows that the sharpness is unbounded if δCC approaches zero, and we can only state that it is less than infinity, because δCC=0 means no motion. Hence the curvature is continuous and the maximal allowed speed is greater than zero everywhere.Let us determine the traveling time along anE segment in case of κCC=κmax. For this purpose we can use the velocity constraint (14) along the path and the following identities related to the α, γs, δ, and κ parameters of a clothoid [50]: (86)γs=2δκ,α=2δs2.Based on these we obtain the length of an E segment (twice as long as a clothoid), the maximal allowed absolute velocity and the minimal travel time along the segment: (87)sδCC=4δCCκmax,vmaxδCC=σmaxαδCC,tminδCC=sδCCvmaxδCC=4δCCκmax·κmax22δCCσmax=2κmaxσmax.It turns out that the traveling time along such an E segment is finite and lower bounded by 2κmax/σmax, independently from δCC. Thus we can state that the car can go along the planned path in finite time if the number of path segments is finite.


## 4.5.1. Definition ofee-S Paths

In the sequel we take onlyEES paths into account for the purpose of designing the exact steering method. This choice will make possible proving the topological property, because EES paths have closed form expressions. In contrast to that, TTS paths with a given sharpness can only be obtained by iterative deriving from EES solutions, as seen in Section 4.4.2.More possibilities can be found to restrict the class ofEES paths to exactly one specific solution for every query pair (qI,qG). For example, we may choose the sign of κ2 and the equality in condition (A.4) in Appendix A and fix δ1 to any chosen value except 0 and -θI±π. In this case the remaining parameter κ1 is determined unequivocally. This would result in an EES path with κ2=κmax and having the intermediate orientation θ~I=θI+2δ1 at the end of the first E segment. This approach has the problem that it is hard to formulate any reasonable direct rule for choosing δ1 and the sign of κ2 for a given query. Instead of this we introduce the following approach. Let us choose κ1=-κ2 such that κ1=κ2 is maximal. In this case we do not have to specify any other parameters. The resulting EES path has two E segments with same maximal absolute curvature, but having opposite signs (the steering wheel is turned from right to left or reversely). This type of paths is denoted as ee-S paths in the sequel to emphasize that the resulting turning radii (reciprocal of the curvature) are opposite, equal, and minimal. The ee-S planning problem is formulated as follows: (72)qI⟶δ1,κ1Eq~I⟶δ2,κ2Eq~G⟶s3,γ3SqGsuch that (73)qI=xI,yI,θI,0,qG=0,0,0,0,(74)κ≔κ1=-κ2,(75)κ≤κmax,(76)κ⟶max.Using (74) we can reformulate (60a), (60b), and (60c) as (77a)xI+γ3s3+A2δ1+θI+C2δ1,θIκ=0(77b)yI+B2δ1+θI+D2δ1,θI︷G2δ1,θIκ=0(77c)δ2=-δ1-θI2.The necessary and sufficient condition for existence of this kind ofEES paths is obtained by (75) and (77b): (78)G2δ1,θI=yI·κ≤yI·κmax.In the sequel we use some properties of the G(·,·) function, which are established in Appendix C.Lemma 3.
For anyyI,θI there exists δ1 such that G2δ1,θI≤yIκmax.Proof.
According to PropertiesC.6 and C.7 of the G function we can state that for all θI there exist δ1 values for both G2δ1,θI≠0 and G2δ1,θI=0. Since G is a continuous function, this means that, for all θI, G2δ1,θI can take an arbitrarily small value.


## 4.5.2. Theee-S Steering Method

Based on the existence condition (78) and the optimization criterion (76) we can construct a steering method which generates a single ee-S path for any local planning query. We denote it as the ee-S-planner.Let us examine the special caseyI=0 first, where the existence condition (78) takes the form (79)G2δ1,θI=0.According to the Proof of Property C.7 in Appendix, we can state that G2δ1,θI has a zero for all θI in the interval δ1∈-θI/2,0. Let us denote this δ1 solution as δ1,z: (80)δ1,zθI:δ1∈-θI2,0∣G2δ1,θI=0In this case κ could be chosen arbitrarily, but the condition (76) suggests the choice of κ=±κmax.IfyI≠0 then it follows that (81)κ=-G2δ1,θIyI,and δ1 which maximizes κ is defined by (82)δ1∗θI=arg⁡maxδ1∈IδθI⁡G2δ1,θI.Let us define κ∗ as (83)κ∗yI,θI=-G2δ1∗θI,θIyI,which can be treated as the unbounded optimum of κ. Since G has a maximum at δ1∗ and zero at δ1,z, an arbitrary κ∈0,κ∗ can be achieved by δ1∈δ1,z,δ1∗. It follows that if κ∗yI,θI>κmax, then a δ1,maxyI,θI can be found between δ1∗θI and δ1,zθI such that κ=κmax: (84)δ1,maxyI,θI:δ1∈δ1,z,δ1∗∣G2δ1,θIyI=κmaxWe can summarize theee-S steering method as follows. For a given qI we have to determine δ1 and κ=κ1 first: (85)Conditionδ1κ1yI=0δ1,zθI±κmaxyI≠0,κ∗≤κmaxδ1∗θIκ∗yI,θIyI≠0,κ∗>κmaxδ1,maxyI,θIsgn⁡κ∗κmaxThe remaining path parameters δ2, κ2, and γ3s3 can be determined using (77c), (74), and (77a), respectively.We have proven that the above definedee-S steering method verifies the topological property which renders it suitable for application in any approximation-based planning approach. The topological property theoretically guarantees convergence of the approximation process in any cases. This advantageous fact is stated by Theorem B.2 in Appendix B and the proof is given there in detail as well.


## 4.5.3. Implementation Issues

It turns out by numerical examination ofG2δ1,θI that for any θI there is exactly one δ1,z and δ1∗ value; hence δ1,max must be unique as well. Figure 9 shows a contour plot of the G function, where the thick red lines show the zeros and the thick black lines the maxima of G. It can be seen that for every θI there is another zero δ^1,z outside the interval -θI/2,0, but δ1,z inside the interval is unique.Figure 9
Contour plot of theG function.Unfortunately there are no closed form expressions for calculatingδ1,z, δ1∗, and δ1,max values, because G contains Fresnel integrals with δ1 in the argument. Thus a numerical evaluation is needed which can be done by approximating the Fresnel integrals, for example, by using precalculated lookup tables for Xβ and Yβ. It is important to note that no two-dimensional lookup table for G is needed because we can decompose it to one-dimensional factors and terms of Xβ, Yβ and trigonometric functions.


## 4.5.4. Remarks on Bounded Sharpness

In the above described local planner we payed attention only to the upper bound of the curvature. One could argue that the sharpness should have an upper bound as well to achieve paths which can be traversed by a given minimal velocity. Indeed, as already seen in (15), when a minimal traveling speed is required, then the sharpness of the path should have a given upper bound. The E segments in the EES or ee-S have a sharpness of α=κCC2/2δCC, which has the worst-case value α=κmax2/2δCC. It follows that the sharpness is unbounded if δCC approaches zero, and we can only state that it is less than infinity, because δCC=0 means no motion. Hence the curvature is continuous and the maximal allowed speed is greater than zero everywhere.Let us determine the traveling time along anE segment in case of κCC=κmax. For this purpose we can use the velocity constraint (14) along the path and the following identities related to the α, γs, δ, and κ parameters of a clothoid [50]: (86)γs=2δκ,α=2δs2.Based on these we obtain the length of an E segment (twice as long as a clothoid), the maximal allowed absolute velocity and the minimal travel time along the segment: (87)sδCC=4δCCκmax,vmaxδCC=σmaxαδCC,tminδCC=sδCCvmaxδCC=4δCCκmax·κmax22δCCσmax=2κmaxσmax.It turns out that the traveling time along such an E segment is finite and lower bounded by 2κmax/σmax, independently from δCC. Thus we can state that the car can go along the planned path in finite time if the number of path segments is finite.


## 4.6. TheTTS Planning Algorithm

At the end of this section, let us summarize how we can useTTS paths and the ee-S steering method in an approximation framework for the purpose of generating collision-free local paths in the presence of obstacles. A mixed algorithm was implemented, which consists of two parts: the first building block is a sampling-based method relying on general TTS paths; the second one is the exact ee-S steering method, as described in Section 4.5.2. We will denote this mixed algorithm as theTTS-planner in the sequel. It takes a number of samples from the set of TTS paths connecting qI and qG and fulfilling the maximal curvature and sharpness constraints. Additionally, it computes the unique ee-S solution between qI and qG as well. From the resulting local path candidates the colliding ones are excluded and the shortest is returned from the remaining set. If every candidate is in collision, then the TTS-planner tries to find new candidates by switching the roles of initial and goal configurations. When no collision-free local path candidate could be found, the TTS-planner reports failure, and the approximation algorithm proceeds with subdividing the global path. The reason of using this mixed local planner approach is twofold. At one hand the ee-S-planner part guarantees convergence of the approximation process, according to its topological property. On the other hand, the sampling part gives a good chance of generating good quality paths by reducing the number of necessary subdivision steps during approximation. In fact, the use of ee-S steering method alone would be sufficient to ensure convergence, but the sampling part gives a number of alternatives which contribute to path quality and are beneficial when the ee-S solution collides.


## 5. Global Planning: The RTR-Planner

In Section4.4 we argued that “human-like” driving prefers moving along straight path segments. We designed the TTS-planner with this assumption in mind; indeed it tries to find a local path which arrives at the straight line defined by qG (or qI). In order to obtain an effective approximation-based planning solution, we need a global planner which facilitates the local planning phase. Thus a preliminary global path is required which is not necessarily feasible for the car, but can be approximated easily with the TTS-planner.For this purpose we decided not to use any general holonomic planning algorithm; however the topological property of our local planner ensures approximation convergence for any collision-free paths. Instead of this, we looked for a preliminary path which is “almost” feasible for the car. The solution has been found by omitting the minimum turning radius and designing a global planner which uses only straight motion and turning-in-place primitives. The resulting paths are directly applicable for differential drive robots, which have similar motion model as cars except the turning radius constraint. The planning method is based on the Rapidly Exploring Random Trees (RRT) approach [8–10] which is known for its fast convergence properties even in high dimensional configuration spaces and widely used in path planning applications.Let us first summarize the basic RRT planning process and after that we derive our RTR (rotate-translate-rotate) approach.


### 5.1. Rapidly Exploring Random Trees

The basic RRT construction process can be seen in Algorithm1. The building of the tree T starts from the initial configuration.RandomConfig() returns a random configuration qrand from the configuration space C (sampling step).NearestNeighbor() determines the nearest configuration qnear in the tree, according to a metric defined on the configuration space (vertex selection step). It depends on the implementation if this function can return only graph vertices or inner configurations of edges as well.Connect() tries to connect qnear to qrand by simply interpolating between them (tree extension step). It extends qnear until it is connected to qrand or a collision is detected. In the latter case qnew will be the farthest collision-free configuration towards qrand. In order to reach the goal configuration, the random sampling can be biased to include qgoal sometimes in the random sequence, or a bidirectional search [10] can be performed by growing two trees from both the initial and goal configurations.Algorithm 1:The basic RRT construction algorithm [44].
(
1
)
T
.
I
n
i
t
(
q
i
n
i
t
)
(
2
)
for all  k=1 to K  do
(
3
)
q
r
a
n
d
⟵
R
a
n
d
o
m
C
o
n
f
i
g
(
)
(
4
)
q
n
e
a
r
⟵
T
.
N
e
a
r
e
s
t
N
e
i
g
h
b
o
r
q
r
a
n
d
(
5
)
if  qnew⟵T.Connect(qrand,qnear)  then
(
6
)
T
.
A
d
d
V
e
r
t
e
x
(
q
n
e
w
)
(
7
)
T
.
A
d
d
E
d
g
e
(
q
n
e
a
r
,
q
n
e
w
)
(
8
)
end if
(
9
)
end for
(
10
)
return  TThe RRT method can be inherently applied to nonholonomic systems. Instead of a simple interpolation towardsqrand (which assumes free mobility in any directions) a system-specific local planner should be applied in theConnect() step. The original RRT version proposed in [8] recommended choosing an input and applying for a given time quantum Δt to obtain a Δq which brings qnear closer to qrand. This was augmented in [9] for such systems where an explicit local planning (steering) method is available to connect two configurations. Actually we build our approach on thisRRT-Connect version.For our purpose (i.e., for differential drive) the simplest steering method is the following sequence of straight motion and turning-in-place primitives:(1) turn to head the next position, (2) move straight until it is reached, and (3) turn to the desired orientation. If a collision occurs during this motion sequence, the extension is stopped at the last collision-free configuration. This simple method has the advantage that it delivers an exact solution between two configurations (no sampling of the input set is needed). Furthermore the tree edges will be straight thus making the nearest neighbor search easy, even if inner configurations of edges are involved in the search. This latter property helps to keep the number of tree vertices as low as possible, although we experienced that a naive application of this rotate-translate-rotate steering method together with RRT has poor performance if narrow areas have to be crossed in order to obtain a result. The reason is that in narrow places the collision will occur most likely during the first rotation primitive; hence no translation, that is, no effective extension of the tree will be achieved. An example scenario with a narrow passage is depicted in Figure 10, with illustrative results of RRT and the below described RTR planners. Both algorithms have been run 100 times; in every run maximum 1000 iterations were allowed. We found that RRT had only 25% success rate and 882.5 iterations on average, while RTR was successful in every trial with averagely 65.4 iterations (see Table 1). We describe the operation of RTR-planner in the sequel.Table 1
Performance measures of RRT and RTR (based on 100 runs).
RRT
RTR
Success Ratio
25%
100%
Avg. no. of iterations
882.5
65.4Figure 10
Illustration of RRT and RTR global planning. (a) RRT trees after 1000 iterations and (b) RTR trees and solution path after 65 iterations.
(a)
(b)


### 5.2. RT-Trees and Primitives

RTR-planner is similar to a bidirectional RRT-Connect algorithm; however, it has differences in the sampling, the vertex selection, and the extension steps as well. It uses translation (T) and rotation (R) primitives for building the trees. In the extension steps, only RT (rotate-translate) sequences are used instead of the exact rotate-translate-rotate steering method. For this reason, we denote the constructed trees as RT-trees. The vertices of the trees are configurations as usual and the edges are continua of configurations. According to T and R motion primitives, these are called Translational Configuration Intervals (TCIs) and Rotational Configuration Intervals (RCIs). The process of an RT-tree construction can be seen in Algorithm2 and is detailed in the sequel.Algorithm 2:Construction of an RT-tree.
(
1
)
function  RT_CONSTRUCTqinit
(
2
)
T
.
I
n
i
t
(
q
i
n
i
t
)
(
3
)
for all  k=1 to K  do
(
4
)
p
G
⟵
R
a
n
d
o
m
P
o
s
(
)
(
5
)
q
n
e
a
r
⟵
T
.
N
e
a
r
e
s
t
N
e
i
g
h
b
o
r
(
p
G
)
(
6
)
t
u
r
n
D
i
r
⟵
M
i
n
T
u
r
n
D
i
r
(
q
n
e
a
r
,
p
G
)
(
7
)
c
o
l
l
i
s
i
o
n
⟵
T
.
E
x
t
e
n
d
(
q
n
e
a
r
,
t
u
r
n
D
i
r
)
(
8
)
if  collision  then
(
9
)
T
.
E
x
t
e
n
d
(
q
n
e
a
r
,
-
t
u
r
n
D
i
r
)
(
10
)
end if
(
11
)
end for
(
12
)
return  T
(
13
)
end function
(
14
)
(
15
)
function  T.Init(q)
(
16
)
T
.
A
d
d
V
e
r
t
e
x
(
q
)
(
17
)
T
C
I
⟵
T
C
I
E
x
t
e
n
d (q, “forward”)
(
18
)
T
.
A
d
d
V
e
r
t
e
x
(
T
C
I
.
q
e
n
d
)
(
19
)
T
.
A
d
d
E
d
g
e
(
q
,
T
C
I
)
(
20
)
T
C
I
⟵
T
C
I
E
x
t
e
n
d (q, “backward”)
(
21
)
T
.
A
d
d
V
e
r
t
e
x
(
T
C
I
.
q
e
n
d
)
(
22
)
T
.
A
d
d
E
d
g
e
(
q
,
T
C
I
)
(
23
)
end function
(
24
)
(
25
)
function  T.Extend(q,turnDir)
(
26
)
[
R
C
I
,
c
o
l
l
i
s
i
o
n
]
⟵
R
C
I
E
x
t
e
n
d
(
q
,
t
u
r
n
D
i
r
)
(
27
)
T
.
A
d
d
V
e
r
t
e
x
(
R
C
I
.
q
e
n
d
)
(
28
)
T
.
A
d
d
E
d
g
e
q
,
R
C
I
(
29
)
T
C
I
⟵
T
C
I
E
x
t
e
n
d (RCI.qend, “forward”)
(
30
)
T
.
A
d
d
V
e
r
t
e
x
(
T
C
I
.
q
e
n
d
)
(
31
)
T
.
A
d
d
E
d
g
e
R
C
I
.
q
e
n
d
,
T
C
I
(
32
)
T
C
I
⟵
T
C
I
E
x
t
e
n
d(RCI.qend, “backward”)
(
33
)
T
.
A
d
d
V
e
r
t
e
x
(
T
C
I
.
q
e
n
d
)
(
34
)
T
.
A
d
d
E
d
g
e
(
R
C
I
.
q
e
n
d
,
T
C
I
)
(
35
)
return  collision
(
36
)
end function


#### 5.2.1. Sampling

The first difference to RRT can be found in the sampling step.RandomPos() returns a random position pG without orientation, denoted as the guiding position in the sequel, instead of a configuration. It can be treated as a one-dimensional continuous set of configurations, from which any element can serve as local goal in the tree extension step.


#### 5.2.2. Vertex Selection

T
.NearestNeighbor() returns the configuration in the existing tree which has the smallestposition distance to pG. This step uses a simple Euclidean metric; hence no special configuration space metrics are needed.


#### 5.2.3. Extension

The main difference to the RRT method can be found in the tree extension step. On the one hand, translation is always performed in both forward and backward directions. Additionally, the translation is not stopped whenpG is reached, but continued until the first collision in both directions (this is the functionality ofTCIExtend(), called by both T.Init() and T.Extend() functions). On the other hand, an important difference is the fact that an R-T sequence will always be planned, even if a collision occurs in the rotation phase. The RTR-planner balances between reaching guiding positions and extending the tree. If pG cannot be reached due to a collision, then two things can be done:(1)
If the collision occurred during the T primitive, then the iteration is finished (because the tree has been extended).(2)
If the collision occurred during the R primitive, then TCI_EXTEND is performed in both forward and backward directions at the colliding orientation (first extension), and the rotation is tried again in the other turning direction as well. After the second rotation, independently from its success or collision, TCI_EXTEND is called again (second extension). An example of this procedure can be seen in Figure11.Figure 11
Illustration of the tree extension procedure if the direction topG is blocked.This extension strategy causes a more aggressive free space exploration than in case of the basic RRT method.


### 5.3. Connecting RT-Trees

As the RTR-planner builds two RT-trees—from both the initial and goal configurations—in order to obtain a final path, the two trees have to be connected. This is attempted in every iteration. The newly added TCIs are checked against every TCI in the other tree, starting from its root. If an intersection is found and if a collision-free RCI can be put between the intersecting TCIs to connect them, then the two trees can be merged and the path determined easily by tracing the trees back to their roots.


## 5.1. Rapidly Exploring Random Trees

The basic RRT construction process can be seen in Algorithm1. The building of the tree T starts from the initial configuration.RandomConfig() returns a random configuration qrand from the configuration space C (sampling step).NearestNeighbor() determines the nearest configuration qnear in the tree, according to a metric defined on the configuration space (vertex selection step). It depends on the implementation if this function can return only graph vertices or inner configurations of edges as well.Connect() tries to connect qnear to qrand by simply interpolating between them (tree extension step). It extends qnear until it is connected to qrand or a collision is detected. In the latter case qnew will be the farthest collision-free configuration towards qrand. In order to reach the goal configuration, the random sampling can be biased to include qgoal sometimes in the random sequence, or a bidirectional search [10] can be performed by growing two trees from both the initial and goal configurations.Algorithm 1:The basic RRT construction algorithm [44].
(
1
)
T
.
I
n
i
t
(
q
i
n
i
t
)
(
2
)
for all  k=1 to K  do
(
3
)
q
r
a
n
d
⟵
R
a
n
d
o
m
C
o
n
f
i
g
(
)
(
4
)
q
n
e
a
r
⟵
T
.
N
e
a
r
e
s
t
N
e
i
g
h
b
o
r
q
r
a
n
d
(
5
)
if  qnew⟵T.Connect(qrand,qnear)  then
(
6
)
T
.
A
d
d
V
e
r
t
e
x
(
q
n
e
w
)
(
7
)
T
.
A
d
d
E
d
g
e
(
q
n
e
a
r
,
q
n
e
w
)
(
8
)
end if
(
9
)
end for
(
10
)
return  TThe RRT method can be inherently applied to nonholonomic systems. Instead of a simple interpolation towardsqrand (which assumes free mobility in any directions) a system-specific local planner should be applied in theConnect() step. The original RRT version proposed in [8] recommended choosing an input and applying for a given time quantum Δt to obtain a Δq which brings qnear closer to qrand. This was augmented in [9] for such systems where an explicit local planning (steering) method is available to connect two configurations. Actually we build our approach on thisRRT-Connect version.For our purpose (i.e., for differential drive) the simplest steering method is the following sequence of straight motion and turning-in-place primitives:(1) turn to head the next position, (2) move straight until it is reached, and (3) turn to the desired orientation. If a collision occurs during this motion sequence, the extension is stopped at the last collision-free configuration. This simple method has the advantage that it delivers an exact solution between two configurations (no sampling of the input set is needed). Furthermore the tree edges will be straight thus making the nearest neighbor search easy, even if inner configurations of edges are involved in the search. This latter property helps to keep the number of tree vertices as low as possible, although we experienced that a naive application of this rotate-translate-rotate steering method together with RRT has poor performance if narrow areas have to be crossed in order to obtain a result. The reason is that in narrow places the collision will occur most likely during the first rotation primitive; hence no translation, that is, no effective extension of the tree will be achieved. An example scenario with a narrow passage is depicted in Figure 10, with illustrative results of RRT and the below described RTR planners. Both algorithms have been run 100 times; in every run maximum 1000 iterations were allowed. We found that RRT had only 25% success rate and 882.5 iterations on average, while RTR was successful in every trial with averagely 65.4 iterations (see Table 1). We describe the operation of RTR-planner in the sequel.Table 1
Performance measures of RRT and RTR (based on 100 runs).
RRT
RTR
Success Ratio
25%
100%
Avg. no. of iterations
882.5
65.4Figure 10
Illustration of RRT and RTR global planning. (a) RRT trees after 1000 iterations and (b) RTR trees and solution path after 65 iterations.
(a)
(b)


## 5.2. RT-Trees and Primitives

RTR-planner is similar to a bidirectional RRT-Connect algorithm; however, it has differences in the sampling, the vertex selection, and the extension steps as well. It uses translation (T) and rotation (R) primitives for building the trees. In the extension steps, only RT (rotate-translate) sequences are used instead of the exact rotate-translate-rotate steering method. For this reason, we denote the constructed trees as RT-trees. The vertices of the trees are configurations as usual and the edges are continua of configurations. According to T and R motion primitives, these are called Translational Configuration Intervals (TCIs) and Rotational Configuration Intervals (RCIs). The process of an RT-tree construction can be seen in Algorithm2 and is detailed in the sequel.Algorithm 2:Construction of an RT-tree.
(
1
)
function  RT_CONSTRUCTqinit
(
2
)
T
.
I
n
i
t
(
q
i
n
i
t
)
(
3
)
for all  k=1 to K  do
(
4
)
p
G
⟵
R
a
n
d
o
m
P
o
s
(
)
(
5
)
q
n
e
a
r
⟵
T
.
N
e
a
r
e
s
t
N
e
i
g
h
b
o
r
(
p
G
)
(
6
)
t
u
r
n
D
i
r
⟵
M
i
n
T
u
r
n
D
i
r
(
q
n
e
a
r
,
p
G
)
(
7
)
c
o
l
l
i
s
i
o
n
⟵
T
.
E
x
t
e
n
d
(
q
n
e
a
r
,
t
u
r
n
D
i
r
)
(
8
)
if  collision  then
(
9
)
T
.
E
x
t
e
n
d
(
q
n
e
a
r
,
-
t
u
r
n
D
i
r
)
(
10
)
end if
(
11
)
end for
(
12
)
return  T
(
13
)
end function
(
14
)
(
15
)
function  T.Init(q)
(
16
)
T
.
A
d
d
V
e
r
t
e
x
(
q
)
(
17
)
T
C
I
⟵
T
C
I
E
x
t
e
n
d (q, “forward”)
(
18
)
T
.
A
d
d
V
e
r
t
e
x
(
T
C
I
.
q
e
n
d
)
(
19
)
T
.
A
d
d
E
d
g
e
(
q
,
T
C
I
)
(
20
)
T
C
I
⟵
T
C
I
E
x
t
e
n
d (q, “backward”)
(
21
)
T
.
A
d
d
V
e
r
t
e
x
(
T
C
I
.
q
e
n
d
)
(
22
)
T
.
A
d
d
E
d
g
e
(
q
,
T
C
I
)
(
23
)
end function
(
24
)
(
25
)
function  T.Extend(q,turnDir)
(
26
)
[
R
C
I
,
c
o
l
l
i
s
i
o
n
]
⟵
R
C
I
E
x
t
e
n
d
(
q
,
t
u
r
n
D
i
r
)
(
27
)
T
.
A
d
d
V
e
r
t
e
x
(
R
C
I
.
q
e
n
d
)
(
28
)
T
.
A
d
d
E
d
g
e
q
,
R
C
I
(
29
)
T
C
I
⟵
T
C
I
E
x
t
e
n
d (RCI.qend, “forward”)
(
30
)
T
.
A
d
d
V
e
r
t
e
x
(
T
C
I
.
q
e
n
d
)
(
31
)
T
.
A
d
d
E
d
g
e
R
C
I
.
q
e
n
d
,
T
C
I
(
32
)
T
C
I
⟵
T
C
I
E
x
t
e
n
d(RCI.qend, “backward”)
(
33
)
T
.
A
d
d
V
e
r
t
e
x
(
T
C
I
.
q
e
n
d
)
(
34
)
T
.
A
d
d
E
d
g
e
(
R
C
I
.
q
e
n
d
,
T
C
I
)
(
35
)
return  collision
(
36
)
end function


### 5.2.1. Sampling

The first difference to RRT can be found in the sampling step.RandomPos() returns a random position pG without orientation, denoted as the guiding position in the sequel, instead of a configuration. It can be treated as a one-dimensional continuous set of configurations, from which any element can serve as local goal in the tree extension step.


### 5.2.2. Vertex Selection

T
.NearestNeighbor() returns the configuration in the existing tree which has the smallestposition distance to pG. This step uses a simple Euclidean metric; hence no special configuration space metrics are needed.


### 5.2.3. Extension

The main difference to the RRT method can be found in the tree extension step. On the one hand, translation is always performed in both forward and backward directions. Additionally, the translation is not stopped whenpG is reached, but continued until the first collision in both directions (this is the functionality ofTCIExtend(), called by both T.Init() and T.Extend() functions). On the other hand, an important difference is the fact that an R-T sequence will always be planned, even if a collision occurs in the rotation phase. The RTR-planner balances between reaching guiding positions and extending the tree. If pG cannot be reached due to a collision, then two things can be done:(1)
If the collision occurred during the T primitive, then the iteration is finished (because the tree has been extended).(2)
If the collision occurred during the R primitive, then TCI_EXTEND is performed in both forward and backward directions at the colliding orientation (first extension), and the rotation is tried again in the other turning direction as well. After the second rotation, independently from its success or collision, TCI_EXTEND is called again (second extension). An example of this procedure can be seen in Figure11.Figure 11
Illustration of the tree extension procedure if the direction topG is blocked.This extension strategy causes a more aggressive free space exploration than in case of the basic RRT method.


## 5.2.1. Sampling

The first difference to RRT can be found in the sampling step.RandomPos() returns a random position pG without orientation, denoted as the guiding position in the sequel, instead of a configuration. It can be treated as a one-dimensional continuous set of configurations, from which any element can serve as local goal in the tree extension step.


## 5.2.2. Vertex Selection

T
.NearestNeighbor() returns the configuration in the existing tree which has the smallestposition distance to pG. This step uses a simple Euclidean metric; hence no special configuration space metrics are needed.


## 5.2.3. Extension

The main difference to the RRT method can be found in the tree extension step. On the one hand, translation is always performed in both forward and backward directions. Additionally, the translation is not stopped whenpG is reached, but continued until the first collision in both directions (this is the functionality ofTCIExtend(), called by both T.Init() and T.Extend() functions). On the other hand, an important difference is the fact that an R-T sequence will always be planned, even if a collision occurs in the rotation phase. The RTR-planner balances between reaching guiding positions and extending the tree. If pG cannot be reached due to a collision, then two things can be done:(1)
If the collision occurred during the T primitive, then the iteration is finished (because the tree has been extended).(2)
If the collision occurred during the R primitive, then TCI_EXTEND is performed in both forward and backward directions at the colliding orientation (first extension), and the rotation is tried again in the other turning direction as well. After the second rotation, independently from its success or collision, TCI_EXTEND is called again (second extension). An example of this procedure can be seen in Figure11.Figure 11
Illustration of the tree extension procedure if the direction topG is blocked.This extension strategy causes a more aggressive free space exploration than in case of the basic RRT method.


## 5.3. Connecting RT-Trees

As the RTR-planner builds two RT-trees—from both the initial and goal configurations—in order to obtain a final path, the two trees have to be connected. This is attempted in every iteration. The newly added TCIs are checked against every TCI in the other tree, starting from its root. If an intersection is found and if a collision-free RCI can be put between the intersecting TCIs to connect them, then the two trees can be merged and the path determined easily by tracing the trees back to their roots.


## 6. Simulation Results

The RTR + TTS planning algorithm was tested extensively in simulations and compared to other similar approaches in terms of effectiveness and path quality. To two other algorithms were investigated:(i)
RRT-Connect (CCRS). Bidirectional RRT using the CCRS steering method as local planner (direct one-step approach, no approximation needed).(ii)
RTR + CCRS. The RTR-planner is used for generating a preliminary global path, followed by an approximation phase using CCRS paths.The following scenarios have been used for testing:(i)
A wide environment with few obstacles (can be treated as an easy planning task, see Figure12).(ii)
A simple parallel parking scenario (a well known problem for path planning algorithms, see Figure13).(iii)
Driving through a narrow corridor and parking next to the wall (can be treated as a difficult planning task, see Figure14).(iv)
Crossing three perpendicular narrow corridors with bigger free areas at the corners (can be treated as a very difficult planning task, see Figure15).Figure 12
Path planning in a wide environment.
(a)
RRT-Connect (CCRS)
(b)
RTR + CCRS
(c)
RTR + TTS (proposed method)Figure 13
Path planning for parallel parking.
(a)
RRT-Connect (CCRS)
(b)
RTR + CCRS
(c)
RTR + TTS (proposed method)Figure 14
Crossing a narrow corridor.
(a)
RTR + CCRS
(b)
RTR + TTS (proposed method)Figure 15
Crossing three narrow corridors.
(a)
RTR + CCRS
(b)
RTR + TTS (proposed method)The simulated car is4 m long and 2 m wide with 4.42 m minimal turning radius.To obtain practical results, we constrained the maximum iteration count of the tree building process to 10000 in every algorithm and applied a lower bound of path subdivision at the approximation phase. Every algorithm has been run 100 times in every scenario, and the average results are shown in Figure16. The performance and path quality is measured by the following metrics:(i)
Success Ratio. Percentage of successful runs.(ii)
Number of Cusps. The number of reversals along the path. A good path contains as few reversals as possible.(iii)
Steering Amount. The absolute integral of the path curvature. It gives the amount of turning along the path. A path with less sharp turns or with more straight segments is more comfortable than a path requiring much turning along the way.(iv)
Travel Time. The time of driving along the resulting path is estimated as follows. A traveling speed function with vmax=5 m/s and vmin=1 m/s is assigned to the path, which is inversely proportional to the actual path curvature. The cusps are penalized with 0.5 s “reversing time.” Those paths are better, which require less time to travel along.Figure 16
Summary of the simulation results.Numerical results are listed in Figure16, while illustrative examples can be examined in Figures 12–15.As expected, the “easy” path planning task in wide environment can be flawlessly solved by all selected algorithm combinations (Figure12). The path quality measures are similar (first column in Figure 16); however the proposed RTR + TTS-planner combination results in the most comfortable and human-like paths with respect to steering amount and travel time.Similarly, the parallel parking task is solvable as well by every tested algorithm (Figure13). The TTS-based local planner obtains the highest quality paths. This is not surprising because the most “natural” parallel parking maneuver is a TTS path itself. The reason for obtaining more complicated paths with CCRS-based methods is that the CCRS planner does not have the “natural” solution amongst its local path candidates; hence it relies stronger on the global planning phase. During parallel parking we want to achieve a lateral shift to a narrow place which is quite unnatural for the car; this results in many approximation steps for the RTR + CCRS planner, producing an enormous number of small movements. The result is much better if the CCRS local planner is applied directly in the RRT-Connect planning phase, because in this case the unnatural approximation is avoided. However, the RTR + TTS-planner outperforms this as well (see the second column in Figure 16).As we move to more complicated planning challenges in environments containing narrow corridors, sampling-based planners need more iterations to find a solution, resulting in lower success rates (because the number of iterations is limited). Those global planners which tend to use mainly straight movements are more successful than others. In the scenario with one corridor the direct RRT-Connect method completely fails since it uses curved local paths to connect configuration samples (for this reason only RTR + CCRS and RTR + TTS solutions are depicted in Figure14). Approximation methods are better suited to such environments and it can be seen in the third column of 14 that the TTS local planner results in higher success ratios and better quality paths. The effectiveness of RTR as global planner originates from the aggressive extension step and from the fact that it contains only straight forward and backward translation movements. Because TTS paths are designed to arrive at straight lines, they are well suited for the approximation of RTR paths, resulting in less subdivision steps (efficiency) and simpler path geometry (quality).The effectiveness of the RTR global planner can mostly be examined in the last scenario, where three narrow corridors have to be passed (Figure15). Comparing the approximation result using CCRS and TTS local planners, we can find a much higher efficiency and much better path quality in case of TTS. Furthermore, we can see the different philosophies behind the two local planners; namely, CCRS paths strive to have the maximal allowed curvature, while TTS paths are not restricted to this. In the larger “turning spaces” between corridors, the TTS-planner generates less sharp turns and needs less reversals, which increases the comfort of passengers in the car. The direct RRT-Connect method with CCRS local planner completely fails, similarly to the previous example. As can be seen in the last column of Figure 16, the RTR + TTS-planner significantly outperforms the other algorithms.


## 7. Conclusions

In this paper we presented a global planning method for car-like vehicles, producing paths with continuous curvature. A comparative analysis with possible alternatives was performed as well and we could see that both components (RTR and TTS) of the two-phase planning approach contribute to the effectiveness and path quality of the resulting algorithm. The (preliminary) RTR path planner is capable of designing paths consisting of straight movements and turning-in-place. We apply the TTS local planner to the RTR path in a second approximation phase, in order to obtain a final path obeying the bounded and continuous curvature conditions. The TTS-planner uses straight segments, CC-turns, and elementary paths in order to generate a feasible and human-like solution even in cluttered and narrow environments. We gave an overview of how to build CC-turns and elementary paths; the class of curvature and sharpness boundedEES and TTS paths were introduced, and existence conditions were given for them. An exact steering method: the ee-S-planner was presented as well, which delivers a unique EES solution for a planning query and verifies the topological property. Simulation studies were presented to illustrate that the proposed RTR + TTS planning algorithm can be applied universally in less and more difficult situations and the obtained paths are quite “natural.”We continue this work in the future by implementing the proposed algorithm on our physical testbed [52, 53] utilizing small-sized model cars and later on a real vehicle to perform real-life tests and performance measurements. Further research includes the investigation of possible applications in previously unknown and changing environments, and the interaction possibilities of this planning method with online map building algorithms.



---

*Source: 2521638-2017-10-31.xml*