自动停车员

打开脚本

这个例子展示了如何构建一个自动代客停车系统。在本例中,您学习工具和技术支持路径规划、轨迹生成和车辆控制。万博1manbetx虽然这个例子着重于MATLAB®生产型企业工作流,这些工具也可以在仿真软件®。万博1manbetx这个例子的仿万博1manbetx真软件版本,请参阅自动停车员在仿真软件万博1manbetx。

概述

自动停车的汽车,离开前一个停车场是一个具有挑战性的问题。汽车的自动化系统将接管控制和引导车辆一个可用的停车位。这样一个函数利用多个车载传感器。例如:

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

车载传感器感知车辆周围的环境。感知到的环境包括路标的理解来解释地区道路交通规则和推断修好,识别障碍,检测可用的停车位。

汽车传感器感知世界,车辆必须计划一个路径通过环境对一个免费的停车位和执行一系列驱动所需的控制行动。虽然这样做,它必须响应动态环境的变化,比如行人穿越道路,并调整其计划。

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

环境模型

环境模型表示环境的地图。代客停车系统,这张地图包括可用并占领停车位,路标和障碍,如行人或其他车辆。入住率地图是一种常见的表示对这种形式的环境模型。这样的地图通常使用同时定位和地图构建(大满贯)通过集成观测激光雷达和摄像头传感器。这个例子集中于一个简单的场景,地图已经提供,例如,通过一个vehicle-to-infrastructure (V2X)系统或相机俯瞰整个停车位。它使用一个静态地图一个停车场和假定的self-localization车辆是准确的。

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

固定障碍:这一层包含固定障碍像墙壁,障碍,和停车场的界限。
路标:该层包含与路标占用信息,包括停车位的路标。
停放的汽车:这一层包含哪些信息停车位已经占领了。

每个地图层包含不同类型的障碍表示不同程度的危险车穿越它。使用这种结构,每一层都可以处理,更新,保持独立。

加载和显示三个映射层。在每一层,光和暗细胞代表占据细胞,细胞代表自由细胞。

mapLayers = loadParkingLotMapLayers;plotMapLayers (mapLayers)

为简单起见,将三层到单个costmap。

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

合并后的costmap是一个vehicleCostmap对象,该对象代表了车辆环境作为一个二维网格占用。每个网格单元的值在0和1之间,代表浏览手机的成本。障碍有更高的成本,而自由空间有更低的成本。一个细胞被认为是一个障碍是否高于其成本OccupiedThreshold财产,自由是否低于其成本FreeThreshold财产。

的costmap涵盖整个75 m-by-50m停车场区域,分为由- 0.5 - 0.5 m - m细胞。

costmap.MapExtent% (x, y,加强筋的宽度和高度)米costmap.CellSize%细胞大小米

= 0 75 0 50岁= 0.5000

创建一个vehicleDimensions对象存储车辆的尺寸将自动公园。还定义的最大转向角。这个值决定了转弯半径限制在运动规划和控制。

vehicleDims = vehicleDimensions;maxSteeringAngle = 35;%的度

更新VehicleDimensionscostmap碰撞的属性检查器与车辆的尺寸公园。这个设置调整通货膨胀的程度在障碍地图对应的大小车辆停放,确保无碰撞路径可以通过找到停车场。

costmap.CollisionChecker。VehicleDimensions= vehicleDims;

定义的起始姿势。姿势是通过定位,本例中为简单起见。车辆被指定为 $[x, y,θ\]美元$ ,在世界坐标。 (x, y)美元代表了汽车的后桥的中心位置在世界坐标系统。 $\θ美元$ 代表车辆的方向对世界X轴。更多细节,请参阅坐标系统在自动驾驶的工具箱。

currentPose = 12 0 [4];% (x, y,θ)

行为层

计划涉及到组织成层次层所有相关信息。每一层负责一个更细粒度的任务。行为层[1]这个堆栈的顶部。它负责激活和管理任务的不同部分通过提供一系列的导航任务。行为层组装系统的所有相关部分的信息,包括:

定位:行为层检查定位模块的估计车辆的当前位置。
环境模型:知觉和传感器融合系统报告车辆周围的环境的地图。
确定一个停车位:行为层分析地图确定最近的可用的停车位。
找到一个全局路线:路由模块计算全球路线通过道路网络获得从一个映射服务或从V2X基础设施。分解全球航线的一系列道路链接允许计划每个链接的轨迹不同。例如,最后停车操作需要一个不同的速度剖面的方法停车的地方。在更一般的环境中,这对穿越街道变得至关重要,涉及不同的限速,数量的车道,道路标志。

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

