自动停车员

打开脚本

这个例子展示了如何构建一个自动代客泊车系统。在本例中，您将了解支持路径规划、轨迹生成和车辆控制的工具和技术。虽然本例关注的是面向MATLAB®的工作流，但这些工具也可以在Simulink®中使用。有关此示例的Simulink版本，请参见Simulink中的自动泊车服务．

概述

自动把停在停车场前面的汽车停好是一个具有挑战性的问题。预计车辆的自动系统将接管控制权，引导车辆到一个可用的停车位。这种功能使用多个机载传感器。例如:

用于检测车道标记、路标(停车标志、出口标志等)、其他车辆和行人的前和侧摄像头
激光雷达和超声波传感器用于探测障碍物和计算精确的距离测量
用于障碍物检测的超声波传感器
用于航位推算的IMU和轮编码器

车载传感器用于感知车辆周围的环境。感知环境包括理解道路标记来解释道路规则和推断可驾驶区域，识别障碍和检测可用的停车位。

当车辆的传感器感知世界时，车辆必须在环境中规划一条通往免费停车位的路径，并执行一系列所需的控制动作，以驾驶到那里。在这样做的同时，它必须对环境的动态变化做出反应，例如行人穿过它的路径，并重新调整它的计划。

这个例子实现了实现这样一个系统所需的功能的一个子集。它专注于规划通过环境的可行路径，并执行遍历该路径所需的操作。这个例子不包括地图创建和动态障碍物回避。

环境模型

环境模型表示环境的映射。对于代客泊车系统，该地图包括可用和已占用的停车位、道路标记和障碍物，如行人或其他车辆。占用图是这种形式的环境模型的常见表示。这种地图通常使用同步定位和测绘(SLAM)，通过集成激光雷达和相机传感器的观测数据来构建。这个例子集中于一个更简单的场景，在这个场景中，地图已经被提供了，例如，由车辆到基础设施(V2X)系统或俯瞰整个停车位的摄像头提供。它使用停车场的静态地图，并假设车辆的自我定位是准确的。

本例中使用的停车场示例由三个占用网格层组成。

固定障碍:该层包含固定障碍，如墙壁、障碍物和停车场边界。
道路标记:这一层包含与道路标记相关的占用信息，包括用于停车位的道路标记。
停放的车辆:这一层包含关于哪些停车位已经被占用的信息。

每个地图层都包含不同种类的障碍，这些障碍代表了汽车通过时所面临的不同程度的危险。通过这种结构，可以独立地处理、更新和维护每一层。

加载并显示三个地图层。在每一层中，深色细胞代表已占据的细胞，浅色细胞代表游离细胞。

mapLayers = loadParkingLotMapLayers;plotMapLayers (mapLayers)

为了简单起见，将这三层组合成一个成本图。

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

合并后的costmap是一个vehicleCostmap对象，它将车辆环境表示为二维占用网格。单元格中的每个网格都有0到1之间的值，表示在单元格中导航的成本。障碍物的成本较高，而自由空间的成本较低。如果一个电池的成本高于OccupiedThreshold如果它的成本低于FreeThreshold财产。

的costmap覆盖了整个75m × 50m的停车场区域，划分为0.5m × 0.5m的正方形单元。

costmap。MapExtent% [x，宽度，y，高度]，单位为米costmap。CellSize%单元大小，单位为米

Ans = 0 75 0 50 Ans = 0.5000

创建一个vehicleDimensions对象，用于存储将自动停车的车辆的尺寸。还要定义车辆的最大转向角度。这个值决定了在运动规划和控制过程中转弯半径的限制。

vehicleDims = vehicleDimensions;maxSteeringAngle = 35;%的度

更新VehicleDimensions属性的成本图碰撞检查器与待停放车辆的尺寸。该设置调整地图上障碍物周围的膨胀程度，以对应停放的车辆的大小，确保可以通过停车场找到无碰撞的路径。

costmap.CollisionChecker.VehicleDimensions = vehicleDims;

