Robot Path Planning using grid

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

Answer 1

[Walter Roberson]

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.

[Ken]

Thanks. Changed min ==.... to mn==... but then it goes into an endless loop. Think problem may be with the 2 "for" statements but don't know what.

[Walter Roberson]

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.

[Ken]

Tried it but get "Subscript indices must either be real positive integers or logicals."

[Walter Roberson]

    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]

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]

Your line

[path(i,1),path(i,2)]=SearchGoal;

needs to be

path(i,1:2)= SearchGoal;

[Ken]

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]

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.

http://www.cs.nott.ac.uk/~psznza/G52ADS/dijkstra1.pdf

[Ken]

Thanks but not much use as my level is too low to comprehend Djikstra.

[Walter Roberson]

It would be easiest if you posted the exact assignment question.

[Ken]

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

[Ken]

After asking me to post the exact assignment - suddenly silence!

[Walter Roberson]

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.

[Ken]

I really don't know what you are asking. This is the exact quote from the assignment and many have completed it based on this.

[Walter Roberson]

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]

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]

Here.

[Ken]

Works like a charm! Thanks. May have some questions where I don't understand.

[Ken]

Could not understand the code i.e. QUESTION_308547 - what does this do?

[Walter Roberson]

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]

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]

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]

Thanks a lot, Walter.

[Ken]

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]

Sounds like you used min as a variable name.

[Ken]

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]

Change

               [r,c]=find(subarray==min);

to

               [r,c]=find(subarray==mn);

[Ken]

Thanks Walter - Ken

[Ken]

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]

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.