Import and Adapt any image to a 3D plot grid for a path planning problem

Currently working on a 3D path planning algorithm for a project,to learn path planning,I took a 2D tutorial A* algorithm, adapted it to my project circumstances(changed the expansion n number to 3 and added a dimension to have 3D path planning algorithm).

I would like to further upgrade this code into a non binary 1 or 0 obstacle path planning with a global function cost, since I didn't find any base algorithm on the internet.

While I'm pretty sure I can adapt the algorithm from binary obstacles to function cost constraint weights I would first want to know if it is possible to import any kind of map( for now 2D and with different colours and shapes to help me implement a costraint cost) and transform it into a grid based image that matlab can read and feed to the algorithm to exploit.

The algorithm for now uses a matrix generated from this code:

%DEFINE THE 2-D MAP ARRAY

MAX_X=10;

MAX_Y=10;

MAX_VAL=10;

%This array stores the coordinates of the map and the

%Objects in each coordinate

MAP=2*(ones(MAX_X,MAX_Y));

all of the nodes will be stored as coordinates and maybe later into indexes, the code will surely be used in 3D where one of the dimension is a derivative of the one of the dimensions(angle and angular rate) so I'm also pretty confused on how to implement that but it's a question for another time.

I'm kind of a beginner in MATLAB sorry if this is a trivial question and thank you for whoever takes the time to answer.

The very useful and easy to manipulate base algorithm comes from here:

Paul Premakumar (2023). A* (A Star) search for path planning tutorial (https://www.mathworks.com/matlabcentral/fileexchange/26248-a-a-star-search-for-path-planning-tutorial), MATLAB Central File Exchange. Retrieved May 11, 2023.

Answer 1

[Shivam Malviya]

Hi Moatassem,

It is my understanding that you are interested in using A* to plan a path in a 3D map that has non-binary occupancy values. You are also looking for ways to transform maps of various kind into a suitable input for the path-planning algorithm.

Starting from MATLAB R2023a, the Navigation Toolbox offers "plannerAStar" and "navGraph". Together, these can be used for nD path planning. An nD map can be modelled using "navGraph", which can then be provided as input to the "plannerAStar". The following example uses "plannerAStar" and "navGraph" to plan a path in a 3D map.

% Create a 3D graph with 10 nodes and 15 edges
nodes = [0 0 0; 1 0 0; 2 0 0; 3 0 0; 4 0 0; ...
         2 1 0; 2 -1 0; 2 -1 -1; 2 -1 1; 2 -2 -1];
edges = [1 2; 2 3; 3 4; 4 5; ...
         3 6; 6 7; 3 8; 8 9; 9 10];
weights = [1;1;1;1; ...
           sqrt(2);sqrt(2); sqrt(2); ...
           sqrt(3); sqrt(3)];
       
% Create a navGraph object from the nodes and edges
graph = navGraph(nodes,edges,'Weight',weights);

% Create a plannerAStar object from the graph
planner = plannerAStar(graph);

% Define the start and goal nodes
start = 1;
goal = 10;

% Find the shortest path using the plannerAStar object
[pathOutput,solutionInfo] = plan(planner,start,goal);

% Visualize the graph and the path
h = show(graph);
set(h,'XData',graph.States.StateVector(:,1), ...
      'YData',graph.States.StateVector(:,2), ...
      'ZData',graph.States.StateVector(:,3))
pathStateIDs = solutionInfo.PathStateIDs;
highlight(h,pathStateIDs,'EdgeColor','#EDB120','LineWidth',4)
highlight(h,pathStateIDs(1),'NodeColor','#77AC30','MarkerSize',5)
highlight(h,pathStateIDs(end),'NodeColor','#D95319','MarkerSize',5)

Refer to the following links for a better understanding;

• https://in.mathworks.com/help/nav/ref/plannerastar.html
• https://in.mathworks.com/help/nav/ref/navgraph.html