自动代客泊车

打开脚本

此示例示出了如何构造一个自动停车系统代客。在这个例子中，你将学习，支持路径规划，轨迹生成和车辆控制的工具和技术。金宝app虽然此示例主要关注面向MATLAB®的工作流程，这些工具还提供的Simulink。金宝app在这个例子中金宝app的的Simulink版本，请参阅自动代客在Simulink金宝app。

概观

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

正面和侧面的摄像头检测车道标线，路牌（停止的迹象，出口标志等），其他车辆，行人
激光雷达和用于检测障碍物和计算准确的距离测量传感器的超声波
超声波传感器的障碍物检测
IMU和车轮编码器的航位推算

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

由于车辆传感器感知世界，车辆必须通过向免费停车位环境规划的路径和执行的开车到它所需要的控制动作的顺序。虽然这样做，必须在环境中的动态变化，如行人横过其路径响应，并调整其计划。

此示例实现的功能的子集需要实现这样的系统。它侧重于通过环境规划的可行路径，并执行遍历路径需要采取的行动。地图创建和动态避障被排除在这个例子。

环境模型

环境模型代表环境地图的。对于代客系统，该地图包括可用并占领停车位，道路标记，以及诸如行人或其他车辆的障碍。入住地图对于这种形式的环境模型的通用表示。这样的图是利用同步定位和地图创建（SLAM）通过从激光雷达和摄像机传感器集成观测通常构建。本实施例中集中在一个更简单的情形，其中一个地图已经被提供，例如，由车辆对基础设施（V2X）系统或俯瞰整个停车空间的照相机。它采用一个停车场的静态地图，并假设车辆的自定位是准确的。

在这个例子中使用的停车场例子是由三个占用网格层。

固定的障碍：这一层包含如墙壁，障碍物和停车场的界限固定的障碍。
道路标记层：这一层包含与道路标记，包括道路标志车位占用信息。
停好车：这一层包含关于停车位已被占用的信息。

每个地图层包含不同类型的代表不同级别的危险一辆车通过它导航的障碍。利用这种结构，每个层可以被处理，更新和独立地保持。

加载和显示三个图层。在每一层中，暗细胞代表占用细胞，和光细胞代表游离细胞。

mapLayers = loadParkingLotMapLayers;plotMapLayers（mapLayers）

为了简单起见，这三个层结合成一个单一costmap。

costmap = combineMapLayers（mapLayers）;图图（costmap，'通货膨胀'，“关”）传说离

合并costmap是vehicleCostmap对象，它表示车辆环境作为2 d占用网格。小区中的每个网格具有0和1之间的值，表示通过细胞导航的成本。障碍有较高的性价比，而自由空间具有更低的成本。小区被认为是障碍如果成本比更高OccupiedThreshold财产，免费的，如果它的成本比低FreeThreshold属性。

该costmap覆盖整个75米-通过-50M停车场面积，分为0.5米逐0.5米方形单元。

costmap.MapExtent% [x，宽度，y，高度]，单位为米costmap.CellSize在米％的细胞大小

ans = 0 75 0 50 ans = 0.5000

创建一个vehicleDimensions对象，用于存储将自动将车辆驻车的尺寸。还定义了车辆的最大转向角。此值确定运动计划和控制期间，在转弯半径的限制。

vehicleDims = vehicleDimensions;maxSteeringAngle = 35;％的度

更新VehicleDimensions与车辆的尺寸costmap碰撞检查的财产停车。此设置在调整周围的障碍物，以对应于车辆的大小的地图膨胀被停放的程度，从而确保可以通过停车场被发现，无碰撞路径。

costmap.CollisionChecker.VehicleDimensions = vehicleDims;

定义车辆的起始姿势。姿势是通过定位，这是本实施例中为了简化省略了获得。车辆姿态被指定为 $[X，Y，\ THETA]$ 在世界坐标。 $（X，Y）$ 表示车辆的后轴的在世界坐标系中的中心的位置。 $$ \ $ THETA$ 表示车辆相对于世界X轴方向。有关详细信息，请参阅坐标系统在自动驾驶的工具箱。

currentPose = [4 12 0];％[X，Y，θ-]

行为层