的HelperBehavioralPlanner类模拟接口的行为规划层。的HelperBehavioralPlanner使用地图和创建的全球航线计划。这个示例使用一个静态全局路线计划存储在MATLAB表,但通常一个路由算法提供的地方停车基础设施或映射服务决定了这个计划。全球航线计划被描述为一系列巷段导线达到一个停车位。

加载MAT-file包含一个路线计划,存储在一个表中。桌子上有三个变量: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 32 70 19 90 70 90 1×1 struct 70 32 90 53 39 180 1×1结构

情节车辆目前的姿势,沿线的每个目标和计划。

%情节车辆目前的姿势持有在helperPlotVehicle (currentPose vehicleDims,“DisplayName的”,目前提出的传说)为n = 1:高度(routePlan)%提取目标航点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家庭找到一个路径规划算法通过构造树连接,无碰撞车辆构成。造成连接使用杜宾或Reeds-Shepp指导,确保生成的路径运动学上地是可行的。

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

计划从当前对道路使用的第一个目标计划函数。返回的driving.Path对象,refPath,是一个可行的和无碰撞的参考路径。

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

参考路径由一序列路径段。每个路径段描述了组杜宾或Reeds-Shepp演习用于连接到下一段。检查路径段。

refPath.PathSegments

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

参考路径包含过渡姿势,代表点相对应的路径从一个操作转换到下一个。他们还可以表示方向的变化,例如,从反向运动沿着Reeds-Shepp路径。

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

[transitionPoses,方向]=插入(refPath);%可视化规划路径。情节(motionPlanner)

除了计划参考路径,注意图上的红色区域。这些领域的代表区域costmap在汽车的起源(后轴中心)必须不交叉,以避免触及任何障碍。pathPlannerRRT找到路径,避免障碍通过检查,确保车辆提出了生成不躺在这些领域。

路径平滑和轨迹生成

参考路径生成的路径规划是由杜宾的或Reeds-Shepp段。曲率的连接两个部分不是连续的和可能导致突然的转向角的变化。为了避免这种不自然的动作,确保旅客舒适度,路径需要连续可微的,因此顺利[2]。一种方法平滑路径包括拟合参数三次样条。样条拟合可以生成一个控制器可以执行一个平滑的路径。

使用smoothPathSpline适合参数三次样条,穿过所有的过渡点的参考路径。花键约比赛开始和结束的开始和结束的方向车辆的航向角。

%指定数量的姿势返回使用大约0.1的分离% m。approxSeparation = 0.1;%米numSmoothPoses =圆(refPath。长度/ approxSeparation);%沿着光滑的路径返回离散的姿势。[refPoses,方向,cumLengths曲率)= smoothPathSpline (transitionPoses,方向,numSmoothPoses);%画出平滑的路径。持有在hSmoothPath =情节(refPoses (: 1), refPoses (:, 2),“r”,“线宽”2,…“DisplayName的”,“平滑路径”);持有从

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

指定初始、最大和终端速度,车辆开始平稳,加速5米/秒的速度,和停止。

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

生成速度剖面。

refVelocities = helperGenerateVelocityProfile(方向、cumLengths、曲率、startSpeed endSpeed, maxSpeed);

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

plotVelocityProfile (cumLengths、refVelocities maxSpeed)

车辆控制和仿真

参考速度和平滑路径,组成一个可行的车辆可以遵循的轨迹。反馈控制器用于遵循这个轨迹。控制器中纠正错误跟踪轨迹,来自轮胎滑移和其他来源的噪音,如定位不准确。特别是,控制器包括两个部分:

横向控制:调整转向角,这样车辆遵循的参考路径。
纵向控制:当参考路径后,维护所需的速度通过控制油门和刹车。

因为这个场景涉及到缓慢的速度,可以简化控制器只考虑一个运动学模型。在这个例子中,横向控制实现的lateralControllerStanley函数。由辅助系统纵向控制实现对象™HelperLongitudinalController,计算加速和减速命令基于比例积分法。

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

$$ \点x_r = v_r * \ cosθ(\)$$

$$ \点y_r = v_r * \罪(\θ)$$

$$ \点\θ= \压裂{v_r} {1} * \ tanδ)(\ $$

$v_r = a_r $ $ $ $ \点$

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

$$ x_f = x_r + l cosθ(\)$$

$$ y_f = y_r + l罪(\θ)$$

$$ 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 (“SampleTime”,sampleTime);

使用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,方向);%等固定利率执行。等待(controlRate);%得到车辆的当前姿态和速度。currentPose = getVehiclePose (vehicleSim);currentVel = getVehicleVelocity (vehicleSim);结束%显示车辆仿真图。showFigure (vehicleSim);

