mapLayers = loadParkingLotMapLayers;plotMapLayers (mapLayers)

costmap = combineMapLayers (mapLayers);图绘制(costmap,“通货膨胀”,“关闭”传说)从

costmap.MapExtent% [x，宽度，y，高度]，单位为米costmap.CellSize单元格大小(以米为单位)

vehicleDims = vehicleDimensions;maxSteeringAngle = 35;%的度

costmap.CollisionChecker。VehicleDimensions= vehicleDims;

currentPose = [4 12 0];% [x, y, theta]

data =负载(“routePlan.mat”);routePlan = data.routePlan% #好< NOPTS >

%绘制当前位置的车辆持有在helperPlotVehicle (currentPose vehicleDims,“DisplayName的”,目前提出的传说)为n = 1:高度(路径图)提取目标路径点vehiclePose = routePlan {n,“EndPose”};%绘制姿势legendEntry = sprintf (“目标%我”n);helperPlotVehicle (vehiclePose vehicleDims,“DisplayName的”,legendEntry);结束持有从

= HelperBehavioralPlanner(routePlan, maxSteeringAngle);

motionPlanner = pathPlannerRRT (costmap,“MinIterations”,1000,…“ConnectionDistance”10“MinTurningRadius”,20);

goalPose = routePlan {1,“EndPose”};= plan(motionPlanner, currentPose, goalPose);

refPath.PathSegments

[transitionpose, directions] =插值(refPath);可视化计划的路径情节(motionPlanner)

%指定使用约0.1米的间距返回的姿态数approxSeparation = 0.1;%米numSmoothPoses =圆(refPath。长度/ approxSeparation);%返回沿光滑路径的离散姿态= smoothPathSpline(transitionpose, directions, cumlength，);绘制平滑的路径持有在hSmoothPath = plot(ref(:， 1)， ref(:， 2)，“r”,“线宽”2,…“DisplayName的”,“平滑路径”);持有从

maxSpeed = 5;%在米/秒startSpeed = 0;%在米/秒endSpeed = 0;%在米/秒

速度= helperGenerateVelocityProfile(方向、长度、曲率、起始速度、终止速度、最大速度);

plotVelocityProfile (cumLengths、refVelocities maxSpeed)

关闭所有的数字closeFigures;创建车辆模拟器【翻译】(成本地图，车价);设置车辆姿态和速度vehicleSim.setVehiclePose (currentPose);currentVel = 0;vehicleSim.setVehicleVelocity (currentVel);配置模拟器来显示轨迹vehicleSim.showTrajectory(真正的);%隐藏车辆仿真图hideFigure (vehicleSim);

热分析仪(refpose, ref, directions，)…的轴距,vehicleDims.Wheelbase);

sampleTime = 0.05;lonController = HelperLongitudinalController (“SampleTime”,sampleTime);

controlRate = HelperFixedRate (1 / sampleTime);%在赫兹

reachGoal = false;而~ reachGoal找到路径上的参考位姿和相应的速度[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);更新模拟器的驾驶方向updateDrivingDirection (vehicleSim、方向);%计算操舵命令(1)主句为steeringAngle，主句为currentVel，主句为currentVel。…“方向”、方向、的轴距,vehicleDims.Wheelbase);计算加速和减速命令lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);%使用控制器输出模拟车辆驾驶(车辆、仪表、仪表、舵角);检查车辆是否达到目标reachGoal = helperGoalChecker(goalPose, currentPose, currentVel, endSpeed, direction);等待固定速率执行等待(controlRate);%得到当前的姿态和速度的车辆currentPose = getVehiclePose (vehicleSim);currentVel = getVehicleVelocity (vehicleSim);结束%显示车辆仿真图showFigure (vehicleSim);

