Facilitating Automatic Setup in a Robotised Test Framework for Autonomous Vehicles by Path Planning and Real-Time Trajectory Generation

IN

DEGREE PROJECT ELECTRICAL ENGINEERING, SECOND CYCLE, 30 CREDITS

STOCKHOLM SWEDEN 2018 ,

Facilitating Automatic Setup in a Robotised Test Framework for Autonomous Vehicles by Path

Planning and Real-Time Trajectory Generation

CHEERUDEEP CHINTHA

KTH ROYAL INSTITUTE OF TECHNOLOGY

SCHOOL OF ELECTRICAL ENGINEERING AND COMPUTER SCIENCE

iii

Abstract

The research in the field of autonomous vehicles and self-driving cars is growing at a rapid pace and strong initiatives are being taken to ver- ify the capability and functionality of such autonomous vehicles.With continuous development being carried out in the field of Advanced Driver Assist Systems (ADAS) and Autonomous Drive (AD) functions, ensuring safety, robustness and reliability of these functions is chal- lenging and it requires advanced ways of verification and testing be- fore these functions are deployed on the vehicle and delivered to the customer. Testing of these modern features can be done either on test track, real driving roads or in simulations by Computer Aided En- gineering (CAE) . But testing a high-risk scenario in the real-world would be challenging due to safety concerns. Also, high regression and continuous testing requires a test framework where the develop- ment and testing can be done in an efficient way.

At Volvo Cars, it is envisioned that the best approach to test the AD vehicles is by subjecting the vehicle under test to several high risk scenarios by simulation based engineering and replicate the subset of these tests on a closed-loop test framework developed on the test track.

This thesis is a part of FFI Funded Research Project called CHRONOS2 where Volvo Car Corporation and other project partners aim to de- velop the closed-loop test framework for verification of AD Vehicles.

This thesis work focuses on ensuring efficient and reproducible test- ing in the said test framework by accurate path planning and trajec- tory generation to drive the multiple test objects to their starting posi- tions in an unstructured test environment. The algorithm developed for path planning should also ensure the generation of a safe path in real-time for the test objects in case of failure or error in the test frame- work.

The path-planning algorithm has been successfully implemented

taking the unstructured environment and vehicle dimensions into con-

sideration resulting in a safe path avoiding obstacles and satisfying

nonholonomic constraints of the vehicle. The implemented architec-

ture utilizes the parallel-process framework of Robot Operation Sys-

tem (ROS) and results in a algorithm which can run in real-time.

iv

Sammanfattning

Forskningen inom autonoma fordon och självkörande bilar växer i snabb takt och starka initiativ vidtas för att verifiera kapaciteten och funktionaliteten hos sådana autonoma fordon. Den kontinuerliga ut- vecklingen inom avancerade förarstödssystem (ADAS) och autonoma enheter (AD) gör det utmanande att garantera säkerhet, robusthet och tillförlitlighet för dessa funktioner och avancerade kontroller och tes- ter krävs innan dessa funktioner kan användas på fordonet och leve- reras till kunden. Testning av dessa moderna funktioner kan göras an- tingen på provspår, på riktiga vägar eller i simuleringar via “Compu- ter Assisted Engineering” (CAE). Att testa ett högriskscenario i verk- ligheten är svårt på grund av säkerhetshänsyn. Dessutom kräver hög regression och kontinuerlig provning ett testramverk där utveckling och testning kan ske på ett effektivt sätt.

Volvo Personvagnar anser att det bästa sättet att testa AD-fordon är att låta fordonet testas under flera högriskscenarier via simuleringsba- serad teknik och replikera en delmängd av dessa test på en provspår.

Detta examensarbete är en del av det FFI-finansierade forskningspro- jektet CHRONOS2, där Volvo Car Corporation och andra projektpart- ners utvecklar ett ramverk för verifiering av AD-fordon. Examensar- betet fokuserar på att säkerställa effektiv och reproducerbar testning i nämnda testramverk genom noggrann vägplanering och bangenere- ring för att driva testobjekten till sina startpositioner i en ostrukture- rad testmiljö. Den algoritm som utvecklats för vägplanering bör också säkerställa att en säker väg genereras i realtid för testobjekten om fel uppstår i ramverket.

Banplaneringsalgoritmen har implementerats framgångsrikt med

hänsyn till ostrukturerad miljö och fordonsdimensioner vilket resulte-

rar i en säker väg som undviker hinder och uppfyller icke-holonoma

begränsningar hos fordonet. Den implementerade arkitekturen använ-

der parallellprocessramverket för Robot Operation System (ROS) och

resulterar i en algoritm som kan köras i realtid.

v

Acknowledgements

As the thesis has been done at Volvo Car Corporation. I would like to thank everyone at Drive Assistance and Active Safety CAE group for providing me a wonderful topic and support.

Special thanks to my supervisors Siddhant Gupta and Francesco Costagliola, who continuously supported me through out the work with a great feedback and kept my thesis to progress on track. I would extend my thanks to Goksan Isil for sharing great research material and helping me out with all the doubts regarding SPAS and to my ex- aminer at KTH Mikael Johansson for his valuable help and guidance, who provided me with great ideas and research material to carry my work forward whenever I was stuck.

I’m thankful to other master thesis students at Driver Assistance

and Active Safety CAE group Vanessa Olsson, Per Ohlsson, Christo-

pher Febo, Preethi Bodduluri and Shiva Ajay for giving me a fun, won-

derful time and knowledge sharing through out our work.

Contents

Contents . . . vii

List of Figures viii Glossary . . . . x

1 Introduction 1 1.1 Problem Description . . . . 1

1.2 Thesis Objective . . . . 3

1.3 Literature Review . . . . 4

1.4 Outline and Contributions . . . . 5

2 Background 6 2.1 Optimal Methods . . . . 7

2.1.1 Dubins Curves . . . . 7

2.1.2 Reeds-Shepp Curves . . . . 8

2.2 Graph and Tree Search Methods . . . . 9

2.2.1 Breadth-first search . . . 10

2.2.2 A* . . . 11

2.2.3 RRT . . . 11

2.2.4 Hybrid A* . . . 13

2.3 Collision Detection Methods . . . 13

2.3.1 Bounding Volume Hierarchies . . . 14

2.3.2 Spatial Partitioning . . . 16

3 Motion Model 18 4 Implementation 21 4.1 Method . . . 21

4.1.1 Hybrid A* . . . 22

4.1.2 Analytic Expansions . . . 24

4.1.3 Collision Checking . . . 25

vi

CONTENTS vii

4.1.4 Trajectory Optimization . . . 25

4.2 ROS framework . . . 27

4.3 SPAS . . . 29

5 Results 31 5.1 Simulation Results . . . 31

5.2 Results from SPAS . . . 37

5.3 Re-planning . . . 39

6 Conclusions and Future Work 42 6.1 Conclusions . . . 42

6.2 Future Work . . . 43

6.2.1 Semi-Structured Environment . . . 43

