Finding an Optimal Trajectory for Autonomous Parking Under Uncertain Conditions

(1)

Finding an Optimal Trajectory for Autonomous Parking Under Uncertain Conditions

Tommy Hjertberg and Vidar Greinsmark

Abstract—Path planning that considers accurate vehicle dynamics and obstacle avoidance is an important problem in the area of autonomous driving. This paper describes a method of im- plementing trajectory planning for autonomous parking in conditions where the starting point and the position of fixed obstacles are uncertain. The narrow spaces and complicated manoeuvres required for parking demands a lot from the trajectory planning algorithm. It needs to have the ability to accurately model vehicle dynamics and find an efficient way around obstacles. Having obstacles in the way of the parking vehicle makes this a non- convex problem - the goal can usually not be reached by travelling in a straight line and finding a perfect trajectory around them is generally not computationally tractable. This paper reviews a two tiered approach to solving this problem. First a rough path is found using a modified Rapidly-exploring Random Tree (RRT) algorithm called Forward-Backward RRT, which runs two tree- building processes in parallel and constructs a feasible path from where they intersect. Using optimisation this is then improved into a trajectory that is at least a local optimum. These methods will be demonstrated to produce efficient and feasible trajectories that respects the dynamic constraints of the vehicle and avoids collisions.

Index Terms—RRT, autonomous parking, trajectory planning, optimisation, warm start, heuristics

TRITA-EECS-EX-2019:118

I. INTRODUCTION

Navigating a car park is a challenging task. Space is usually restricted and obstacles are placed in random inconvenient places. Planning and executing the perfect parking trajectory is sometimes challenging even for experienced human drivers. It is challenging for autonomous vehicles as well; commercially available systems are limited in the specific conditions they require to work. The recently released auto park function for Tesla model 3, for instance, requires the driver to align the vehicle with the space in a predefined way for the function to work [1]. As far as the authors of this paper are aware, no commercial system implements the level of autonomy described here.

To the human driver path planning comes intuitively. The problem for us is rather to be able to follow the path and accurately estimate the distance to objects around us. For the computers controlling autonomous vehicles the problem is quite different. They are great at calculating distances and at keeping to the planned path, but they have no intuition to rely on in planning the path, but must be provided an algorithm to go by.

Common algorithms for handling obstacle avoidance when planning a trajectory include Mixed Integer Programming (MIP), heuristic methods like Rapidly-exploring Random Tree

and optimisation. MIP is NP-hard, meaning that there is no general solution in polynomial time, limiting the possibility to generalise such solutions for problems of any size [2].

Allowing for uncertain starting points and varied obstacles adds computational complexities to an autonomous parking system. Any problem where the obstacles hinder the vehicle from driving in a straight line to the indented target is defined as non-convex. Almost all parking situations become non- convex when the dynamical limits of the car are taken into consideration: it has a turning radius and it can only steer with the front wheels.

Non-convex problems in general are very hard to optimise for. There is no universal method for finding globally optimal solutions [3]. Instead, algorithms for solving such problems attempt to find a local minimum that is good enough for the intended purpose.

As convexity makes the dynamics of the parking problem nonlinear, trying to find an optimal path using a so called ”cold start”, where the optimisation algorithm is not given any sort of direction to start from would be very computationally costly, basically requiring the solver to evaluate every trajectory for their feasibility.

Such a direct approach to using optimisation without another algorithm to provide the starting guess was used by [4] to create very well optimised parallel parking trajectories, but with the concession that it would be to computationally expensive to implement as an online system with current limitations in computing power.

Fig. 1: The input is the starting position and the goal, as well as the set of obstacles in the way. The output is a locally optimal path.

(2)

Warm starting the optimisation means that the algorithm is given a starting guess of the entire set of states [5]. In this paper the expression ”warm start” will be used to refer to providing the optimisation solver with a guess of the trajectory that our algorithm guarantees to be feasible.

This paper describes a new algorithm for creating a feasible path to warm-start the optimisation. This algorithm creates a good template for the optimisation as it takes the vehicles dynamics into consideration when creating its draft trajectory.