%设置车辆姿态回初始起点currentPose = [4 12 0];% [x, y, theta]vehicleSim.setVehiclePose (currentPose);%重置速度currentVel = 0;%米/秒vehicleSim.setVehicleVelocity (currentVel);而~ reachedDestination (behavioralPlanner)%请求行为层下一次机动[nextGoal, plannerConfig, speedConfig] = request机动(behavioralPlanner，…currentPose currentVel);配置运动规划器configurePlanner (motionPlanner plannerConfig);使用RRT* planner为下一个目标位姿规划一个参考路径refPath = plan(motionPlanner, currentPose, nextGoal);检查路径是否有效。如果规划者未能计算出路径，或由于对映射的更新，路径不是无冲突的系统需要重新计划。这个场景使用一个静态映射，也就是路径%将永远是无碰撞。isreplanneed = ~checkPathValidity(refPath, costmap);如果isReplanNeeded警告('无法找到有效的路径。试图重新计划”。)请求行为计划员重新计划replanNeeded (behavioralPlanner);继续;结束从计划的路径中检索过渡姿态和方向[transitionpose, directions] =插值(refPath);使道路平坦numSmoothPoses =圆(refPath。长度/ approxSeparation);= smoothPathSpline(transitionpose, directions, cumlength，);生成速度剖面速度= helperGenerateVelocityProfile(方向、长度、曲率、起始速度、终止速度、最大速度);%配置路径分析器pathAnalyzer。RefPoses = RefPoses;pathAnalyzer。方向=方向;pathAnalyzer。VelocityProfile = refVelocities;复位纵向控制器重置(lonController);reachGoal = false;执行控制循环而~ reachGoal找到路径上的参考位姿和相应的速度[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);更新模拟器的驾驶方向updateDrivingDirection (vehicleSim、方向);%计算操舵命令(1)主句为steeringAngle，主句为currentVel，主句为currentVel。…“方向”、方向、的轴距,vehicleDims.Wheelbase);计算加速和减速命令lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);%使用控制器输出模拟车辆驾驶(车辆、仪表、仪表、舵角);检查车辆是否达到目标reachGoal = helperGoalChecker(nextGoal, currentPose, currentVel, speedConfig)。EndSpeed、方向);等待固定速率执行等待(controlRate);%得到当前的姿态和速度的车辆currentPose = getVehiclePose (vehicleSim);currentVel = getVehicleVelocity (vehicleSim);结束结束%显示车辆仿真图showFigure (vehicleSim);

%隐藏车辆仿真图hideFigure (vehicleSim);

ccConfig = costmap.CollisionChecker;图绘制(ccConfig)标题(“当前的碰撞检查器”)

ccConfig。NumCircles = 4;图绘制(ccConfig)标题(“新碰撞检查器”)

costmap。CollisionChecker = ccConfig;

图绘制(costmap)标题(“更新冲突检查的成本地图”)设置pathPlannerRRT以使用更新的成本图parkMotionPlanner = pathPlannerRRT (costmap,“MinIterations”,1000);定义所需的姿势为停车位，由V2X系统返回parkPose = [36 44 90];preParkPose = currentPose;计算所需的停车机动= plan(parkMotionPlanner，准备姿势，parkPose);%绘制结果停车机动figure plotparking机动(costmap, refPath, preparekpose, parkPose)

从计划的路径中检索过渡姿态和方向[transitionpose, directions] =插值(refPath);使道路平坦numSmoothPoses =圆(refPath。长度/ approxSeparation);= smoothPathSpline(transitionpose, directions, cumlength，);设置速度剖面发生器在轨迹结束时停止，限速为每小时5英里速度= helperGenerateVelocityProfile(方向、长度、曲率、电流等);pathAnalyzer。RefPoses = RefPoses;pathAnalyzer。方向=方向;pathAnalyzer。VelocityProfile = refVelocities;复位纵向控制器重置(lonController);reachGoal = false;而~ reachGoal找到路径上的参考位姿和相应的速度[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);更新模拟器的驾驶方向updateDrivingDirection (vehicleSim、方向);%计算操舵命令(1)主句为steeringAngle，主句为currentVel，主句为currentVel。…“方向”、方向、的轴距,vehicleDims.Wheelbase);计算加速和减速命令lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);%使用控制器输出模拟车辆驾驶(车辆、仪表、仪表、舵角);检查车辆是否达到目标reachGoal = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction);等待固定速率执行等待(controlRate);%得到当前的姿态和速度的车辆currentPose = getVehiclePose (vehicleSim);currentVel = getVehicleVelocity (vehicleSim);结束%显示车辆仿真图closeFigures;showFigure (vehicleSim);

指定与倒车泊车动作相对应的泊车姿势parkPose = [49 47 -90];改变连接方式，允许反向运动parkMotionPlanner。ConnectionMethod =“Reeds-Shepp”;

parkMotionPlanner。MinTurningRadius = 10;%米parkMotionPlanner。ConnectionDistance = 15;复位车辆姿态和速度currentVel = 0;vehicleSim.setVehiclePose (preParkPose);vehicleSim.setVehicleVelocity (currentVel);计算停车机动重新计划= true;而replan refPath = plan(parkMotionPlanner, prepare kpose, parkPose);与停车机动相对应的路径小且要求大%精确的操纵。而不是只在过渡的姿势内插，沿路径长度进行更精细的内插。numSamples = 10;stepSize = refPath。长度/ numSamples;length = 0: stepSize: refPath.Length;[transitionpose, directions] =插值(refPath，长度);如果路径包含多个方向切换姿势，则重新计划%或如果路径太长replan = sum(abs(diff(directions)))~=2 || refPath。长度> 20;结束想象停车的动作figure plotparking机动(costmap, refPath, preparekpose, parkPose)