6.2.2 Supervisory Logic . . . 43

Bibliography 44

List of Figures

1.1 Chronos-2 objective . . . . 2

1.2 Chronos-2 architecture . . . . 2

1.3 Return back after scenario execution . . . . 3

1.4 High Risk Scenario . . . . 4

2.1 Path planning comic [1] . . . . 6

2.2 Dubins and Reeds-Shepp Curves . . . . 8

2.3 Graph Search Method . . . . 9

2.4 Breadth-First Search Algorithm . . . 11

2.5 A-Star Algorithm . . . 12

2.6 RRT Algorithm . . . 12

2.7 Hybrid A* Algorithm . . . 13

2.8 Example of Collision . . . 14

2.9 Various types of bounding volumes . . . 15

2.10 Bouding Volume Hierarchies . . . 15

2.11 Spatial Occupancy showing two cases . . . 17

3.1 Bicycle Model [17] . . . 20

4.1 Analytic Expansions . . . 24

4.2 Trajectory optimization vs HA* output . . . 26

4.3 Optimization solution before and after clamping . . . 28

4.4 ROS Framework . . . 29

4.5 Architecture implemented in ROS . . . 29

4.6 SPAS Environment . . . 30

5.1 With A Star heuristic . . . 33

5.2 With Reeds-Shepp heuristic . . . 34

5.3 With out analytical expansion . . . 34

5.4 With both heuristics and analytical expansion . . . 35

viii

LIST OF FIGURES ix

5.5 U-Shaped Obstacle with Heuristic - Reeds-Shepp . . . 36

5.6 U-Shaped Obstacle with Heuristic - A Star . . . 36

5.7 U-Shaped Obstacle with Heuristics . . . 37

5.8 HA* - Example 1 - Parking Lot . . . 38

5.9 HA* - Example 2 - Vertical obstacle . . . 38

5.10 HA* Example 3- Curves . . . 39

5.11 SPAS Simulation, Example - 1 . . . 39

5.12 SPAS Simulation, Example - 2 . . . 39

5.13 SPAS Simulation, Example - 3 . . . 40

5.14 Trajectory length vs Execution time . . . 40

5.15 Dense obstacle map . . . 41

Glossary

ADAS - Advanced driver assist systems AD - Autonomous Driving

AR - Virtual Reality BV - Bounding Volume BFS - Breadth-first search

BVH - Bounding Volume Hierarchy CV - Constant Velocity

CA - Constant Acceleration CT - Coordinated Turn DoF - Degrees of Freedom FIFO - First In First Out HA* - Hybrid A star MIL - Model in Loop

ROS - Robot Operating System R/C Car - Radio Controlled Car RRT - Rapidly-exploring random tree RDT - Rapidly-exploring dense tree

SPAS - Simulation Platform for Active Safety VUT - Vehicle Under Test

VR - Virtual Reality

x

Chapter 1 Introduction

The development in the field of Advanced Driver Assist Systems (ADAS) and Autonomous Drive (AD) functions is going in a fast pace. Ensur- ing high performance, safety and robustness of this new technology should also go with the same pace. In order to ensure this, testing plays a critical role. Simulation based testing has matured in this field providing millions of miles of driving in various environments and weather conditions within a few weeks of time. At the same time safety can’t be guaranteed solely based on Virtual testing (Simulations) where Waymo (Former Google Self-driving) [19] simulates billion of miles, testing on a test-track of few critical scenarios, need to be per- formed to validate it.

To perform rapid, autonomous and identical testing on the test- track this involves development of complete test platform which in- volves various algorithms and hardware put together. A high-level description would be 1) Creating an environment replicating the sim- ulated scenario 2) Steer objects (actors) along the trajectories.

Steering objects can be broken down into navigation, control and perception. Where the navigation would define a trajectory to follow, control has the vehicle dynamics modelled and takes the appropriate action and perception would give feedback about the environment.

1.1 Problem Description

This thesis is a part of a research project "Chronos-2" at Volvo Car Corporation. The project aims to integrate simulations with test-track technology, which is to replicate simulated scenarios on the test track.

1

2 CHAPTER 1. INTRODUCTION

The over all diagram on how this is proposed to be achieved, is shown in figure 1.1, where a central server would be responsible to steer all the actors (VUT, Virtual objects and other objects) to replicate the sim- ulated scenario. VUT would be injected with virtual targets which will make it perceive that, it’s in a similar simulated scenario even though it’s in an open field.

t

t

t

Real vehicles

Dynamic soft targets

Virtual targets

Figure 1.1: Chronos-2 objective

The architecture of Chronos-2, is shown in figure 1.2, where the central server (Test server) would inject fake objects into VUT, steer the VUT and other objects. The driver is also provided with a VR headset where he gets the experience of the created world around and get to drive in this virtual environment.

Te st s er ve r

Position receiver

Physical object control server Simulator core

engine Dynamic

trajectories engine Supervisor

Ob je ct s VU T

Rendering &

VR headset

Ce nt ra l lo gg in g A R t est vi ew er

C-ITS I/F Position

sender Object control I/F

C-ITS simulation Trajectory updates

Part of thesis

Figure 1.2: Chronos-2 architecture

This thesis would focus on developing part of test server as high-

CHAPTER 1. INTRODUCTION 3

lighted in figure 1.2, which is to generate safe trajectories and update them to both VUT and real-objects.

1.2 Thesis Objective

The thesis aims to develop a path planning algorithm, which given start, goal positions and obstacle map, would return a smooth, safe and traversable path satisfying non-holonomic constraints. Replan- ning of the path will be done if the goal position is not changed and must happen in real-time.

Research questions

• How can the objects be restored to their original positions ?

• Can the generated path be followed by the objects ?

• Can the trajectory be with minimum steering angles ?

• Can it avoid obstacles ?

?

?

?

?

Is the path feasible Is there a solution

Figure 1.3: Return back after scenario execution

Two scenarios have been shown on how the developed algorithm

would be used. Figure 1.3 shows that, once a scenario is planned, the

vehicles would go to their destinations, after which a path planning

algorithm would be used to get them back to their start positions to

repeat the test. A second scenario is shown in figure 1.4 where a AD

or ADAS function might be under test and should be triggered before

it crosses the trigger point. In case the algorithm fails to trigger, then

a path-planning algorithm would run in the background in real-time,

planning for a safe path and the vehicle would be made to follow this

safe path.

4 CHAPTER 1. INTRODUCTION

Trigger point 1

Safe Place 2

Trigger point Safe Place 2

1

Figure 1.4: High Risk Scenario

1.3 Literature Review

Path planning has a wide range of applications in the field of robotics like automation, robotic surgery, gaming and now, in self-driving cars as well. In most of the fields, the common goal is to avoid obstacles [11] and reach a specified point. Most of the challenges in path plan- ning occur due to the fact that, the control space for all the actuators is continuous, resulting in continuous trajectories and hence the prob- lem formulation becomes a continuous variable optimization problem [16].

