CS计算机代考程序代写 matlab classdef AstarPlanner < handle

classdef AstarPlanner < handle % Reference: % Astar_tutorial.pdf % A* zip @ http://www.mathworks.com/matlabcentral/fileexchange/26248-a---a-star--search-for-path-planning-tutorial properties (SetAccess=public, GetAccess=public) costmap % world cost map: obstacle = Inf G % index of goal point S % index of start point openlist openlist_maxlen quiet = true % flag to show intermediate results % tag state values (different from Dstar) b % backpointer (0 means not set), point to parent t % tag: NEW OPEN CLOSED h % distance map, path cost to start NEW = 0; OPEN = 1; CLOSED = 2; occgrid goal start spincount = 0 end methods %%%%%%%%%%%%%%%%%%%% constructor %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function obj = AstarPlanner(world, goal, start) obj.goal = goal; obj.start = start; obj.occgrid = world; obj.occgrid2costmap(obj.occgrid); % init the A* state variables obj.reset(); end %%%%%%%%%%%%%%%%%%%% occgrid2costmap %%%%%%%%%%%%%%%%%%%%%%%%%%%% function occgrid2costmap(obj, occgrid, cost) if nargin < 3 cost = 1; end obj.costmap = occgrid; obj.costmap(obj.costmap==1) = Inf; % occupied cells have Inf driving cost obj.costmap(obj.costmap==0) = cost; % unoccupied cells have driving cost end %%%%%%%%%%%%%%%%%%%% reset %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function reset(obj) % reset() resets the A* planner. The next instantiation % of plan() will perform a global replan. obj.openlist = zeros(5,0); % the open list, one column per point obj.openlist_maxlen = -Inf; % keep goal in index rather than row,col format obj.G = sub2ind(size(obj.occgrid), obj.goal(2), obj.goal(1)); obj.S = sub2ind(size(obj.occgrid), obj.start(2), obj.start(1)); obj.b = zeros(size(obj.costmap), 'uint32'); % backpointers obj.t = zeros(size(obj.costmap), 'uint8'); % tags, default NEW obj.t(obj.occgrid == 1) = obj.CLOSED; % put all obtacle to CLOSED obj.h = Inf*ones(size(obj.costmap)); % path cost estimate end %%%%%%%%%%%%%%%%%%%% neighbours %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function Y = neighbours(obj, X) dims = size(obj.costmap); [r,c] = ind2sub(dims, X); % list of 8-way neighbours Y = [r-1 r-1 r-1 r r r+1 r+1 r+1; c-1 c c+1 c-1 c+1 c-1 c c+1]; k = (min(Y)>0) & (Y(1,:)<=dims(1)) & (Y(2,:)<=dims(2)); Y = Y(:,k); Y = sub2ind(dims, Y(1,:)', Y(2,:)')'; end %%%%%%%%%%%%%%%%%%%% euclidean_distance %%%%%%%%%%%%%%%%%%%%%%%% function heuristic = euclidean_distance(obj, X, Y) [r,c] = ind2sub(size(obj.costmap), [X; Y]); heuristic = sqrt(sum(diff([r c]).^2)); end %%%%%%%%%%%%%%%%%%%%%% insert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function insert(obj, array, parent) % openlist: each col contains % openlist(1,i) = node index, not subscript % openlist(2,i) = parent index, not subscript % openlist(3,i) = g = path cost % openlist(4,i) = h = heuristic % openlist(5,i) = f = g + h array(2,1) = parent; obj.openlist = [obj.openlist, array]; obj.t(array(1,1)) = obj.OPEN; end %%%%%%%%%%%%%%%%%%%% DELETE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function DELETE(obj, X, parent, g) % if ~obj.quiet % fprintf('delete %d\n', X); % end i = find(obj.openlist(1,:) == X); % if length(i) ~= 1 % error('D*:DELETE: state %d doesnt exist', X); % end obj.openlist(:,i) = []; % remove the column obj.t(X) = obj.CLOSED; obj.b(X) = parent; obj.h(X) = g; end %%%%%%%%%%%%%%%%%%%% expand_array %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function exp_array = expand_array(obj, X, g) % exp_array: ech col contains % exp_array(1,i) = node index, not subscript % exp_array(2,i) = nan % exp_array(3,i) = g = path cost % exp_array(4,i) = h = heuristic % exp_array(5,i) = f = g + h exp_array=[]; exp_count=1; Y = obj.neighbours(X); for Y = obj.neighbours(X) if obj.t(Y) ~= obj.CLOSED exp_array(1, exp_count) = Y; exp_array(2, exp_count) = nan; exp_array(3, exp_count) = g + obj.euclidean_distance(X, Y); exp_array(4, exp_count) = obj.euclidean_distance(Y, obj.G); exp_array(5, exp_count) = exp_array(3, exp_count) + exp_array(4, exp_count); exp_count = exp_count+1; end end end %%%%%%%%%%%%%%%%%%%% MIN_STATE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [X, parent, g] = MIN_STATE(obj) % return the index of the open state with the smallest k value if isempty(obj.openlist) X = []; g = 0; else % find the minimum f value on the openlist [~,i] = min(obj.openlist(5,:)); % return its index X = obj.openlist(1,i); parent = obj.openlist(2,i); g = obj.openlist(3,i); end end %%%%%%%%%%%%%%%%%%%%%% plan %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function plan(obj) if ~obj.quiet obj.plot(); hold on end X = obj.S; g = 0; obj.t(X) = obj.CLOSED; while( X ~= obj.G) %obj.spinner(); if ~obj.quiet [r,c] = ind2sub(size(obj.costmap), X); plot(c, r, 'c.', 'MarkerSize', 12); drawnow end %size(obj.openlist,2) exp_array = obj.expand_array(X, g); for i = 1: size(exp_array,2) % if the successor is already on the openlist if obj.t( exp_array(1,i) ) == obj.OPEN % find the position of this successor in the % openlist j = find(obj.openlist(1,:) == exp_array(1,i) ); obj.openlist(5,j) = min(obj.openlist(5,j), exp_array(5,i)); % openlist update cost if obj.openlist(5,j) == exp_array(5,i) obj.openlist(2,j) = X; obj.openlist(3,j) = exp_array(3,i); obj.openlist(4,j) = exp_array(4,i); end % if the successor is NEW, insert to the openlist else obj.insert(exp_array(:,i), X); end end [X, parent, g] = obj.MIN_STATE();obj.DELETE(X, parent, g); end %Find out the node with the smallest fn disp('done planning') end %%%%%%%%%%%%%%%%%%%% path %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function pp = path(obj) obj.plot(); hold on robot = obj.goal; p = []; % iterate using the next() method until we reach the goal while true plot(robot(1), robot(2), 'g.', 'MarkerSize', 12); drawnow % move to parent on path robot = obj.parent(robot); % are we there yet? if robot == obj.start % yes, exit the loop break else % no, append it to the path p = [robot(:)'; p]; end end pp = p; end %%%%%%%%%%%%%%%%%%%%%% plot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function plot(obj) clf % create color map for distance field + obstacle: % obstacle, color index = 1, red % free space, color index > 1, greyscale
distance = obj.costmap;

