高速公路轨迹规划使用FreneT参考路径GydF4y2Ba

打开直播脚本GydF4y2Ba

此示例演示如何在高速公路驾驶场景中规划局部轨迹。此示例使用参考路径和动态障碍物列表，以为自助车辆生成替代轨迹。自我车辆通过提供的驱动场景中定义的流量来导航GydF4y2Ba驱动器Cenario.GydF4y2Ba对象。基于成本、可行性和无碰撞运动，车辆在自适应巡航控制、变道和车辆跟随机动之间交替。GydF4y2Ba

负载驱动的场景GydF4y2Ba

首先装载提供的GydF4y2Ba驱动器Cenario.GydF4y2Ba对象，它在当前工作空间中定义了车辆和道路属性。使用此方案使用GydF4y2Ba驾驶场景设计师GydF4y2Ba（自动驾驶工具箱）GydF4y2Ba应用程序并导出到Matlab®功能，GydF4y2BaDrivingscenariotrafficexample.GydF4y2Ba。有关更多信息，请参见GydF4y2Ba以编程方式创建驱动场景变体GydF4y2Ba（自动驾驶工具箱）GydF4y2Ba。GydF4y2Ba

场景=驱动程序Cenariotrafficexample;GydF4y2Ba%默认汽车属性GydF4y2BacarLen = 4.7;GydF4y2Ba%在米GydF4y2BaCarwidth = 1.8;GydF4y2Ba%在米GydF4y2Ba后索= .25;GydF4y2Ba%界定道路尺寸GydF4y2BaLaneWidth = Carwidth * 2;GydF4y2Ba%在米GydF4y2Ba情节（情景）;GydF4y2Ba

图中包含一个轴。坐标轴包含了patch、line类型的9个对象。GydF4y2Ba

构建参考路径GydF4y2Ba

该示例中的所有本地规划是关于参考路径执行的，由A表示GydF4y2Ba参考血统GydF4y2Ba对象。该对象可以返回沿路径给定长度的曲线状态，找到沿路径到某个全局的最近点GydF4y2BaxyGydF4y2Ba-location，便于全局坐标系和弗雷内坐标系之间的坐标转换。GydF4y2Ba

为此示例，参考路径被视为四车道高速公路的中心，航点与所提供的道路匹配GydF4y2Ba驱动器Cenario.GydF4y2Ba对象。GydF4y2Ba

Waypoints = [0 50;150 50;300 75;310 75;400 0;300 -50;290 -50;0 -50];GydF4y2Ba%在米GydF4y2BaRefpath =参考水平血统（航点）;斧头= show（Refpath）;轴（斧头，GydF4y2Ba“平等”GydF4y2Ba）;GydF4y2Ba

图中包含一个轴。坐标轴包含两个line类型的对象。GydF4y2Ba

构建轨迹发生器GydF4y2Ba

对于本地规划师，目标通常是在满足当前运动和动态条件的同时对最终目标进行移动的各种可能的动作。这GydF4y2BaTraometOryGeneratorFromet.GydF4y2Ba对象通过使用四阶或五阶多项式轨迹连接初始状态和一组终端状态来实现这一点。在Frenet坐标系中定义初态和终态，每个多项式解满足横向和纵向位置、速度和加速度边界条件，同时使冲击最小。GydF4y2Ba

终端状态通常使用自定义行为计算。这些行为利用当地环境中发现的信息，例如自我车辆附近的演员的车道信息，限速和当前或未来或未来预测。GydF4y2Ba

构造一个GydF4y2BaTraometOryGeneratorFromet.GydF4y2Ba使用参考路径的对象GydF4y2Ba

连接器= TrajectoryGeneratorFromet（RefPath）;GydF4y2Ba

构造动态碰撞检查器GydF4y2Ba

这GydF4y2BadynamicCapsuleListGydF4y2Ba对象是一种数据结构，表示通过离散的时间集中的动态环境状态。然后，该环境可以用于有效地验证自我车辆的多个电位轨迹。场景中的每个对象都表示：GydF4y2Ba

独特的整数值标识符GydF4y2Ba
用于高效碰撞检查的胶囊几何的属性GydF4y2Ba
SE2状态序列，其中每个状态表示时间上的离散快照。GydF4y2Ba

在此示例中，由此产生的轨迹GydF4y2BaTraometOryGeneratorFromet.GydF4y2Ba对象发生在某些时间段内，称为时间范围。确保碰撞检查涵盖所有可能的轨迹，GydF4y2BadynamicCapsuleListGydF4y2Ba对象应包含所有跨越最大预期时间范围的演员的预测轨迹。GydF4y2Ba

caplist =动态浮夸的人;GydF4y2Ba

创建一个几何结构的自我车辆与给定的参数。GydF4y2Ba