Most of the early search algorithms proposed work in discrete space [7], [6] and hence does produce paths which are traversable due to vio- lation of the non-holonomic constraints. Analytical solutions also exist which satisfy the kinematic feasibility of the vehicles [18], [5] but do not consider the obstacle map.

An alternative method would be to use tree-search methods like RRT’s [14], [15] which consider the constraints and work in continu- ous space. But for RRT’s to be implemented in real time, constraints occur from the heuristics on how well it can guide the search towards the goal. Also on the feasibility - whether it can always search for a solution.

Path planning used in DARPA Urban challenge [4] considered both discrete and continuous space, kinematic constraints with a possibility to pre-compute heuristics which guide to the specified point, has led to improvements in the computational time with a feasibility to imple- ment in real-time. A similar approach has been implemented in [12]

showing it could possibly work in real-time but no results on the com-

putation time were discussed and the implemented architecture was

not optimized for speed.

CHAPTER 1. INTRODUCTION 5

1.4 Outline and Contributions

The remaining part of the thesis is organized as follows

• Chapter 2 introduces various path planning algorithms which are both optimal and sub-optimal and gives theoretical back- ground before discussing the actual implemented algorithms to ensure the reader thoroughly understand the underlining theory.

Also, the collision avoidance algorithms are discussed.

• Chapter 3 briefly discusses various vehicle motion models across the literature, giving a general idea and concludes which is best situated for the implemented path planning algorithm.

• Chapter 4 discusses on what algorithms have been implemented in-order to perform path-planning in real-time and the computer languages, software’s used and the architecture to make compu- tation fast.

• Chapter 5 presents simulation results in various environments discussing on how the path-planning algorithm behaves. Also, simulations with a 7DoF vehicle model.

• Chapter 6 concludes the work done in this thesis and ends by suggesting some improvements which can be made in the future to ensure the algorithm be used in various other scenarios.

The thesis makes the following contributions relative to the state of the

art to ensure the algorithm runs in real-time, heuristics for the search

algorithm is efficiently stored in an offline table and the implemented

architecture takes the advantage of ROS nodes to run various parts of

the algorithm in parallel compared with the implementation by kurzer

[12] which ran the algorithm sequentially. The trajectory optimization

algorithm improved the smoothness of the computed trajectories en-

suring unnecessary steering angles are removed.

Chapter 2 Background

Planning is perceived differently by different group of people. In this context, planning refers to translating high-level commands from hu- mans to low-level inputs on how to move [13]. Motion, path or trajec- tory planning all refer to the same.

In this thesis the planning problem is defined as follows given a euclidean space ∈ R ² , obstacles O ⊂ R ² , start x s and goal x g position in euclidean space. Find a path P ∈ R ² from x s to x g such that P ∩ O = ∅.

In case of a no solution ∅ is returned.

Figure 2.1: Path planning comic [1]

6

CHAPTER 2. BACKGROUND 7

2.1 Optimal Methods

The path planning problem described above can be solved using con- trol theory approach, where it can be considered as an optimal control problem [16],[3]. Given the dynamical system and constraints to be satisfied both on the states and controls we can obtain the necessary controls required, which is the objective and when applied to the dy- namical system will result in the required solution. A general problem can be formulated as shown below.

min u(t)

Z tf 0

F (x(t), u(t)) + φ(x(tf ), u(tf ))

s.t

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

x(t) ∈ X u(t) ∈ U

(2.1)

Where, x(t) ∈ R ⁿ are the states, u(t) ∈ R ^p are the controls, φ(x(tf ), u(tf )) is the final cost, x 0 ∈ R ⁿ is the initial state and t f is the final time.

This approach gives the optimal solution for the stated problem but might be computationally intensive as there might not be an analyti- cal solutions for every problem stated, which requires numerical ap- proximation. In our case, where the obstacles in the environment are not static, constraints in the problem continuously change which also increases the computation time. But for simple dynamics there exist analytical solutions statisfying the non-holonomic constraints.

2.1.1 Dubins Curves

In 1957, Lester Eli Dubins [5] proved analytically that a shortest path

exists between two given points x 0 , x ₁ ∈ X in the Euclidean plane with

a constraint on the curvature r ⁻¹ of the obtained path. The constraint

on the curvature is obtained by specifying the minimum turning ra-

dius of the vehicle. The obtained path connecting two points is made

of a maximum of three segments which are a combination of straight

lines (S) and curves (C). Curves can turn right (C R ) or left (C L ). In 1974,

H.H Johnson proved using Pontryagin’s maximum principle that Du-

bins curves are optimal, making it the shortest distance between two

given points under the assumed constraints. One of the advantages

of the obtained path, it considers the non-holonomic constraints of the

8 CHAPTER 2. BACKGROUND

vehicle. Whereas, the drawbacks with the Dubins Curves lies in the assumption that the vehicle can only move forward and does not con- sider any obstacles. So, it could be well used as a heuristic with Graph and Tree search methods which will be discussed further.

Kinematic model used in Dubins Vehicle, assumes constant velocity V , constant turn rate bounded by u obtained from minimum turning radius.

˙x = V cos(θ)

˙y = V cos(θ)

˙θ = u

(2.2)

Start

Goal

Start

Goal

Dubins Solution Reeds-Shepp’s Solution

Figure 2.2: Dubins and Reeds-Shepp Curves

2.1.2 Reeds-Shepp Curves

In 1990, Reeds and Shepp [18] solved a similar problem as that of Du- bins to find an analytical solution between two points in the Euclidean plan, but they also considered that the vehicle can move both forwards and backwards, where Dubins assumed it can only move forward.

Due to this assumption the solution obtained will have a maximum of two cusps due to reversing and the length of path might be much shorter in terms of distance compared with Dubins path. The path ob- tained are constructed with segments similar to Dubins with straight lines and curves, but the maximum number of segments in a path is now five. It also considers the constraint on the curvature which is ob- tained from the maximum turning radius r.

Kinematic model used in Reeds-Shepp Vehicle, assumes constant ve-

locity V , constant turn rate bounded by u obtained from minimum

CHAPTER 2. BACKGROUND 9

turning radius, Also includes direction D ∈ {−1, 1}

˙x = D ∗ V cos(θ)

˙y = D ∗ V cos(θ)

˙θ = D ∗ u

(2.3)

2.2 Graph and Tree Search Methods

A general graph search algorithm consists of vertices (nodes) and edges connecting two vertices as shown in the graph 2.3. Every graph search

1 5

1

2

2 2 A

B

C

D

E

F

Figure 2.3: Graph Search Method

algorithm proceeds to reach the goal node through a set of operations, which can be generalized as follows.

• Being the search from the start node.

• Expand from the start node through a set of operations to find out the successors.

• Maintain a back pointer for every node to know through which node each successor has evolved to trace back the path once a goal node is reached.

• After every expansion successors are verified if any of them is a goal node. If not, expansion continues or else it terminates.

