自动代泊车员

打开脚本

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

概述

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

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

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

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

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

环境模型

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

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

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

每个地图层都包含不同类型的障碍物，代表着汽车在其中行驶时的不同危险程度。使用这种结构，每一层都可以独立地处理、更新和维护。

加载并显示三个地图层。在每一层中，深色细胞代表已占用细胞，浅色细胞代表空闲细胞。

mapLayers = loadParkingLotMapLayers;plotMapLayers (mapLayers)

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

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

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

的costmap覆盖整个75米乘50米的停车场区域，分为0.5米乘0.5米的方形单元。

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

Ans = 0 75 0 50 Ans = 0.5000

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

vehicleDims =车辆尺寸;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,θ\]美元$ ．属性指定段的属性，例如速度限制。

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

routePlan = 4×3 table StartPose EndPose Attributes ______________ ________________ __________ 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: height(routePlan)提取目标路径点vehiclePose = routePlan{n，“EndPose”};绘制姿势legendEntry = sprintf(“目标%我”n);helperPlotVehicle (vehiclePose vehicleDims,“DisplayName的”, legendEntry);结束持有从

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

行为计划=帮助行为计划(routePlan, maxSteeringAngle);

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

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

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

运动规划

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

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

motionPlanner = pathPlannerRRT(成本地图，“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路径从向前运动到反向运动。

从计划的路径中检索转换姿势和方向。

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

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

路径平滑和轨迹生成

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

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

使用大约0.1的间隔指定返回的姿势数% m。approxSeparation = 0.1;%米numsmoothposed = round(refPath.)长度/近似分离);沿平滑路径返回离散的姿态。[refpositions, directions, cumlength, curvatures] = smoothPathSpline(transitionpositions, directions, numsmoothpositions);绘制平滑路径。持有在hSmoothPath = plot(refpose (:， 1)， refpose (:， 2)，“r”，“线宽”2,.．.“DisplayName的”，“平滑路径”）;持有从

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

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

maxSpeed = 5;%，单位为米/秒startSpeed = 0;%，单位为米/秒endSpeed = 0;%，单位为米/秒

生成一个速度剖面。

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

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

plotVelocityProfile(cumlength, refvelocity, maxSpeed)

车辆控制与仿真

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

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

由于这种情况涉及到较慢的速度，您可以简化控制器，只考虑运动学模型。在本例中，横向控制由lateralControllerStanley函数。纵向控制由辅助系统对象™实现HelperLongitudinalController，该软件根据比例积分定律计算加速和减速指令。

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

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

$\dot 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 = helpvehiclesemulator(成本图，vehicleDims);设置车辆的姿态和速度。vehicleSim.setVehiclePose (currentPose);currentVel = 0;vehicleSim.setVehicleVelocity (currentVel);配置模拟器以显示弹道。vehicleSim.showTrajectory(真正的);隐藏车辆模拟图。hideFigure (vehicleSim);

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

pathAnalyzer = HelperPathAnalyzer(refpose, refvelocity，方向，.．.的轴距, vehicleDims.Wheelbase);

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

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

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

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

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

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

reachGoal = false;而~ reachGoal找到路径上的参考位姿和相应的速度。[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);更新模拟器的驾驶方向。updateDrivingDirection (vehicleSim、方向);计算转向命令。转向角度=侧控制器stanley (refPose, currentPose, currentVel，.．.“方向”、方向、的轴距, vehicleDims.Wheelbase);计算加速和减速命令。lonController。方向=方向;[accelCmd，减速度cmd] = lonController(refVel, currentVel);使用控制器输出模拟车辆。drive(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] =请求机动(行为计划，.．.currentPose currentVel);配置运动规划器。configurePlanner (motionPlanner plannerConfig);使用RRT*规划器计划到下一个目标姿态的参考路径。refPath = plan(motionPlanner, currentPose, nextGoal);检查路径是否有效。如果规划器无法计算路径，%或路径不是无碰撞的，因为映射的更新%系统需要重新规划。此场景使用静态映射，因此路径%将总是无碰撞的。isReplanNeeded = ~checkPathValidity(refPath, costmap);如果isReplanNeeded警告(“无法找到有效路径。试图重新规划。”）请求行为规划师重新计划replanNeeded (behavioralPlanner);继续；结束从计划的路径中检索转换姿势和方向。[transitionpositions, directions] = interpolate(refPath);使路径平滑。numsmoothposed = round(refPath.)长度/近似分离);[refpositions, directions, cumlength, curvatures] = smoothPathSpline(transitionpositions, directions, numsmoothpositions);生成一个速度剖面。refvelocity = helperGenerateVelocityProfile(方向，cumlength，曲率，startSpeed, endSpeed, maxSpeed);配置路径分析器。pathAnalyzer。refpose = refpose;pathAnalyzer。方向=方向;pathAnalyzer。VelocityProfile = refvelocity;%复位纵向控制器。重置(lonController);reachGoal = false;执行控制循环。而~ reachGoal找到路径上的参考位姿和对应的%的速度。[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);更新模拟器的驾驶方向。updateDrivingDirection (vehicleSim、方向);计算转向命令。转向角度=侧控制器stanley (refPose, currentPose, currentVel，.．.“方向”、方向、的轴距, vehicleDims.Wheelbase);计算加速和减速命令。lonController。方向=方向;[accelCmd，减速度cmd] = lonController(refVel, currentVel);使用控制器输出模拟车辆。drive(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）设置pathPlannerRRT以使用更新后的成本图。parkMotionPlanner = pathPlannerRRT(成本地图，“MinIterations”, 1000);定义V2X系统返回的停车位所需位置。%parkPose = [36 44 90];preparekpose = currentPose;计算所需的泊车机动。refPath = plan(parkMotionPlanner, preparekpose, parkPose);绘制结果停车机动图。figure plotparking机动(costmap, refPath, preparekpose, parkPose)

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

从计划的路径中检索转换姿势和方向。[transitionpositions, directions] = interpolate(refPath);使路径平滑。numsmoothposed = round(refPath.)长度/近似分离);[refpositions, directions, cumlength, curvatures] = smoothPathSpline(transitionpositions, directions, numsmoothpositions);设置速度剖面发生器，使其在弹道结束时停止，%速度限制为每小时5英里。refvelocity = helperGenerateVelocityProfile(方向，cumlength，曲率，currentVel, 0, 2.2352);pathAnalyzer。refpose = refpose;pathAnalyzer。方向=方向;pathAnalyzer。VelocityProfile = refvelocity;%复位纵向控制器。重置(lonController);reachGoal = false;而~ reachGoal找到路径上的参考位姿和相应的速度。[refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);更新模拟器的驾驶方向。updateDrivingDirection (vehicleSim、方向);计算转向命令。转向角度=侧控制器stanley (refPose, currentPose, currentVel，.．.“方向”、方向、的轴距, vehicleDims.Wheelbase);计算加速和减速命令。lonController。方向=方向;[accelCmd，减速度cmd] = lonController(refVel, currentVel);使用控制器输出模拟车辆。drive(vehicleSim, accelCmd, cmd，转向角度);检查车辆是否达到目标。。reachGoal = helpgoalchecker (parkPose, currentPose, currentVel, 0，方向);%等待固定速率执行。等待(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);计算停车机动。Replan = true;而replan refPath = plan(parkMotionPlanner, preparekpose, parkPose);停车机动对应的路径很小，需要精确操纵。不是只在过渡姿势上进行插值，沿着路径的长度插入更精细。numSamples = 10;stepSize = refPath。长度/ numSamples;长度= 0:stepSize: refPath.Length;[transitionpositions, directions] = interpolate(refPath，长度);%如果路径包含多个方向切换姿势，则重新规划%或路径太长。replan = sum(abs(diff(directions)))~=2 || refPath。长度> 20;结束视觉化停车动作。figure plotparking机动(costmap, refPath, preparekpose, parkPose)

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

结论

这个例子展示了如何:

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

参考文献

比勒、马丁、卡尔·伊格尼玛和桑吉夫·辛格。DARPA城市挑战赛:城市交通中的自动驾驶汽车(第1版)。施普林格出版股份有限公司，2009。

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

金宝app支持功能

loadParkingLotMapLayers装载停车场的环境地图层

函数mapLayers = loadParkingLotMapLayers()% loadParkingLotMapLayers%负载占用地图对应3层-障碍，道路%标记和使用过的斑点。mapLayers。固定障碍= 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 numel(cellOfMaps)]，“边界”， [5 5]，“ThumbnailSize”， [300 NaN])标题(地图层-固定障碍，道路标记和停放的汽车）结束

combineMapLayers将地图图层组合成一个成本地图

函数costmap = combineMapLayers(mapLayers)% combineMapLayers将地图层结构组合成一个单独的车辆ostmap。combinedMap = mapLayers。静止障碍+玩家。路标+.．.mapLayers.ParkedCars;组合地图= im2single(组合地图);Res = 0.5;%米costmap = vehicleCostmap(组合映射，“CellSize”res);结束

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

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

plotVelocityProfile地块速度剖面

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

closeFigures

函数closeFigures ()%关闭除模拟器可视化外的所有图形。查找所有图形对象。打架= 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)]);hi = max([currentPose(1:2);parkPose (1:2)]);Buffer = 6;%米Xlim ([lo(1)-buffer hi(1)+buffer]) ylim([lo(2)-buffer hi(2)+buffer])结束