File Exchange

image thumbnail


version 2.0.0 (224 KB) by Einar Ueland
An update of the Astar algorithm will be posted here


Updated 03 Nov 2019

GitHub view license on GitHub

Can handle any heigth and width of occupancy grid? YES
Possible to specify multiple goal nodes? YES.
Fast and efficient? YES.
Possible to specify connecting distance to other nodes? YES (in other words the algorithm is not restriced to 8-directions)
There are no nested functions, subfunctions, plotters, or any other mess in the actual pathfinder script.
Algorithm has simple inputs: An occupancy grid. A goal Matrix, the start node and preffered connecting distance.
The zip file includes an example on the use of the script.

See linked GitHub page fore more information.
This code was written for a project, which I have written a paper about:

This paper provides some more details on the code, and how it can be applied in a practical example. If you use the code for academic work/publishing I will appreciate if you could cite the above paper.

There will probably soon come an update, which among other, will include 3D pathfinding. If you liked the code, please give it a positive rating.

Cite As

Einar Ueland (2020). Astar-Algorithm (, GitHub. Retrieved .

Comments and Ratings (46)

Einar Ueland

@Manoela Pizyblski
The easiest is to preprocess your map. If p is your safety distance, then all free cells that are within a distance p to a object cell should be considered closed as well.

@Domi;. Basically the cost of cells near objects should be higher. (I used the cell-cost w=1+5./(0.1+d), where d is the distance to objects). In the algorithm, the cell-cost is then multiplied with Euclidian distance transversed over the cell.
This is right now not implemented in the published version. I will include it on the GitHub Project later.

Hi Einar,

I'd like to know if there's any parameter which helps me to set a minimum distance between the calculated route and the obstacles (e.g. a car width).

Thanks in advance.


Works well and code is clearly understandable! Perfect for a student to get in touch with path finding algorithms in matlab.
1 Question left: How can one integrate a weightening, so the path gets smoother as well as objects will not directly be touched by the path?

Di Ao

Carlo Karam

Bo Liu

shufu yuan


Einar Ueland

Seems doable, but you probably need to remap your parameters. ProbabilitySaturation should be mapped to either 0 or 1 using your Thresholds.
As it does not directly relate to the code, I don't think I can help you any further on this.

The map I'm using is generated from a pgm file using gmapping in ROS. I have managed to get the properties of the map generated after using the robotics.OccupancyGrid function in matlab. My problem is I can't seem to use these properties in your code, since you manually created the map. Is there any work around this? I'm sorry if I'm not making any sense.

OccupancyGrid with properties:

OccupiedThreshold: 0.6500
FreeThreshold: 0.2000
ProbabilitySaturation: [0.0010 0.9990]
GridSize: [181 181]
Resolution: 20
XWorldLimits: [0 9.0500]
YWorldLimits: [0 9.0500]
GridLocationInWorld: [0 0]

Einar Ueland

Im not certain what you mean. The size can be set as you like and is defined in the first line of the example file

Hi Einar,
I am an undergrad student who is currently using your code to implement A-star algorithm for an occupancy grid map obtained from gmapping in ROS. I am new to both MATLAB and ROS and have just been following the tutorials provided in MATHWORKS for occupancy grids.

My question: how can I obtain the height and width of the occupancy grid map to use this in the example you provided? I can send you the m file of what I have so far but not sure how to go about doing this?

Tim König

1-In this for loop we find the Heuristic of each cell in the grid, one at a time. So Mat= [dX,dY] is the distance to goal cells in X and Y direction, The repmat just repeats the current coordinate, such that we can compare against all goalcells at the same time (in case there is more than one goal cell). In the next line these we use this to find the Euclidian distance, where the "min" ensure that we use the distance to the closest goal as the Heuristic.

2-This is a somewhat messy way to check that the node we want to expand can be reached from the current node. It does so by checking that each node that is crossed is not a closed cell. The loop over K is supposed to ensure that we check all nodes that need to be Clear.

Since this is something that is repeated a lot, a more efficient way to check this would be to just to set up all potential cases in a table which would output the nodes needed to be clear. I may include this in a future update, where I plan to include a lot of new features (3D, weighted node-cost, preprocessing for increased search efficiency,. etc.). However I never seem to get the inspiration/time to finish and upload this update).

Hi Einar,
I am a master's thesis student who has used your code to implement a pathfinding algorithm but I didn't understand few lines:
- Mat = RegisteredGoals-(repmat([uint16(j) uint16(k)],(Nodesfound),1));
- the usage of jumpcells and why we have that formulas for XPOS and YPOS(how you get that formula):
if (abs(i)>1||abs(j)>1);
% Need to check that the path does not pass an object
for K=1:JumpCells
Would you mind explaining me these two things?
Of couse i will cite you on my thesis.

Nay U


It help me a lot!

Kai Zhang

Nay U

Einar Ueland

@lunatic. Hi, the map is printed using a built in function, which has simple inputs so I am not really sure where the problem might be. Check that your input to imagesc(-) is the one you expect. Though it should not be necessary, you might also consider checking that the axis (xlim and ylim) is set according to the size of your map. If you want to you can send me your code and I can quickly see if I find the problem.


Hello! I have loaded an image map and after converting it into a binary one, i ve set one start and one goal point. The code finds the corect path successfully and prints it ok on the map, but the output image map is missing a part comparing it to the input one. Do you have any idea which might be the problem?

Einar Ueland

@Norris. Its is basicly just defining a grid that during planning, defines the which cells the code will investigate the cost to, from the current cell. The connecting distance relates to the size of this grid. The coding for it quite simple. Try setting showNeighboors=1 (line 91 i think) and I have included some figures that may help you understanding it.


Hello, Einar Ueland.
Coudl you please explain a little bit on why the variable "connecting distance" is able to change the number of search directions?

Md. Munjure

Hi Einar Ueland,
Your code includes very good explanation. Thanks for your suggestion (1). I just need to assign different cost to different links.
If you can help me, then please knock me :

Einar Ueland

@Md. Munjure
(1). If you then have only one goal node, then simply set multiple start positions in the goal-matrix and set the goal as the start position.
(2) Unfortunally not possible in this Version of the upload (1.02). I have a version of this script on my local computer where one can specify the cost of crossing each node, and I am planning on including it in a future update (it needs some cleaning up before I can post it).

Md. Munjure

Hi Einar Ueland, Thanks for your code. I have two questions,
(1) How can I use multiple source nodes as start positions?
(2) How to assign different cost values to different links?

Thank you; please let us know if a 3D version becomes available.

Einar Ueland

@Michael Gauthier
(1) While searching for paths, the algorithm keeps track of only one path to each cell it has opened. The path to an individual cell is only updated if the search has found a lower-cost path to it.
If a new path is found to have the same cost as the old one, the path is not updated.

The answer is therefore that the algorithm will always find a shortest path. If more than one equally optimal paths exist, only the first of these that the algorithm found are stored and shown.

(2) Hi, unfortunately not. However I think I could transfer the current setup to 3d quite easily. I will probably look into it soon.

Perfect and easy to use. Two questions:
(1) What happens if there are two equally optimal paths, are both displayed, or is one chosen over the other based on some logic?
(2) Do you have a version that works in 3-dimensions?




Connected to GitHub, and did some updates.
Version 1: As before
Version2: Using sparse representaiton, 2-Sided A*, new method for finding neighbooring nodes.
In summary a bit faster than previous version, especially with large occupancy grids.

Changed description.

Changed description

Changed description

MATLAB Release Compatibility
Created with R2015b
Compatible with any release
Platform Compatibility
Windows macOS Linux