Final Year MEng Project Report MATLAB A* Based Path Planner for Autonomous …engineer.john-whittington.co.uk/wp-content/uploads/201… · · 2016-12-13University of Bath Department

University of BathDepartment of Mechanical Engineering

Final Year MEng Project Report

MATLAB A* Based PathPlanner for Autonomous

Off-Road Vehicle

by

John Whittington

Supervisor: Dr. Necip SahinkayaAssessor: Prof. Patrick Keogh

Bath, May 2012

‘I certify that I have read and understood the entry in the StudentHandbook for the Department of Mechanical Engineering on Cheatingand Plagiarism and that all material in this assignment is my ownwork, except where I have indicated with appropriate references. Iagree that, in line with Regulation 15.3(e), if requested I will submitan electronic copy of this work for submission to a PlagiarismDetection Service for quality assurance purposes.’

Abstract

A path planner is required as part of an autonomous off-road vehicle projectat the University of Bath, which is able to generate both an initial path(off-line) around obstacles to a goal, and update using data from cameradetection during driving (on-line). The A* search algorithm is selectedas the program foundation (including a custom cost function with terraintraversability and steering penalty) and developed using the MATLAB com-puting package. Limitations of the algorithm are clear, with a 45◦ anglestep making the method unsuitable for driving autonomy. Following thisconclusion the planner is improved and better tailored to our project, bytesting three ‘angle-angle’ methods: Line of Sight Path Smoothing, Theta*and Field D* Interpolation. The two former are found to create the desiredline, unbounded by the nodal grid, but as ‘short-cut’ techniques still provideno method of limiting the degree of turning.

Field D* interpolation is successfully implemented and forms the finalplanner in both off-line and on-line routines, generating a path in averagetimes 1.8s and 50ms respectively, for a 0.1km2 workspace, 1m node size,and a 7.5◦ interpolation step; parameters found to be most suiting to ourvehicle with a 22.5◦ steering limit. Heuristic weighting is found to improvesearching time by 130%, using a value 20. Steering weight is less decisiveand depends on the workspace configuration, a marginal inclusion in f(n)using a value 15 is seen to reduce drive time. All results are simulated due tothe project and other sub-systems not yet being at a testing stage; real-worldtesting remains as the principle of any future work.

CONTENTS 3

Contents

1 Introduction 61.1 Project Background . . . . . . . . . . . . . . . . . . . . . . . 61.2 Aims and Objectives . . . . . . . . . . . . . . . . . . . . . . . 71.3 Report Structure . . . . . . . . . . . . . . . . . . . . . . . . . 9

2 Literature Review 102.1 The A* Algorithm . . . . . . . . . . . . . . . . . . . . . . . . 112.2 A* Optimisations . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.2.1 Cost Function . . . . . . . . . . . . . . . . . . . . . . . 122.2.2 Line of Sight Path Smoothing . . . . . . . . . . . . . . 122.2.3 Field D* . . . . . . . . . . . . . . . . . . . . . . . . . . 132.2.4 Theta* . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.3 Existing MATLAB Code . . . . . . . . . . . . . . . . . . . . . 14

3 Basic A* Path Planner 153.1 Off-line . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.1.1 Discretisation . . . . . . . . . . . . . . . . . . . . . . . 153.1.2 Graph Space Population . . . . . . . . . . . . . . . . . 153.1.3 Graph Space Plotting . . . . . . . . . . . . . . . . . . 173.1.4 Path Generation . . . . . . . . . . . . . . . . . . . . . 183.1.5 Path Plotting . . . . . . . . . . . . . . . . . . . . . . . 20

3.2 On-line . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203.2.1 Simulated Driving and Obstacle Detection . . . . . . . 203.2.2 Local Re-Planning . . . . . . . . . . . . . . . . . . . . 21

3.3 Results and Disussion . . . . . . . . . . . . . . . . . . . . . . 233.3.1 Off-line . . . . . . . . . . . . . . . . . . . . . . . . . . 233.3.2 On-Line . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

4 A* Search Improvements 304.1 A* Path Smoothing . . . . . . . . . . . . . . . . . . . . . . . 30

4.1.1 Method . . . . . . . . . . . . . . . . . . . . . . . . . . 304.1.2 Results and Discussion . . . . . . . . . . . . . . . . . . 314.1.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . 32

4.2 Theta* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 324.2.1 Method . . . . . . . . . . . . . . . . . . . . . . . . . . 324.2.2 Results and Discussion . . . . . . . . . . . . . . . . . . 334.2.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . 34

4.3 FieldD* Interpolation . . . . . . . . . . . . . . . . . . . . . . 344.3.1 Method . . . . . . . . . . . . . . . . . . . . . . . . . . 344.3.2 Results and Discussion . . . . . . . . . . . . . . . . . . 364.3.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . 38

CONTENTS 4

4.4 Steering Weighting . . . . . . . . . . . . . . . . . . . . . . . . 394.4.1 Method . . . . . . . . . . . . . . . . . . . . . . . . . . 394.4.2 Results and Discussion . . . . . . . . . . . . . . . . . . 404.4.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . 42

5 Integration with Overall Project 435.1 Steering, Control and Trajectory Tracking . . . . . . . . . . . 43

5.1.1 Method . . . . . . . . . . . . . . . . . . . . . . . . . . 435.1.2 Results and Discussion . . . . . . . . . . . . . . . . . . 445.1.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . 45

6 Discussion 46

7 Conclusion 48

8 Future Work 498.1 Real-World Testing . . . . . . . . . . . . . . . . . . . . . . . . 498.2 Improve Velocity Optimisation . . . . . . . . . . . . . . . . . 498.3 Parallel Computing . . . . . . . . . . . . . . . . . . . . . . . . 508.4 Moving Obstacle Re-Planning . . . . . . . . . . . . . . . . . . 518.5 Off-Course Re-Planning . . . . . . . . . . . . . . . . . . . . . 518.6 Improved Heuristic Cost . . . . . . . . . . . . . . . . . . . . . 518.7 Adaptive Node Grid . . . . . . . . . . . . . . . . . . . . . . . 52

A Quad and Design Specification 55

B Supporting Planner Data 57

C Path Planner V1.0 Code 59C.1 Scripts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

C.1.1 main.m . . . . . . . . . . . . . . . . . . . . . . . . . . 59C.1.2 driving.m . . . . . . . . . . . . . . . . . . . . . . . . . 62C.1.3 driving path.m . . . . . . . . . . . . . . . . . . . . . . 63

C.2 Functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66C.2.1 pathing.m . . . . . . . . . . . . . . . . . . . . . . . . . 66C.2.2 update path.m . . . . . . . . . . . . . . . . . . . . . . 70C.2.3 adjacent.m . . . . . . . . . . . . . . . . . . . . . . . . 71C.2.4 lineofsight.m . . . . . . . . . . . . . . . . . . . . . . . 73C.2.5 obstacle insert.m . . . . . . . . . . . . . . . . . . . . . 75C.2.6 obstacle field.m . . . . . . . . . . . . . . . . . . . . . . 78C.2.7 obstacle roulette.m . . . . . . . . . . . . . . . . . . . . 79C.2.8 path smoother.m . . . . . . . . . . . . . . . . . . . . . 80

CONTENTS 5

Acknowledgements

Necip for his always useful insights at 10am on a Monday morning.

Pejman Iravani for creating the Autonomous Quad Project and for actingas quasi-project manager.

1 INTRODUCTION 6

1 Introduction

With the steady progression of electronic vehicle management within carsand low cost - high performance computing, the autonomous car is a naturalprogression. Indeed, the automotive industry realise this and as such, mostof the main manufacturers now have a department working on autonomy.

Recent advances in sensory perception such as LIDAR (Light Detectionand Ranging: 3D mapping of surroundings) and GPS, have brought devel-opment to the stage where vehicles such as Google’s can navigate a busy citydriverless [1]. General Motors have suggested their autonomous cars couldbe on the road as soon as 2018 [2] and the Royal Academy of Engineeringclaim Britain could see driverless trucks by 2019 [3].

Whilst on-road autonomy has obvious commercial interest, the subject ofthis project regards off-road terrain; a sector with a variety of applications.The DARPA Grand Challenge, has promoted development in this terrainwith Sanford University’s, Stanley, able to convincingly navigate itself alonga 150km course through the Nevada Desert [4]. Need for exploration ofunsafe or inaccessible areas such as minefields, the Moon and as far a fieldas Mars, and it is easy to see the demand for research into off-road autonomy.

Cross-over between on-road and off-road development is clear but with-out a predefined road to follow, path generation and navigation are keycomponents of an off-road autonomous vehicle. This report covers the devel-opment of such a path planner, to be implemented in an off-road autonomousquad bike.

1.1 Project Background

A new venture for the University of Bath and a collaboration with Mechan-ical and Electrical Engineering departments, the autonomous quad projectaims to convert an ‘off the shelf’ miniature quad bike to a driverless one.With the intention of driving around the University campus, the terrain canbe considered grassy and rolling, with numerous trees. To help with thedesign and specification process, a UK farmer was defined as a theoreticalcustomer; a similar terrain to that of the campus. The autonomous require-ment is for the quad to drive, without auxiliary input, from an initial pointto a pre-defined goal point within a maximum 1km area. With obstaclesand terrain types to avoid, a path must be generated for the quad to follow;the basis of this project.

A quad bike is a non-holomonic vehicle (3 degrees of freedom (DOF),only 2 controllable): it must have forward velocity to steer. Secondly, it iskinodynamic: the velocity acceleration bond must be satisfied, it cannot stopor change to a constant velocity instantly. The former simplifies the pathplaning task. Whilst the path must be smoothed and limited to the turningradius for a given speed, it is vastly simpler than that of a holomonic vehicle

1 INTRODUCTION 7

Figure 1: Project sub-systems with main interfaces illustrated by arrows. Hard-ware and control oversees the components within the quad and as suchcan be represented as the central system. The path planner can be seento take input from obstacle detection and GPS, as well as input/outputto steering control.

such as a Swiss wheel driven robot. For an off-road robot in an open space,it is ideal. The latter point complicates the robot dynamics somewhat, butcan be largely ignored for path planning and is more a consideration forthe steering and tracking system. Additional quad parameters along witha design specification can be found within Appendix A. Of particular noteis the maximum turning radius, 2.1m (22.5◦ steering lock), and the speed,5mph, of which the generated path must also be constrained.

The project consists of nine sub-projects, shown in Figure 1. The projectswhich the path planner most depends on are the camera based obstacle de-tection, GPS mapping and localisation, and the steering, control and tra-jectory tracking.

1.2 Aims and Objectives

By using map data provided from the GPS mapping system, including:starting point, the goal, known obstacles and terrain information, an initialpath must be generated - the off-line portion of the planner - before thequad moves. Then, receiving data from the GPS and trajectory trackingsystems, localise the quad on the path as it drives. During the drivingprocess, the planner must be listening for obstacle data from the cameradetection system. If a new obstacle appears, the planner must check forcollision with the current path and generate a new path as necessary - on-

1 INTRODUCTION 8

line path planning. A flow diagram of the intended inputs and outputs ofthe system can be seen in Figure 2.

The aims of the sub-system is summarised as follows:

• Create a discretised grid of the robot workspace from map data pro-vided by the GPS and localisation system, including traversabilitydata.

• Apply the A* algorithm to the grid in order to find the least cost pathto the goal. The path must be tailored to the quad specification inorder to be trackable, this means a method of any-angle pathing isrequired.

• Whilst driving the path, the planner must constantly interface withthe obstacle detection system in order to re-plan around obstacles.

• Include a visual output of the generated path and subsequent re-plans.

• Interface with the trajectory tracking system: supply the path coor-dinates and receive current quad position for the on-line routine.

• Run on minimal hardware (basic laptop/Pico ITX), which will haveother programs running in the background from the other sub-systems.MATLAB has been selected as the platform which trajectory track-ing, obstacle detection, GPS mapping and this system will use.

Figure 2: A flow diagram of the path planning sub-system including various inputsand outputs.

1 INTRODUCTION 9

1.3 Report Structure

Development of the path planner was first considered in two stages:

1. Off-line: Create the off-line path generator by implementing the A*algorithm in MATLAB, tailored to the quad requirements. With hind-sight for online planning, the programme was designed such that thegraph space configuration and algorithm could be generated and up-dated independently.

2. On-line: Create the program layer to run whilst the quad drives thepath, which updates the path on receiving issue of new obstacles fromthe obstacle detection system. The new path segment is generatedlocally, using the same pathing algorithm function used in the off-linestage. In order to test this process without a working quad, a simpledriving simulation is developed.

As such, review of the planner is initially broken up into these parts, withthe basic planning functions described. Many of the ‘Off-line’ functionsare independent of main and called in later scripts - this was one of therequirements during development.

Secondly, the initial planner was a basic implementation of A*, ignoringpathing restrictions due to quad steering. Following the completion of theplanner and review of the results, refinements to the algorithm were madeincluding a number of any-angle A* hybrids, cost functions and, with anglenow incorporated into the design, a quad steering restriction added.

Finally, the planner is integrated with the trajectory tracking sub-systemsoftware - the system which creates the signals to be sent to the actuators- and compatibility with other sub-systems is confirmed (full testing couldnot be completed).

2 LITERATURE REVIEW 10

2 Literature Review

[5] provides a good definition for the role of a path planner in mobilerobotics: ‘Given partial knowledge about its environment and a goal positionor series of positions, navigation encompasses the ability of the robot to actbased on its knowledge and sensor values so as to reach its goal positions asefficiently and as reliably as possible.’

The quote encompasses the role of the path planner within this projectwell. From mapped data and a goal position, an initial path is to be devel-oped - only large obstacles and basic terrain data will be known. As the quaddrives along the path, a camera detection system will pick up new obstaclesand if these are found to lie on the path, the path will adjust accordingly,leading to the most efficient and reliable route to the goal.

Path planning can be split into two main types: searching algorithmsand the potential field method (PFM). A full review of these methods canbe found in [6]. As the A* search algorithm was selected as the basis ofthis path planner, a review of this method and its variants will be presentedwithin this section.

A* is a grid search technique first developed in 1968 [7]. Since its in-carnation, it has become a popular algorithm in both robotics and videogame AI. In [8], an A* path planner for a off-road vehicle is demonstratedthough simulation to be effecttive and it includes steering, and traversalcosts; features we aim to include. One of the inherent limitations of A*are the non-smooth paths created due to the 45◦ nodal spacing. Methodsto overcome this (known as any-angle path planning) include during thepathing process, Field D* [9], Theta* [10], and post-processing, A* PS [11],each are presented in 2.1.

A project which is surprisingly similar in requirement and specification toours are the Mars Exploration Rovers Spirit and Oppertunity. The robotsgenerate an off-line path from basic mapping of Mars’ surface; costs fordriving, obstacles and traversability are applied. The on-line planner workswith data received from a stereo, camera based obstacle detection system[12]. A* was built into the project to replace an inferior localised plannerwhich caused many false dead-ends, with much success. The 2007 DARPAChallenge winner, Stanley, incorporates a hybrid A* planner which includespath smoothing, PFM for obstacle avoidance and a camera based terraintraversability detection [13]. The result is a very comprehensive plannerand although equipped with technologies beyond the scope of this project,many of the principles can be considered.

Our algorithm bases itself upon that of A* but adds traversability cost,interpolation between nodes to create any-angle paths, and a steering limitwith a reduction factor.


2.1 The A* Algorithm

Figure 3: The A* planning foundations applied to a nodal grid. Upper cell, g, isthe total cost to travel from current node n to that node n′. Lower cell,h, is the estimated cost to the goal. [14]

The basic A* algorithm works upon a grid of nodes, each with a costassociated to it: g(n) the total path cost to travel to that node and h(n), aheuristic cost estimate from the node to the goal [Figure 3]. Therefore, theinitial step is to discretise the robot workspace into a nodal map.

Starting at the initial node S, the adjacent nodes are ‘explored’ usingtwo global data arrays: OPEN and CLOSED. OPEN contains the nodesbeing considered and contains only S on first step. The cost function f(n)of all nodes in OPEN is calculated:

f(n) = g(n) + h(n) (1)

The node of least cost - referred to as n - is removed from OPEN andplaced in CLOSED. CLOSED contains nodes to be ignored; nodes whichare obstacles or have been travelled too.

All eight nodes adjacent to n are ‘expanded’ and referred to as n’. Thenext step can follow two routines [14]:

a. If n’ is not in OPEN or CLOSED, place it in OPEN, mark their parentas n. The total cost of moving to n’ is found by adding the cost ofmoving from n to n’ to the path cost g(n), equal to g(n’). The costpossible function f(n’) is calculated and assigned to n’ in the OPENarray.

b. If n’ is in OPEN check whether f(n’) < f(n). If it is, make n the parentof n’, otherwise, move on.


Figure 4: A* PS path versus true shortest path. [10]

In a basic sense, the algorithm moves along from the initial point toothe goal by exploring eight nodes at a time. Since the total path cost isrecorded, the algorithm has an element of knowledge and is complete; it willalways find a path to the goal (G) if one exists. The algorithm ends whenG is added to CLOSED or if OPEN becomes empty and G has not beenfound; there is no path to the goal.

With the parent of each node recorded, the final path is generated byworking backwards from G, saving the coordinate of each parent in anotherarray until S is reached.

2.2 A* Optimisations

Although A* is a robust and reliable path generator, many hybrids exist toimprovement upon its limitations. As explained above, because A* move-ment is limited to grid corners, the generated path is inherently truncatedand is rarely the true, shortest path. For this reason, most real world A*implementation includes a form of any-angle pathing.

2.2.1 Cost Function

The cost function can be designed beyond basic direct (Pythagoras) pathcosts. [8] includes functions for steering angle, position and roll, creatinga planner which weighes up options such as reversing or doing a u-turn toavoid adverse inclines. It is standard practice in off-road planners to includea terrain traversal cost; an arbitrary value dependant on terrain type andadded to each node, then included in the cost function [8], [12], [13].

2.2.2 Line of Sight Path Smoothing

Line of sight path smoothing is a post-pathing technique [11], which checksthe hypotenuse between adjacent nodes on the path for collision with obsta-cles. If it is clear, the line is extended to the parent node and checked again.This process continues until the line is broken by an obstacle, the previous(clear) line is then used as the new path between the two nodes, skipping the


nodes between the two points [Figure 4]. The line created can be any-angleand is not bound by the grid space and whilst it does find shorter paths thanA*, it still may not be the true optimum. Secondly, without intervention, itcan create paths which clip the corners of obstacles.

2.2.3 Field D*

