自动停车员

打开脚本

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

概述

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

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

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

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

此示例实现了实现此类系统所需的功能子集。它关注于在环境中规划一条可行的路径，并执行遍历该路径所需的操作。本例中不包括地图创建和动态避障。

环境模型

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

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

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

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

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

mapLayers = loadParkingLotMapLayers;plotMapLayers (mapLayers)

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

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

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

的成本图占地面积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，]

行为层

规划包括将所有相关信息组织到分层的层次中。每个后续层负责一个更细粒度的任务。行为层[1]位于这个堆栈的顶部。它负责启动和管理任务的不同部分，提供一系列导航任务。行为层从系统的所有相关部分收集信息，包括:

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

本例使用的地图来自一个智能停车场，通过V2X通信，而不是依靠车辆传感器来构建环境地图。为简单起见，假设该地图以占用网格的形式呈现，V2X提供了道路连接和可用停车位的位置。

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

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

data =负载(“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,“结束姿势”};%绘制姿势legendEntry = sprintf (“目标%我”n);helperPlotVehicle (vehiclePose vehicleDims,“DisplayName的”, legendEntry);结束持有从

创建行为规划器助手对象。的请求机动方法从行为规划器请求导航任务流，直到到达目标。

behavioralPlanner=HelperBehavioralPlanner（routePlan，maxSteeringAngle）；

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

运动规划：利用最优快速探索随机树(RRT*)算法通过环境地图规划可行路径(pathPlannerRRT)．
路径平滑:通过使用将样条曲线拟合到参考路径，使其平滑光滑路径样条．
轨迹生成：转换平滑的路径到一个轨迹生成速度剖面使用helperGenerateVelocityProfile．
车辆控制:给定平滑的参考路径，HelperPathAnalyzer根据车辆当前的姿态和速度计算参考的姿态和速度。有了参考值，lateralControllerStanley计算转向角度来控制车辆的航向。HelperLongitudinalController计算加速和减速命令，以保持所需的车辆速度。
目标检查：检查飞行器是否达到了使用的部分的最终姿势helperGoalChecker．

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

运动规划

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

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

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

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

goalPose = routePlan {1,“结束姿势”};refPath = plan(motionPlanner, currentPose, goalPose);

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

refPath。路径Segments

ans = 1×6 DubinsPathSegment数组的属性:StartPose目标pose MinTurningRadius motionlength MotionTypes Length

参考路径包含了沿途的过渡姿态，代表了与从一个机动到下一个机动的过渡相对应的路径上的点。它们也可以表示方向上的变化，例如，沿着reed - shepp路径从正向运动到反向运动。

从规划的路径检索转换姿态和方向。

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

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

路径平滑和轨迹生成

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

使用光滑路径样条拟合通过参考路径中所有过渡点的参数三次样条。样条近似匹配起始和结束方向与车辆的起始和结束航向角。

%指定要返回的姿态数量，间隔大约为0.1% m。approxSeparation = 0.1;%仪表numSmoothPoses =圆(refPath。长度/ approxSeparation);沿着平滑的路径返回离散的姿态。[refPoses, directions, cumlength，曲率]= smoothPathSpline(transitionPoses, directions, numSmoothPoses);绘制平滑的路径。持有在hSmoothPath = plot(refPoses(:， 1)， refPoses(:， 2)，“r”，“线宽”2....“DisplayName的”，“平滑路径”）;持有从

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

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

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

生成速度剖面图。

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

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

plotVelocityProfile (cumLengths、refVelocities maxSpeed)

车辆控制与仿真

参考速度，连同平滑的路径，组成一个可行的轨迹，车辆可以跟随。采用反馈控制器跟踪该轨迹。该控制器纠正了由于轮胎打滑和其他噪声源(如定位不准确)引起的轨迹跟踪误差。特别地，控制器由两个组件组成:

横向控制:调整转向角度，使车辆遵循参考路径。
纵向控制:当跟随参考路径时，通过控制油门和刹车保持所需的速度。

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

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

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

/ * * * * * * * * * * * * * * * * *

$\dot\theta=\frac{v_r}{l}*\tan（\delta）$

$$ v_r = a_r $$

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

$x_f = x_r + lcos (\theta)$

$y\u f=y\u r+l sin（\theta）$

$v_f=\frac{v_r}{cos（\delta）}$

关闭所有的数字。closeFigures;%创建车辆模拟器。vehicleSim = helpervehiclessimulator (costmap, vehicledimms);设置车辆的姿态和速度。vehicleSim.setVehiclePose (currentPose);currentVel = 0;vehicleSim.SetVehicleLevelocity（当前级别）；配置模拟器以显示轨迹。车辆模拟显示轨迹（真实）；%隐藏车辆仿真图。hideFigure (vehicleSim);

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

路径分析器=HelperPathAnalyzer（参照姿势、参照速度、方向、，...的轴距，车辆（单位：轴距）；

创建一个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，...“方向”、方向、的轴距，车辆（单位：轴距）；%计算加减速命令。lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);%使用控制器输出模拟车辆。drive(vehicleSim, accelCmd, decelCmd, steingangle);%检查车辆是否达到目标。reachGoal = helperGoalChecker(goalPose, currentPose, currentVel, endSpeed, direction);%等待固定速率执行。等待(controlRate);获得飞行器当前的姿态和速度。GetLevelSpose（GetLevelSpose=VehiclesVelocity）；结束显示车辆仿真图。showFigure (vehicleSim);