规划涉及组织所有相关信息，为分级层中。每个连续的层负责一个更细粒度的任务。行为层[1]坐在该堆叠的顶部。它负责激活并通过提供的导航任务序列管理任务的不同部分。行为层从系统，包括所有相关部分组装信息：

位置：该行为层检查的车辆的当前位置的估计定位模块。
环境模型:感知和传感器融合系统报告车辆周围环境的地图。
确定停车位:行为层分析地图以确定最近的可用停车位。
寻找全球航线：路由模块通过计算从一个地图服务或从V2X基础设施得到无论是路网的全局路径。分解为一系列道路联络线的全球航线允许轨迹进行不同的规划每一个环节。例如，最终的驻车操作需要比的方法来停车点不同的速度曲线。在更一般的设置，这将成为通过涉及不同速度的限制，车道数量，和路标街道导航至关重要。

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

该HelperBehavioralPlanner类模仿行为规划层的界面。该HelperBehavioralPlanner使用地图和全球航线计划创建。此示例使用存储在MATLAB表中的静态全球航线计划，但通常由当地停车基础设施或地图服务提供的路由算法决定了这个计划。全局路由方案被描述为车道段的序列来遍历到达停车位。

加载包含存储在表中的路由计划的mat文件。该表有三个变量:StartPose，EndPose和属性。StartPose和EndPose指定段的开始和结束姿势，表示为 $[X，Y，\ THETA]$ 。属性指定诸如速度限制的链段的性质。

数据=负载（'routePlan.mat'）;routePlan = data.routePlan％＃确定

routePlan = 4×3表StartPose EndPose属性______________ ________________ ____________ 4 12 0 56 11 0 [1×1结构] 56 11 0 70 19 90 [1×1结构] 70 19 90 70 32 90 [1×1结构] 70 3290 53 39 180 [1×1结构]

在当前姿态绘制车辆，以及沿航线计划中的每个目标。

在当前姿态％车辆地块保持上helperPlotVehicle（currentPose，vehicleDims，'显示名称'，“当前姿势”）传说对于n = 1时：高度（routePlan）％提取目标航点vehiclePose = routePlan {N，'EndPose'};％绘制的姿势legendEntry = sprintf ('进球％I'中，n）;helperPlotVehicle（vehiclePose，vehicleDims，'显示名称',legendEntry);结束保持离

创建行为策划师辅助对象。该requestManeuver到达目的地，直到方法请求从行为规划师导航任务流。

behavioralPlanner = HelperBehavioralPlanner（routePlan，maxSteeringAngle）;

车辆使用以下步骤导航每个路径段:

运动规划：使用最佳的快速随机树（RRT *）算法，通过规划环境地图的可行路径（pathPlannerRRT）。
路径平滑：通过使用样条拟合到它平滑参考路径smoothPathSpline。
轨迹生成：通过使用产生速度曲线转换平滑路径为轨迹helperGenerateVelocityProfile。
车辆控制：考虑到平滑的参考路径，HelperPathAnalyzer基于所述车辆的当前姿势和速度的基准姿势和速度。与参考值提供，lateralControllerStanley计算转向角度来控制车辆的航向。HelperLongitudinalController计算加速和减速指令，以维持所需的车辆速度。
目标检查：检查车辆已达到使用段的最终姿态helperGoalChecker。

该实施例的其余部分详细描述了这些步骤，将它们组装成一个完整的解决方案之前。

运动规划

由于全球性的路线，运动规划可用于通过环境规划的路径到达每个中间航点，直到车辆到达最终目的地。每个链接的规划路径必须是可行的，无碰撞。一个可行的路径是一个可以由下式给出的运动和强加于它动态约束车辆来实现。一个代客系统包括低速和低加速度。这使我们能够安全地忽略惯性效应引起的动态约束。

创建一个pathPlannerRRT对象使用最优快速随机树（RRT *）的方式来配置路径规划。规划算法的RRT家人发现通过构建连接，无碰撞的车辆姿态的树的路径。姿势使用的Dubins或芦苇-Shepp转向连接，从而确保所生成的路径是可行的运动学。

motionPlanner = pathPlannerRRT（costmap，'MinIterations'，1000，...'ConnectionDistance'10，'MinTurningRadius'，20）;

