Heuristic for vehicle motion planning with search trees

(1)

IT 20 092

Examensarbete 30 hp December 2020

Heuristic for vehicle motion planning with search trees

Peili Guo

Institutionen för informationsteknologi

(2)

(3)

Teknisk- naturvetenskaplig fakultet UTH-enheten

Besöksadress:

Ångströmlaboratoriet Lägerhyddsvägen 1 Hus 4, Plan 0 Postadress:

Box 536 751 21 Uppsala Telefon:

018 – 471 30 03 Telefax:

018 – 471 30 00 Hemsida:

http://www.teknat.uu.se/student

Abstract

Heuristic for vehicle motion planning with search trees

Peili Guo

Motion planning is an essential part of an autonomous system. It finds a sequence of actions for the vehicle to move from start to goal. In this thesis, we study the use of heuristic in the context of motion planning. We propose a heuristic method using a pre-computed heuristic look up table for sampling based motion planning method Rapidly-exploring random trees (RRT). The proposed method is successfully implemented and evaluated in nine different scenarios. We have found improvement in the quality of the path at a short time out in at least six out of the nine scenarios. In certain scenarios, we are able to observe up to 20% reduction of execution time and up to 90% of improvement of the path quality.

Examinator: Stefan Engblom Ämnesgranskare: Justin Pearson Handledare: Robin Andersson

(4)

(5)

1 Introduction

1.1 Motivation

Eighty years ago, Futurama, an exhibition by General Motors at the 1939 New York World’s fair, showed the public an Utopian vision of the future world where infrastructures supported smart highways and self-driving cars. Not only would the vehicles run on electricity, but also the cars would drive themselves. Though the dream of fully autonomous vehicles in our everyday life has yet to come true. Autonomous technology has advanced considerably thanks to sensor technology, computing power, and artificial intelligence. Laser-based lidar sensing system and high-resolution cameras help the autonomous vehicle to see the environment. The computers use each data point captured to reconstruct a three-dimensional model to perceive the environment and then control the vehicle to move [1]. Industry 4.0 technologies [2] such as network communication, cloud-computing, internet of things, and big data, are expanding rapidly and we are almost certain that fully self-driving vehicles are not too far away in the future for us.

In our daily life, we still need drivers to operate di↵erent types of vehicles. Statistics shows that 90% of traffic accidents in United States are caused by human error [1] and 95% of road traffic accidents in Europe involve human error [3]. Advance vehicle technology could potentially perceive the surrounding better and react more quickly to provide a safer driving experience that can reduce injuries and deaths.

While urban self-driving vehicles might not be realistic in the short term, autonomous vehicles for industrial application in a more controlled environment is already available. Mckin- sey [4] described the three growth phases of autonomous vehicles, namely development, early adoption, and going mainstream. Currently we are still in the development phase where industrial solution is available and new mobility and business models begin to emerge. It is expected in the next few years, new business models, supply chain and logistics would emerge and redefine the transportation industry to adapt to autonomous vehicles. The continuous development focus on electrification, connection, and automation in autonomous vehicles will potentially make road transportation safer with fewer accidents, more efficient with optimised fuel consumption and minimised downtime for service and maintenance, and create a more sustainable transport system for our society.

1.2 Autonomous vehicles

There has been intensive research and development on autonomous vehicles from both aca- demia and industry in the last twenty years [5–9]. Driverless vehicle have multiple benefits, for example, safer transportation, increasing economical growth, and more mobility options for the citizens [7]. Clements and Kockelman [10] estimated that connected and fully autonomous vehicles has an economic impact of $5.6 trillion worldwide combing the reduced collision and increased drive productivity. While we are anticipating the deployment of fully autonomous vehicles on public roads, the demand for autonomous heavy-duty vehicles for industrial usage could be higher. Scania CV AB (Scania), one of the biggest truck manufacturer in the world, reported in 2012 that drivers and fuel accounted for 70% of the total cost for European haulier and the critical part of profitability depends on the driving style of the driver that a↵ects fuel consumption [11].

Trucks are often used in industrial environment, where it is more controlled with less pedestrians and other vehicles than open public roads. One example is mining sites, Scania has deployed self-driving trucks in customer’s industrial operations with safety driver in a pilot study. A concept vehicle Scania AXL [12] for industrial application which does not have a driver’s cab is in figure 1 and it is steered and monitored by intelligent control systems.

(8)

Figure 1: Scania AXL autonomous cabless truck. (Courtesy of Scania CV AB)

Source: https://www.scania.com/group/en/a-new-cabless-concept-revealing-scania-axl/

Creative Common 3.0 license

1.3 Problem formulation and project scope

In industrial use cases, autonomous vehicles should be able to run uninterruptedly during a long period of time. Motion planners that calculate a path for the vehicle to follow need to be very precise to navigate through di↵erent obstacles with consideration of the vehicle’s dimensions and limitations. This thesis will explore using heuristic in search tree within the context of motion planning for heavy-duty vehicles, such as trucks and buses. The objective of this thesis is to study and define a suitable heuristic that can be used in search tree for motion planning and to evaluate how the heuristic could potentially increase the efficiency and quality of the output path for the autonomous vehicle. An ultimate goal for autonomous vehicle is that it can be safer and more efficient by reacting faster in the uncertain environment and avoiding unnecessary excess steering that might results in wearing the vehicle components and shortening the vehicle’s lifetime.

In order to achieve the desired outcome of the project, this thesis has been divided into three major phases. The first phase is literature study and system familiarisation. After that the second phase consists of computer simulation and detailed implementation within the system.

Lastly, the third phase focuses on testing and evaluation.

1.4 Thesis outline

This thesis is divided into 7 sections. In Section 1, we introduce the thesis project with its motivation, objectives, and scope. Background information and theory related to autonomous vehicles is introduced in Section 2 together with the concepts of Rapidly-exploring random trees (RRT) and heuristic. Section 3 discusses previous research on heuristic in RRT. The methods and tools used in this project are described in Section 4. Section 5 presents the results of the heuristic implementation in simulation. Following that in Section 6 we discuss the results and in Section 7 we draw a conclusion and give suggestions to possible future work.

(9)

1.5 Summary of method and results

We propose a heuristic method to estimate the cost from a randomly sampled node to the existing nodes in the RRT. The method is successfully implemented. In the evaluation, we try di↵erent strategies for connecting a sampled node to the parent nodes. The results are positive as we have found improvement in at least six out of nine scenarios. We observe a reduction in the execution time of up to 20% using the heuristic method and up to 90% of improvement of the quality of the path found at time out.

