基于Frenet参考路径的公路轨迹规划GydF4y2Ba

此示例使用：GydF4y2Ba

这个示例演示了如何在高速公路驾驶场景中规划局部轨迹。这个例子使用参考路径和动态障碍列表来为自我飞行器生成替代轨迹。自我车辆导航通过在提供的驾驶场景中定义的交通GydF4y2BadrivingScenarioGydF4y2Ba对象车辆根据成本、可行性和无碰撞运动在自适应巡航控制、车道变换和车辆跟随机动之间交替。GydF4y2Ba

负载驱动场景GydF4y2Ba

从装载所提供的开始GydF4y2BadrivingScenarioGydF4y2Ba对象，它在当前工作空间中定义了车辆和道路属性。使用此方案使用GydF4y2Ba驾驶场景设计师GydF4y2Baapp导出到MATLAB®函数，GydF4y2BadrivingScenarioTrafficExampleGydF4y2Ba．有关更多信息，请参见GydF4y2Ba以编程方式创建驾驶场景变化GydF4y2Ba．GydF4y2Ba

场景=驾驶场景交通示例；GydF4y2Ba汽车默认属性GydF4y2Ba卡伦=4.7；GydF4y2Ba%在米GydF4y2BacarWidth = 1.8;GydF4y2Ba%在米GydF4y2BarearAxleRatio =升至;GydF4y2Ba定义道路尺寸GydF4y2Ba巷宽= carWidth * 2;GydF4y2Ba%在米GydF4y2Ba情节(场景);GydF4y2Ba

图中包含一个轴对象。轴对象包含9个面片、线类型的对象。GydF4y2Ba

构造参考路径GydF4y2Ba

该示例中的所有本地规划是关于参考路径执行的，由A表示GydF4y2Ba参考血统GydF4y2Ba(导航工具箱)GydF4y2Ba对象。该对象可以返回沿路径的给定长度的曲线状态，找到沿路径到某个全局点的最近点GydF4y2BaxyGydF4y2Ba-位置，并促进全局参考系和Frenet参考系之间的坐标变换。GydF4y2Ba

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

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

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

构建轨迹发生器GydF4y2Ba

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

终端状态通常使用自定义行为计算。这些行为利用本地环境中的信息，例如车道信息、速度限制以及车辆附近参与者的当前或未来预测。GydF4y2Ba

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

连接器=轨迹生成器FRENET（参考路径）；GydF4y2Ba

构造动态碰撞检查器GydF4y2Ba

这GydF4y2Ba动态封装列表GydF4y2Ba(导航工具箱)GydF4y2Ba对象是一种数据结构，它表示通过一组离散的时间步骤的动态环境的状态。这个环境可以用来有效地验证自我飞行器的多种潜在轨迹。场景中的每个对象都表示为:GydF4y2Ba

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

在这个例子中，轨迹由GydF4y2BaTraometOryGeneratorFromet.GydF4y2Ba对象发生在某些时间段内，称为时间范围。确保碰撞检查涵盖所有可能的轨迹，GydF4y2Ba动态封装列表GydF4y2Ba对象应包含跨越最大预期时间范围的所有参与者的预测轨迹。GydF4y2Ba

caplist =动态浮夸的人;GydF4y2Ba

使用给定参数为ego车辆创建几何结构。GydF4y2Ba

egoID = 1;[egoID, egoGeom] = egoGeometry(capList,egoID);egoGeom.Geometry.Length = carLen;GydF4y2Ba%在米GydF4y2BaegoGeom.Geometry.Radius = carWidth / 2;GydF4y2Ba%在米GydF4y2Ba几何几何固定变换（1，结束）=-carLen*后轴公差；GydF4y2Ba%在米GydF4y2Ba

将ego车辆添加到动态胶囊列表中。GydF4y2Ba

updateEgoGeometry (capList egoID egoGeom);GydF4y2Ba

添加GydF4y2BadrivingScenarioGydF4y2Ba演员到了GydF4y2Ba动态封装列表GydF4y2Ba对象。几何图形是在这里设置的，状态是在规划循环期间定义的。你可以看到GydF4y2Ba动态封装列表GydF4y2Ba现在有一个自我载体和五个障碍。GydF4y2Ba

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

ans=dynamicCapsuleList，其属性为：MaxNumSteps:31个Egoid:1个ObstacleIDs:[5x1双精度]NumObstacles:5个NumEgos:1GydF4y2Ba

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

规划工具支持一种局部规划策略，即在选择最优轨迹金宝app之前，根据当前和可预见的环境状态对一组局部轨迹进行采样。模拟循环被组织成以下几个部分:GydF4y2Ba

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