% create the color map
cmap = [1 0 0; 0 0 0];

% ensure obstacles appear as red pixels
distance(obj.occgrid > 0) = 0;
% create the color map
cmap = [1 0 0; 1 1 1];

% display it with colorbar
image(distance+1, ‘CDataMapping’, ‘direct’);
set(gcf, ‘Renderer’, ‘Zbuffer’)
colormap(cmap)

% label the grid
set(gca, ‘Ydir’, ‘normal’);
xlabel(‘x’);
ylabel(‘y’);
grid on
axis equal
hold on
if ~isempty(obj.goal)
plot(obj.goal(1), obj.goal(2), ‘bd’, ‘MarkerFaceColor’, ‘b’);
end
hold on
end

%%%%%%%%%%%%%%%%%%%% spinner %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function spinner(obj)
% spinner() displays a simple ASCII progress spinner, a rotating bar.
spinchars = ‘-\|/’;
obj.spincount = obj.spincount + 1;
fprintf(‘\b%c’, spinchars( mod(obj.spincount, length(spinchars))+1 ) );
end
%%%%%%%%%%%%%%%%%%%% parent %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function n = parent(obj, current)
X = sub2ind(size(obj.costmap), current(2), current(1));
X = obj.b(X);
if X == 0
n = [];
else
[r,c] = ind2sub(size(obj.costmap), X);
n = [c;r];
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end