通过该计划从当前姿势到第一目标的路径计划函数。返回的driving.Path宾语，refPath，是一种可行的和无碰撞的参考路径。

goalPose = routePlan {1，'EndPose'};refPath =计划（motionPlanner，currentPose，goalPose）;

参考路径由路径段的序列组成。每个路径段介绍了一组用于连接到下一个段的Dubins或芦苇-Shepp操纵的。检查路径段。

refPath.PathSegments

ANS = 1×6 DubinsPathSegment具有属性阵列：StartPose GoalPose MinTurningRadius MotionLengths MotionTypes长度

参考路径包含过渡姿势沿途，表示沿着对应于从一个操纵到下一个过渡路径点。它们也可以表示在方向上的变化，例如，从正向沿芦苇-Shepp路径反向运动。

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

[transitionpose, directions] =插值(refPath);％可视化的规划路径图（motionPlanner）

除了计划参考路径，注意在图上红色区域。这些区域代表其中车辆的原点（后轴中心）不能在阶交叉避免击中任何障碍costmap的区域。pathPlannerRRT的发现路径，通过检查，以确保产生不处在这些领域的车辆姿态避开障碍物。

路径平滑和轨迹生成

由路径规划器生成的基准路径的任一的Dubins或芦苇-Shepp段组成。在两个这样的节段的接合处的弯曲度不连续的并且可能会导致突然变化与转向角。为了避免这样的不自然的运动，并保证乘客的舒适性，路径需要是连续可微的，因此光滑[2]。一种方法来平滑路径包括拟合参数三次样条。花键配合可以生成一个控制器可以执行的平滑路径。

使用smoothPathSpline适合通过在所述参考路径中的所有过渡点通过的参数三次样条。花键大致匹配车辆的开始和结束与起始方向和结束航向角。

％指定的姿势的数目使用的大约0.1μm的分离，以返回approxSeparation = 0.1;％米numSmoothPoses = ROUND（refPath.Length / approxSeparation）;％返回离散的姿势沿平滑路径[refPoses，方向，cumLengths，曲率] = smoothPathSpline（transitionPoses，方向，numSmoothPoses）;％绘制平滑路径保持上hSmoothPath =情节（refPoses（：，1），refPoses（：，2），'R'，'行宽'，2，...'显示名称'，“平滑路径”）;保持离

接着，转换所生成的平滑路径，可以使用速度曲线来执行的轨迹。计算每个路径的三个阶段的序列的速度曲线：加速到设定最大速度，保持最大速度和减速到终端速度。该helperGenerateVelocityProfile函数生成这样的速度曲线。

使得车辆开始静止，加速到5米/秒的速度，并停下来指定初始值，最大值，和终端的速度。

MAXSPEED = 5;％的米/秒startSpeed = 0;％的米/秒endSpeed = 0;％的米/秒

生成速度曲线

refVelocities = helperGenerateVelocityProfile（方向，cumLengths，曲率，startSpeed，endSpeed，MAXSPEED）;

refVelocities包含用于沿平滑的路径每个点的参考速度。画出产生的速度轮廓。

plotVelocityProfile（cumLengths，refVelocities，MAXSPEED）

车辆控制与仿真

基准速度，与平滑的路径一起，包括可行轨迹能够使车辆跟随。反馈控制器来遵循这个轨迹。该控制器在跟踪从轮胎打滑和其它噪声源，例如在定位不精确出现轨迹校正错误。特别地，控制器由两个部件组成：

横向控制：调整转向角度，使得车辆跟随参考路径。
纵向控制：虽然下面参考路径，通过控制油门和制动器保持所需的速度。

由于该方案涉及低速，可以简化控制器只考虑运动学模型。在这个例子中，横向的控制由实现lateralControllerStanley函数。纵向控制由辅助系统对象实现™HelperLongitudinalController，，基于比例 - 积分法单位计算的加速和减速的命令。

反馈控制器需要可使用合适的车辆模型执行期望的控制器的命令的模拟器。该HelperVehicleSimulatorclass使用以下运动学自行车模型来模拟这种车辆:

$\点x_r = V_R * \ COS（\ THETA）$

$\点y_r = V_R * \罪（\ THETA）$

