ROBOT LOCALIZATION AND MOTION PLANNING
Show older comments
The state of the robot is fully described by its position and orientation xk=[xk,yk,ϕk]T , expressed in the global coordinate frame marked with x and y . Given a control input uk=[rk,Δϕk]T , the robots first turns on the spot by an angle Δϕk and then moves in a straight line for a distance of rk in the direction it is facing.
Please implement the motion model as an anonymous function assigned to f that models this behaviour. This motion model will be employed to arrive at a prior estimate x^ k of the robot pose given a posterior estimate of the pose at a previous time instance xk−1 and control inputs uk as x^k=f(xk−1,uk). (x^k is estimate of xk). Please also provide the Jacobians with respect to the state xk−1 and the input uk as anonymous functions and assign them to Fx and Fu respectively.
1 f = @(x, u) ????;
2 Fx = @(x,u) ????;
3 Fu = @(x,u) ????;
3 Comments
Walter Roberson
on 2 Dec 2016
What is the MATLAB question here? This seems to be a doit4me
Ken
on 2 Dec 2016
Edited: Walter Roberson
on 2 Dec 2016
Accepted Answer
Walter Roberson
on 16 Jan 2017
0 votes
7 Comments
Ken
on 17 Jan 2017
Walter Roberson
on 17 Jan 2017
Sorry this was a test post that I forgot to delete.
Walter Roberson
on 17 Jan 2017
Take the pseudocode and mentally place the test for eligibility after each line. Does the test refer to values that have not been defined yet? Does the test refer to changing values that have not yet been correctly assigned their current value? Does the test refer to changing values which have been just changed in a way that makes it impossible to carry out the test correctly? What are the consequences of inserting the test at that point compared to inserting it one step later?
Ken
on 17 Jan 2017
Edited: Walter Roberson
on 18 Jan 2017
Walter Roberson
on 18 Jan 2017
You need to strictly pair the operations on Queue and Cost -- every time you push onto Queue you must push onto Cost, and every time you pop from Queue you must pop from Cost.
You still need Closed, to avoid having locations processed multiple times.
curr will always be a coordinate pair. You need to check what is stored in the map at that coordinate pair.
Ken
on 18 Jan 2017
Ken
on 20 Jan 2017
More Answers (1)
Walter Roberson
on 2 Dec 2016
Assuming your formulae are correct,
Fx = @(x,u) [1;1;1];
Fu = @(x,u) [cos(x(2)+1);sin(x(2)+1);1];
115 Comments
Walter Roberson
on 2 Dec 2016
x = sym('x',[3,1]);
u = sym('u',[2,1]);
F = [x(1)+u(1)*cos(x(3)+u(2));
x(2)+u(1)*sin(x(3)+u(2)) ; x(3)+u(2)];
Fx = matlabFunction( jacobian(F,x), 'Vars', {x,u})
Fu = matlabFunction( jacobian(F,u), 'Vars', {x,u})
Ken
on 3 Dec 2016
Edited: Walter Roberson
on 3 Dec 2016
Ken
on 3 Dec 2016
Edited: Walter Roberson
on 3 Dec 2016
Walter Roberson
on 3 Dec 2016
Your x is not a single variable, it is a vector. Taking the Jacobian with respect to x is going to give you an answer with respect to each component
jacobian(F,x)
ans =
[ 1, 0, -u1*sin(u2 + x3)]
[ 0, 1, u1*cos(u2 + x3)]
[ 0, 0, 1]
Ken
on 5 Dec 2016
Walter Roberson
on 5 Dec 2016
Generate some arrays of various sizes to return as potential Jacobians and try them, and see what size it stops complaining about the size for. Then, knowing the size it expects, we could work on getting the content right.
One variation to try:
x = sym('x',[3,1]);
u = sym('u',[2,1]);
F = [x(1)+u(1)*cos(x(3)+u(2));
x(2)+u(1)*sin(x(3)+u(2)) ; x(3)+u(2)];
Fx = matlabFunction( jacobian(F,x), 'Vars', {x.', u.'})
Fu = matlabFunction( jacobian(F,u), 'Vars', {x.', u.'})
Ken
on 8 Dec 2016
Edited: Walter Roberson
on 8 Dec 2016
Ken
on 14 Dec 2016
Walter Roberson
on 14 Dec 2016
After you extract the sub-matrix but before you do the min(), you could set the diagonal values in the sub-matrix to inf. However you need to pay attention to the boundary conditions as the extracted sub-matrix is not always going to be 3 x 3.
Ken
on 14 Dec 2016
Edited: Walter Roberson
on 15 Dec 2016
Walter Roberson
on 15 Dec 2016
What you are already doing could be called a Breadth-First Search, under the understanding that you are pruning the search at each step. But it could also be called a Depth-First Search with the same understanding.
More typically, Breadth-First Searches require some form of queuing. At any step, you have a list of nodes to be examined (the first step initialized this list to the starting node), and you run through all of those nodes finding all of the possible next steps, queuing those next steps into a list. So step #1 finds all the nodes 1 step away from the origin, step #2 finds all of the nodes 2 steps away from the origin, and so on. At each step, the number of nodes "deep" in the path is the same for all candidates.
With a Breath-First Search, you would not talk about "back-tracking". However, you do need to consider the termination conditions: you can stop on the first step in which the target is reached (taking the least expensive out of all the routes found with that many steps), but the route that has the least number of steps will not necessarily be the least expensive route. If you are looking for the least expensive route, then the search needs to continue until either you have exhausted all possible routes, or until you can prove that any further route would have to be more expensive than some known route.
A Depth-First search also requires some form of queuing. At any step you pick the least expensive of all of the queued routes that are "in focus", and you find the possible next steps from there and queue those, and shift focus to what you just queued. The queued nodes at any one time will have many different depths. At the point where you know that you cannot go any further (no more candidate nodes that are not already in the tree between the source and the current node) then you pop that focus and move back one step and take the next least expensive and focus on there. That would commonly be referred to as back-tracking.
You have to pay attention to the termination condition for Depth-First Search, too: you might have taken the least expensive route at each point, but that could have led you the long way around. Sometimes the least expensive overall route might involve going up the steepest hill, and that would not be the route that would be found first by Depth-First. For example, traveling between x and y on
333
x5y
the least expensive from x (with diagonals not allowed) would be up to the 3, then across to the second 3, then across to the third 3, then down to the y, for a path total of 9; in this case it is cheaper overall to take the more expensive 5 step.
The algorithm you were using originally has no queuing at all, and so can easily get stuck: it had no provision for saying "Oops, dead end! Let's go back a few steps and make a different decision!"
There is no trivial modification of the previous algorithm to make it Depth First or Breadth First: you have to design in the queuing (or "stack").
Ken
on 16 Dec 2016
Edited: Walter Roberson
on 18 Dec 2016
Ken
on 18 Dec 2016
Walter Roberson
on 18 Dec 2016
What is the point of
min = abs(-5);
?
And do not use 'min' as a variable name: you are almost certain to slip up and try to also use min as a function. And if you do not, then those of us trying to read your code certainly are confused by it.
Ken
on 19 Dec 2016
Walter Roberson
on 19 Dec 2016
I do not understand your code. The way you include the position information in the calculation of "min" does not make sense to me.
Ken
on 19 Dec 2016
Walter Roberson
on 19 Dec 2016
Edited: Walter Roberson
on 19 Dec 2016
Your code uses a key variable SMap which you have not defined in your previous postings, so I am not able to test your code.
Ken
on 19 Dec 2016
Edited: Walter Roberson
on 20 Dec 2016
Walter Roberson
on 20 Dec 2016
I have attached the cleaned-up source so that we are working with common code, instead of my having to guess about what code you might have.
I had to restore the initialization of "min".
I renamed "min" to "least".
I removed extra semi-colons and added semi-colons where recommended.
I did not change the algorithm.
Your code has
for x=3:1:size(SolutionMap,1)
and inside of that it has
x=least1(1)+1;
You should not change a loop control variable inside of the loop, not unless you really know what you are doing. When you change the value of a loop control variable, then as soon as you start the next iteration, the change is undone and the loop control variable is assigned whatever it would have been if you had not changed the variable.
Ken
on 20 Dec 2016
Edited: Walter Roberson
on 20 Dec 2016
Walter Roberson
on 20 Dec 2016
Including code to protect against going outside of the array:
nrow = size(SolutionMap,1);
ncol = size(SolutionMap,2);
idx = sub2ind([nrow, ncol], x, y);
candidates = [];
if x > 1; candidates = [candidates, idx - 1]; end %(x-1,y)
if x < nrow; candidates = [candidates, idx + 1]; end %(x+1, y)
if y > 1; candidates = [candidates, idx - nrow]; end %(x, y-1)
if y < ncol; candidates = [candidates, idx + nrow]; end %(x, y+1)
[least, whichleast] = min( SolutionMap(candidates) );
[newx, newy] = ind2sub([nrow, ncol], candidates(whichleast));
This code makes use of linear indexing to avoid doing a bunch of sub2ind(). It builds up a list of linear indices of locations that are permitted to be examined, finds the minimum of the map at those permitted locations, and turns the found linear index back to row and column index.
Ken
on 21 Dec 2016
Edited: Walter Roberson
on 21 Dec 2016
Walter Roberson
on 21 Dec 2016
You did not post a copy of the error.
One thing I notice is that when you read the following statements together:
if SearchGoal(1) > CurrPos(1);x = x + 1;
end
if SearchGoal(1) < CurrPos(1);x = x - 1;
end
if SearchGoal(2) > CurrPos(2);y = y + 1;
end
if SearchGoal(2) < CurrPos(2);y = y - 1;
end
then you can end up moving on the diagonal, which is prohibited in your case.
Walter Roberson
on 21 Dec 2016
Where are you modifying SearchGoal or CurrPos or SolutionMap? You always test fixed locations in SearchGoal and CurrPos but you do not modify either variable. Also you are testing values in SolutionMap but you initialized SolutionMap to all inf
Also notice
while ~isequal(SolutionMap(x,y),SearchGoal)
SolutionMap(x,y) is going to be one particular entry in SolutionMap, but SearchGoal is a vector.
Ken
on 21 Dec 2016
Walter Roberson
on 21 Dec 2016
I need an exact copy of the error message showing where it occurred.
Remember, I am working on many different questions simultaneously, so if you do not want me to skip yours for being too much trouble, you are advised to post complete error messages, and (when appropriate) complete code. I should not have to reverse-engineer your changes based upon your vague description. I might also be away from my laptop and reading through my phone, in which case I might not even have access to MATLAB at the time -- but a complete copy of the error message might allow me to figure out it mentally instead of having to run the code to see what happens.
You should always be striving to make your questions as easy as practical for the volunteers to understand.
Walter Roberson
on 21 Dec 2016
Please remove the '#' that you put at the beginning of the lines. I have removed those from your previous postings but it is time consuming to do. Your code cannot be run the way it is.
When you post code, you should highly the code section and click on the '{} Code' button, not the '1 --- 2 --- 3 ---' button.
Walter Roberson
on 21 Dec 2016
Your code
if SearchGoal(1) > CurrPos(1);x = x + 1;
(and related lines) do not check for running off the edge of the array. Those lines are probably not useful, as the code I suggested already handles looking in the appropriate directions.
Ken
on 21 Dec 2016
Walter Roberson
on 23 Dec 2016
newx, newy is the new location.
If following the minima of the 4 surrounding locations does not eventually get you to the goal, then you need a new algorithm. For example you might need an algorithm which backtracks. I described suitable breadth first search and depth first search algorithms above.
Ken
on 23 Dec 2016
Edited: Walter Roberson
on 23 Dec 2016
Walter Roberson
on 23 Dec 2016
For breadth-first search, at each step you have to make a "hypothesis" that each of the possibilities is the right way to go, so you enter into a list the chain with each of the possible moves relative to where you are focusing on. When one of the chains gets stuck, remove it from the list. Keep going until either you have found one possibility or else until you have accounted for all possibilities. If you find one possibility then it will be a version with the minimum number of steps, but it will not necessarily be the least expensive route. If you cover all of the possibilities then you can pick the least expensive amount them.
For example,
while ~isempty(active_chains)
new_chains = {};
for K = 1 : length(active_chains)
this_chain = active_chains{K};
if this_chain ends at the goal then
successful_chains{end+1} = this_chain;
else
find all steps that are immediately legal on this_chain,
discarding any potential step that would go outside the
matrix and discarding any potential step that would
re-visit a location that is already present in this_chain.
Any potential step you find,
new_chains{end+1} = [this_chain, the next step]
end
end
active_chains = new_chains;
end
Eventually you will get to the point where there are no further steps possible in any chain and so new_chains will be empty and so active_chains will become empty. At that point, successful_chains will contain all of the possible routes. You can then figure out which of them is least expensive.
Ken
on 23 Dec 2016
Edited: Walter Roberson
on 24 Dec 2016
Walter Roberson
on 24 Dec 2016
The code I gave is an implementation of that structure, pretty much.
curr = Queue.pop()
would be the same as
curr = active_chains{1};
active_chains(1) = [];
and
Queue.push(value)
would be the same as
active_chains{end+1} = value;
Walter Roberson
on 24 Dec 2016
The section
candidates = [];
if x > 1; candidates = [candidates, idx - 1]; end;%(x-1,y)
[candidates, idx - 1];
if x < nrow; candidates = [candidates, idx + 1]; end %(x+1, y)
[candidates, idx + 1];
if y > 1; candidates = [candidates, idx - nrow]; end %(x, y-1)
[candidates, idx - nrow];
if y < ncol; candidates = [candidates, idx + nrow]; end %(x, y+1)
[candidates, idx + nrow];
corresponds to expand(curr), I think.
I am not clear as to what "next not in Closed" is intended to mean. (I have an idea of what it might mean, but if my idea is correct then I have to think more about how the queue is working.)
Ken
on 24 Dec 2016
Edited: Walter Roberson
on 24 Dec 2016
Ken
on 25 Dec 2016
Walter Roberson
on 25 Dec 2016
I reformatted what you posted, making it into paragraphs.
Ken
on 26 Dec 2016
Walter Roberson
on 26 Dec 2016
Well, variable.Queue(value) for FIFO is equivalent to
variable(end+1) = value;
and variable.Pop() for a FIFO is equivalent to
result = variable(1);
variable = variable(2:end);
and "value in Closed" is equivalent to
ismember(value, Closed)
Ken
on 26 Dec 2016
Edited: Walter Roberson
on 26 Dec 2016
Walter Roberson
on 26 Dec 2016
The code I gave is not relevant. You need to follow the structure they gave with the Queue stuff.
The description says that Closed should contain all of the blocked points together with all of the places you have already visited.
The costs are not relevant either as the problem statement says to assume that the cost of each link is 1.
The code you are being asked to develop is effectively code to find a path with the smallest number of steps.
Ken
on 26 Dec 2016
Ken
on 26 Dec 2016
Edited: Walter Roberson
on 26 Dec 2016
Walter Roberson
on 26 Dec 2016
You should not be using a double for loop. You should be using the Queue structure that they told you to use.
Walter Roberson
on 27 Dec 2016
Look back at the algorithm you gave in http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_416044 . It contains no double-nested for loops. It contains
Nodes next = expand(curr)
which knows which location it is at now and looks only at the locations adjacent to it -- which as I showed before does not require any for loop to produce the candidates.
Ken
on 28 Dec 2016
Edited: Walter Roberson
on 28 Dec 2016
Walter Roberson
on 28 Dec 2016
You need to initialize
active_chains = {};
an you missed out on doing the initial Push. A push for a FIFO is equivalent to
achive_chains{end+1} = new_value;
Ken
on 29 Dec 2016
Walter Roberson
on 29 Dec 2016
Your code does
curr = active_chains{1};
before having pushed on the start information.
Ken
on 30 Dec 2016
Walter Roberson
on 30 Dec 2016
Edited: Walter Roberson
on 30 Dec 2016
A bunch of what is written in that code needs to be in the form of comments, or translated from pseudocode into MATLAB.
Walter Roberson
on 31 Dec 2016
Translate the imposed algorithm into MATLAB:
function BF(G, Start, Goal)
Queue = Queue_init_FIFO();
Closed = Queue_init_FIFO();
Queue = Queue_push_FIFO(Queue, Start);
while ~Queue_isempty_FIFO(Queue)
[curr, Queue] = Queue_pop_FIFO(Queue)
if isequal(curr, Goal)
return
end
Closed = Queue_push_FIFO(Closed, curr);
next = Graph_expand(G, curr);
for K = 1 : size(next, 1)
this_next = next(K,:);
if ~Queue_ismember_FIFO(Closed, next)
Queue = Queue_push_FIFO(Queue, this_next);
end
end
end
Now write the routines Queue_init_FIFO, Queue_push_FIFO, Queue_isempty_FIFO, Queue_pop_FIFO, Queue_ismember_FIFO, and Graph_expand
After that you just have to deal with the problem that your required algorithm doesn't return anything particular, so all you know is that you reached the goal, not how you got there.
Ken
on 2 Jan 2017
Edited: Walter Roberson
on 2 Jan 2017
Walter Roberson
on 2 Jan 2017
The lines
Queue[end + 1] = value
If Queue(1:end) = []
are not valid MATLAB code. MATLAB does not use [] for indexing, and MATLAB uses == for testing equality. Also using == [] as a comparison will always give false as the result.
The line
Queue(1) = []
is only valid if Queue exists already, in which case it deletes the first element of Queue.
Queue_ismember_FIFO needs to give back a true or false result, but instead your code just "falls off the end".
You need to write real code with real functions. And you can test them even if you do not know yet how to write the Graph_expand function.
You have a design decision to make: will your Goal and Start and curr and next values be coordinate pairs, or will they be linear indices into the array? Your choice will determine the data structure to use to represent your queue, and will determine how you write your Queue_ismember_FIFO code.
Walter Roberson
on 3 Jan 2017
Use isempty() to test if something is empty.
Queue (end + 1) = value
that will work if value is a scalar, or if you initialized Queue as a cell array and value is a cell array. However, you have chosen the style of making the value into a coordinate pair. In order to be able to store a vector into a single location like Queue(end+1) then you need to be using cell arrays. You would then use
Queue{end+1} = value;
Your line
if Queue(n) = next
is not syntactically valid in MATLAB. MATLAB uses == for comparisons.
Remember that you have defined next to be a vector, but your Queue(n) is going to be either a scalar (if you define the queue to hold scalars) or a cell array.
And remember to read carefully:
"if expression, statements, end evaluates an expression, and executes a group of statements when the expression is true. An expression is true when its result is nonempty and contains only nonzero elements (logical or real numeric). Otherwise, the expression is false."
You need to be careful using if with vectors: when you are working with vectors you should seriously consider using any() or all(), even if only to re-assure the people reading your code that you knew exactly what you wanted to happen.
Walter Roberson
on 3 Jan 2017
function tf = Queue_ismember_FIFO(Queue, value)
tf = any( cellfun(@(entry) isequal(entry, value), Queue) );
Walter Roberson
on 3 Jan 2017
function tf = Queue_isempty_FIFO(Queue)
tf = isempty(Queue);
Ken
on 3 Jan 2017
Walter Roberson
on 4 Jan 2017
You barely seem to be making any attempt.
function Queue = Queue_init_Fifo
Queue = {};
function Queue = Queue_push_FIFO(value)
Queue{end+1} = value;
function tf = Queue_isempty_FIFO(Queue)
tf = isempty(Queue);
function [value, Queue] = Queue_pop_FIFO(Queue)
value = Queue{1};
Queue(1) = [];
function tf = Queue_ismember_FIFO(Queue, value)
tf = any( cellfun(@(entry) isequal(entry, value), Queue) );
These are simple (except perhaps the ismember, which could have been done with an easy loop.)
I am not going to write the Graph_expand code for you. Or rather, I am not going to re-write the Graph_expand code for you. It is simple code: you know where you are, and you just have to check the four directions to see if they are off the edge of the matrix or if they are marked as blocked, and if they exist and are not blocked you return the index pair for each.
Ken
on 4 Jan 2017
Edited: Walter Roberson
on 4 Jan 2017
Walter Roberson
on 4 Jan 2017
No, you should not be taking min() for this situation, you should be returning all of them.
Ken
on 5 Jan 2017
Walter Roberson
on 5 Jan 2017
You had not asked a question. I presume you implemented that as a function and tested it and put everything together and tested that and everything worked as well as possible considering that the algorithm you are expected to use has no way to return a solution when it is found.
Ken
on 5 Jan 2017
Walter Roberson
on 5 Jan 2017
"Function definitions in a script must appear at the end of the file."
That should not be a problem because you should not be programming a script. Everything I showed you was in the form of a function. Put each function into its own .m (so that you can make use of the functions for future projects) and call upon the functions from code that creates your map.
Walter Roberson
on 6 Jan 2017
What is the required form for SolutionMap ?
What is your current code?
Ken
on 6 Jan 2017
Edited: Walter Roberson
on 6 Jan 2017
SolutionMap:
Code:
% initialize map, search start and goal as well as the solution 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;
SearchStart = [3,7];
SearchGoal = [9,6];
CurrPos = SearchStart;
SolutionMap = Inf*ones(size(Map)); %store g-values in here.
nrow = size(SolutionMap,1);
ncol = size(SolutionMap,2);
function BF(G, Start, Goal)
Queue = Queue_init_FIFO();
Closed = Queue_init_FIFO();
Queue = Queue_push_FIFO(Queue, Start);
while ~Queue_isempty_FIFO(Queue)
[currPos, Queue] = Queue_pop_FIFO(Queue)
if isequal(currPos, Goal)
return
end
Closed = Queue_push_FIFO(Closed, currPos);
next = Graph_expand(G, currPos);
for K = 1 : size(next, 1)
this_next = next(K,:);
if ~Queue_ismember_FIFO(Closed, next)
Queue = Queue_push_FIFO(Queue, this_next);
end
end
end
imagesc(SolutionMap)
set(gca,'dataAspectRatio',[1 1 1])
end
Walter Roberson
on 6 Jan 2017
Your code does not call BF, and your code does not change SolutionMap
Is there a reason why you set Map(11,:) =1 when all the other assignments in that section set entries to -1 ?
Ken
on 6 Jan 2017
Walter Roberson
on 6 Jan 2017
"Function definitions in a script must appear at the end of the file."
That should not be a problem because you should not be programming a script. Everything I showed you was in the form of a function. Put each function into its own .m (so that you can make use of the functions for future projects) and call upon the functions from code that creates your map.
Ken
on 6 Jan 2017
Walter Roberson
on 6 Jan 2017
It is not possible to have an error report about the position of functions in a script file if the script file does not contain any functions.
To be explict: function BF itself should be in its own .m file.
The script code you posted does not have any invocation of BF. Nowhere in the script was there BF(something, somethingelse) as required to call BF with two parameters.
Ken
on 6 Jan 2017
Edited: Walter Roberson
on 6 Jan 2017
Walter Roberson
on 6 Jan 2017
The line
function BF(G, Start, Goal)
defines the function BF, rather than calling it. Remove the word "function" in order to call BF.
I am getting somewhat frustrated with your low efforts. There are lots of examples of how to use functions. https://www.mathworks.com/help/matlab/matlab_prog/create-functions-in-files.html
Ken
on 7 Jan 2017
Ken
on 8 Jan 2017
Walter Roberson
on 9 Jan 2017
The CurrPos that you are passing in to Graph_expand is a pair of values, x and y.
Ken
on 9 Jan 2017
Walter Roberson
on 9 Jan 2017
function Queue = Queue_push_FIFO(Queue, value)
Queue{end+1} = value;
Ken
on 9 Jan 2017
Ken
on 9 Jan 2017
Edited: Walter Roberson
on 9 Jan 2017
Walter Roberson
on 9 Jan 2017
The line of code I provided was
Queue = Queue_init_FIFO();
You are failing to assign the output of Queue_init_FIFO to Queue.
Ken
on 9 Jan 2017
Ken
on 9 Jan 2017
Ken
on 10 Jan 2017
Walter Roberson
on 10 Jan 2017
You did not run the function. To test it you need something like
Queue = {2,1,3};
value = 3;
result = Queue_ismember_FIFO(Queue, value)
Remember, when you define a function, the variable on the left of the "=" of the "function" statement is not assigned to in the calling routine unless you deliberately assign it there.
Ken
on 10 Jan 2017
Ken
on 10 Jan 2017
Ken
on 11 Jan 2017
Walter Roberson
on 11 Jan 2017
You lost an "end" statement in BF.
I had given code for function Queue_init_Fifo instead of the required Queue_init_FIFO, a typo on my part; it appears you noticed and fixed that yourself.
When you created G_EXP you lost the sub2ind() that creates idx. But you should probably just rewrite your G_EXP in terms of building rows of x y pairs instead of indices. Remove the "return" statements while you are there, since you need to construct the complete list. And remember you need to create the output variable, which you have named "next".
You should be passing your Map into BF, not the SolutionMap. Your SolutionMap does not have any information about which places are blocked. The SolutionMap should be an output from the algorithm, but the BF algorithm you were told to use does not have any way to set the SolutionMap entries.
Ah, a bug in my code: in BF, the line
if ~Queue_ismember_FIFO(Closed, next)
should be
if ~Queue_ismember_FIFO(Closed, this_next)
Walter Roberson
on 11 Jan 2017
The algorithm you gave in http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_416044 has some flaws:
- It does not check that the current node is eligible for expansion (the expand routine is not intended to do that in the algorithm). This would result in incorrect paths
- the algorithm does not give any clue as to how to create the solution map
- Potential nodes are checked for being closed at the time of expand(), but are not checked when they become current. Nodes can end up being closed after queuing if they become current and are found to be blocked; and whether or not they are found to be blocked, the algorithm defines that they get queued as closed when they become current. Because of this algorithm set-up, a node can end up queued twice. It is inefficient to expand the same node twice. The algorithm should either re-check for closed when a node becomes current or else it should avoid entering anything into the queue when it is already in the queue.
It could make sense to have a node in the queue multiple times if the step costs are variable, or if you were not using a breadth-first search, as the alternate entries could potentially represent a less-expensive route, but in the case of a breadth-first search with constant step cost, it is not possible that the other route might be less expensive.
To deal with the creation of the solution map, I found it effective to add a new queue for the costs. Initialize that queue with a 0. Each time you pop off something from Queue, also pop the current cost from the cost queue. Each time you add something onto the Queue, also queue (1 more than the current cost). When a location becomes current and is not a location that is blocked, then set the solution map at that location to be the current cost (that was popped off the cost queue.) At each point, the N'th element of the Queue and the N'th element of the cost queue are paired, representing the cost to reach that location.
Ken
on 12 Jan 2017
Ken
on 12 Jan 2017
Edited: Walter Roberson
on 13 Jan 2017
Ken
on 13 Jan 2017
Walter Roberson
on 13 Jan 2017
Your G_Exp1 looks plausible.
In http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_416142 you wrote,
"However, the algorithm selects the next cell and if that cell is not to be expanded (because it may be an obstacle or may have been previously expanded), it goes to the closed list not to be expanded."
but the algorithm you gave in http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_416044 has just
Closed.push(curr)
That unconditional push onto the closed list pushes the current item onto the closed list so that later the closed list can be tested to see if the item has already been expanded. Unfortunately that algorithm you were given does not handle the "because it may have been an obstacle" part (there is no test anywhere in the algorithm for the current location being an obstacle), and the algorithm does not properly handle "or may have been previously expanded" because the algorithm given does not test whether the current item is on the closed list, only whether a prospective location is on the closed list. As I described earlier, that can result in a location being put onto the Queue multiple times.
Ken
on 14 Jan 2017
Walter Roberson
on 14 Jan 2017
You specifically said, "The course has outlined these steps", which I interpreted as indicating that you were given that pseudocode.
Ken
on 15 Jan 2017
Walter Roberson
on 16 Jan 2017
Please re-read http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_419758 as I was strongly hinting at the solutions there.
Ken
on 16 Jan 2017
Walter Roberson
on 16 Jan 2017
Break it into pieces. When I write
"It does not check that the current node is eligible for expansion (the expand routine is not intended to do that in the algorithm). This would result in incorrect paths"
then the obvious solution is that the code should check that the current node is eligible for expansion -- that it is not a blocked location (check within the graph!) and that it has not already been added to the Closed list.
Categories
Find more on Marine and Underwater Vehicles in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
