泊车基于matlab强化学习智能泊车【含Matlab源码 2269期】.zip

classdef ParkingLot < handle % MAP = ParkingLot(FREEIDXS) creates a parking lot with the free spots % specified in the array FREEIDXS. % % MAP is a struct containing the following fields: % 1. XLimits - map X limits % 2. YLimits - map Y limits % 3. SpotLocations = [x y] of occupied spaces % 4. OccupiedSpots = binary array of size 1x64, 1 occupied, 0 free % 5. TotalSpots - total number of parking spots % 6. AxesHandle - axes handle of the figure % % Total number of parking spots in this parking lot is 64 (non-tunable). % Copyright 2020 The MathWorks, Inc. properties VehiclePose = [20 16 0] TrainingZoneXLimits = [36.5 59] TrainingZoneYLimits = [0 20] end properties (SetAccess = private) XLimits = [0 100] YLimits = [0 60] SpotDimensions = [4.5 7] ObstacleDimensions = [4.7 2.5] VehicleDimensions = [4.7 1.8] Wheelbase = 2.8 PassageWidth = 16 MaxSpots = 64 SpotLocations = zeros(64,2) SensorLocations = zeros(64,2) OccupiedSpots = ones(1,64) ObstacleMatrix end properties (Hidden) Figure VehicleBodyHandle VehicleAxlesHandle VehicleWheelsHandle LidarLinesHandle LidarCrossHandle CameraFOVHandle TrainZoneHandle VehicleStatusHandle ControllerStatusHandle TimeDisplayHandle ReferencePathRect ReferencePathCurvature = 0.3 end methods function map = ParkingLot(freeidxs) arguments freeidxs (1,1) double {localValidateIdx} end map.OccupiedSpots(freeidxs) = 0; map.ReferencePathRect = [ map.XLimits(1)+map.SpotDimensions(2)+0.5*map.PassageWidth, ... map.YLimits(1)+map.SpotDimensions(2)+0.5*map.PassageWidth, ... diff(map.XLimits)-2*(map.SpotDimensions(2)+0.5*map.PassageWidth), ... diff(map.YLimits)-2*(map.SpotDimensions(2)+0.5*map.PassageWidth) ]; buildFigure(map); initializeMap(map); map.ObstacleMatrix = getObstacles(map, find(map.OccupiedSpots==1)); map.Figure.Visible = 'on'; end function show(map) % show(map) displays the map in a figure. if isempty(map.Figure) updateMap(map); else map.Figure.Visible = 'on'; end end function buildFigure(map) % Build the figure map.Figure = figure; map.Figure.Position = [435 200 1290 740]; map.Figure.Name = 'Auto Parking Valet'; map.Figure.NumberTitle = 'off'; %map.Figure.MenuBar = 'none'; %map.Figure.CloseRequestFcn = @localHideFigure; ax = gca(map.Figure); legend(ax,'off'); hold(ax, 'on'); ax.XLimMode = 'manual'; ax.YLimMode = 'manual'; ax.XLim = map.XLimits; ax.YLim = map.YLimits; end function highlight(map,idx) % highlight(map,idx) highlights a spot in the map specified in idx. arguments map idx (1,1) double end ax = gca(map.Figure); iseastspot = idx >= 15 && idx <= 22; iswestspot = idx >= 37 && idx <= 40; if iseastspot || iswestspot posx = map.SpotLocations(idx,1) - 0.5*map.ObstacleDimensions(1) - 0.5; posy = map.SpotLocations(idx,2) - 0.5*map.ObstacleDimensions(2) - 0.5; len = map.ObstacleDimensions(1) + 1; wid = map.ObstacleDimensions(2) + 1; else posx = map.SpotLocations(idx,1) - 0.5*map.ObstacleDimensions(2) - 0.5; posy = map.SpotLocations(idx,2) - 0.5*map.ObstacleDimensions(1) - 0.5; len = map.ObstacleDimensions(2) + 1; wid = map.ObstacleDimensions(1) + 1; end hlight = findobj(ax,'Tag','hlight'); if isempty(hlight) rectangle(ax,'Position',[posx posy len wid],'EdgeColor','g','LineWidth',2,'Tag','hlight') else hlight.Position = [posx posy len wid]; end drawnow; end function setFree(map,idxs) % setFree(map,idxs) frees the spots with index IDXS. localValidateIdx(idxs) map.OccupiedSpots(idxs) = 0; map.ObstacleMatrix = getObstacles(map, find(map.OccupiedSpots==1)); initializeMap(map); end function setOccupied(map,idxs) % setOccupied(map,idxs) occupies the spots with index IDXS. localValidateIdx(idxs) map.OccupiedSpots(idxs) = 1; map.ObstacleMatrix = getObstacles(map, find(map.OccupiedSpots==1)); initializeMap(map); end function obs = getObstacles(map,spotidx) % obs = getObstacles(map) returns the list of obstacles in the % parking lot in [x y xlen ylen theta] format. % % obs = getObstacles(map,spotidx) returns the obstacle at the % spot index spotidx in [x y xlen ylen theta] format. arguments map spotidx (1,:) double = find(map.OccupiedSpots) end obs = zeros(numel(spotidx)+11,5); ct = 1; while ct <= numel(spotidx) iseastspot = spotidx(ct) >= 15 && spotidx(ct) <= 22; iswestspot = spotidx(ct) >= 37 && spotidx(ct) <= 40; if iseastspot || iswestspot obs(ct,1) = map.SpotLocations(spotidx(ct),1); obs(ct,2) = map.SpotLocations(spotidx(ct),2); obs(ct,3) = map.ObstacleDimensions(1); obs(ct,4) = map.ObstacleDimensions(2); obs(ct,5) = 0; else obs(ct,1) = map.SpotLocations(spotidx(ct),1); obs(ct,2) = map.SpotLocations(spotidx(ct),2); obs(ct,3) = map.ObstacleDimensions(2); obs(ct,4) = map.ObstacleDimensions(1); obs(ct,5) = 0; end ct = ct + 1; end % add obstacles at corners obs(ct:ct+5,:) = [9.25 3.35 16.5 4.7 0; ... 90.75 3.35 16.5 4.7 0; ... 96.35 8.5 4.7 5 0; ... 96.35 51.5 4.7 5 0; ... 90.75 56.35 16.5 4.7 0; ... 9.25 56.35 16.5 4.7 0]; % add obstacles at map boundary obs(ct+6:end,:) = ... [50 0.25 100 0.5 0; ... 99.75 30 0.5 60 0; ... 50 59.75 100 0.5 0; ... 0.25 30 0.5 60 0; ... 50 30 54 0.5 0]; end function pose = createTargetPose(map,idx) % pose = createTargetPose(map,idx,wbase,theta) returns a target % pose for the vehicle based on the specified parking spot index % idx and wheelbase length wbase. arguments map idx (1,1) double {mustBePositive,mustBeInteger} end wbase = map.Wheelbase; loc = map.SpotLocations(idx,:); issouthspot = idx <= 14; iseastspot = idx >= 15 && idx <= 22; isnorthspot = idx >= 23 && idx <= 36; iswestspot = idx >= 37 && idx <= 40; %#ok<NASGU> if issouthspot pose(1) = loc(1); pose(2) = loc(2) + 0.5*wbase; pose(3) = -pi/2; els