$\点\ THETA = \压裂{V_R} {1} * \黄褐色（\三角洲）$

$\点V_R = A_R$

在上述等式中， $（x_r，y_r，\ THETA）$ 代表世界坐标中的车辆姿态。 $ V_R $ ， $ A_R $ ， $ L $ 和 $$ \ $三角洲$ 表示后轮速度，后轮加速度，轴距，和转向角，分别。前轮的位置和速度可以通过以下步骤获得：

$x_f = x_r + 1 COS（\ THETA）$

$y_f = y_r + 1 SIN（\ THETA）$

$V_F = \压裂{V_R} {COS（\三角洲）}$

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

创建一个HelperPathAnalyzer目的是计算参考姿态，参考速度和行驶方向的控制器。

pathAnalyzer = HelperPathAnalyzer（refPoses，refVelocities，方向，...“轴距”，vehicleDims.Wheelbase）;

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

sampleTime = 0.05;lonController = HelperLongitudinalController（'采样时间'， 采样时间）;

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

controlRate = HelperFixedRate（1 / sampleTime）;％的赫兹

直到达到目标，做到以下几点：

计算需要跟踪计划轨迹的转向和加速/减速命令。
进给控制命令到模拟器。
记录返回的车辆姿态和速度，以进料至下一次迭代的控制器。

reachGoal = FALSE;而〜reachGoal％查找路径上的参考姿态和相应的速度[refPose，refVel，方向] = pathAnalyzer（currentPose，currentVel）;％更新驱动方向为模拟器updateDrivingDirection（vehicleSim，方向）;％计算转向命令steeringAngle = lateralControllerStanley（refPose，currentPose，currentVel，...'方向'，方向“轴距”，vehicleDims.Wheelbase）;％计算的加速和减速指令lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);使用％模拟车辆中的控制器输出驱动（vehicleSim，accelCmd，decelCmd，steeringAngle）;检查车辆是否达到目标reachGoal = helperGoalChecker（goalPose，currentPose，currentVel，endSpeed，方向）;％等待固定速率执行WAITFOR（controlRate）;％获取车辆的当前姿态和速度currentPose = getVehiclePose（vehicleSim）;currentVel = getVehicleVelocity（vehicleSim）;结束％显示车辆仿真图showFigure（vehicleSim）;

这样就完成了路径计划的所述第一腿部和显示处理过程的每个步骤。接下来的部分运行的整个路线，这需要车辆靠近停车位，并最终执行停车过程将车辆进入停车位模拟器。

执行一个完整的计划

下面结合前面所有的步骤，在规划过程中，运行模拟的完整路线规划。这个过程涉及将行为策划者。

％中集车辆姿态回到最初的起点currentPose = [4 12 0];％[X，Y，θ-]vehicleSim.setVehiclePose (currentPose);％重置速度currentVel = 0;％米/秒vehicleSim.setVehicleVelocity（currentVel）;而〜reachedDestination（behavioralPlanner）从行为层％请求下一个动作[nextGoal, plannerConfig, speedConfig] = request机动(behavioralPlanner，...currentPose，currentVel）;％配置运动计划configurePlanner（motionPlanner，plannerConfig）;％计划的参考路径采用RRT *规划者的下一个目标姿势refPath =计划（motionPlanner，currentPose，nextGoal）;％检查路径是否有效。如果规划者无法计算路径，％或路径是因为更新的地图不无碰撞时，％系统需要重新计划。此方案使用静态地图，这样的路径％将永远是无碰撞。isReplanNeeded =〜checkPathValidity（refPath，costmap）;如果isReplanNeeded警告（“无法找到一个有效的路径。试图重新计划“。）％请求行为规划者重新规划replanNeeded（behavioralPlanner）;继续;结束％从检索的规划路径转换的姿势和方向[transitionpose, directions] =插值(refPath);使道路平坦numSmoothPoses = ROUND（refPath.Length / approxSeparation）;[refPoses，方向，cumLengths，曲率] = smoothPathSpline（transitionPoses，方向，numSmoothPoses）;％生成速度曲线refVelocities = helperGenerateVelocityProfile（方向，cumLengths，曲率，startSpeed，endSpeed，MAXSPEED）;％配置路径分析器pathAnalyzer.RefPoses = refPoses;pathAnalyzer.Directions =方向;pathAnalyzer.VelocityProfile = refVelocities;％重置纵向控制器复位（lonController）;reachGoal = FALSE;％执行控制回路而〜reachGoal％查找路径上的参考姿态和相应的速度[refPose，refVel，方向] = pathAnalyzer（currentPose，currentVel）;％更新驱动方向为模拟器updateDrivingDirection（vehicleSim，方向）;％计算转向命令steeringAngle = lateralControllerStanley（refPose，currentPose，currentVel，...'方向'，方向“轴距”，vehicleDims.Wheelbase）;％计算的加速和减速指令lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);使用％模拟车辆中的控制器输出驱动（vehicleSim，accelCmd，decelCmd，steeringAngle）;检查车辆是否达到目标reachGoal = helperGoalChecker（nextGoal，currentPose，currentVel，speedConfig.EndSpeed，方向）;％等待固定速率执行WAITFOR（controlRate）;％获取车辆的当前姿态和速度currentPose = getVehiclePose（vehicleSim）;currentVel = getVehicleVelocity（vehicleSim）;结束结束％显示车辆仿真图showFigure（vehicleSim）;