Figure 5: [8] describes D* interpolation ‘(a) The typical transitions (in blue)allowed from a node (shown at the centre) in a uniform grid. Noticethat only headings of 45 degree increments are available. (b) Usinglinear interpolation, the path cost of any point s′ on an edge betweentwo grid nodes s, and s2 can be approximated. This can be used toplan paths through grids that are not restricted to just the 45 degreeheading transitions.’

Field D* improves A* by interpolating between nodal points; removingthe grid corner restriction. Consider two nodes s1 and s2, and the pointbetween these nodes s’, at a distance y from s1 and (1 − y) from s2 [Figure5]. The path cost to s’ can be derived as:

g(s′) = y.g(s2) + (1 − y).g(s1) (2)

Using this method, the true path of least cost can be found by finding theminimum g(s′) value along the cell edge. Many techniques to achieve thisare documented in [9], [15] and can be simple or complex, depending onthe quality of the final path. A good example of real-world Field D* isdemonstrated on the Mars Rovers [12].

2.2.4 Theta*

Theta* is an implementation of the line of sight function during the pathingprocess. When expanding nodes, the line of sight is checked between n’ andparent(n). If the path is clear, parent(n′) = parent(n), thus avoiding n andcreating a path unbound from the grid [Figure 6]. It is a simple and lowcost addition to A*, which has been found to find shorter paths than bothA* PS and Field D* [10].


Figure 6: Paths generated by Theta*. Left: Path 2 (Theta*) would be used asline of sight clear. Right: Path 1 (A*) would be used as line of sightblocked. [10]

2.3 Existing MATLAB Code

A search on the Mathworks File Exchange found a good example of A*search in the form of a tutorial [16]. More of a tool to demonstrate the A*algorithm, the program structure and functions provide useful groundworkfor the design of our planner, and implementing basic A* in MATLAB.

3 BASIC A* PATH PLANNER 15

3 Basic A* Path Planner

The sections below explain the operation of MATLAB path planner. Ac-companying scripts can be found in [Appendix C] and run from the USBstick included with this report.

3.1 Off-line

The planner runs from the script main all functions, including the on-lineplanning phase are called from here. The script also contains any userconfigurable variables relating to the workspace, quad and planner.

3.1.1 Discretisation

First, the graph space of the robot is constructed, cspace. cspace is a 3Darray: four layers which each layer the size of the node space. The fourlayers are used to store various costs of each node so that they can be easilyreferred to and also plotted:

Layer 1 Used to store the node type: 1 - obstacle, 0 - free space, 1 - initialpoint, 2 - goal. This allows a function to find the various points withoutrequiring another input and is essential for obstacle detection.

Layer 2 Path cost g(n). The distance to travel to this node from the cur-rent position and previous path.

Layer 3 Heuristic estimate to goal h(n). The estimated cost to the goalusing function heuristic.

Layer 4 Traversability cost t(n). Using the data from the GPS system, thetype of terrain can have a cost associated to it. Additionally, obstaclescan be given a high traversability ‘field’ so that the quad does not passnear.

3.1.2 Graph Space Population

Following the creation of the cspace array, costs are populated. Goal andinitial points are simply inserted from user defined coordinates. Known ob-stacle and traversability data is inserted using the functions obstacle insertand traversability insert, described below.

Function - obstacle insert

An interface between the GPS/camera detection obstacle data and pathplanner was needed. We decided an array obs, containing various data forindividual obstacles was the best means, providing a quick and effectiveway to transfer obstacle data between the systems; Figure 7 illustrates the


array structure. Obstacles can be single node, square, circle or lines andoverlapped to create almost any shape. For simulation, a random obstaclegenerator obstacles was created, which randomly creates a given number ofobstacles. When implemented, the array obs would be predefined by theGPS system.

Figure 7: The obs array specifies the coordinates of the obstacle (local to thegraph space), main x dimension, y ratio to x, and the shape: 0 - singlenode, 1 - square, 2 - circle or 3 - line. fsize is a predefined percentageof the obstacle size and is used for the surrounding traversability field.

obstacle insert takes obs and draws the obstacles into cspace. It doesthis using a number of expansion processes from the central node, dependanton the obstacle shape.

As part of the obstacle drawing process, a traversability value, whichdecays over a distance fsize (a percentage of the obstacle size) from theobstacle edge is added using the function obstacle field. Without this field,the path would ‘hug’ obstacle edges in order to avoid them, if they lay on theoptimum route to the goal. The field adds a factor of error; the obstacle maynot be perfectly shaped or plotted, and the quad may brush the obstacleotherwise. The value added and length of decay should be refined throughreal-world testing.

Function - traversability insert

The function to insert traversability takes the array trav and applies itto the corresonponding nodes in cspace. trav is an array equal in size tothe graph space space, and is populated by the GPS system. Values fortraversability are arbitrary; the affect to f(n) is the same, regardless of the


scale. Since the algorithm seeks the node of least cost, less desirable areasare given a larger value. For example, grass could have a value 10, tarmac avalue 5 and gravel 15 (to avoid slip). Traversability is not limited to surface,contours can also be given a value, these options are explored by the GPSand mapping sub-system [17].

For the purpose of testing, traversability insert has the option of gen-erating a random integer within a given range for each node - used in allresults within this report. In practice the array trav will be supplied bythe mapping system, which uses a combination of mapped data and visionbased information gathering [17].

3.1.3 Graph Space Plotting

10 20 30 40 50 60 70 80 90 100

10

20

30

40

50

60

70

80

90

100

C−Space

Horizontal x (m)

Ve

rtic

al y (

m)

Node Edge

Node Edge

Low Trav Cost

Med Trav Cost

High Trav Cost

Obstacle

Initial

Goal

Figure 8: The discretised workspace of the quad or ‘graph space’. A grid is appliedto the figure to visualise the nodes. Each node is then painted by theplot cspace function, depending on the type and traversability cost (seelegend). Here, the space is 100x100m with nodes 1x1m.

We decided a way of viewing the planned path, obstacles and path asit updates was required. Using two functions plot cspace and plot path, asingle figure window is refreshed as and when necessary by the planner.

main creates the figure window on the first run of plot cspace (first runis denoted using an input flag). Drawing of the figure uses basic MATLABfunctions and is well commented. The various node types and traversabilitycosts are plotted using ‘markers’. First, arrays of the nodal coordinates ofeach object to be plotted are extracted from the cspace using if statements,then these are drawn to the plot in various marker types and colours 8. Itis worth noting that nodes are considered at the corner points of each gridcell.


Using hold, the figure is kept in the foreground and updated with eachconsecutive run of the plot functions. Although not necessary for the oper-ation of the planner, plotting allows us to see generated path during bothsimulation and real-world running. When implemented into the autonomousquad using a ‘barebones’ computer, the screen can be shared using VNC overWiFi to allow us to see the current position on the path and any possibleobstacle conflicts. In the same manner, a software based kill can be per-formed (a hardware based, default to kill-switch is also included in the quaddesign [18]).

3.1.4 Path Generation

The A* algorithm is run using the function pathing, which, given inputsof the various workspace variables, outputs an array path, containing nodalcoordinates of the best path to the goal.

Path searching begins by creating the OPEN (3) and CLOSED (4)lists as arrays; the integral components of the A* algorithm [Section 2.1].The structure of OPEN allows the node of least cost to be found, and itsvariables extracted.

OPEN =

active(boolean)1 x1 y1 parentx1 parenty1 g(n)1 h(n)1 t(n)1 f(n)1 bearing1

......

......

......

......

......

active(boolean)n xn yn parentxn parentyn g(n)n h(n)n t(n)n f(n)n bearingn

(3)

CLOSED =

x1 y1

......

xn yn

(4)

Expansion

Next, obstacles defined in the graph space are placed directly into CLOSED.The initial point is added to OPEN and as the current node n, its locationand cost values are stored in the corresponding n variables - used duringthe expansion and comparison routine. As this node is now current andexplored, it is marked inactive in OPEN and placed in CLOSED.

A while loop is used for the expansion and progression routine - set torun until n = G or the path is blocked. The first step is expansion of thenodes adjacent to n using the function adjacent.

Function - adjacent

adjacent explores the eight nodes surrounding n. It does this using a verticalincrement within a horizontal increment to circumvent n. Each adjacentnode is checked for locale within the graph space, that it is not ‘closed’


and, for the later Field D* implementation, whether it is within the steeringrange. If it meets these requirements, it is loaded into the next row ofexp array; the output of the function. exp array is similar in shape toOPEN (3) as it is used to populate OPEN during the expansion process.It does not have parent or active columns as it is not a direct row insertion.It is at this stage that the cost functions for the node are calculated (5).

f(n) = g(n)︸︷︷︸path travel cost

+

heuristic distance to goal︷︸︸︷h(n) + t(n)︸︷︷︸

traversability cost

(5)

Where:

• g(n) - Distance already travelled plus Pythagorean distance to node(distance.m). Later, a steering cost based on weighted angle turnedis included here (steering.m).

• h(n) - Pythagorean distance to goal from node (distance.m). Initially,using the same function as g(n), searching was slow. The solution tothis is described in Section 3.3.1.

• t(n) - The traversability cost received from the GPS system, extractedfrom cspace.

Searching

With the adjacent nodes expanded, the next stage in pathing and A* iscomparison of current path cost with that of the nodes in OPEN , to seeif a lower path cost exists then current. Essentially, OPEN is a priorityqueue. The current path cost is that of f(n), if the expanded path is foundto be less than this, the current path is abandoned and changed to the lowercost f(n′), by setting n as the parent of n′. This is only true for nodescurrently in OPEN which, on first run, will not be the recently expandednodes. For these nodes, a flag is used to add them to OPEN using thefunction insert open, which creates a single row array of OPEN structure(3).

The final part of the while loop searches OPEN for the node of lowestcost function, to move to and become n. It does this using the functionminfn. The function extracts all the active nodes in OPEN , rememberingtheir index - this step is required to ignore nodes already explored. Usingmin, it then finds the least cost active node in this temporary array andoutputs its index within OPEN . With the index of the least cost functionnode, pathing then extracts that row from OPEN into the variables usedfor n, for the next expansion and searching loop.


Backtracking

Once G has been reached, a backtracking process must be completed inorder to create the path for which the quad will follow. The process startsfrom the last node in CLOSED (the goal will be the last explored) andtracing back down the family tree by reading the parent nodes. Functionnode index is used to find the index of a node, given its coordinates and isvital to the working of this process. It works using a for statement to checkeach row of OPEN until the inputted coordinates match. Using this index,the parents of the nodal coordinates can by found.

An array path is filled using a while loop to implement the methoddescribed above, until the parent nodes are equal to the initial point. Eachrow of path contains the x and y coordinates of the next node to be travelled- to be read by the trajectory tracking system. Since path is filled from thegoal point, the path must be followed by reading the array from the topdown; the array is upside down. Whilst this is not how the trajectorytracking system reads the path, an inversion process is performed in theinitial stages of that programme.

3.1.5 Path Plotting

The final path is added to the graph space figure using the function plot path.Other than simply adding the line to the held plot, the function may be usedto animate this process so that the path route can be more easily understoodand also colour demoted so that different paths can be easily compared.

3.2 On-line

The principle of the on-line planner is to re-run the pathing algorithm,locally, around the obstacle, on receipt of new obstacle data from the cameradetection system. As this was developed before the trajectory and pathfollowing system was finished, a temporary driving simulator was developed(driving.m).

We needed a way of denoting new obstacles and their parameters inorder for the on-line planner to be active - to react only when a new obstacleappears. For this, a flag interrupt was decided obs flag, which the cameradetection system would make true when a new obstacle to be avoided. Thenew obstacle would be transferred in an array new obs, of same shape asthe obstacle array obs [Figure 7].

3.2.1 Simulated Driving and Obstacle Detection

driving is a script, to be run after main, which simply traverses along thex and y positions in path. At each step, the position is first plotted as a dotso that the on-line planning can be visually interpreted. Following this, an


if is used to determine if a new obstacle has been recognised by the cameradetection, by checking the value of new obs. If true, the new obstacle is plot-ted by adding it to cspace and running the plot cspace function. Followingthis, the path update function is run to adapt the path as necessary.

With the camera detection system being developed in parallel and not yetcompleted, a way of randomising obstacles to test the planner was required.For this, obstacle roulette was developed.

Function - obstacle roulette

This function is run on each step but only creates a new obstacle if a randominteger is true. The chance of a new obstacle being created is determined bythe size of the integer range. The idea can be likened to Russian roulette,where chance is the size of the barrel, and the single integer which runs theobstacle generator, the bullet (5 in this case). If the value 5 is generated,new obs is filled using the obstacle generator obstacles [Section 3.1.2], andthe new obstacle flag made true.

The position of the new obstacle can be weighted such that obstacleswill be generated on the path using the input near path. This provides amore realistic simulation of what the camera will be detecting.

3.2.2 Local Re-Planning

A new obstacle will only obstruct a small portion of the complete pathand as such, it is not necessary for the whole path to be re-drawn - savingprocessing and ensuring obstacle avoidance is completed as promptly aspossible, minimising any delay to the quad. pathing was developed withthis in mind: taking initial and goal points as inputs, these can be anypoints anywhere within cspace, not just the initial points used for off-lineplanning.

Function - update path

The re-planing routine takes place in the function update path. Firstly, thenew obstacle is checked for interference with the path; it may be the casethat the camera detection has picked up an obstacle in its field of view, priorto the path changing course, thus already avoiding the obstacle. To do this,the function obstacle scan is used.

Function - obstacle scan

Two while loops are used to scan the rows of path. First until the coordi-nates lie in an obstacle (using node type in cspace), this is the start pointof the obstacle, the coordinates are saved in the output obsstart. If upon


reaching the end of path, no obstacle type has been found, the new obstacledoes not interfere and the no obstacle flag (no obs) is raised.

With the collision point found, the point where the quad can re-enterthe old path must be found. A second while scans path (using the sameincrement as before) until the coordinates no longer lie in an obstacle - theend point - and saves this in the output obs end. If the obstacle does notend, the goal is obstructed, and is flagged using no obs. The path indexesof the exit and enter points are included as outputs so that the new pathsegment can be inserted into path in the correct position.

Returning to update path: Having found the points of interference withpath, a path around these points is generated. A buffer of five nodes isincluded either side of the obstacle and then the pathing function is runusing these points as initial and goal. As pathing inherently finds the leastcost path to the goal avoiding obstacles, the local path traverses the shortestparameter of the obstacle in order to return to the old path - see Figure 15.

The new segment must be inserted into path and invalid part overwrit-ten, before it can be followed. To do this, the function array insert is used.With path adjusted to avoid the obstacle, the final steps plot the new path.

Function - array insert

A function which re-constructs one array, with another array of any lengthincluded at a specified index. The top and bottom segments (in referenceto the entry and exit indexes) of the initial array are saved. In the case ofpath, this means only the clear, unobstructed segments. main array is thenfilled with new array from the entry index, overwriting the collision pathand expanding its size. The top segment of main array is then added backin at the end point of new array.

The final steps in the re-planning routine are to include new obs in themain obstacle array obs, then clear new obs and obs flag so that the routineis not run again until a new obstacle is detected.


3.3 Results and Disussion

10 20 30 40 50 60 70 80 90 100

10

20

30

40

50

60

70

80

90

100

C−Space

Horizontal x (m)

Ve

rtic

al y (

m)

Figure 9: Basic A* off-line planning. Open space traversability has been excludedto make the resulting path more visible. The planner successfully findsa path to the goal, avoiding obstacles and preferring low traversabilityregions. The limitations of the 45◦ movement are clear, with truncationswhich the quad may be unable to precisely track.

3.3.1 Off-line

Figure 9 shows the A* off-line planner works to a basic level. The inher-ent limitations of A* are clear, mainly the 45◦ angle step, but the plannerachieves the basic foundations.

The planner operates on a discritised grid, a sensitivity analysis was per-formed in order to find the optimum node size for the quad [Table 7]. Figure10 shows nsize has an exponential affect on pathing time. One must considerthis and also whether nsize is a factor of the initial and goal coordinates(they are local to cspace). Using this and the size of the quad (1240x740mm[Table 1]), a value 1(m) provides a satisfactory resolution, creating a paththat the simulated vehicle is able to follow, within the required time [Section5].

Although the path created is not perfectly trackable by the quad due toits truncated nature, the trajectory tracker would in effect smooth this, asthe feedback loop operates. This is only the case when there is not multipleor sharp turns beyond the quad turning radius, when the error would becometoo large, causing the quad to veer far off course - for example, Figure 11.On the whole, this would not be the case but in order for the planner tobe fully competent, a method of any-angle with steering limit applied isrequired.


1 2 3 4 5 6 7 8 90

5

10

15

20

25

nsize (m)

Path

Tim

e (

s)

25 10 15 300.05 0.1 0.5 1

nsize (m)

Figure 10: Node sensitivity analysis: an exponential affect on pathing time. nsizemust also be a factor of initial and goal coordinates; the reason for therange of values tested.

During first testing, the planner was found to be very slow, especiallywith t(n) included. Analysis of the searching routine revealed that thesearch would near the goal coordinates then return to a search near initial.Inspecting f(n) values, the problem was being caused by a balancing of thefunction upon nearing the goal: since both g(n) and h(n) use distance tofind the cost, near the goal, h(n) would possess very similar values to g(n)near initial, creating f(n) values almost matching those initially expandedin OPEN . The search routine would return to looking at this nodes, sincethey have a lower f(n) than n. Searching would reset in the manner manytimes before finally reaching the goal. In order to remedy this, a weightingfactor is applied to h(n) (6) so that it is distinct and all f(n) values areunique.

h(n′) = k ·√

(xg − xn′)2 + (yg − yn′)2 (6)

Originally, pathing time was 0.3s and 1.6s for A* and FieldD* respec-tively, introducing the weighting factor yielded a 130% decrease [Table 3].In order to find the effective weighting range, an analysis was performed[Figure 12]. Path distance is seen to flat-line beyond a value of 30, as h(n)overcomes t(n); traversability is ignored. A value of 20 was found to be theoptimum for the arbitrary traversability range used but anything greaterthan 1 provides the desired effect on pathing time [Table 3].

With random traversability included [Figure 13] the path is seen to de-viate or ‘wobble’, hunting the least cost path. Exacerbated by the randomareas of traversability, an increased heuristic weighting factor can remove it -if desired: in practice, the planner is achieving its task and given a real-worldmap, traversability would not be seen to change with such frequency.


6 8 10 12 14 16 18

6

8

10

12

14

16

C−Space

Horizontal x (m)

Ve

rtic

al y (

m)

Node Edge

Node Edge

Low Trav Cost

Med Trav Cost

High Trav Cost

Obstacle

Initial

Goal

Figure 11: An obstacle placed between close proximity initial and goal highlightsthe no steering limit issue. Here a u-turn is created, which the quadwould have no chance of tracking.

3.3.2 On-Line