(10)

Figure 2: Overview of autonomous vehicle system architecture.

Inspired by Lima [6]

2 Background and theory

Due to globalisation and the increasing need for transportation, the demand for specialised vehicles has been increasing. Buses for urban passenger transportation and trucks for o↵- road construction material transportation require di↵erent configurations of engines and cabins.

Scania, a successful and leading company in the automotive industry developed a modular system in the 1980s. The modular system made it possible to have minimum numbers of parts and allow many di↵erent variations of the same part. High degree of customisation can be achieved easier with this modular system while maintaining a low product development cycle and production cost.

One of Scania’s core values is customer first where Scania provide customised solutions and products to help customers be profitable and sustainable in the transportation industry from a life cycle’s perspective. Scania has applied modular system to the vehicle’s hardware and software, which has enabled Scania to produce individual product configurations that fulfill the customer’s need. There is standardised interface between di↵erent modules that makes service, upgrade, and replace easy [13], which can help reduce vehicle down time and increase profitability. The modular system is one of Scania’s key success factors.

An autonomous vehicle is a complex product that consists of multiple modules and each module has its own functionality. A simplified system architecture consists of route planning, perception, situation awareness, motion planning, and motion control systems [7]. An overview of an autonomous vehicle system architecture can be found in Figure 2.

(11)

2.1 Motion planning

Motion planning has been studied for many years within the field of robotics. There are many successful applications in di↵erent industries, for example robot navigation, automated assembly, autonomous vehicle, robotic surgery, and computer game characters [14]. One classic example of motion planning problems is the Piano Mover’s problem [15], which is a hard computational problem. For the Piano Mover’s problem, the motion planner needs to determine how to move the piano from one room to another room in a house without hitting anything given a precise CAD model of the house and the piano [14].

Motion planning is a fundamental and essential module in an autonomous system that it computes a path between a start configuration and a goal configuration. A typical motion planning module receives instructions in high-level, such as English or mathematical symbols.

Examples of such instructions are drive to destination, change lane, or make a U turn. A sequence of actions are then produced to achieve these instructions. These actions are then sent to a local control module that controls the vehicle to follow the planned motion as precise as possible. The ideal output from motion planning shall be safe, smooth, comfortable for passenger or cargo inside the vehicle, and efficient for both fuel usage and travel time.

In this thesis, motion planning is limited to autonomous vehicles, especially heavy-duty vehicles, e.g. buses and trucks. The motion planner uses information about static and dynamic obstacles around the vehicle and generates a path that is collision-free. The goal of motion planning is to calculate a path for the vehicle to follow that meets the dynamic and kinematic constraints of the vehicle, as well as the surrounding environment constraints.

There are many di↵erent motion planning algorithms that solve motion planning problems.

Most of the algorithms search for an optimal path between start and goal. The optimality of the path is defined by minimising a path cost function, for example, distance or time. Before we go into details about common motion planning methods, it is important to introduce some concepts and terminologies within motion planning.

Actions: A motion planner generates a series of actions to manipulate the state of the vehicle.

Collision Checking: One of the important requirements for the output of motion planner is that the path needs to be collision free. Therefore, it is necessary to perform collision checking to see if the vehicle and the obstacles are in the same location. There are multiple ways to perform collision checking, for example, occupancy grid based collision checking and distance map [7].

Configuration and Obstacle Space: A series of actions shall be collision free. Obstacles includes both static obstacles and dynamic obstacles. Examples of static obstacles are parked cars and road blocks. Examples of dynamic obstacles are pedestrians and cyclists. A configuration space X includes all the possible states for the vehicle. We can create an obstacle space X_obs from the set of obstacles and this space is larger than the obstacle itself since the vehicle needs to keep a minimum distance to obstacles. The free space X_free is the space that is collision free [7].

Nonholonomic constraints: Autonomous vehicles are subject to rolling constraints.

Rolling constraint is the constraint that wheels must spin around their horizontal axis with an appropriate amount based on their motions along the wheel plane so that rolling occurs at the ground contact point. Cars with wheels cannot move in all directions. For example, a car cannot move sideways directly. A car needs to move forward and backward with turns in order to perform parallel parking. The constraints that limit the possible directions of movements but the motions can be achieved by a series combination of manoeuvring are called nonholonomic

(12)

constrains [16].

Optimal motion planning: The quality of the path generated by a motion planner is important in many applications. We are interested in the minimum distance path in most cases.

Triggs [16] describes that optimal motion planning problems as follows:

Find a control u(t) for ˙x = f (t, x, u) which minimise the cost Rx_f

xs c(t, x, u)dt to travel from x_s to x_f, subject to constraint u(t)2 U for some set of permitted control values U.

Pose: a pose consists of position and heading. It is useful for describing vehicle states.

pose : q = (x, y, ✓)

x, y are the Cartesian coordinates of the vehicle and ✓ is the heading of the vehicle, expressed by the angle between the vehicle and the positive X axis.

Start and Goal State: The start state x_i 2 Xfree is the initial vehicle state at time t₀, where x_s = (q_s, t₀). The goal state x_g 2 Xfree is the final vehicle state at time t_f, where x_g = (q_g, t_f). At the start state and goal state, the vehicle shall not be in collision with any obstacles.

Steering method: Steering method is a subset of motion planning problems. Obstacles are ignored in the steering method. The steering method considers boundary constraints and nonholonomic constraints. Dubins paths [17] are examples that fulfill the nonholonomic constraints while only travelling forward. The paths consist of segments of straight lines and arc circles. Reeds-Shepp paths [18] are similar to Dubins path that fulfill the noholonomic constraints while the vehicle can travel both forward and backward.

In summary, motion planning in an autonomous system is mainly responsible for [7]:

1. Move to a goal configuration, or make progress towards it.

2. Respect the vehicle’s kinematic and dynamic constraints.

3. Avoid obstacles in the environment.

4. Optimise the desired metrics.

2.1.1 Control system - motion control

The motion control module is responsible for stabilising and controlling the vehicle to follow the path generated from the motion planning module. It does not need to know high-level instructions such as where obstacles are or where the start or goal position is. A Motion controller receives a collision-free feasible path from the motion planner as well as vehicle states from the perception module then use these information to actuate on the steering angle, throttle, and brakes of the vehicle [6].

