MATLAB中AVP例子学习

MATLAB中AVP例子学习

仅用于参考，因为是自己理解。会有错误！！！！！

一、Create a costmap from a 3D scene（从 3D 场景创建成本图）

% Load and display the image of the parking lot (获取停车场的范围信息)
sceneName = 'LargeParkingLot';
[sceneImage, sceneRef] = helperGetSceneImage(sceneName);
% Visualize the scene image(可视化场景图像 )有点没有看懂!
figure
helperShowSceneImage(sceneImage, sceneRef)
% Alternatively, load a pregenerated binary image.（加载预先生成的二进制图像。）
sceneImageBinary = imread('sim3d_LargeParkingLotBinary.bmp');
% Get the left-bottom corner location of the map（获取地图左下角位置） （x,y）=(-78.800m,-75m)
mapLocation = [sceneRef.XWorldLimits(1), sceneRef.YWorldLimits(1)]; 
% Compute resolution (计算分辨率)
mapWidth = sceneRef.XWorldLimits(2)-sceneRef.XWorldLimits(1); % meters
cellSize = mapWidth/size(sceneImageBinary, 2);%size返回值的是：行的个数%
% Create the costmap（创造成本图）
costmap = vehicleCostmap(im2single(sceneImageBinary), 'CellSize', cellSize, 'MapLocation', mapLocation);
figure
plot(costmap, 'Inflation', 'off');
legend off
%设置车辆的尺寸
centerToFront = 1.104; % meters
centerToRear  = 1.343; % meters
frontOverhang = 0.828; % meters
rearOverhang  = 0.589; % meters
vehicleWidth  = 1.653; % meters
vehicleHeight = 1.513; % meters
vehicleLength = centerToFront + centerToRear + frontOverhang + rearOverhang;

vehicleDims = vehicleDimensions(vehicleLength, vehicleWidth, vehicleHeight,...
    'FrontOverhang', frontOverhang, 'RearOverhang', rearOverhang);
costmap.CollisionChecker.VehicleDimensions = vehicleDims;
%%
% Set the inflation radius by specifying the number of circles enclosing
% the vehicle.
costmap.CollisionChecker.NumCircles = 5;

1.调用helphelperGetSceneImage.m文件的函数。

其中sceneName名字为LargeParkingLot， sceneImage 是真彩色 RGB 图像。 sceneRef 是 imref2d 类型的空间参考对象，描述了内在图像坐标和世界坐标之间的百分比关系。
helphelperGetSceneImage.m文件。

function [sceneImage, sceneRef] = helperGetSceneImage(sceneName)
supportedScenes = {'LargeParkingLot', 'ParkingLot', 'DoubleLaneChange', ...
    'USCityBlock', 'USHighway', 'CurvedRoad', 'VirtualMCity', 'StraightRoad'};
sceneName = validatestring(sceneName, supportedScenes, mfilename, 'sceneName');
% Read image 读取jpg图片
imageName = strcat('sim3d_', sceneName, '.jpg');
sceneImage = imread(imageName);
% Read spatial reference，读取相关参数
data = load('sim3d_SpatialReferences.mat');
sceneRef = data.spatialReference.(sceneName);
end

validatestring函数：
matchedStr = validatestring(str,validStrings,funcName,varName)的用法：找到这个场景的命名。LargeParkingLot这个停车场的场景被封装到sim3d_SpatialReferences.mat文件中。

strcat函数：将’sim3d_’, sceneName, '.jpg’拼接即sim3d_LargeParkingLot.jpg。

2.调用helperShowSceneImage.m文件的函数。

hIm = helperShowSceneImage(…) 返回图像对象的句柄 。

function varargout = helperShowSceneImage(varargin)
narginchk(1,2);%输入的两个值%
nargoutchk(0,1);%输出的一个值%
if nargin==1
    sceneName = varargin{1};
    [sceneImage, sceneRef] = helperGetSceneImage(sceneName);
elseif nargin==2
    sceneImage = varargin{1};
    sceneRef   = varargin{2};
end
validateattributes(sceneRef, {'imref2d'}, {'scalar'}, mfilename, 'sceneRef');
imageSize = sceneRef.ImageSize;
validateattributes(sceneImage, {'numeric'}, {'real','nonsparse','size',[imageSize 3]}, mfilename, 'sceneImage');
hIm = imshow(sceneImage, sceneRef);
set(gca, 'YDir', 'normal', 'Visible', 'on')
if nargin==1
    title(sceneName)
end
xlabel('X (m)')
ylabel('Y (m)')
if nargout==1
    varargout{1} = hIm;
end
end

narginchk(minArgs,maxArgs):如果调用中指定的输入数小于 minArgs 或大于 maxArgs，则 narginchk 将引发错误。 如果输入的数量介于 minArgs 和 maxArgs 之间（含），则 narginchk 不执行任何操作。
nargoutchk(minArgs,maxArgs):验证当前函数调用中指定的输出参数的数量。 如果输出数量小于 minArgs 或大于 maxArgs，nargoutchk 会抛出错误。 如果输出数量在 minArgs 和 maxArgs 之间（包括在内），则 nargoutchk 不执行任何操作。
上述这个两个是一个保障！！！
nargin：就是判断进来的数据是否是两个sceneImage, sceneRef。
validateattributes(A,classes,attributes,funcName,varName)：验证数组 A 至少属于指定的类（或其子类）之一并具有所有指定的属性。 如果 A 不符合条件，则 MATLAB® 会引发错误并显示格式化的错误消息。 否则，validateattributes 完成而不显示任何输出， 在生成的错误消息中包含指定的变量名称。

3.调用vehicleCostmap的函数。

分为 Occupied(obstacle)、occupied(inflated area)、unknown、Free。

二、Create a route plan from the scene（从现场创建路线计划）

% Plot vehicle at the starting pose
startPose = routePlan.StartPose(1,:);%选出起始点%
hold on
helperPlotVehicle(startPose, vehicleDims, 'DisplayName', 'Current Pose')
legend
 %选出中途的5个点就是以前定义好的！！！%
for n = 1 : height(routePlan)
    % Extract the goal waypoint
    vehiclePose = routePlan{n, 'EndPose'};

    % Plot the pose
    legendEntry = sprintf('Goal %i', n);
    helperPlotVehicle(vehiclePose, vehicleDims, 'DisplayName', legendEntry);
    hold on
end
hold off

这就是之前定义好的起始点和中间定义好的5个点

三、Configure the 3D scene and ego vehicle in Simulink（在 Simulink 中配置 3D 场景和自我车辆）

% Close the figures and open the model.%关闭图像打开模型。%
helperCloseFigures
if ~ispc
    error(['3D Simulation is only supported on Microsoft', char(174), ' Windows', char(174), '.']);
end
modelName = 'APVWith3DSimulation.slx';
open_system(modelName)
snapnow 

四、Simulate and visualize the vehicle’s motion in the 3D scene（在 3D 场景中模拟和可视化车辆的运动）

sim(modelName)
% Path Planner 块通过环境地图规划一条可行的路径
% 使用最佳快速探索随机树 (RRT*) 算法。
%模块被修改为包含两个单独的模块： * *Driving Mode*：使用整个停车场的costmap。 * *停车模式*：使用本地成本图进行最终停车操作。
open_system([modelName, '/Path Planner'], 'force')
% Close the model and the figures.
bdclose all
helperCloseFigures

五、总结

就是讲一张图片转化为类似于栅格形式， 最终采用2D的路径规划来做。