It is also computationally efficient as it runs two tree-building processes in parallel and only needs them to intersect for a feasible path to be found.

Using such a two level approach is a common way to try to solve autonomous parking problems. [6] takes the approach of using an Augmented Particle Swarm Optimisation to create a warm start for the optimisation. Apart from using a different algorithm, the approach of [6] also differs from ours in that we use the first feasible path that our algorithm finds and leaves the rest to the optimisation, whereas they generate several warm start paths and then chooses the best among them to start the optimisation.

Our solution tackles the problem outlined by [7] where a bidirectional Rapidly-exploring Random Tree (RRT) is described that cannot account for vehicle dynamics. Our solution uses the bicycle model of a vehicle and takes the space requirements for the vehicle fully into consideration.

The implementation of vehicle dynamics into our version of RRT is based on [8], but differs from that implementation in that its solution is not bidirectional, nor have we implemented an algorithm for refining the trajectory. Since our trajectory is to be provided as a starting guess for the optimisation, refining it out prior to the optimisation would probably not lead to a better end result.

The optimisation is formulated in continuous time and the obstacle avoidance is also formulated in a continuous form as described in Lemma 1. This makes the optimisation problem smooth, if still non-linear, thus avoiding the np-hardness of Mixed Integer Programming.

The main contributions of this paper is:

• A two tiered solution to autonomous parking using heuristics and optimisation.

• A new heuristic algorithm called Forward-Backward Rapidly-exploring Random Tree which always converges to a feasible trajectory if such a trajectory is possible

• A formulation of a control objective that formulates a complex parking situation as a smooth time-continuous problem that can be solved by optimisation.

The paper is structured as follows: Section II describes the problem. Section III describes the method used for solving the problem: the control objective and the FB-RRT algorithm.

Section IV describes the simulation environment used and the results of the simulations. Section V discusses how the findings of this paper can be implemented. Section VI concludes the paper by listing its main contributions.

II. PROBLEM STATEMENT

The goal of this project has been to create a control system that can find a feasible parking route and guide a

simulated vehicle to the designated parking spot. This should be done in as short time as possible while keeping within the constraints of the vehicles dynamics. Computation time is only a secondary in that it has to be short enough for simulations to get done in a reasonable time. The model is implemented in a Matlab simulation, which means that computation time will not reflect its true performance but rather the performance limitations of using an interpreted language. The benchmark of success is instead the travelling time, the ability to avoid obstacles and that a feasible path is found if a possible path exists.

A. Model description

The vehicle is modelled using the bicycle model. Since the speeds are low, the impact of things like slip, inertia and drag will be very low. Therefore a kinematic bicycle model can be to represent a car, without having to use complex and computationally demanding dynamic models [9]. Its state is described using the vector x ∈ R³that consists of its position and the angle of its heading. x is given by

x =



 p_x p_y ϑ



, (1)

where px∈ R is the horizontal position relative to the frame, py ∈ R is the vertical position relative to the frame and ϑ ∈ R is the heading angle as can be seen in Fig. 2. The time derivative of x becomes

˙ x =





˙ px

˙ py

ϑ˙



=



 v cos ϑ v sin ϑ

v tan δ

`



, (2)

where v ∈ R is the cars velocity, δ ∈ R is the steering angle and ` is the length of the wheelbase, which in this case is defined as the whole length of the car.

The input is described with u ∈ R², which is given by u =

v δ

, (3)

belonging to the set U =

v δ

∈ R²:

v_min≤ v ≤ vmax, δ_min≤ δ ≤ δmax

, (4) where min-values are negative and max-values positive. The time derivative of u is vector of the acceleration and the rate of change of the steering angle,

˙ u =

˙v

˙δ

=

a α

, (5)

and ˙u is limited by