推进地面真实场景GydF4y2Ba

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

生成终端状态GydF4y2Ba

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

巡航控制GydF4y2Ba

这GydF4y2Ba示例HelperBasicCruiseControlGydF4y2Ba函数生成执行巡航控制行为的终端状态。该功能使用ego车辆的横向速度和时间范围来预测ego车辆未来N秒的预期车道。车道中心将被计算并成为终端状态的横向偏差，并且横向速度和加速度设置为零。GydF4y2Ba

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

$CGydF4y2Ba R.GydF4y2Ba 你GydF4y2Ba 一世GydF4y2Ba S.GydF4y2Ba E.GydF4y2Ba CGydF4y2Ba O.GydF4y2Ba NGydF4y2Ba 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 NGydF4y2Ba E.GydF4y2Ba} 0.GydF4y2Ba 0.GydF4y2Ba]GydF4y2Ba$

车道改变GydF4y2Ba

这GydF4y2Baexamplehelperbasiclanechange.GydF4y2Ba函数生成将车辆从当前车道过渡到相邻车道的终端状态。该函数首先确定自我车辆当前的车道，然后检查左右车道是否存在。对于每条已有车道，终端状态的定义与巡航控制行为相同，只是终端速度设置为当前速度而不是道路限速:GydF4y2Ba

$L.GydF4y2Ba 一种GydF4y2Ba NGydF4y2Ba E.GydF4y2Ba CGydF4y2Ba HGydF4y2Ba 一种GydF4y2Ba NGydF4y2Ba 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 NGydF4y2Ba E.GydF4y2Ba} 0.GydF4y2Ba 0.GydF4y2Ba]$

跟车GydF4y2Ba

这GydF4y2Ba下面的示例帮助器辅助车辆GydF4y2Ba函数生成试图跟踪在自我车辆之前发现的车辆的终端状态。该函数首先确定自我车辆的当前车道。对于每一个提供GydF4y2BatimeHorizonGydF4y2Ba，该函数预测每个行动者的未来状态，找出与自我车辆占据同一车道的所有行动者，并确定哪个行动者最接近GydF4y2Ba领导GydF4y2Ba车辆(如果没有找到领头的车辆，该函数不返回任何东西)。GydF4y2Ba

