Trajectory Planning Under Vehicle Dimension Constraints Using Sequential Linear Programming
arXiv:1704.06325v1 [cs.SY] 20 Apr 2017
Mogens Graf Plessen∗,1, Pedro F. Lima∗,2 , Jonas M˚artensson2, Alberto Bemporad1, and Bo Wahlberg2
Abstract— This paper presents a spatial-based trajectory planning method for automated vehicles under actuator, obstacle avoidance, and vehicle dimension constraints. Starting from a nonlinear kinematic bicycle model, vehicle dynamics are transformed to a road-aligned coordinate frame with path along the road centerline replacing time as the dependent variable. Space-varying vehicle dimension constraints are linearized around a reference path to pose convex optimization problems. Such constraints do not require to inflate obstacles by safety-margins and therefore maximize performance in very constrained environments. A sequential linear programming (SLP) algorithm is motivated. A linear program (LP) is solved at each SLP-iteration. The relation between LP formulation and maximum admissible traveling speeds within vehicle tire friction limits is discussed. The proposed method is evaluated in a roomy and in a tight maneuvering driving scenario, whereby a comparison to a semi-analytical clothoid-based path planner is given. Effectiveness is demonstrated particularly for very constrained environments, requiring to account for constraints and planning over the entire obstacle constellation space.
I. I NTRODUCTION Challenges facing society include climate warming, safety, and congested cities. Automated vehicles may be a promising approach to contribute tackling these. With regard to climate warming, fuel consumption and exhaust gas emission can be reduced by means of platooning [1], and anticipative driving in car-2-car and car-2-infrastructure communicating environments [2], [3]. With regard to safety, for example, by means of optimized and automated handling of vehicles at their friction limits [4]–[6]. With regard to congested cities by means of automated and coordinated traffic flows [7]. We can distinguish between longitudinal and steering-related vehicle control, whereby the former is significantly simpler when considered isolatedly and it is readily introduced in commercial products [8]. Steering-applications are more complicated, since the exact traveling trajectory is decisive for permissible traveling speeds within friction limits, thereby affecting vehicle stability. In general, we can distinguish between high- and low-velocity driving scenarios. For the former, steering is relevant for obstacle avoidance and throughput maximization on highways with vehicles of different agility capabilities. For the latter, steering is relevant for tight maneuvering, for example, in car parking areas and congested cities. *The authors contributed equally when MGP was visiting KTH. 1 MGP and AB are with IMT School for Advanced Studies Lucca, Piazza S. Francesco 19, 55100 Lucca, Italy, mogens.plessen,
[email protected]. 2 PFL, JM, and BW are with Integrated Transport Research Lab and ACCESS Linnaeus Centre, Department of Automatic Control, KTH Royal Institute of Technology, SE-10044 Stockholm, Sweden, pfrdal, jonas1,
[email protected].
This paper addresses trajectory planning with obstacle avoidance for automated vehicles. The main motivation and contribution is the development of a linear programmingbased framework for the incorporation of actuator, obstacle avoidance, and, foremost, vehicle dimension constraints that are suitable for navigation in very constrained environments and enables planning over the entire obstacle constellation space. A spatial-based problem formulation is employed, eliminating time-dependency [9]–[13]. A sequential linear programming algorithm is motivated to successively improve planned trajectories. Regarding obstacle avoidance, in [14], vehicle dimensions are accounted for by decomposing the vehicle shape into several circles of specific radius laid out equidistantly along the longitudinal vehicle axis. In [15], safety distances are added to each side of the vehicle before making a hierarchical zero/one decision about interference with other obstacles. In [16], a quadratic cost distribution in the vicinity of obstacles is considered as obstacle proximity cost. Alternatively, in [10], [11], [13] safety margins are added to obstacle contours (“inflating” the obstacles) such that pointmass trajectories can be planned; see also [17] for a method transforming obstacle contours. Regarding trajectory planning, in [6], [18], [19] reference paths composed of straights, arcs, and clothoids (generalized elementary paths) are used for planning. An important concept are symmetric configurations. With respect to lanechange, they imply equal initial and final vehicle heading on start and target lane. In [20], a path sparsification method is presented that enables to fit a reduced number of clothoid segments to a reference path. Alternative trajectory planning approaches include sampling-based methods, such as rapidlyexploring random trees (RRT) [21], B-splines [22], latticebased motion planners [23], hybrid [24], configuration-space planners [17], and hierarchical methods, such as [25], where the output of a graph search based planner (A⋆ ) is consequently smoothed by a nonlinear optimization scheme to improve the quality of the solution. Though not addressing obstacle avoidance tasks, a sequential convex programming approach is employed in [26] when seeking the time-optimal trajectory (racing-line) along a tube-like road segment. For a recent survey on motion planning techniques for automated vehicles, see [27]; according to its taxonomy, the method presented in this paper can be classified as a numerical optimization approach. This paper is organized as follows. The problem formulation and main notation are defined in Section II. The SLPAlgorithm is stated in Section III. Simulation results are reported in Section IV, before concluding.
y [m]
30 20 10 0 −10
c1 b ρs (s)
δ c4
0
20
40
60
x [m] Fig. 1. Given a start pose (blue), a path is sought avoiding any obstacles (red), accounting for vehicle dimensions, traveling within corridor boundaries, respecting physical actuator constraints, and preferring smooth trajectories, thereby enabling high maximum traveling speeds within friction limits, such that an end pose (green) is reached. The black dotted path connects start and end pose traveling through obstacle corners in a piecewise-affine (PWA) fashion. It is infeasible to track by a realworld vehicle and, thus, requires smoothing: either spontaneously by direct tracking under actuator constraints, or, alternatively, using an explicit path planner preceding a tracking algorithm.
II. P ROBLEM F ORMULATION AND N OTATION The problem is formulated in Fig. 1. We consider two coordinate frames: a global one within the (x, y)-plane and a road-aligned one within the (s, ey )-plane. Their relation is illustrated in Fig. 2. The road centerline coordinate is denoted by s. Object positions can always be described in either frame by means of projections of point-masses to piecewiseaffine (PWA) line-segments. Let a trajectory planned at time t be defined by T z(s) = eψ (s) ey (s) , (1) with s ∈ [st , st + S], where the corridor length of interest is defined by S > 0 and st denotes the vehicle’s position along the road centerline coordinate at time t The driving corridor is defined by spatially dependent bounds such that max emin y (s) ≤ ey (s) ≤ ey (s). These bounds are assumed to define convex constraints on state ey and already account for obstacles to be avoided. Moving obstacles are accounted for by a velocity-adjusted mapping to the road-aligned coordinate system according to [13]. For few obstacles, a corridor may be determined based on heuristics (overtaking left or right). In general, a combinatorial problem has to be solved. In all of the following, this paper concentrates on the development of a trajectory planning method and assumes that a traversable corridor, that already accounted for velocity-adjusted projections of obstacle trajectories, is given. Throughout this paper forward motion is assumed, i.e., eψ (s) ∈ (− π2 , π2 ). Let the associated trajectory in the (x, y)-plane be defined by T X (s) = x(s) y(s) ψ(s) . (2) The actual path length traveled by the vehicle is denoted by η(s). It holds that η(s) = s if eψ (s) = 0 and ey (s) = 0, ∀s. Otherwise, η(s) 6= s because of lateral deviations of the vehicle’s traveled path from the road centerline. For brevity, the distance argument is dropped in the following. We model vehicles as rectangles. This is a simple and yet an accurate vehicle representation. As illustrated in Fig. 2, parameters a, b, and w indicate distances between the center of gravity (CoG) and rear, front and lateral vehicle sides,
a
y
v
eψ ψs ey
c2
road centerline tangent
2w
s c3
x
Fig. 2. A nonlinear dynamic bicycle model, including the representation of the curvilinear (road-aligned) coordinate system, and vehicle dimensions.
respectively. The four vehicle corners ci , i = 1, . . . , 4, can be expressed as sci = s + ξcsi cos(eψ ) + ζcsi sin(eψ ), ey,ci = ey + ξceiy cos(eψ ) + ζceiy sin(eψ ),
(3) (4)
e
with ξcsi , ξciy ∈ {b, −a, −a, b}, ζcsi ∈ {−w, −w, w, w}, and e ζciy ∈ {w, w, −w, −w} for ci , i = 1, . . . , 4, respectively. Let the maximum and the rate steering actuator limitations be denoted by δ max and δ˙ max . We assume symmetry, i.e., δ min = −δ max and δ˙ min = −δ˙ max . A general guiding notion throughout this paper is the minimization of absolute curvature peaks to maximize the lower bound on maximum permissible velocity within vehicle friction limits [6]. According to (1), let the initial and desired end vehicle pose be indicated by z(st ) and z(st + S). For obstacle avoidance, this paper incorporates vehicle dimension constraints starting directly from nonlinear equations (3) and (4). The consideration of vehicle dimensions and exploitation of available space is of particular relevance for maneuvering in tight spaces, and for large-sized vehicles, such as buses and heavy-duty trucks. III. S EQUENTIAL LINEAR PROGRAMMING A. Spatial-based vehicle dynamics Consider the nonlinear kinematic bicycle model T T x˙ y˙ ψ˙ = v cos(ψ) v sin(ψ) vl tan(δ) ,
(5)
assuming the CoG to be located at the rear axle and l denoting the wheelbase. We abbreviate time and spatial dx ′ derivatives by x˙ = dx dt and x = ds , respectively. In order to derive a spatial representation, we briefly review [13]. In accordance with Fig. 2, we have e˙ ψ = ψ˙ − ψ˙ s , e˙ y = v sin(eψ ), ρ v cos(e ) e˙ e˙ and s˙ = s ρs −ey ψ . Expressing e′ψ = sψ˙ and e′y = s˙y , the spatial-based representation of (5) is iT ′ T h ρs −ey y ) tan(δ) ′ eψ e′y = (ρρs s−e tan(eψ ) . (6) l cos(eψ ) − ψs ρs The control variable is the front-axle steering angle δ. Note that the spatial transformation eliminates any velocitydependence in (6). This is characteristic for kinematic vehicle
Let us derive vehicle dimension constraints for the formulation of convex optimization problems. At every sj , assuming forward motion only, we can describe lateral vehicle boundaries affine in s and nonlinear in eψ,j as (7) (8)
accounting for (3). We define the set −1 S˜j = {{sk }N k=1 : sj − ∆sj,min ≤ sk ≤ sj + ∆sj,max } (9) =: {sk˜1 , sk˜2 , . . . , sk˜ ˜ }, Nj
with ∆sj,min = min(sj,c2 , sj,c3 )
upper,(i)
Lsj =14
T (i) 22 T ref,(i) = T (i−1) 20 lower,(i)
Lsj =14
(i−1)
CoGsj =14
18 12
13
14
15
16
emin y (s) 17
18
s [m] Fig. 3. Illustration of vehicle dimension constraints. Indices (i) and (i − 1) indicate the corresponding SLP-iteration. The planning result at path coordinate sj = 14m is displayed.
and ∆sj,max = max(sj,c1 , sj,c4 ), further Sj = {sk˜1 −1 , sk˜1 , . . . , sk˜ ˜ , sk˜ ˜
} =: {sk1 , . . . , skN¯j }, (10) to also guarantee coverage of vehicle corners in between any two grid points. The linearization of (7) and (8) yields T lower elower (s) 1 eψ,j ey,j + hlower lin,j (s), (11) y,lin,j (s) = g T upper upper (s) 1 eψ,j ey,j + hupper ey,lin,j (s) = g lin,j (s), (12) Nj
Nj +1
upper with g lower (s), hlower (s), and hupper lin,j (s), g lin,j (s) parameterref ized by sj , eref , and e . The main motivation of vehicle y,j ψ,j dimension constraints is to ensure that the vehicle geometry is constrained to the interior of the road corridor. By evaluating (11) and (12) at the discrete grid points of (10), this can be expressed as the set of inequalities lower min ey,lin,j (sk1 ) ey (sk1 ) .. .. (13) , ≥ . .
emin ¯ ) y (skN j upper max ey,lin,j (sk1 ) ey (sk1 ) .. .. ≤ . . . elower ¯ ) y,lin,j (skN j
B. Linear vehicle dimension constraints
elower y,j (s) = tan(eψ,j )(s − sj,c3 ) + ey,c3 , upper ey,j (s) = tan(eψ,j )(s − sj,c2 ) + ey,c2 ,
(i)
CoGsj =14
24
ey [m]
models but not the case for dynamic models [13]. Note that (6) is expressed explicitly as a function of the road centerline coordinate s, see Fig. 2. In [20], curvature κ along the traveled vehicle path is related to steering angle δ. For path sparsification, an ℓ1 optimization problem is then solved, where the decision variable is a set of discrete κ expressed along a trajectory of waypoints. The optimized curvature sequence κ is ultimately inverted and fed to a low-level feedback controller, which translates it to steering commands. It is well known that paths composed of clothoid concatenations are desirable and frequently employed in road design [28]. In a clothoid, the curvature varies linearly with the path arc-length. Thus, the curvature of paths composed of clothoid concatenations is PWA. Here an important remark has to be made. Remark 1: Expressing states and control variables as a function of path arc-length s is advantageous when formulating linearly constrained optimization problems. This is since it is possible to formulate linear bounds on state variable ey . These bounds are derived from, in general, spatially-varying road widths and coordinate transformation-distorted obstacles that can be approximated by their minimal rectangleenvelope within the (s, ey )-plane. However, because of the resulting obstacle avoiding trajectory, the actual planned path may significantly deviate from the road centerline. In accordance with Section II, the path arc-length along the actual planned path is η. A PWA curvature profile κ(η) would ensure clothoid-based path planning. However, a PWA κ(s) does not yield a clothoid path, unless s = η. Let us denote (6) by z ′ = f (z, u) with u = δ. Let a discretization grid along the road centerline be defined by {sj }N j=0 = {s0 , s1 , . . . , sN }, whereby for simplicity we abbreviated sj for st+j when planning at time t. For a userdefined number of discretization points N , the discretization grid is initialized uniformly. New grid points are added such that all (potentially safety margin-adjusted) obstacle corners within the (s, ey )-frame are accounted for. Consequently, the grid is, in general, non-uniformly spaced. Then, given N ref N a set of corresponding references {eref ψ,j }j=0 , {ey,j }j=0 and ref N −1 {uj }j=0 , the linearized and discretized system dynamics are zj+1 = Aj zj + Bj uj + gj .
eupper ¯ ) y,lin,j (skN j
(14)
emax ¯ ) y (skN j
We summarize the left-hand sides of the inequality signs by and Lupper Llower sj , respectively. For visualization, see Fig. 3. sj Inequalities (13) and (14) are linear in state zj at position sj and can be compactly summarized as Qlower zj ≥ qjlower and Qupper zj ≤ qjupper , j j ¯ ¯ ¯j with Qlower , Qupper ∈ RNj ×2 , qjlower , qjupper ∈ RNj , and N j j variable for each sj and dependent on references eref and ψ,j eref y,j . Finally, note that instead of (9), as a least-conservative variant, it could be differentiated between two grid segments
S˜jlower and S˜jupper that are different for both lateral vehicle sides, instead of having one S˜j from (9) common to both.
for l = 1, . . . , L, and where
C. Linear programming formulation
Jlobs = {j : sobs,b ≤ sj ≤ sobs,e , j = 1, . . . , N }, l l
(15a)
where eobs ψ,l denotes the heading of the rectangle-envelope of obstacle l, located between sobs,b and sobs,e , within the l l road-aligned coordinate system. Note that we here reused slack variable σeNψ in order to not introduce another decision variable to (15).
(15b)
D. SLP-Algorithm
zj = [eψ,j ey,j ] , j = 0, . . . , N, zj+1 = Aj zj + Bj uj + gj , j = 0, . . . , N − 1,
(15c) (15d)
eψ (st + S) − σeNψ ≤ eψ,N ≤ eψ (st + S) + σeNψ ,
(15e)
For obstacle constellations requiring larger steering maneuvers, transition dynamics (15d) and vehicle dimension constraints (15k) and (15j) are strongly dependent on underlying reference trajectories used for linearization and discretization. This is of particular relevance for the first iniN tialization of references, for which we reconstruct {eref ψ,j }j=0 ref N and {ey,j }j=0 from the least-heading-varying PWA path avoiding all obstacles, as shown in Fig. 1. Therefore, we propose a sequential linear programming (SLP) approach, i.e., we sequentially solve (15) using as reference trajectory for linearization and discretization the solution of the previous SLP-iteration. We initialize steering commands as N −1 {uref j }j=0 = 0. The maximum admissible number of SLPiterations is denoted by I max (for example, I max = 5). Algorithm 1 summarizes the proposed algorithm, whereby we distinguish between SLP and SLPp. The latter formulation additionally incorporates (16) into (15). A termination criterion can be employed as follows. At every SLP-iteration i, we use the trajectory resulting from Step 5 of Algorithm 1 and, consequently, evaluate (3) and (4) along {sj }N j=0 in order to conduct a feasibility check. In case any obstacle or road boundary is hit or, in order to ref,(i−1) N ref,(i) }j=0 | > γ max , an avoid “wiggles”, max |{ey,j − ey,j additional SLP-iteration is conducted. The parameter choice γ max > 0 is selected heuristically in numerical experiments as two meters.
We propose a linear programming (LP) for spatial-based trajectory planning under vehicle dimension constraints: min max |u| + λ max |D1 u| + Wσ (σ + σeNψ + σeNy ) s.t. z0 = z(st ), u−1 = u(st − Ds ), T
ey (st + S) −
σeNy
≤ ey,N ≤ ey (st + S) +
σeNy ,
(15f)
max emin y,j − σ ≤ ey,j ≤ ey,j + σ, j = 1, . . . , N,
(15g)
umin ≤ uj ≤ umax , j = 0, . . . , N − 1,
(15h)
∆umin ≤ uj − uj−1 ≤ ∆umax , j = 0, . . . , N − 1,
(15i)
zj Qlower j upper Q j zj
≥ ≤
qjlower qjupper
− σ1N¯ j , j = 1, . . . , N,
(15j)
+ σ1N¯j , j = 1, . . . , N,
(15k)
≥ 0, σeN ≥ 0, σ ≥ 0, σeN y ψ
(15l)
−1 N N with decision variables {uj }N j=0 , σ, σeψ , and σey , and where | · | and 1 denote the absolute value and a columnvector of ones, respectively. LP (15) is solved repeatedly as discussed in the next Section III-D. The initial state is z(st ) according to Section II. The previous input u−1 is relevant for rate constraints (15i). The desired end pose is given by z(st + S). Constant upper and lower bounds are umin , umax , ∆umin = u˙ min Ts , and ∆umax = u˙ max Ts , where time Ts is related heuristically to the discretization grid taking reference speed into account. In general, Ts may also account for a curved reference trajectory using a spatial transformation [13]. Vehicle dimension constraints (15k) and (15j) were discussed in Section III-B. All state constraints are softened by the introduction of slack variables σ, σeNψ and σeNy to ensure feasibility of (15). Matrix D1 denotes the space-based first-order difference operator acting on vectorized input u ∈ RN ×1 . Let us motivate the objective function choice (15a). The first term is to minimize the maximum absolute curvature along traveled path; thereby simultaneously maximizing the lower bound on maximum admissible speed within vehicle friction limits. The second term max |D1 u| is for input signal smoothing. To strongly penalize state constraint violations, we select a high scalar weight Wσ = 104 . In simulations, we selected the trade-off weight λ = 1. Various reformulations including quadratic, ℓ1 -norm penalizations, and second-order difference operators were tested. We opted for (15a), since it yields a LP that can be solved very efficiently, and, in combination with the discussed state constraints, it provided PWA input trajectories, as will be illustrated. A major benefit of the proposed LP (15) is that additional constraints can easily be added. For example, to enforce the overtaking of L obstacles in parallel (without specifying the lateral distance though), we may formulate constraints N obs N obs eobs ψ,l − σeψ ≤ eψ,j ≤ eψ,l + σeψ , ∀j ∈ Jl ,
(16)
Algorithm 1 SLP-Algorithm 1: Selection: SLP or SLPp. N ref N ref N−1 2: Initialize references: {eref ψ,j }j=0 , {ey,j }j=0 and {uj }j=0
expressed on discretization grid {sj }N j=0 .
3: For i ∈ {1, . . . , I max }: 4: Build and solve (15); including (16) in case of SLPp. N ref N ref N−1 5: Update {eref ψ,j }j=0 , {ey,j }j=0 and {uj }j=0 . 6: If termination criterion satisfied: break loop; End. 7: End For N ref N ref N−1 N 8: Output: {eref ψ,j }j=0 , {ey,j }j=0 and {uj }j=0 on {sj }j=0 .
E. Bounds on admissible traveling speeds The spatial transformation eliminates any velocity dependence of a kinematic vehicle model expressed in the roadaligned coordinate system, see Section III-A. In order for our trajectory planning method to still provide velocity information, we employ the method from [6, Sect. 3] to determine spatially varying upper bounds, v max,fric (η), on admissible traveling speeds. Besides the gravitational acceleration constant, a friction coefficient µ must be assumed. It is set as
CPP SLPp SLP
8 6
6 4 ey [m]
y [m]
4 2
0
0
−2
−2 −4
2
0
50
100
150
200
x [m]
−4
0
50
100
150
200
s [m]
Fig. 4. Example 1. Comparison of the resulting trajectories in the global and in the road-aligned plane, in which computations are conducted. The max obstacles (red) are inflated by a safety margin (light red). The light gray lines (right plot) indicate road bounds emin y (s) and ey (s).
40
CPP SLPp SLP
δ [◦ ]
20 0 −20 −40
0
50
100
150
200
η [m] Fig. 5. Example 1. Resulting actuator trajectories. The dashed lines indicate absolute actuator constraints. For l = 4.3m, the bound δmax = 40◦ corresponds to a minimal turning radius of Rmin = l/ tan(δmax ) = 5.1m. Because of the roomy maneuvering space, steering is confined closely to the origin, i.e., far from the actuation limits.
µ = 0.8 in subsequent simulations. Any reference traveling speeds v ref (η) ≤ v max,fric (η) are consequently within vehicle tire friction limits. IV. N UMERICAL
RESULTS
Two examples are discussed. For both, N = 200 discretization points are employed, resulting in different spatial discretization step sizes according to Section III-A. All simulations are conducted on a laptop running Ubuntu 16.04 equipped with an Intel Core i7 CPU @2.80GHz×8, 15.6GB of memory, and using MATLAB 9.1 (R2016b). For the solution of convex optimization problems and for fast prototyping we employ CVX [29], a MATLAB package for specifying and solving convex optimization problems. Its default settings and solvers are used. A. Concatenating clothoids for path planning For comparison to a method from the literature, we consider a semi-analytical path planning method based on [6],
where three path primitives (straights, arcs and clothoids) were employed to plan emergency lane change maneuvers up to the vehicle’s friction limits. Here, treating the corners of safety margin-adjusted obstacles as waypoints, an obstacle avoiding path is planned as the concatenation of clothoids and straights connecting these waypoints. Safety margins are added to obstacle contours to account for vehicle dimensions. Planning based on path primitives requires to select multiple parameters, such as arc length, straight length and symmetric point fractions [6]. These selections are not trivial and can significantly affect results. To maintain simplicity, we focus on clothoids, straights, and symmetric trajectory design when performing lane changes (no “early” or “late” steering – see [6] and the discussion of the symmetric point fraction). In the following, the comparative method is abbreviated as CPP (clothoid path planning). Clothoids can be fitted in either the (x, y)-domain, or, alternatively, in the (s, ey )-domain before then requiring a retransformation to the (x, y)-domain. In the first example, for interest, we employ the latter method. For the second example, the (x, y)- and (s, ey )-domains are identical (no curved road). Ultimately, by first-order discretization and inversion of (5), we reconstruct steering command δ(η) along the traveled path coordinate η. B. Example 1: roomy maneuvering space A curvy road profile with large inter-obstacle distance is considered (roomy maneuvering space). Specifically for a comparison between CPP and SLPp, we add safety margins to obstacles (admitting a safe overtaking in parallel) and dismiss, for this example, constraints (15j) and (15k) from (15). Results are visualized in Figs. 4, 6, 5 and 7. Several observations can be made. First, as desired, SLP yields a trajectory that simultaneously maximizes the upper bound on maximal permissible velocity within friction limits. This comes, however, at the cost of reaching road boundary saturation and a non-parallel overtaking of obstacles, see Fig. 6. Second, while SLPp overtakes the obstacles in par-
8
6
6
4
4 y [m]
y [m]
8
2 0
0
−2
−2
−4
0
50
100
150
200
x [m] Fig. 6.
−4
0
50
100
150
200
x [m]
Example 1. The vehicle trajectories for SLP (left plot) and SLPp (right plot). Vehicle dimensions are visualized (displayed every 5th sampling).
150 v max,fric [km/h]
2
Algorithm 1 was met. For SLPp, only one iteration was needed; this can be explained by the fact that the initial PWA reference trajectory according to Section III-D already served as a sufficiently good reference.
100 50 0
CPP SLPp SLP
0
50
100
150
200
η [m] Fig. 7. Example 1. The maximum admissible traveling speed within vehicle tire friction limits is overall significantly higher for SLP in comparison to CPP and SLPp. For SLP, the lowest vmax,fric (η) along its traveled path coordinate η is 121km/h. For CPP, the equivalent is 81km/h. Thus, the road segment could in principle be traversed much faster for SLP.
allel as desired, the solution of (15) with (16) produces a trajectory, that is laterally further displaced from the second obstacle in comparison to CPP (which is enforced to proceed along the waypoints). See Fig. 7 for the effects on v max,fric . Third, Fig. 4 depicts the distortions of obstacles, resulting from the spatial coordinate transformation from the (x, y)to the (s, ey )-plane. These distortions are minor for the given example. Note the difference between planned trajectories in the (s, ey )- and (x, y)-plane after retransformation (from the curvilinear to the global coordinate system). While obstacles are overtaken in parallel in the (s, ey )-plane for CPP and SLPp, the resulting (x, y)-trajectories avoid obstacles in a curved fashion after retransformation. Fourth, as shown in Fig. 5, required steering actuation is confined to a region close to 0. Fifth, recorded computational times for CPP are 4ms. For SLP-based methods the average CPU-time for the solution of one instance of (15) was 325ms (one SLPiteration). The execution time is expected to be significantly lowered by a tailored LP-implementation. For SLP, two SLPiterations were required until the termination criterion of
C. Example 2: tight maneuvering space For a second example, a tight maneuvering space is assumed. Consider a planning scenario in a parking area. We incorporate constraints (15j) and (15k) into (15). Results are visualized in Figs. 8, 9, 10 and 11. Several observations can be made. First, steering commands of CPP, which were recomputed from planned trajectories according to Section IV-A, violate actuator constraints, see Fig. 9. This has two causes: a) the absence of explicitly accounting for actuator constraints when trajectory planning according to CPP, and b) the enforcement of proceeding pairwise along waypoints, thereby also enforcing to overtake obstacles in parallel. In contrast, as Fig. 9 illustrates, SLP remains easily within actuation limits. This is enabled by the fact that SLP accounts for the entire obstacle constellation space (anticipative steering). Second, Fig. 10 illustrates that the solution of (15) results in PWA steering commands along the spatialbased discretization coordinate s. This is a consequence of the minmax-type objective function (15a). Third, a detail; as Fig. 10 illustrates, the final steering angle upon reaching the end pose is turned at -22.1◦. This is entirely in line with the formulation of (15) connecting the given start and end pose (and not planning beyond these). Fourth, the average CPUtime for the solution of one instance of (15) was 500ms (one SLP-iteration). Four SLP-iterations were required overall to meet the termination criterion of Section III-D. D. Discussion and limitations of the method First, for the formulation of convex optimization problems in a road-aligned coordinate system, system dynamics must be expressed uniquely as a function of s. This implies forward motion along the s-coordinate, and thus naturally constrains eψ,j ∈ (− π2 , π2 ), ∀j = 1, . . . , N . While this paper focuses on driving corridors as outlined in Section II,
30
20
20
y [m]
y [m]
30
10
10
0
−10
0 CPP SLP
0
10
20
30
40
50
60
70
−10
0
10
20
x [m]
30
40
50
60
70
x [m]
Fig. 8. Example 2. Resulting vehicle trajectories in the (x, y)-plane. To account for vehicle dimensions, trajectory planning with CPP assumed an obstacle inflated by a safety margin of 1.1m. For SLP, the right plot visualizes vehicle dimensions (displayed every 5th sampling). According to the optimization problem formulation, obstacles are avoided tightly. In practice, small safety margins may additionally be added to the obstacle contours.
40
v max,fric [km/h]
20 δ [◦ ]
80
CPP SLP
0 −20 −40
0
20
40
60 η [m]
δ [◦ ]
40 20 0
20
40
60
80
η [m]
Fig. 9. Example 2. Actuator trajectories expressed along the traveled path coordinate η. The dashed lines indicate absolute actuator constraints. See Section IV-C for the discussion of constraint violations for CPP.
40 20 0 −20 −40
60
0
80
CPP SLP
Fig. 11. Example 2. The maximum admissible traveling speeds within vehicle tire friction limits. For SLP, the lowest vmax,fric (η) along the traveled path coordinate η is 32km/h. For CPP, the equivalent is 19km/h.
SLP
0
10
20
30
40
50
60
70
s [m]
Fig. 10. Example 2. The result of the solution of (15). Note that in contrast to Fig. 9, the abscissa indicates the discretization coordinate s. See Section IV-C for the discussion of the PWA nature of δ(s).
for more general trajectory planning, a different approach has to be taken: the planned trajectory of the most recent SLP-iteration will serve as the basis for the road-aligned coordinate systems of the next SLP-iteration. While for a max fixed driving corridor z(st + S), emin y (s), and ey (s) remain constant throughout all SLP-iterations of Algorithm 1, this would not hold for the more general trajectory planning
method. All quantities including obstacles and road bounds require projections to the respective coordinate frame at each SLP-iteration. Second, the formulation of (15) was based on kinematic vehicle model (5). Dynamic vehicle models can be accounted for equivalently [13]. It is not obvious to what extent this can improve safety. Rather than incorporating tire-dynamics in trajectory planning directly, the presented method simultaneously accounts for vehicle dimensions and plans trajectories according to a minmax objective involving steering angle; thereby already maximizing a stability measure in the form of maximized v max,fric (η). Any reference velocity with v ref (η) ≤ v max,fric (η) ensures vehicle operation within friction limits (for a given µ). Here, a reference velocity planning scheme may be based on [30]. The employment of a kinematic model for trajectory planning bears the advantage of reduced computational complexity.
V. C ONCLUSION We proposed a novel spatial-based trajectory planning method for automated vehicles. The main contribution was the incorporation of linearized vehicle dimension constraints within a sequential linear programming (SLP), solving a Linear Program (LP) at each SLP-iteration. The combination of a minmax objective function and the formulation of driving corridors in a road-aligned coordinate frame generated steering trajectories that translate to maximized admissible traveling speeds within vehicle tire friction limits. An evaluation was given by means of a comparison to a clothoidbased trajectory planning method, and the discussion of a roomy and a tight maneuvering scenario. Numerical results demonstrate the effectiveness, particularly for operation within very constrained environments. Anticipative steering accounting for the entire obstacle configuration space, and the ability to incorporate various constraints and alternative system descriptions are the key benefits of the proposed method. Future work can be divided into two main tasks: 1) Bottleneck and main disadvantage of the presented method is computational complexity; namely, the requirement of typically more than one SLP-iterations until convergence. It is well known that there exist very efficient LP-solvers. Subject of next work must include a tailored LP-implementation to estimate a realistic complexity and run-times on embedded hardware. 2) The proposed method is well suited for combining trajectory planning and tracking in one step within a receding horizon control (RHC) scheme. It is sought to tailor such scheme according to the outcome of step 2), including a shorter optimization horizon and other simplifications. R EFERENCES [1] A. Al Alam, A. Gattami, and K. H. Johansson, “An experimental study on the fuel reduction potential of heavy duty vehicle platooning,” in IEEE Conference on Intelligent Transportation Systems, pp. 306–311, 2010. [2] B. HomChaudhuri, A. Vahidi, and P. Pisu, “Fast model predictive control-based fuel efficient control strategy for a group of connected vehicles in urban road conditions,” IEEE Transactions on Control Systems Technology, vol. 25, no. 2, pp. 760–767, 2017. [3] J. Goz´alvez, M. Sepulcre, and R. Bauza, “IEEE 802.11p vehicle to infrastructure communications in urban environments,” IEEE Communications Magazine, vol. 50, no. 5, 2012. [4] C. M. Massera, M. H. Terra, and D. F. Wolf, “Guaranteed cost model predictive control-based driver assistance system for vehicle stabilization under tire parameters uncertainties,” in IEEE Conference on Intelligent Transportation Systems, pp. 322–327, 2016. [5] F. Altch´e, P. Polack, and A. de La Fortelle, “High-speed trajectory planning for autonomous vehicles using a simple dynamic model,” arXiv preprint arXiv:1704.01003, 2017. [6] J. Funke and J. C. Gerdes, “Simple clothoid lane change trajectories for automated vehicles incorporating friction constraints,” Journal of Dynamic Systems, Measurement, and Control, vol. 138, no. 2, pp. 021002–021002–9, 2016. [7] R. Hult, G. R. Campos, E. Steinmetz, L. Hammarstrand, P. Falcone, and H. Wymeersch, “Coordination of cooperative autonomous vehicles: Toward safer and more efficient road transportation,” IEEE Signal Processing Magazine, vol. 33, no. 6, pp. 74–84, 2016. [8] BMW, “Traffic jam assistant.” http://www.bmw.com /com/en/newvehicles/x/x5/2013/\showroom/driver_ assistance/traffic_jam_assistant.html, 2013. [9] D. Kogan and R. Murray, “Optimization-based navigation for the DARPA grand challenge,” in IEEE Conference on Decision and Control, 2006.
[10] Y. Gao, A. Gray, J. Frasch, T. Lin, E. Tseng, J. Hedrick, and F. Borrelli, “Spatial predictive control for agile semi-autonomous ground vehicles,” in International Symposium on Advanced Vehicle Control, Sept. 2012. [11] J. V. Frasch, A. Gray, M. Zanon, H. J. Ferreau, S. Sager, F. Borrelli, and M. Diehl, “An auto-generated nonlinear MPC algorithm for real-time obstacle avoidance of ground vehicles,” in IEEE European Control Conference, pp. 4136–4141, 2013. [12] R. Verschueren, S. De Bruyne, M. Zanon, J. V. Frasch, and M. Diehl, “Towards time-optimal race car driving using nonlinear MPC in realtime,” in IEEE Conference on Decision and Control, pp. 2505–2510, 2014. [13] M. Graf Plessen, D. Bernardini, H. Esen, and A. Bemporad, “Spatialbased predictive control and geometric corridor planning for adaptive cruise control coupled with obstacle avoidance,” IEEE Transactions on Control Systems Technology, 2017. (Available Online). [14] J. Ziegler, P. Bender, T. Dang, and C. Stiller, “Trajectory planning for Bertha – A local, continuous method,” in IEEE Intelligent Vehicles Symposium, pp. 450–457, 2014. [15] M. Werling and D. Liccardo, “Automatic collision avoidance using model-predictive online optimization,” in IEEE Conference on Decision and Control, pp. 6309–6314, IEEE, 2012. [16] M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenarios in a frenet frame,” in IEEE International Conference on Robotics and Automation, pp. 987–993, May 2010. [17] T. Lozano-Perez, “Spatial planning: A configuration space approach,” in Autonomous robot vehicles, pp. 259–271, Springer, 1990. [18] T. Fraichard and A. Scheuer, “From Reeds and Shepp’s to continuouscurvature paths,” IEEE Transactions on Robotics, vol. 20, no. 6, pp. 1025–1035, 2004. [19] H. Vorobieva, S. Glaser, N. Minoiu-Enache, and S. Mammar, “Automatic parallel parking with geometric continuous-curvature path planning,” in IEEE Intelligent Vehicles Symposium, pp. 465–471, 2014. [20] P. Lima, M. Trincavelli, J. M˚artensson, and B. Wahlberg, “Clothoidbased model predictive control for autonomous driving,” in IEEE European Control Conference, pp. 2983 – 2990, July 2015. [21] S. Lavalle, “Rapidly-exploring random trees: A new tool for path planning,” 1998. [22] K. Komoriya and K. Tanie, “Trajectory design and control of a wheel-type mobile robot using B-spline curve,” in Proceedings of the IEEE/RSJ International Workshop on Intelligent Robots and Systems, pp. 398–405, September 1989. [23] M. Pivtoraiko, R. Knepper, and A. Kelly, “Differentially constrained mobile robot motion planning in state lattices,” Journal of Field Robotics, vol. 26, no. 3, pp. 308–333, 2009. [24] M. Montemerlo, J. Becker, S. Bhat, H. Dahlkamp, D. Dolgov, S. Ettinger, D. Haehnel, T. Hilden, G. Hoffmann, B. Huhnke, D. Johnston, S. Klumpp, D. Langer, A. Levandowski, J. Levinson, J. Marcil, D. Orenstein, J. Paefgen, I. Penny, A. Petrovskaya, M. Pflueger, G. Stanek, D. Stavens, A. Vogt, and S. Thrun, “Junior: The Stanford entry in the urban challenge,” Journal of Field Robotics, vol. 25, no. 9, pp. 569–597, 2008. [25] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Path planning for autonomous vehicles in unknown semi-structured environments,” The International Journal of Robotics Research, vol. 29, no. 5, pp. 485– 501, 2010. [26] Q. T. Dinh and M. Diehl, “An application of sequential convex programming to time optimal trajectory planning for a car motion,” in IEEE Conference on Decision and Control, pp. 16–18, December 2009. [27] D. Gonz´alez, J. P´erez, V. Milan´es, and F. Nashashibi, “A review of motion planning techniques for automated vehicles,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 4, pp. 1135–1145, 2016. [28] H. Marzbani, R. Jazar, and M. Fard, “Better road design using clothoids,” in Sustainable Automotive Technologies 2014, pp. 25–40, Springer International Publishing, 2015. [29] M. Grant and S. Boyd, “CVX: Matlab software for disciplined convex programming, version 2.1.” http://cvxr.com/cvx, Mar. 2014. [30] R. Solea and U. Nunes, “Trajectory planning with velocity planner for fully-automated passenger vehicles,” in IEEE Conference on Intelligent Transportation Systems, pp. 474–480, IEEE, 2006.