Figure 14 demonstrates a good example of the on-line planner in action.Obstacles which appear on already driven sections of path are ignored -although this is unlikely in real-world driving it is still a useful resourcesaving feature. A large obstacle appeared 3/4 of the way along the path andthe path is successfully adjusted (b). Following this adjustment, anotherobstacle appeared in the same vicinity. Rather than create another exit pointfrom the old path, the planner re-adjusts the re-planned section, providingthe optimum route (c).

The limited angle-step problem - apparent in off-line planning - is moreprominent on-line, as paths are tighter with more cornering [Figure 14]. Thequad could often have difficulty tracking around obstacles using basic A*; asituation where tracking error is less forgiving.

Re-planning takes an average of 19ms [Table 4]. Considering MATLABscripts do not run in parallel, this time is vital to the operation of theplanner, as all other routines, including tracking, must pause whilst thepath is re-planned. Whilst the tracking system has no path to follow, theDAC used by the system to actuate the driving controls will hold the lastsample, causing the quad to maintain current course. At the specified speedof 5mph, 20ms represents a distance 45µ; negligible and well within trackingerror (5% [Table 2]).

Consideration was made for the optimum path exit and re-entry points


0 5 10 15 20 25 30 35 40 45 50110

120

130

140Goal Weighting Factor Analysis

Pa

th D

ista

nce

(m

)

0 5 10 15 20 25 30 35 40 45 500

10

20

30

Tim

e t

o P

lot

(s)

Weighting Factor

Figure 12: Heuristic distance to goal weighting analysis. Values above 30 out-weigh t(n), causing a flat-line. The intended effect of reducing thepathing time is clear by the sharp drop with factor 2 - due to thedifferentiation from g(n).

during the re-planning routine. Rather than use the default five node buffer,re-planning from the current position to the goal was considered. The ideathat a new obstacle may lend to a better route, completely un-like theoriginal path was the thinking behind this. In practice, this was neverfound to be the case [Figure 15] and even if the new obstacle did overlap thefive node buffer, a small amount of back-tracking would not problematic.The advantage of the defined buffer over current position is minimal pathgeneration, re-planning time and the previous course is more likely to still becorrect (during re-plan). For example, an obstacle spotted early in drivingwould require re-planning of a great deal of the path, taking up to 10x aslong [Table 5]. Instead, the buffer remains but with an if statement tore-plan to G if within 15 nodes.

3.4 Conclusion

Basic A* has been created and shown to be a simple and quick methodfor finding a least cost path from start to finish. The basic planner is, onthe whole, a robust solution and provides good foundations for a number ofimprovements:

• A method of any-angle pathing is required as 45◦ angle between nodesis beyond that of the quad’s steering range (22◦).

• Second to the any-angle requirement, a way of limiting the node ex-


Figure 13: Traversability can create a path which ‘wobbles’ between high costareas, making paths which are difficult for the quad to track - high-lighted by the dashed circles. Heuristic weighting can help to preventthis if necessary.

pansion to those which are reachable by the quad’s steering range isneeded.

• The traversability range and other csapce variables, need tailoring withreal-world data from the GPS mapping, and other systems.

• An adaptive node grid density (low in the open space, high near ob-stacle boundaries) could greatly improve the pathing time (althoughtime is not an issue presently).

True to the experiences of others, we can conclude basic A* is good forliteral pathing, in computer game AI for example but not competent enoughfor vehicle use. The following sections document the development of A* intoa planner, more tailored to our quad.


Figure 14: Demonstration of online re-planning (traversability is removed for clar-ity). (a) obstacles which appear on path which has already been trav-elled are ignored. (b) a section of re-planned path has since had anobstacle appear. (c) a re-planned section is re-planned again as an ob-stacle appeared at re-entry. The routine was stopped before the goalwas reached to show the simulated quad position (turquoise dot).


Figure 15: Part of testing the on-line planner included analysis of the best initialand goal points for local re-planning. By default, a buffer of 5 nodes isadded at path exit and re-entry (green line). The current position canalso be used (red line) and if within a defined distance of the goal (15nodes), that is used as the finish. It was noted that on many instances- and in this case - plotting from current had the same effect as a 5node buffer, only requiring more time for generation of path alreadygenerated.

4 A* SEARCH IMPROVEMENTS 30

4 A* Search Improvements

4.1 A* Path Smoothing

4.1.1 Method

In order to implement line of sight path smoothing described in Section 2.2.2,the functions lineofsight and path smoother were developed. path smootheris run after the standard A* pathing function; it is a post-processing tech-nique.

Function - lineofsight

This function works by finding the difference between the two points tobe checked and scanning across this distance for obstacle interference. Itchecks across which ever change in axis is greater to ensure competency.The output los clear defaults to true. If at any point during the delta scanan obstacle is detected, los clear is flagged false.

Function - path smoother

Figure 16: The basic operation of the path smoother function, demonstrated byexaggerating path deviations. The blue line shows the new, smoothedpath, red the old path. The blue line would be a straight line throughthe original path, if not for an obstacle which breaks the line of sight.

path smoother implements lineofsight by incrementally checking theline of sight between nodes in path, using a for decrement of path (pathstarts from the goal). Starting at the initial point, the next node in path


is checked for clear line of sight. Only if it is not clear is the new outputarray path smooth incremented and current node in path added. Thus,path smooth contains coordinates of nodes which can be driven to in astraight line, and is an ‘any-angle’ path - unrestricted by the grid [Figure16]. The process is best understood by looking at the plot created, whenlineofsight has plotting enabled [Figure 17].

4.1.2 Results and Discussion

Using the path smoothing technique, paths are around 5% shorter thanthe standard A* method, for a negligible time cost [Table 5]. Indeed, theyappear to be the true, absolute minimum distance to the goal.

10 20 30 40 50 60 70 80 90 100

10

20

30

40

50

60

70

80

90

100

C−Space

Horizontal x (m)

Ve

rtic

al y (

m)

Figure 17: The operation of path smoother is clear with plotting at each line ofsight check enabled. One can see the original path by tracing the edgeof the magenta scanning process. Incrementally checking each pointin path until the line of sight is blocked, short-cuts are produced anda true, shortest path. Traversability is ignored however and obstacleedges may be clipped.

Path smoothing was selected as the initial ‘any-angle’ method due to itsease and self-contained nature. Whilst the implementation was successful,the limitations are clear:

• As a post-processing technique - applied after the least cost searchingroutine - traversability cost is ignored. Whilst this may not be amassive problem in the real-world as the smoothed path rarely veersfar from the generated path, it is still an unsatisfactory result.

• Clear line of sight does not mean safe driving range as smoothed lineswill often ‘brush’ the edges of obstacle nodes [Figure 18]. A quad size


Figure 18: A* path smoothing with random open space traversability. Althoughtraversability is enabled, the path smoother function does not accountfor this when creating the ‘any-angle’ lines; it is a shortfall of thetechnique. True shortest paths are created but like standard A*, thejunctions created still may not be drivable by the quad. The brushingof obstacles is highlighted with the dashed circle.

factor would need to be included, making the technique considerablymore complex.

• Most importantly, whilst lines may lie at any angle in the graph space,junctions are still truncated and impossible for the quad to follow;the issue demonstrated in Figure 11 is still present. When directionchanges are minimal this is not an issue but in a complex workspacewith many obstacles, the path would likely be untrackable.

4.1.3 Conclusion

It was apparent that A* path smoothing is not suitable for this project orautonomous vehicles as a whole. A simple and quick method, it does find thetrue, least distance path to the goal but without consideration for steeringlimits or vehicle size. The pursuit was worthwhile however as the functionscreated allowed implementation of the Theta* technique, explained in thefollowing section.

4.2 Theta*

4.2.1 Method

Application of the Theta* method [Section 2.2.4] is by means of the lineofsightfunction during the expansion process. Rather than compare f(n) of the ad-


jacent nodes only, the line of sight between the parent of n and n′ is checked.If clear, the path cost is found and if less than the current, n′ is updated inOPEN with its parent nodes defined as the parents of n - the grandparentsof n′. If the line of sight is clear on the next expansion, the parents willbe the grand-grandparents of n′ and so the process continues, creating an‘any-angle’ line between nodes.

The advantage of this over the post-smoothing technique is that becausethis process occurs during the expansion routine, the cost function of the‘shortcut’ node is compared with that of the standard A* expansion duringthe searching of OPEN . Whilst g(n) will generally be lower than othernodes, with traversability and steering included, the ‘brushing’ of obstaclesand frequent turning can be avoided with appropriate cost weighting.


10 20 30 40 50 60 70 80 90 100

10

20

30

40

50

60

70

80

90

100

C−Space

Horizontal x (m)

Ve

rtic

al y (

m)

Node Edge

Node Edge

Low Trav Cost

Med Trav Cost

High Trav Cost

Obstacle

Initial

Goal

Theta*

A*

A* PS

Figure 19: Comparison of A* (green), A* PS (magenta) and Theta* (blue). A*PS is clearly the shortest path but brushes the edges of two obstacles,whilst Theta* still produces an any-angle path but taking into accountt(n). Although all three paths had the same start bearing, Theta*continues N, due to the removal of heuristic weighting.

Theta* successfully incorporates any-angle pathing, whilst still account-ing for t(n), allowing it to avoid the obstacle fields and removing the ‘brush-ing’ which plagued A* PS, shown in Figure 19. As expected and experiencedin [10], the paths created are not as short as A* PS. The paths should beshorter than basic A*, this is not always the case however. Average pathlength for Theta* was 129m, compared to 121m for A* [Table 5].

In testing it was found that the heuristic weighting prevented Theta*working, requiring it be set to 1, preventing direct comparisons. Figure 19


highlights the difference in path choice which may have occurred because ofthis and would effect the total distance. Pathing time is greatly increased(129s: 1000x greater than A*) but again, this cannot be directly compared.In a couple of instances, the Theta* path was slightly shorter than A* andit is clear that it would be the case (any-angle paths, without constraint tothe grid can take paths A* cannot) if the heuristic problem could be solved.

4.2.3 Conclusion

Using the Theta* method we were able to avoid a couple of the issues whichplagued path-smoothing: cost function ignorance, leading to passing veryclose to obstacle edges. However, the issue of steering limits and gradualturning still exist, along with the heuristic bug. Whilst the number of turnscan be restricted using cost weightings, the junctions are still instant andoften beyond that of the quad steering range. What was required was theability to vary the path angle on a node by node basis, such that the turningangle is only dependant on the nodal resolution and can be tailored to thepath tracking sensitivity. Following this conclusion, FieldD* interpolationwas developed.

4.3 FieldD* Interpolation

4.3.1 Method