这就完成了路线计划的第一步，并演示了流程的每个步骤。接下来的部分运行整个路线的模拟器，这将车辆靠近停车位，并最终执行一个停车机动，将车辆放置到停车位。

执行完整的计划

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

将车辆的姿态设置回起始点。currentPose = [4 12 0];% [x, y，]vehicleSim.setVehiclePose (currentPose);%重置速度。currentVel = 0;%米/秒vehicleSim.SetVehicleLevelocity（当前级别）；而~ reachedDestination (behavioralPlanner)从行为层请求下一次机动。[nextGoal, planerconfig, speedConfig] = requestManeuver(行为规划器，...currentPose currentVel);%配置运动规划器。configurePlanner (motionPlanner plannerConfig);%使用RRT* planner计划一个参考路径到下一个目标姿势。参考路径=平面（motionPlanner、currentPose、nextGoal）；%检查路径是否有效。如果规划器无法计算路径，%或者由于地图的更新，路径不是无冲突的系统需要重新规划。此场景使用静态映射，因此路径%将永远是无冲突的。isReplanNeeded=~checkPathValidity（refPath，costmap）；如果isReplanNeeded警告('无法找到有效路径。试图重新计划”。）请求行为规划师重新规划replanNeeded (behavioralPlanner);持续；结束从规划的路径中检索转换姿态和方向。[transitionpose, directions] =插值(refPath);%铺平道路。numMoothPoses=圆形（refPath.Length/approxSeparation）；[refPoses，方向，CumLength，曲率]=平滑路径样条曲线（transitionPoses，directions，NumMoothPoses）；生成一个速度剖面。refvelocity = helperGenerateVelocityProfile(方向，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，...“方向”、方向、的轴距，车辆（单位：轴距）；%计算加减速命令。lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);%使用控制器输出模拟车辆。drive(vehicleSim, accelCmd, decelCmd, steingangle);%检查车辆是否达到目标。reachGoal=helperGoalChecker（nextGoal、currentPose、currentVel、speedConfig.EndSpeed、direction）；%等待固定速率执行。等待(controlRate);获得飞行器当前的姿态和速度。GetLevelSpose（GetLevelSpose=VehiclesVelocity）；结束结束显示车辆仿真图。showFigure (vehicleSim);

停车操作

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

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

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

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

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

在碰撞检查器中使用更多的圆，并目视检查碰撞检查器。这允许通过狭窄的通道进行规划。

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

更新成本图以使用此碰撞检查器。

costmap。CollisionChecker = ccConfig;

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

图绘制(costmap)标题(“带有更新碰撞检查功能的Costmap”）%设置pathplanerrrt以使用更新的costmap。parkMotionPlanner = pathPlannerRRT (costmap,“MinIterations”, 1000);为V2X系统返回的停车位定义所需的姿势。parkPose=[36 44 90]；preParkPose=当前姿势；%计算所需的停车机动。refPath = plan(parkMotionPlanner, preparekpose, parkPose);绘制结果停车机动图。figure plotParkingManeuver(costmap, refPath, preparekpose, parkPose)

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

从规划的路径中检索转换姿态和方向。[transitionpose, directions] =插值(refPath);%铺平道路。numMoothPoses=圆形（refPath.Length/approxSeparation）；[refPoses，方向，CumLength，曲率]=平滑路径样条曲线（transitionPoses，directions，NumMoothPoses）；%设置速度剖面发生器，在轨迹结束时停止，%，限速为每小时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，...“方向”、方向、的轴距，车辆（单位：轴距）；%计算加减速命令。lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);%使用控制器输出模拟车辆。drive(vehicleSim, accelCmd, decelCmd, steingangle);%检查车辆是否达到目标。reachGoal = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction);%等待固定速率执行。等待(controlRate);获得飞行器当前的姿态和速度。GetLevelSpose（GetLevelSpose=VehiclesVelocity）；结束显示车辆仿真图。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（准备阶段）；vehicleSim.SetVehicleLevelocity（当前级别）；%计算停车策略。重新计划= true;而replan refPath = plan(parkMotionPlanner, preparekpose, parkPose);%停车机动所对应的路径小且需要%精确的操纵。不是只在过渡姿势时插入，%沿路径长度进行更精细的插值。numSamples = 10;stepSize = refPath。长度/ numSamples;长度= 0:stepSize: refPath.Length;[transitionpose, directions] =插值(refPath，长度);如果路径包含多个方向切换姿态，则重新规划%或者路径太长。replan=总和（绝对值（不同方向））~=2 | |参考路径长度>20；结束想象停车的动作。figure plotParkingManeuver(costmap, refPath, preparekpose, parkPose)

