Robot Path Planning using grid

Question

Ken 2016-10-22

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/308547-robot-path-planning-using-grid

评论： Walter Roberson 2016-11-30

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

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Walter Roberson 2016-10-22

1
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/308547-robot-path-planning-using-grid#answer_240147

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 个评论
显示 28更早的评论隐藏 28更早的评论

Walter Roberson 2016-10-24

在 MATLAB Online 中打开

    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 2016-10-24

编辑：Walter Roberson 2016-10-25

在 MATLAB Online 中打开

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.

Ken 2016-10-25

编辑：Walter Roberson 2016-10-25

在 MATLAB Online 中打开

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"

Ken 2016-10-25

编辑：Walter Roberson 2016-10-25

在 MATLAB Online 中打开

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 2016-11-30

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 2016-11-30

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.

请先登录，再进行评论。