这就完成了第一回合的路线计划和演示过程的每一步。下一小节将运行模拟器为整个路线,将车辆靠近停车位,最后执行停车机动车辆进入停车场。

执行一个完整的计划

现在把所有前面的步骤的规划过程和运行模拟完整的路线计划。这个过程涉及到合并行为计划。

%设置车辆构成回到最初的起点。currentPose = 12 0 [4];% (x, y,θ)vehicleSim.setVehiclePose (currentPose);%重置速度。currentVel = 0;%米/秒vehicleSim.setVehicleVelocity (currentVel);而~ reachedDestination (behavioralPlanner)%的请求下一个操作从行为层。[nextGoal, plannerConfig speedConfig] = requestManeuver (behavioralPlanner,…currentPose currentVel);%配置运动计划。configurePlanner (motionPlanner plannerConfig);%计划参考路径使用RRT *计划下一个目标。refPath =计划(motionPlanner currentPose nextGoal);%检查路径是否有效。如果计划不能计算路径,%或不是无碰撞路径,因为更新的地图,%系统需要重新计划。这个场景使用一个静态映射,所以路径%将永远无碰撞。isReplanNeeded = ~ checkPathValidity (refPath costmap);如果isReplanNeeded警告(无法找到一个有效的路径。试图重新计划”。)%请求行为计划重新计划replanNeeded (behavioralPlanner);继续;结束%从计划路径检索过渡的姿态和方向。[transitionPoses,方向]=插入(refPath);%铺平道路。numSmoothPoses =圆(refPath。长度/ approxSeparation);[refPoses,方向,cumLengths曲率)= smoothPathSpline (transitionPoses,方向,numSmoothPoses);%生成速度剖面。refVelocities = helperGenerateVelocityProfile(方向、cumLengths、曲率、startSpeed endSpeed, maxSpeed);%分析器配置路径。pathAnalyzer。RefPoses = RefPoses;pathAnalyzer。方向=方向;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、方向);%等固定利率执行。等待(controlRate);%得到车辆的当前姿态和速度。currentPose = getVehiclePose (vehicleSim);currentVel = getVehicleVelocity (vehicleSim);结束结束%显示车辆仿真图。showFigure (vehicleSim);

停车操作

现在停车位附近的车辆,专门停放机动车辆用于公园在最后的停车位。这个策略需要穿过一条狭窄的走廊两侧的边缘两端停车位。这种策略通常是伴随着超声波传感器或激光扫描仪不断检查障碍。

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

的vehicleCostmap使用把碰撞检查。首先,视觉检查当前碰撞检查器在使用中。

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

碰撞检测是由膨胀障碍costmap通货膨胀半径,并检查是否上面所示的圆心位于一个充气的网格单元。最后停车操作需要一个更精确,更少的保守collision-checking机制。这通常由代表解决车辆使用多个(3 - 5)的形状重叠的圆圈,而不是一个圆。

使用更多的圈在碰撞检查和视觉检查碰撞检查。这允许通过狭窄通道规划。

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

更新costmap使用这个碰撞检查。

costmap。CollisionChecker = ccConfig;

注意,通货膨胀半径减少了,使规划师停车位找到一个没有障碍的路径。

图绘制(costmap)标题(“与更新Costmap碰撞检查器”)%设置pathPlannerRRT使用costmap更新。parkMotionPlanner = pathPlannerRRT (costmap,“MinIterations”,1000);%定义想要的姿势停车位,由V2X返回系统。parkPose = (36 44 90);preParkPose = currentPose;%计算所需的机动停车。refPath =计划(parkMotionPlanner preParkPose parkPose);%的情节产生的停车操作。图plotParkingManeuver (costmap refPath、preParkPose parkPose)

一旦发现操作,重复前面的过程来确定一个完整的计划:铺平道路,生成速度剖面和遵循使用反馈控制器的轨迹。

%从计划路径检索过渡的姿态和方向。[transitionPoses,方向]=插入(refPath);%铺平道路。numSmoothPoses =圆(refPath。长度/ approxSeparation);[refPoses,方向,cumLengths曲率)= smoothPathSpline (transitionPoses,方向,numSmoothPoses);%设置速度剖面发电机停止末端的轨迹,%的限速每小时5英里。refVelocities = helperGenerateVelocityProfile(方向、cumLengths、曲率、currentVel 0, 2.2352);pathAnalyzer。RefPoses = RefPoses;pathAnalyzer。方向=方向;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、方向);%等固定利率执行。等待(controlRate);%得到车辆的当前姿态和速度。currentPose = getVehiclePose (vehicleSim);currentVel = getVehicleVelocity (vehicleSim);结束%显示车辆仿真图。closeFigures;showFigure (vehicleSim);