Generally speaking, the control system in an autonomous vehicle focuses on longitudinal and lateral controller based on the mathematical model of the vehicle. Longitudinal controller is in charge of the speed of vehicle. In other words, controlling the longitudinal dynamic of the vehicle, such as acceleration and braking. Lateral controller, also known as steering controller, is the other important controller in the motion control system. The main task for motion control is to follow the planned path generated by motion planning. This is achieved by maintaining steering angle to keep the desired position on the planned path. The sensors detects the location of the vehicle and the planned path, and at the same time, the controller computes the error between the planned path and the actual location and controls the steering wheel to follow the path [19]. This system is the last and the most direct one to make the vehicle self-driving

(13)

which means that with autonomous motion control, the drivers are able to take the hands o↵

the steering wheel and let the vehicle steer itself.

2.1.2 Nonholonomic systems

In Section 2.1 we discussed that there are many potential applications with motion planning.

However, motion planning for autonomous vehicles is not a simple task because the vehicles are subject to dynamic and kinematic constraints that limit the possibility to move in all directions. For example, vehicles cannot move sideways directly, otherwise we would not need to learn parallel parking. In order for the vehicle to make lateral displacement, it needs to move forward and/or backward while steering. Thus moving from one pose to another pose sometimes require complicated manoeuvres even without the presence of obstacles. The constraints that limit the possible directions of motion at a point but the movement can be achieved by a series combination of manoeuvring are called nonholonomic constraints [16].

A basic model for vehicle in motion planning consists of two wheels connected by a rigid link and is restricted to move in a plane. It is assumed that the wheels can rotate freely about their axes of rotation without slipping at the point of contact with the ground. The movement of such a nonholonomic system at low speed can be described by the time-domain kinematic equations in Equation (1) where v_r is the forward speed of the vehicle, x, y are the coordinates of the vehicle in the global coordinate system, ✓ is the heading of the vehicle, l is the wheelbase [6, 9].

One variation of this vehicle model is when v_r is a fixed constant. One of these cases where the vehicle moves only forward at a fixed speed is called Dubins car, Another case where the car can move both forward and backward is called Reeds-Shepp car. They will be discuss in details in Section 2.1.3.

8>

<

>:

˙x = ^dx_dt = v_rcos(✓)

˙y = ^dy_dt = vrsin(✓)

˙✓ = ^d✓_dt = ^v_l^rtan(✓)

(1)

2.1.3 Shortest path for nonholonomic systems

We mentioned previously in Section 2.1.2 about nonholonomic systems. Trucks and buses are nonholonomic systems and they might require a significant amount of manoeuvring forward and backward to travel to a nearby goal pose. It turns out that nonholonomic motion planning is much more difficult than holonomic motion planning [20]. Take the example of parallel parking, we can generate a path to perform parallel parking by moving directly sideways but this path may be infeasible because of the vehicle’s dynamic constraints. In such case, the Euclidean distance does not estimate the actual driving distance well. The exact distance for nonholonomic system might be complicated to compute. But there are minimum length paths that take into consideration of the nonholonomic constraints such as Dubins paths [21] and Reeds-Shepp paths [18] that use only circular arcs with minimum turning radius and straight lines to construct a feasible path between the start and goal pose.

Both Dubins paths and Reeds-Shepp paths have wide applications within motion planning especially for autonomous system where it is nonholonomic. For a vehicle with finite yaw rate, it means that the vehicle has a minimum turning radius. Dubin’s method assumes that the vehicle only travels forward, and the optimal path is within a set of trajectories that fulfill the curvature constrains. An illustration for Dubins paths is shown in Figure 3. If we denote straight line with S and circular arc with C, the shortest path from Dubins method could be either CSC or CCC. If we denote anti-clockwise circular arc L and clockwise circular arc R,

(14)

Figure 3: Examples of Dubins path: the vehicle can only move forward

Figure 4: Examples of Reeds-shepp path:

the vehicle can move forward and backward

the shortest Dubins path can only be one of the following LSL, RSL, RSR, LSR, LRL and RLR [22].

Reeds-Shepp paths [18] are similar to Dubins paths, but reverse is allowed. The vehicle can travel both forward and backward. An illustration for Reeds-Shepp paths is shown in Figure 4.

The shortest path has the form of C|CSC|C, where | indicates an option for reverse. The optimal Reeds-Shepp path lies in a set of 48 di↵erent trajectories.

It is worth pointing out that even though Dubins paths and Reed-Shepps paths are good estimations for paths under nonholonomic constraints, these paths do not have continuous curvature which means that when turning into an arc path or out of an arc path, the vehicle is assumed to be able to change the steering wheel instantly from maximum turn to straight or vice versa. In reality, changing the steering angle in infinitely small time interval is physically impossible.

2.2 Common motion planning methods

In this thesis, the motion planning problem consists of finding a path from the start configuration to the goal configuration that satisfies the vehicle and environment constraints. There can be many feasible paths to a problem. If we consider the quality of the path, we can find an optimal path where certain quality is optimised, for example, time, or distance.

NP (non-deterministic polynomial time) problems can be solved in polynomial time by a non-deterministic Turing machine. NP-complete problems can be verified in polynomial time but is unknown if can be solved in polynomial time. The famous travelling salesman problem is NP-complete problem [23]. Motion planning problems can be categorised into PSPACE problem that the problem can be solved in polynomial space. Motion planning problems for autonomous vehicles are known to be PSPACE-hard, which is believed to be at least as hard as NP-hard problem [9].

One common model for motion planning is to decouple the problem into solving a basic path planning problem and generating low level instructions for the controller that satisfy the dynamic constrains of the vehicle to follow the planned path [24]. Randomised techniques have been widely discussed and applied to get an efficient basic path. One advantage of randomisation is that it can help escape from the local minimum or maximum.

(15)

Common methods to solve the motion planning problem includes graph-based search and stochastic incremental search [25]. Graph-based search can be related and applied to many en- gineering and scientific problems, including motion planning problem. There are mathematical approach and heuristic approach in graph-based search. The mathematical approach typically deals with abstract graph and focus more on the final solution other than the computational feasibility of the algorithms. The heuristic approach uses special knowledge within the domain of the problem and aims to improve the computational efficiency of solutions [26]. Below are some common motion planning methods:

2.2.1 Sampling-based motion planning

Sampling-based methods search for collision-free paths by sampling valid nodes in the free configuration space [27, 28]. The free configuration space is where the vehicle does not result in collusion with any obstacles. The nodes in the tree contain only those can be reached from the start configuration. This path is then verified using steering method and collision checking method to ensure that the path is feasible.