(amin≤ a ≤ amax,

α_min≤ α ≤ α_max, (6)

where min-values are negative and max-values positive to allow for going backwards as well as forward as well as steering in both directions.

(3)

B. Control Objective

The vehicles starting state is denoted by x^s and is referred to as the very first point in time

x(t = 0) = x^s=



 p^s_x p^s_y ϑ^s



. (7)

The vehicle has reached its goal at state x^f. The time needed to achieve this is t^f ∈ R+, thus

x(t = t^f) = x^f =



 p^f_x p^f_y ϑ^f



. (8)

To ensure that the state won’t change after the car has reached its goal and assuming that the cars starts standing still we get the equation

˙

x(t = 0) = ˙x(t = t^f) = 0. (9)

The initial region occupied by the vehicle at t = 0 is denoted with B ⊂ R². B can be described as

B =

p_x p_y

∈ R²:

0 ≤ px≤ `

−h

2 ≤ py ≤^h₂

, (10)

where l ∈ R+is the length of the car and h ∈ R+is its width.

B can also be written as

B = {y ∈ R²: Gy ≤ g}, (11) where G ∈ R^4×2 and g ∈ R⁴ are constants [10]. The region occupied by the vehicle at any given time is called E(x) ⊂ R² as can be seen in Fig. 2. E(x) can be calculated by rotating the vehicles initial region according to the heading angle and adding the position relative to the frame, thus we get the equation

E(x) = R(ϑ)B +

p_x p_y

. (12)

In which R(ϑ) ∈ R^2×2 is the rotation matrix given by

R(ϑ) =

cos ϑ − sin ϑ sin ϑ cos ϑ

. (13)

The obstacles the car has to avoid are denoted Om ⊂ R² where m ∈ {1, 2, . . . , M } determine the obstacle and M is the amount of obstacle. The constant d ∈ R+ describe the smallest distance allowed between the car and an obstacle.

Thus for any given obstacle m

dist(E, Om) > d. (14)

Fig. 2: The vehicle starts at origin, occupying B. Its general position is E.

In order to be able to find an optimal path we must choose a cost function that take into account the total time it takes to park and how ”smooth” the trajectory is. From those criteria we chose the cost function

J (u, t^f) = t^f+ Z t^f

0

(u^>Qu + ˙u^>Qdu)dt.˙ (15) The first term is used to minimise the final time. The integral is used to get a ”smooth” path with weights Q ∈ R^2×2 and Qd ∈ R^2×2 that are positive semi-definite matrices meaning u^>Qu ≥ 0 and, ˙u^>Qdu ≥ 0. Q and Q˙ d represent a cost imposed for control actions, such as accelerating, decelerating and turning the steering wheel.

The control objective can be formulated as an optimisation problem as:

min

x,u,t^f

t^f+ Z t^f

0

(u^>Qu + ˙u^>Qdu)dt˙ s.t. x(t = 0) = x^s, x(t = t^f) = x^f,

˙

x(t = 0) = ˙x(t = t^f) = 0,

˙ x =



v tan δ

`



,

v_min δmin

≤ u ≤

v_max δmax

,

amin

αmin

≤ ˙u ≤

amax

αmax

, dist(E, O^m) > d,

∀t ∈ [0, t^f], ∀m = 1, 2, ..., M.

(16)

III. METHOD

The solution for finding a trajectory to reach the parking region is divided into two steps. An RRT-based heuristic algorithm for determining a sub-optimal, but feasible, path, and an optimisation process for refining it into a locally optimal path.

(4)

A. Forward-Backward RRT algorithm

Since the entire purpose of using the FB-RRT is to provide the optimisation solver with a warm-start, it does not provide for any smoothing of the path after having constructed it. But the fact that it respects the dynamic constraints of the vehicle makes sure that the path is easy to optimise for.

The Forward-Backward RRT (FB-RRT) algorithm is de- tailed in Algorithm 1. In this algorithm the state set for the input is denoted U, as described in equation (4), with the limits and the region that is occupied by the vehicle E as described in equation (12).

The algorithm takes the starting and finishing positions, x0

and xf, as well as the obstacles, O1...Mand the parking region, P as its inputs. It then initialises by creating two tree graphs rooted at the start and finish point respectively. These trees are referred to as the forward tree and the backward tree.

The next step of the algorithm is to build these trees until they intersect. This means that the Algorithms 2 and 3 are run repeatedly until the trees intersect.

Intersection is defined as having a smaller distance between the two nodes in the trees that are closest than a minimum distance. This is done in the Distance function by using a solver to find

min

a∈F T ,b∈BT||a − b||. (17) It should be noted that the nodes contain position as well as angle, so in this context two nodes with the same angle may be considered as closer than two nodes which are closer in the plane.

B. Building the forward tree

Updating the forward tree starts with choosing a random point, qr which does not collide with any of the obstacles. It is important to note that whether the point collides with an obstacle is not evaluated based only on the point itself, but based on the region occupied by the vehicle once it reaches this position, E(qr). This is important for the algorithms ability to work in narrow spaces, where it is unacceptable for the algorithm to generate a suggestion that means that the vehicle steers its front into an obstacle while being content that its rear axis in a legal position.

The optimisation solver is used to find the node that is closest to the random point q_r

qF := arg min

q∈F T||q − qr||. (18) Then a new node is created, representing a path segment in the direction of qr, within the limits of where the vehicle would be able to travel in a time-step considering its dynamic constraints

q_new:= arg min

q∈R³

||q − q_r||

st. q = qF + τ f (qF, u), u ∈ U f (x, u) = ˙x

E(q) ∩ O1...M = ∅ (19)

and the new node, qnew, is added as a child to the qF node.

This node now contains the trajectory segment that is as long

Algorithm 1 Forward-Backward RRT

1: procedure FB − RRT(x0, xf, O1...M, P) . x0= starting point, xf = parking target, O = obstacles, P = parking region 2: Initialisation:

3: F T.root := x0 . Forward tree, rooted at x0

4: BT.root := xf . Backward tree rooted at xf

5: dmax := 0.1 . The max distance that count as intersecting 6: while distance(F T , BT ) > dmaxdo

7: F T := Update ForwardTree(F T , O1...M) 8: Intersection := BT.latest node 9: if distance(F T , BT ) > dmax then

10: BT := Update BackwardTree(BT, O^1...M, P) 11: Intersection := BT.latest node

12: end if

13: end while 14: N ode := arg min

x∈F T||x − Intersection||

15: P ath := [∅] . Start with empty path

16: while x0∈ P ath do/ . Build the path backwards to x0

17: P ath := [N ode, P ath] . Add the node first in the path 18: N ode := N ode.parent

19: end while 20: N ode := arg min

x∈BT||x − Intersection||

21: while xf ∈ P ath do . Build the last part of the path to x/ f

22: P ath := [P ath, N ode] . Add the node last in the path 23: N ode := N ode.parent . Prepare to process parent 24: end while

25: return P ath 26: end procedure

27: functionDISTANCE(F T , BT ) 28: return min

a∈F T ,b∈BT||a − b||

29: end function

Algorithm 2 Forward tree updating function

1: function UPDATE FORWARDTREE(F T , O1...M)

2: randomize qr:= (X, Y, Θ) . qr is a random point 3: while E(qr) ∩ O1...M 6= ∅ do . Repeat until no collision 4: randomize qr:= (X, Y, Θ)

5: end while 6: qF := arg min

q∈F T||q − qr|| . Find the node closest to qr

7:

qnew:= arg min

q∈R³||q − qr||

st. q = qF+ τ f (qF, u), u ∈ U f (x, u) = ˙x

E(q) ∩ O^1...M= ∅ 8: qF.child := qnew

9: F T.latest node := qF.child 10: return F T

11: end function

as the vehicle can travel in the selected time step, τ , in the direction of qr. The added node is then set as the latest node of the forward tree. Since the function for determining the trajectory used is based on ˙x and the input is limited to U, the trajectory will always be feasible for the vehicle.

C. Building the backward tree

The algorithm for the backward tree is very similar to the forward tree, except that it also needs to handle getting out of the confined parking region, P.

(5)

Algorithm 3 Backward tree updating function

1: function UPDATE BACKWARDTREE(BT, O^1...M, P) 2: randomize qr:= (X, Y, Θ)

3: while E(q^r) ∩ O^1...M 6= ∅ do 4: randomize qr= (X, Y, Θ) 5: end while

6: qB:= arg min

q∈BT||q − qr||

7: if ∀q ∈ BT, q ∈ P then . Getting out of P 8: P^∗:= R²\ (P ∪ O^1...M)

9:

qnew:= arg min ||q − P^∗||

s.t. q = qB+ τ f (qB, u), u ∈ U f (x, u) = ˙x

E(q) ∩ O1...M= ∅ 10: else

11:

qnew:= arg min

q∈R³||q − qr||

st. q = qB+ τ f (qB, u), u ∈ U f (x, u) = ˙x

E(q) ∩ O^1...M= ∅ 12: end if

13: qB.child := qnew

14: BT.latest node := qB.child 15: return BT

16: end function

To speed up the initial path-finding out of narrow parking spaces for the backward tree, as described in Algorithm 3, we define the region P ⊂ R²\SM

i=1Oiaccording to the geometry of the specific problem.

In this region the space is so narrow, that it would be a waste of time to try to get out of it using random directions. The purpose of using random directions is to cast a wider ”net”

for the other tree to intersect. A tree confined to a small area does not give any such benefit. As long as the tree has not grown outside this region, the algorithm always optimises for the the closest spot that is neither inside the parking region nor covered by an obstacle.

If the entire backward tree is contained in P, an optimisation solver will be used to find the node closest to the complement of the union of P and the obstacles O1...M

qnew:= arg min ||q − P^∗||

s.t. q = qB+ τ f (qB, u), u ∈ U f (x, u) = ˙x

E(q) ∩ O1...M = ∅. (20)

This node will then have a child node created that contains a path of the maximum length possible for the car to travel in the selected time step τ . Because these constraints are being taken into account, a path going out from a parking space that is narrow lengthwise to the car will always develop in a zigzag pattern. Once the backward tree has a node outside P, it continues to be generated in a identical way as the forward tree.

D. Determining a path

Using two trees speeds up the algorithm substantially, since it removes the need for a random tree to find its way to a small target. Instead the two trees can branch out and when

they happen to intersect the path from the intersection to the root of each tree will be a feasible path.

Once the two trees has intersected a feasible path from the starting point to the parking goal can be created by combining the parent nodes of the nodes that intersected into a new set of nodes, referred to as P ath in Algorithm 1. As the path is made up of a tree of nodes with path segments, the path from the intersection back to the start and end points will always be unambiguous.

FB-RRT is a time discrete algorithm. So its time-step, τ ∈ R+, can be varied to increase accuracy or speed as desired. As it is only intended to provide the optimisation with a starting guess, it does not take into account the cost function described in (15). This means that it will not do anything to minimise the number of control actions nor the time it takes to drive along the path it suggests. Its only priority is to provide the first feasible path it finds to the optimiser.

Since this implementation uses repeated randomisation and is terminated only when a working path is found, this algorithm will continue until any feasible path is found. So to be able to handle situations that are unfeasible the number of tries has to be limited or the algorithm continues infinitely.

In practice the number of iterations needed to complete a trajectory for the most complex parking scenario, as seen in Fig. 9 was an average of 223 over 10 tries, with the maximum being 369 iterations and the minimum 122.

E. Path optimisation

In order to solve the problem a better mathematical description of the model will be used. To begin with, the obstacles are

Om= {y ∈ R²: Amy ≤ bm}. (21) Where Am∈ R^l^m^×2 is a matrix, bm ∈ R^l^m is a vector, and l_m∈ N correspond to the number of faces on the obstacle.

In order to calculate whether or not a collisions occurred, we will use Lemma 1.

Lemma 1: [11] The condition that dist(E, Om) > d is equivalent to that

∃λm≥ 0, µm≥ 0 :

− g^>µ_m+ (A_m

px

py

− b_m)^>λ_m> d, G^>µm+ R(ϑ)^>A^>_mλm=

0 0

, kA^>_mλmk ≤ 1.

Intuitively Lemma 1 means there’s no collision if and only if you can find a λm∈ R^l^m and a µm∈ R⁴ that satisfy the equation(each obstacle have to be considered separately). Thus

(6)

the optimisation problem (16) can be written as min

x,u,t^f,λ,µ

t^f+ Z t^f

0

(u^>Qu + ˙u^>Q_du)dt˙ s.t. x(t = 0) = x^s, x(t = t^f) = x^f,

˙

x(t = 0) = ˙x(t = t^f) = 0,

˙ x =



v tan δ

`



,

vmin

δmin

≤ u ≤

vmax

δmax

,

amin

α_min

≤ ˙u ≤

amax

α_max

,

−g^>µm+ (Am

p_x p_y

− bm)^>λm> d, G^>µ_m+ R(ϑ)^>A^>_mλ_m= 0,

kA^>_mλ_mk ≤ 1,

∀t ∈ [0, t^f], ∀m = 1, 2, ..., M.

(22)

IV. SIMULATION

Simulated implementation of theoretical models have been done in Matlab running on Linux using a Intel i7-3820 at 3.6 GHz and 16 GB RAM. The following auxiliary libraries ICLOCS2 and MPT3 were alse used. Using ICLOCS2 the problem has been formulated in time-continuous form and the software has taken care of converting it into time-discrete form [12]. The solver used for the optimisation in ICLOCS2 was Ipopt.

In the implementation of FB-RRT, Yalmip was used for solving the optimisation problem of finding minimum distances. Since Matlab is an interpreted language which is highly optimised for Matrix algebra, every mathematical operation was implemented in matrix form. MPT3 was used to handle the geometric figures that symbolise the car and obstacles in the simulation.

During simulation, Q and Qd in the cost function described in Eq. (15) was set to 0. This means that the optimisation does not take into account how many control actions are made (other than that they a maximum limit). In a practical implementation these cost functions would need to be adjusted to reduce wear on vehicle components as well as making the ride more comfortable, but optimising ride comfort and machine wear was outside the scope of this project.

The FB-RRT algorithm depends on randomised points for building up its trajectory so each run will result in a different path. The algorithm may suggest any number of paths from a starting point to an endpoint, but the optimisation will ensure that the path becomes a local minimum. Its changes to the trajectory suggested by the FB-RRT are usually minor, but if the FB-RRT happens to make a suggestion that is very poorly optimised the optimisation will make major changes to it. The results of the simulation can be summed up as six findings:

1) The FB-RRT, as described in Algorithm 1 and shown in Fig. 3, can be used to create a heuristic warm-start for the optimisation solver. It works by building two trees made up of nodes that represent feasible pieces of a driving path, whose heading is randomised but computed to conform with the dynamic constraints of

-15 -10 -5 0 5 10 15

-2 0 2 4 6 8 10 12 14

Fig. 3: A graphic view of how FB-RRT works. The green tree is the backwards tree, and the characteristic zigzag pattern indicates how it gets out pf the narrow space as fast as possible while still respecting the vehicle dynamics. The black line represents the forward tree and the dotted red line marks the final trajectory. Crosses and circles mark the random guesses.

the vehicle. The trees will be certain to intersect, if there is a possible path exists, and this helps to get around the computational complexity of optimising for non-convex trajectories.

2) The method of combining the FB-RRT algorithm with optimisation has produced a robust way of finding an efficient trajectory from a random point to a parking space. The computation time became drastically short- ened with FB-RRT. Running the simulation shown in Fig. 6 without warm-starting yielded a computation time of 4500 s, compared to around 150 s with warm start.

3) FB-RRT uses randomisation, so it produces slightly different results every time, as can be seen in Fig. 4 which represents three different ways that the algorithm has found for parallel parking in an extremely narrow space. But the trajectories that it produces are always smooth and feasible.

4) The benchmark set for this project has been five parking scenarios:

1. Parallel parking in a very narrow slot, Fig. 5 2. Driving head first and parking in a narrow alley,

Fig. 6

3. Reversing through a narrow alley to park, Fig. 7 4. Parking in a slanted parking lot, coming from the

side opposite to the slant, Fig. 8

5. Parking in a crowded parking place between build- ings, Fig. 9

These scenarios were chosen because they are common in literature that deals with autonomous parking. They represent typical parking situations that contains difficult angles, narrow spaces and varying obstacles that would be difficult for a human driver. These shows that the solution works, even for quite complicated parking situations.

(7)

-15 -10 -5 0 5 10 15 -2

0 2 4 6 8 10 12 14 16 18 20

(a) First try entering a very narrow parking space. Time to finish 21.3 s

-15 -10 -5 0 5 10 15

-2 0 2 4 6 8 10 12 14 16 18 20

(b) Second try entering a very narrow parking space. Time to finish 17.0 s

-15 -10 -5 0 5 10 15

-2 0 2 4 6 8 10 12 14 16 18 20

(c) Third try entering a very narrow parking space. Time to finish 28.1 s Fig. 4: Different results for each run

(a) FB-RRT for for scenario 1 (b) Trajectory after optimisation

(c) Velocity during parallel parking. The horizontal lines show the limits. Negative values represent reversing.

(d) Steering angle during parallel parking.

The horizontal lines show the limits.

Fig. 5: Scenario 1 - Parallel parking

5) The time to reach the parking space varies depending on how far the vehicle has to travel as well as how complicated turns have to be made. The dynamic constraints that keep it from making arbitrarily narrow turns can make parking trajectories that are short in absolute distances take a lot longer than trajectories that are longer but straighter. This can be seen in Fig. 10.

6) The final trajectory respects the limits set on velocity and steering angle, as can be seen in Fig. 5, 6, 7, 8 and 9. It should be noted, however, that the simulations were made with relatively long time-steps on the order of

whole seconds, to make computationally tractable. This means that the limitations on the rate of change of the inputs could not be verified as a single time-step is long enough for the input to be saturated without violating the rate constraints.

V. DISCUSSION

As shown in the simulations, our implementation of FB- RRT combined with optimisation is reliable and efficient in planning a parking trajectory that avoids obstacles and always reaches its destination if there is a feasible path.

(8)

(c) Velocity while parking. The horizontal lines show the limits. Negative values represent reversing.

(d) Steering angle while parking. The horizontal lines show the limits.

Fig. 6: Scenario 2 - Parking front first in a narrow alley

Fig. 7: Scenario 3 - Parking back first in a narrow alley

(9)

Fig. 8: Scenario 4 - slanted parking space

(c) Velocity during parking. The horizontal lines show the limits. Negative values represent reversing.

(d) Steering angle during parking. The horizontal lines show the limits.

Fig. 9: Scenario 5 - Parking in a cramped and complex spot

(10)

-15 -10 -5 0 5 10 15 -2

0 2 4 6 8 10 12 14 16 18

20 100 s

20 s 50 s

Fig. 10: Time to parallel park from different positions. The times are colour coded, so the fastest time, 14 s is clear green and the slowest, 99 s, is clear red. Timings in between those extremes are represented by a gradient from red to green.

A practical implementation of this model on an actual vehicle would require that the vehicle model is extended with external factors such as inertia and limits in friction. The cost function for control actions would also need to be refined in order to satisfy the needs for the implementation.

It would also seem likely that a practical implementation would use a different set of inputs. In our model velocity is taken as an input, but it would seem more suitable for a practical implementation to use the throttle of a petrol engine or the frequency of the power fed to an electric motor.

This would mean changing u in equation (3), as well as its derivative, limits and cost function.

One should also note that the input given by equation (3) might have to be changed if the car model has different input.

As a result some equations have to be altered including its derivative, state space, limits and the cost. But a lot of the rest can be kept the same.

Another change would need to be made in the dynamics to account for the placement of the vehicles rear axle. Since this project has not been based on any actual vehicle, the rear axle was arbitrarily defined as being placed in the extreme back end of the vehicle, so that in other words the wheelbase is the entire length of the car. For a more accurate simulation of a typical personal vehicle one should redefine the region occupied by the car as

B =

px

py

∈ R²:

−or≤ px≤ ` + of

−h

2 ≤ p_y≤ ^h₂

(23) where or is the rear overhang and of is the front overhang.

For a practical implementation of this project, a system of sensors and a software for mapping of the environment would have to be used. Since this project was entirely based on simulations, the parking environment was considered as completely known.

Another limitation of this project is that it considers a static environment where no changes occur after parking has started.

For a practical implementation there would have to be a routine for determining when the environment has changed and a new

trajectory would have to be calculated. Since not all targets are stationary it would also have to implement a method for calculating the trajectories of moving obstacles.

For this project the code that implements the mathematical models described, was written in Matlab and while some of the linked libraries were compiled, important parts of MPT3 were written in Matlab. To make a computationally efficient solution, the parts of these libraries that are used should be re-implemented in a more efficient language. One particular area of improvement would be that the regions occupied by objects should be calculated using a specially written routine for this, since this function is used many times during every part of the parking process.

A benefit of the FB-RRT is that it enables multi-threading of the forward and backward tree, which potentially almost dou- bles performance on a computer with more than one processor core. This could be expanded, so that several branches of each tree are being generated in parallel, but performance gains would be diminishing because it would add to the complexity of determining which branch is the optimal.

VI. CONCLUSIONS

In this report an efficient, two tiered, solution using a new heuristic method called FB-RRT was applied to warm start an optimisation to produce a parking trajectory for autonomous parking. A proof of concept for the described method was given in the form of simulations. The simulations showed it to be reliable for finding a feasible path even i complex parking situations respecting the dynamic constraints. The application of a warm start was shown to greatly increase computation performance in the optimisation stage, while also adding a certain amount of variability in the solutions. The performance was bench marked using five different parking scenarios, and the solution was shown to work for all of them.

VII. ACKNOWLEDGEMENTS

The authors would like to thank our supervisor, PhD student Yulong Gao at the Division of Decision and Control Systems at the KTH Royal Institute of Technology, who provided invalu- able help and feedback in all parts of the project. Without him we would not have had the mathematical knowledge needed to produce this paper, and our solution would have been much less advanced.

REFERENCES

[1] “Owners guide,” 2019. [Online]. Available: https:

//www.tesla.com/content/dam/tesla/Ownership/Own/Model%203%

20Owners%20Manual.pdf

[2] R. Hemmecke, M. K¨oppe, J. Lee, and R. Weismantel, “Nonlinear integer programming,” in 50 Years of Integer Programming 1958-2008.

Springer, 2010, pp. 561–618.

[3] P. Jain, P. Kar et al., “Non-convex optimization for machine learning,”

vol. 10, no. 3-4. Now Publishers, Inc., 2017, pp. 142–336.

[4] B. Li, K. Wang, and Z. Shao, “Time-optimal maneuver planning in automatic parallel parking using a simultaneous dynamic optimization approach,” vol. 17, no. 11, 2016, pp. 3263–3274.

[5] A. W¨achter, “An interior point algorithm for large-scale nonlinear optimization with applications in process engineering,” Ph.D. dissertation, Carnegie Mellon University, Pittsburgh, Pennsylvania, 2002.

(11)

[6] R. Chai, A. Tsourdos, A. Savvaris, S. Chai, and Y. Xia, “Two-stage trajectory optimization for autonomous ground vehicles parking maneuver,” 2018.

[7] S. M. Lavalle and J. J. K. Jr., “Rapidly-exploring random trees: Progress and prospects,” in Algorithmic and Computational Robotics: New Di- rections, 2000, pp. 293–308.

[8] Z. Feng, S. tao Chen, Y. Chen, and N. Zheng, “Model-based decision making with imagination for autonomous parking,” 2018, pp. 2216–

2223.

[9] A. Boyali, S. Mita, and V. John, “A tutorial on autonomous vehicle steering controller design, simulation and implementation,” CoRR, vol.

abs/1803.03758, 2018. [Online]. Available: http://arxiv.org/abs/1803.

03758

[10] X. Zhang, A. Liniger, A. Sakai, and F. Borrelli, “Autonomous parking using optimization-based collision avoidance,” in 2018 IEEE Conference on Decision and Control (CDC), 2018, pp. 4327–4332.

[11] X. Zhang, A. Liniger, and F. Borrelli, “Optimization-based collision avoidance,” arXiv preprint arXiv:1711.03449, 2017. [Online]. Available:

https://arxiv.org/abs/1711.03449

[12] P. Falugi, E. Kerrigan, and E. V. Wyk, “Imperial college london optimal control software user guide (iclocs),” 2010.