Implementation of Autonomous Parking with Two Path Planning Algorithms
Linda Bui and Malin H¨aggstr¨om
Abstract—In urban environments, many drivers find it difficult to park, calling for an autonomous system to step in. Two path planning algorithms, Hybrid A* and RRT* Reeds-Shepp, were compared in parking scenarios with a small model car both in simulation and implementation. Two scenarios were tested: the vehicle facing inwards and outwards from the parking space, the latter requiring the vehicle to change direction at some point. The Hybrid A* algorithm proved to be more reliable than RRT* Reeds-Shepp since the latter generates random paths, while the first found longer paths that were not as optimal.
RRT* Reeds-Shepp found a path in the forward scenario 9 out 10 times and 4 out of 5 in the reverse scenario. The planned paths were tested with the Stanley controller. The front wheels were given as guiding wheels both in forward and reverse, which worked well in simulation. In implementation, the first scenario matched the simulation quite closely. In the second scenario in implementation, the model cars were not able follow the path when reversing with the front wheels as guiding wheels. In conclusion, one algorithm did not outshine other but it also depended on the scenario. However, it was not optimal to use the front axle as a reference in the reverse driving situation.
Index Terms—Hybrid A*, RRT* Reeds-Shepp, Stanley con- troller, path planning, path tracking, implementation, low speed parking
TRITA-EECS-EX-2019:117
I. INTRODUCTION
In busy cities and crowded streets many drivers find them- selves in the difficult situation of not only finding a decent parking spot, but also being able to park. The parking spot might be very narrow and ongoing traffic might be stressful for the driver. In addition to this, the human factor adds several issues such as moving vehicles with unclear intention or persons loading and unloading their car.
Today, we have autonomous parking systems in some of our cars, but these systems often require some form of interaction with the drivers; meaning that these parking systems are not entirely autonomous. It can be difficult for autonomous park- ing systems to determine e.g if there is a risk of collision when parking or change gear when needed, which is why the driver has to step in. An approach, where the autonomous parking situation is divided into path planning and path tracking, is proposed.
In this project, the objective of the path planning problem is to find a feasible parking solution for the vehicle in question. Two well known path planning algorithms, Hybrid A* and RRT* Reeds-Shepp, are compared and implemented in small model cars in two different scenarios. When the path planners generates a trajectory the chosen path tracker, Stanley
controller, is executed to follow it. Input is provided from the user to the path planner. The output is processed by the path tracker, which gives control signals to the vehicle. Motion capture refers to the system used in the implementation of the autonomous parking system. It is utilized when testing the parking system and provides the tracker with the position of the vehicle. Overview of the approach used in this project can be seen in figure 1.
Input
Planner
Tracker
Vehicle Motion Capture
Fig. 1. Overview of the system structure
II. PROBLEM FORMULATION
The project focuses on planning a path to park a vehicle and test how accurately it can be followed by a tracker through implementation on a small car. The planning will be done by two different algorithms, Hybrid A* and RRT* Reeds-Shepp, and they will be compared to see which suits the scenarios the best. As for the tracking, Stanley controller is used to follow the paths that has been calculated and to adjust the model car’s steering and velocity to follow the path as closely as possible.
The input provided to the Hybrid A* and RRT* Reeds-Shepp are the initial position, goal position, initial speed, and target speed. The position of the car includes the direction and heading angle of the vehicle.
The path planners are compared in two different scenarios.
The first scenario is perpendicular parking, meaning the ve- hicle drives forward and turns when needed, into a parking space. The vehicle will then face into the parking space. The second scenario is reverse parking with the vehicle facing outwards, which requires the vehicle to change direction at some point and reverse into the parking space.
III. BICYCLEMODEL
The bicycle model, also called single-track vehicle model, is used as an approximation of the dynamics of a vehicle with four wheels, which is yielded by:
˙
x = uscos(ψ)
˙
y = ussin(ψ) ψ =˙ uLstan(δ)
, (1)
where us denotes the velocity, L is the wheelbase of the vehicle, ψ is the rear axle orientation with regard to the path, and δ is the steering angle of the car, see figure 3. It is assumed that the there is no friction coefficient between the wheels and the trajectory when simulating [1].
The controller used in this project, see section V-A, de- scribes the motion of the vehicle by using the front axle as a reference.
IV. PATHPLANNING
General path planning in this project is viewed as a search in a state space X. Let xsdenote a state, which is a representation of a specific real world event. The state space X is a set of all possible states in e.g Rm, where m ∈ N. The path planning problem involves finding the applied actions, or control inputs, u, that transforms an initial state xi∈ X to a goal state xg∈ Xg, where Xg∈ X is a set of several goal states [1].
A. Reeds-Shepp curves
The Reeds-Shepp curves are extensions of Dubins curves [2]. Dubins curves are applied when finding the shortest trajectory between two points in R2. If there is no limitations on the steering angle δ, then the shortest path between start and goal is a straight line. In Dubins curve the speed is constant and the vehicle only has a forward direction, which yields the system:
˙
x = cos(ψ)
˙
y = sin(ψ) ψ = u˙
, (2)
where u is the input:
u ∈ {− tan(δmax), 0, tan(δmax)}
According to [3], every optimized trajectory can be de- scribed with three motion primitives in a certain order. A sequence is called a word and each primitive refers to a constant motion during a time period. Let L, R, S denote the motion primitives and correspond to a sharp left turn, a sharp right turn, and a straight forward move respectively. The word that represents the shortest trajectory is called a Dubins curve.
Table I shows the steering for the motion primitives below.
TABLE I
STEERING FOR MOTION PRIMITIVESL,S,R
Motion primitives Steer L u = − tan(δmax)
S 0
R u = tan(δmax)
Two equal symbols in a sequence results in a merge, which means that e.g in a sequence {L1, L2, L3} : L1 6= L2 6= L3. Dubins, proves the following six words are optimal [3]:
{LRL, RLR, LSL, LSR, RSL, RSR} (3) To determine the exact Dubins path, let the period of the primitives:
α, γ ∈ [0, 2π) β ∈ (π, 2π) d ≥ 0
, (4)
where α, γ, β is the total rotation of L and R, and d is the distance of S traveled, during the time period of L, S, R. This results in the characterized Dubins curves:
{LαRβLγ, RαLβRγ, LαSdLγ, LαSdRγ, RαSdLγ, RαSdRγ} (5) Let C be a symbol that represents a curve similar to R or L. The six words in (5) can then be written in a more compact form:
{CαCβCγ, CαSdCγ} (6) Reeds-Shepp curves use the same motion primitives as Dubins curves. A Dubins curve has its constraints since it only expects a positive velocity and forward direction. Reeds-Shepp curves allows the vehicle to drive forward and backwards, making it possible to handle reverse driving. The Reeds- Shepp’s control system can be seen as an extension of (2) in the following way:
˙
x = u1cos(ψ)
˙
y = u1cos(ψ) ψ = u˙ 1u2
, (7)
where u1 represents the directions and u2 represents the steering; u2 is the same as u in (2). Once again, the Reeds- Shepp curves allows two directions, forwards and backwards, which results in u1= 1 or u1= −1 respectively. The Reeds- Shepp curves are represented by 46 words and a new motion primitive, |, is presented and refers to a switch from one direction to another [2]. In addition to this, the subscripts ± are used to indicate the current direction [1].
B. Hybrid A*
Hybrid A*, pronounced A star, is an extension of the A*
algorithm [4]. A* is a forward search algorithm that is applied in discrete planning. When applying an action u from the state xs, xsgenerates a new state, ˙xs, which can be described with a state transition function, f :
˙
xs= f (xs, u) (8)
Every possible u applied from xs can be represented by an action space U (xs), where the set U is defined as:
U = [
xs∈X
U (xs), (9)
meaning U is the set of all potential actions over all states.
Further, let Xg denote a set of goal states, where Xg ⊂ X.
According to [1], the discrete planning can be formulated as a directed state transition graph where X is a set of vertices.
If there is an action u ∈ U (xs) such that a new state ˙xs∈ X can be generated from xs∈ X, there exists an edge from xs
to ˙xs.
Once again, A* is a forward search algorithm. In a general forward search, there are three different states during a search:
• Non-visited: Refers to states that are non-visited. Every state is non-visited except xi initially.
• Dead: Refers to visited states and every possible next state ˙xs that has been visited, that does not contribute to finding a possible plan.
• Alive: Refers to states that has been visited but still have non-visited next states.
Let l(e) (≥ 0) be the cost of an applied action in an edge e in a discrete planning scenario represented by a graph. l(e) can be rewritten in a state-space form l(x, u), where l(x, u) is the cost of applying an action u from a state xs. Every xs has a value C∗(xs), in which C∗(xs) is the optimal cost- to-come from an initial state xi. The optimal cost-to-come is calculated by adding all l(e) over every possible path from xi
to xs and choosing the minimum cost. When a new state ˙xs
is generated, the cost of this state is calculated by:
C( ˙xs) = C∗(xs) + l(e), (10) where e is the edge from state xs to new state ˙xs. The A*
algorithm is based on a function that estimates the cost from xs to xg. Now let G(xs) represent the cost-to-go from xsto a goal state xg ∈ Xg. The true optimal cost-to-go G∗ is not possible to know beforehand but there is a way to calculate an underestimate of this value. Let the underestimate of G∗(x) be denoted by ˆG∗(x). Further, let Q be a priority queue that is arranged after the sum:
C∗( ˙xs) + ˆG∗( ˙xs), (11) in which it is implied that Q is arranged by approximations of the optimal cost from xi to xg∈ Xg [1].
The difference between A* and Hybrid A* is that the vertices in Hybrid A* can find continuous points in the graph grid, even though the graph is discrete. This means that during an A* search, the vertices can only expand to the middle of the cell while Hybrid A* can expand to any point within the cell, see figure 2.
The state space X in Hybrid A* consists of states defined as xs = (x, y, ψ), which makes it a three dimensional search space. x, y denote the vertex’s position and ψ denote the vertex’s heading. Since the search space is three dimensional, it needs to be discretized to enable the Hybrid A* search.
Hybrid A* calculates the Reeds-Shepp curves, see section IV-A, when its vertices are expanded towards xg. The calcu- lation of Reed Shepp curves implies that Hybrid A* makes it possible for backward direction. The Reeds-Shepp curves are a part of the calculations towards xg since the discrete control actions are not sufficient alone. The search is ended when a generated path is collision checked and returns no collisions.
However, the generated path from Hybrid A* is not always
the most optimal path because of the continuous states in the discrete grid though there are penalty costs added for switching direction and driving in reverse [4].
The Hybrid A* algorithm was chosen as one of the path planners because it is an extension of the well-known A* al- gorithm and calculates the Reeds-Shepp curves, which handles reverse driving.
(a) A* search (b) Hybrid A* search
Fig. 2. Search grids. Taken from [4].
In this project, the Hybrid A* algorithm written by Chen Bohan was used [5].
C. Rapidly-exploring Random Trees* Reeds-Shepp
Lavalle introduced the Rapidly-exploring Random Trees, RRT, as an exploring algorithm in [6]. RRT generates a tree that utilizes random sampling of states in a given state space X. If there are obstacles in the search space, let Xodenote the obstacle region in X, Xo⊂ X. Further, let ρ denote a function of the distance between two points in X. It is assumed that ρ is optimal, meaning the distance between point a and point c is less than or equal to the resulting distance when traveling through point b situated between a and c.
Given the initial and goal states, xsand xg respectively, the tree expands itself from xi to xg through several iterations.
During every iteration, a state xr from the random sample in X, is generated and connected to the nearest point that exists in the tree. The nearest point is decided according to ρ. If the random state xr ∈ X/ o, and is within a specified ρ from the nearest point, the tree expands by connecting the given points.
However, if the nearest point is not within ρ, or xr∈ Xo, a new state ˙xr is created and connected with the nearest point.
This is done by a function, called steer, that provides an action u, enabling the expansion of the tree through the connection between ˙xr and the nearest point. The connection between the points represents a path and is checked for obstacles with a collision check function. The collision check is a boolean function that returns True or False depending on if there is an obstacle free path or not. The algorithm does not stop running until the given amount of iterations are executed, even if the desired path is found already [7].
The Rapidly-exploring Random Tree*, RRT*, is an algo- rithm that extends RRT to overcome its shortcomings. One difference is that the RRT* algorithm has a function called near neighbour search that finds the nearby point, within a specified area, with the least cost from xr; meaning that the nearest point does not necessarily has the least cost from xr.
If one of the neighbouring points has a lesser cost than the nearest point, the cheapest point replaces the nearest point and connects with xr. Another difference is a function that rewires the tree. When xr is linked with the cheapest neighbour, an additional near neighbour search is initiated to check if rewiring the tree to one of these points decreases the cost.
The tree rewires to a new neighbouring point if it yields a lesser cost [8].
RRT* Reeds-Shepp was used in this project and is a combi- nation of RRT* and Reeds-Shepp. The Reeds-Shepp curves are calculated in the steer function, implying the ˙xr provided are generated Reeds-Shepp curves; enabling backwards driving for the vehicle. Examples of tree expansions can be seen in figure 8, 10 and 12.
The RRT* Reeds-Shepp algorithm was chosen as one of the path planners because it is an extension of the well known RRT*, converges towards xg fast, and calculates the Reeds- Shepp curves, which handles reverse driving.
In this project, the RRT* Reeds-Shepp algorithm, written by Atsushi Sakai, was used [9].
V. PATHTRACKING
When the path planning algorithms generate feasible paths, the path tracking algorithms are used to follow the paths with a certain precision. The path tracker used in this project is called Stanley controller. The Stanley controller is a path tracking approach developed at Stanford University in an au- tonomous vehicle challenge started by the Defense Advanced Research Projects Agency, DARPA. Stanley controller takes the parameters speed and heading angle of the vehicle, and the trajectory’s x and y coordinates as well as the heading angle of the coordinates. The initial state of the car is set manually and includes the starting point, initial heading angle and speed [10].
A. Stanley Controller
The Stanley controller analyzes the direction of the front wheels with regard to the planned trajectory. This method is based on a nonlinear feedback function of an error measured from the center of the car’s front axle to the nearest point of the trajectory, see figure 3. The trajectory generated from a path planning algorithm, can be given in x and y coordinates in the following way:
[x1, x2, . . . , xn−1, xn], [y1, y2, . . . , yn−1, yn] (12) The front axle position, in x and y coordinates, is calculated by:
fx,y =
(xs+ L cos(ψs)
ys+ L sin(ψs) , (13) where (xs, ys) is the current position of the vehicle, L is the wheel base of the vehicle and ψs is the current heading angle of the vehicle, see figure 3. The error between the center of the front axle and the nearest path point, (xp, yp), is called the cross track error ef a, and is determined by comparing
the distance from the front axle position,(fx, fy), to every trajectory point.
The steering control law consists of two terms, the heading error, θe, and θd, which corrects the cross track error. θe
normalizes the heading error, depending on what the current heading angle is, and θd is given by:
θd= arctan kef a(t) v(t)
, (14)
where k is the gain parameter and v(t) is the velocity of the car. This results in the following steering control law, δ(t):
δ(t) = θe(t) + arctan kef a(t) v(t)
(15) If ef a increases, the steering control law adjusts δ to steer the front wheels closer to the desired path. When ef ais small, the position of the vehicle converges to the path with the constant k, since the term v(t)1 is multiplied with k.
However, the vehicle model used in this project sets con- straints on some of the parameters, such as the maximal steering angle, ±δmax. When θdis greater or less than ±δmax, the steering angle δ(t) is set to ±δmax. Another constrained parameter is v(t), since the project is about autonomous parking in certain scenarios with low speed.
x y
0 1 2 3 4 5 6
0 1 2 3 4 5 6
δ ψ p
(fx, fy)
Fig. 3. A vehicle following a path with Stanley controller, where ψ is the angle between the x-axis of the coordinate system and the vehicle’s direction, δ is the angle the front wheel base is turned, (fx, fy) is the front axle position, and p is the next point on the path.
The Stanley method was chosen as the path tracker because it is a well-known method that handles the low-speed driving [10].
In this project, the Stanley controller algorithm written by Atsushi Sakai was used [11].
VI. ADDITIONS
The path planning and path tracking algorithms were picked right off the shelf. To handle the reverse parking scenario, some additions were added to code.
A. Reeds-Shepp Curves
When generating the most optimal trajectory, Reeds-Shepp returns some information of the path including x and y coordinates, and the heading angle ψ of each path point. The addition to this code is that Reeds-Shepp also returnes the direction of each point of the path; 1 representing forward and −1 representing reverse.
B. RRT* Reeds-Shepp
As mentioned in section IV-C, RRT* Reeds-Shepp uses Reeds-Shepp curves in the steer function. The addition in Reeds-Shepp curves results in that RRT* Reeds-Shepp also returns the direction of the path points.
C. Stanley Method
In reverse parking scenarios and some perpendicular parking scenarios, the vehicle needs to change direction in order to park correctly. In some of the perpendicular parking scenarios, the car may need to reverse when e.g correct the heading angle, see figure 9. In order for the Stanley controller to follow the path properly, the direction is provided as an input. Depending on the direction, the front axle position is calculated according to equation (13) if the vehicle drives forward. The backward direction results in the following:
fx,y =
(xs− Lcos(ψs)
ys− Lsin(ψs) , (16) thus making sure the front wheels are still used as steering wheels.
D. Splitting and Prolonging Paths
When switching from forward to reverse and vice versa, the paths were split each time they switched. The planning algorithms provided a direction for each coordinate which had one value for forward and another for reverse. Each direction was compared to the next, and the path was split every time it changed.
In order to ensure the vehicle moved to the end coordinate of the planned path the Stanley controller was given some marginal through prolonging the paths. The next point of the prolonged part was calculated by
xn+1= sstepxn± cos(ψn) (17) with a plus sign for forward driving and minus for reverse.
The step size, sstep, was calculated with Pythagoras theorem between two adjacent coordinates. yn+1was calculated by the same equation with sin instead, ψn+1 and the direction were set the same as the nth element of each.
VII. IMPLEMENTATIONTOOLS
A. Model Car
A model car was used with the following measurements:
• Length: 0.586 m
• Width: 0.2485 m
• Wheel base: 0.324 m
• Wheel width: 0.02 m
• Back to wheel: 0.16 m
which were also used as measurements in the simulations.
B. ROS-platform
The Robot Operating System, ROS, is an open source operating system created for robots which the model cars use [12]. All the code implemented the model cars were launched via ROS and modified from code written by Frank Jiang who developed the code used in the model cars.
C. Qualisys
Qualisys track manager is a motion capture system which was used to capture the model car’s positions in the lab [13].
VIII. RESULTS
A. Simulation
The following input variables were set for the perpendicular parking scenario:
• start point: sx = 0.5 m, sy = 1.5 m, sψ = 0
• goal point: gx = 2.5 m, gy = 0.8 m, gψ = π2
• target velocity: v = ±0.6 m/s, and for the reverse parking scenario:
• start point: sx = 0.5 m, sy = 1.5 m, sψ = 0
• goal point: gx = 2.5 m, gy = 0.8 m, gψ = 3π4
• velocity: v = ±0.6 m/s
with the maximum steering set as π6 rad. The speed is positive or negative, if the car drives forwards or backwards respectively.
Different numbers of iterations for RRT* Reeds-Shepp were tested to find an appropriate number during the simulations.
The numbers of iterations that was tested was 400 iterations, 300 iterations, 100 iterations and 70 iterations. It was decided that 70 iterations was appropriate for this project.
The objective of this project was to compare two different planning algorithms, with the same tracking algorithm, in two different parking situations; perpendicular parking and reverse parking. Each scenario was tested with both Hybrid A* and RRT* Reeds-Shepp, where RRT* Reeds-Shepp was tested several times for each scenario. If a feasible path was generated, it was extended and the Stanley controller was executed to follow it. The path was extended by dividing it into smaller parts depending on the direction and then each part was elongated. The extension resulted in the vehicle adjusting to certain position and direction when switching direction, which made it possible to follow the given trajectories as closely as possible, as seen in figure 5, 7, 9, and 11.
The bicycle model was used to simulate the vehicle model.
The star in the simulation figures below represents the rear axle of the car. The cross in each figure describes the next path point p, which the car follows.
For the Hybrid A* algorithm, the same path was generated if there was a path. The planned trajectories shown in figure 4 and figure 6 were generated in all the tests.
Fig. 4. The perpendicular parking path planned with Hybrid A*, with the car in starting and goal positions.
Fig. 5. The perpendicular parking path planned with Hybrid A*, with Stanley controller following the path, showing the car where the tracker ends its route.
The extended and pursued trajectories from Hybrid A* can be seen in figure 5 and figure 7.
The RRT* Reeds-Shepp was executed 10 times in the per- pendicular parking scenario and 5 times in the reverse parking scenario. In simulation, the RRT* Reeds-Shepp algorithm found a path 9 out of 10 times in the forward scenario, and 4 out of 5 times in the reverse scenario. Figure 12 shows one of the failed attempts in finding a feasible path with RRT*
Reeds-Shepp; there is no dotted line since there is no path to generate.
The dotted lines in figure 8 and figure 10 represents the final generated courses that RRT* Reeds-Shepp found most optimal in the expanded tree.
B. Implementation
The model cars deviated slightly from the simulated route.
The plotted route from figure 5 was projected from above scaled 1 : 1 and the model car was placed in the start of the
Fig. 6. The reverse parking path planned with Hybrid A*, with the car in starting and goal positions.
Fig. 7. The reverse parking path planned with Hybrid A*, followed by Stanley controller, showing the car where the tracker ends its route.
projected path. The environment where the implementation was tested was in a lab for both model cars and drones, with a safety net hanging above the testing area. In the figures the shadow of this is seen; it is not a projected grid. The green cross marks where the vehicle should stop. The figures below shows the tests of the implementations in a sequential order.
In the first scenario with the path planned by Hybrid A*, the model car started to turn later than in the simulation and stopped about 0.3 m too early from the goal position, seen in figure 13.
In the reverse scenario, the model car followed the path with forward direction correctly. However, when reversing the vehicle lost its way and reversed into the obstacles, as seen in figure 14.
The RRT* Reeds-Shepp’s optimal path was to drive in a bit further in the parking spot and then reverse to the goal position.
The vehicle followed the forward direction correctly and due to the path extension, the vehicle corrected its position to switch
Fig. 8. The perpendicular parking path planned with RRT* Reeds-Shepp, with the car in starting and goal positions. The green lines represent the tree expansion.
Fig. 9. The perpendicular parking path planned with RRT* Reeds-Shepp, followed by Stanley controller, showing the car where the tracker ends its route.
direction. The car followed the reverse path accordingly but did not park in the given spot, see figure 15.
The forward parking results with Hybrid A* and RRT*
Reeds-Shepp are rather similar. The difference is that RRT*
Reeds-Shepp finds it more optimal to drive a bit further in and then reverse into the goal position, seen in figure 15. However, the results from the planners in the reverse parking scenario, figure 14 and figure 16, was very different. The Hybrid A*
generated a more complicated trajectory, switching direction 3 times, whereas RRT* Reeds-Shepp chose a more simple trajectory where the vehicle only switched direction 1 time.
In the reverse parking scenario with RRT* Reeds-Shepp, the vehicle deviated from the path when it switched direction. The vehicle steered into the obstacles and parked in the projected obstacle wall, see figure 16.
Fig. 10. The reverse parking path planned with RRT* Reeds-Shepp, with the car in starting and goal positions. The green lines represent the tree expansion.
Fig. 11. The reverse parking path planned with RRT* Reeds-Shepp, followed by Stanley controller, showing the car where the tracker ends its route.
IX. DISCUSSION
A. Hybrid A*
Since the grid is discretized, not ensuring an optimal path, Hybrid A* generated a much longer path than what was needed in the reverse scenario. Different penalty costs were tested but resulted in the same path. In a real world parking scenario, this could be quite inconvenient, especially if there are many other vehicles around waiting to park. It could take a lot of time, risking the environment to change if other vehicles move which might make the planned path impossible to follow without crashing. The forward scenario was however simple and only required the vehicle to drive forward.
B. RRT* Reeds-Shepp
Since the nodes for the path are generated at random, the algorithm might not always find a path, depending on the number of iterations. The tree expands itself through several
Fig. 12. Failed attempt in finding a feasible path with RRT* Reeds-Shepp.
The green lines represent the tree expansion.
Fig. 13. The model car in its parking position with Hybrid A* in the perpendicular parking scenario. The line with arrows represents the path traveled by the model car. The simulated route with Stanley controller is projected from above.
Fig. 14. The model car in its parking position with Hybrid A* in the reverse parking scenario. The line with arrows represents the path traveled by the model car. The simulated route with Stanley controller is projected from above.
Fig. 15. The model car in its parking position with RRT* Reeds-Shepp in the perpendicular parking scenario. The line with arrows represents the path traveled by the model car. The simulated route with Stanley controller is projected from above.
Fig. 16. The model car in its parking position with RRT* Reeds-Shepp in the reverse parking scenario. The line with arrows represents the path traveled by the model car. The simulated route with Stanley controller is projected from above.
iterations towards spaces that has not been searched yet; often, the tree expands towards the to goal state.
Choosing the number of iterations was a trade-off between the ability to find the given goal state and the time it took to execute. When increasing the number of iterations, the execution time also increases. In reality, this is inconvenient when one has to find a parking spot quickly, since the RRT*
Reeds Shepp stops searching first after the iterations are executed. It is also unnecessary to have a large number of iterations if the parking search space is small.
On the contrary, if the number of iterations is too small, there is a risk that the the tree does not encounter the goal state, resulting in no parking at all. An advantage with less iter- ations would be the decreasing execution time, accomplishing efficient parking if the algorithm find a spot.
70 iterations was chosen since the parking scenarios are within a limited space, as seen in the simulation figures above.
The execution time of the algorithms was not a main focus because that was not the main objective with this project.
However, it was kept in mind that the execution time was to be under 1 min.
A problem with RRT* Reeds Shepp was that it did not return the direction of the path. The solution to this was to add the direction information to the states in the state space search, which did not affect the algorithm in any significant way.
C. Stanley Controller
In simulation, the extensions of the paths proved to be sufficient in correcting how far the Stanley controller made the vehicle follow the path, since the tracker ends before the vehicle reaches the final point. However, the implementation deviated a lot from the simulation when driving in reverse direction, as seen in section VIII-B. The main cause of this would be that the Stanley controller used the front axle wheels as guiding wheels even if the vehicle switched direction. In figure 16, the car has parked too far from the goal position.
The car follows the green mark when driving and when the green cross stops, it marks the final position. It was discovered that in reverse direction, the car stopped when the front wheels reached the green cross. This is proof that the Stanley controller used the front wheels as guiding wheels.
The extension of the paths not only corrected the car’s position when parking, but also ensured the car reversed correctly. When reversing, the rear wheels was not used as guiding wheels which made the path tracking a bit unstable.
This can be seen in the figures after the Stanley controller have followed the path in simulation; the cars are not perfectly directed towards the given goal angle, π2 and 3π4.
Another cause is that the model cars did not function properly e.g the reversing velocity was greater than the froward velocity, despite being specified the same size.
D. Future Work
When driving in reverse, the front wheels were used as guiding wheels which proved not to work in this project. An extension of this project could be to use the back wheels as guiding wheels.
The approach in this project requires a system to inform the vehicle of the situation, for example a parking garage with cameras and sensors that can tell if a parking spot is available or not. Future work could include equipping the vehicle with sensors and software that could identify vacant parking spots without being dependent on an outward system, for example on an outside parking lot or on a private driveway. Another improvement would be to include several cars; often in busy parking lots there is competition of the vacant spaces and it is not always clear which vehicle should get the space. A system could be developed for optimizing where the cars should park and how they should compete for the spaces, including scenarios with only autonomous cars as well as mixed with human drivers.
X. CONCLUSION
In this work, the path planning algorithms Hybrid A*
and RRT* Reeds-Shepp were compared as well as simulated
and implemented in model cars. When comparing Hybrid A* and RRT* Reeds-Shepp, Hybrid A* proved to be more reliable while sometimes finding longer, not optimal paths, while RRT* Reeds-Shepp not always found a path but found more optimal paths. In simulation, the Stanley controller made the vehicle follow the paths closely both in forward and reverse driving with the front wheels as guiding wheels, while only being able to follow the paths when driving forward in implementation with the same settings.
ACKNOWLEDGMENT
The authors would like to thank their supervisor Yuchao Li for guidance, support and for molding the project to their interest. They also thank Frank Jiang for his invaluable help and support with the code and implementation for the model cars. Lastly, they thank Thomas Lundqvist and Nils Paulsrud for their help and patience in sharing troubles with the model cars.
REFERENCES
[1] S. M. LaValle, Planning Algorithms. Cambridge, U.K.: Cambridge University Press, May 2006, available at http://planning.cs.uiuc.edu/.
[2] J. A. Reeds and L. A. Shepp, “Optimal paths for a car that goes both forwards and backwards,” Pacific Journal of Mathematics, vol. 145, no. 2, pp. 367–393, 1990.
[3] L. E. Dubins, “On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents,” American Journal of Mathematics, vol. 79, pp. 497–516, 1957.
[4] J. Petereit, T. Emter, C. W. Frey, T. Kopfstedt, and A. Beutel, “Appli- cation of hybrid a* to an autonomous mobile robot for path planning in unstructured outdoor environments,” in ROBOTIK 2012; 7th German Conference on Robotics, Munich, Germany, May 2012, pp. 1–6.
[5] C. Bohan, “Auto-car-cooperative-path-planning-03-hybrid-a- star-trajectory-planning,” April 2019. [Online]. Available:
https://github.com/ChenBohan/Robotics-Cooperative-Path-Planning- 03-Hybrid-A-Star-Trajectory-Planning
[6] S. M. Lavalle, “Rapidly-exploring random trees: A new tool for path planning,” May 1999.
[7] P. Cheng, Z. Shen, and S. M. Lavalle, “Rrt-based trajectory design for autonomous automobiles and spacecraft,” Archives of Control Sciences, vol. 11, January 2001.
[8] T. Chin. (2019, February) Robotic path planning: Rrt and rrt*. A Medium Corporation, New York, USA. [Online].
Available: https://medium.com/@theclassytim/robotic-path-planning- rrt-and-rrt-212319121378
[9] A. Sakai, “Rrt star reeds shepp,” April 2019. [Online].
Available: https://github.com/AtsushiSakai/PythonRobotics/blob/master/
PathPlanning/RRTStarReedsShepp/rrt star reeds shepp.py
[10] G. Hoffmann, C. J. Tomlin, M. Montemerlo, and S. Thrun, “Autonomous automobile trajectory tracking for off-road driving: Controller design, experimental validation and racing,” in Proceedings of the American Control Conference, Californa, USA, August 2007, pp. 2296 – 2301.
[11] A. Sakai, “Stanley controller,” April 2019. [Online].
Available: https://github.com/AtsushiSakai/PythonRobotics/blob/master/
PathTracking/stanley controller/stanley controller.py
[12] (2019, April) Ros/introduction. [Online]. Available: http://wiki.ros.org/
ROS/Introduction
[13] —. (2019, April) Qualisys track manager. Qualisys, Gothenburg, Sweden. [Online]. Available: https://www.qualisys.com/software/
qualisys-track-manager/