公园的另一个途径回到停车位的车辆。当车辆需要备份到一个地方,运动规划师需要使用Reeds-Shepp连接方法来寻找一个可行的路径。在规划Reeds-Shepp连接允许反向运动。

%指定停车造成对应学校停车操作。parkPose = (49 -90 - 47.2);变化百分比的连接方法允许反向运动。parkMotionPlanner。ConnectionMethod =“Reeds-Shepp”;

找到一个可行的路径,运动计划需要调整。使用更大的转弯半径和连接距离允许光滑的后端。

parkMotionPlanner。MinTurningRadius = 10;%米parkMotionPlanner。ConnectionDistance = 15;%重置车辆姿态和速度。currentVel = 0;vehicleSim.setVehiclePose (preParkPose);vehicleSim.setVehicleVelocity (currentVel);%计算停车操作。重新计划= true;而重新计划refPath =计划(parkMotionPlanner、preParkPose parkPose);%的路径对应的回旋余地很小,需要停车%精确的操纵。而不是只在过渡姿势,插值%插入更精细沿着路径的长度。numSamples = 10;stepSize = refPath。长度/ numSamples;长度= 0:stepSize: refPath.Length;[transitionPoses,方向]=插入(refPath长度);%重新计划如果路径包含不止一个方向切换姿态%或者路径太长。重新计划=总和(abs (diff(方向)))~ = 2 | | refPath。长度> 20;结束%可视化操作停车。图plotParkingManeuver (costmap refPath、preParkPose parkPose)

%铺平道路。numSmoothPoses =圆(refPath。长度/ approxSeparation);[refPoses,方向,cumLengths曲率)= smoothPathSpline (transitionPoses,方向,numSmoothPoses, 0.5);%生成速度剖面。refVelocities = helperGenerateVelocityProfile(方向、cumLengths、曲率、currentVel 0, 1);pathAnalyzer。RefPoses = RefPoses;pathAnalyzer。方向=方向;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、方向);%等固定利率执行。等待(controlRate);%得到车辆的当前姿态和速度。currentPose = getVehiclePose (vehicleSim);currentVel = getVehicleVelocity (vehicleSim);结束%快照的例子。closeFigures;snapnow;%删除模拟器。删除(vehicleSim);

结论

这个例子展示了如何:

计划一个可行的路径在半结构化的环境中,如停车场,使用一个RRT路径规划算法。
铺平道路使用样条函数沿着平滑路径和生成速度剖面。
控制车辆按照参考路径所需的速度。
实现不同的停车行为通过使用不同的运动计划设置。

引用

[1]比勒,马丁,卡尔Iagnemma,桑吉夫•辛格。美国国防部高级研究计划局城市挑战:自主车辆在城市交通(第1版)。斯普林格出版公司,2009年合并。

[2]Lepetic, Marko格雷戈尔Klancar,伊戈尔·Skrjanc德拉戈Matko, Bostjan报告,“时间最优路径规划考虑加速度限制。”机器人和自治系统。3 - 4卷45,问题,2003年,页199 - 210。

万博1manbetx支持功能

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

函数mapLayers = loadParkingLotMapLayers ()% loadParkingLotMapLayers%负载入住率地图对应3层-障碍,道路%的标记,和使用地点。mapLayers。StationaryObstacles = imread (“stationary.bmp”);mapLayers。路标= imread (“road_markings.bmp”);mapLayers。ParkedCars = imread (“parked_cars.bmp”);结束

plotMapLayers情节结构包含映射层

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

combineMapLayers结合层映射到单个costmap

函数costmap = combineMapLayers (mapLayers)% combineMapLayers%结合层结构映射到单个vehicleCostmap。combinedMap = mapLayers。StationaryObstacles + mapLayers。路标+…mapLayers.ParkedCars;combinedMap = im2single (combinedMap);res = 0.5;%米costmap = vehicleCostmap (combinedMap,“CellSize”res);结束

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

函数configurePlanner (pathPlanner配置)% configurePlannerpathPlanner %配置路径规划对象,设置指定%在结构配置。=字段字段名(配置);为n = 1:元素个数(字段名)如果~ 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 +缓冲)%添加标签。包含(“累积路径长度(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“通货膨胀”,“关闭”)%参考停车机动costmap的阴谋。持有在情节(refPath“DisplayName的”,“停车策略”)标题(“停车策略”)%放大停车操作通过设置轴的极限。lo = min ([currentPose (1:2);parkPose (1:2)]);你好= max ([currentPose (1:2);parkPose (1:2)]);缓冲= 6;%米xlim([罗(1)缓冲嗨(1)+缓冲])ylim([罗(2)缓冲嗨(2)+缓冲])结束