移动机器人路径规划 几种A*算法改进matlab实现

classdef Astar < handle properties (SetAccess=private, GetAccess=private) start goal occgrid % occupancy grid as provided by user occgridnav % inflated occupancy grid w2g % transform from world coordinates to grid coordinates T % info kept per cell (state) b % backpointer (0 means not set) t % tag: NEW OPEN CLOSED g % distance map, path cost path line H0 heuristicmethod % defferent methods to enconding the distance of geven node to the goal validplan % a plan has been computed for current costmap openlist % list of open states: 2xN matrix,each open point is a column, row 1 = index of cell, row 2 = k openlist_maxlen % keep track of maximum length % tag state values NEW = 0; OPEN = 1; CLOSED = 2; end properties (SetAccess=private, GetAccess=public) niter = 0; costmap % world cost map: obstacle = Inf end methods (Access=public) %% initialization and properties options function as = Astar(world, varargin) % Create a Navigation object % as = Astar(OCCGRID, OPTIONS) is a Navigation object that holds an % occupancy grid OCCGRID. A number of options can be be passed. % % Options:: % 'inflate',K Inflate all obstacles by K cells. % % Notes:: % - In the occupancy grid a value of zero means free space and non-zero means % occupied (not driveable). % - Obstacle inflation is performed with a round structuring element (kcircle) % with radius given by the 'inflate' option. % - Inflation requires either MVTB or IPT installed. if nargin >= 1 % first argument is the map map = world; if isnumeric(map) && ~isscalar(map) as.occgrid = map; as.w2g = SE2(0, 0, 0); elseif isstruct(map) as.occgrid = map.map; as.w2g = as.T; end end % default values of options opt.inflate = 0; opt.transform = SE2; opt = tb_optparse(opt, varargin); % optionally inflate the obstacles if opt.inflate > 0 if exist('idilate','E:\RoboticsLab\vision-4.1\rvctools\vision') == 2 % use MVTB as.occgridnav = idilate(as.occgrid, kcircle(opt.inflate)); elseif exist('imdilate','E:\RoboticsLab\vision-4.1\rvctools\vision') == 2 % use IPT as.occgridnav = imdilate(as.occgrid, strel('disk',opt.inflate)); else error('RTB:Navigatio:Navigation', 'Need to have MVTB or IPT installed to perform obstacle inflation'); end else as.occgridnav = as.occgrid; end % copy other options into the object as.w2g = opt.transform; reset(as); end function set(as, varargin) opt.start = []; opt.goal = []; opt.heuristicmethod = []; opt = tb_optparse(opt, varargin); if ~isempty(opt.start) as.start = [opt.start(1);opt.start(2)]; if as.isoccupied(as.start) error('Astar:badarg', 'start location inside obtacle'); end end if ~isempty(opt.goal) as.goal = [opt.goal(1);opt.goal(2)]; if as.isoccupied(as.goal) error('Navigation:checkquery:badarg', 'goal location inside obtacle'); end end if ~isempty(opt.heuristicmethod) && isempty(opt.start) && isempty(opt.goal) as.heuristicmethod = opt.heuristicmethod; disp('Please check out whether have set start or goal.'); return ; else as.heuristicmethod = 0; end if isempty(opt.start) && ~isempty(opt.goal) as.plot(); disp('select start location'); beep as.start = round(ginput(1)); as.plot(); fprintf('Get start coordinate (%d, %d)\n',as.start(1),as.start(2)); if as.isoccupied(as.start) error('Navigation:checkquery:badarg', 'start location inside obtacle'); end end if isempty(opt.goal) && ~isempty(opt.start) as.plot(); disp('select goal location'); beep as.goal = round(ginput(1)); as.plot(); fprintf('Get goal coordinate (%d, %d)\n',as.goal(1),as.goal(2)); if as.isoccupied(as.goal) error('Navigation:checkquery:badarg', 'goal location inside obtacle'); end end if isempty(opt.start) && isempty(opt.goal) as.plot(); disp('select start location'); beep as.start = round(ginput(1)); as.plot(); fprintf('Get start coordinate (%d, %d)\n',as.start(1),as.start(2)); if as.isoccupied(as.start) error('Navigation:checkquery:badarg', 'start location inside obtacle'); end disp('select goal location'); beep as.goal = round(ginput(1)); as.plot(); fprintf('Get goal coordinate (%d, %d)\n',as.goal(1),as.goal(2)); if as.isoccupied(as.goal) error('Navigation:checkquery:badarg', 'goal location inside obtacle'); end end end function property = get(as, varargin) Properties={'start','goal','occgrid','occgridnav','b','t','g',... 'costmap','openlist','openlist_maxlen','path','w2g','line'}; index = find(strcmp(Properties,varargin)); if index == 1 property = as.start; elseif index == 2 property = as.goal; elseif index == 3 property = as.occgrid; elseif index == 4 property = as.occgridnav; elseif index == 5 property = as.b; elseif index == 6 property = as.t; elseif index == 7 property = as.g; elseif index == 8 property = as.costmap; elseif index == 9 property = as.openlist; elseif index == 10 property = as.openlist_maxlen; elseif index == 11 property = as.path; elseif index == 12 property = as.w2g; elseif index == 13 property = as.line; else disp('Please input string of property that you want to get !') end end %% path planning of Astar Algorithm function plan(as) assert(~isempty(as.start), 'RTB:Astar:plan', 'no start specified here or in constructor'); assert(~isempty(as.goal), 'RTB:Astar:plan', 'no goal specified here or in constructor'); s_index = sub2ind(size(as.costmap), as.start(2), as.start(1)); as.openlist = []; as.b = zeros(size(as.costmap), 'uint3