1 篇文章 0 订阅

# 一、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.（加载预先生成的二进制图像。）
% 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文件的函数。

helphelperGetSceneImage.m文件。

function [sceneImage, sceneRef] = helperGetSceneImage(sceneName)
supportedScenes = {'LargeParkingLot', 'ParkingLot', 'DoubleLaneChange', ...
sceneName = validatestring(sceneName, supportedScenes, mfilename, 'sceneName');
imageName = strcat('sim3d_', sceneName, '.jpg');
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 完成而不显示任何输出， 在生成的错误消息中包含指定的变量名称。

# 二、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


# 三、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


# 五、总结

• 0
点赞
• 3
收藏
• 打赏
• 2
评论
10-07 561
12-01 6762
10-02 7480
09-11 3556
03-17 625
02-02 1434
06-28 243

### “相关推荐”对你有帮助么？

• 非常没帮助
• 没帮助
• 一般
• 有帮助
• 非常有帮助

¥2 ¥4 ¥6 ¥10 ¥20

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、C币套餐、付费专栏及课程。