ROBOT LOCALIZATION AND MOTION PLANNING

4 次查看(过去 30 天)
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 个评论
Ken
Ken 2016-12-2
编辑:Ken 2016-12-2
  • Thanks Walter.
  • It is to find the Jacobian Fx, Fu of f w.r.t. x and u. I tried this but get error "Undefined function 'x' for input arguments of type 'double' :
  • f = @(x, u) [x(1)+u(1)*cos(x(3)+u(2));
  • x(2)+u(1)*sin(x(3)+u(2)) ; x(3)+u(2)];
  • Fx = @(x,u) 1;1;1;
  • Fu = @(x,u) cos(x(2)+1);sin(x(2)+1);1;
Ken
Ken 2016-12-2
编辑:Walter Roberson 2016-12-2
Some added info:
x = [1.; 2. ; pi/4]
u = [.1; pi/8]

请先登录,再进行评论。

采纳的回答

Walter Roberson
Walter Roberson 2017-1-16
  7 个评论
Ken
Ken 2017-1-18
Thanks Walter.
Feel I am out of my league here so am trying a simpler problem i.e. to find the shortest path from start to goal using the same cost i.e. 1 for vert edge, 1 for horiz edge. Tried this but stuck at Min - goes into an endless loop. Please help.
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;
Start = [3,7];
Goal = [9,6];
SolutionMap = Inf*ones(size(Map)); %store g-values in here.
G = Map;
%function BF(G, Start, Goal)
Queue = Queue_init_FIFO();
Closed = Queue_init_FIFO();
Cost = Queue_init_FIFO()
Queue = Queue_push_FIFO(Queue, Start);
Closed = Closed_push_FIFO(Queue, Start);
while ~Queue_isempty_FIFO(Queue)
[curr, Queue] = Queue_pop_FIFO(Queue);
if isequal(curr, Goal)
return
end
1. is curr =-1 i.e. an obstacle?
for curr ~= -1
continue
if ~ Closed_ismember_FIFO(Closed, next)
next = curr;
next=Start;
while next ~= Goal
if next ~=-1
x=next(1);
y=next(2);
end
Queue = Queue_push_FIFO(Queue, this_next);
NextSolutionMap(x,y) = min((SolutionMap(x-1,y) - Goal (1)+ SolutionMap(x-1,y)- Goal(2)),...
(SolutionMap(x+1,y)- Goal(1) + SolutionMap(x+1,y)- Goal(2)),...
(SolutionMap(x,y-1) - Goal(1) + SolutionMap(x,y-1) - Goal(2)),...
(SolutionMap(x,y+1) - Goal(1) + SolutionMap(x,y+1) - Goal(2)));
next=NextSolutionMap(x,y);
%Cost = Cost_push_FIFO(Cost,[x,y]);
%SolutionMap(x,y) = Cost(x,y) %problem asks for g i.e. cost values to be stored here
end
end
Ken
Ken 2017-1-20
Walter, Maybe I tested your patience but please help!!

请先登录,再进行评论。

更多回答(1 个)

Walter Roberson
Walter Roberson 2016-12-2
Assuming your formulae are correct,
Fx = @(x,u) [1;1;1];
Fu = @(x,u) [cos(x(2)+1);sin(x(2)+1);1];
  115 个评论
Ken
Ken 2017-1-16
Thanks Walter. Looked at it - too complex for me!
Walter Roberson
Walter Roberson 2017-1-16
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.

请先登录,再进行评论。

类别

Help CenterFile Exchange 中查找有关 Marine and Underwater Vehicles 的更多信息

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by