You are now following this question
- You will see updates in your followed content feed.
- You may receive emails, depending on your communication preferences.
Robot Path Planning using grid
5 views (last 30 days)
Show older comments
Trying to plot a path for a robot from start to goal. I get error in the 2 "for" for x,ystatements. Get error "Subscript indices must either be real positive integers or logicals". Program is:
SearchSolution=[1.0000 0.9705 0.9513 0.9471 0.9557 0.9661 0.9770 0.9883;...
1.0000 0.9629 0.9403 0.9418 0.9629 0.9744 0.9833 0.9916;...
1.0000 0.9581 0.9350 0.9451 1.0000 1.0000 1.0000 1.0000;...
1.0000 0.9534 0.9219 0.9271 1.0000 1.0000 1.0000 1.0000;...
1.0000 0.9487 0.8997 0.8593 0.8349 0.8100 0.8635 0.9331;...
1.0000 0.9574 0.8886 0.8000 0.6815 0.5499 0.7154 0.8711;...
1.0000 1.0000 0.9171 0.7871 0.5575 0 0.5830 0.8391];
OptimalPath=[2,7];
CurrentPos=[2,7];
min=99;
SearchGoal=[7,6];
while not(isequal(CurrentPos,SearchGoal))
for x=SearchSolution(OptimalPath(end,1)-1:OptimalPath(end,1)+1),
for y=SearchSolution(OptimalPath(end,2)-1:OptimalPath(end,2)+1),
[r,c] = find(min == min(SearchSolution(:)));
end
end
CurrentPos=[r,c];
OptimalPath=[OptimalPath;CurrentPos];
end
end
Accepted Answer
Walter Roberson
on 22 Oct 2016
You are using min as a variable, but you are also trying to use min as a function call. As soon as you assigned a value to min, min stops being a function call, so instead of being a function call on a set of floating point values, it is an attempt to index the variable at those floating point values.
30 Comments
Walter Roberson
on 22 Oct 2016
Edited: Walter Roberson
on 22 Oct 2016
Your statement
[r,c] = find(min == min(SearchSolution(:)));
is writing over all of r and c on each iteration. You might as well only do the last iteration if that was your intention.
SearchSolution(:) is all of SearchSolution, so your find() does not depend upon your x or y, so there really is no point doing those loops.
Perhaps you wanted something like
x = OptimalPath(end,1)-1:OptimalPath(end,1)+1);
y = OptimalPath(end,2)-1:OptimalPath(end,2)+1);
subarray = SearchSolution(x, y);
mn = min(subarray(:));
[r, c] = find(subarray == mn);
r = r + OptimalPath(end,1) - 2;
c = c + OptimalPath(end,2) - 2
without the for loops.
Walter Roberson
on 24 Oct 2016
SearchSolution=[1.0000 0.9705 0.9513 0.9471 0.9557 0.9661 0.9770 0.9883;...
1.0000 0.9629 0.9403 0.9418 0.9629 0.9744 0.9833 0.9916;...
1.0000 0.9581 0.9350 0.9451 1.0000 1.0000 1.0000 1.0000;...
1.0000 0.9534 0.9219 0.9271 1.0000 1.0000 1.0000 1.0000;...
1.0000 0.9487 0.8997 0.8593 0.8349 0.8100 0.8635 0.9331;...
1.0000 0.9574 0.8886 0.8000 0.6815 0.5499 0.7154 0.8711;...
1.0000 1.0000 0.9171 0.7871 0.5575 0 0.5830 0.8391];
OptimalPath=[2,7];
CurrentPos=[2,7];
SearchGoal=[7,6];
numrow = size(SearchSolution, 1);
numcol = size(SearchSolution, 2);
while not(isequal(CurrentPos,SearchGoal))
x = max(OptimalPath(end,1)-1, 1) : min(OptimalPath(end,1)+1, numrow);
y = max(OptimalPath(end,2)-1, 1) : min(OptimalPath(end,2)+1, numcol);
subarray = SearchSolution(x, y);
mn = min(subarray(:));
[r, c] = find(subarray == mn);
r = r + x(1) - 1;
c = c + y(1) - 2;
CurrentPos=[r,c];
OptimalPath=[OptimalPath;CurrentPos];
end
If you execute this, you will notice that it infinite loops. This is due to a bug in your algorithm. Your algorithm does not prevent any location being visited a second time, including not preventing the located next step from being to the location you are already at. Your algorithm will always get stuck if there is a local minima. Your algorithm also has problems if the local minima in the sub-array is duplicated, because you ask to find() all of the locations in the sub-array that are equal to the local minima.
Any algorithm that step-wise decides on the next step without looking further on is going to produce non-optimal results. Consider
x.9 0.4 0.4 0.4
0.2 0.2 0.5 0.2
0.4 0.6 0.3 0.8
y.7 1.0 1.0 1.0
Target is y.7
Start at the x.9 node. Lowest from there is either of the 0.2 .
If you take the "down" one, 0.2@(2,1) then the lowest from there is the other 0.2@(2,2) and from there you would 0.3@(3,3) then 0.2@(4,2) then 0.5@(2,3). From there, no matter which of the 0.4 in the first row you choose, you are stuck, as you have already visited all of the nodes in row 2 and so cannot get out of the 0.4 stretch on top.
If instead of going to 0.2@(2,1) originally, you take the "right down" 0.2@(2,2) then from there you would go to 0.2@(2,1) then 0.4@(3,1) then y.7@(4,1) . You made it, at a cost of 0.2 + 0.2 + 0.4 + 0.7 = 1.5.
But clearly the optimal path is 0.2@(2,1), 0.4@(3,1), y.7@(4,1) = 0.2 + 0.4 + 0.7 = 1.3 .
I will not fix your code because it requires a completely new algorithm, and you are responsible for the algorithm.
Remember:
- heading to a local minima can take you away from your goal.
- two small steps can be more expensive than one larger step.
Ken
on 24 Oct 2016
Edited: Walter Roberson
on 25 Oct 2016
Thanks. Based on this, I changed my algorithm to:
i=1;
[path(i,1),path(i,2)]=SearchGoal;-
while(currentCell ~= SearchStart)
% You should set currentCell
value inside the loop, while you
traverse the grid map:
currentCell = [path(i,1), path(i,2)];
%Take the cell where you are right now and its 8 neighbours, and store it in a variable
u=currentCell(1)
v=currentCell(2)
% Just in case, I would try to
verify if you have reached the
SearchStart cell before entering
the last block of code of the loop:
if(currentCell ~= SearchStart)
neighborhood8 = SearchSolution(v-1:v+1, u-1:u+1);
%Assign an Inf value to
the cell you are on right now, so
that it doesn't interfere when
finding the minimum of the
neighborhood array
neighborhood8(2,2) = Inf;
[path(i+1,1), path(i+1,2)] =
find(neighborhood8==min(min
(neighborhood8)));
%Increment the index of the
OptimalPath array last element
i = i + 1;
end
end
OptimalPath = path();
Now I get error - Insufficient number of outputs from right hand side of equal sign to satisfy assignment.
Walter Roberson
on 25 Oct 2016
Your line
[path(i,1),path(i,2)]=SearchGoal;
needs to be
path(i,1:2)= SearchGoal;
Ken
on 25 Oct 2016
Edited: Walter Roberson
on 25 Oct 2016
Thanks.
Changed to:
i=1;
path(i,1:2)=SearchGoal;
currentCell=[path(i,1:2)];
while(currentCell ~= SearchGoal)
% You should set currentCell value inside the loop, while you
traverse the grid map:
currentCell = [path(i,1:2)];
%Take the cell where you are right now and its 8 neighbours, and store it in a variable
u=currentCell(1)
v=currentCell(2)
% Just in case, I would try to verify if you have reached the
SearchStart cell before entering the last block of code of the loop:
if(currentCell ~= SearchStart)
neighborhood8 = SearchSolution(v-1:v+1, u-1:u+1);
%Assign an Inf value to the cell you are on right now,
so that it doesn't interfere when finding the minimum of the
neighborhood array
neighborhood8(2,2) = Inf;
[path(i+1,1), path(i+1,2)] = find(neighborhood8==min(min(neighborhood8)));
%Increment the index of the OptimalPath array last element
i = i + 1;
end
end
Still no luck! Error - "elements within OptimalPath are wrong"
Walter Roberson
on 25 Oct 2016
Assigning inf just one method of preventing the path from visiting the same node twice. As I demonstrated above, that is not enough for optimal planning. "Greedy" algorithms do not work for shortest-path unless there are mechanisms to try alternate routes.
Walter Roberson
on 25 Oct 2016
It would be easiest if you posted the exact assignment question.
Ken
on 25 Oct 2016
Edited: Walter Roberson
on 25 Oct 2016
Thanks.
The part including SearchSolution is completed. What's remaining is to exract the solution path as an n by 2 matrix within the variable 'Optimal Path'. Its first column should correspond to cells' x coordinates, and the second column to their y coordinates. Furthermore, the first row of 'OptimalPath' should correspond to the Robot's start position, while the n-th row should correspond to the goal location. When evaluating neighbor cells, make sure to check all eight neighbors, not just the four straight ones:
tol = 0.01;
maxIter = 50;
% initialize map
Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) = -1; Map(:,1) = -1; Map(:,9) = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;
% initialize search start and goal locations
SearchStart = [3,7];
SearchGoal = [9,6];
% initialize iterative search
SearchSolution = zeros(size(Map));
SearchSolution(Map==-1)=1; %set obstacle cells to "1"
SearchSolution(Map==0) =0.5; %set free cells to "0.5"
SearchSolution(SearchGoal(1),SearchGoal(2)) = 0;
% iteratively solve the discrete Laplace Equation with Dirichlet boundary conditions
iter = 0; maxChange = inf;
while maxChange > tol
iter = iter+1;
assert(maxIter>iter, 'maxIter assert triggered. Aborting.');
NextSearchSolution = SearchSolution;
for x=1:1:size(Map,1)
for y=1:1:size(Map,2)
if and(SearchSolution(x,y)~=0,SearchSolution(x,y)~=1)
NextSearchSolution(x,y) = 1/4*(SearchSolution(x-1,y) + ...
SearchSolution(x+1,y) + ...
SearchSolution(x,y-1) + ...
SearchSolution(x,y+1) );
end
end
end
maxChange = max(max(abs(SearchSolution-NextSearchSolution)));
SearchSolution = NextSearchSolution;
end
i=1;
path(i,1:2)=SearchGoal;
currentCell=[path(i,1:2)];
while(currentCell ~= SearchGoal)
% You should set currentCell value inside the loop, while you traverse the grid map:
currentCell = [path(i,1:2)];
%Take the cell where you are right now and its 8 neighbours, and store it in a variable
u=currentCell(1)
v=currentCell(2)
% Just in case, I would try to verify if you have reached the SearchStart cell before entering the last block of code of the loop:
if(currentCell ~= SearchStart)
neighborhood8 = SearchSolution(v-1:v+1, u-1:u+1);
%Assign an Inf value to the cell you are on right now, so that it doesn't interfere when finding the minimum of the neighborhood array
neighborhood8(2,2) = Inf;
[path(i+1,1), path(i+1,2)] = find(neighborhood8==min(min(neighborhood8)));
%Increment the index of the OptimalPath array last element
i = i + 1;
end
end
Walter Roberson
on 29 Oct 2016
You wrote "The part including SearchSolution is completed."
That is the part I was assisting you with.
"my level is too low to comprehend Djikstra"
"iteratively solve the discrete Laplace Equation with Dirichlet boundary conditions"
Djikstra is much simpler than discrete Laplace Equation with Dirichlet boundary conditions. I do not know how to solve the discrete Laplace Equation, so I do know understand how your revised code works. How to modify your current algorithm is pretty much a different question than you had been asking.
"After asking me to post the exact assignment - suddenly silence!"
You still have not posted the exact assignment: you have posted code intended to solve an assignment, but without the exact wording of the assignment we cannot offer an opinion as to whether it does what needs to be done.
Walter Roberson
on 29 Oct 2016
You are saying that "The part including SearchSolution is completed" is a literal quote from the assignment ?
I was expecting text along the lines of "Using such-and-such an algorithm and given this-and-that, find a path between P and Q that satisfies these conditions...".
In any case I still do not know anything about how discrete Laplace Equation works.
Ken
on 29 Oct 2016
Edited: Walter Roberson
on 29 Oct 2016
Thanks.
To simplify, I have a matrix A, my starting point in this matrix is A(2,3)which has a value of 10. I want to find a path called Optimal Path which holds the x,y coordinates to get to SearchGoal which is A(5,4) which has a value of 0. I have to find a path using a 3X3 matrix which is a subset of A. My first step is to pick the 3X3 matrix centred at SearchStart, so x-1:x+1, y-1:y+1 i.e. 1,2 to 3,4. I find the min of this 3X3 matrix and add the cords of this min to SearchStart and insert these cords in OptimalPath. This would be the centre of my next 3X3 matrix and I would proceed in this way till I reach SearchGoal, so the last pair of cords in OptimalPath would be SearchGoal.:
A = [ 17 2 24 14 8;
11 23 10 10 4;
11 12 20 7 15;
6 9 22 13 5;
25 3 21 0 7];
OP=[2,3];
SG=[5,4];
Walter Roberson
on 16 Nov 2016
You do a min() within a sub-matrix of the original matrix and get back a value. Your algorithm then asks for the row and column of where the value was located within the sub-matrix. Then the code has to translate the row and column relative to the sub-matrix into being row and column relative to the entire large matrix, by adding on the known beginning corner of the sub-matrix.
Your algorithm is incorrect in multiple ways, and the handling of locating the minimum is one of the ways, but you made it clear earlier that you were not concerned with a generally correct algorithm, so I made no attempt to fix those bugs.
Ken
on 17 Nov 2016
Thanks Walter.
Just trying to understand the following lines:
[NUMBER,QUESTION_308547]=find(FROM==MATLAB_ANSWERS);
NUMBER=NUMBER+WAS(1)-1;
QUESTION_308547=QUESTION_308547+COPIED(1)-1;
CurrentPos=[NUMBER,QUESTION_308547];
OptimalPath=[OptimalPath;CurrentPos];
SearchSolution(NUMBER,QUESTION_308547)=inf;
end
Walter Roberson
on 17 Nov 2016
Your algorithm then asks for the row and column of where the value was located within the sub-matrix. Then the code has to translate the row and column relative to the sub-matrix into being row and column relative to the entire large matrix, by adding on the known beginning corner of the sub-matrix. Then you record the position and set the cost there to inf so that it is not visited again.
Ken
on 18 Nov 2016
Edited: Walter Roberson
on 18 Nov 2016
I tried (just for learning) using your code based on your post of Oct 22 but get error
"Subscript indices must either be real positive integers or logicals.
Error in Untitled10 (line 50)
min(subarray(:))"
while not(isequal(OptimalPath(end,:),SearchGoal))
x=OptimalPath(end,1)-1:OptimalPath(end,1)+1;
y= OptimalPath(end,2)-1:OptimalPath(end,2)+1;
subarray=SearchSolution(x,y);
min(subarray(:))
mn=min(subarray(:));
[r,c]=find(subarray==min);
r=r+OP(end,1)-2;
c=c+OP(end,2)-2;
CurrentPos=[r,c];
OptimalPath=[OptimalPath;CurrentPos];
SearchSolution(r,c)=inf;
end
Walter Roberson
on 18 Nov 2016
Sounds like you used min as a variable name.
Ken
on 19 Nov 2016
Edited: Walter Roberson
on 20 Nov 2016
Thanks. That has gone away but now error "not enough input arguments":
while not(isequal(OptimalPath(end,:),SearchGoal))
x=OptimalPath(end,1)-1:OptimalPath(end,1)+1;
y= OptimalPath(end,2)-1:OptimalPath(end,2)+1;
subarray=SearchSolution(x,y);
min(subarray(:))
mn=min(subarray(:));
[r,c]=find(subarray==min);
r=r+OP(end,1)-2;
c=c+OP(end,2)-2;
CurrentPos=[r,c];
OptimalPath=[OptimalPath;CurrentPos];
SearchSolution(r,c)=inf;
end
Walter Roberson
on 20 Nov 2016
Change
[r,c]=find(subarray==min);
to
[r,c]=find(subarray==mn);
Ken
on 30 Nov 2016
I am now trying to backtrack i.e. find the solution path from goal to start (as opposed to start to goal done previously). Any help appreciated.
% extract solution path from goal to start. Also check, whether % the search actually found a solution (check for "Inf" values) OptimalPath = ...;
Walter Roberson
on 30 Nov 2016
Backtracking is the same search code as going forward except exchanging the two goals. The results can be different because your code picks one of the routes when there are two possibilities of equivalent weight. That can end up leading to very different paths.
If you were doing actual dijkstra planning then there would not be a lot of point in doing backtracking, ad you would simply get one of the routes of exactly equivalent cost. But because your route finding is not optimal, backtracking might happen to find a less expensive route that could then be reversed. Or not: it might find a more expensive route instead.
More Answers (0)
See Also
Categories
Find more on Get Started with Optimization Toolbox in Help Center and File Exchange
Tags
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!An Error Occurred
Unable to complete the action because of changes made to the page. Reload the page to see its updated state.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)
Asia Pacific
- Australia (English)
- India (English)
- New Zealand (English)
- 中国
- 日本Japanese (日本語)
- 한국Korean (한국어)