egoID = 1;[egoID, egoGeom] = egoGeometry(capList,egoID);egoGeom.Geometry.Length = carLen;GydF4y2Ba%在米GydF4y2BaeGoGeom.geometry.radius = Carwidth / 2;GydF4y2Ba%在米GydF4y2BaegoGeom.Geometry.FixedTransform (1) = -carLen * rearAxleRatio;GydF4y2Ba%在米GydF4y2Ba

将自我载体添加到动态胶囊列表中。GydF4y2Ba

updateEgoGeometry (capList egoID egoGeom);GydF4y2Ba

添加GydF4y2Ba驱动器Cenario.GydF4y2Ba演员到了GydF4y2BadynamicCapsuleListGydF4y2Ba对象。几何图形是在这里设置的，状态是在规划循环期间定义的。你可以看到GydF4y2BadynamicCapsuleListGydF4y2Ba现在包含一个自我车辆和五个障碍。GydF4y2Ba

Actorid =（1：5）';Actorgeom = Repelem（eGoogeom，5,1）;updateobstraclegeometry（Caplist，Actorid，Actorgeom）GydF4y2Ba

ANS =具有属性的动态通信镜头：MAXNUMSTEPS：31 egoids：1 explicsids：[5x1 double] numobstacles：5 numegos：1GydF4y2Ba

通过流量计划自适应路线GydF4y2Ba

规划公用事业支持一种局部规划策略，该策略基于当万博1manbetx前和可预见的环境状态，在选择最优轨迹之前采样一组局部轨迹。模拟回路被组织成以下几个部分:GydF4y2Ba

单击每个部分的标题以导航到模拟循环中的相关代码。GydF4y2Ba

Ground Truth场景GydF4y2Ba

在进行动态环境时，通常需要估计环境状态或在不久的将来预测其状态。为简单起见，此示例使用GydF4y2Ba驱动器Cenario.GydF4y2Ba作为每个行动者在规划地平线上轨迹的地面真相来源。要测试自定义预测算法，可以替换或修改GydF4y2BaexampleHelperRetrieveActorGroundTruthGydF4y2Ba使用自定义代码。GydF4y2Ba

生成终端状态GydF4y2Ba

自动驾驶的一个共同目标是确保规划的轨迹不仅可行，而且自然。典型的高速公路驾驶包括保持车道、保持限速、变换车道、适应交通等要素。每个自定义行为可能需要不同的环境信息。这个例子演示了如何生成实现三种行为的终端状态:巡航控制、车道变化和跟随领头车辆。GydF4y2Ba

巡航控制系统GydF4y2Ba

这GydF4y2BaexampleHelperBasicCruiseControlGydF4y2Ba函数生成执行巡航控制行为的终端状态。该函数利用自我车辆的横向速度和时间视界来预测自我车辆未来的预期车道n秒。计算车道中心，得到终端状态的横向偏移，并将横向速度和加速度设为零。GydF4y2Ba

对于纵向边界条件，终端速度设置为道路限速，终端加速度设置为零。纵向位置不受限制，指定为GydF4y2Ba南GydF4y2Ba。通过删除经度约束，GydF4y2BaTraometOryGeneratorFromet.GydF4y2Ba可以使用一个较低的四阶多项式来解决纵向边值问题，从而得到一个在给定时间范围内平滑匹配道路速度的轨迹:GydF4y2Ba

$CGydF4y2Ba R.GydF4y2Ba 你GydF4y2Ba 一世GydF4y2Ba S.GydF4y2Ba E.GydF4y2Ba CGydF4y2Ba O.GydF4y2Ba N.GydF4y2Ba T.GydF4y2Ba R.GydF4y2Ba O.GydF4y2Ba L.GydF4y2Ba S.GydF4y2Ba T.GydF4y2Ba 一种GydF4y2Ba T.GydF4y2Ba E.GydF4y2Ba =GydF4y2Ba [GydF4y2Ba 南GydF4y2Ba {\overset{˙GydF4y2Ba}{S.GydF4y2Ba}}_{D.GydF4y2Ba E.GydF4y2Ba S.GydF4y2Ba} 0.GydF4y2Ba {L.GydF4y2Ba}_{E.GydF4y2Ba XGydF4y2Ba P.GydF4y2Ba L.GydF4y2Ba 一种GydF4y2Ba N.GydF4y2Ba E.GydF4y2Ba} 0.GydF4y2Ba 0.GydF4y2Ba]GydF4y2Ba$

车道改变GydF4y2Ba

这GydF4y2Baexamplehelperbasiclanechange.GydF4y2Ba功能生成从当前通道转换到相邻车道的车辆的终端状态。该功能首先确定EGO车辆的当前通道，然后检查是否存在左侧和右侧泳道。对于每个现有车道，以与巡航控制行为相同的方式定义终端状态，但终端速度设定为当前速度而不是道路的速度限制：GydF4y2Ba

