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

这个示例使用:gydF4y2Ba

此示例演示如何在高速公路驾驶场景中规划局部轨迹。本例使用参考路径和动态障碍列表为自我交通工具生成可选轨迹。自我车辆在提供的驾驶场景中定义的交通中导航gydF4y2BadrivingScenariogydF4y2Ba对象。根据成本、可行性和无碰撞运动，车辆在自适应巡航控制、变道和车辆跟随机动之间进行交替。gydF4y2Ba

HighwayTrajectoryPlanningUsingFrenetReferencePathExample_01.gifgydF4y2Ba

负载驱动的场景gydF4y2Ba

首先加载所提供的gydF4y2BadrivingScenariogydF4y2Ba对象，该对象定义当前工作空间中的车辆和道路属性。方法生成此场景gydF4y2Ba驾驶场景设计师gydF4y2Ba(自动驾驶工具箱)gydF4y2Baapp和导出到MATLAB®函数，gydF4y2BadrivingScenarioTrafficExamplegydF4y2Ba。有关更多信息，请参见gydF4y2Ba以编程方式创建驾驶场景变化gydF4y2Ba(自动驾驶工具箱)gydF4y2Ba。gydF4y2Ba

场景= drivingScenarioTrafficExample;gydF4y2Ba%默认汽车属性gydF4y2BacarLen = 4.7;gydF4y2Ba%在米gydF4y2BacarWidth = 1.8;gydF4y2Ba%在米gydF4y2BarearAxleRatio =升至;gydF4y2Ba%定义道路尺寸gydF4y2Ba巷宽= carWidth * 2;gydF4y2Ba%在米gydF4y2Ba情节(场景);gydF4y2Ba

图中包含一个axes对象。axis对象包含9个类型为patch、line的对象。gydF4y2Ba

构造参考路径gydF4y2Ba

本例中的所有本地规划都是根据引用路径执行的，该路径由a表示gydF4y2BareferencePathFrenetgydF4y2Ba对象。这个对象可以返回沿路径给定长度处曲线的状态，找到沿路径到某个全局点的最近点gydF4y2BaxygydF4y2Ba-location，并促进全局参考帧和Frenet参考帧之间的坐标转换。gydF4y2Ba

对于本例，参考路径被视为四车道高速公路的中心，并且路径点与所提供的定义的道路相匹配gydF4y2BadrivingScenariogydF4y2Ba对象。gydF4y2Ba