One challenge using sampling-based algorithm lies in constructing a discretisation that rep- resent the connectivity in the free configuration space. One typical approach is to discretise the configuration space to pre-computed motion primitives (fixed maneuvers) and apply graph search algorithm to find a feasible path.

Lattice-based motion planners are common in autonomous systems. It is a graph based planning method combined with state lattice. A set of feasible fixed maneuvers is chosen and a search graph is generated by recursively applying these maneuvers starting from the vehicle’s initial configuration. The performance of lattice-based motion planner highly depends on the state-space discretisation. One drawback of lattice-based motion planner is that the planned motion cannot arrive at any possible locations within the environment. It can only plan motion to locations on the lattice.

Two other common methods within the category of sampling-based algorithms are Probab- ilistic RoadMaps (PRM) and Rapidly-exploring Random Trees (RRT). PRM algorithms first construct a roadmap that represents a set of collision-free trajectories by attemping to connect n randomly-sampled nodes in the collision-free space X_free during the pre-processing phase. It then computes a shortest path that connects the start to the goal through the roadmap. We will introduce RRT in details in Section 2.3.

2.2.2 Interpolation-based motion planning

Interpolation technique was first introduced by Waring [29] in 1779. An interpolation problem is described as given m pairs of (x_i, y_i), find a function f = f (x) such that f (x_i) = y_i for i = 1, 2, 3, ..., m. In the process of interpolation, we insert a set of new data within the range of a known set. Common interpolation methods includes piecewise polynomial interpolation and trigonometric interpolation.

Interpolation-based motion planning methods use a discrete set of possible transitions between nodes to generate a feasible path. Example of such path includes Dubins paths and Reeds-Shepp paths, which are discussed in Section 2.1.3. They are good estimations with relaxed constraints. But these paths does not have continuous curvature that means when turning into an arc path or out of an arc path, the vehicle is assumed to be able to change the steering wheel instantly from maximum turn to straight or vice versa.

Interpolation based motion planning methods are among the most applied in real application by research groups in the world according to Gonz´ales et al. [8]. The curves generated by interpolation are widely used for on-road driving motion planning for tasks such as lane

(16)

change and overtaking. Ferguson and Stentz [30] introduced an interpolation-based planning and replanning algorithm field D* using linear interpolation. Apart from using straight lines and circles to interpolate the nodes, clothoid curves, polynomial curves, and B´ezier curves are other common implementations for motion planning.

2.2.3 Optimisation-based motion planning

Optimisation-based methods aim to either minimise or maximise a function subject to di↵erent constraints. This method has gained attention in research recently because of the increasing computational power and the availability of di↵erent numerical optimisation methods.

The optimisation-based method formulates the motion planning problem to a convex optimisation problem. It is often used as a post process after a collision-free path is computed [31].

An object cost function about constraints such as kinematic and collision can be formulated and then we can apply numerical optimisation algorithms to solve it. Ratli↵ et al. [32] presented a gradient optimisation technique CHOMP that does not require an input collision free path and it can transform an initial guess into a trajectory suitable for execution.

This method has been successfully evaluated in urban driving and it takes into consideration of other road users. It is suitable for comfort and safety planning in structured environment. One drawback of this method is the requirements for high computing power since the optimisation of the object function takes place at each motion state and it is especially challenging in real time execution with dynamic obstacles [8].

2.3 Rapidly-exploring random trees (RRT)

It has been proven that sampling based motion planning techniques can be successfully applied to a number of problems such as robotics, graphics animation, manufacturing, and computational biology [33]. Steven LaValle [24, 34] introduced the concept of RRT in 1998 as a randomised data structure for path planning that is designed for systems with non-holonomic constraints. RRT is a sampling based method that grows a tree structure consists of di↵erent dynamically feasible trajectories by randomly sampled nodes. It does not require discretisation of the state space. It can be considered a Monte Carlo method that uses randomisation to implicitly compute the Voronoi regions of each node in a tree.

Algorithm 1 Generate RRT with K vertices

1: procedure GenerateRRT(x_init, K, t)

2: T.init(x_init)

3: for k = 1 to K do

4: x_rand RandomState();

5: x_near NearestNeighbour(xrand, T);

6: u SelectInput(xrand, xnear);

7: x_new NewState(xnear, u, t);

8: T.addVertex(x_new);

9: T.addEdge(xnear, xnew, u);

10: end for

11: Return T

12: end procedure

The basic RRT with a given initial state x_init, an RRT T, K vertices, and fixed time interval t is constructed in Algorithm 1 [34]. A random state, x_rand is selected from the configuration space X at line 4. NearestNeighbour finds the closest vertex to x_randat line 5. u is an input that

(17)

minimises the distance from x_near to x_rand. In line 7, a new state x_new is generated by applying u and a new vertex is added to T . An edge from x_near to x_new by the input u is recorded.

Figure 5 shows di↵erent RRTs with di↵erent number of nodes that explore the space without obstacles and the tree is rooted at (0, 0).

In autonous vehicles, using RRT in motion planning has the following work flow: (1) sample a random node, (2) select the parent node to connect, (3) try to expand the tree by connect the parent node and the sampled node, (4) collision and constraint checking.

(a) RRT 200 nodes (b) RRT 500 nodes (c) RRT 1000 nodes

(d) RRT 5000 nodes (e) RRT 10000 nodes (f) RRT 100000 nodes Figure 5: Use RRT method to grow a tree with di↵erent number of nodes from the origin (0, 0)

LaValle [35] summarised the benefits of RRT:

1. The expansion of an RRT is heavily biased toward unexplored portions of the state space.

2. The distribution of vertices in an RRT approaches the sampling distribution, leading to consistent behavior.

3. An RRT is probabilistically complete under very general conditions.

4. The RRT algorithm is relatively simple, which facilitates performance analysis.

5. An RRT always remains connected, even though the number of edges is minimal.

6. An RRT can be considered as a path planning module, which can be adapted to incor- porate into a wide variety of planning systems.

7. Entire path planning algorithms can be constructed without requiring the ability to steer the system between two prescribed states.

(18)

