Using optimization algorithms to find minimum path with obstacles

Hello

I'm trying to optimize the path in the figure trying to avoid that the resulting path passes through obstacles. The approach I'm using is to use an optimization algorithm (fminunc) that improves the cost function (this computes the total path length with the Euclidean distances between each point) while keeping the start and end points fixed. The algorithm varies the coordinates of the green points and I insert as the starting point x0 the matrix containing the green points.

Not knowing how to model each obstacle (all cubes of which I have the 8 extremes) as a constraint to avoid, I added to the cost function a function that adds a cost as a penalty if there are points inside the obstacles. However I always get the same result:

Does anyone know if there is a better method to get the desired result? Or even if it knows how to model obstacles as constraints? Then using the function fmincon.

This is the path I hope to obtain:

Answer 1

[Matt J]

You could represent the forbidden area by a polyshape object, pgon, then form a nonlinear constraint function compatible with fmincon as below. You will need to sample the edges of the region very finely, and use polyshape(___,'Simplify',false) to ensure that samples are not trimmed away by the polyshape constructor.

function [c,ceq]=nonlcon(x,pgon)

 ceq=[];

 V=pgon.Vertices(pgon.nearestvertex(x),:);
 c=vecnorm(x-V,2,2).^2 .* pgon.isinterior(x); %distance to admissible region boundary
 
 
end

[Andrea Di Mauro]

Thank you! Could you explain what is x and what c and ceq represent?

[Matt J]

x are your unknown variables. I assume them to be an Nx2 matrix of 2D points. c and ceq are as in the fmincon documentation and examples.

[Matt J]

For 3D, there are analogous routines on the File Exchange to test whether 3D points are inside a triangulated volume, e.g.,

https://www.mathworks.com/matlabcentral/fileexchange/37856-inpolyhedron-are-points-inside-a-triangulated-volume?s_tid=srchtitle

[Andrea Di Mauro]

In the end I managed to fix the version where I add a penalty to the cost function for each point inside an obstacle. Even before this, however, I had implemented nlcon based on yours and it worked the same. Thank you

Answer 2

[Matt J]

It's doubtful you can do it with fmincon, but you might be able to do it by creating a graph object to model steps between allowable locations. You could then use shortestpath to solve the problem. The code below would just need a simple modification to delete the nodes from the graph were obstacles are located.

N=20; %Represent the field as NxN grid of points

[X,Y]=ndgrid(1:N); XY=[X(:),Y(:)]; M=numel(X);


I=repmat(1:M,9,1);
[D,J]=pdist2(XY,XY,'euclidean','Smallest',9);

idx=(D>=sqrt(2)+10*eps); 
D(idx)=0; 

A=sparse(I,J,D,M,M);

G=graph(A,'omitself'); %Non-directed graph of the grid lattice

plot(G,'XData',X(:),'YData',Y(:))

[Andrea Di Mauro]

The path in red you see is already the output of an A* algorith performed on the grid built on the points of the STL files of the solid. I just wanted to optimize it because I have different costs to take into consideration in addition to the total distance