航路点= [0 50;150 50;300 75;310 75;400 0;300 -50;290 -50;0 -50];gydF4y2Ba%在米gydF4y2BarefPath = referencePathFrenet(锚点);ax =显示(refPath);轴(ax,gydF4y2Ba“平等”gydF4y2Ba）;包含(gydF4y2Ba“X”gydF4y2Ba）;ylabel (gydF4y2Ba“Y”gydF4y2Ba）;gydF4y2Ba

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

构建轨迹发生器gydF4y2Ba

对于一个局部规划者来说，其目标通常是在满足当前运动学和动力学条件的同时，对朝着最终目标移动的各种可能的运动进行抽样。的gydF4y2BatrajectoryGeneratorFrenetgydF4y2Ba对象通过使用四阶或五阶多项式轨迹将初始状态与一组终端状态连接起来来实现这一点。在Frenet坐标系中定义初始和终点状态，每个多项式解都满足横向和纵向的位置、速度和加速度边界条件，同时最大限度地减小抖动。gydF4y2Ba

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

构造一个gydF4y2BatrajectoryGeneratorFrenetgydF4y2Ba对象使用引用路径gydF4y2Ba

连接器= trajectoryGeneratorFrenet (refPath);gydF4y2Ba

构造动态碰撞检查器gydF4y2Ba

的gydF4y2BadynamicCapsuleListgydF4y2Ba对象是一个数据结构，它表示动态环境在一组离散时间步骤上的状态。这个环境可以用来有效地验证自我载体的多个潜在轨迹。场景中的每个物体都用:gydF4y2Ba

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

在这个例子中，轨迹由gydF4y2BatrajectoryGeneratorFrenetgydF4y2Ba物体在一定的时间跨度内发生，称为时间视界。为了确保碰撞检查涵盖所有可能的轨迹gydF4y2BadynamicCapsuleListgydF4y2Ba对象应包含所有参与者在最大预期时间范围内的预测轨迹。gydF4y2Ba

capList = dynamicCapsuleList;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

添加gydF4y2BadrivingScenariogydF4y2Ba演员的gydF4y2BadynamicCapsuleListgydF4y2Ba对象。在这里设置几何图形，并在规划循环期间定义状态。你可以看到gydF4y2BadynamicCapsuleListgydF4y2Ba现在包含了一个自我载体和五个障碍。gydF4y2Ba

actorID = (1:5) ';actorGeom = repelem (egoGeom 5 1);updateObstacleGeometry (capList actorID actorGeom)gydF4y2Ba

ans = dynamicCapsuleList with properties: MaxNumSteps: 31 EgoIDs: 1 ObstacleIDs: [5x1 double] NumObstacles: 5 NumEgos: 1gydF4y2Ba

通过交通规划自适应路由gydF4y2Ba

规划实用程序支持一个局部规划策略，该策略基于当前和可预见的环境状态对一组局部轨迹进行采样，然后选择最优轨迹。模拟循环被组织成以下部分:gydF4y2Ba

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

提前地面真相场景gydF4y2Ba

在动态环境中进行规划时，通常需要估计环境的状态或预测其在不久的将来的状态。为简单起见，本示例使用gydF4y2BadrivingScenariogydF4y2Ba作为规划视界中每个参与者轨迹的基本真相来源。属性可替换或修改自定义预测算法gydF4y2BaexampleHelperRetrieveActorGroundTruthgydF4y2Ba使用自定义代码函数。gydF4y2Ba

生成终端状态gydF4y2Ba

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

巡航控制系统gydF4y2Ba

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

对于纵向边界条件，终端速度设置为道路限速，终端加速度设置为零。纵向位置不受限制，指定为gydF4y2Ba南gydF4y2Ba。通过取消经度约束，gydF4y2BatrajectoryGeneratorFrenetgydF4y2Ba可以使用下四阶多项式来解决纵向边值问题，得到在给定时间范围内与道路速度平滑匹配的轨迹:gydF4y2Ba

$cgydF4y2Ba rgydF4y2Ba ugydF4y2Ba 我gydF4y2Ba 年代gydF4y2Ba egydF4y2Ba CgydF4y2Ba ogydF4y2Ba ngydF4y2Ba tgydF4y2Ba rgydF4y2Ba ogydF4y2Ba lgydF4y2Ba 年代gydF4y2Ba tgydF4y2Ba 一个gydF4y2Ba tgydF4y2Ba egydF4y2Ba ＝gydF4y2Ba ［gydF4y2Ba 南gydF4y2Ba {\overset{˙gydF4y2Ba}{年代gydF4y2Ba}}_{dgydF4y2Ba egydF4y2Ba 年代gydF4y2Ba} 0gydF4y2Ba {lgydF4y2Ba}_{egydF4y2Ba xgydF4y2Ba pgydF4y2Ba lgydF4y2Ba 一个gydF4y2Ba ngydF4y2Ba egydF4y2Ba} 0gydF4y2Ba 0gydF4y2Ba ］gydF4y2Ba$

车道改变gydF4y2Ba

的gydF4y2BaexampleHelperBasicLaneChangegydF4y2Ba函数生成将车辆从当前车道转移到相邻车道的终端状态。该函数首先确定自我车辆的当前车道，然后检查左右车道是否存在。对于每个现有车道，终端状态的定义方式与巡航控制行为相同，除了终端速度设置为当前速度，而不是道路的速度限制:gydF4y2Ba

$lgydF4y2Ba 一个gydF4y2Ba ngydF4y2Ba egydF4y2Ba CgydF4y2Ba hgydF4y2Ba 一个gydF4y2Ba ngydF4y2Ba ggydF4y2Ba egydF4y2Ba 年代gydF4y2Ba tgydF4y2Ba 一个gydF4y2Ba tgydF4y2Ba egydF4y2Ba ＝gydF4y2Ba [南gydF4y2Ba {\overset{˙gydF4y2Ba}{年代gydF4y2Ba}}_{cgydF4y2Ba ugydF4y2Ba rgydF4y2Ba} 0gydF4y2Ba {lgydF4y2Ba}_{dgydF4y2Ba egydF4y2Ba 年代gydF4y2Ba lgydF4y2Ba 一个gydF4y2Ba ngydF4y2Ba egydF4y2Ba} 0gydF4y2Ba 0gydF4y2Ba]$

跟随车辆gydF4y2Ba

的gydF4y2BaexampleHelperBasicLeadVehicleFollowgydF4y2Ba函数生成试图跟踪在自我车辆前面找到的车辆的终端状态。该函数首先确定自我车辆的当前车道。对于每一个提供gydF4y2BatimeHorizongydF4y2Ba，该函数预测每个行为人的未来状态，找出与自我载体占据同一车道的所有行为人，并确定哪个是最近的gydF4y2Ba引领gydF4y2Ba车辆(如果没有找到前导车辆，则函数不返回任何内容)。gydF4y2Ba

自我车终点状态的计算方法是取前车的位置和速度，并将前车的终点纵向位置减小一定的安全距离:gydF4y2Ba

$cgydF4y2Ba lgydF4y2Ba ogydF4y2Ba 年代gydF4y2Ba egydF4y2Ba 年代gydF4y2Ba tgydF4y2Ba lgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba VgydF4y2Ba egydF4y2Ba hgydF4y2Ba 我gydF4y2Ba cgydF4y2Ba lgydF4y2Ba 房地产gydF4y2Ba ＝gydF4y2Ba ［gydF4y2Ba {年代gydF4y2Ba}_{lgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba} {\overset{˙gydF4y2Ba}{年代gydF4y2Ba}}_{lgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba} 0gydF4y2Ba {lgydF4y2Ba}_{lgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba} {\overset{˙gydF4y2Ba}{lgydF4y2Ba}}_{lgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba} 0gydF4y2Ba ］gydF4y2Ba$

$followStategydF4y2Ba ＝gydF4y2Ba ［gydF4y2Ba （gydF4y2Ba {年代gydF4y2Ba}_{lgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba} -gydF4y2Ba {dgydF4y2Ba}_{安全gydF4y2Ba} ）gydF4y2Ba {\overset{˙gydF4y2Ba}{年代gydF4y2Ba}}_{lgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba} 0gydF4y2Ba {lgydF4y2Ba}_{lgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba} {\overset{˙gydF4y2Ba}{lgydF4y2Ba}}_{lgydF4y2Ba egydF4y2Ba 一个gydF4y2Ba dgydF4y2Ba} 0gydF4y2Ba ］gydF4y2Ba$

评估终端国的成本gydF4y2Ba

终端状态生成后，就可以对其代价进行评估。轨迹评估和优先考虑潜在解决方案的方法是高度主观的。为了简单起见，我们gydF4y2BaexampleHelperEvaluateTSCostgydF4y2Ba函数将成本定义为三个加权和的组合。gydF4y2Ba

横向偏差成本(gydF4y2Ba ${CgydF4y2Ba}_{lgydF4y2Ba 一个gydF4y2Ba tgydF4y2Ba DgydF4y2Ba egydF4y2Ba vgydF4y2Ba}$ ）gydF4y2Ba-惩罚偏离车道中心的状态的正权值。gydF4y2Ba

${CgydF4y2Ba}_{lgydF4y2Ba 一个gydF4y2Ba tgydF4y2Ba DgydF4y2Ba egydF4y2Ba vgydF4y2Ba} ＝gydF4y2Ba {wgydF4y2Ba}_{ΔgydF4y2Ba lgydF4y2Ba} ＊gydF4y2Ba ΔgydF4y2Ba lgydF4y2Ba$

$ΔgydF4y2Ba lgydF4y2Ba ＝gydF4y2Ba {argmingydF4y2Ba}_{我gydF4y2Ba} （gydF4y2Ba |gydF4y2Ba {lgydF4y2Ba}_{termStategydF4y2Ba} -gydF4y2Ba {lgydF4y2Ba}_{{车道gydF4y2Ba}_{我gydF4y2Ba}} |gydF4y2Ba ）gydF4y2Ba$

时间成本(gydF4y2Ba ${CgydF4y2Ba}_{tgydF4y2Ba 我gydF4y2Ba 米gydF4y2Ba egydF4y2Ba}$ ）gydF4y2Ba-一个负权重，优先发生在较长时间间隔的运动，导致更平滑的轨迹。gydF4y2Ba

${CgydF4y2Ba}_{tgydF4y2Ba 我gydF4y2Ba 米gydF4y2Ba egydF4y2Ba} ＝gydF4y2Ba {wgydF4y2Ba}_{ΔgydF4y2Ba tgydF4y2Ba} ＊gydF4y2Ba ΔgydF4y2Ba tgydF4y2Ba$

终端速度成本(gydF4y2Ba ${CgydF4y2Ba}_{年代gydF4y2Ba pgydF4y2Ba egydF4y2Ba egydF4y2Ba dgydF4y2Ba}$ ）gydF4y2Ba-优先考虑保持速度限制的运动的正权重，导致较少的动态机动。gydF4y2Ba

${CgydF4y2Ba}_{年代gydF4y2Ba pgydF4y2Ba egydF4y2Ba egydF4y2Ba dgydF4y2Ba} ＝gydF4y2Ba {wgydF4y2Ba}_{ΔgydF4y2Ba vgydF4y2Ba} ＊gydF4y2Ba ΔgydF4y2Ba vgydF4y2Ba$

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

除了具有成本最小的终端状态外，最优轨迹通常还必须满足与运动学可行性和平顺性相关的附加约束。轨迹约束是一种强制执行所需乘坐质量的方法，但这是以减少驾驶包络为代价的。gydF4y2Ba

在本例中，gydF4y2BaexampleHelperEvaluateTrajectorygydF4y2Ba函数验证每个轨迹满足以下约束:gydF4y2Ba

MaxAccelerationgydF4y2Ba:整个轨迹的绝对加速度必须小于指定值。较小的值降低了驱动的侵略性，并消除了运动学上不可行的轨迹。这种限制可以消除车辆本来可以执行的机动。gydF4y2Ba
MaxCurvaturegydF4y2Ba:整个轨迹允许的最小转弯半径。与gydF4y2BaMaxAccelerationgydF4y2Ba，降低这个值会带来更平稳的驾驶体验，但可能会消除其他可行的轨迹。gydF4y2Ba
MinVelocitygydF4y2Ba:这个例子通过设置最小速度来约束自我车辆只向前运动。这种限制在高速公路驾驶场景中是需要的，并消除轨迹适合过度约束或条件差的边界值。gydF4y2Ba

检查碰撞轨迹并选择最佳轨迹gydF4y2Ba

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

定义模拟器和规划参数gydF4y2Ba

本节定义运行模拟器所需的属性以及规划器和行为实用程序使用的参数。属性,如gydF4y2Ba场景。年代一个米pleTime and连接器。TimeResolutiongydF4y2Ba是同步的，这样在基本真相的状态，行为人的轨迹和计划的自我的轨迹发生在相同的时间步。同样的,gydF4y2BareplanRategydF4y2Ba，gydF4y2BatimeHorizonsgydF4y2Ba,gydF4y2BamaxHorizongydF4y2Ba的选择使它们是模拟速率的整数倍。gydF4y2Ba

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

最后,定义gydF4y2BaspeedLimitgydF4y2Ba和gydF4y2BasafetyGapgydF4y2Ba参数，这些参数用于为计划器生成终端状态。gydF4y2Ba

同步模拟器的更新速率，以匹配轨迹发生器的gydF4y2Ba%离散化区间。gydF4y2Ba场景。年代一个米pleTime = connector.TimeResolution;%在几秒钟内gydF4y2Ba定义规划参数。gydF4y2BareplanRate = 10;gydF4y2Ba%赫兹gydF4y2Ba定义当前状态和计划状态之间的时间间隔。。gydF4y2BatimeHorizons = 1:3;gydF4y2Ba%在几秒钟内gydF4y2BamaxHorizon = max (timeHorizons);gydF4y2Ba%在几秒钟内gydF4y2Ba定义成本参数。gydF4y2BalatDevWeight = 1;timeWeight = 1;speedWeight = 1;gydF4y2Ba拒绝违反下列约束的轨迹。。gydF4y2BamaxAcceleration = 15;gydF4y2Ba% m / s ^ 2gydF4y2BamaxCurvature = 1;gydF4y2Ba% 1/米，或弧度/米gydF4y2BaminVelocity = 0;gydF4y2Ba%在米/秒gydF4y2Ba%期望速度设定值，用于巡航控制行为和何时gydF4y2Ba%评估弹道的成本。gydF4y2BaspeedLimit = 11;gydF4y2Ba%在米/秒gydF4y2Ba%计划者应针对下列行为设定的最小距离。gydF4y2BasafetyGap = 10;gydF4y2Ba%在米gydF4y2Ba

初始化模拟器gydF4y2Ba

初始化模拟器并创建一个gydF4y2BachasePlotgydF4y2Ba(自动驾驶工具箱)gydF4y2Ba查看器。gydF4y2Ba

[scenarioViewer, futureTrajectory actorID、actorPoses egoID, egoPoses, stepPerUpdate, egoState,正在,lineHandles] =gydF4y2Ba.．.gydF4y2BaexampleHelperInitializeSimulator(场景、capList refPath巷宽,replanRate, carLen);gydF4y2Ba

驾驶模拟运行gydF4y2Ba

抽搐gydF4y2Ba而gydF4y2Ba正在gydF4y2Ba检索角色载具的当前状态及其轨迹。gydF4y2Ba规划的范围。gydF4y2Ba[curActorState futureTrajectory,正在]=gydF4y2Ba.．.gydF4y2BaexampleHelperRetrieveActorGroundTruth(场景、futureTrajectory replanRate maxHorizon);gydF4y2Ba

生成巡航控制状态。gydF4y2Ba[termStatesCC, timesCC] = exampleHelperBasicCruiseControl (gydF4y2Ba.．.gydF4y2BarefPath巷宽,egoState、speedLimit timeHorizons);gydF4y2Ba生成变道状态。gydF4y2Ba[termStatesLC, timesLC] = exampleHelperBasicLaneChange (gydF4y2Ba.．.gydF4y2BarefPath巷宽,egoState timeHorizons);gydF4y2Ba生成车辆跟随状态。gydF4y2Ba[termStatesF, timesF] = exampleHelperBasicLeadVehicleFollow (gydF4y2Ba.．.gydF4y2BarefPath,巷宽、safetyGap egoState、curActorState timeHorizons);gydF4y2Ba

结合终端状态和时间。。gydF4y2BaallTS = [termStatesCC;termStatesLC;termStatesF];allDT = [timesCC;timesLC;timesF];numTS =[元素个数(timesCC);元素个数(timesLC);元素个数(timesF)];gydF4y2Ba评估所有终端状态的成本。。gydF4y2BacostTS = exampleHelperEvaluateTSCost (allTS allDT巷宽,speedLimit,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(actorposed) actorposed (i)。年代t一个te年代＝futureTrajectory(i).Trajectory(:,1:3);结束gydF4y2BaupdateObstaclePose (capList actorID actorPoses);gydF4y2Ba确定评估顺序。gydF4y2Ba[cost, idx] = sort(cost);optimalTrajectory = [];trajectoryEvaluation =南(元素个数(isValid), 1);gydF4y2Ba检查每一个碰撞轨迹，从最少的代价开始。。gydF4y2Ba为gydF4y2Bai = 1:元素个数(idx)gydF4y2Ba如果gydF4y2BaisValid (idx(我))gydF4y2Ba用自我对象的候选轨迹更新胶囊列表。。gydF4y2BaegoPoses。年代t一个te年代＝globalTraj(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其他的gydF4y2BatrajectoryEvaluation (idx (i)) = 0;gydF4y2Ba结束gydF4y2Ba结束gydF4y2Ba结束gydF4y2Ba

显示采样轨迹。gydF4y2BalineHandles = exampleHelperVisualizeScene (lineHandles、globalTraj isValid trajectoryEvaluation);gydF4y2Ba

持有gydF4y2Ba在gydF4y2Ba；显示(capListgydF4y2Ba“步伐”gydF4y2Ba1: capList。MaxNumSteps，gydF4y2Ba“FastUpdate”gydF4y2Ba1);持有gydF4y2Ba从gydF4y2Ba；gydF4y2Ba如果gydF4y2Baisempty (optimalTrajectory)gydF4y2Ba所有轨迹要么违反约束，要么导致碰撞。。gydF4y2Ba％gydF4y2Ba如果计划立即失败，重新审视模拟器、计划器、gydF4y2Ba%和行为属性。gydF4y2Ba％gydF4y2Ba如果规划者在模拟中途失败，额外。gydF4y2Ba可以引入行为来处理更复杂的规划条件。。gydF4y2Ba错误(gydF4y2Ba“没有发现有效的弹道。”gydF4y2Ba）;gydF4y2Ba其他的gydF4y2Ba在重新规划之间想象场景。gydF4y2Ba为gydF4y2Bai = (2 + (0: (stepPerUpdate-1)))gydF4y2Ba近似实时可视化。gydF4y2Badt = toc;gydF4y2Ba如果gydF4y2Ba场景。年代一个米pleTime-dt > 0 pause(scenario.SampleTime-dt);结束gydF4y2BaegoState = optimalTrajectory(我);scenarioViewer.Actors (1) .Position (1:2) = egoState (1:2);scenarioViewer.Actors(1).Velocity(1:2) = [cos(egoState(3)) sin(egoState(3))]*egoState(5);scenarioViewer.Actors(1)。偏航= egoState(3) * 180 /π;scenarioViewer.Actors (1) .AngularVelocity (3) = egoState (4) * egoState (5);gydF4y2Ba更新胶囊可视化。gydF4y2Ba持有gydF4y2Ba在gydF4y2Ba；显示(capListgydF4y2Ba“步伐”gydF4y2Ba我:capList。MaxNumSteps，gydF4y2Ba“FastUpdate”gydF4y2Ba1);持有gydF4y2Ba从gydF4y2Ba；gydF4y2Ba%更新驾驶场景。gydF4y2Ba推进(scenarioViewer);抽搐;gydF4y2Ba结束gydF4y2Ba结束gydF4y2Ba结束gydF4y2Ba

计划器定制和其他注意事项gydF4y2Ba

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

动态膜表gydF4y2Ba

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

timeHorizons = 1:5;gydF4y2Ba%在几秒钟内gydF4y2BamaxTimeHorizon = max (timeHorizons);gydF4y2Ba%在几秒钟内gydF4y2Ba

现在有两个选择:gydF4y2Ba

的gydF4y2BaMaxNumStepsgydF4y2Ba属性保持不变。gydF4y2Ba
的gydF4y2BaMaxNumStepsgydF4y2Ba属性已更新以适应新的最大时间跨度。gydF4y2Ba

如果属性保持不变，那么胶囊列表只验证任何轨迹的前3秒，如果计算效率非常重要或预测确定性迅速下降，这可能是可取的。gydF4y2Ba

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

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

列表的另一个间接可调属性是胶囊几何形状。自我载体或角色的几何形状可以通过增加gydF4y2Ba半径gydF4y2Ba，并可通过修改为车辆添加缓冲区gydF4y2Ba长度gydF4y2Ba和gydF4y2BaFixedTransformgydF4y2Ba属性。gydF4y2Ba

通过增加半径使自我战车的整个足迹膨胀。gydF4y2Ba

egoGeom.Geometry.Radius =巷宽/ 2;gydF4y2Ba%在米gydF4y2BaupdateEgoGeometry (capList egoID egoGeom);gydF4y2Ba

为所有角色添加前后缓冲区。gydF4y2Ba

actorGeom (1) .Geometry。长度= carLen * 1.5;gydF4y2Ba%在米gydF4y2BaactorGeom (1) .Geometry.FixedTransform (1) = -actorGeom (1) .Geometry.Length * rearAxleRatio;gydF4y2Ba%在米gydF4y2BaactorGeom = repmat (actorGeom (1), 5, 1);updateObstacleGeometry (capList actorID actorGeom);gydF4y2Ba

重新运行带有更新属性的模拟gydF4y2Ba

重新运行模拟。由此产生的模拟有一些有趣的发展:gydF4y2Ba

更长的5秒时间范围会带来更平稳的驾驶体验。由于消极因素，规划者仍然优先考虑较长的轨迹gydF4y2BatimeWeightgydF4y2Ba。gydF4y2Ba
更新后的gydF4y2BaMaxNumStepsgydF4y2Ba属性启用了对整个轨迹的碰撞检查。当与较长的规划视距配对时，规划者识别并丢弃先前最优的左车道改变，并返回原车道。gydF4y2Ba
充气胶囊能更早地发现碰撞并排除轨迹，从而导致更保守的驾驶行为。这样做的一个潜在缺点是减少了规划包络线，这可能会导致规划人员无法找到有效的轨迹。gydF4y2Ba

初始化模拟器并创建一个chasePlot查看器。gydF4y2Ba[scenarioViewer, futureTrajectory actorID、actorPoses egoID, egoPoses, stepPerUpdate, egoState,正在,lineHandles] =gydF4y2Ba.．.gydF4y2BaexampleHelperInitializeSimulator(场景、capList refPath巷宽,replanRate, carLen);抽搐;gydF4y2Ba而gydF4y2Ba正在gydF4y2Ba检索角色载具的当前状态及其轨迹。gydF4y2Ba规划的范围。gydF4y2Ba[curActorState futureTrajectory,正在]= exampleHelperRetrieveActorGroundTruth (gydF4y2Ba.．.gydF4y2Ba场景、futureTrajectory replanRate maxHorizon);gydF4y2Ba生成巡航控制状态。gydF4y2Ba[termStatesCC, timesCC] = exampleHelperBasicCruiseControl (gydF4y2Ba.．.gydF4y2BarefPath巷宽,egoState、speedLimit timeHorizons);gydF4y2Ba生成变道状态。gydF4y2Ba[termStatesLC, timesLC] = exampleHelperBasicLaneChange (gydF4y2Ba.．.gydF4y2BarefPath巷宽,egoState timeHorizons);gydF4y2Ba生成车辆跟随状态。gydF4y2Ba[termStatesF, timesF] = exampleHelperBasicLeadVehicleFollow (gydF4y2Ba.．.gydF4y2BarefPath,巷宽、safetyGap egoState、curActorState timeHorizons);gydF4y2Ba结合终端状态和时间。。gydF4y2BaallTS = [termStatesCC;termStatesLC;termStatesF];allDT = [timesCC;timesLC;timesF];numTS =[元素个数(timesCC);元素个数(timesLC);元素个数(timesF)];gydF4y2Ba评估所有终端状态的成本。。gydF4y2BacostTS = exampleHelperEvaluateTSCost (allTS allDT巷宽,speedLimit,gydF4y2Ba.．.gydF4y2BaspeedWeight、latDevWeight timeWeight);gydF4y2Ba%生成轨迹。gydF4y2BaegoFrenetState = global2frenet (refPath egoState);[frenetTraj, globalTraj] =连接(连接器,egoFrenetState、allTS allDT);gydF4y2Ba消除违反约束的轨迹。。gydF4y2Ba= exampleHelperEvaluateTrajectory(是否是可用的。gydF4y2Ba.．.gydF4y2BaglobalTraj, maxAcceleration, max曲率，minVelocity);gydF4y2Ba用预测的轨迹更新碰撞检查器。gydF4y2Ba%所有演员在现场。gydF4y2Ba为gydF4y2Bai = 1:numel(actorposed) actorposed (i)。年代t一个te年代＝futureTrajectory(i).Trajectory(:,1:3);结束gydF4y2BaupdateObstaclePose (capList actorID actorPoses);gydF4y2Ba确定评估顺序。gydF4y2Ba[cost, idx] = sort(cost);optimalTrajectory = [];trajectoryEvaluation =南(元素个数(isValid), 1);gydF4y2Ba检查每一个碰撞轨迹，从最少的代价开始。。gydF4y2Ba为gydF4y2Bai = 1:元素个数(idx)gydF4y2Ba如果gydF4y2BaisValid (idx(我))gydF4y2Ba用自我对象的候选轨迹更新胶囊列表。。gydF4y2BaegoPoses。年代t一个te年代＝globalTraj(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其他的gydF4y2BatrajectoryEvaluation (idx (i)) = 0;gydF4y2Ba结束gydF4y2Ba结束gydF4y2Ba结束gydF4y2Ba显示采样轨迹。gydF4y2BalineHandles = exampleHelperVisualizeScene(lineHandles, globalTraj, isValid, trajectory yevaluation);gydF4y2Ba如果gydF4y2Baisempty (optimalTrajectory)gydF4y2Ba所有轨迹要么违反约束，要么导致碰撞。。gydF4y2Ba％gydF4y2Ba如果计划立即失败，重新审视模拟器、计划器、gydF4y2Ba%和行为属性。gydF4y2Ba％gydF4y2Ba如果规划者在模拟中途失败，额外。gydF4y2Ba可以引入行为来处理更复杂的规划条件。。gydF4y2Ba错误(gydF4y2Ba“没有发现有效的弹道。”gydF4y2Ba）;gydF4y2Ba其他的gydF4y2Ba在重新规划之间想象场景。gydF4y2Ba为gydF4y2Bai = (2 + (0: (stepPerUpdate-1)))gydF4y2Ba近似实时可视化。gydF4y2Badt = toc;gydF4y2Ba如果gydF4y2Ba场景。年代一个米pleTime-dt > 0 pause(scenario.SampleTime-dt);结束gydF4y2BaegoState = optimalTrajectory(我);scenarioViewer.Actors (1) .Position (1:2) = egoState (1:2);scenarioViewer.Actors(1).Velocity(1:2) = [cos(egoState(3)) sin(egoState(3))]*egoState(5);scenarioViewer.Actors(1)。偏航= egoState(3) * 180 /π;scenarioViewer.Actors (1) .AngularVelocity (3) = egoState (4) * egoState (5);gydF4y2Ba更新胶囊可视化。gydF4y2Ba持有gydF4y2Ba在gydF4y2Ba；显示(capListgydF4y2Ba“步伐”gydF4y2Ba我:capList。MaxNumSteps，gydF4y2Ba“FastUpdate”gydF4y2Ba1);持有gydF4y2Ba从gydF4y2Ba；gydF4y2Ba%更新驾驶场景。gydF4y2Ba推进(scenarioViewer);抽搐;gydF4y2Ba结束gydF4y2Ba结束gydF4y2Ba结束gydF4y2Ba