停车场机动

现在车辆靠近停车点，有专门的驻车操作使用在最后停车位停放车辆。这个动作，需要通过由两端的停车点的边缘侧接的狭窄的走廊。这种机动通常伴随着超声波传感器或激光扫描仪的障碍物连续地检查。

％隐藏车辆模拟图hideFigure（vehicleSim）;

该vehicleCostmap使用基于通货膨胀的碰撞检测。首先，目视检查当前使用的碰撞检查。

ccConfig = costmap.CollisionChecker;图图（ccConfig）称号（“当前碰撞检查”）

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

使用圈子更大数量的碰撞检查和目视检查碰撞检查。这使得规划通过狭窄的通道。

ccConfig.NumCircles = 4;图图（ccConfig）称号（“新碰撞检查”）

更新使用此碰撞检查的costmap。

costmap.CollisionChecker = ccConfig;

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

图图（costmap）称号（“Costmap与更新碰撞检查”）设置pathPlannerRRT以使用更新的成本图parkMotionPlanner = pathPlannerRRT（costmap，'MinIterations'，1000）;％定义停车点所需的姿势，由V2X系统返回parkPose = [36 44 90];preParkPose = currentPose;％计算所需的停车引导refPath =计划（parkMotionPlanner，preParkPose，parkPose）;％绘制所产生的停车引导图plotParkingManeuver（costmap，refPath，preParkPose，parkPose）

一旦演习中发现，重复前面的过程，以确定一个完整的计划：铺平道路，产生的速度特性和使用反馈控制器遵循的轨迹。

％从检索的规划路径转换的姿势和方向[transitionpose, directions] =插值(refPath);使道路平坦numSmoothPoses = ROUND（refPath.Length / approxSeparation）;[refPoses，方向，cumLengths，曲率] = smoothPathSpline（transitionPoses，方向，numSmoothPoses）;％设置速度曲线发生器停在轨道末端，％与速度限制5英里每小时的refVelocities = helperGenerateVelocityProfile（方向，cumLengths，曲率，currentVel，0，2.2352）;pathAnalyzer.RefPoses = refPoses;pathAnalyzer.Directions =方向;pathAnalyzer.VelocityProfile = refVelocities;％重置纵向控制器复位（lonController）;reachGoal = FALSE;而〜reachGoal％查找路径上的参考姿态和相应的速度[refPose，refVel，方向] = pathAnalyzer（currentPose，currentVel）;％更新驱动方向为模拟器updateDrivingDirection（vehicleSim，方向）;％计算转向命令steeringAngle = lateralControllerStanley（refPose，currentPose，currentVel，...'方向'，方向“轴距”，vehicleDims.Wheelbase）;％计算的加速和减速指令lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);使用％模拟车辆中的控制器输出驱动（vehicleSim，accelCmd，decelCmd，steeringAngle）;检查车辆是否达到目标reachGoal = helperGoalChecker（parkPose，currentPose，currentVel，0，方向）;％等待固定速率执行WAITFOR（controlRate）;％获取车辆的当前姿态和速度currentPose = getVehiclePose（vehicleSim）;currentVel = getVehicleVelocity（vehicleSim）;结束％显示车辆仿真图closeFigures;showFigure（vehicleSim）;