通过取前车的位置和速度，将末端纵向位置减小一定的安全距离，计算自我车辆的终端状态: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 {阿明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场景。S.一种mP.L.E.Time and时间分辨率GydF4y2Ba同步，以便在地面真理演员轨迹和计划的自我轨迹处发生在同一时间表中。相似地，GydF4y2BareplanRateGydF4y2Ba那GydF4y2BaTimehorizons.GydF4y2Ba,GydF4y2BamaxHorizonGydF4y2Ba被选中使得它们是模拟速率的整数倍数。GydF4y2Ba

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

最后，定义GydF4y2Ba限速GydF4y2Ba和GydF4y2Ba安全间隙GydF4y2Ba参数，用于为规划器生成终端状态。GydF4y2Ba

%同步模拟器的更新速率以匹配轨迹生成器的更新速率GydF4y2Ba%离散化区间。GydF4y2Ba场景.sampletime = connector.timeresolution;GydF4y2Ba%几秒钟内GydF4y2Ba％定义计划参数。GydF4y2BareplanRate = 10;GydF4y2Ba%赫兹GydF4y2Ba%定义当前状态和计划状态之间的时间间隔。GydF4y2BatimeHorizons = 1:3;GydF4y2Ba%几秒钟内GydF4y2BamaxHorizon=最大（时间范围）；GydF4y2Ba%几秒钟内GydF4y2Ba％定义成本参数。GydF4y2BaLatdevweight = 1;timeweight = -1;Speedweight = 1;GydF4y2Ba%拒绝违反下列约束的轨迹。GydF4y2Ba最大加速度=15；GydF4y2Ba% m / s ^ 2GydF4y2BamaxCurvature = 1;GydF4y2Ba%1/米或弧度/米GydF4y2Ba最小速度=0；GydF4y2Ba%在米/秒GydF4y2Ba%巡航控制行为使用的期望速度设定点，以及GydF4y2Ba评估轨迹的成本。GydF4y2Baspeedlimit = 11;GydF4y2Ba%在米/秒GydF4y2Ba%计划员应针对以下行为设定的最小距离。GydF4y2Ba安全间距=10；GydF4y2Ba%在米GydF4y2Ba

初始化模拟器GydF4y2Ba

初始化模拟器并创建一个GydF4y2Ba追逐图GydF4y2Ba观众GydF4y2Ba

[scenarioViewer, futureTrajectory actorID、actorPoses egoID, egoPoses, stepPerUpdate, egoState,正在,lineHandles] =GydF4y2Ba．..GydF4y2Baexamplehelperinitializingimulator（场景，Caplist，Refpath，Lanewidth，Replanrate，Carlen）;GydF4y2Ba

图中包含uipanel类型的对象。GydF4y2Ba

行车模拟GydF4y2Ba

Tic.GydF4y2Ba而GydF4y2Ba在跑GydF4y2Ba找回参与者车辆的当前状态和轨迹GydF4y2Ba%规划视野。GydF4y2Ba[策展人状态，未来轨迹，正在运行]=GydF4y2Ba．..GydF4y2BaexampleHelperRetrieveActorGroundTruth(场景、futureTrajectory replanRate maxHorizon);GydF4y2Ba

%生成巡航控制状态。GydF4y2Ba[termStatesCC，timesCC]=示例HelperBasicCruiseControl(GydF4y2Ba．..GydF4y2BarefPath巷宽,egoState、speedLimit timeHorizons);GydF4y2Ba%生成变道状态。GydF4y2Ba[termstateslc，timelc] = examplehelperbasiclanechange（GydF4y2Ba．..GydF4y2Ba参考路径、laneWidth、自我状态、时间范围）；GydF4y2Ba%生成车辆跟踪状态。GydF4y2Ba[termStatesF, timesF] = exampleHelperBasicLeadVehicleFollow (GydF4y2Ba．..GydF4y2Ba参考路径、laneWidth、安全间隙、自我状态、策展人状态、时间范围）；GydF4y2Ba

%合并终端状态和时间。GydF4y2BaallTS = [termStatesCC;termStatesLC;termStatesF];allDT = [timesCC;timesLC;timesF];numTS =[元素个数(timesCC);元素个数(timesLC);元素个数(timesF)];GydF4y2Ba评估所有终端状态的成本。GydF4y2Ba成本=示例帮助评估成本（allTS、allDT、laneWidth、限速、，GydF4y2Ba．..GydF4y2BaspeedWeight、latDevWeight timeWeight);GydF4y2Ba

%生成轨迹。GydF4y2BaegoFrenetState = global2frenet (refPath egoState);[frenetTraj, globalTraj] =连接(连接器,egoFrenetState、allTS allDT);GydF4y2Ba消除违反约束的轨迹。GydF4y2BaisValid = exampleHelperEvaluateTrajectory (globalTraj maxAcceleration、maxCurvature minVelocity);GydF4y2Ba

%使用预测的轨迹更新碰撞检查器GydF4y2Ba场景中所有演员的百分比。GydF4y2Ba为了GydF4y2Bai=1:numel（actorPoses）actorPoses（i）。状态=未来轨迹（i）。轨迹（：，1:3）；GydF4y2Ba结尾GydF4y2Baupdateobstraclepose（Caplist，Actorid，Actorposes）;GydF4y2Ba%确定评估顺序。GydF4y2Ba[cost，idx]=排序（costTS）；最优轨迹=[]；轨迹评估=nan（numel（isValid），1）；GydF4y2Ba以最低成本开始检查每个轨道的碰撞。GydF4y2Ba为了GydF4y2Bai=1:numel（idx）GydF4y2Ba如果GydF4y2BaIsValid（IDX（i））GydF4y2Ba用ego对象的候选轨迹更新胶囊列表。GydF4y2Ba状态=globalTraj（idx（i））。轨迹（：，1:3）；更新姿态（capList，egoID，egoPoses）；GydF4y2Ba％检查碰撞。GydF4y2BaisColliding = checkCollision (capList);GydF4y2Ba如果GydF4y2Ba(~ isColliding)GydF4y2Ba%如果未发现碰撞，则这是最佳选择。GydF4y2Ba%轨迹。GydF4y2BatrajectoryEvaluation (idx (i)) = 1;optimalTrajectory = globalTraj .Trajectory (idx (i));GydF4y2Ba打破GydF4y2Ba;GydF4y2Ba其他的GydF4y2Ba轨迹评价（idx（i））=0；GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba

％显示采样的轨迹。GydF4y2BalineHandles = exampleHelperVisualizeScene (lineHandles、globalTraj isValid trajectoryEvaluation);GydF4y2Ba

图中包含uipanel类型的对象。GydF4y2Ba

抓住GydF4y2Ba在GydF4y2Ba;显示（Caplist，GydF4y2Ba“时间步”GydF4y2Ba，1:capList.MaxNumSteps，GydF4y2Ba“FastUpdate”GydF4y2Ba，1）;抓住GydF4y2Ba从GydF4y2Ba;GydF4y2Ba如果GydF4y2Ba等空（最优轨迹）GydF4y2Ba％所有轨迹都违反了约束或导致碰撞。GydF4y2Ba％GydF4y2Ba%如果计划立即失败，重新访问模拟器，计划器，GydF4y2Ba%和行为属性。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）;scenarioViewer.Actors（1）.Velocity（1：2）= [cos（egoState（3））SIN（egoState（3））] * egoState（5）;场景（1）.yaw = egostate（3）* 180 / pi;场景（1）.AngularVelocity（3）= Egostate（4）* Egostate（5）;GydF4y2Ba%更新胶囊可视化。GydF4y2Ba抓住GydF4y2Ba在GydF4y2Ba;显示（Caplist，GydF4y2Ba“时间步”GydF4y2Ba，i:capList.MaxNumSteps，GydF4y2Ba“FastUpdate”GydF4y2Ba，1）;抓住GydF4y2Ba从GydF4y2Ba;GydF4y2Ba%更新驱动场景。GydF4y2Ba推进(scenarioViewer);抽搐;GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba

计划定制和其他考虑GydF4y2Ba

自定义解决方案金宝搏官方网站通常涉及许多可调参数，每个参数都能够以难以预测的方式更改最终行为。本节重点介绍一些特定于功能的属性及其对上述规划器的影响。然后，建议提供了调整或增强自定义逻辑的方法。GydF4y2Ba

动态胶囊列表GydF4y2Ba

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

时间范围=1:5；GydF4y2Ba%几秒钟内GydF4y2BamaxTimeHorizon = max (timeHorizons);GydF4y2Ba%几秒钟内GydF4y2Ba

现在有两种选择:GydF4y2Ba

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

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

或者，一个人可能正在使用地面真实数据(如上所示)，或者环境的未来状态是众所周知的(例如，具有集中控制的全自动化环境)。由于本例为参与者使用了地面真实数据，所以更新属性。GydF4y2Ba

capList.MaxNumSteps=1+楼层（maxTimeHorizon/scenario.SampleTime）；GydF4y2Ba

这个列表的另一个间接可调的属性是胶囊的几何结构。通过增加GydF4y2Ba半径GydF4y2Ba并且可以通过修改车辆来将缓冲区添加到车辆中GydF4y2Ba长度GydF4y2Ba和GydF4y2Bafixedtransform.GydF4y2Ba属性。GydF4y2Ba

通过增加半径，使ego车辆的整个足迹膨胀。GydF4y2Ba

egoGeom.Geometry.Radius =巷宽/ 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*rearAxleRatio；GydF4y2Ba%在米GydF4y2BaactorGeom = repmat (actorGeom (1), 5, 1);updateObstacleGeometry (capList actorID actorGeom);GydF4y2Ba

Rerun模拟更新属性GydF4y2Ba

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

更长的5秒时间范围会带来更流畅的驾驶体验。由于负面影响，计划者仍然优先考虑更长的轨迹GydF4y2Ba时重GydF4y2Ba．GydF4y2Ba
更新后的GydF4y2Ba最大步数GydF4y2Ba属性已启用完整轨迹的碰撞检查。与较长的规划地平线配对时，规划师识别并丢弃先前最佳的左车道的变化并返回原始车道。GydF4y2Ba
充气胶囊能更早发现碰撞并排除轨迹，从而导致更保守的驾驶行为。这样做的一个潜在缺点是减少了规划包络线，这就存在规划者无法找到有效轨迹的风险。GydF4y2Ba

％初始化模拟器并创建一个ChasePlot查看器。GydF4y2Ba[scenarioViewer, futureTrajectory actorID、actorPoses egoID, egoPoses, stepPerUpdate, egoState,正在,lineHandles] =GydF4y2Ba．..GydF4y2Ba示例HelperInitializeSimulator（场景、capList、refPath、laneWidth、replanRate、carLen）；tic；GydF4y2Ba而GydF4y2Ba在跑GydF4y2Ba找回参与者车辆的当前状态和轨迹GydF4y2Ba%规划视野。GydF4y2Ba[CURACTORSTATE，FUTURETRAPORIORY，ISRUNNENG] = exampleHelperRetrieveactorgroundtruth（GydF4y2Ba．..GydF4y2Ba场景，Futuretrajortory，Replanrate，Maxhorizon）;GydF4y2Ba%生成巡航控制状态。GydF4y2Ba[termStatesCC，timesCC]=示例HelperBasicCruiseControl(GydF4y2Ba．..GydF4y2BarefPath巷宽,egoState、speedLimit timeHorizons);GydF4y2Ba%生成变道状态。GydF4y2Ba[termstateslc，timelc] = examplehelperbasiclanechange（GydF4y2Ba．..GydF4y2Ba参考路径、laneWidth、自我状态、时间范围）；GydF4y2Ba%生成车辆跟踪状态。GydF4y2Ba[termStatesF, timesF] = exampleHelperBasicLeadVehicleFollow (GydF4y2Ba．..GydF4y2Ba参考路径、laneWidth、安全间隙、自我状态、策展人状态、时间范围）；GydF4y2Ba%合并终端状态和时间。GydF4y2BaallTS = [termStatesCC;termStatesLC;termStatesF];allDT = [timesCC;timesLC;timesF];numTS =[元素个数(timesCC);元素个数(timesLC);元素个数(timesF)];GydF4y2Ba评估所有终端状态的成本。GydF4y2Ba成本=示例帮助评估成本（allTS、allDT、laneWidth、限速、，GydF4y2Ba．..GydF4y2BaSpeedweight，Latdevweight，Timeweight）;GydF4y2Ba%生成轨迹。GydF4y2BaegoFrenetState = global2frenet (refPath egoState);[frenetTraj, globalTraj] =连接(连接器,egoFrenetState、allTS allDT);GydF4y2Ba消除违反约束的轨迹。GydF4y2BaisValid=ExampleHelperEvaluateTracectory(GydF4y2Ba．..GydF4y2BaglobalTraj, maxAcceleration, max曲率，minVelocity);GydF4y2Ba%使用预测的轨迹更新碰撞检查器GydF4y2Ba场景中所有演员的百分比。GydF4y2Ba为了GydF4y2Bai=1:numel（actorPoses）actorPoses（i）。状态=未来轨迹（i）。轨迹（：，1:3）；GydF4y2Ba结尾GydF4y2BaupdateObstaclePose (capList actorID actorPoses);GydF4y2Ba%确定评估顺序。GydF4y2Ba[cost，idx]=排序（costTS）；最优轨迹=[]；轨迹评估=nan（numel（isValid），1）；GydF4y2Ba以最低成本开始检查每个轨道的碰撞。GydF4y2Ba为了GydF4y2Bai=1:numel（idx）GydF4y2Ba如果GydF4y2BaIsValid（IDX（i））GydF4y2Ba用ego对象的候选轨迹更新胶囊列表。GydF4y2BaegoPoses。S.T.一种T.E.S.=GL.O.balTraj(idx(i)).Trajectory(:,1:3); updateEgoPose(capList, egoID, egoPoses);％检查碰撞。GydF4y2BaisColliding = checkCollision (capList);GydF4y2Ba如果GydF4y2Ba(~ isColliding)GydF4y2Ba%如果未发现碰撞，则这是最佳选择GydF4y2Ba%轨迹。GydF4y2BatrajectoryEvaluation (idx (i)) = 1;optimalTrajectory = globalTraj .Trajectory (idx (i));GydF4y2Ba打破GydF4y2Ba;GydF4y2Ba其他的GydF4y2Ba轨迹评价（idx（i））=0；GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba％显示采样的轨迹。GydF4y2BalineHandles=ExampleHelperServiceSualizeScene（lineHandles、globalTraj、isValid、trajectoryEvaluation）；GydF4y2Ba如果GydF4y2Ba等空（最优轨迹）GydF4y2Ba％所有轨迹都违反了约束或导致碰撞。GydF4y2Ba％GydF4y2Ba%如果计划立即失败，重新访问模拟器，计划器，GydF4y2Ba%和行为属性。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）;scenarioViewer.Actors（1）.Velocity（1：2）= [cos（egoState（3））SIN（egoState（3））] * egoState（5）;场景（1）.yaw = egostate（3）* 180 / pi;场景（1）.AngularVelocity（3）= Egostate（4）* Egostate（5）;GydF4y2Ba%更新胶囊可视化。GydF4y2Ba抓住GydF4y2Ba在GydF4y2Ba;显示（Caplist，GydF4y2Ba“时间步”GydF4y2Ba，i:capList.MaxNumSteps，GydF4y2Ba“FastUpdate”GydF4y2Ba，1）;抓住GydF4y2Ba从GydF4y2Ba;GydF4y2Ba%更新驱动场景。GydF4y2Ba推进(scenarioViewer);抽搐;GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba结尾GydF4y2Ba