numSmoothPoses =圆(refPath。长度/ approxSeparation);= smoothPathSpline(transitionpose, directions, numsmoothpose, 0.5);生成速度剖面速度= helperGenerateVelocityProfile(方向、长度、曲率、电流等);pathAnalyzer。RefPoses = RefPoses;pathAnalyzer。方向=方向;pathAnalyzer。VelocityProfile = refVelocities;复位纵向控制器重置(lonController);reachGoal = false;而~ reachGoal得到当前的驱动方向currentDir = getDrivingDirection (vehicleSim);找到路径上的参考位姿和相应的速度。[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);%若车辆改变行驶方向，则复位车辆速度in模拟器和复位纵向控制器如果currentDir ~=方向电流= 0;setVehicleVelocity (vehicleSim currentVel);重置(lonController);结束更新模拟器的驾驶方向。如果车辆改变%行驶方向，复位，返回当前车辆速度为零。currentVel = updateDrivingDirection(vehicle, direction, currentDir);%计算操舵命令(1)主句为steeringAngle，主句为currentVel，主句为currentVel。…“方向”、方向、的轴距,vehicleDims.Wheelbase);计算加速和减速命令lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);%使用控制器输出模拟车辆驾驶(车辆、仪表、仪表、舵角);检查车辆是否达到目标reachGoal = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction);等待固定速率执行等待(controlRate);%得到当前的姿态和速度的车辆currentPose = getVehiclePose (vehicleSim);currentVel = getVehicleVelocity (vehicleSim);结束以快照为例closeFigures;snapnow;%删除模拟器删除(vehicleSim);

函数mapLayers = loadParkingLotMapLayers ()% loadParkingLotMapLayers%负荷占用图对应的3层-障碍，道路标记和使用过的斑点。mapLayers。StationaryObstacles = imread (“stationary.bmp”);mapLayers。路标= imread (“road_markings.bmp”);mapLayers。ParkedCars = imread (“parked_cars.bmp”);结束

函数plotMapLayers (mapLayers)% plotMapLayers在一个图形窗口中绘制多个地图层。figure cellOfMaps = cellfun(@ im补体，struct2cell(maplayer)，“UniformOutput”、假);蒙太奇(cellOfMaps“大小”[1元素个数(cellOfMaps)),“边界”,5 [5],“ThumbnailSize”， [300 NaN])标题(“地图图层-固定的障碍物、道路标记和停放的车辆”)结束

函数costmap = combineMapLayers (mapLayers)% combineMapLayers将地图层结构合并成一个单独的汽车地图。combinedMap = mapLayers。StationaryObstacles + mapLayers。路标+…mapLayers.ParkedCars;combinedMap = im2single (combinedMap);res = 0.5;%米costmap = vehicleCostmap (combinedMap,“CellSize”res);结束

函数configurePlanner (pathPlanner配置)% configurePlanner配置路径规划器对象，路径规划器，设置指定%在结构配置。=字段字段名(配置);为n = 1: numel(字段名)如果~ strcmpi(字段名{n},“IsParkManeuver”) pathPlanner.(fieldNames{n}) = config.(fieldNames{n});结束结束结束

函数plotVelocityProfile (cumPathLength、refVelocities maxSpeed)% plotVelocityProfile绘制生成的速度剖面%绘制参考速度沿路径长度情节(cumPathLength refVelocities,“线宽”2);绘制一条线来显示最大速度持有在线([0;cumPathLength(结束)],[maxSpeed; maxSpeed],“颜色”,“r”)举行从设置轴限制缓冲= 2;xlim ([0 cumPathLength(结束)]);ylim([0 maxSpeed + buffer])%添加标签包含(“累积路径长度(m)”);ylabel (“速度(米/秒)”);添加图例和标题传奇(“速度剖面”,的最高速度)标题(生成速度剖面的)结束

函数closeFigures ()关闭所有的数字，除了模拟器的可视化找到所有的图形对象figHandles = findobj (“类型”,“图”);为i = 1:长度(fight andles)如果~ strcmp (figHandles . name(我),“自动代客泊车”)密切(figHandles (i));结束结束结束

函数plotparking机动(costmap, refPath, currentPose, parkPose)% plotParkingManeuver%在成本地图上绘制生成的停车机动。绘制成本图，不包括膨胀的区域。情节(costmap“通货膨胀”,“关闭”)在成本地图上的地块参考停车机动持有在情节(refPath“DisplayName的”,“停车策略”)标题(“停车策略”)通过设置轴向限制，可缩放到停车机动lo = min ([currentPose (1:2);parkPose (1:2)]);你好= max ([currentPose (1:2);parkPose (1:2)]);缓冲= 6;%米xlim([lo(1)-buffer hi(1)+buffer]) ylim([lo(2)-buffer hi(2)+buffer])结束