确定车辆的启动姿势。姿势是通过定位获得的，为了简单起见，本例中省略了这一点。车辆的姿态指定为 $[x, y,θ\]美元$ ，以世界坐标表示。 (x, y)美元表示车辆后桥在世界坐标系中的中心位置。 $\θ美元$ 表示飞行器相对于世界X轴的方向。有关更多细节，请参见自动驾驶工具箱中的坐标系统．

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

行为层

计划包括将所有相关信息组织到层次层中。每个连续的层负责一个更细粒度的任务。行为层[1]位于堆栈的顶部。它负责通过提供一系列导航任务来激活和管理任务的不同部分。行为层汇集来自系统所有相关部分的信息，包括:

定位:行为层检查定位模块，以估计车辆的当前位置。
环境模型:感知和传感器融合系统报告车辆周围环境的地图。
确定停车位:行为层分析地图以确定最近的可用停车位。
寻找全局路由:路由模块通过从地图服务或V2X基础设施获得的路网计算全局路由。将全球路线分解为一系列道路链接，可以对每个链接的轨迹进行不同的规划。例如，最后的停车机动需要一个不同的速度剖面比接近停车点。在更普遍的情况下，这对于在涉及不同限速、车道数量和路标的街道上导航至关重要。

这个例子没有依赖于车辆传感器来构建环境地图，而是使用了通过V2X通信来自智能停车场的地图。为了简单起见，假设地图是占用网格的形式，道路连接和V2X提供的可用停车位的位置。

的HelperBehavioralPlanner类模拟行为规划层的接口。的HelperBehavioralPlanner使用地图和全局路线计划创建。本例使用了存储在MATLAB表中的静态全局路线计划，但通常由本地停车基础设施或地图服务提供的路由算法确定该计划。全局路线规划被描述为经过一系列车道段到达一个停车位。

加载包含存储在表中的路由计划的mat文件。表格中有三个变量:StartPose，EndPose,属性．StartPose而且EndPose指定段的开始和结束姿势，表示为 $[x, y,θ\]美元$ ．属性指定段的属性，例如速度限制。

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

routePlan = 4×3表StartPose EndPose属性______________ ________________ __________ 4 12 0 56 11 0 1×1 struct 56 11 0 70 19 90 1×1 struct 70 19 90 70 32 90 1×1 struct 70 32 90 53 39 180 1×1 struct

画一辆汽车在当前的姿态，并沿着每个目标在路线计划。

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

创建行为计划器助手对象。的requestManeuver方法从行为计划器请求导航任务流，直到到达目的地。

behavioralPlanner = HelperBehavioralPlanner(routePlan, maxSteeringAngle);

车辆通过以下步骤导航每个路径段:

运动规划:使用最优快速搜索随机树(RRT*)算法(pathPlannerRRT）.
路径平滑:平滑参考路径拟合样条到它使用smoothPathSpline．
轨迹生成:使用生成速度剖面，将平滑路径转换为轨迹helperGenerateVelocityProfile．
车辆控制:给定平滑的参考路径，HelperPathAnalyzer根据车辆当前的姿态和速度计算参考姿态和速度。提供了参考值，lateralControllerStanley计算转向角度以控制车辆的航向。HelperLongitudinalController计算加速和减速命令，以保持所需的车辆速度。
目标检查:检查车辆是否已达到所使用路段的最终姿态helperGoalChecker．

在将这些步骤组装成一个完整的解决方案之前，本示例的其余部分将详细描述这些步骤。

运动规划

给定一个全局路线，运动规划可以用来规划通过环境到达每个中间路径点的路径，直到车辆到达最终目的地。每个环节的规划路径必须是可行的和无碰撞的。可行路径是在给定的运动和动态约束条件下，车辆可以实现的路径。代客泊车系统涉及低速和低加速度。这使我们可以安全地忽略由惯性效应引起的动态约束。

创建一个pathPlannerRRT对象使用最优快速探索随机树(RRT*)方法配置路径规划器。RRT系列规划算法通过构建一个连接的无碰撞车辆姿态树来寻找路径。使用Dubins或reed - shepp转向连接姿态，确保生成的路径在运动学上是可行的。

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

规划从当前姿势到第一个目标的路径计划函数。返回的开车。路径对象,refPath是一种可行的无碰撞参考路径。

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

参考路径由一系列路径段组成。每个路径段描述用于连接到下一个段的Dubins或reed - shepp机动集。检查路径段。

refPath。路径Segments

ans = 1×6 DubinsPathSegment数组属性:StartPose GoalPose MinTurningRadius motionlengthmotiontypes长度

参考路径包含沿途的转换姿势，表示路径上对应从一个机动到下一个机动的转换的点。它们也可以表示方向的变化，例如，沿着reed - shepp路径从前进到反向运动。

从计划的路径中检索过渡姿势和方向。

[transitionposed, directions] = interpolate(refPath);可视化规划的路径。情节(motionPlanner)

除了规划的参考路径，请注意地块上的红色区域。这些区域代表成本图中车辆原点(后轴中心)不能越过的区域，以避免撞上任何障碍。pathPlannerRRT通过检查以确保生成的车辆姿态不位于这些区域，找到避开障碍物的路径。

路径平滑和轨迹生成

路径规划器生成的参考路径由Dubins段或reed - shepp段组成。两个这样的节段的连接处的曲率不是连续的，可以导致转向角度的突然变化。为了避免这种不自然的运动，并确保乘客的舒适度，路径需要连续可微，因此[2]是光滑的。平滑路径的一种方法是拟合参数三次样条。样条拟合使您能够生成控制器可以执行的平滑路径。

使用smoothPathSpline拟合经过参考路径中所有过渡点的参数三次样条曲线。样条将车辆的起止方向与车辆的起止航向角近似匹配。

指定要返回的姿势的数量，使用大约0.1的间隔% m。approxSeparation = 0.1;%米numSmoothPoses =圆(refPath。长度/ approxSeparation);沿着平滑的路径返回离散的姿势。[refposed, directions, cumlength, curvatures] = smoothPathSpline(transitionposed, directions, numsmoothposed);绘制平滑路径。持有在hSmoothPath = plot(refposed (:， 1)， refposed (:， 2)，“r”，“线宽”2,…“DisplayName的”，“平滑路径”）;持有从

接下来，将生成的平滑路径转换为可以使用速度配置文件执行的轨迹。计算每个路径的速度剖面，作为三个阶段的序列:加速到设定的最大速度，保持最大速度和减速到终端速度。的helperGenerateVelocityProfile函数生成这样的速度剖面。

指定初始速度、最大速度和最终速度，以便车辆开始静止，加速到5米/秒的速度，然后停下来。

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

生成一个速度剖面。

refvelocity = helperGenerateVelocityProfile(directions, cumlength，曲率，startSpeed, endSpeed, maxSpeed);

refVelocities包含光滑路径上每个点的参考速度。绘制生成的速度剖面。

plotVelocityProfile (cumLengths、refVelocities maxSpeed)

车辆控制与仿真

参考速度与平滑路径一起构成了车辆可以遵循的可行轨迹。一个反馈控制器被用来跟踪这个轨迹。该控制器可以纠正由于轮胎打滑和其他噪声源(如定位不准确)引起的跟踪轨迹的错误。具体来说，控制器由两个组件组成:

横向控制:调整转向角度，使车辆沿参考路径行驶。
纵向控制:在遵循参考路径的同时，通过控制油门和刹车来保持所需的速度。

由于这种情况涉及低速，您可以简化控制器，只考虑一个运动学模型。在本例中，横向控制由lateralControllerStanley函数。纵向控制由辅助系统对象™实现HelperLongitudinalController，它根据比例积分定律计算加减速命令。

反馈控制器需要一个模拟器，它可以使用合适的车辆模型执行所需的控制器命令。的HelperVehicleSimulator类使用以下运动学自行车模型模拟这样的车辆:

$dot x_r = v_r*\cos(\theta)$

$\点y_r = v_r*\sin(\theta)$

$\dot \theta = \frac{v_r}{l}*\tan(\delta)$

$\dot v_r = a_r$

在上述方程中， $(x_r、y_r \θ)美元$ 在世界坐标中表示车辆的姿态。 v_r美元， a_r美元， l美元 , $\三角洲美元$ 分别表示后轮速度、后轮加速度、轴距和转向角。前轮位置和速度可由:

$x_f = x_r + lcos (\theta)$

$y_f = y_r + lsin (\theta)$

$v_f = \frac{v_r} {cos(\delta)}$

关闭所有的数字。closeFigures;创建车辆模拟器。vehicleSim = HelperVehicleSimulator(costmap, vehicleDims);设置车辆的姿态和速度。。vehicleSim.setVehiclePose (currentPose);currentVel = 0;vehicleSim.setVehicleVelocity (currentVel);配置模拟器以显示轨迹。vehicleSim.showTrajectory(真正的);隐藏车辆仿真图。hideFigure (vehicleSim);

创建一个HelperPathAnalyzer对象来计算控制器的参考位姿、参考速度和驱动方向。

pathAnalyzer = HelperPathAnalyzer(refpose, refvelocity, directions，…的轴距, vehicleDims.Wheelbase);

创建一个HelperLongitudinalController对象来控制车辆的速度并指定采样时间。

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

使用HelperFixedRate对象，以确保反馈控制器的固定速率执行。使用与纵向控制器一致的控制速率。

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

在目标达成之前，做以下事情:

计算跟踪计划轨迹所需的转向和加速/减速命令。
向模拟器输入控制命令。
记录返回车辆的姿态和速度，以便在下一次迭代中输入控制器。

reachGoal = false;而~ reachGoal找到路径上的参考位姿和相应的速度。[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);更新模拟器的驾驶方向。。updateDrivingDirection (vehicleSim、方向);%计算转向命令。(refPose, currentPose, currentVel，…“方向”、方向、的轴距, vehicleDims.Wheelbase);计算加速和减速命令。lonController。方向=方向;[accelCmd，减速cmd] = lonController(refVel, currentVel);使用控制器输出模拟车辆。驱动(vehicleSim, accelCmd，减速cmd，转向角度);检查车辆是否达到目标。。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机动(行为规划器，…currentPose currentVel);配置运动规划器。configurePlanner (motionPlanner plannerConfig);计划一个参考路径使用RRT*计划到下一个目标姿势。refPath = plan(motionPlanner, currentPose, nextGoal);检查路径是否有效。如果规划器计算路径失败，%或路径不是无冲突的，因为对映射的更新系统需要重新计划。此场景使用静态映射，因此路径%将永远无碰撞。isReplanNeeded = ~checkPathValidity(refPath, costmap);如果isReplanNeeded警告(“无法找到有效路径。”试图重新计划”。）请求行为规划者重新计划。replanNeeded (behavioralPlanner);继续；结束从计划的路径中检索过渡姿势和方向。[transitionposed, directions] = interpolate(refPath);铺平道路。numSmoothPoses =圆(refPath。长度/ approxSeparation);[refposed, directions, cumlength, curvatures] = smoothPathSpline(transitionposed, directions, numsmoothposed);生成一个速度配置文件。refvelocity = helperGenerateVelocityProfile(directions, cumlength，曲率，startSpeed, endSpeed, maxSpeed);配置路径分析器。pathAnalyzer。RefPoses = RefPoses;pathAnalyzer。方向=方向;pathAnalyzer。VelocityProfile = refVelocities;重置纵向控制器。重置(lonController);reachGoal = false;执行控制循环。而~ reachGoal查找路径上的参考位姿和相应的%的速度。[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);更新模拟器的驾驶方向。。updateDrivingDirection (vehicleSim、方向);%计算转向命令。(refPose, currentPose, currentVel，…“方向”、方向、的轴距, vehicleDims.Wheelbase);计算加速和减速命令。lonController。方向=方向;[accelCmd，减速cmd] = lonController(refVel, currentVel);使用控制器输出模拟车辆。驱动(vehicleSim, accelCmd，减速cmd，转向角度);检查车辆是否达到目标。。reachGoal = helperGoalChecker(nextGoal, currentPose, currentVel, speedConfig。EndSpeed、方向);等待固定利率执行。等待(controlRate);获取当前姿态和速度的车辆。。currentPose = getVehiclePose (vehicleSim);currentVel = getVehicleVelocity (vehicleSim);结束结束%显示车辆仿真图。showFigure (vehicleSim);

停车操作

现在，车辆接近停车点，一个专门的停车机动是用来将车辆停放在最后的停车位。这种操作需要通过一条狭窄的走廊，两边是停车位的边缘。这种机动通常伴随着超声传感器或激光扫描仪不断检查障碍物。

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

的vehicleCostmap使用基于膨胀的碰撞检查。首先，视觉上检查当前使用的碰撞检查器。

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

碰撞检查是通过按膨胀半径膨胀成本图中的障碍来执行的，并检查上图所示的圆心是否位于膨胀的网格单元格上。最后的停车操作需要更精确，更少保守的碰撞检查机制。这通常是通过使用多个(3-5)个重叠的圆而不是一个圆来表示车辆的形状来解决的。

在碰撞检查器中使用更多的圆圈，并视觉检查碰撞检查器。这使得计划可以通过狭窄的通道。

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

更新成本图以使用此冲突检查器。

costmap。CollisionChecker = ccConfig;

注意，膨胀半径已经减小，允许规划师找到一个畅通无阻的路径到停车位。

图绘制(costmap)标题(“带有更新的碰撞检查器的Costmap”）设置pathplannerrt以使用更新的成本图。parkMotionPlanner = pathPlannerRRT (costmap,“MinIterations”, 1000);定义V2X系统返回的停车位置。。parkPose = [36 44 90];preParkPose = currentPose;计算所需的停车机动。refPath = plan(parkMotionPlanner, preparekpose, parkPose);绘制结果停车机动图。。图plotparking机动(costmap, refPath, preparekpose, parkPose)

一旦发现机动，重复前面的过程以确定一个完整的计划:平滑路径，生成速度剖面，并使用反馈控制器跟踪轨迹。

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

另一种停车方式是倒进停车位。当车辆需要倒车到一个点时，运动规划器需要使用reed - shepp连接方法来寻找可行路径。reed - shepp连接在规划过程中允许反向运动。

指定一个停车姿势对应于向后停车机动。。parkPose = [49 47.2 -90];更改连接方法以允许反向运动。parkMotionPlanner。ConnectionMethod =“Reeds-Shepp”；

为了找到一条可行的路径，需要调整运动规划器。使用更大的转弯半径和连接距离，以允许顺利返回。

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

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

结论

这个例子展示了如何:

在半结构化环境(如停车场)中，使用RRT*路径规划算法规划可行路径。
使用样条平滑路径，并沿平滑路径生成一个速度轮廓。
控制车辆以所需的速度沿着参考路径行驶。
通过使用不同的运动规划器设置实现不同的停车行为。

参考文献

[1]比勒，马丁，卡尔·伊阿涅姆玛和桑吉夫·辛格。DARPA城市挑战赛:城市交通中的自动驾驶车辆(第1版)。施普林格出版公司，2009年。

[2] Lepetic, Marko, Gregor Klancar, Igor Skrjanc, Drago Matko, Bostjan Potocnik，“考虑加速限制的时间最优路径规划。”机器人与自主系统．第45卷，2003年3-4期，199-210页。

支持功能

loadParkingLotMapLayers加载停车场环境地图层

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

plotMapLayers包含映射层的Plot结构

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

combineMapLayers将地图层合并为单个成本地图

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

configurePlanner使用指定的设置配置路径规划器

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

plotVelocityProfile情节速度剖面

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

closeFigures

函数closeFigures ()关闭除模拟器可视化外的所有图形。。查找所有的图形对象。figHandles = findobj (“类型”，“图”）;为i = 1: length(fighting andles)如果~ strcmp (figHandles(我)。的名字,“自动代客泊车”)密切(figHandles (i));结束结束结束

plotParkingManeuver在成本图上显示生成的停车机动。

函数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])结束