RRT and similar methods for path planning are very good at navigating through difficult, unstructured environments clustered with obstacles. RRT is popular in motion planning because it finds solutions efficiently to single-query problems [4]. However, the basic algorithm does not consider the total path cost [36] and it has poor convergency to the optimal path. The initial solution RRT found could be one of the many feasible paths from the starting configuration to the goal configuration. This initial solution path can be very oscillatory, that the steering direction changes often and the paths are often much longer than optimal. An example is that in a large, open, obstacle free area, a basic RRT might start o↵ in the opposite direction to the goal - especially if the goal is far away from the start. This is caused by the weakness of RRT that it does not consider the cost of the complete path from start to goal. This often leads to paths that are longer than necessary and su↵er from excessive steering. One way of improving this issue in motion planning is using heuristic. With heuristic, we approach the problem with special knowledge about motion planning, say in this project using motion planning domain knowledge to find a better estimation of the cost to reach the sampled node. By incorporating heuristic, the algorithm will expand nodes and explore regions that is more likely to reach the goal instead of randomly exploring the space. Using heuristic, it can potentially lead to solutions that are less oscillatory, shorter, and that finds a steady path to the goal in fewer iterations. This might however come at the cost of increased computational time and a reduction of the exploratory properties of the planner. Urmson and Simmons [36] suggested k-Nearest algorithms, iterative k-nearest neighbour algorithm, best of k-nearest neighbours algorithm and there is experiment showing heuristic RRT can significantly improve the quality of path found with little to no computational cost.

2.4 Heuristic

The word heuristic comes from ancient Greek heuriskein and latin heuristicus. it means to find out, discover. Romanycia and Pelletier [37] summarised that in artificial intelligence, heuristic is any device used in problem solving which one is not entirely confident will be useful in providing a practical solution, but which one has reason to believe will be useful, and which is added to a problem-solving system in expectation that on average the performance will improve. An example of a device could be a program, a data stricture, a strategy, or domain specific knowledge.

In computer science and artificially intelligence, we often know heuristic when it is associated with heuristic search. One of the famous best-first search algorithms A* uses heuristic. A*

heuristic search method decides which node in the graph to explore by computing an evaluation function f (n) = g(n) + h(n). Here g(n) is the cost function correspond to the path cost from the start to the current node n, and h(n) is a heuristic function that estimate the cost from node n to the goal.

A heuristic is admissible if it always underestimate the cost to reach the goal. A heuristic is consistent if for every node n and every successor n⁰ of n, the estimated cost of reaching the goal from n is no greater than the step cost of getting to n⁰ plus the estimated cost of reaching the goal from n⁰: h(n)  c(n, n⁰) + h(n⁰). The tree-search version of A* is optimal if h(n) is admissible, and the graph-search version of A* is optimal if h(n) is consistent. [38]

(19)

3 Previous work

In this section, we introduce previous relevant research using heuristic for sampling based motion planning. There are mainly two categories for heuristic in sampling based motion planning.

One focus on bias sampling and the other focus on pre-computation. The common goal of the heuristic is to improve the performance of path searching in motion planning by guiding the search in a promising direction.

3.1 Biasing RRT grow

The basic RRT algorithm described in Section 2.3 connects the sampled node to the closest node in the tree. The cost function to evaluate the parent nodes consider only the distance between the sampled node and the existing parent nodes in the tree. The cost from the root to reach the existing parent node is not considered, which is one of the weakness of the basic RRT [39] and the path found using the basic RRT could be far from optimal.

Urmson and Simmons [39] use heuristic to guide the growth of RRT. The algorithm has bias in extending lower cost paths heading towards the goal and at the same time maintain a reasonable bias towards exploration. The heuristic function shapes the probability distribution to make the likelihood of selecting a node depends on the size of its Voronoi region, which is a bias toward exploration, and the quality of the path to a node, which is exploiting known good parts of the space leading to the goal. A coefficient m_quality is computed to accomplish the guiding:

m_quality = 1 C_vertex C_opt Cmax Copt

C_vertex is the total cost associated with a node, C_opt is the estimated total cost of the optimal path from start to goal, C_max is the value of the highest total cost for any node considered so far.

Below are two other variation of the heuristic RRT proposed by Urms and Simmons.

• IkRRT: consider k-nearest neighbour parents rather than one single neighbour parent node and once a node is sufficient good to pass the probability test, it is selected to connect to the sampled node.

• BkRRT: similar to the above IkRRT method, but it connect to the best of the k-nearest neighbour parent.

The result of this heuristic shows improvement in the quality of the paths [39, 40]. Using goal biasing reduces the computational time, and improves the solution of path cost. The k- nearest neighbour method improves further on the path cost solution but with a more expensive computational cost.

3.2 Pre-computed heuristic

For heuristic search algorithms, a more accurate heuristic to estimate the true path cost can result in a better performance of the search. We have showed the inefficiency of the Euclidean distance in Section 2.1.2 together with Figure 3 and 4. In an environment without obstacles, the exact path cost can be pre-computed and stored in a heuristic look-up table. At run time, we can directly query a heuristic look-up table.

Choi [41] proposed a hybrid heuristic estimate function for nonholonomic motion planning.

This heuristic is designed for lattice-based motion planners. The heuristic first partition the space into visible and invisible sections. The visible space is from the goal pose’s perspective

(20)

and if the vehicle is in the visible space, we can directly get the heuristic value from a heuristic look-up table. If the vehicle is in the invisible space, the state space is projected on to a 2D Euclidean space and dynamic programming is applied to find the best value. The heuristic is created based on the observation that in the area closer to the goal, where the goal is visible, using nonholonomic pre-computed heuristic estimate the optimal costs better. While in other area where the goal is invisible, Euclidean distance estimates better than nonholonomic distance but underestimate true optimal costs.

Choi’s heuristic is similar to one of the most e↵ective approach in literature. A heuristic applied by Dolgov et al. [42] in the DARPA Urban Challenge called maximum heuristic. The heuristic is the maximum of either nonholonomic-without-obstacle distance or holonomic-with- obstacle distance. The nonholonomic-without-obstacle distance ignores the obstacles but it takes into account all the nonholonomic constraints of the vehicle. Dolgov et al. [42] implemented a pre-computed nonholonomic distance without obstacle with a grid of size 160x160 with 1m resolution in x, y and 5 in ✓. They then select the maximum of the nonholonomic-without- obstacle distance and the 2D euclidean distance with obstacles as the heuristic.

Cirillo [43] used a similar pre-computed heuristic in a lattice based motion planner. The proposed algorithm is called Lattice time-bounded A* and the algorithm rely on a heuristic function to direct the path search. All the possible path cost resulting from maneuvers from the state space discretisation in x, y and ✓ was pre-computed.