It was decided that the best way to adapt the current code to include inter-polation between nodes (as in FieldD* [Section 2.2.3] was by changing thesteps between the adjacent nodes routine to non-integer values. By doingthis, points off the nodal grid can be considered.

The step value used in the horizontal and vertical scan within adjacentis divided by a factor delta, such that the function can remain universal withthe other methods (by setting delta to be 1). By varying the value of deltathe discrete value of interpolation between the nodal points is adjusted andthus, the number of adjacent points. The number of adjacent points p isdefined in (7), for example, a delta of 5 will create 40 adjacent points forthe expansion routine.

p = 8.delta (7)

With multiple possible points now possible along the adjacent grid bound-ary, the possible angle changes are also increased from the previous 45◦ limit.Similarly to the number of points, the new angle step is related to delta andthe angle between the eight adjacent nodes (8). Using this angle, we arenow able to limit the outputted adjacent nodes to those which are reachableby the quad’s steering range.

θ = 45/delta (8)


In order to make use of angle, it must be considered as a bearing relativeto the graph space, rather than local to n. To do this, the function anglewas created.

Function - angle

Figure 20: Figure to show the method of discretised interpolation and bearingreference. Red dot is n, blue n′ and gray interpolation steps - denotedby delta value (4 in this case). Red line is current heading, the range ofthe expandable nodes is highlighted when using a steering limit (22.5◦).Restriction of turning is demonstrated by showing the following rangeof the maximum step n′.

The angle between the coordinates of n (x,y) and n′ (x′,y′) will only everbe between 0-45, making it impossible to determine the true severity of aturn; remaining on the same heading could also be a full 180◦ u-turn forexample. In order to remedy this, a nautical bearing system was employed[Figure 20] with North at 0◦, increasing clockwise. Function angle providesthis conversion using a number of if statements to interpret the location ofthe scan routine (using the j and k step values) and so the orientation ofthe angle. Once known, the actual angle is calculated using distance to findthe hypotenuse, as a function of sin with the step length. The angle is thenadjusted according to its segment location.

The bearing of the point being expanded theta allows a vetting processwhich ignores points beyond the quads steering range steering. Included


with the CLOSED and edge check, if the absolute angle between the cur-rent heading and theta is greater than steering, the row insertion flag isremoved. Given the considerably greater number of points being expanded,this process is also vital to the speed of the algorithm. heading needs to berecalled, this is done by including the bearing of each point in exp arrayand also OPEN so that it can be called in a similar manor to cost functions.

Cost functions must be found for the points which do not lie withincspace. The distance functions work as before but traversability, as a pre-defined value, must be estimated as a function of the closest two nodes.This estimation is achieved using (2) but with t(n) in place of g(n) and thecurrent step value representing y.


Figure 21: An example path created using FieldD* with steering limit 20◦ anddelta 4. (a) The steering limit in full effect, creating a gentle arcwithin the turning radius of the quad rather than a junction. (b) Theshortcut used to add the goal node to adjacent can create an untidyfinish.

During testing, FieldD* was often slow at reaching the goal. Inspectionfound it to be ‘hovering’ in the goal locale but not quite reaching it. Theproblem was due to now being off the defined node grid, G was no longerappearing in the expansion of adjacent nodes. In order to remedy this, an ifstatement was included in adjacent, which upon detecting the goal withinone node step, would add it to exp array regardless. The only issue withthis solution is that reaching the final goal point in this manor will almostalways be beyond the steering limit - (b) Figure 21 and Figure 23. Since it


Figure 22: An example of the on-line routine using FieldD*, off-line path is inblue, on-line in pink. (a) The goal shortcutting in adjacent createsmore of an issue in local re-planning.

is within one node space, it was considered an acceptable error. Includingthis, pathing time is around 90% greater than A* with an average 1.8s butstill acceptable [Table 5]. Path distance is 2% longer than A*, as one wouldexpect due to the imposed steering limit - the path created is that of leastcost which the quad could follow.

Including FieldD* in the on-line routine works as intended, see Figure22. Re-planning time is shown to be 62% longer than A* but at 50ms[Table 4], it is still insignificant at the specified speed of 5mph (worst case0.18mm on last heading, 0.02% of a 1m node). The short-cut to goal routinementioned above does pose more of an issue during local re-plan. Figure22 (a) shows a very tight junction (almost 180◦) at re-entry. Generally, thetrajectory tracking system is able to compensate for this and it does not posea problem, however, a better method of finding G or smoothing techniqueshould to be found.

Steering limited to the quad’s range works well [Figure 21 (a)]. Using thesame set-up as Figure 11 the problem of sharp cornering is best demonstrated[Figure 23]. Although a path is created, it is only due to the goal short-cutand in truth, the configuration is not reachable by the quad; the turningradius is too shallow.

Analysis of the discrete interpolation factor delta produced a sawtoothplot [Figure 24]. The pattern is produced as the angle step resonates withthe steering limit. Figure 20 shows that the value of delta has a minimumvalue, dependant on the steering range. Values either side of this range will


5 10 15 20 25

5

10

15

20

25

C−Space

Horizontal x (m)

Ve

rtic

al y (

m)

Node Edge

Node Edge

Low Trav Cost

Med Trav Cost

High Trav Cost

Obstacle

Initial

Goal

Figure 23: With the steering limit imposed, the problem of Figure 11 is solved.Judging from the arc of the path, the goal is not actually reachableby the quad’s turning radius however, and is only achieved because ofthe goal short-cut.

create angle ranges which will, at times, limit the node expansion process.Thus, the delta value must be tailored to the steering range of the vehicleused. Using Figure 24, the correct value will lie between 3-15, beyond this,the pathing distance does not decrease but time does. In the case of thequad, with a steering range 22◦, a value 6 is adequate; providing at least 3points either side of the current heading.

4.3.3 Conclusion

The implementation of FieldD* interpolation provided the means to a steer-ing limit, which has allowed for the creation of paths our quad is able tofollow. Whilst the path is still a discrete approximation of the path the quadwill actually trace, a delta value of four provides enough of a resolution forthe trajectory tracking system. As one would expect, interpolation has comeat the cost of pathing time but the difference - although significant - is stillacceptable for both off-line and more importantly, on-line path planning.

An unforeseen result of the development of FieldD* also allowed us togreatly improve the overall speed of the planning algorithm by inclusion ofthe h(n) weighting factor.


0 5 10 15 20 25 30 35 40 45 50119.2

119.25

119.3Delta Step Analysis

Pa

th D

ista

nce

(m

)

0 5 10 15 20 25 30 35 40 45 500

50

100

Tim

e t

o P

lot

(s)

Weighting Factor

Figure 24: Delta step factor analysis. A steering limit of 45 (standard A*) is usedto enable starting from 1. A sawtoothed pathing distance is createdas the angle step resonates with the steering limit - highlighting theimportance of the delta selection with regards to the steering limit.Pathing time is seen to increase exponentially, as more points are hav-ing to be searched.

4.4 Steering Weighting

With the planner tailored to the quad and now achieving the specification,development turned to optimisation of the generated path. Although thespeed is specified constant at 5pmh, this is somewhat slow and limiting. Thetrajectory tracker is able to vary the speed depending on error - causing thequad to slow down at corners and reach maximum velocity during straightsections.

The planner is currently designed to find the shortest path - the fastestwhen driving at constant speed. If the speed is allowed to vary as per thetrajectory tracking system, the shortest path may not be the fastest pathto the goal, since turning comes at a cost to speed. It was therefore decidedthat a steering based cost would enable tailoring of a path to best matchthat of the tracking system velocity profile.

4.4.1 Method

Implementation of a steering weighting was as simple as including a costbased on the change in angle required to reach the nodal point. The valuetheta is divided by a weighing factor k to form this new cost, performed bythe function steering cost. Rather than generate a new cost function andso change the shape of many arrays within the planner, the steering cost is


included as part of the path function g(n) (9).

g(n′) = g(n) +√

(x′ − x)2 + (y′ − y)2︸︷︷︸distance

+ theta/k︸︷︷︸steering

(9)


0 5 10 15 20 25 30 35 40 45 50115

120

125

130

135

140

145

150

X: 3.5Y: 122.3

Steering Weighting Factor Analysis

Pa

th D

ista

nce

(m

)

0 5 10 15 20 25 30 35 40 45 500

2

4

6

8

10

12

14

Tim

e t

o P

lot

(s)

Weighting Factor

Figure 25: Steering cost weighting analysis (division factor, increasing value re-duces effect). The active range is found to be 2-11. The value cannotbe selected from this graph on a simple least distance basis (2), thedrive time must also be considered [Figure 26].

Analysis of the steering weight factor lead to an effective range [Figure25], it is worth noting however that this range is dependant on the arbitrarytraversability range used and also the heuristic weighting. A balance must bemade between these two weighting values in order to avoid a naive planner.It is shown to have little effect on the overall pathing time, unlike heuristicweighting, as it is not aiding the searching routine.

Figure 26 demonstrates the importance of the value in this range, thegreat effect on the length of the final path and the balance which mustbe made with the heuristic function. We can explain the rough sine waveof Figure 25 which forms this range: path length reduces as traversabilitywobble is reduced then increases again as the cost of steering begins todetrement path searching - straight lines are preferred over progress to thegoal, only turning at the last opportunity [Figure 26 (a)]. As the h(n) beginsto factor again, the path develops as intended, with reduced traversabilitywobble (b). It then decays away to a constant value as the factor increasesand steering cost becomes negligible.


Figure 26: Comparison of steering cost weight factor: blue line with steering cost,green without. (a) a steering cost of 1 outweighs heuristic, creatinga naive planner which prefers straight lies over progress to the goal.(b) comparison of initial bearing: starting 45◦ away from G with aweight 2 creates a vastly different path to starting on the same bearing(magenta). (c) A good balance with steering 2 and h(n) 20, drivetime is reduced (17s) compared to the path without a steering penalty(21.5s). (d) The same weighting values as (c) creates a vastly differentresult and causes the opposite of intended, when a certain obstacleconfiguration appears.

The good balance is struck using a value 2, with heuristic 20 as foundearlier [Figure 26 (c)]. The path without a steering cost (green) turns to-wards the goal, without knowledge of the obstacle obstructing that route,whilst the blue weighted path, maintains a straight line. Drive times were21.5s and 17s respectively - proving the benefit of the new cost in this case.The result is in part due to the obstacle configuration however, and in differ-ent configurations, the results can be vastly opposite (d) and in fact, createmore steering. As such, if the steering cost is to be used, investigation dur-ing off-line planning of the optimum value (within the active range) for thecurrent configuration should be performed.

From a number of simulated scenarios, a value 15 - marginal affect onf(n) - appears optimal for most configurations [Table 6]. This value confirmsthe initial reasoning for the steering penalty, that the least distance path(value 2 [Figure 25]) is not necessarily the fastest.


The initial bearing in relation to the goal bearing also has great effecton the benefit of the steering cost. As stated, when the weighting is high,steering towards the goal becomes less important than straight driving. Ifthe initial bearing is the same as the goal, the drive time can be reducedby using the steering reduction. If however, the start bearing is 45◦ awayfrom the goal for example, the same weighting factor can create considerablylonger drive times. Figure 26 (b) best highlights this. With a start bearingof 0◦, the steering cost causes a much longer drive time then without butstarting at 45◦, a straight path to G is found (although driving close to anobstacle, another consideration that must be made). Again, this is a generalexample and not always the case, the best case of (c) is with an initial bearing45◦ away from G. It can be concluded however that if steering weighting isto be used, the quad should be orientated on the same bearing as the goalpoint.

4.4.3 Conclusion

From the results shown in Figure 26, the steering penalty clearly needssome additional work to be a flawless addition to the planner. Consideringthis, a marginal inclusion in f(n) by using a weight 15, appears to achievequicker drive times in most instances [Table 6]. The discussion brings upthe question: is the cost simply counter-balancing the others, at a cost toheuristic and traversability? The apparent importance of the workspaceconfiguration certainly suggests this and perhaps a better method of drivetime optimisation needs considering.

An improved implementation would be to develop a number of pathiterations across the active weight factor range and compare drive times(with the trajectory tracker) during off-line planning - since time is not anissue here. The least time to drive path would be used and steering costignored during on-line re-planning.

5 INTEGRATION WITH OVERALL PROJECT 43

5 Integration with Overall Project

Having completed the planner to the specified requirement, developmentturned to integration with the systems to which it directly integrates: GPSMapping, Camera Based Obstacle Detection and Trajectory Tracking.

Unfortunately, only the trajectory tracking system was at a stage whereintegration and testing could be performed. Data transfer arrays were de-signed in collaboration with GPS mapping however and as such, much ofthe two systems can be considered unified. Similarly, arrays and obstacleinsertion for camera detection were understood from an early stage andwhilst it would have been good to demonstrate re-planning on detection ofa real-world obstacle, the on-line simulation provides sufficient testing.

5.1 Steering, Control and Trajectory Tracking

5.1.1 Method

Thanks to the initial discussion of array structuring before design of eithersystem, the tracking of path was a relatively simple affair. The method usedto track the path is explained in [19] but the inclusion and code additionsof this system are explained below.

As explained in Section 3.1.4, path is inverted due to the backtrack-ing technique. The trajectory tracker reads from row 1 up so an inversionprocess is performed on path in the initial lines. Secondly, a polar bearingsystem is used here rather than the nautical system of the planner, requiringanother conversion process.

During the tracking routine, the on-line planner needs to run, this isachieved in a similar manner to driving [Section 3.2.1]. The obs flag vari-able is used as an interrupt during each increment of path. If true (anobstacle detected by the camera system), the update path function is run.The inversion process performed on path on loading is required once againafter each run of update path - the only additional lines from that of driving.

The trajectory tracker includes a time step run, which is used to outputthe drive time at the end of the tracking process and is the value used inthe steering weighting comparisons [Section 4.4]



Figure 27: Demonstration of the trajectory tracking system working with the on-line path planner. (a) The on-line path planner output: an obstacleappeared 2/5 along the path, which is adjusted accordingly. (b) Thepath being tracked is also seen to update and the (simulated) quadreaches the goal successfully.

As mentioned in the methodology, combining the two systems proved tobe relatively simple, other than a few conversions. These conversions couldbe removed by standardising one of the systems but both variables are deeprooted in the operation of each and as such, it would not be an easy task. Asit stands, the few lines of code do not increase the operation time as they areperformed before tracking begins and were found not to affect re-planning(50ms as before [Table 4]).

Figure 27 provides a good example of the tracking system working well


with the on-line planner. The tracker works using a feedback loop, creatingan arc which then controls the steering rate and velocity (the green circlein (b)). During straight lines, the arc is off the scale, a turn creates error,which reduces the radius of the arc. Thus, the arc provides a smoothingeffect on the path generated by the planner; a delta interpolation 4 providesenough resolution for good tracking.

The truncation created by the goal short-cut [Section 4.3.2] does notprove to be an issue during tracking. Trajectory planning works a few pointsahead of the current position and as such, includes an element of awareness.As the goal short-cut is only 1 node on the path, the tracker is able tosmooth over it and continue a course, drivable by the quad and towards thegoal. Although this is not directly demonstrated in Figure 27, the fact thatthe quad has reached the goal and remains on track, proves that the sharpjunction created did not hinder progress.

5.1.3 Conclusion

The trajectory tracker successfully followed the path generated by the plan-ner and on-line updating was also proved to be working. Although driving isonly simulated, the functions required of the path planner and its connectionwith the system can be considered complete.

6 DISCUSSION 46

6 Discussion

Comparison of the A* optimisations [Figure 28], allows us to see that FieldD*is around 10x longer to generate the off-line path. Although this sounds sig-nificant it is still well withing the desired time [(5s) Table 2] and so not anissue. As discussed in Section 4.2, the greatly inflated Theta* time cannotbe directly compared.

A* A*PS T* FD*0

20

40

60

80

100

120

140

Distance (m)

P Time (s)

115.28

121.82123.99 124.28

129.06

1.180.1380.134

Figure 28: Run time and distance comparison of various pathing techniques used.Path smoothing has negligible effect on time but creates the true short-est path. Theta* time can be considered an anomaly due to lack ofheuristic. Although FeildD* is around 10x longer to path, it is stillwithin the required specification.

The marginal distance increased seen when using FieldD* can be putdown to the steering limit and inability to turn in one node expansion.As found in [10], line of sight pathing is 8% shorter than FieldD* but isa path that cannot be reliably tracked. If the method could be combinedwith FieldD* to create smooth, limited corners, a optimum could be cre-ated, unbound by the discretised grid. As it stands however, the FieldD*based planner created meets the required specification for our project [Table2], confirming that of others ( [12], [8], [13]), that the interpolation basedtechnique is the method of choice for off-road path planning.

Our method of on-line re-planning on recipt of new obstacle data hasalso proved to be sucuessfull in simulated results. It is worth pointing outthat the obstacles are considered as stationary. In the given enviroment (afarmers field [Table 2]) it is likely there will be moving animals as un-mappedobstacles. Whilst active obstacles would still be detected by the camera andaccounted for by the on-line planner, we can presume it would not copewell. We do not have test data for this senario but the camera would likely

6 DISCUSSION 47

update with another obstacle instance each time the obstacle moves, creatingan obstacle which would grow, possibly creating a wall. Depending on there-planning course and the direction in which the obstacle moves, a pathmay still be created around this obstacle. However, the constant updatingas the obstacle moves may cause the planner to loop too many times (duringwhich tracking is stopped) and possibly break. A solution is discussed inSection 8.

A video demonstrating the final planner, working with the trajectorytracking system, in both off-line and on-line modes can be found on theUSB stick accompanying this report (PathP lanner.wmv).

With only simulated results at this stage, the robustness of the plannercannot be guaranteed. A number of inaccuracies found during developmentleave a certain element of uncertainty as to the planner’s operation onceimplemented:

• The ‘jump to goal node’ work-around often creates angles considerablybeyond the steering range [Section 4.3.2] . Although the trajectorytracker appears to compensate for this we cannot be certain until thepath is driven.

• With only simulated terrains used, we do not know how the pathgenerator will react to real obstacles, which may be closely packed orof greatly varing sizes.

• Drive times used in steering weighting comparison [Section 4.4] aresimulated and actual drive times will invariably differ, which couldaffect the choice of weighting.

• It is not fully understood how the system will react during the on-linere-planning, when the tracjectory tracker is taken off-line. Althoughit is assumed the DAC will hold the last sample - leaving the quad onits current course - the actual results of this could create difficulties.

These issues require solving through the real-world testing discussed inSection 8.

7 CONCLUSION 48

Figure 29: Myself (left) and two other member of the project posing with thequad in April 2012. The computer case at the rear houses the hard-ware control circuits and will also house the PicoITX board for centralprocessor (not included at this stage).

7 Conclusion

From the implementation and adaption of an A* based path planner inMATLAB, we have successfully completed the path generation requirementfor the project. The planner adds to A*, with Field D* interpolation betweennodal points, providing a discretised ‘any-angle’ path. Using the techniquewe were able to limit the path to turns within the quad’s steering range of22.5◦, meeting a key point of the design specification [Table 2].

The on-line portion of the planner was proved to be working to therequirement using both a basic simulator and after integration with thetrajectory tracking system. Average time to re-plan 50ms is well within the0.5s specification and correlates to a 0.02% tracking error - 0.18mm of a 1mnode. The tracking error could be removed by parallel running of scriptsrather than the current line by line.

Variables for the running of the path generator were developed usingsimulations (the project was not at a real-world testing stage). Heuristicand steering weight factors 20 and 15 respectively should be used with aninterpolation step factor 6 (7.5◦ angle between points, providing at least 3points either side of current bearing). A node size 1 provides a good balancebetween trackability and pathing time in the defined 0.1km2 workspace.

Figure 29 shows the project at its current stage. Whilst the path planner

8 FUTURE WORK 49

can be considered complete to the intended and required level, there is plentythat can be done to further develop the sub-system and project as a whole;discussed in the following section.

8 Future Work

8.1 Real-World Testing

With the project systems yet to be fully integrated, real-world testing couldnot be completed during the development of the planner. Testing needs tobe conducted with the following objectives:

• Confirm the quad is able to track the path and that tracking errordue to re-planning is negelible. Simulated trajectory tracking provedsuccessful but will the actuators/control limits respond as modelled? Ifnessessary, refine the generated path by adjusting delta, nsize, weightfactors, etc. accordingly.

• Check the GPS mapping data transfers to cspace accurately. Doesthe quad drive around obstacles and with adiquate room? Configurethe traversability range used, by driving in and collecting data of theenvironment, whilst considering what preferences we want the quad tohave.

• Conduct testing with the obstacle detection system. Is new obsta-cle data recieved with adiquate time for re-planning; adjust obstaclebuffers as requried. Ensure re-planning works as intended: a prob-lem could occour when encountering large obstacles which the cameradetection cannot guage the full size in current FOV.

8.2 Improve Velocity Optimisation

A steering weight was applied in an attempt to find the fastest path ratherthan the shortest [Section 4.4].The effect was dependant on the obstacleconfiguration and could only be realised after running the driving simulationto find the drive time. One solution is to run the off-line planner across arange of steering factors and use the fastest path, this is lengthly and sub-optimal.

A better method would be to include the velocity profile of the quadso that it can be considered in the searching routine. Such a profile wouldinclude the relationship between steering/velocity for certain t(n) values[Figure 30], and be included in the cost function of a node. The searchwould then be able to make a literal consideration for the fastest path to thegoal, rather than the disconnected method of a steering penalty. Secondly,it would allow the path planner to calculate the overall drive time of the

8 FUTURE WORK 50

Figure 30: An example figure of what would be required from a velocity profile,to be included when calculating the f(n) of a node. The green linecould represent gravel, red grass and blue tarmac.

path (using distancespeed ), rather than relying on the driving simulation; allowing

instant comparison.To create this, a veloity profile is required, ideally from real-world testing

of the quad in the intended environment. A simulated profile could beextracted in coordination with the ‘Dynamics and Model of the Quad’ sub-system [20] however.

8.3 Parallel Computing

Currently, all code runs in series during the on-line routine, with an interuptat each step of the tracking script for obstacle detection and re-planning.The process stops the tracking of the path, leaving the quad to maintain thelast heading and the potential to vear off course. Although the worst casepause was found to be neglibile and within tracking error [Section 4.3.2], amore lengthy pause would create a problem.

In order to remedy this, a method of parallel processing should be im-plemented. MATLAB recently released a ‘Parallel Computer Toolbox’ [21],which appears to enable what is required by this project. for loops can beexcuted in parallel using parfor. The for loop of the trajectory trackerwould be run in parallel with another for loop to run until the end oftracking and which runs the on-line routine.

Failsafe methods could also be included. For example, if re-planning had

8 FUTURE WORK 51

not completed after three steps of the path (within the five node obstaclebuffer), driving should be stopped in order to allow the process to completeand avoid collision with the new obstacle - currently, if the pathing functiondoes not complete, the quad would continue on the previous course.

8.4 Moving Obstacle Re-Planning

A solution to the likely occourance of a moving obstacle, explained in Section6 would be to include an active flag in the new obs array. If an obstacle isdetected to be moving (by a second consecutive occourance in the samenodal coordinates), the flag could be raised by the camera detection system,and then, rather than insert the obstacle as a new row into obs, the previousobstacle would be overwritten; preventing an erronous expanding obstacleand allowing the path to go through the previous position of the obstacle.

Testing would need to be conducted to ensure that the multiple runningof the pathing algorithm in quick sucsession does not hinder the drivingprocess, but once the obstacle is out of sight, the path should not requiretoo many iterations; unless we are dealing with a particulary indecisiveanimal!

8.5 Off-Course Re-Planning

Dispite tracking appearing to be fluid and maxmimum tracking error 0.02%during re-plan [Section 4.3.2], if the quad were to veer far from the plannedpath, there must be a contingency. A consideration of the trajectory trackingsystem [19], a function must be called from the path planner. A solutionwould probably be as simple as running the search algorithm pathing, withinitial as the quad’s current position and goal as the original goal, requiringno additional functions from the planner. Alternativly, if the orginal pathwere to be salvaged, the nearest point in path could be found and this usedas the new goal.

8.6 Improved Heuristic Cost

The heuristic cost is currently calculated as the direct (ignoring obstacles)distance to the goal from n′. Viewing the search and expansion process usingbreakpoints, if an obstacle lies on the least cost path, the search will hit theedge of the obstacle, move back to n, then hit the obstacle node adjacent ton′; traversing around an obstacle parameter by repeated dead-ends is slow.

A heuristic cost which takes into account obstacles would prevent thisslow process, as the least cost path around an obstacle would be immediateto the planner. The method of defining this cost is complex and so wasignored during development as pathing time was not an issue. The obviousanswer is to use the pathing function but this would be contradictory to

8 FUTURE WORK 52

any time saving. One method which could be investigated would be a costassociated to obstacle nodes in the vacinity of n′.

8.7 Adaptive Node Grid

Originally on the roadmap for the planner, a grid which has varying nodedensity depending on the locale of obstacles would improve the pathing time.Although dropped as the time did need reducing, if the workspace were toincrease in size or complexity, this would be a attractive feature to have.Essentially, the grid would have increased node density around obstaclesand high area of traversability, which would decrease to a set size in openspace.

REFERENCES 53

References

[1] J. Markoff, “Google cars drive themselves, in traffic.” http://www.

nytimes.com/2010/10/10/science/10google.html, 2010. WebsiteAccessed: 24/02/2012.

[2] C. Squatrigila, “Gm says driverless cars could be on the road by 2018.”http://www.wired.com/autopia/2008/01/gm-says-driverl/, 2008.Website Accessed: 24/02/2012.

[3] RoadSafetyGB.org.uk, “Driverless trucks by 2019.” http://www.

roadsafetygb.org.uk/news/541.html, 2009. Website Accessed:24/02/2012.

[4] H. D. Sebastian Thrun, Michael Montemerlo, “Stanley: The robot thatwon the darpa grand challenge,” Journal of Field Robotics, vol. 23,no. 9, 2006.

[5] D. S. Roland Seigwart, Illah R. Nourbakhsh, Introduction to Au-tonomous Mobile Robots. The MIT Press, 2nd ed., 2011.

[6] J. Whittington, “Autonomous off-road vehicle path planning: Litera-ture review.” 2011.

[7] P. Hart, N. Nilsson, and B. Raphael, “A formal basis for the heuristicdetermination of minimum cost paths,” Systems Science and Cybernet-ics, IEEE Transactions on, vol. 4, pp. 100 –107, july 1968.

[8] R. Linker and T. Blass, “Optimal path planning for car-like off-roadvehicles,” in Robotics, Automation and Mechatronics, 2008 IEEE Con-ference on, pp. 150 –154, sept. 2008.

[9] D. Ferguson and A. T. Stentz, “Using interpolation to improve pathplanning: The field d* algorithm,” Journal of Field Robotics, vol. 23,pp. 79–101, February 2006.

[10] K. S. Nash A., Daniel K. and F. A., “Theta*: Any-angle path planningon grids,” Journal of Artificial Intelligence Research, vol. 39, pp. 533–579, 2010.

[11] C. Thorpe, “Path relaxation: Path planning for a mobile robot,” Tech.Rep. CMU-RI-TR-84-05, Robotics Institute, Pittsburgh, PA, April1984.

[12] J. Carsten, A. Rankin, D. Ferguson, and A. Stentz, “Global path plan-ning on board the mars exploration rovers,” in Aerospace Conference,2007 IEEE, pp. 1 –11, march 2007.

http://www.nytimes.com/2010/10/10/science/10google.html

http://www.nytimes.com/2010/10/10/science/10google.html

http://www.wired.com/autopia/2008/01/gm-says-driverl/

http://www.roadsafetygb.org.uk/news/541.html

http://www.roadsafetygb.org.uk/news/541.html

REFERENCES 54

[13] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Practical searchtechniques in path planning for autonomous driving,” in Proceedings ofthe First International Symposium on Search Techniques in ArtificialIntelligence and Robotics (STAIR-08), (Chicago, USA), AAAI, June2008.

[14] C. Warren, “Fast path planning using modified a* method,” in Roboticsand Automation, 1993. Proceedings., 1993 IEEE International Confer-ence on, pp. 662 –667 vol.2, may 1993.

[15] A. Stentz, “Optimal and efficient path planning for partially-knownenvironments,” in In Proceedings of the IEEE International Conferenceon Robotics and Automation, pp. 3310–3317, 1994.

[16] P. Premakumar, “A* search for path planning tu-torial.” mathworks.com/matlabcentral/fileexchange/

26248-a-a-star-search-for-path-planning-tutorial, 2010.Website Accessed: 24/02/2012.

[17] S. Armstrong, “Gps and computer vision based terrain informationgathering.” 2012.

[18] T. Charton, “Vehicle hardware and control of an off-road autonomousvehicle.” 2012.

[19] O. Gibler, “Control, steering and trajectory tracking of autonomousvehicle.” 2012.

[20] T. Reeves, “Dynamics and model of the quad.” 2012.

[21] MathWorks, “Parallel computing toolbox.” mathworks.co.uk/

products/parallel-computing/description1.html, 2012. WebsiteAccessed: 19/04/2012.

mathworks.com/matlabcentral/fileexchange/26248-a-a-star-search-for-path-planning-tutorial

mathworks.com/matlabcentral/fileexchange/26248-a-a-star-search-for-path-planning-tutorial

mathworks.co.uk/products/parallel-computing/description1.html

mathworks.co.uk/products/parallel-computing/description1.html

A QUAD AND DESIGN SPECIFICATION 55

A Quad and Design Specification

Table 1: Minature Quad Design Specification

Overall

Dimensions * 124 x 74 x 64 cmWeight * 82 kg

Load weight * 150 kgSpeed * 3 to 49 mph

Turning Radius 2.1mSteering Angle 22

Wheels and TyresTyre size 140/70 - 6

Measured diameter 350mmRim size 6 x 4.5”

SprocketsFront teeth 14Rear teeth 37

Front diameter -Rear diameter -

Pitch -

EngineType* Single cylinder 4 strokeBore* 41mm

Stroke* 37.4mmSize* 110cc

Rated power* 2.2kW @ 7500rpm100

AQUAD

AND

DESIG

NSPECIF

ICATIO

N56

Table 2: Design specification of project, including pathing specific points.

Defined/Wish Description

D Drive at a constant speed of 5mphD As a UK farmer as the customer, work within a 100x100m fieldD Remote kill-switch (default on)W Remote control of driving/ability to see and change pathD Track the generated path with a 5% deviation threshold

Pathing SpecificD Limit maximum path arc angle to that of the quad (22deg)D Given an initial point and goal, construct the shortest safe path joining the two (off-line)W Complete off-line planning within 5sD Adjust path according to obstacles detected by camera detection system, whilst driving path (on-line)W Complete on-line re-planning within 0.5sD Run on minimal hardware - Pico-ITX/laptopD Define a method of obstacle insertion, universal to GPS mapping and obstacle detectionW Include terrain information from GPS system when calculating pathW Ability to tailor workspace as required: size, resolution, accuracy, path considerations

B SUPPORTING PLANNER DATA 57

B Supporting Planner Data

Table 3: Comparison of run times with and without the heurestic weighting factor.A factor greater than 1 is enough to provide the desired effect, althoughhigh values can tailor the path generation.

Factor A* Change FieldD* Change

1 0.29845 1.6338542 0.126202 -136.49% 0.714401 -128.70%3 0.123016 -2.59% 0.630922 -13.23%4 0.121618 -1.15% 0.642706 1.83%5 0.124757 2.52% 0.634468 -1.30%6 0.127198 1.92% 0.600873 -5.59%7 0.119968 -6.03% 0.610213 1.53%8 0.121211 1.02% 0.594534 -2.64%9 0.121928 0.59% 0.594521 0.00%

10 0.118947 -2.51% 0.592863 -0.28%

Table 4: Times to re-plot during the online routine, based on random obstaclegeneration. The difference between A* and FieldD* is noticable but bothprovide satisfactory times for real-world driving: At 5mph, the maximumthe quad could veer off-course using worst case FieldD* is 0.18mm.

A* FieldD* Difference

0.0221 0.0820.0123 0.04450.0193 0.04830.0253 0.04320.0156 0.0312

0.01892 0.04984 62.04%

B SUPPORTING PLANNER DATA 58

Table 5: Using five workspace configurations, the four pathing methods are com-pared against A*. 100x100m workspace, 1m node size, 0◦ start bearingand 20◦ (for FieldD*) were used. The slow pathing time for Theta* isdue to the lack of heurestic weighting and not directly compariable.

A* T (s) A* D (m) A*PS T (s) A*PS D (m) T* T (s) T* D (m) FD* T (s) FD D (m)

0.115541 118.4092 0.129025 114.6699 76.8439529 118.2906 1.4563312 126.77930.172821 127.9239 0.173394 117.248 74.6075361 129.5167 4.3078342 126.55650.147145 125.4802 0.150884 115.5778 175.052689 130.3886 0.4486339 120.99340.118174 118.6518 0.119559 113.455 184.797534 124.6353 0.6158088 120.77930.11858 118.6518 0.119473 115.4968 134.027061 117.1494 2.1817809 126.302

0.134452 121.8234 0.138467 115.2895 129.065754 123.9961 1.8020778 124.2821Compared to A* 2.90% -5.67% 99.90% 1.75% 92.54% 1.98%

Table 6: Steering factor weighting analysis. Given the active range of 2-15, 1000(1st row) can be considered null. A marginal steering penalty by usinga factor 15 has the effect of improving the drive time of the path.

Steering Factor Drive Time 1 (s) ... 2 (s) ... 3 (s) ... 4 (s) ... Av (s)

1000 15 16 21.5 18.5 17.7515 14.7 16.8 15.2 15.9 15.6510 18 15.5 16 16.2 16.4255 15.6 17.9 16.5 18.9 17.2251 20.3 15.7 17 17 17.5

Table 7: nsize sensitivity analysis. The optimum value given the size of the quadand pathing time requirement is 1.

nsize (m) Path Time (s)

0.05 20.2440.1 4.1340.5 0.46791 0.21582 0.1135 0.0877

10 0.067115 0.061130 0.0612

C PATH PLANNER V1.0 CODE 59

C Path Planner V1.0 Code

Please note, only the 3 scripts and a selection of the most important func-tions are presented here. For the full program, please refer to the USB stickincluded with this report (within the folder ’Path Planner V1.0’)

C.1 Scripts

C.1.1 main.m

1 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−2 % A∗ based path generat i on f o r autonomous quad p ro j e c t at the3 % Unive r s i ty o f Bath4 %5 % Scr ip t to de f i n e v a r i a b l e s and generate path given ob s t a c l e data6 %7 % J . Whittington , 2012 − Path Planner f o r Autononous Off−Road Vehic l e8 %9 % Refernces :

10 % MathWorks Inc . , 2005 − I n t e r a t i v e A∗ Search Demo11 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−12 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−1314 %Housekeeping15 c l e a r a l l ; %c l e a r MATLAB workspace16 c l o s e a l l ; %c l e a r f i g u r e s1718 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗19 %∗∗∗∗ Set Up ∗∗∗∗20 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗2122 %−−−−−Def ine Graph Space−−−−−−−−2324 c x = 100 ; %C−Space s i z e in x (m25 c y = 100 ; %C−Space s i z e in y (m)26 n s i z e = 1 ; %I n i t i a l node s i z e (m)27 n x = f l o o r ( c x / n s i z e ) ; %Number o f nodes in x28 n y = f l o o r ( c y / n s i z e ) ; %Number o f nodes in y29 c fnc = 4 ; %No . co s t f unc t i on s :30 ctp = 1 ; %node type31 cgn = 2 ; %path cos t32 chn = 3 ; %h e u r i s t i c to goa l33 ctn = 4 ; %t r a v e r s a b i l i t y3435 %Obstalce data array :36 %[ x | y | diamention | r a t i o | shape (0 node , 1 square , 2 c i r c l e , 3 l i n e ) ]37 %r a t i o = x : y , in the case o f a l i n e i t i s used to denote r o t a t i on : 0 = 0deg38 %(x ) 1 = 90deg (y )39 randobs = 0 ;40 obs = ob s t a c l e s ( randobs , 10 , n x , n s i z e ) ;4142 %Trave r sab l i ty data 0 f o r loaded , 1 f o r random :43 randtrav = 1 ;4445 %−−−−−Quad Pos i t i on ing−−−−−−4647 i n i t i a l = [ 1 0 , 1 0 ] ; % [ x , y ] nodal cord48 goa l = [ 9 0 , 9 0 ] ; % [ x , y ] nodal cord49 bear = 0 ; %i n i t i a l bear ing o f quad (0 deg in +ve y )50 s t r a n g l e = 20 ; %s t e e r i n g angle l im i t ( deg )51 quadx = 1240e−3; % length o f quad (m)52 quady = 740e−3; % width o f quad (m)53 quadv = 0 ; % speed o f quad (m/ s )54 nquadx = c e i l ( ( quadx/ n s i z e ) ) ;55 nquady = c e i l ( ( quady/ n s i z e ) ) ;5657 %−−−−−Other Var iab les−−−−−−−−5859 %Graph space po s i t i o n d e f i n i t i o n s60 i n i t i a l c = 1 ;61 goa l c = 2 ;62 obsc = −1;6364 %Open l i s t column index ing65 %[ act ive , x , y , parent x , parent y , g (n) , h(n) , t (n) , f (n) ]66 oa = 1 ;67 ox = 2 ;


68 oy = 3 ;69 opx = 4 ;70 opy = 5 ;71 ogn = 6 ;72 ohn = 7 ;73 otn = 8 ;74 ofn = 9 ;7576 %−−−−−Searching type−−−−−−−−−7778 % type = ’ astar ’79 % type = ’ astarPS ’80 % type = ’ the tas ta r ’81 type = ’ f i e l dD ’8283 %Change in step o f i n t e r p o l a t i o n func t i on f o r f i e l d D∗84 de l t a = 4 ;8586 %−−−−−Plott ing−−−−−−−−−−−−−−−87 f u l l s c r n = 1 ; %f u l l s c r een togg l e f o r graph space p l o t t i n g8889 %−−−−−Graph Space Generation−−−−90 t i c %s t a r t pathing time c lock9192 %Create graph space from d i s c r e t i s e d c−space93 %3d matrix f o r co s t f unc t i on s o f each node94 cspace = ze ro s ( n x , n y , c fnc ) ;9596 %In s e r t i n i t i a l po int and goa l97 cspace ( i n i t i a l ( 1 , 1 ) , i n i t i a l ( 1 , 2 ) , ctp ) = i n i t i a l c ; %Quad has a value 198 cspace ( goa l (1 , 1 ) , goa l (1 , 2 ) , ctp ) = goa l c ;99

100 %In s e r t t r a v e r s a b i l i t y co s t t (n)101 cspace = t r a v e r s a b i l i t y i n s e r t ( cspace , n x , n y , ctn , goal , randtrav , 0 ) ;102103 %In s e r t the known ob s t a c l e s104 cspace = ob s t a c l e i n s e r t ( n x , n y , ns i ze , obsc , cspace , obs ) ;105106 %Stop now i f any ob s t a c l e s l i e on the goa l or i n i t i a l po in t s107 i f cspace ( goa l (1 , 1 ) , goa l (1 , 2 ) , ctp ) == −1108 e r r = e r r o r d l g ( ’The goa l l i e s with in an ob s t a c l e ! ’ ) ;109 e r r o r ( ’The goa l l i e s with in an ob s t a c l e ! ’ ) ;110 e l s e i f cspace ( i n i t i a l ( 1 , 1 ) , i n i t i a l ( 1 , 2 ) , ctp ) == −1111 e r r = e r r o r d l g ( ’The s t a r t l i e s with in an ob s t a c l e ! ’ ) ;112 e r r o r ( ’The s t a r t l i e s with in an ob s t a c l e ! ’ ) ;113 end114115 %Plot C−Space116 p l o t c spa c e ( c x , c y , n x , n y , ns i ze , cspace , ctp , cgn , chn , ctn , obsc , i n i t i a l c , goalc ,

f u l l s c r n ) ;117118 %Generate the i n i t i a l path ( o f f−l i n e )119 switch type120 case ’ a s ta r ’121 %Stee r ing angle must be at l e a s t 45122 i f s t r a n g l e <= 45123 s t r a n g l e = 46 ;124 end125 path = pathing ( ’ a s ta r ’ , cspace , n x , n y , ctp , ctn , obsc , i n i t i a l , goal , bear ,

s t r a n g l e ) ;126 case ’ astarPS ’127 i f s t r a n g l e <= 45128 s t r a n g l e = 46 ;129 end130 path = pathing ( ’ a s ta r ’ , cspace , n x , n y , ctp , ctn , obsc , i n i t i a l , goal , bear ,

s t r a n g l e ) ;131 path = path smoother ( path , cspace , ctp ) ; %apply l i n e o f s i gh t smoothing132 case ’ t h e t a s t a r ’133 i f s t r a n g l e <= 45134 s t r a n g l e = 46 ;135 end136 path = pathing ( ’ t h e t a s t a r ’ , cspace , n x , n y , ctp , ctn , obsc , i n i t i a l , goal , bear ,

s t r a n g l e ) ;137 case ’ f i e l dD ’138 path = pathing ( ’ a s ta r ’ , cspace , n x , n y , ctp , ctn , obsc , i n i t i a l , goal , bear ,

s t r ang l e , d e l t a ) ;139 end140141 t = toc ; %pathing time toc142143 %Update the p lo t with the path144 p lo t path ( path )145146 %Run the d r i v ing s imulator


147 % dr iv ing148149 %Run the t r a j e c t o r y t rack ing system150 dr ive path


C.1.2 driving.m

1 %Simulate d r i v ing along the path23 %Var iab l e s4 j = s i z e ( path , 1 ) ; %path decrement5 x = i n i t i a l ( 1 , 1 ) ; %x cord o f quad bike6 y = i n i t i a l ( 1 , 2 ) ; %y cord o f quad bike7 quad pos i t i on = [ x , y ] ; %po s i t i o n o f quad in csapce8 new obs = [ ] ; %new obs t a c l e array9 ob s f l a g = 0 ; %f l a g when new obs t a c l e appears

10 r o u l e t t e = 20 ; %chance o f a new obs t a c l e11 near path = 1 ; %generate ob s t a c l e s on path f o r t e s t i n g12 f rom current = 0 ; %generate l o c a l path from current po s i t i o n ?1314 %Plot the quad bike at the s t a r t po int15 p = p lo t (x , y , ’ bo ’ , ’ MarkerFaceColor ’ , ’ c ’ ) ;1617 %Move the dot along the path18 whi le ( x ˜= goa l (1 , 1 ) && y ˜= goa l (1 , 2 ) )19 x = path ( j , 1 ) ;20 y = path ( j , 2 ) ;21 pause ( quadv ) ;22 s e t (p , ’XData ’ , path ( j , 1 ) , ’YData ’ , path ( j , 2 ) ) ;23 drawnow ;2425 %∗∗ t e s t code∗∗26 %Create a fake ob j e c t a f t e r 20 s t ep s27 % i f j == ( s i z e ( path , 1 )−70)28 % new obs = [ path (10 ,1 ) , path (10 ,2 ) , 2 , 1 , 1 ] ;29 % cspace = ob s t a c l e i n s e r t ( n x , n y , ns i ze , obsc , cspace , new obs ) ;30 % p l o t c spa c e ( c x , c y , n x , n y , ns i ze , cspace , ctp , cgn , chn , ctn , obsc , i n i t i a l c ,

goalc , f u l l s c r n )31 % ob s f l a g = 1 ;32 % end33 %∗∗∗∗∗∗∗∗∗∗∗∗∗3435 %Run the ob s t a c l e r o u l e t t e to s imulate camera de t e c t i on36 %I f wanted on the path37 i f near path == 138 [ new obs , o b s f l a g ] = ob s t a c l e r o u l e t t e ( n x , ns i ze , r ou l e t t e , path , near path )

;39 %Otherwise complete ly random40 e l s e41 [ new obs , o b s f l a g ] = ob s t a c l e r o u l e t t e ( n x , ns i ze , r o u l e t t e ) ;42 end4344 %I f the ob s t a c l e i n t e r f e a r s with path45 i f o b s f l a g == 146 %In s e r t the new obs t a c l e to the graph space47 cspace = ob s t a c l e i n s e r t ( n x , n y , ns i ze , obsc , cspace , new obs ) ;48 %Replot the cspace with the new obs t a c l e so we can see i t49 p l o t c spa c e ( c x , c y , n x , n y , ns i ze , cspace , ctp , cgn , chn , ctn , obsc , i n i t i a l c ,

goalc , f u l l s c r n , 0 )50 %Generate the new path around the ob s t a c l e51 [ path , new path ] = update path ( cspace , n x , n y , ns i ze , ctp , ctn , obsc , path , j ,

s t r ang l e , type , de l t a ) ;5253 %Obstacle dea l t with , s t i c k i t in obs and c l e a r the f l a g array54 obs ( ( s i z e ( obs , 1 ) +1) , : ) = new obs ;55 new obs ( 1 , : ) = 0 ;56 ob s f l a g = 0 ;57 end58 j = j −1;59 end


C.1.3 driving path.m

1 % Path f o l l ow ing a lgor i thms2 % Owen Gebler og205@bath . ac . uk 201234 % I t eg r a t ed with path planner ( code h i gh l i gh t ed by ∗)5 % 20/03/2012 − J . Whittington − Uni e r s i t y o f Bath67 no po int s = s i z e ( path , 1 ) ; % no po int s along path8 t = [ 0 : ( 2 ∗ pi ) / no po int s :2∗ pi ] ;9

10 %∗∗∗∗∗∗∗∗∗∗∗∗∗JWW11 %crea t e path value ar rays12 pathx = ze ro s ( no points , 1 ) ;13 pathy = ze ro s ( no points , 1 ) ;14 pathb = ze ro s ( no points , 1 ) ;15 j = 1 ; %ex t r a c t i on inc .1617 %Inver t path array and ex t ra c t va lues18 f o r i = no po int s :−1:119 pathx ( j , : ) = path ( i , 1 ) ;20 pathy ( j , : ) = path ( i , 2 ) ;21 pathb ( j , : ) = path ( i , 3 ) ;22 j = j +1;23 end2425 %convert bear ing : John Nautical , Owen po lar (CCW from x)26 i f pathb (1 ,1 ) <= 90 && pathb (1 ,1 ) >= 027 pathb (1 ,1 ) = abs ( pathb (1 ,1 )−90) ;28 e l s e29 pathb (1 ,1 ) = 360−(pathb (1 ,1 )−90) ;30 end31 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗3233 v eh i c l e p o s = [ pathx (1 , 1 ) , pathy (1 ,1 ) , degtorad ( pathb (1 ,1 ) ) ] ; % v eh i c l e

i n i t i a l po s i t i on , x , y , theta34 vehlength = 1 ; % wheelbase o f v eh i c l e35 bodywidth = 0 . 8 ; % c r o s s body width36 ty r ed i a = 0 . 5 ; % diameter o f t y r e s37 tyrewidth = 0 . 2 ; % edge−2−edge width o f tyre38 maxstang = s t r a n g l e ; % max s t e e r i n g angle p o s s i b l e39 t imestep = 0 . 1 ; %time step s i z e40 % ve l o c i t y = 1 ; % veh i c l e l o n g i t ud i n a l v e l o c i t y (ms−1)41 run = 1 ; %run inc .42 goa l i nd = 1 ; %goa l index inc .43 obs once = 1 ; %zero ob s t a c l e once f l a g ( only one ob s t a c l e w i l l be randomly

generated , 1 to remove )44 ob s f l a g = 0 ; %zero ob s t a c l e f l a g4546 nt = 500 ;47 M1 = moviein ( nt ) ;4849 whi le goa l i nd < s i z e ( pathx , 1 )50 f o r i =1: l ength ( pathx )51 pathd i s t ( i )=sq r t ( ( pathx ( i )−v eh i c l e p o s (1 ) ) ˆ2+(pathy ( i )−v eh i c l e p o s (2 ) ) ˆ2)

;52 end53 % f ind index within pathd i s t array o f nea r e s t po int54 n e a r e s t p o i n t i nd = f ind ( pathd i s t==min( pathd i s t ) ) ;55 n e a r e s t p o i n t i nd = nea r e s t p o i n t i nd (1 , 1 ) ;5657 % in t e g e r d i s t ance to nea re s t po int f o r adapt ive lookahead58 adapt la = abs ( round (0 .2∗min( pathd i s t ) ) ) ;59 % adapt la = 060 % se t goa l po int index61 goa l i nd = nea r e s t p o i n t i nd + 4 + ( adapt la ) ;62 % cond i t i on f o r cont inuous path loop ing63 i f g oa l i nd > no po int s64 goa l i nd = goa l i nd − no po int s ;65 end6667 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗JWW68 i f obs once == 069 [ new obs , o b s f l a g ] = ob s t a c l e r o u l e t t e ( n x , ns i ze , 15 , path , 1 ) ;70 end7172 i f o b s f l a g == 173 o ld ind = s i z e ( path , 1 )−goa l i nd ;74 %In s e r t the new obs t a c l e to the graph space75 cspace = ob s t a c l e i n s e r t ( n x , n y , ns i ze , obsc , cspace , new obs ) ;76 %Replot the cspace with the new obs t a c l e so we can see i t77 p l o t c spa c e ( c x , c y , n x , n y , ns i ze , cspace , ctp , cgn , chn , ctn , obsc , i n i t i a l c ,

goalc , f u l l s c r n , 0 )


78 %Generate the new path around the ob s t a c l e79 path = update path ( cspace , n x , n y , ns i ze , ctp , ctn , obsc , path , o ld ind ,

s t r ang l e , type , de l t a ) ;80 %cr ea t e path value ar rays81 pathx = ze ro s ( no points , 1 ) ;82 pathy = ze ro s ( no points , 1 ) ;83 pathb = ze ro s ( no points , 1 ) ;84 j = 1 ; %ex t r a c t i on inc .8586 %Inve r t path array and ex t ra c t va lues87 no po int s = s i z e ( path , 1 ) ;88 f o r i = no po int s :−1:189 pathx ( j , : ) = path ( i , 1 ) ;90 pathy ( j , : ) = path ( i , 2 ) ;91 pathb ( j , : ) = path ( i , 3 ) ;92 j = j +1;93 end9495 %Obstacle dea l t with , s t i c k i t in obs and c l e a r the f l a g array96 obs ( ( s i z e ( obs , 1 ) +1) , : ) = new obs ;97 new obs ( 1 , : ) = 0 ;98 ob s f l a g = 0 ;99 obs once = 1 ;

100 end101 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗JWW102103104 % se t goa l po int based upon t h i s index and105 % assumption that vec to r s are o f same form106 goa lpo intx = pathx ( goa l i nd ) ;107 goa lpo inty = pathy ( goa l i nd ) ;108109110 % ca l c u l a t e curvature o f path to get to goa l po int111 goa lv e c to r = [ goalpo intx−v eh i c l e p o s (1 ) goalpo inty−v eh i c l e p o s (2 ) ] ;112 [ goa lang le , D] = ca r t2po l ( goa lv e c to r (1 ) , goa lv e c to r (2 ) ) ;113 alpha = pi /2 − v eh i c l e p o s (3 ) + goa lang l e ;114115 % de f i n e heading e r r o r116 ErrorAngle = goa lang l e − v eh i c l e p o s (3 ) ;117 i f ErrorAngle > pi118 ErrorAngle = ErrorAngle − (2∗ pi ) ;119 e l s e i f ErrorAngle < −pi120 ErrorAngle = ErrorAngle + (2∗ pi ) ;121 end122123 v e l o c i t y = abs (1 / ErrorAngle ) ;124 i f v e l o c i t y > 10125 v e l o c i t y = 10 ;126 end127128129 % l a t e r a l ( x ) o f f s e t o f goa l po int and v eh i c l e130 % in v eh i c l e coord frame131 dx = D ∗ cos ( alpha ) ;132 % rad ius o f curvature133 rad ius = −Dˆ2/(2∗dx ) ;134 % se t minimum rad ius based upon v eh i c l e p r op e r t i e s135 minrad = vehlength / tand (maxstang ) ; % based on ackerman geometry136 % l im i t rad ius based upon s t e e r i n g angle c on s t r a i n t137 i f rad ius > −minrad && rad ius < minrad138 rad ius = minrad ;139 end140141 % s t r a i g h t l i n e d i s t ance from veh i c l e c ent r e to goa l po int142 vehgoa ld i s t = sq r t ( ( goa lpo intx − v eh i c l e p o s (1 ) ) ˆ2+( goa lpo inty − v eh i c l e p o s

(2 ) ) ˆ2) ;143 % note D and vehgoa ld i s t should be same , one i s in po la r144 % other i s c a r t e s i a n145146 % cent r epo in t o f goa l curve c i r c l e147 tu rnc i r c enx = rad ius ∗ cos ( v eh i c l e p o s (3 )+pi /2) + veh i c l e p o s (1 ) ;148 tu rnc i r c eny = rad ius ∗ s i n ( v eh i c l e p o s (3 )+pi /2) + veh i c l e p o s (2 ) ;149150 % plo t circum of t a rg e t curve151 h = [ 0 : ( 2 ∗ pi ) /100:2∗ pi ] ; % i . e . 1 f u l l c y c l e152 x c i r p l o t = rad ius ∗ cos (h) + turnc i r c enx ;153 y c i r p l o t = rad ius ∗ s i n (h) + turnc i r c eny ;154155 % s t r a i g h t l i n e d i s t ance t r a v e l l e d per t imestep156 d = ve l o c i t y ∗ t imestep ;157 % angle o f arc s e c t i on moved through during t imestep158 phi = d/ rad ius ;159


160 % generate s t e e r i n g angle r equ i r ed to f o l l ow given curve161 sigma = atan ( vehlength / rad ius ) ;162 sigma2 = atan ( 2∗ vehlength∗ s i n ( phi ) / vehgoa ld i s t ) ;163164 % add l i n e to l im i t s t e e r i n g angle p o s s i b l e165166167 % use v eh i c l e c oo rd s func t i on to determine c a r t e s i a n168 % coords o f v eh i c l e body and tyre co rne r s169 [ out l inex , out l iney , o u t l i n e l f t x , o u t l i n e l f t y , o u t l i n e r f t x , o u t l i n e r f t y ,

o u t l i n e l r t x , o u t l i n e l r t y , ou t l i n e r r t x , o u t l i n e r r t y ] = veh i c l e c oo rd s (bodywidth , vehlength , v eh i c l e po s , tyrewidth , tyred ia , sigma ) ;

170171 f i g u r e (2)172 % plo t o f zoomed s e c t i on173 % plo t the path , goa l point , turn ing c i r c l e and turn ing c i r c l e c ent r e174 subplot (1 , 2 , 1 )175 p lo t ( pathx , pathy , goa lpo intx , goa lpo inty , ’ rX ’ , turnc i rcenx , turnc i rceny , ’ gx ’ ,

x c i r p l o t , y c i r p l o t )176177 % 2D polygons o f v eh i c l e f o r animation178 % Body patch179 patch ( out l inex , ou t l i n ey , ’ r ’ ) ;180 % l e f t r ea r tyre patch181 patch ( ou t l i n e l r t x , o u t l i n e l r t y , ’ k ’ ) ;182 % r i gh t r ea r tyre patch183 patch ( ou t l i n e r r t x , ou t l i n e r r t y , ’ k ’ ) ;184 % l e f t f r on t tyre patch185 patch ( o u t l i n e l f t x , o u t l i n e l f t y , ’ k ’ ) ;186 % r i gh t f r on t tyre patch187 patch ( ou t l i n e r f t x , o u t l i n e r f t y , ’ k ’ ) ;188189 % axes p r op e r t i e s190 ax i s equal ;191 ax i s ( [ v eh i c l e p o s (1 )−10 v eh i c l e p o s (1 )+10 v eh i c l e p o s (2 )−10 v eh i c l e p o s (2 )+10

] )192 x l ab e l ( ’x−d i s tance (m) ’ )193 y l ab e l ( ’y−d i s tance (m) ’ )194195 % plo t o f complete path196 % plo t the path , goa l point , turn ing c i r c l e and turn ing c i r c l e c ent r e197 subplot (1 , 2 , 2 )198 p lo t ( pathx , pathy , goa lpo intx , goa lpo inty , ’ rX ’ , turnc i rcenx , turnc i rceny , ’ gx ’ ,

x c i r p l o t , y c i r p l o t )199200 % 2D polygons o f v eh i c l e f o r animation201 % Body patch202 patch ( out l inex , ou t l i n ey , ’ r ’ ) ;203 % l e f t r ea r tyre patch204 patch ( ou t l i n e l r t x , o u t l i n e l r t y , ’ k ’ ) ;205 % r i gh t r ea r tyre patch206 patch ( ou t l i n e r r t x , ou t l i n e r r t y , ’ k ’ ) ;207 % l e f t f r on t tyre patch208 patch ( o u t l i n e l f t x , o u t l i n e l f t y , ’ k ’ ) ;209 % r i gh t f r on t tyre patch210 patch ( ou t l i n e r f t x , o u t l i n e r f t y , ’ k ’ ) ;211212 % axes p r op e r t i e s213 ax i s equal ;214 ax i s ( [ 0 c x 0 c y ] )215 x l ab e l ( ’x−d i s tance (m) ’ )216 y l ab e l ( ’y−d i s tance (m) ’ )217218219 % update v eh i c l e parameters220 % add cond i t i on f o r s t e e r i n g angle equa l s 0221 % new o r i e n t a t i o n equa l s o ld p lus angle o f arc moved through222 v eh i c l e p o s (3 ) = veh i c l e p o s (3 ) + phi ;223 v eh i c l e p o s (1 ) = turnc i r c enx − rad ius ∗ cos ( v eh i c l e p o s (3 )+pi /2) ;224 v eh i c l e p o s (2 ) = turnc i r c eny − rad ius ∗ s i n ( v eh i c l e p o s (3 )+pi /2) ;225 [ xfn , yfn ] = po l 2 ca r t ( v eh i c l e p o s (3 ) , vehlength ) ;226 vehfx = veh i c l e p o s (1 ) + xfn ;227 vehfy = veh i c l e p o s (2 ) + yfn ;228 run = run + timestep ;229 %then loop back to s t a r t230 M1( : , i )=getframe ;231232 end233234 %Output the dr ive time235 run = run


C.2 Functions

C.2.1 pathing.m

1 func t i on [ path ] = pathing ( hybrid , cspace , n x , n y , ctp , ctn , obsc , i n i t i a l , goal ,bearing , s t r , d e l t a )

2 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−3 % Function which implements the A∗ algor i thm to generate a path , g iven the4 % graph space and i n i t i a l / goa l po in t s .5 %6 % 15/02/2012 − J . Whittington − Uni e r s i t y o f Bath7 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−8 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−9

1011 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗12 %∗∗∗∗ Set Up ∗∗∗∗13 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1415 %Remove i n t e r p o l a t i o n i f no de l t a s p e c i f i e d16 i f narg in < 1217 de l t a = 1 ;18 end1920 t i c2122 %Open l i s t column index ing23 %[ act ive , x , y , parent x , parent y , g (n) , h(n) , t (n) , f (n) , bear ing ]24 oa = 1 ;25 ox = 2 ;26 oy = 3 ;27 opx = 4 ;28 opy = 5 ;29 ogn = 6 ;30 ohn = 7 ;31 otn = 8 ;32 ofn = 9 ;33 brn = 10 ;3435 %Cost weight ing [ h eu r i s t i c , s t e e r i n g ]36 weight = [ 2 0 , 1 5 ] ;3738 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗39 %∗∗∗∗ I n i t i a t e Algorithm ∗∗∗∗40 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗4142 %open l i s t [ a c t i v e (1/0) , x cord , y cord , parent x , parent y , gn , hn , tn , fn ,

bear ing ]43 open = [ ] ;44 %c l o s ed l i s t : [ x cord , y cord ]45 c l o s ed = [ ] ;4647 %Put known ob s t a c l e s s t r a i g h t in to c l o s ed l i s t by scanning48 %graph space f o r ob s t a c l e s .49 k=1; %counter f o r c l o s ed po s i t i o n r e f50 f o r i =1: n x51 f o r j =1: n y52 i f ( cspace ( i , j , ctp ) == obsc )53 c l o s ed (k , 1 ) = i ;54 c l o s ed (k , 2 ) = j ;55 k = k+1; %move to next row56 end57 end58 end5960 %Number o f nodes in c l o s ed l i s t b r ings the row counter to c o r r e c t po s i t i o n61 c l o s ed count = s i z e ( c losed , 1 ) ;6263 %Set i n i t i a l po int as f i r s t node in open l i s t64 xNode = i n i t i a l ( 1 , 1 ) ;65 yNode = i n i t i a l ( 1 , 2 ) ;66 open count = 1 ; %row increment f o r open l i s t , s t a r t on f i r s t l i n e67 g co s t = 0 ; %al ready on point68 h co s t = h e u r i s t i c (xNode , yNode , goa l ) ; %f i nd the h e u r i s t i c co s t to goa l69 t c o s t = cspace ( f l o o r ( xNode ) , f l o o r ( yNode ) , ctn ) ; %t r a v e r s a l co s t70 f c o s t = h co s t + t c o s t ;7172 %Add the node to the open l i s t , parent = node s i n c e i n i t i a l73 open ( open count , : ) = in s e r t op en (xNode , yNode , xNode , yNode , g cost , h cost , t c o s t ,

f c o s t , bear ing ) ;74 open ( open count , oa ) = 0 ; %no longe r a c t i v e on c l o s ed l i s t s i n c e a l ready ’ explored

’


7576 %Add the i n i t a l po int to the next row on the c l o s ed l i s t s i n c e explored77 c l o s ed count = c lo s ed count + 1 ;78 c l o s ed ( c lo sed count , 1 ) = xNode ;79 c l o s ed ( c lo sed count , 2 ) = yNode ;8081 %Create a f l a g f o r path p o s s b i l i t y : 0 whi le path i s not blocked82 blocked = 0 ;8384 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗85 %∗∗∗∗ Run Algorithm ∗∗∗∗86 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗8788 switch hybrid89 case ’ a s ta r ’90 l o s ch e ck = 0 ;91 case ’ t h e t a s t a r ’92 weight = [ 1 , 1 ] ;93 l o s ch e ck = 1 ;94 end9596 whi le ( ( xNode ˜= goa l (1 , 1 ) | | yNode ˜= goa l (1 , 2 ) ) && blocked ˜= 1)97 adj nodes = adjacent (xNode , yNode , bearing , s t r , g cos t , goal , c losed , n x , n y ,

cspace , de l ta , weight ) ;98 adj count = s i z e ( adj nodes , 1 ) ;99

100 %For the number o f adjacent nodes101 f o r i = 1 : adj count102 f l a g = 0 ;103 %Search the open l i s t to see i f i t i s on i t104 f o r j = 1 : open count105 i f ( ad j nodes ( i , 1 ) == open ( j , ox ) && adj nodes ( i , 2 ) == open ( j , oy ) )106 %−−−−−−−−ThetaSTAR−−−−−−−−−−107 i f l o s ch e ck == 1108 %Find the parents o f xNode , yNode f o r l o s check .109 inode = node index ( open , xNode , yNode ) ;110 parentx = open ( inode , opx ) ;111 parenty = open ( inode , opy ) ;112 %I s the l i n e o f s i gh t c l e a r between the l a s t node and t h i s113 %adjacent node? ( i f us ing theta ∗)114 i f ( ( l i n e o f s i g h t ( parentx , parenty , ad j nodes ( i , 1 ) , ad j nodes ( i

, 2 ) , cspace , ctp ) == 1) && ( l o s ch e ck == 1) )115 %update the path cos t to r e f l e c t the new d i s tance116 %between parent and adj node117 adj nodes ( i , 3 ) = g co s t + d i s t ance ( parentx , parenty ,

ad j nodes ( i , 1 ) , ad j nodes ( i , 2 ) ) ;118 %ca l c u l a t e the new cos t func t i on119 adj nodes ( i , 6 ) = adj nodes ( i , 4 )+adj nodes ( i , 3 )+adj nodes (

i , 5 ) ;120 open ( j , ofn ) = min ( open ( j , ofn ) , ad j nodes ( i , 6 ) ) ;121 i f open ( j , ofn ) == adj nodes ( i , 6 )122 open ( j , opx ) = parentx ; %se t parents to grandparents123 open ( j , opy ) = parenty ; %se t parents to grandparents124 open ( j , ogn ) = adj nodes ( i , 3 ) ; %g (n)125 open ( j , ohn ) = adj nodes ( i , 4 ) ; %h(n)126 open ( j , otn ) = adj nodes ( i , 5 ) ; %t (n)127 open ( j , brn ) = adj nodes ( i , 7 ) ; %bear ing128 end129 e l s e130 %I s f (n) to t h i s node sma l l e r than current ?131 open ( j , ofn ) = min ( open ( j , ofn ) , ad j nodes ( i , 6 ) ) ;132 i f open ( j , ofn ) == adj nodes ( i , 6 )133 %I f i t i s , update the parent node to cur rent pos i t i on

,134 %new cos t va lues135 open ( j , opx ) = xNode ;136 open ( j , opy ) = yNode ;137 open ( j , ogn ) = adj nodes ( i , 3 ) ; %g (n)138 open ( j , ohn ) = adj nodes ( i , 4 ) ; %h(n)139 open ( j , otn ) = adj nodes ( i , 5 ) ; %t (n)140 open ( j , brn ) = adj nodes ( i , 7 ) ; %bear ing141 end142 end143144 %−−−−−−standard astar−−−−−−−−−145 e l s e146 %I s f (n) to t h i s node sma l l e r than current ?147 open ( j , ofn ) = min ( open ( j , ofn ) , ad j nodes ( i , 6 ) ) ;148 i f open ( j , ofn ) == adj nodes ( i , 6 )149 %I f i t i s , update the parent node to cur rent pos i t i on

,150 %new cos t va lues151 open ( j , opx ) = xNode ;152 open ( j , opy ) = yNode ;


153 open ( j , ogn ) = adj nodes ( i , 3 ) ; %g (n)154 open ( j , ohn ) = adj nodes ( i , 4 ) ; %h(n)155 open ( j , otn ) = adj nodes ( i , 5 ) ; %t (n)156 open ( j , brn ) = adj nodes ( i , 7 ) ; %bear ing157 end158 end159 f l a g = 1 ; %se t open l i s t check f l a g to true160 end161 end %end open l i s t scan162 %Add the node to open l i s t i f not on i t163 i f f l a g == 0164 open count = open count + 1 ;165 open ( open count , : ) = in s e r t op en ( adj nodes ( i , 1 ) , ad j nodes ( i , 2 ) , xNode ,

yNode , ad j nodes ( i , 3 ) , ad j nodes ( i , 4 ) , ad j nodes ( i , 5 ) , ad j nodes ( i , 6 ), ad j nodes ( i , 7 ) ) ;

166 end167 end %end node i check168169 %Now f ind node o f l e a s t f (n)170 min node = min fn ( open , open count , goa l ) ;171172 i f ( min node ˜= −1) %I f the path has somewhere to go173 %’move ’ to t h i s l e a s t co s t node174 xNode = open (min node , ox ) ;175 yNode = open (min node , oy ) ;176 g co s t = open (min node , ogn ) ;177 bear ing = open (min node , brn ) ;178 %and p lace i t on the c l o s ed l i s t ( looked at )179 c l o s ed count = c lo s ed count + 1 ;180 c l o s ed ( c lo sed count , 1 ) = xNode ;181 c l o s ed ( c lo sed count , 2 ) = yNode ;182 open (min node , oa ) = 0 ; %make the node i n a c t i v e in open183 e l s e184 %Path i s blocked , s e t f l a g to true to e x i t loop185 blocked = 1 ;186 end187 end188189 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗190 %∗∗∗∗ Back Tracking ∗∗∗∗191 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗192193 %Sta r t ing at the l a s t node on c l o s ed ( which should be the goa l ) t r a c e back194 %down the path by r e f e r i n g to the parents o f each node in order to generate195 %the i n i t i a l path196197 %Create an array to hold path198 %[ x cord , y cord , bear ing ]199 path = [ ] ;200201 %Load l a s t node t r ave l ed in to f i r s t row o f path array202 xcord = c l o s ed ( s i z e ( c losed , 1 ) ,1 ) ;203 ycord = c l o s ed ( s i z e ( c losed , 1 ) ,2 ) ;204 i = 1 ; %row increament205 path ( i , 1 ) = xcord ;206 path ( i , 2 ) = ycord ;207 path ( i , 3 ) = bear ing ;208 i = i +1;209 inode = 0 ; %index ho lder210211 i f ( ( xcord == goa l (1 , 1 ) ) && ( ycord == goa l (1 , 2 ) ) )212 %Load the parent node o f the cur rent node213 inode = node index ( open , xcord , ycord ) ;214 parentx = open ( inode , opx ) ;215 parenty = open ( inode , opy ) ;216 bearingpx = open ( inode , brn ) ;217218 %Trace the route back to the i n i t i a l po int219 whi le ( parentx ˜= i n i t i a l ( 1 , 1 ) | | parenty ˜= i n i t i a l ( 1 , 2 ) )220 path ( i , 1 ) = parentx ;221 path ( i , 2 ) = parenty ;222 path ( i , 3 ) = bearingpx ;223 %Find the parents o f the parent node ( grandparents )224 inode = node index ( open , parentx , parenty ) ;225 parentx = open ( inode , opx ) ;226 parenty = open ( inode , opy ) ;227 bearingpx = open ( inode , brn ) ;228 i = i +1; %move to next row in path229 end230 %Add the i n i t i a l po int to the path231 path ( i , 1 ) = i n i t i a l ( 1 , 1 ) ;232 path ( i , 2 ) = i n i t i a l ( 1 , 2 ) ;233 path ( i , 3 ) = open (1 , brn ) ;234 e l s e


235 pause (1) ;236 h=msgbox ( ’ Goal blocked , no path e x i s t s . ’ , ’ warn ’ ) ;237 u iwa i t (h , 5 ) ;238 e r r o r ( ’ Goal blocked , no path e x i s t s . ’ ) ;239 end240241 path t = toc242243 end


C.2.2 update path.m

1 func t i on [ path , new path ] = update path ( cspace , n x , n y , ns i ze , ctp , ctn , obsc , path ,j , s t r ang l e , type , de l t a )

2 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−3 % Function which c r e a t e s a new path around an ob s t a c l e and updates the4 % graph space p lo t .5 %6 % j i s the cur rent row index within the path array7 %8 % 27/03/2012 − J . Whittington − Uni e r s i t y o f Bath9 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

10 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−1112 f rom current = 0 ; %r ep l o t from current po s i t i o n ?13 x = path ( j , 1 ) ; %current x po s i t i o n o f quad14 y = path ( j , 2 ) ; %current y po s i t i o n o f quad1516 %Find i f and where the ob s t a c l e ob s t ruc t s cur rent path17 [ ob s s t a r t , obs end , pathent index , pathext index , no obs ] = ob s t a c l e s c an ( cspace ,

path , j , ctp ) ;1819 %Create the l o c a l path around the ob j e c t i f i t does obs t ruc t20 i f no obs == 021 %I f the ob s t a c l e i s c l o s e to the goal , plan the new path to the22 %o r i g i n a l goa l23 i f ( pathext index < (15/ n s i z e ) )24 obs end = [ path (1 , 1 ) , path (1 , 2 ) ] ; %s e t the end point as the o r i g i n a l goa l

f o r best path25 pathent index = 0 ; %in which case , index = 026 end2728 %I f opt ion to generate from current po s i t i o n i s t rue29 i f f rom current == 130 ob s s t a r t = [ x , y ] ;31 pathext index = j ;32 end3334 %generate the path us ing algor i thm35 switch type36 case ’ f i e l dD ’37 new path = pathing ( ’ a s t a r ’ , cspace , n x , n y , ctp , ctn , obsc , ob s s t a r t ,

obs end , path ( pathext index , 3 ) , s t r ang l e , d e l t a ) ;38 case { ’ a s t a r ’ , ’ astarPS ’ , ’ t h e t a s t a r ’}39 new path = pathing ( ’ a s t a r ’ , cspace , n x , n y , ctp , ctn , obsc , ob s s t a r t ,

obs end , path ( pathext index , 3 ) , s t r a n g l e ) ;40 end4142 %rep l a c e segment o f path which pas se s through ob s t a c l e with new43 %path44 path = a r r a y i n s e r t ( path , new path , pathent index , pathext index ) ;4546 %Plot the adjusted path47 p lo t path ( path )48 p = p lo t (x , y , ’ bo ’ , ’ MarkerFaceColor ’ , ’ c ’ ) ; %r ep l o t quad so i t can be seen49 e l s e i f no obs == 250 %e r r o r case when ob s t a c l e doesn ’ t end : ove r l aps goa l .51 e r r = e r r o r d l g ( ’New obs t a c l e l i e s with in goa l ! ’ ) ;52 e r r o r ( ’New obs t a c l e l i e s with in goa l ! ’ ) ;53 end5455 end


C.2.3 adjacent.m

1 func t i on [ exp array ] = adjacent ( x , y , heading , s t e e r i ng , path , goal , c losed , n x , n y ,cmap , de l ta , weight )

2 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−3 % Function which c r e a t e s an array o f nodes adjacent to node with cords ’x ’4 % and ’y ’ . The array i s formed as f o l l ow s :5 %6 % [ adj x cord , adj y cord , gn , hn , tn , fn , bear ing ] ad jacent node 17 % [ ’ . . ’ ] ’ . . ’ 28 %9 % de l t a i s used to c r ea t e a f i e l d d∗ i n t e r po l a t ed output o f g r id edges and

10 % i s equal to the number o f s t ep s per edge used11 %12 % 15/02/2012 − J . Whittington − Uni e r s i t y o f Bath13 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−14 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−1516 % de l t a and weight inputs can be ignored17 i f narg in < 1118 de l t a = 1 ;19 weight = 1 ;20 end2122 %exp array : [ x cord , y cord , g (n) , h(n) , t (n) , f (n) , bear ing ] adjacent nodes

l i s t e d23 %through rows24 exp array = [ ] ;2526 %row counter27 exp count = 1 ;28 row c lo sed = s i z e ( c losed , 1 ) ;2930 %For loops which e n c i r c l e node by tak ing cords 1 e i t h e r s i d e in ho r i z on t a l31 %and v e r t i c a l32 f o r k = 1:(−1/ de l t a ) :−1 %ho r i z on t a l scan33 f o r j = 1:(−1/ de l t a ) :−1 %v e r t i c a l scan within with ho r i z on t a l34 i f ( k ˜= j | | k ˜= 0)35 %I s the goa l with in the adjacent area ? I f i t i s , add i t36 %r e g a r d l e s s . This i s r equ i r ed when us ing i n t e r p o l a t i o n .37 i f ( ( x+k) == goa l (1 , 1 ) && (y+j ) == goa l (1 , 2 ) )38 adj x = x+k ;39 adj y = y+j ;40 %Populate the row of the output array41 exp array ( exp count , 1 ) = adj x ;42 exp array ( exp count , 2 ) = adj y ;43 exp array ( exp count , 3 ) = path+d i s tance (x , y , adj x , ad j y ) ;44 exp array ( exp count , 4 ) = h e u r i s t i c ( adj x , adj y , goal , weight (1 , 1 ) ) ;45 exp array ( exp count , 5 ) = cmap( f l o o r ( ad j x ) , f l o o r ( ad j y ) ,4) ; %

f l o o r e d f o r on l i n e l o c a l path generat i on46 exp array ( exp count , 6 ) = exp array ( exp count , 3 )+exp array (

exp count , 4 )+exp array ( exp count , 5 ) ;47 exp array ( exp count , 7 ) = heading ;48 %Move to next row index on array49 exp count = exp count+1;50 end51 %Are we on the edge o f the g r id ?52 i f ( ( abs (k ) == 1) | | ( abs ( j ) == 1) | | ( ( abs (k )+abs ( j ) ) == 2) )53 %The cords o f the n ’54 ad j i n t x = x + k ;55 ad j i n t y = y + j ;56 %and the bear ing57 theta = angle (x , y , ad j in t x , ad j in t y , j , k ) ;5859 %I s the edge with in graph space ? I f i t i s f l a g i t to be added60 %to open61 i f ( ( ad j i n t x > 1 && ad j i n t x <= n x ) && ( ad j i n t y > 1 &&

ad j i n t y <= n y ) )62 f l a g = 1 ;6364 %Scan rows o f c l o s ed to check c l o s e s t nodes not in l i s t65 f o r c = 1 : row c lo sed66 i f ( ( c e i l ( ad j i n t x ) == c lo s ed ( c , 1 ) && c e i l ( ad j i n t y ) ==

c lo s ed ( c , 2 ) ) | | ( f l o o r ( ad j i n t x ) == c lo s ed ( c , 1 ) &&f l o o r ( ad j i n t y ) == c lo s ed ( c , 2 ) ) )

67 f l a g = 0 ; %i f i t i s , remove the f l a g68 end69 end7071 %I s the po int with in the s t e e r i n g range ?72 i f abs ( heading−theta ) > s t e e r i n g73 f l a g = 0 ;74 end


7576 i f ( f l a g == 1)77 %f ind c l o s e s t nodes78 s2 = [ c e i l ( ad j i n t x ) , c e i l ( ad j i n t y ) ] ;79 s1 = [ f l o o r ( ad j i n t x ) , f l o o r ( ad j i n t y ) ] ;80 exp array ( exp count , 1 ) = ad j i n t x ;81 exp array ( exp count , 2 ) = ad j i n t y ;82 exp array ( exp count , 3 ) = path+d i s tance (x , y , ad j in t x ,

ad j i n t y )+s t e e r i n g c o s t ( heading , theta , weight (1 , 2 ) ) ;83 exp array ( exp count , 4 ) = h e u r i s t i c ( ad j in t x , ad j in t y , goal

, weight (1 , 1 ) ) ;84 %get co s t in f i nd i ng a func t i on o f d i s t ance85 %from neare s t nodes i f between nodes86 i f (mod( ( x+k) ,1) == 0 && mod( ( y+j ) ,1) == 0)87 exp array ( exp count , 5 ) = cmap( ad j in t x , ad j in t y , 4 ) ;88 e l s e89 exp array ( exp count , 5 ) = ( ( cmap( s2 (1 , 1 ) , s2 (1 , 2 ) ,4 ) ∗

abs (k+j ) )+(cmap( s1 (1 , 1 ) , s1 (1 , 2 ) ,4 ) ∗abs(1− j−k ) ) ) ;90 end91 exp array ( exp count , 6 ) = exp array ( exp count , 3 )+exp array

( exp count , 4 )+exp array ( exp count , 5 ) ;92 exp array ( exp count , 7 ) = theta ;93 %Move to next row index on array94 exp count = exp count+1;95 end9697 end %node space check98 end %edge check99

100 end %not node i t s e l f end101 end %v e r t i c a l scan end102 end %ho r i z on t a l scan end103104 end


C.2.4 lineofsight.m

1 func t i on [ l o s c l e a r ] = l i n e o f s i g h t ( x , y , xdash , ydash , cspace , ctp , p l o t t i n g )2 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−3 % Function which checks the l i n e o f s i gh t between two nodes (x , y ) and4 % ( xdash , ydash ) .5 %6 % The output ’ l o s c l e a r ’ a boolan : f a l s e i f l i n e o f s i gh t i s blocked by7 % obs t a c l e and true i f c l e a r .8 %9 % 07/03/2012 − J . Whittington − Uni e r s i t y o f Bath

10 % r e f : Alex Nash , 2007 ’ Theta ∗ : Any−Angle Path Planning on Grids ’ ,11 % LineOfSight pseudo code .12 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−13 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−1415 i f narg in < 716 p l o t t i n g = 0 ;17 end1819 %Find the change in x and y between nodes20 dx = xdash−x ;21 dy = ydash−y ;22 %Distance increament23 f = 0 ;24 %Set output d e f au l t s to t rue25 l o s c l e a r = 1 ;2627 %∗∗∗∗∗ t e s t i n g code ∗∗∗∗∗∗∗28 %Plot the l o s being t e s t ed29 i f p l o t t i n g == 130 p lo t ( [ x , xdash ] , [ y , ydash ] , ’m’ ) ;31 % p lo t (x , y , ’ g ’ ) ;32 drawnow ;33 end34 %∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗3536 %I s the ydash lower than y? I f i t i s , must use decreament37 i f dy < 038 dy = dy∗−1;39 sy = −1;40 e l s e41 sy = 1 ;42 end4344 %I s xdash be fo r e x? I f i t i s , must use decreament45 i f dx < 046 dx = dx∗−1;47 sx = −1;48 e l s e49 sx = 1 ;50 end5152 %Which de l t a i s greate r , x or y . Run on which i s g r e a t e s t53 i f dx >= dy54 whi le ( x ˜= xdash && l o s c l e a r == 1)55 f = f + dy ;56 i f f >= dx57 i f cspace ( ( x+(( sx−1)/2) ) , ( y+(( sy−1)/2) ) , ctp ) == −158 l o s c l e a r = 0 ;59 end60 y = y + sy ;61 f = f − dx ;62 end63 i f ( ( f ˜= 0) && ( cspace ( ( x+(( sx−1)/2) ) , ( y+(( sy−1)/2) ) , ctp ) == −1) )64 l o s c l e a r = 0 ;65 end66 i f ( ( dy == 0) && ( cspace ( ( x+(( sx−1)/2) ) ,y , ctp ) == −1) && ( cspace ( ( x+(( sx

−1)/2) ) , ( y−1) , ctp ) == −1) )67 l o s c l e a r = 0 ;68 end69 x = x +sx ;70 end71 e l s e72 whi le ( y ˜= ydash && l o s c l e a r == 1)73 f = f + dx ;74 i f f >= dy75 i f cspace ( ( x+(( sx−1)/2) ) , ( y+(( sy−1)/2) ) , ctp ) == −176 l o s c l e a r = 0 ;77 end78 x = x + sx ;79 f = f − dy ;80 end


81 i f ( ( f ˜= 0) && ( cspace ( ( x+(( sx−1)/2) ) , ( y+(( sy−1)/2) ) , ctp ) == −1) )82 l o s c l e a r = 0 ;83 end84 i f ( ( dx == 0) && ( cspace (x , ( y+(( sy−1)/2) ) , ctp ) == −1) && ( cspace ( ( x−1) , (

y+(( sy−1)/2) ) , ctp ) == −1) )85 l o s c l e a r = 0 ;86 end87 y = y +sy ;88 end89 end909192 end


C.2.5 obstacle insert.m

1 func t i on [ cspace ] = o b s t a c l e i n s e r t ( n x , n y , ns i ze , obsc , cspace , obs )2 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−3 % Function to add ob s t a c l e s to the graph space generated by the path4 % planner . Nodal l o c a t i o n o f ob s t a c l e s are de f ined a matrix and given5 % a value o f −1.6 %7 % 15/02/2012 − J . Whittington − Uni e r s i t y o f Bath8 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−9 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

1011 %Trav e r s ab i l i t y co s t f i e l d s i z e12 f s i z e n = c e i l ( n x ∗0 .02) ; %2% of node space f o r s i n g l e node ( rounded up)13 f s i z e = n x /20 ; %de f au l t f i e l d s i z e (most are va r i ab l e depending on ob j e c t s i z e −

see below14 f s c a l e = 1 ;1516 %∗∗−Obstacle bu i lder −∗∗1718 f o r i = 1 : s i z e ( obs , 1 )19 %S ing l e node20 i f ( obs ( i , 5 ) == 0)21 x1 = obs ( i , 1 ) ;22 y1 = obs ( i , 2 ) ;23 cspace ( x1 , y1 , 1 ) = −1;24 %Create t r a v e r s a b i l i t y ’ s tar ’25 cspace = o b s t a c l e f i e l d ( x1 , y1 , f s i z en , ns i ze , n x , n y , cspace , 1 , 1 ) ;26 cspace = o b s t a c l e f i e l d ( x1 , y1 , f s i z en , ns i ze , n x , n y , cspace , 3 , 1 ) ;27 cspace = o b s t a c l e f i e l d ( x1 , y1 , f s i z en , ns i ze , n x , n y , cspace , 2 , 2 ) ;28 cspace = o b s t a c l e f i e l d ( x1 , y1 , f s i z en , ns i ze , n x , n y , cspace , 4 , 2 ) ;2930 %Rectangle or square31 e l s e i f ( obs ( i , 5 ) == 1)32 ob s i z e = obs ( i , 3 ) ; %get s i z e from array33 ob r a t i o = obs ( i , 4 ) ; %get r a t i o34 x1 = obs ( i , 1 ) − ( o b s i z e ∗ ob r a t i o ) ; %get l e f t po int35 x2 = obs ( i , 1 ) + ( ob s i z e ∗ ob r a t i o ) ; %get r i gh t po int36 y1 = obs ( i , 2 ) − ob s i z e ; %get bottom point37 y2 = obs ( i , 2 ) + ob s i z e ; %get top point38 i f f s c a l e == 139 f s i z e = c e i l ( o b s i z e ∗ ob r a t i o ∗0 .4 ) ; %f i e l d s i z e i s 40% of ob s t a c l e40 end41 %fo r the width o f the ob s t a c l e42 %ensure with in l im i t s f o r graph space43 i f x1 < 144 x1 = 1 ;45 end46 i f x2 > n x47 x2 = n x ;48 end49 i f y1 < 150 y1 = 1 ;51 end52 i f y2 > n y53 y2 = n y ;54 end55 f o r k = x1 : n s i z e : x256 %f i l l the he ight with ob s t a c l e nodes57 f o r l = y1 : n s i z e : y258 cspace (k , l , 1 ) = obsc ;59 %Apply t r a v e r s a b i l i t y f i e l d around square60 i f k == x161 cspace = o b s t a c l e f i e l d ( x1 , l , f s i z e , ns i ze , n x , n y , cspace , 1 , 1 ) ;62 e l s e i f k == x263 cspace = o b s t a c l e f i e l d ( x2 , l , f s i z e , ns i ze , n x , n y , cspace , 3 , 1 ) ;64 e l s e i f l == y165 cspace = o b s t a c l e f i e l d (k , y1 , f s i z e , ns i ze , n x , n y , cspace , 4 , 2 ) ;66 e l s e i f l == y267 cspace = o b s t a c l e f i e l d (k , y2 , f s i z e , ns i ze , n x , n y , cspace , 2 , 2 ) ;68 end69 end70 end71 %square end7273 %C i r c l e74 e l s e i f ( obs ( i , 5 ) == 2)75 ob rad = obs ( i , 3 ) ; %rad ius76 ob r a t i o = obs ( i , 4 ) ; %r a t i o77 x1 = obs ( i , 1 ) − ( ob rad∗ ob r a t i o ) ; %l e f t po int78 x2 = obs ( i , 1 ) + ( ob rad∗ ob r a t i o ) ; %r i gh t po int79 i f f s c a l e == 180 f s i z e = c e i l ( ob rad∗ ob r a t i o ∗0 .2 ) ; %f i e l d s i z e i s 20% of ob s t a c l e


81 end82 %ensure with in l im i t s f o r graph space83 i f x1 < 184 x1 = 1 ;85 end86 i f x2 > n x87 x2 = n x ;88 end89 y = obs ( i , 2 ) ; %v e r t i c a l node cente r90 cente r = obs ( i , 1 ) ; %cente r91 %c i r 1 and c i r 2 are ar rays conta in ing the gradual r a d i a l92 %in c r e a s e93 c i r 1 = 0 : n s i z e : ob rad ;94 c i r 2 = ( ob rad−1):− n s i z e : 0 ;95 c i r = [ c i r1 , c i r 2 ] ; %combine to make a pyramid array96 p1 = 1 ; %point r e f e r e n c e 197 [num maxindx ] = max( c i r ( 1 , : ) ) ; %l o c a t i o n o f maximum span98 i i = 1 ; %r e s e t l im i t check inc99

100 %−−−−−−−Build c i r c l e−−−−−−101 %f o r broad (x b igge r than y ) c i r c l e s102 i f ( ob r a t i o >= 1)103 %bu i l d s the l e f t h a l f o f the c i r c l e104 f o r k = x1 : n s i z e : c ente r105 y1 = y−c i r ( 1 , 1 : p1 ) ;106 y2 = y+c i r ( 1 , 1 : p1 ) ;107 f o r i i = 1 : s i z e ( y1 , 2 )108 i f y1 ( i i ) < 1109 y1 ( i i ) = 1 ;110 end111 end112 i i = 1 ; %r e s e t l im i t check inc113 f o r i i = 1 : s i z e ( y2 , 2 )114 i f y2 ( i i ) > n y115 y2 ( i i ) = n y ;116 end117 end118 cspace (k , y1 , 1 ) = −1;119 cspace (k , y2 , 1 ) = −1;120 %Apply t r a v e r s a b i l i t y f i e l d around semi−c i r c l e121 cspace = o b s t a c l e f i e l d (k , min ( y1 ) , f s i z e , ns i ze , n x , n y , cspace , 1 , 2 )

;122 cspace = o b s t a c l e f i e l d (k ,max( y2 ) , f s i z e , ns i ze , n x , n y , cspace , 3 , 2 )

;123 i f p1 < maxindx124 p1 = p1+1;125 end126 end127 p2 = s i z e ( c i r , 2 ) ; %point r e f e r e n c e 2128 i i = 1 ; %r e s e t l im i t check inc129 i f ( ob r a t i o == 1) %i f symetr ic , r e f e r n c e must be inc . to avoid

double cente r130 p1 = p1+1;131 end132 %bu i l d s the r i gh t ha l f o f c i r c l e133 f o r k = ( cente r+1) : n s i z e : x2134 y1 = y−c i r (1 , p1 : p2 ) ;135 y2 = y+c i r (1 , p1 : p2 ) ;136 f o r i i = 1 : s i z e ( y1 , 2 )137 i f y1 ( i i ) < 1138 y1 ( i i ) = 1 ;139 end140 end141 i i = 1 ; %r e s e t l im i t check inc142 f o r i i = 1 : s i z e ( y2 , 2 )143 i f y2 ( i i ) > n y144 y2 ( i i ) = n y ;145 end146 end147 cspace (k , y1 , 1 ) = −1;148 cspace (k , y2 , 1 ) = −1;149 %Apply t r a v e r s a b i l i t y f i e l d around semi−c i r c l e150 cspace = o b s t a c l e f i e l d (k , min ( y1 ) , f s i z e , ns i ze , n x , n y , cspace , 1 , 2 )

;151 cspace = o b s t a c l e f i e l d (k ,max( y2 ) , f s i z e , ns i ze , n x , n y , cspace , 3 , 2 )

;152 %in c r e a s e the array r e f e r e n c e i f beyond middle o f c i r c l e153 i f ( ( p1 < s i z e ( c i r , 2 ) ) && (k > ( x2−ob rad ) ) )154 p1 = p1+1;155 end156 end157 %fo r e l i p s e s , r i gh t t i p must be added158 i f ( ob r a t i o > 1)159 cspace ( ( x2+ns i z e ) ,y , 1 ) = −1;


160 %as we l l as f i e l d161 cspace = o b s t a c l e f i e l d ( x2+ns ize , y , f s i z e , ns i ze , n x , n y , cspace

, 1 , 2 ) ;162 cspace = o b s t a c l e f i e l d ( x2+ns ize , y , f s i z e , ns i ze , n x , n y , cspace

, 3 , 2 ) ;163 end164 %Add ho r i z on t a l f i e l d to ends165 cspace = o b s t a c l e f i e l d ( x1 , y , f s i z e , ns i ze , n x , n y , cspace , 1 , 1 ) ;166 cspace = o b s t a c l e f i e l d ( x2 , y , f s i z e , ns i ze , n x , n y , cspace , 3 , 1 ) ;167168 %f o r t a l l c i r c l e s ( y b igge r than x )169 e l s e i f ( ob r a t i o >= 1)170 rad ius = ob rad∗ ob r a t i o ;171 midy = maxindx−rad ius ;172 end173 %c i r c l e end174 %St r i gh t l i n e175 e l s e i f obs ( i , 5 ) == 3176 i f obs ( i , 4 ) == 0 %hor i z on t a l177 ob s i z e = obs ( i , 3 ) ;178 x1 = obs ( i , 1 ) − ob s i z e ;179 x2 = obs ( i , 1 ) + ob s i z e ;180 y = obs ( i , 2 ) ;181 i f f s c a l e == 1182 f s i z e = c e i l ( o b s i z e ∗0 .1 ) ; %f i e l d s i z e i s 10% of ob s t a c l e183 end184 %ensure with in l im i t s f o r graph space185 i f x1 < 1186 x1 = 1 ;187 end188 i f x2 > n x189 x2 = n x ;190 end191 f o r x = x1 : n s i z e : x2192 cspace (x , y , 1 ) = −1;193 cspace = o b s t a c l e f i e l d (x , y , f s i z e , ns i ze , n x , n y , cspace , 2 , 2 ) ; %

bottom edge194 cspace = o b s t a c l e f i e l d (x , y , f s i z e , ns i ze , n x , n y , cspace , 4 , 2 ) ; %top

edge195 end196 cspace = o b s t a c l e f i e l d ( x1 , y , f s i z e , ns i ze , n x , n y , cspace , 1 , 1 ) ; %l e f t

edge197 cspace = o b s t a c l e f i e l d ( x2 , y , f s i z e , ns i ze , n x , n y , cspace , 3 , 1 ) ; %r i gh t

edge198 e l s e i f obs ( i , 4 ) >= 1 %v e r t i c a l ( a l s o d e f au l t i f other r a t i o inputted )199 ob s i z e = obs ( i , 3 ) ;200 y1 = obs ( i , 2 ) − ob s i z e ;201 y2 = obs ( i , 2 ) + ob s i z e ;202 x = obs ( i , 1 ) ;203 i f f s c a l e == 1204 f s i z e = c e i l ( o b s i z e ∗0 .1 ) ; %f i e l d s i z e i s 10% of ob s t a c l e205 end206 %ensure with in l im i t s f o r graph space207 i f y1 < 1208 y1 = 1 ;209 end210 i f y2 > n y211 y2 = n y ;212 end213 f o r y = y1 : n s i z e : y2214 cspace (x , y , 1 ) = −1;215 cspace = o b s t a c l e f i e l d (x , y , f s i z e , ns i ze , n x , n y , cspace , 1 , 1 ) ; %

l e f t edge f i l l216 cspace = o b s t a c l e f i e l d (x , y , f s i z e , ns i ze , n x , n y , cspace , 3 , 1 ) ; %

r i gh t edge f i l l217 end218 cspace = o b s t a c l e f i e l d (x , y1 , f s i z e , ns i ze , n x , n y , cspace , 4 , 2 ) ; %bottom

edge219 cspace = o b s t a c l e f i e l d (x , y2 , f s i z e , ns i ze , n x , n y , cspace , 2 , 2 ) ; %top

edge220 e l s e221 d i sp l ay ( ’ I nva l i d r a t i o f o r l i n e : 0 = hor i zonta l , 1 = v e r i t c a l .

Defaulted to v e r t i c a l ’ )222 end223224 %Warn o f unknown obs t a c l e225 e l s e226 pause (1) ;227 h = msgbox ( ’ Obstac le type in o b s t a c l e i n s e r t .m unknown ’ , ’warn ’ ) ;228 u iwa i t (h , 5 ) ;229 end230 end


C.2.6 obstacle field.m

1 func t i on [ cspace ] = o b s t a c l e f i e l d ( x , y , s i z e , ns i ze , n x , n y , cspace , s ide , type )2 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−3 % Function to c r ea t e a t r a v e r s a b i l i t y f i e l d around a given ob s t a c l e .4 % The f i e l d dec r ea s e s with d i s t ance from obs t a c l e and i s spread ac ro s s5 % nodes de f ined by ’ s i z e ’ . ’ s ide ’ denotes edge o f ob s t a c l e :6 % 27 % −−−−−−−−8 % | |9 %1 | |3 and type i s ho r i z on t a l spread : 1 , v e r t i c a l : 2

10 % | |11 % −−−−−−−−12 % 413 % 24/02/2012 − J . Whittington − Uni e r s i t y o f Bath14 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−15 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−1617 maxtn = 25 ; %max t (n) s i z e o f f i e l d18 mintn = 0 ; %min t (n) s i z e o f f i e l d19 spread = maxtn/ s i z e ; %c a l c u l a t e spread20 f i e l d ( 1 , : ) = maxtn:− spread : mintn ; %bui ld the f i e l d2122 %apply the f i e l d23 f o r i = n s i z e : n s i z e : s i z e24 i f ( ( s i d e == 1) | | ( s i d e == 4) )25 x f i e l d = x − i ;26 y f i e l d = y − i ;27 i f ( x f i e l d < 1)28 x f i e l d = 1 ;29 end30 i f ( y f i e l d < 1)31 y f i e l d = 1 ;32 end33 i f ( x f i e l d > n x )34 x f i e l d = n x ;35 end36 i f ( y f i e l d > n y )37 y f i e l d = n y ;38 end39 e l s e i f ( s i d e == 3 | | s i d e == 2)40 x f i e l d = x + i ;41 y f i e l d = y + i ;42 i f ( x f i e l d > n x )43 x f i e l d = n x ;44 end45 i f ( y f i e l d > n y )46 y f i e l d = n y ;47 end48 i f ( x f i e l d < 1)49 x f i e l d = 1 ;50 end51 i f ( y f i e l d < 1)52 y f i e l d = 1 ;53 end54 end5556 i f type == 157 cspace ( x f i e l d , y , 4 ) = cspace ( x f i e l d , y , 4 ) + f i e l d (1 , i ) ;58 e l s e i f type == 259 cspace (x , y f i e l d , 4 ) = cspace (x , y f i e l d , 4 ) +f i e l d (1 , i ) ;60 end61 end


C.2.7 obstacle roulette.m

1 func t i on [ new obs , o b s f l a g ] = ob s t a c l e r o u l e t t e ( n x , ns i ze , chance , path ,near path )

2 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−3 % Function which runs the random obs t a c l e generator , with a p r obab i l i t y4 % between 1 and ’ chance ’ that i t w i l l run5 %6 % 01/03/2012 − J . Whittington − Unive r s i ty o f Bath7 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−8 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−9

10 %i f to i gnore c l o s e to path input s t a t e s11 i f narg in < 412 path = 0 ;13 near path = 0 ;14 end1516 %chance must be a l e a s t 517 i f chance < 518 chance = 5 ;19 end2021 %load the ba r r e l22 gun = randi ( chance , 1 ) ;2324 %5 = l i v e round , i f gun = 5 , generate a new obs t a c l e in the cspace25 i f gun == 526 i f near path == 127 rand index = randi ( [ 8 ( s i z e ( path , 1 )−8) ] , 1 ) ;28 new obs = [ f l o o r ( path ( rand index , 1 ) ) , f l o o r ( path ( rand index , 2 ) ) , randi (5 , 1 )

, 1 , 1 ] ;29 e l s e30 new obs = ob s t a c l e s (1 ,1 , n x , n s i z e ) ; %run the random obs t a c l e generator31 end32 ob s f l a g = 1 ; %f l a g that a new obs t a c l e has been generated33 e l s e34 ob s f l a g = 0 ; %no new obs t a c l e ; don ’ t bother checking path35 new obs ( 1 , : ) = 0 ;36 end


C.2.8 path smoother.m

1 func t i on [ path smooth ] = path smoother ( path , cspace , ctp )2 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−3 % Function which uses the l i n e o f s i gh t func t i on to c r ea t e any−angle paths4 % from a node to node generated path5 %6 % 01/03/2012 − J . Whittington − Uni e r s i t y o f Bath7 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−8 % −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−9

10 %New smoothed path row inc11 k = 1 ;12 %Star t at the very begining , a very good p lace to s t a r t . .13 path smooth (k , : ) = path ( s i z e ( path , 1 ) , 1 : 2 ) ;1415 %Move along path16 f o r i = ( s i z e ( path , 1 ) ) :−1:217 %Once the l i n e o f s i gh t becomes blocked18 i f l i n e o f s i g h t ( path smooth (k , 1 ) , path smooth (k , 2 ) , path ( i −1 ,1) , path ( i −1 ,2) ,

cspace , ctp , 0 ) == 019 k = k+1; %move to next row of new path20 path smooth (k , : ) = path ( i , 1 : 2 ) ; %and add the l a s t c l e a r s i gh t ed node to

array21 end22 end2324 %Add f i n a l po int25 k = k+1;26 path smooth (k , : ) = path ( 1 , 1 : 2 ) ;2728 end

Documents

Final Year MEng Project Report MATLAB A* Based Path Planner for Autonomous …engineer.john-whittington.co.uk/wp-content/uploads/201… · · 2016-12-13University of Bath Department