停放车辆的另一种方式是将背入停车位。当车辆需要备份成斑，运动计划需要使用芦苇-Shepp连接方法来搜索的可行路径。蒹葭-Shepp连接允许规划过程中的反向运动。

％指定对应于背在泊车操控停车姿势parkPose = [49 47 -90]。变化百分比的连接方法，以允许反向运动parkMotionPlanner.ConnectionMethod ='蒹葭-Shepp';

为了找到一个可行的路径，来调整运动计划的需求。使用较大的转弯半径和连接距离，以便为顺利回项。

parkMotionPlanner.MinTurningRadius = 10;％米parkMotionPlanner.ConnectionDistance = 15;％重置车辆姿势和速度currentVel = 0;vehicleSim.setVehiclePose（preParkPose）;vehicleSim.setVehicleVelocity（currentVel）;计算停车机动重新计划= TRUE;而重新计划refPath =计划（parkMotionPlanner，preParkPose，parkPose）;％对应于停车操作的路径是小，并且需要％精确操纵。相反，只有在过渡姿势插值的，％内插沿路径的长度更精细。NUMSAMPLES = 10;stepSize的= refPath.Length / NUMSAMPLES;长度= 0：stepSize的：refPath.Length;[transitionPoses，方向] =内插（refPath，长度）;％重新计划如果路径中包含多于一个方向切换的姿势％或者如果路径太长重新计划=总和（绝对值（差异（方向）））〜= 2 ||refPath.Length> 20;结束％形象化停车操作图plotParkingManeuver（costmap，refPath，preParkPose，parkPose）

铺平道路

numSmoothPoses = ROUND（refPath.Length / approxSeparation）;[refPoses，方向，cumLengths，曲率] = smoothPathSpline（transitionPoses，方向，numSmoothPoses，0.5）;％生成速度曲线refVelocities = helperGenerateVelocityProfile（方向，cumLengths，曲率，currentVel，0，2）;pathAnalyzer.RefPoses = refPoses;pathAnalyzer.Directions =方向;pathAnalyzer.VelocityProfile = refVelocities;％重置纵向控制器复位（lonController）;reachGoal = FALSE;而〜reachGoal％获取当前行驶方向currentDir = getDrivingDirection（vehicleSim）;％查找路径上的参考姿势和相应速度。[refPose，refVel，方向] = pathAnalyzer（currentPose，currentVel）;％如果车辆改变行驶方向，复位车速在％模拟器和复位纵向控制器如果currentDir〜=方向currentVel = 0;setVehicleVelocity（vehicleSim，currentVel）;复位（lonController）;结束％更新驱动方向为模拟器。如果车辆的变化％行车路线，复位并返回当前车速为零。currentVel = updateDrivingDirection（vehicleSim，方向，currentDir）;％计算转向命令steeringAngle = lateralControllerStanley（refPose，currentPose，currentVel，...'方向'，方向“轴距”，vehicleDims.Wheelbase）;％计算的加速和减速指令lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);使用％模拟车辆中的控制器输出驱动（vehicleSim，accelCmd，decelCmd，steeringAngle）;检查车辆是否达到目标reachGoal = helperGoalChecker（parkPose，currentPose，currentVel，0，方向）;％等待固定速率执行WAITFOR（controlRate）;％获取车辆的当前姿态和速度currentPose = getVehiclePose（vehicleSim）;currentVel = getVehicleVelocity（vehicleSim）;结束以快照为例closeFigures;snapnow;％删除模拟器删除（vehicleSim）;

结论

这个例子说明了如何：

使用RRT*路径规划算法在半结构化环境(如停车场)中规划可行路径。
使用样条平滑路径和产生沿着平滑路径的速度曲线。
控制车辆跟随参考路径以期望的速度。
通过使用不同的运动计划设置实现不同的停车行为。

参考文献