$L.GydF4y2Ba 一种GydF4y2Ba N.GydF4y2Ba E.GydF4y2Ba CGydF4y2Ba HGydF4y2Ba 一种GydF4y2Ba N.GydF4y2Ba GGydF4y2Ba E.GydF4y2Ba S.GydF4y2Ba T.GydF4y2Ba 一种GydF4y2Ba T.GydF4y2Ba E.GydF4y2Ba =GydF4y2Ba [南GydF4y2Ba {\overset{˙GydF4y2Ba}{S.GydF4y2Ba}}_{CGydF4y2Ba 你GydF4y2Ba R.GydF4y2Ba} 0.GydF4y2Ba {L.GydF4y2Ba}_{D.GydF4y2Ba E.GydF4y2Ba S.GydF4y2Ba L.GydF4y2Ba 一种GydF4y2Ba N.GydF4y2Ba E.GydF4y2Ba} 0.GydF4y2Ba 0.GydF4y2Ba]$

遵循铅车辆GydF4y2Ba

这GydF4y2BaexampleHelperBasicLeadVehicleFollowGydF4y2Ba功能生成试图在自助车辆前面发现车辆的终端状态。该功能首先确定自我车辆的当前车道。为每个提供GydF4y2BaTimeHorizon.GydF4y2Ba，该功能预测每个行动者的未来状态，找到所有与自我交通工具在同一车道上的行动者，并决定哪一个是最近的GydF4y2Ba铅GydF4y2Ba车辆（如果没有找到铅载体，则该功能不会返回任何内容）。GydF4y2Ba

通过采取铅车辆的位置和速度并通过一些安全距离降低终端纵向位置来计算EGO车辆的终端状态：GydF4y2Ba

$CGydF4y2Ba L.GydF4y2Ba O.GydF4y2Ba S.GydF4y2Ba E.GydF4y2Ba S.GydF4y2Ba T.GydF4y2Ba L.GydF4y2Ba E.GydF4y2Ba 一种GydF4y2Ba D.GydF4y2Ba V.GydF4y2Ba E.GydF4y2Ba HGydF4y2Ba 一世GydF4y2Ba CGydF4y2Ba L.GydF4y2Ba 房地产GydF4y2Ba =GydF4y2Ba [GydF4y2Ba {S.GydF4y2Ba}_{L.GydF4y2Ba E.GydF4y2Ba 一种GydF4y2Ba D.GydF4y2Ba} {\overset{˙GydF4y2Ba}{S.GydF4y2Ba}}_{L.GydF4y2Ba E.GydF4y2Ba 一种GydF4y2Ba D.GydF4y2Ba} 0.GydF4y2Ba {L.GydF4y2Ba}_{L.GydF4y2Ba E.GydF4y2Ba 一种GydF4y2Ba D.GydF4y2Ba} {\overset{˙GydF4y2Ba}{L.GydF4y2Ba}}_{L.GydF4y2Ba E.GydF4y2Ba 一种GydF4y2Ba D.GydF4y2Ba} 0.GydF4y2Ba]GydF4y2Ba$

$followStateGydF4y2Ba =GydF4y2Ba [GydF4y2Ba （GydF4y2Ba {S.GydF4y2Ba}_{L.GydF4y2Ba E.GydF4y2Ba 一种GydF4y2Ba D.GydF4y2Ba} -GydF4y2Ba {D.GydF4y2Ba}_{安全GydF4y2Ba} ）GydF4y2Ba {\overset{˙GydF4y2Ba}{S.GydF4y2Ba}}_{L.GydF4y2Ba E.GydF4y2Ba 一种GydF4y2Ba D.GydF4y2Ba} 0.GydF4y2Ba {L.GydF4y2Ba}_{L.GydF4y2Ba E.GydF4y2Ba 一种GydF4y2Ba D.GydF4y2Ba} {\overset{˙GydF4y2Ba}{L.GydF4y2Ba}}_{L.GydF4y2Ba E.GydF4y2Ba 一种GydF4y2Ba D.GydF4y2Ba} 0.GydF4y2Ba]GydF4y2Ba$

评估终端状态的成本GydF4y2Ba

在生成终端状态后，可以评估其成本。轨迹评估和优先考虑潜在解决方案的方法是高度主观的。万博尤文图斯为了简单起见，GydF4y2Baexamplehelperevaluatetscost.GydF4y2Ba函数将成本定义为三个加权和的组合。GydF4y2Ba