Open and Closed lists There are two lists that are maintained while a graph search algorithm operates, which are Open and Closed lists.

A open list consists of all the nodes which have not been expanded.

When the algorithm begins the open list only contains the start node,

while expanding the successors are added. The closed list contains all

10 CHAPTER 2. BACKGROUND

the nodes which are expanded, to avoid multiple expansions from the same node.

Priority Queue The open list is prioritized based on a key. The open lists selects the node which has high priority. The priority key sorts the list in increasing or decreasing order. The key in most of the cases is obtained from the cost function which is based on heuristics.

Heuristics In every graph based search algorithm heuristics play a crucial role to determine the optimality of the algorithm. Every graph has a finite number of nodes, if we don’t use any heuristic and run the algorithm blindly we can still find the goal node, but the computation time and memory requirements are huge. Heuristics ensure that the search is being proceeded in the direction of the goal ensuring regions with low probability of the goal are not explored. A simple heuristic can be a euclidean distance from the current node to the goal and this value is used as a key in the priority queue to sort the open list.

2.2.1 Breadth-first search

The algorithm was invented in 1945 by Konrad Zuse. It was rein- vented again in 1959 by Edward F. Moore [7] which was used to find the shortest path. BFS is a graph or tree search algorithm which starts at the root (Initial node) and explores the entire tree or graph by firstly exploring its neighbours. It uses a FIFO priority Queue (Q).

BFS explores a graph layer after a layer, all the k steps are explored first before proceeding with k + 1 steps, which guarantees that the first solution obtained is the shortest path. The search process is also re- ferred to a "series of wavefronts" BFS does not consider any costs as- sociated with the edges so its unweighted graph search algorithm as it does not consider any heuristic to explore its neighbours or select the Q , its time and memory intensive. The computation time for BFS is O (|N |+|E|), BFS terminates when there are no nodes to explore (.i.e. if it explored the entire tree or graph) or if finds the goal.

In order to trace the path to the goal once explored we can proceed

back from the goal as each node stores its predecessor and direction

so we could always traverse back to the start node, this make the BFS

solve the shortest path problems.

CHAPTER 2. BACKGROUND 11

1

2 3

4 5

6 7 8 9

Figure 2.4: Breadth-First Search Algorithm

2.2.2 A*

The A* search algorithm [9] is a refinement from the Dijkstra’s algo- rithm [8] which only uses cost-to-come (f (x)), which is the cost from the initial node to the current node, whereas the A* also uses a heuris- tic which approximately tells the cost-to-go (g(x G , x )) which is the cost to the goal from the current node, which directs the search more ef- ficiently reducing the time to search and avoid exploring larger part of the graph. A* also uses a Q, where a node is selected based on the minimum f (x) + g(x G , x ). The heuristic used in A* makes it find sub-optimal paths but at the same time it could be difficult to find a heuristic function for all the problems, but standard heuristics which are used are Euclidean distance and Manhattan distance which can speed up the algorithm. If g(x G , x ) = 0 ∀x then A* boils down to Dijs- tras’s algorithm.

2.2.3 RRT

RRT were developed by Steven M. LaValle and James J. Kuffner Jr

[14]. The algorithm is designed to work in high dimensional space

efficiently by building up a tree. It randomly chooses a sample and

tires to connect it with the nearest state which is already in the tree. It

only connects it to the nearest state only if given constraints are satis-

fied avoiding obstacles and satisfying control inputs. If it is so, a new

state is added to the tree. By repetitively doing this, the tree grows and

due to uniform sampling it probabilistically grows away from its start-

ing node and expands towards larger voronoi regions and explores the

entire space.

12 CHAPTER 2. BACKGROUND

S ta rt

Goal

Figure 2.5: A-Star Algorithm

As RRT’s are not guided with a heuristic they don’t give a optimal solution. Even though they sample randomly they can be biased to search around the goal state by changing the probability distribution.

In-case the sampling is not random the algorithm will be called RDTs.

In this way it can converge sooner but finding a right probability dis- tribution for a particular problem would be challenging. Also, L. Kang and L. Cheng Mao have modified RRT to include heuristics which is proved to be effective than a conventional RRT [10].

Goal

Start

Goal

Start

After 6 Iterations After 14 Iterations

Figure 2.6: RRT Algorithm

CHAPTER 2. BACKGROUND 13

2.2.4 Hybrid A*

The widely used A* algorithm works in discrete spaces, If the neigh- bours of a A* algorithm are set to 4 or 8 then it would results in a path which requires to turn on the spot, which a vehicle can’t do as it does not satisfy the non-holonomic constraints. The Hybrid A* is an extension to A* which also uses discrete spaces but the transitions between two states happen in continuous space rather than discrete.

It has been successfully in various areas such as planning flight ma- neuvers and autonomous robot navigation used in the DARPA Urban Challenge. The algorithm was first given insights by Dolgov [4].

The hyrbid A* uses a discritized gird instead of a continuous grid as it has to be finite. While state transitions happen between two nodes we can include the non-holonomic constraints of the vehicle.

Thus, usually the state space is three dimensional with states x, y, θ, we can imagine it as a cuboid with the base representing the x, y position where as the height of the cuboid represents the θ. Every node stores information about the continuous value it has reached, the discritized grid in which it lies as well as the information of the predecessor.

Figure 2.7: Hybrid A* Algorithm

2.3 Collision Detection Methods

Collision detection is a problem to detect whether two or more objects

collide. It is used in various fields like gaming, simulations, robotics

and computer animations. The problem involves determining three

questions ”if, when, where” two objects collide. if is whether two ob-

jects collide, when is at what time do they collide and where is how do

they collide [2].

14 CHAPTER 2. BACKGROUND

In the path planning scenario we need to verify if the path is col- lision free or not. Its required to only check the if problem. If the path planning is done using optimal control methods we can spec- ify explicit constraints to obtain collision free paths but in the Graph and tree search methods this would be difficult and we need to have a collision detection algorithm running separately every time a path is planned to check whether its collision free, If its not then we need to plan the path again.

Everytime a path X is planned we need to ensure its collision free X ⊂ X f ree . Incase if the environment changes we can either compute a new path or verify if the existing path is collision free X ∪ X obs = ∅ based on the computational requirements.

Collision A

B

Figure 2.8: Example of Collision

2.3.1 Bounding Volume Hierarchies

The geometry of two objects can be tested for collision, but it would be computationally expensive for testing the overlap of geometries as it would involve overlap test for hundreds of polygons. The alternative and much simpler method would be to test bounding volumes (BV).

A BV is a simple enclosure around a complex geometry object. The intersection test between the bounding volumes will be much simpler.

Testing the geometric shape (Polygons) of two objects involves a complexity of O(n ² ) decreasing the number of polygons will reduce the computation effort drastically thus BV’s are much easier to test.

Even though a BV test results in collision, we need to perform geomet- ric test to verify collision which is complex, but BV’s would reduce the effort by a constant factor. Bounding volume hierarchy (BVH) [2]