%铺平道路。NumMoothPoses=圆形（refPath.Length/approxSeparation）；[refPoses，方向，CumLength，曲率]=平滑路径样条曲线（transitionPoses，directions，NumMoothPoses，0.5）；生成速度剖面。refvelocity = helperGenerateVelocityProfile(方向，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 ~= direction currentVel = 0;setVehicleVelocity (vehicleSim currentVel);重置(lonController);结束%更新模拟器的驾驶方向。如果车辆改变%行驶方向，复位并返回当前车速为零。currentVel = updateDrivingDirection(车辆im，方向，currentDir);%计算转向命令。转向角度=横向控制器斯坦利(refPose, currentPose, currentVel，...“方向”、方向、的轴距，车辆（单位：轴距）；%计算加减速命令。lonController。方向=方向;[accelCmd, decelCmd] = lonController(refVel, currentVel);%使用控制器输出模拟车辆。drive(vehicleSim, accelCmd, decelCmd, steingangle);%检查车辆是否达到目标。reachGoal = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction);%等待固定速率执行。等待(controlRate);获得飞行器当前的姿态和速度。GetLevelSpose（GetLevelSpose=VehiclesVelocity）；结束%以快照为例。closeFigures;snapnow;%删除模拟器。删除(vehicleSim);

结论

此示例演示了如何：

在半结构化环境中，如停车场，使用RRT*路径规划算法规划可行路径。
使用样条平滑路径，并沿平滑路径生成速度剖面。
控制车辆以期望的速度跟随参考路径。
通过使用不同的运动规划器设置实现不同的停车行为。

参考文献

Buehler, Martin, Karl Iagnemma和Sanjiv Singh。DARPA城市挑战：城市交通中的自动车辆(第1版)。施普林格出版公司，2009。

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

金宝app支持功能

loadParkingLotMapLayers加载停车场环境地图图层

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

plotMapLayers包含地图图层的打印结构

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

combineMapLayers将地图层合并成一个单一的成本地图

函数costmap = combineMapLayers (mapLayers)%组合球员%将地图图层结构组合到单个vehicleCostmap中。combinedMap = mapLayers。StationaryObstacles + mapLayers。路标+...mapLayers.ParkedCars;combinedMap = im2single (combinedMap);res = 0.5;%仪表costmap = vehicleCostmap (combinedMap,“CellSize”res);结束

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

函数configurePlanner（路径规划器，配置）% configurePlanner配置路径规划器对象，指定路径规划器的设置%在struct配置。=字段字段名(配置);为n = 1: numel(fieldNames)如果~ strcmpi(字段名{n},“IsParkManeuver”)pathPlanner.（字段名{n}）=配置（字段名{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 (“速度（m/s）”）;%添加图例和标题。传奇(“速度剖面”，的最高速度)标题(生成速度剖面的）结束

closeFigures

函数数字（%关闭所有的数字，除了模拟器可视化。找到所有图形对象。figHandles = findobj (“类型”，“图”）;为i=1：长度（单位：菲甘德尔）如果~ strcmp (figHandles(我)。的名字,“自动代客泊车”)密切(figHandles (i));结束结束结束

plotParkingManeuver在成本地图上显示生成的停车策略。

函数plotparkmaneuver (costmap, refPath, currentPose, parkPose)% plotParkingManeuver%在成本地图上绘制生成的停车策略。%绘制成本图，没有膨胀区域。情节(costmap“通货膨胀”，“关闭”）%在成本图上绘制参考停车策略。持有在情节(refPath“DisplayName的”，“停车机动”)标题(“停车机动”）%通过设置轴的限制放大到停车机动。lo = min ([currentPose (1:2);parkPose (1:2)]);你好= max ([currentPose (1:2);parkPose (1:2)]);缓冲= 6;%仪表ylim([lo(2)-buffer hi(2)+buffer])的用法和样例:结束