横向偏移费用(GydF4y2Ba ${CGydF4y2Ba}_{L.GydF4y2Ba 一种GydF4y2Ba T.GydF4y2Ba D.GydF4y2Ba E.GydF4y2Ba V.GydF4y2Ba}$ ）GydF4y2Ba-一个正的权重，惩罚偏离车道中心的状态。GydF4y2Ba

${CGydF4y2Ba}_{L.GydF4y2Ba 一种GydF4y2Ba T.GydF4y2Ba D.GydF4y2Ba E.GydF4y2Ba V.GydF4y2Ba} =GydF4y2Ba {W.GydF4y2Ba}_{ΔGydF4y2Ba L.GydF4y2Ba} *GydF4y2Ba ΔGydF4y2Ba L.GydF4y2Ba$

$ΔGydF4y2Ba L.GydF4y2Ba =GydF4y2Ba {argmin.GydF4y2Ba}_{一世GydF4y2Ba} （GydF4y2Ba |GydF4y2Ba {L.GydF4y2Ba}_{termStateGydF4y2Ba} -GydF4y2Ba {L.GydF4y2Ba}_{{车道GydF4y2Ba}_{一世GydF4y2Ba}} |GydF4y2Ba ）GydF4y2Ba$

时间成本(GydF4y2Ba ${CGydF4y2Ba}_{T.GydF4y2Ba 一世GydF4y2Ba mGydF4y2Ba E.GydF4y2Ba}$ ）GydF4y2Ba- 优先化在更长间隔内发生的运动的负重，导致更平滑的轨迹。GydF4y2Ba

${CGydF4y2Ba}_{T.GydF4y2Ba 一世GydF4y2Ba mGydF4y2Ba E.GydF4y2Ba} =GydF4y2Ba {W.GydF4y2Ba}_{ΔGydF4y2Ba T.GydF4y2Ba} *GydF4y2Ba ΔGydF4y2Ba T.GydF4y2Ba$

终端速度成本（GydF4y2Ba ${CGydF4y2Ba}_{S.GydF4y2Ba P.GydF4y2Ba E.GydF4y2Ba E.GydF4y2Ba D.GydF4y2Ba}$ ）GydF4y2Ba- 优先考虑维持速度限制的运动的正重，导致较少的动态演用。GydF4y2Ba

${CGydF4y2Ba}_{S.GydF4y2Ba P.GydF4y2Ba E.GydF4y2Ba E.GydF4y2Ba D.GydF4y2Ba} =GydF4y2Ba {W.GydF4y2Ba}_{ΔGydF4y2Ba V.GydF4y2Ba} *GydF4y2Ba ΔGydF4y2Ba V.GydF4y2Ba$

生成轨迹并检查运动学可行性GydF4y2Ba

除了具有最小成本的终端状态之外，最佳轨迹通常必须满足与运动可行性相关的额外约束，并乘坐舒适性。轨迹约束是执行所需的乘坐质量的一种方式，但它们以减少的驾驶信封为代价而这样做。GydF4y2Ba

在这个例子中，GydF4y2BaexampleHelperEvaluateTrajectoryGydF4y2Ba函数验证每个轨迹满足以下约束：GydF4y2Ba

maxacceleration.GydF4y2Ba:整个轨迹的绝对加速度必须低于指定的值。较小的值减少了驱动的侵略性和消除运动学不可行的轨迹。这一限制可能会消除机动，否则可能由车辆执行。GydF4y2Ba
maxcurvature.GydF4y2Ba：在轨迹中允许的最小转换半径。和人一样GydF4y2Bamaxacceleration.GydF4y2Ba，减少这种值导致更平滑的驾驶体验，但可能会消除其他可行的轨迹。GydF4y2Ba
MinVelocityGydF4y2Ba:这个例子通过设置最小速度来约束自我车辆向前移动。这种限制在高速公路驾驶场景中是理想的，消除了适合过约束或条件差的边界值的轨迹。GydF4y2Ba

检查轨迹进行碰撞，然后选择最佳轨迹GydF4y2Ba

规划过程的最后一步是选择最好的轨迹，这也会导致无碰撞路径。碰撞检查通常被推迟到最后，因为这是一个昂贵的操作，所以通过首先评估代价和分析约束，无效的轨迹可以从考虑中删除。然后可以按最佳顺序检查剩余的轨迹，直到找到一条无碰撞路径或所有轨迹都已被评估。GydF4y2Ba

定义模拟器和规划参数GydF4y2Ba

本节定义运行计划程序和行为实用程序使用的模拟器和参数所需的属性。属性如GydF4y2Ba场景.Sampletime和GydF4y2Ba连接器。TimeResolutionGydF4y2Ba同步，以便在地面真理演员轨迹和计划的自我轨迹处发生在同一时间表中。相似地，GydF4y2BareplanRateGydF4y2Ba那GydF4y2BaTimehorizons.GydF4y2Ba，和GydF4y2BaMaxhorizon.GydF4y2Ba被选中使得它们是模拟速率的整数倍数。GydF4y2Ba

如前一节所述，选择重量和约束，以促进光滑的驾驶轨迹，同时遵守道路规则。GydF4y2Ba

最后，定义GydF4y2BaspeedLimitGydF4y2Ba和GydF4y2BasafetyGapGydF4y2Ba参数，用于为策划器生成终端状态。GydF4y2Ba

％同步模拟器的更新速率以匹配轨迹生成器的更新速率GydF4y2Ba％离散间隔。GydF4y2Ba场景.sampletime = connector.timeresolution;GydF4y2Ba%在几秒钟内GydF4y2Ba％定义计划参数。GydF4y2BareplanRate = 10;GydF4y2Ba%赫兹GydF4y2Ba%定义当前状态和计划状态之间的时间间隔。GydF4y2BatimeHorizons = 1:3;GydF4y2Ba%在几秒钟内GydF4y2BamaxHorizon = max (timeHorizons);GydF4y2Ba%在几秒钟内GydF4y2Ba％定义成本参数。GydF4y2BaLatdevweight = 1;timeweight = -1;Speedweight = 1;GydF4y2Ba%拒绝违反下列约束的轨迹。GydF4y2Bamaxacceleration = 15;GydF4y2Ba％在米/秒^ 2GydF4y2Bamaxcurvature = 1;GydF4y2Ba% 1/米，或弧度/米GydF4y2BaminVelocity = 0;GydF4y2Ba％在米/秒GydF4y2Ba％期望的速度设定点，用于巡航控制行为和何时GydF4y2Ba%评估轨迹的成本。GydF4y2Baspeedlimit = 11;GydF4y2Ba％在米/秒GydF4y2BaPlanner的％最小距离应瞄准以下行为。GydF4y2BasafetyGap = 10;GydF4y2Ba%在米GydF4y2Ba

初始化模拟器GydF4y2Ba

初始化模拟器并创建GydF4y2BachasePlotGydF4y2Ba（自动驾驶工具箱）GydF4y2Ba查看器。GydF4y2Ba

[场景，Futuretrajortory，Actorid，Actorposs，Egoid，Efoposs，Stepperupdate，Egostate，Isrunning，LineHandles] =GydF4y2Ba…GydF4y2BaexampleHelperInitializeSimulator(场景、capList refPath巷宽,replanRate, carLen);GydF4y2Ba

图包含了一个uipanel类型的对象。GydF4y2Ba

驾驶模拟运行GydF4y2Ba

Tic.GydF4y2Ba尽管GydF4y2Ba在跑GydF4y2Ba%检索参与者车辆的当前状态及其经过的轨迹GydF4y2Ba规划地平线。GydF4y2Ba[curActorState futureTrajectory,正在]=GydF4y2Ba…GydF4y2Baexamplehelperretrieveactorgroundtruth（场景，futuretraboration，Replanrate，maxhorizon）;GydF4y2Ba

%生成巡航控制状态。GydF4y2Ba[termStatesCC, timesCC] = exampleHelperBasicCruiseControl (GydF4y2Ba…GydF4y2BaRefpath，Lanewidth，Egostate，Speedlimit，TimeHorizons）;GydF4y2Ba%生成车道改变状态。GydF4y2Ba[termstateslc，timelc] = examplehelperbasiclanechange（GydF4y2Ba…GydF4y2BarefPath巷宽,egoState timeHorizons);GydF4y2Ba%产生车辆跟随状态。GydF4y2Ba[termStatesF, timesF] = exampleHelperBasicLeadVehicleFollow (GydF4y2Ba…GydF4y2BaRefpath，Lanewidth，Safetegap，Egostate，Cutactorate，Timehorizons）;GydF4y2Ba

%合并终端状态和时间。GydF4y2BaallTS = [termStatesCC;termStatesLC;termStatesF];allDT = [timesCC;timesLC;timesF];numTS =[元素个数(timesCC);元素个数(timesLC);元素个数(timesF)];GydF4y2Ba％评估所有终端状态的成本。GydF4y2Bacostts = examplehelperevaluatetscost（Allts，Alldt，Lanewidth，SpeedLimit，GydF4y2Ba…GydF4y2BaSpeedweight，Latdevweight，Timeweight）;GydF4y2Ba

%生成轨迹。GydF4y2BaEgofrenetstate = Global2fromet（Refpath，Egostate）;[Frenettraj，GlobalTraj] = Connect（连接器，Egofrenettate，Allts，Alldt）;GydF4y2Ba％消除违反限制的轨迹。GydF4y2Baisvalid = examplehelperevaluatetrajectory（globaltraj，maxacceleration，maxcurvature，minvelocity）;GydF4y2Ba

％使用预测的轨迹更新碰撞检查器GydF4y2Ba场景中所有演员的百分比。GydF4y2Ba为了GydF4y2Bai = 1：numel（Actorpose）Actorpose（i）.states = futuretrajection（i）.traptory（：，1：3）;GydF4y2Ba结尾GydF4y2Baupdateobstraclepose（Caplist，Actorid，Actorposes）;GydF4y2Ba％确定评估顺序。GydF4y2Ba[cost, idx] = sort(costTS);optimalTrajectory = [];trajectoryEvaluation =南(元素个数(isValid), 1);GydF4y2Ba％检查每个轨迹是否以最低成本开始。GydF4y2Ba为了GydF4y2Bai = 1:元素个数(idx)GydF4y2Ba如果GydF4y2BaIsValid（IDX（i））GydF4y2Ba%用自我对象的候选轨迹更新胶囊列表。GydF4y2BaEgoposes.States = GlobalTraj（IDX（i））。轨迹（：，1：3）;updateEgoPose (capList egoID egoPoses);GydF4y2Ba％检查碰撞。GydF4y2BaisColliding = checkCollision (capList);GydF4y2Ba如果GydF4y2Ba所有（〜iscollid）GydF4y2Ba％如果没有找到冲突，这是最佳的。GydF4y2Ba%的轨迹。GydF4y2BatrajectoryEvaluation (idx (i)) = 1;optimalTrajectory = globalTraj .Trajectory (idx (i));GydF4y2Ba休息GydF4y2Ba;GydF4y2Ba别的GydF4y2Ba轨迹（IDX（i））= 0;GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba

％显示采样的轨迹。GydF4y2BaLineHandles = exampleHelpervisualizeSce（LineHandles，GlobalTraj，Isvalid，TrajectoryEvaluation）;GydF4y2Ba

图包含了一个uipanel类型的对象。GydF4y2Ba

抓住GydF4y2Ba在GydF4y2Ba;显示（Caplist，GydF4y2Ba“步伐”GydF4y2Ba1: capList。maxnumsteps.那GydF4y2Ba'fastupdate'GydF4y2Ba，1）;抓住GydF4y2Ba离开GydF4y2Ba;GydF4y2Ba如果GydF4y2Baisempty (optimalTrajectory)GydF4y2Ba％所有轨迹都违反了约束或导致碰撞。GydF4y2Ba％GydF4y2Ba%如果计划立即失败，重新访问模拟器、计划者、GydF4y2Ba%和behavior属性。GydF4y2Ba％GydF4y2Ba％如果策划者通过模拟中途失败，额外的GydF4y2Ba%的行为可以用来处理更复杂的规划条件。GydF4y2Ba错误(GydF4y2Ba'没有找到有效的轨迹。GydF4y2Ba）;GydF4y2Ba别的GydF4y2Ba％可视化重新扫描之间的场景。GydF4y2Ba为了GydF4y2Bai = (2 + (0: (stepPerUpdate-1)))GydF4y2Ba近似实时可视化。GydF4y2Badt = toc;GydF4y2Ba如果GydF4y2Ba场景.sampletime-dt> 0暂停（方案.sampletime-dt）;GydF4y2Ba结尾GydF4y2Baegostate = OptimalTrajectory（I，:);场景（1）.position（1：2）= egostate（1：2）;场景（1）。velocity（1：2）= [cos（egostate（3））sin（eNostate（3））] * egostate（5）;场景（1）.yaw = egostate（3）* 180 / pi;场景（1）.AngularVelocity（3）= Egostate（4）* Egostate（5）;GydF4y2Ba%更新胶囊可视化。GydF4y2Ba抓住GydF4y2Ba在GydF4y2Ba;显示（Caplist，GydF4y2Ba“步伐”GydF4y2Ba我:capList。maxnumsteps.那GydF4y2Ba'fastupdate'GydF4y2Ba，1）;抓住GydF4y2Ba离开GydF4y2Ba;GydF4y2Ba％更新驱动方案。GydF4y2Ba推进(scenarioViewer);甘氨胆酸ax =;斧子。ZLim = [-100 100];抽搐;GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba

规划师定制和其他考虑因素GydF4y2Ba

自定义解决方案万博尤文图斯通常涉及许多可调参数，每个参数都能够以难以预测的方式改变最终行为。本节突出显示了一些特定于特定的属性及其对上述规划器的影响。然后，建议提供调整或增强自定义逻辑的方法。GydF4y2Ba

动态胶囊列表GydF4y2Ba

如前所述，GydF4y2BadynamicCapsuleListGydF4y2Ba对象作为一个时间数据库，其中缓存预测的轨迹障碍。您可以在一段时间内对一个或多个自我身体执行碰撞检查。这GydF4y2Bamaxnumsteps.GydF4y2Ba属性确定对象检查的时间步长总数。在上面的模拟循环中，属性被设置为31。这个值意味着计划器检查任何轨迹的整个1-3秒跨度(每0.1秒采样一次)。现在，增加最大值GydF4y2BaTimehorizons.GydF4y2Ba：GydF4y2Ba

timeHorizons = 1:5;GydF4y2Ba%在几秒钟内GydF4y2Bamaxtimehorizon = max（timehorizons）;GydF4y2Ba%在几秒钟内GydF4y2Ba

现在有两种选择:GydF4y2Ba

这GydF4y2Bamaxnumsteps.GydF4y2Ba财产保持不变。GydF4y2Ba
这GydF4y2Bamaxnumsteps.GydF4y2Ba已更新属性以容纳新的最大Timespan。GydF4y2Ba

如果该属性保持不变，则胶囊列表仅验证任何轨迹的前3秒，如果计算效率是至关重要的，或者预测确定性快速下降，则可能是优选的。GydF4y2Ba

或者，可以使用地面真理数据（如上所示），或者未来环境的状态是众所周知的（例如，具有集中控制的全自动环境）。由于此示例使用actors的地面真理数据，请更新属性。GydF4y2Ba

capList。MaxNumSteps = 1 +地板(maxTimeHorizon / scenario.SampleTime);GydF4y2Ba

另一个，间接可调，列表的属性是胶囊几何形状。通过增加的自我车辆或演员的几何形状可以膨胀GydF4y2Ba半径GydF4y2Ba并且可以通过修改车辆来将缓冲区添加到车辆中GydF4y2Ba长度GydF4y2Ba和GydF4y2Bafixedtransform.GydF4y2Ba特性。GydF4y2Ba

通过增加半径膨胀自我车辆的整个足迹。GydF4y2Ba

egogeom.geometry.radius = Lanewidth / 2;GydF4y2Ba%在米GydF4y2BaupdateEgoGeometry (capList egoID egoGeom);GydF4y2Ba

将前后缓冲区添加到所有演员。GydF4y2Ba

actorGeom (1) .Geometry。长度= carLen * 1.5;GydF4y2Ba%在米GydF4y2BaActorgeom（1）.geometry.fixedTransform（1，End）= -ActorgeOM（1）.geometry.Length * Reastaxleratio;GydF4y2Ba%在米GydF4y2BaActorgeom = Repmat（Ictorgeom（1），5,1）;updateobstraclegeometry（Caplist，Actorid，Actorgeom）;GydF4y2Ba

Rerun模拟更新属性GydF4y2Ba

重新运行模拟。由此产生的模拟具有一些有趣的发展：GydF4y2Ba

更长的5秒时间会带来更顺畅的驾驶体验。由于消极因素，规划者仍然会优先考虑较长的轨迹GydF4y2BatimeWeightGydF4y2Ba。GydF4y2Ba
更新后的GydF4y2Bamaxnumsteps.GydF4y2Ba属性已启用完整轨迹的碰撞检查。与较长的规划地平线配对时，规划师识别并丢弃先前最佳的左车道的变化并返回原始车道。GydF4y2Ba
充气胶囊更早发现碰撞并拒绝轨迹，这导致更保守的驾驶行为。这样做的一个潜在缺点是减少了计划范围，这就带来了计划者无法找到有效轨迹的风险。GydF4y2Ba

％初始化模拟器并创建一个ChasePlot查看器。GydF4y2Ba[场景，Futuretrajortory，Actorid，Actorposs，Egoid，Efoposs，Stepperupdate，Egostate，Isrunning，LineHandles] =GydF4y2Ba…GydF4y2BaexampleHelperInitializeSimulator(场景、capList refPath巷宽,replanRate, carLen);抽搐;GydF4y2Ba尽管GydF4y2Ba在跑GydF4y2Ba%检索参与者车辆的当前状态及其经过的轨迹GydF4y2Ba规划地平线。GydF4y2Ba[CURACTORSTATE，FUTURETRAPORIORY，ISRUNNENG] = exampleHelperRetrieveactorgroundtruth（GydF4y2Ba…GydF4y2Ba场景，Futuretrajortory，Replanrate，Maxhorizon）;GydF4y2Ba%生成巡航控制状态。GydF4y2Ba[termStatesCC, timesCC] = exampleHelperBasicCruiseControl (GydF4y2Ba…GydF4y2BaRefpath，Lanewidth，Egostate，Speedlimit，TimeHorizons）;GydF4y2Ba%生成车道改变状态。GydF4y2Ba[termstateslc，timelc] = examplehelperbasiclanechange（GydF4y2Ba…GydF4y2BarefPath巷宽,egoState timeHorizons);GydF4y2Ba%产生车辆跟随状态。GydF4y2Ba[termStatesF, timesF] = exampleHelperBasicLeadVehicleFollow (GydF4y2Ba…GydF4y2BaRefpath，Lanewidth，Safetegap，Egostate，Cutactorate，Timehorizons）;GydF4y2Ba%合并终端状态和时间。GydF4y2BaallTS = [termStatesCC;termStatesLC;termStatesF];allDT = [timesCC;timesLC;timesF];numTS =[元素个数(timesCC);元素个数(timesLC);元素个数(timesF)];GydF4y2Ba％评估所有终端状态的成本。GydF4y2Bacostts = examplehelperevaluatetscost（Allts，Alldt，Lanewidth，SpeedLimit，GydF4y2Ba…GydF4y2BaSpeedweight，Latdevweight，Timeweight）;GydF4y2Ba%生成轨迹。GydF4y2BaEgofrenetstate = Global2fromet（Refpath，Egostate）;[Frenettraj，GlobalTraj] = Connect（连接器，Egofrenettate，Allts，Alldt）;GydF4y2Ba％消除违反限制的轨迹。GydF4y2Ba= exampleHelperEvaluateTrajectory(是否是可用的。GydF4y2Ba…GydF4y2Baglobaltraj，maxacceleration，maxcurvature，minvelocity）;GydF4y2Ba％使用预测的轨迹更新碰撞检查器GydF4y2Ba场景中所有演员的百分比。GydF4y2Ba为了GydF4y2Bai = 1：numel（Actorpose）Actorpose（i）.states = futuretrajection（i）.traptory（：，1：3）;GydF4y2Ba结尾GydF4y2Baupdateobstraclepose（Caplist，Actorid，Actorposes）;GydF4y2Ba％确定评估顺序。GydF4y2Ba[cost, idx] = sort(costTS);optimalTrajectory = [];trajectoryEvaluation =南(元素个数(isValid), 1);GydF4y2Ba％检查每个轨迹是否以最低成本开始。GydF4y2Ba为了GydF4y2Bai = 1:元素个数(idx)GydF4y2Ba如果GydF4y2BaIsValid（IDX（i））GydF4y2Ba%用自我对象的候选轨迹更新胶囊列表。GydF4y2BaEgoposes.States = GlobalTraj（IDX（i））。轨迹（：，1：3）;UpdateEgopose（Caplist，Egoid，Efoposs）;GydF4y2Ba％检查碰撞。GydF4y2BaisColliding = checkCollision (capList);GydF4y2Ba如果GydF4y2Ba所有（〜iscollid）GydF4y2Ba％如果没有找到冲突，这是最佳的GydF4y2Ba%的轨迹。GydF4y2BatrajectoryEvaluation (idx (i)) = 1;optimalTrajectory = globalTraj .Trajectory (idx (i));GydF4y2Ba休息GydF4y2Ba;GydF4y2Ba别的GydF4y2Ba轨迹（IDX（i））= 0;GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba％显示采样的轨迹。GydF4y2BaLineHandles = exampleHelpervisualizeSce（LineHandles，GlobalTraj，Isvalid，TrajectoryEvaluation）;GydF4y2Ba如果GydF4y2Baisempty (optimalTrajectory)GydF4y2Ba％所有轨迹都违反了约束或导致碰撞。GydF4y2Ba％GydF4y2Ba%如果计划立即失败，重新访问模拟器、计划者、GydF4y2Ba%和behavior属性。GydF4y2Ba％GydF4y2Ba％如果策划者通过模拟中途失败，额外的GydF4y2Ba%的行为可以用来处理更复杂的规划条件。GydF4y2Ba错误(GydF4y2Ba'没有找到有效的轨迹。GydF4y2Ba）;GydF4y2Ba别的GydF4y2Ba％可视化重新扫描之间的场景。GydF4y2Ba为了GydF4y2Bai = (2 + (0: (stepPerUpdate-1)))GydF4y2Ba近似实时可视化。GydF4y2Badt = toc;GydF4y2Ba如果GydF4y2Ba场景.sampletime-dt> 0暂停（方案.sampletime-dt）;GydF4y2Ba结尾GydF4y2Baegostate = OptimalTrajectory（I，:);场景（1）.position（1：2）= egostate（1：2）;场景（1）。velocity（1：2）= [cos（egostate（3））sin（eNostate（3））] * egostate（5）;场景（1）.yaw = egostate（3）* 180 / pi;场景（1）.AngularVelocity（3）= Egostate（4）* Egostate（5）;GydF4y2Ba%更新胶囊可视化。GydF4y2Ba抓住GydF4y2Ba在GydF4y2Ba;显示（Caplist，GydF4y2Ba“步伐”GydF4y2Ba我:capList。maxnumsteps.那GydF4y2Ba'fastupdate'GydF4y2Ba，1）;抓住GydF4y2Ba离开GydF4y2Ba;GydF4y2Ba％更新驱动方案。GydF4y2Ba推进(scenarioViewer);甘氨胆酸ax =;斧子。ZLim = [-100 100];抽搐;GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba

图包含了一个uipanel类型的对象。GydF4y2Ba

高速公路轨迹规划使用FreneT参考路径GydF4y2Ba

负载驱动的场景GydF4y2Ba

构建参考路径GydF4y2Ba

构建轨迹发生器GydF4y2Ba

构造动态碰撞检查器GydF4y2Ba

通过流量计划自适应路线GydF4y2Ba

Ground Truth场景GydF4y2Ba

生成终端状态GydF4y2Ba

巡航控制系统GydF4y2Ba

车道改变GydF4y2Ba

遵循铅车辆GydF4y2Ba

评估终端状态的成本GydF4y2Ba

生成轨迹并检查运动学可行性GydF4y2Ba

检查轨迹进行碰撞，然后选择最佳轨迹GydF4y2Ba

定义模拟器和规划参数GydF4y2Ba

初始化模拟器GydF4y2Ba

驾驶模拟运行GydF4y2Ba

规划师定制和其他考虑因素GydF4y2Ba

动态胶囊列表GydF4y2Ba

Rerun模拟更新属性GydF4y2Ba

导航工具箱文档GydF4y2Ba

万博1manbetx