method can be used to reduce the computation time logarithimically

compared with BV method. In this method a complex object is bro-

ken into more than one simple BV and the BV’s of a single object are

CHAPTER 2. BACKGROUND 15

Figure 2.9: Various types of bounding volumes

arranged into a tree hierarchy. These small sets of BV’s are enclosed into a larger BV. Firstly, the test is performed on overall enclosed BV, if a possible collision is detected then the children nodes are tested for collision. This would reduce the computation time compared with collision detection with geometric shapes and increase the accuracy compared with simple BV’s collision check.

Some desired properties for bounding volumes

• Use little memory

• Tight fitting

• Inexpensive to compute

Similarly, the desired properties for BVH

• The nodes in a subtree should be near each other

• The sum of all BV’s should be minimum

• Each node should be of minimum volume in the hierarchy

Figure 2.10: Bouding Volume Hierarchies

16 CHAPTER 2. BACKGROUND

2.3.2 Spatial Partitioning

Spatial partitioning [2] provides the overlap test by dividing the space into regions and verify whether objects are in the same region. If ob- jects collide they should be in the same region of space, thus its more efficient compared to checking pair-wise collision with every other ob- ject present. There are various type of Spatial partitioning, this section focuses on a method based on grids.

In grid based method we divide the space into uniform girds this will divide the entire space into gird cells of uniform size. Then each object can be associated with the grid cells it overlaps. If a possible collision is suppose to exist a cell should be occupied by more than one object.

The important considerations for grid based method is considering the cell sizes. Some issues are listed below

• Too fine gird - If the cells are too small larger number of cells need to be updated with the associativity information of the objects.

Everytime a object or obstacle changes position the number of cells updated is huge, memory occupied is more and also the computation time.

• Too coarse gird - If the size of the cells is larger compared to the size of the objects then there might be more than one object in each cell and we might end up detecting collisions even when they don’t collide.

• Too fine and too coarse gird - If objects present are largely vary-

ing in dimensions then the grids might be too fine for few objects

and too coarse for others.

CHAPTER 2. BACKGROUND 17

Figure 2.11: Spatial Occupancy showing two cases

Chapter 3

Motion Model

In order to plan a path which can be followed by a vehicle at the same time satisfying the non-holonomic constraints we need to have a kine- matic model (motion model) in the path planning algorithm. There are various models which can represent the motion behaviour of the vehicle, where each model is best suitable in a different situation. Few of the models are CA, CT, CV and bicycle model.

Constant Velocity

In CV model the vehicle moves with constant velocity with zero accel- eration and assumes the angular velocity is zero, so the vehicle can’t turn. The mathematical model is given below.

 ˙p(t)

˙v(t)



= 0 1 0 0

 p(t) v(t)



(3.1) Where p is position and v is Velocity.

Constant Acceleration

Similarly to the CV model, In CA model the vehicle moves with con- stant acceleration with zero angular velocity. The model is given.





˙p(t)

˙v(t)

˙a(t)



 =





0 1 0 0 0 1 0 0 0







 p(t) v(t) a(t)



 (3.2)

Where a is the acceleration.

18

CHAPTER 3. MOTION MODEL 19

Coordinated Turn

This model is similar to CV model, but the mathematical model has an extra state where the vehicle can move with a constant angular veloc- ity.













˙x(t)

˙y(t)

˙v(t) φ(t) ˙

˙ω(t)













=













v(t) cos(φ(t)) v(t) sin(φ(t))

0 ω(t)

0













(3.3)

Where φ is heading angle and ω is angular velocity.

Bicycle Model

The bicycle model can incorporate steering angles as well as continu- ous change in acceleration. It also includes a slide slip angle β. The model defines the vehicle motion more accurately compared to other models.









˙x(t)

˙y(t)

˙v(t) φ(t) ˙









=









v(t) cos(ψ(t) + β(t)) v(t) sin(ψ(t) + β(t))

a(t)

v(t)

l

sin(β(t))









(3.4)

β(t) = tan ⁻¹ ( l _r l _f + l r

tan(δ f (t)))

For path planning we need a vehicle motion model which has a state

to turn the vehicle. Both the CT and bicycle model can be used in path

planning. But the bicycle model has more states which would increase

the dimensions in the search space and would make it quite difficult to

obtain a solution quick enough. Even though its more accurate com-

pared to CT model the computation time required does not make it a

good choice to choose. Therefore, the CT model is used in the path

planning algorithm proposed.

20 CHAPTER 3. MOTION MODEL

Figure 3.1: Bicycle Model [17]

Chapter 4

Implementation

4.1 Method

This section presents the implemented method, which are the algo- rithms used to obtain a fast, collision free and smooth path satisfying the non-holonomic constraints. The path planning algorithm used is Hybrid A* and a parallely running Reeds-Shepp method.

The path planning problem in the continuous domain is the use of complex and multi-variable optimization. If the path planning is done in discrete space using Graph and Tree search methods (A* or Breadth- first) which are simple algorithms easy to implement and will quickly give the results, but they do not produce solutions which are smooth and do not consider non-holonomic constraints. RRT’s discussed in section 2.2.3 have the advantage to produce results in higher dimen- sional space considering the non-holonomic constraints, but come with a disadvantage as they are not optimal, converging to a solution far from the optimal, suffer from problems where they get stuck and also they are highly non-deterministic.

Hybrid A* which is an extension to A* to include the non-holonomic constraints and uses well-informed heuristics to obtain a solution in the near optimal regions. The heuristics used which are discussed in the next section reduce the search nodes significantly which decreases the computation time and combined with the Reed-Shepps method will further boost its speed to obtain a solution.

21

22 CHAPTER 4. IMPLEMENTATION

4.1.1 Hybrid A*

As mentioned in section 2.2.4 hybrid A* performs the search in discrete space rather than continuous but the nodes are expanded in continu- ous space. The algorithm uses coordinated turn (CT) model as a kine- matic model during planning. A node or state is specified by x, y, θ where x and y are positions and θ is heading angle. The algorithm is fed with a obstacle map (binary map as a grid), initial state X I and goal state X G and the output solution is a set of states X i satisfying the kinematic constraints imposed by CT model as minimum turning radius.

The CT model assumes a fixed velocity of the vehicle. An explicit velocity is specified and the kinematic model is propagated for a fixed amount of time. The time parameter can also be used for tuning. If we specify a larger sampling time the search process is faster as it would search a greater area in shorter cycles but might not converge to the goal state due to discritization of the grid and vice versa if we select a smaller sampling time. So, a optimal sampling time should be selected based on the grid dimensions and resolution.

In order to implement the non-holonomic constraints which is im- plemented by maximum and minimum turning angles. Each node is expanded by applying one of the six control actions u ∈ U which are maximum turning left, max turning right, no turning and the same set is applied in reverse.

Even though a constant velocity is chosen in the algorithm, once the required solution is obtained, we can re-calculate the velocity pro- file based on other control requirements. The search continuous un- til it explores the entire gird (Open-list is empty) or until the goal is reached.