The result from Choi’s pre-computed hybrid heuristic has show improvement in simulation compare with the maximum heuristic by Dolgov et al.. It shows that using hybrid heuristic achieved about five times speed up compared with the maximum heuristics. The result from Cirillo’s algorithm has shown successful testing in vehicles and a reduced number of nodes expanded. The vehicle is able to avoid all obstacles without abrupt braking to calculate a new path.

(21)

4 Methods

4.1 Proposed method using heuristic look-up table

The main focus of heuristic in this thesis is to find a better distance estimation between a randomly sampled node nnew= (xnew, ynew, ✓new) and nodes in the RRT with vehicle’s kinematic constraints. Since the performance of the search is expected to be better when there is a more accurate heuristic to estimate the true path cost, we propose a method using a heuristic look-up table in Algorithm 2.

We describe an RRT as a set of nodes and edges RRT = hN, Ei, where each node ni 2 N encodes a pose p_i= (x_i, y_i, ✓_i) of the vehicle in terms of x, y position and vehicle’s heading and each edge e_i,j 2 E connects two nodes, ni, n_j 2 N and represents a kinematically feasible path that would allow the vehicle to move without collisions from p_i to p_j.

The RRT is extended by selecting a randomly sampled node n_new 2 N and connecting it/ to the best parent node already in N according to a cost function. If a suitable parent n_p 2 N is found, the RRT can be extended so that N = N[ nnew and E = E[ ep,new. Finding a good parent candidate can be a daunting task. In previous sections, we have shown that Euclidean distance is a poor estimation of the minimum distance between two poses with nonholonomic constraints.

Here, we propose a method that leverages a heuristic look-up table to get a better estimation of the distance between n_new and candidate parent nodes. The advantage of pre-computation is that the average time complexity for searching an element in a heuristic look-up table is O(1).

Leveraging this speed, we can improve the quality of heuristic without incurring in prohibitive overhead. We detail our approach in Algorithm 2, where we show how to use the heuristic look- up table to find a parent candidate for a new randomly sampled node nnew= (xnew, ynew, ✓new).

Our heuristic table H is constructed by connecting all possible n_i, n_j pairs o↵-line, such that

|xi x_j| < a and |yi y_j| < b, where a, b are the design parameters of H and ✓i, ✓_j are from a set of angles between [ ⇡, ⇡] with a certain resolution.

4.2 Heuristic look-up table simulation

In order to use the heuristic look-up table in a systematic way, we always translate the (x, y) of the randomly sampled node nnew to (0, 0) so that it is easy to reset the problem once we have sampled a new node. We then translate the coordinate of the node n_i in the tree to a local coordinate where the origin is (x_new, y_new). In Algorithm 2, we describe that when x < a and y < b, we will use a pre-computed heuristic value for the estimated distance between the sampled node n_new and n_i in the RRT. We use the simulation described in Algorithm 3 to visualise Dubins distance and Reeds-Shepps distance verses Euclidean distance and therefore decide the parameter a and b. We aim to find a suitable a and b where Euclidean distance does not significantly underestimate the nonholonomic distance.

At the same time, we run a simulation for the normalised di↵erence between the heuristic look-up table and Reeds-Shepps distance in Algorithm 4. This simulation checks how much improvement in distance estimation we can theoretically achieve by using a heuristic look-up table. The result can help us decide the resolution of angle discretisation for ✓.

There is a symmetry rule between poses where we can reduce the heuristic look-up table to a quarter of the full size. That means the heuristic look-up table will only store the goal pose located in Quadrant I. Given a start pose p_s= (x_s, x_s, ✓_s) and p_f = (x_f, y_f, ✓_f), we can use the symmetry function in Algorithm 5 to find the symmetric pose of pf in Quadrant I that has the same nonholonomic path length if p_f is located in Quadrant II, III, and IV. For example, the path length from start pose p_s = ( 5, 0, 0) to goal pose p_f = (8, 10, ⇡/5) is the same as

(22)

Algorithm 2 Select parent to connect to sampling node

1: procedure SelectParent(RRT, H, n_new)

2: candidates =;

3: for each n_i 2 N do

4: x =|xnew x_i|

5: y =|ynew yi|

6: h_i = 0

7: if x < a and y < b then

8: hi= h(xi, xnew)

9: else

10: h_i= EuclideanDist(x_i, x_new)

11: end if

12: candidates = candidates[ hhi, ii

13: end for

14: sort(candidates)

15: while candidates6= ; do

16: hhk, ki = pop(candidates)

17: parent = N_k

18: if 9ek,new then

19: return parent

20: else

21: continue

22: end if

23: end while

24: end procedure

Algorithm 3 Simulation to decide the range of the heuristic table

1: for each pair ofhpi, p_ji do

2: D_euclidean= EuclideanDist(p_i, p_j)

3: D_dubins= DubinsDist(pi, pj)

4: D_rs= ReedsSheppsDist(p_i, p_j)

5: end for

6: Scatter plot D_euclidean vs.D_dubins and D_euclidean vs. Drs

Algorithm 4 Simulation for normalised di↵erence between the estimated distance and Reeds- Shepp distance

1: for i = 1, 2, 3, . . . do

2: psi = (0, 0, ✓s1)

3: pg_i = (x_i, y_i, ✓g_i)

4: h_i= h(ps_i, pg_i)

5: Drs i= ReedsSheppsDist(psi, pgi)

6: (i) = ^hⁱ_D^D^{rs i}

rs i

7: end for

(23)

going to goal pose p_f0 = (8, 10, ⇡/5) and a plot of the above properties is shown in Figure 6.

Algorithm 5 Symmetry rule for ni in Heuristic look-up table

1: n_sym= n_i

2: n⁰_new= n_new

3: x = x_i x_new

4: y = y_i y_new

5: if y < 0 then

6: y_sym = y_sym

7: ✓⁰_new= ✓_new⁰

8: ✓sym = ✓sym 9: end if

10: if x < 0 then

11: xsym= xsym 12: ✓⁰_new= ⇡ ✓_new⁰

13: ✓_sym = ⇡ ✓_sym

14: end if

15: h_i = h(n_sym, n⁰_new)

4.3 Generate and query the pre-computed look-up table

After deciding the heuristic look-up table’s parameters a, b and t, we can generate all the path lengths between goal pose (x, y, ✓₁) and start pose (0, 0, ✓₂) using Reeds-Shepps distance.

The path lengths are computed in MATLAB and the values are stored in an one dimensional array directly to a “.h” file which can be complied to the executable and available at run time instantly. We use Algorithm 6 to generate the “.h” file.

Algorithm 6 Generate the pre-computed heuristic look-up table

1: Initialise 4 parameters x, y, ✓1, ✓2 with x, y, ✓1, ✓2 2: ii = 0

3: for each x_i 2 x do

4: for all y_j 2 y do

5: for all ✓_1k2 ✓1 do

6: for all ✓_2m2 ✓2 do

7: A_ii= ReedsSheppsDistance((x_i, y_j, ✓_1k), (0, 0, ✓_2m))

8: ii = ii + 1

9: end for

10: end for

11: end for

12: end for

Because of the limited number of pairs of start poses and goal poses in the heuristic look-up table, we use the closest pose available in the heuristic look-up table. Algorithm 7 shows how to access the corresponding value. When we try to get the proximal poses with available heuristic value, we take the floor value for both x and y and the round value for heading ✓_i and ✓_new.

(24)

Figure 6: Illustration for symmetry property.

(25)

Algorithm 7 Access the Heuristic value

1: procedure heuristicValue(ni, nnew, H)

2: n⁰_new = (0, 0, ✓_new)

3: n_sym= (x_i x_new, y_i y_new, ✓_i)

4: if ysym< 0 then

5: y_sym= y_sym

6: ✓⁰_new= ✓⁰_new

7: ✓sym= ✓sym

8: end if

9: if x_sym< 0 then

10: x_sym= x_sym

11: ✓⁰_new= ⇡ ✓⁰_new

12: ✓_sym= ⇡ ✓_sym

13: end if

14: Ix = f loor(^x^sym_dx )

15: Iy = f loor(^y^sym_dy )

16: I✓₁= round(^✓^sym_d✓^+⇡)

17: I✓2= round(^✓⁰^new_d✓^+⇡)

18: idx = Ix⇤ (a ⇤ b ⇤ t) + Iy ⇤ (b ⇤ t) + I✓1⇤ t + I✓2 19: return A_idx

20: end procedure

4.4 Test scenarios

We use the following scenarios to evaluate the performance of the method we propose in Al- gorithm 2.

1. Without any obstacles, drive from a start pose to a goal pose. Illustration is in Figure 7.

2. With a flat obstacle between the start pose and the goal pose. Illustration is in Figure 8.

3. The vehicle starts inside a H shaped area and three out of four sides are obstacles. There is only one side that’s obstacles free. The goal pose is in a similar environment. Illustration is in Figure 9.

4. The vehicle is in a narrow road and needs to turn into heading the opposite direction.

Illustration is in Figure 10.

5. The vehicle starts inside a box shaped area, with a very narrow opening that leads to the outside. The vehicle needs to first drive outside of the box and then steering towards the goal. Illustration is in Figure 11.

6. Similar to the Scenario 5, but the goal is inside a box shaped area. The vehicle needs to drive into the box shaped area via a narrow opening.

7. A combination of Scenario 5 and 6. Both the start and goal are inside a box shaped area with a narrow path to enter and exit. Illustration is in Figure 12.

8. The vehicle starts at (0, 0, 0) and drives to as far away as possible without any obstacles.

9. The vehicle starts at (0, 0, 0) and drives to as far away as possible with obstacles on two sides of the goal. Illustration is in Figure 13.

(26)

Figure 7: Test case with no obstacles

Figure 8: Test case with a flat obstacle in between

(27)

Figure 9: Test case with a H shaped obstacle

Figure 10: Test case with obstacles for turn around in narrow space

(28)

Figure 11: Test case with narrow opening to exit

Figure 12: Test case with narrow opening to exit and enter

(29)

Figure 13: Test case with obstacles around two sides of the goal at distance

4.5 Evaluation

We use the following four categories of method to evaluate the proposed heuristic method.

1. Pure RRT Euclidean connection

We use Euclidean distance to estimate the distance between the sampled node and the parent nodes in the tree. We only try to connect the sampled node to the closest node in the tree. If it is not connectable we re-sample a new random node.

2. Pure RRT heuristic connection

We use the heuristic method in Algorithm 2 to estimate the distance between the sampled node and the parent nodes in the tree. We only connect the sampled node to the closest node in the tree. If it is not connectable we re-sample a new random node.

3. Best total path cost Euclidean connection

We use Euclidean distance to estimate the distance between the sampled node and the parent nodes in the tree. We choose the parent to connect to based on the total path cost:

D_total = D_parent+ D_sample. Then we sort the list of parent nodes in ascending order of D_total and select the node with lowest overall cost. Before confirming the connection, we use the collision checking to check connectivity and steering method to get an exact cost.

If the exact cost is larger than the second best estimated cost in the list or in collision, we will repeat the above process to check the next best parent node until we find the best parent to connect to.

4. Best total path cost heuristic connection

It is similar to the previous one but we use the proposed heuristic look-up table method in Algorithm 2 to estimate the distance between the sampled node and the parent nodes in the tree. We choose the parent to connect to based on the total path cost: D_total = D_parent+ D_sample. Then we sort the list of parent nodes in ascending order of D_total and select the node with lowest overall cost. Before confirming the connection, we use the collision checking to check connectivity and steering method to get an exact cost. If the exact cost is larger than the second best estimated cost in the list or in collision, we will

(30)

repeat the above process to check the next best parent node until we find the best parent to connect to.

To test how the discretisation of x, y, ✓₁, ✓₂ in the heuristic look-up table would influence the performance of the methods, we use the following di↵erent tables in the evaluation:

1. HLUT-1: 8

>>

<

>>

:

x0= 0, xmax = 10, x = 1;

y₀ = 0, y_max= 10, y = 1;

✓10 = ⇡, ✓1max = ⇡, ✓1 = ^⇡₄;

✓₂₀ = ⇡, ✓₂_max = ⇡, ✓₂ = ^⇡₄;

2. HLUT-2: 8

>>

<

>>

:

x0= 0, xmax = 20, x = 1;

y₀ = 0, y_max= 20, y = 1;

✓₁₀ = ⇡, ✓₁_max = ⇡, ✓₁ = ^⇡₄;

✓20 = ⇡, ✓2max = ⇡, ✓2 = ^⇡₄;

3. HLUT-3: 8

>>

<

>>

:

x₀ = 0, x_max= 10, x = 1;

y0 = 0, ymax = 10, y = 1;

✓₁₀ = ⇡, ✓₁_max = ⇡, ✓₁= ₁₂^⇡;

✓₂₀ = ⇡, ✓₂_max = ⇡, ✓₂= ₁₂^⇡;

4. HLUT-4: 8

>>

<

>>

:

x₀ = 0, x_max= 30, x = 1;

y₀ = 0, y_max = 30, y = 1;

✓₁₀ = ⇡, ✓₁_max = ⇡, ✓₁= ₁₂^⇡;

✓₂₀ = ⇡, ✓₂_max = ⇡, ✓₂= ₁₂^⇡;

There are a total of 10 di↵erent methods that we use to evaluate each of the nine scenarios listed in Section 4.4 using either the Euclidean distance or heuristic look-up table. All tests are repeated 5 times and the average results are presented. The detailed 10 di↵erent methods are listed below.

1. RRT-BSC-EUC Basic RRT Euclidean

We only try to connect the sampled node to the closest parent node in the RRT according to the Euclidean distance.

2. RRT-BSC-HLUT-1

Basic RRT heuristic table 1

We only try to connect the sampled node to the closest parent node in the RRT according to HLUT-1 in Section 5.3 where the table is of size 10 by 10, ✓1 = ⇡/4 and ✓2 = ⇡/4.

3. RRT-BSC-HLUT-2

We only try to connect the sampled node to the closest parent node in the RRT according to HLUT-2 in Section 5.3 where the table is of size 20 by 20, ✓₁ = ⇡/4 and ✓₂ = ⇡/4.

(31)

4. RRT-BSC-HLUT-3

5. RRT-BSC-HLUT-4

6. RRT-OPT-EUC

Best total cost Euclidean

We use Euclidean distance to estimate the distance between the sampled node and the closest parent node, and try to make a connection to the best node. We apply the steering method to get the exact path cost. If the exact path cost is smaller than the next best estimated cost in the RRT and there is no collision, we will make a connection to this parent node. If the current parent candidate is not optimal, we save it as a temporary connection node and go on checking the next node until the best one is found. If there is collision or kinematic constraints, we try to connect to the next best nodes, until we have either found the best parent or reached a limit for checking.

7. RRT-OPT-HLUT-1

Best total cost heuristic table 1

Similar to No.6, but use Algorithm 2 with HLUT-1 to estimate the distance between sampled node and nodes in the RRT.

8. RRT-OPT-HLUT-2

Similar to No.6, but use Algorithm 2 with HLUT-2 to estimate the distance between the sampled node and nodes in the RRT.

9. RRT-OPT-HLUT-2

10. RRT-OPT-HLUT-2

4.6 Software

In the early stage of the project, ideas and tests are carried out in MATLAB version R2019b.

The functions dubinsConnection and reedsSheppConnection in MATLAB are extremely useful in this project. The main implementation of methods and evaluation is written in C++.

All experiments on test scenarios are run on windows virtual machine with Ubuntu 18.04.

Processor: Intel i7-8850H CPU @2.60GHz RAM: 8 GB

(32)

5 Results

There are a few important parameters for the simulation and evaluation:

• Minimum turning radius (Vehicle specific parameter): 7m

• Maximum nodes in a tree: 5000

• Maximum parents to try to connect: 50

e.g. Make 50 attempts to connect to the best parent node.

• Random point sampling limit: [ 200, 200]

5.1 MATLAB simulation on deciding the range of heuristic look-up table We set up the simulation as described in Algorithm 3 and 4. The result from MATLAB simulation to decide the range of the heuristic look-up table is presented in Figure 14 and 15.

When the start pose and goal pose are very close to each other, the nonholonomic distance is much longer than Euclidean distance, which can be directly observed in Figure 15. Distance between Reeds-Shepps distance and Euclidean distance decreases as the start and goal pose gets further away from each other due to the fact that the length of the straight line section increases and the percentage of the arc sections reduce. We can see that in Figure 15 that when the Euclidean distance between start and goal poses are more than 10 meters, the maximum Distance between Reeds-Shepps distance and Euclidean distance is converging to 10 meters.

Therefore, we decide to start experimenting with a heuristic look-up table of size 10 by 10, which means that if both x and y between the two poses are less than 10 we use the value from heuristic look-up table, else we use the Eucliden distance to estimate the distance between the two nodes.

5.2 Accuracy of the heuristic look-up table

We simulate the accuracy of the heuristic look-up table according to Algorithm 4. The result of the normalised di↵erence is presented in Figure 15 with a total number of 10⁶ simulations for the cases where ✓ = ^⇡₄ (90 ) and ✓ = ₃₆^⇡ (15 ). We know that the Euclidean distance always underestimates the true nonholonomic distance as the Euclidean distance ignores the distance needed to change the heading of the vehicle. When the heuristic look-up table has

✓ = ^⇡₄, the histogram for the normalised di↵erence is similar to a normal distribution with mean µ = 0. There are around 50% of the similulations that over-estimate the distance using the heuristic look-up table, but at the same time the accuracy of the nonholonomic distance has improved that more than 50% of the simulation has a normalised di↵erence of [ 0.1, 0.1]. When we use a smaller discretisation of ✓ = ₃₆^⇡, the accuracy of the heuristic look-up table increases significantly with more than 70% of the simulation slightly underestimate the distance at less or equal than 10%, and over 90% of the simulations have a normalised di↵erence of [ 0.1, 0.1].

5.3 Heuristic look-up tables

Using the results from Section 5.1 of the accuracy simulation, we decide on the discretisation for x, y and ✓₁, ✓₂. Below are the heuristic look-up tables we have generated for evaluation:

1. x_min = 0, x_max= 10, x = 1 y_min= 0, y_max = 10, y = 1

✓_{1 min}= ⇡, ✓_{1 max}= ⇡, ✓₁ = ⇡/4

✓2 min= ⇡, ✓2 max= ⇡, ✓2 = ⇡/4

(33)

Figure 14: Simulation for connection between two poses using Euclidean distance, Dubins distance, and Reeds-Shepps distance.

(34)

Figure 15: Histogram for normalised di↵erence between the estimated distance using heuristic look-up table and Reeds-Shepps distance to connect two random poses (x₁, y₁, ✓₁) and (x₂, y₂, ✓₂) that are sampled within|x| < 10, |y| < 10, |✓| < ⇡ with 10⁶ simulations.

Heuristic for vehicle motion planning with search trees

Examensarbete 30 hp December 2020

Heuristic for vehicle motion planning with search trees

Peili Guo

Institutionen för informationsteknologi

Abstract

Heuristic for vehicle motion planning with search trees

Contents

1 Introduction

2 Background and theory

3 Previous work

4 Methods

5 Results