[1]标乐，马丁，卡尔·格尼马，和桑吉夫辛格。DARPA的城市挑战：自主车在城市交通（第1版）。施普林格出版公司，股份有限公司，2009年。

[2] Lepetic，马尔科，格雷戈尔Klancar，伊戈尔Skrjanc，德拉戈Matko，波斯蒂安波托奇尼克， “时间最优路径规划考虑加速度极限。”机器人和自治系统。第45卷，3-4个问题，2003年，页199-210。

金宝app支持功能

loadParkingLotMapLayers对于停车场负载环境地图图层

功能mapLayers = loadParkingLotMapLayers（）％loadParkingLotMapLayers％负载占用映射对应于3层 - 障碍物，道路％的标记，并使用斑点。mapLayers.StationaryObstacles = imread（“stationary.bmp”）;mapLayers.RoadMarkings = imread（'road_markings.bmp'）;mapLayers.ParkedCars = imread（'parked_cars.bmp'）;结束

plotMapLayers包含地图层积结构体

功能plotMapLayers（mapLayers）％plotMapLayers％绘制一个图形窗口上的多个地图层。图cellOfMaps = cellfun（@imcomplement，struct2cell（mapLayers），'UniformOutput'，假）;蒙太奇（cellOfMaps，'尺寸'[1 numel（cellOfMaps）]'边境'[5 5]，'ThumbnailSize'，[300为NaN]）标题（“地图图层 - 固定障碍物，道路标记和停放的汽车”）结束

combineMapLayers结合图层成单个costmap

功能costmap = combineMapLayers（mapLayers）％combineMapLayers％结合图层结构成一个单一的vehicleCostmap。combinedMap = mapLayers.StationaryObstacles + mapLayers.RoadMarkings +...mapLayers.ParkedCars;combinedMap = im2single（combinedMap）;RES = 0.5;％米costmap = vehicleCostmap（combinedMap，'CELLSIZE'，RES）;结束

configurePlanner与指定的设置配置路径规划

功能configurePlanner（pathPlanner，配置）％configurePlanner％配置路径规划对象，pathPlanner，与设置中指定％的结构配置。FIELDNAMES =字段（配置）;对于n = 1时：numel（FIELDNAMES）如果〜strcmpi（FIELDNAMES {N}，'IsParkManeuver'。）pathPlanner（FIELDNAMES {N}）=配置（FIELDNAMES {N}）。结束结束结束

plotVelocityProfile绘图速度曲线

功能plotVelocityProfile（cumPathLength，refVelocities，MAXSPEED）％plotVelocityProfile％绘制所生成的速度分布％沿路径的长度叠加的参考速度图（cumPathLength，refVelocities，'行宽'，2）;％绘制一条线，以显示最大速度保持上线（[0; cumPathLength（结束）]，[MAXSPEED; MAXSPEED]“颜色”，'R'）保持离％组轴限制缓冲= 2;xlim ([0 cumPathLength(结束)]);ylim([0 maxSpeed + buffer])％添加标签xlabel（“累积路径长度（m）”）;ylabel（“速度（米/秒）”）;％添加图例和标题传说（“速度曲线”，'最大速度'）标题（生成速度剖面的）结束

closeFigures

功能closeFigures（）％关闭所有除模拟器可视化的数字％找到所有的图形对象figHandles = findobj（'类型'，“图”）;对于I = 1：长度（figHandles）如果〜的strcmp（figHandles（ⅰ）.name和“自动代客泊车”）关闭（figHandles（I））;结束结束结束

plotParkingManeuver显示器上的costmap所生成的停车操作

功能plotParkingManeuver（costmap，refPath，currentPose，parkPose）％plotParkingManeuver％上绘制costmap生成的停车引导。绘制成本图，不包括膨胀的区域。图（costmap，'通货膨胀'，“关”）在costmap％地块参考停放调度保持上图（refPath，'显示名称'，“停车引导”）标题（“停车引导”）通过设置轴向限制，可缩放到停车机动LO =分钟（[currentPose（1：2）; parkPose（1：2）]）;HI = MAX（[currentPose（1：2）; parkPose（1：2）]）;缓冲= 6;％米xlim([lo(1)-buffer hi(1)+buffer]) ylim([lo(2)-buffer hi(2)+buffer])结束