Heuristics

While the hybrid A* is guaranteed to reach the goal if a feasible solu- tion exists but the time it would take is not limited and how quickly it would converge to the goal depends on the heuristics considered. HA*

considers two heuristics, considering only one among them should

also work, but its proved [4] that considering the maximum of the both

CHAPTER 4. IMPLEMENTATION 23

makes the algorithm much quicker and search in a smaller space to find the goal. Both the heuristics capture different assumptions 1) "non holonomic without obstacles" ignores the obstacles in the environment and only considers the kinematic constraints, whereas 2) "holonomic with obstacles" ignores the kinematic constraints and only considers the obstacles.

Non-holonomic without obstacles

This heuristic considers the kinematic constraints of the vehicle while not taking into account the obstacles given by the obstacle map. The heuristic is implemented with Reeds-Shepp curves [18]. As they are considered to be optimal and take into account the minimum turning radius and heading angle it ensures HA* would search or expand the nodes in the right direction and approach the goal with a correct head- ing. In case if this heuristic is not considered the HA* might approach the goal in the wrong direction and would only realize once it reaches the goal and the HA* would struggle by having unnecessary steering behaviour to put the vehicle in the right heading.

As Reeds-Shepp curves do not consider the obstacle information, the path given two nodes (start,goal) in the eulidean space (x, y) is the same even though both these points are translated equally. So, this heuristic table is pre-computed and stored as a look up table, which improved the performance in terms of computation time by a factor of ten.

Holonomic with obstacles

This heuristic is the dual of the first that is it ignores the kinematic con- straints but only considers the information from obstacle map. It en- sures that HA* does not tend to expand its nodes into a dead end and also avoids U shaped obstacles. It does so by computing the shortest distance to the goal from the current node which can be accomplished with A* algorithm using a Euclidean distance heuristic.

The heuristic is stored as a look up table to increase the perfor-

mance. The look up table is obtained by storing the closed-list with

the cost information from every position in the grid to the goal and the

A* is run until the open-list is empty.

24 CHAPTER 4. IMPLEMENTATION

4.1.2 Analytic Expansions

The node expansion in Hybrid A* may not reach the goal node ex- actly due to discretization of space and due to expansion in continuous space with a fixed sampling time.

To overcome this problem the HA* is agumented with search from Reeds-Shepp curves. Every time HA* adds a new node to the open- list its put on to a queue with only one entry. The Reeds-Shepp’s al- gorithm will grab the latest node put on the queue and solve it until the goal. Unlike [12], where the Reeds-Shepp is run every nth itera- tion and more often when reaching the goal, the implementation in ROS takes the advantage of ROS parallel processing power and runs Reeds-Shepps on a separate processor node which will give an advan- tage to not limit it for every nth iteration but run as frequently as it could.

After every solution from Reeds-Shepp curves it is checked whether its colliding with the obstacles, If it is so the solution is rejected and proceeds with the next node on the queue. If it finds a obstacle free solution the HA* algorithm is terminated.

In a obstacle free environment this approach would increase the speed of the algorithm significantly. As the solution from Reeds-Shepp is significantly faster compared with HA*. An example is shown in figure 4.1 where the HA* search solution contributes to approximately 1/4th of the path while the rest is given by Reeds-Shepp.

HA* Sol.

Breaking Point

ReedsShepp Sol.

Figure 4.1: Analytic Expansions

CHAPTER 4. IMPLEMENTATION 25

4.1.3 Collision Checking

Spatial partitioning as described in section 2.3.2 is used for collision checking X ⊂ X f ree . As this method allows to pre-compute the re- quired information and store in a look-up table, which makes the al- gorithm work in constant time for any given vehicle orientation and location.

In order to compute the look-up table a basic bounding box is com- puted around the vehicle. The vehicle is then places at the origin of the grid and all the cells occupied by a particular configuration (head- ing angle) is stored in a look-up table, the vehicle is then rotated with a particular heading resolution from 0 − 360 degrees. In this way the look-up table is complete and can easily be translated to a particular vehicle location.

Once we know the cells occupied by the vehicle we can cross check the cells with the obstacle map to check for a potential collision. This algorithm is used in various places - In HA* if collision exists then a particular successor node in the open-list is not added, In Reeds- Shepp analytic expression the solution won’t be considered valid and In Trajectory Optimization algorithm the solution won’t be valid and optimization will continue.

4.1.4 Trajectory Optimization

The path generated by HA* may not be smooth and may contain un- necessary steering commands and might be close enough to obsta- cles. To overcome this the solution of Hybrid A* is passed through a smoothning algorithm.

The algorithm uses a non-linear optimization technique on the ver- tices (states) of the path, which will move the vertices ensuring the kinematic constraints are satisfied and smoothning the path. The opti- mization is solved using conjugate gradient descent technique. Figure 4.2 shows the output of trajectory optimization with less steering in- puts compared with HA* solution.

The cost function is given in equation 4.1, the first term is the obsta- cle term which ensures the vertices are far away from obstacles where o _i is the distance between vertex i and the closest obstacle to it, d max

being the maximum distance allowed between a vertex x i and obsta-

cle o i , σ o is a quadratic penalty function and w o is weight which will

impact the change in the path.

26 CHAPTER 4. IMPLEMENTATION

Output of Trajectory Optimization Output of HA*

Obstacle

Figure 4.2: Trajectory optimization vs HA* output

Gradient for Obstacle term

∂σ _o

∂x _i = 2(|x i − o _i |−d _max ) x _i − o _i

|x _i − o _i |

The second term in equation 4.1 is the curvature term which would ensure the minimum turning radius is maintained, where κ max is the maximum allowed curvature, σ k quadratic penalty function and w k is the weight which will give preference to maintain the curvature.

Gradient for Curvature term

∂κ _i

∂x i

= − 1

|∆x i |

∂∆φ i

∂ cos(∆φ i )

∂ cos(∆φ i )

∂x i

− ∆φ i

(∆x i 2 )

∂ ∆x i

∂x i

,

∂κ _i

∂x _i−1 = − 1

|∆x i |

∂∆φ i

∂ cos(∆φ i )

∂ cos(∆φ i )

∂x _i−1 − ∆φ i

(∆x i 2 )

∂∆x i

∂x _i−1 ,

∂κ _i

∂x _i+1 = − 1

|∆x i |

∂∆φ i

∂ cos(∆φ i )

∂ cos(∆φ i )

∂x _i+1

CHAPTER 4. IMPLEMENTATION 27

Where,

∆φ i = cos ⁻¹ ∆x ^T _i ∆x i+1

|∆x i ||∆x i+1 | ,

∂∆φ i

∂ cos(∆φ i ) = ∆ cos ⁻¹ (cos(∆φ i ))

∂cos(∆φ i ) = −1

(1 − cos ² (∆φ i )) ^1/2 Cost Function

w _o

N

X

i=1

σ _o (|x i −o _i |−d _max )+w k N −1

X

i=1

σ _k ( ∆φ i

|∆x i | −κ _max )+w s N −1

X

i=1

(∆x i+1 −∆x i ) ² (4.1) The third term in the equation 4.1 is the smoothning which is the most important and would ensure that the path obtained after op- timization contains less steering angles .i.e. eliminting unnecessary steering behaviour.

Gradient for Smoothness term

∂

∂x _i = −4(∆x i+1 − ∆x i )

∂

∂x _i+1 = ∂

∂x _i−1 = 2(∆x i+1 − ∆x i )

A clamping method mentioned in [4] is used in the optimization algorithm. After the optimization is done if the path is colliding with the obstacles then the particular states which are colliding with the obstacles are replaced with the HA* solution and will be ensured that they cannot be changed by the optimizer, then the optimization is run again. In this way we can guarantee safety for the path obtained from the optimizer. In the worst case if all the states are clamped then the original HA* solution is returned by optimizer. Figure 4.3 gives an idea on how it works and improves the solution.

4.2 ROS framework

Robot Operating System (ROS) is not an operating system which can

run stand-alone, but provides services for message-passing between

processes, package management etc. ROS can run in any language if

28 CHAPTER 4. IMPLEMENTATION

A

B

A

B HA* Solution

HA* Solution Optimization Solution

before clamping Collision

Optimization Solution after clamping

Clamped point to HA*

Figure 4.3: Optimization solution before and after clamping

the client libraries exist. In this thesis C++, python and matlab are used, this makes it most preferable software to use, as different pieces of code can be written in different languages.

Also, the vast support from open-source community and open- source libraries available for path planning, perception and control makes it best suitable for robotics applications, the capability to run different ROS nodes in parallel, 3D visualisation tools available in ROS, distributed computing power, all put together makes it best platform to use for rapid development and deployment. The path planning al- gorithm presented in this thesis is entirely developed in ROS.

ROS framework is shown in figure 4.4, where 4.4a shows how com- munication is achieved between different nodes. In ROS there is only one master which facilitates communication between different nodes through topics. Every node can run standalone and each node can be coded in a different programming language. Every node commu- nicates with every other through ROS topics which run on TCP/IP protocol which gives the advantage to not only run all the nodes on one computer but run different nodes on different machines enabling distributed computing as shown in figure 4.4b.

The implemented architecture in ROS for the proposed path plan- ning algorithm is shown in figure 4.5. The ’Map Server’ nodes pro- vides the binary obstacle map via the topic ’MAP’ to ’HA*’ and ’Traj.

Opti’ nodes which is used for collision checking. ’HA*’ expands the

search nodes and gives it to ’ReedsShepp’ (Analytical expansion) via

CHAPTER 4. IMPLEMENTATION 29

Master ROS

ROS ROS Node ROS

Node Topic

TCP/IP ROS Communication

(a) Communication

(b) Distributed Computing, Source - MathWorks

Figure 4.4: ROS Framework

’R.S. Search’, this topic has queue length of 1. If a solution exists through analytical expansion it is communicated to ’HA*’ through ’Sucess Bit’ which would terminate the entire algorithm and return the path (Solution) via ’Merged Path’. ’Matlab Node’ runs SPAS, which takes the path and simulates it with the 7DoF model.

Matlab Node Frame Broadcaster tf

HA*

StartPos.

GoalPos.

Map Server Map

Success Bit

Traj. Opti. TrajOutput ReedsShepp

RS. Path HA* Path RS. Search

Merged Path Visualization Marker

ROS Nodes ROS Topics Runs SPAS

Gives Obstacle Map

RS. — ReedsShepp Path containing

HA* + RS.

Figure 4.5: Architecture implemented in ROS

4.3 SPAS

SPAS is a simulation platform developed in Matlab/Simulink which

enables to run virtual testing of vehicles in various traffic environ-

ments. SPAS consists driver, power-plant, transmission, drive-line,

30 CHAPTER 4. IMPLEMENTATION

chassis, brakes, steering and the electrical systems models to design a complete vehicle considering all the major details, It also includes models for other traffic elements, such as sensors, roads and road signs.

The SPAS environment is shown figure 4.6.

It also aids to achieve a closed loop simulation of the shown ar- chitecture in figure 4.5. Thus the path planning algorithm in ROS can be combined with SPAS driver model (Controller) and vehicle mod- els to virtually test whether the vehicle models are able to follow the planned path.

Figure 4.6: SPAS Environment

The 7DoF model used in SPAS has degrees of freedom in longitude,

lateral and yaw (heading) axis and in the 4 wheels along its rotational

axis.

Chapter 5 Results

5.1 Simulation Results

The results for Hyrbid A* algorithm are presented in this section. Var- ious scenarios are presented discussing the computation time needed and how HA* would perform in different scenarios. Also, turning off some of the heuristics used in HA* is compared by looking into the number of explored nodes and computation time. Analytical expan- sion explained in section 4.1.2 is also discussed on how it would effect the performance.

The HA* algorithm is running in ROS with a architecture specified in section 4.2. The results from these simulations are presented first.

Later on the path generated from HA* is given to SPAS to simulate with the 7-DoF vehicle model to check if it could follow these paths.

A Matlab ROS node is initiated which will import the HA* paths from ROS to Matlab and then SPAS in ran.

Colour coding for ROS simulations: As mentioned in the previ- ous section the obstacle map is given to ROS as a binary map. White represents free space whereas black represents obstacles. The bound- aries of the obstacle map is removed for better visualization.

• Bounding box blue: Encloses the vehicle dimensions to visualize, ensuring it does not collide with obstacles.

• Blue arrows: Path generated from HA*

31

32 CHAPTER 5. RESULTS

• Yellow arrows: Path generated from trajectory optimization al- gorithm.

• Red lines: Explored nodes in forward search from current node.

• Green lines: Explored nodes in backward search from current node.

The vehicle dimensions used for collision detection is 1mx1m as the algorithm is planned to be tested on a RC car which would fit into this bounding box. The turning radius used is 6 meters. This parameters are kept constant for all the simulations presented in this section.

ROS was installed on "Ubuntu 16.04 LTS" which was ran on "Oracle VM VirtualBox" with 10 GB RAM and 7 CPU’s allocated disabling 3D acceleration mode. The laptop on which Virtual Box was running is in- stalled with Windows 7 64-bit OS, 16 GB RAM, Intel Core i7 2.90 GHz CPU.

Scenario - 1

The entire obstacle map for this scenario is show in fig 5.15. Table 5.1 gives the specifications including the start and goal positions used. A comparison is made among the two heuristics used and also with the analytical expansion method.

Start Position, x s [45, 7, 180]

Goal Position, x g [35, 14, 180]

Grid size 50mX50m Grid resolution 1m Angular resolution 1deg

Table 5.1: Scenario-1 specifications

Simulation with only one heuristic 4.1.1 (A Star) and analytical ex-

pansion 4.1.2 is show in figure 5.1. The number of nodes explored are

55 with a time 0.4648 sec. The numbers of explored nodes are few but

the solution has three cusps (Change in directions) and more length

compared with other results. Also, the nodes explored are in the di-

rection of the goal.

CHAPTER 5. RESULTS 33

Figure 5.1: With A Star heuristic

Comparing this with only heuristic 4.1.1 (ReedsShepp length) shown in figure 5.2 the length of solution is much smaller with only one cusp which is better, but the number of nodes explored 486 with a time 8.664 sec. As the heuristic does not consider the obstacles, the expan- sion of nodes is always on the wrong side which in-turn resulted in a increased number of explorations.

Without using the analytical expansion method proposed in sec- tion 4.1.2, its difficult to reach the exact goal node due to discritization of the grid, as seen in figure 5.3 it can’t reach the exact goal state, the solution stops around the goal state. Where the tolerance in distance is specified ≤ 2m. The explored nodes 646 with time 19.2 sec.

Including both the heuristics and analytical expansion method the solution is shown in figure 5.4. The search nodes are quite few 53 with a time 0.2302 sec. The length of solution is also less compared with only A star heuristic. The search direction also happens in the correct direction without exploring the entire space in a random way.

Scenario - 2, U Shaped Obstacle

In this scenario the obstacle map consist of only one U-Shaped obsta-

cle. The start x s and goal x g are set on either sides of the obstacle. To

34 CHAPTER 5. RESULTS

Figure 5.2: With Reeds-Shepp heuristic

Figure 5.3: With out analytical expansion

understand if the implemented algorithm would get stuck expanding nodes inside the U-Shape.

Using only heuristic 4.1.1 (Nonholonomic without obstacles) the so-

CHAPTER 5. RESULTS 35

Figure 5.4: With both heuristics and analytical expansion

Start Position [12, 17, 360]

Goal Position [38, 18, 360]

Grid size 60mX30m Grid resolution 1m Angular resolution 1deg

Table 5.2: Scenario-2 specifications

lution is shown in figure 5.5, where the algorithm spends most of the time inside the U-shape exploring nodes. As the heuristic does not consider obstacles this solution is obvious, with 1005 explored nodes and 32.5411 sec to reach the goal state.

The break in the solution seen in figure 5.5 between the blue arrows is where the HA* solution ends and analytic expansion starts, In trajec- tory optimisation this is compensated through interpolation increas- ing the resolution of the path.

Using heuristic 4.1.1 (Holonomic with obstacles) is shown in figure 5.6 with 289 explored nodes with 3.3685 sec, whereas the case includ- ing all the heuristics the explored nodes are 288 with a time 3.4511 sec.

The solution is almost the same in all the three cases.

36 CHAPTER 5. RESULTS

Figure 5.5: U-Shaped Obstacle with Heuristic - Reeds-Shepp

Figure 5.6: U-Shaped Obstacle with Heuristic - A Star

Scenario - 3, Examples

Few more additional scenarios are tested figure 5.8 shows a parking

lot scenario where few parked vehicles are considered as obtacles and

CHAPTER 5. RESULTS 37

Figure 5.7: U-Shaped Obstacle with Heuristics

a start (red arrow) and goal (green arrow) positions are given. There is a smooth trajectory generated with one cusp, explored nodes are 56 in a time of 0.2597 sec.

A scenario with one vertical obstacle blocking the two regions with a opening to pass between the two regions is shown in Figure 5.9. After specifying the start and goal a path is generated exploring 98 nodes in a time of 0.5945 sec, this path has unnecessary steering angles due to the tuned cost function. Which can be compensated by tuning the cost function in HA* or considering semi-structured environment which will be discussed in section 6.2.1. A similar scenario with few more vertical obstacles is shown in figure 5.10 with 154 explored nodes in 1.04 sec.

5.2 Results from SPAS

After the path planning algorithm returns a solution, it is imported

into matlab by running matlab node in ROS and registering to the topic

of interest. In this case it registers to the ’TrajOutput’ as shown in fig-

ure 4.5, after which this trajectory is given to SPAS which has a driver

model acting as a controller and various vehicle models. The vehicle

model used here is a 7DoF model. 3 Scenario have been simulated as

38 CHAPTER 5. RESULTS

Figure 5.8: HA* - Example 1 - Parking Lot

Figure 5.9: HA* - Example 2 - Vertical obstacle

shown in figures 5.11, 5.12 and 5.13, the left figure shows the algorith

running in ROS, whereas the right figure shows the imported trajec-

tory into matlab and simulated output from 7DoF model. In turnings

and curvatures its not able to track the path exactly. One reason might

be due to the driver model which is not accessible to tune it.

CHAPTER 5. RESULTS 39

Figure 5.10: HA* Example 3- Curves

60 65 70 75 80 85 90 95 100

X-Pos - Meters 0

5 10 15 20 25

Y-Pos - Meters

ActualTrajectory Vs Followed

7DoF Path

Figure 5.11: SPAS Simulation, Example - 1

22 24 26 28 30 32 34 36 38

X-Pos - Meters 40

45 50 55 60 65 70 75 80

Y-Pos - Meters

ActualTrajectory Vs Followed

7DoF Path

Figure 5.12: SPAS Simulation, Example - 2

5.3 Re-planning

In most situations replanning of the path might be required while the

vehicle is following the previously planned trajectory. Replanning

might happen due to various reasons few might be the vehicle devi-

ated significantly from the given trajectory due to control algorithms

or obstacles are updated on the go and new obstacles are occupying

the path generated. Replanning is viable only if its fast enough. This

40 CHAPTER 5. RESULTS

15 20 25 30

X-Pos - Meters 15

20 25 30 35 40 45

Y-Pos - Meters

ActualTrajectory Vs Followed

7DoF Path

Figure 5.13: SPAS Simulation, Example - 3

section presents timing results from HA*, where the start position is changed whereas the goal is kept constant, this scenario is tested in a dense obstacle map which is 50X50 meters with a grid resolution of 1 meter and 1 degree angular resolution shown in figure 5.15. By chang- ing the start positions continously, computation time and path length are noted which are shown in figure 5.14, where the trajectory length is plotted in meters on the X-Axis with time measured in seconds on the Y-Axis.

We can note that increasing path length does not increase the com- putation time but increasing complexity might be a strong possibility for a sharp increase in computation time. 80% of the cases the com- putation is ≤ 0.3 sec(3.33Hz), which makes it possible to use the algo- rithm in real-time. As the algorithm was running on a Virutal box it can be quite slower. The timing can be significantly improved if it was running native Ubuntu.

Length vs Execution time

Time - Sec.

0 0.3 0.6 0.9 1.2

Length - Meters

31.964389 41.199651 67.101024 46.249151257 25.08184250

Figure 5.14: Trajectory length vs Execution time

CHAPTER 5. RESULTS 41

Figure 5.15: Dense obstacle map