高速公路使用Frenet参考路径轨迹规划gydF4y2Ba

这个示例使用:gydF4y2Ba

这个例子演示了如何计划一个当地的轨道在高速公路驾驶的情况下。这个例子使用一个引用路径和障碍产生的动态列表选择自我车辆的轨迹。自我提供驾驶车辆导航通过交通定义在一个场景从一个gydF4y2BadrivingScenariogydF4y2Ba对象。车辆自适应巡航控制系统之间的交替,车道变化,基于成本和车辆演习后,可行性,无碰撞运动。gydF4y2Ba

负载驱动的场景gydF4y2Ba

首先加载gydF4y2BadrivingScenariogydF4y2Ba对象,它定义了车辆和道路属性在当前工作区中。这个场景是使用生成的gydF4y2Ba驾驶场景设计师gydF4y2Ba(自动驾驶工具箱)gydF4y2Ba应用程序和出口到一个MATLAB®函数,gydF4y2BadrivingScenarioTrafficExamplegydF4y2Ba。有关更多信息,请参见gydF4y2Ba通过编程方式创建驾驶场景变化gydF4y2Ba(自动驾驶工具箱)gydF4y2Ba。gydF4y2Ba

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

图包含一个坐标轴对象。坐标轴对象包含9块类型的对象。gydF4y2Ba

构造参考路径gydF4y2Ba

所有的地方规划在这个例子对参考路径,执行由一个表示gydF4y2BareferencePathFrenetgydF4y2Ba对象。这个对象可以返回的状态曲线上给定长度的路径,找到最接近的点以及一些全球之路gydF4y2BaxygydF4y2Ba位置,并促进全球和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

图包含一个坐标轴对象。坐标轴对象包含2线类型的对象。gydF4y2Ba

构建轨迹发生器gydF4y2Ba

地方规划,目标通常是尝试各种可能的运动走向最终目标,满足当前的运动学和动力学条件。的gydF4y2BatrajectoryGeneratorFrenetgydF4y2Ba对象完成通过连接终端状态的初始状态与一组使用4 -或5阶多项式轨迹。初始状态和终端状态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。长度= carLen;gydF4y2Ba%在米gydF4y2BaegoGeom.Geometry。半径=c一个rWidth/2;%在米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属性:MaxNumSteps: 31 EgoIDs: 1 ObstacleIDs: x1双[5]NumObstacles: 5 NumEgos: 1gydF4y2Ba

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

规划工具支持当地规划策略,一组样本轨迹基于当前金宝app和预测状态的环境在选择最优的轨迹。仿真回路分成以下部分:gydF4y2Ba

单击每个部分的标题导航到相关代码的仿真循环。gydF4y2Ba

推进地面实况场景gydF4y2Ba

当计划在动态环境中,常常需要估计的状态环境或预测在不久的将来,它的状态。为简单起见,这个示例使用了gydF4y2BadrivingScenariogydF4y2Ba作为地面真理的来源为每个演员在规划周期轨迹。测试一个定制的预测算法,可以替换或修改gydF4y2BaexampleHelperRetrieveActorGroundTruthgydF4y2Ba函数和自定义代码。gydF4y2Ba

生成终端状态gydF4y2Ba

在自动驾驶一个共同的目标是确保计划轨迹不仅是可行的而且自然。典型高速公路驾驶包括元素的车道保持、维护的速度极限,换车道,适应交通速度,等等。每个定制行为可能需要不同的环境信息。这个案例展示了如何生成终端状态,实现三个这样的行为:巡航控制、车道变化,跟随车辆。gydF4y2Ba

巡航控制系统gydF4y2Ba

的gydF4y2BaexampleHelperBasicCruiseControlgydF4y2Ba函数生成终端状态,进行巡航控制行为。这个函数使用自我车辆的横向速度和时间范围预测自我车辆的预期将来巷n秒。计算lane-center并成为终端状态的横向偏差和横向速度和加速度都设置为零。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 andconnector.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

图包含一个uipanel类型的对象。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:元素个数(actorPoses) actorPoses(我)。年代t一个te年代=futureTrajectory(i).Trajectory(:,1:3);结束gydF4y2BaupdateObstaclePose (capList actorID actorPoses);gydF4y2Ba%确定评价的顺序。gydF4y2Ba(成本、idx) = (costTS)进行排序;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

图包含一个uipanel类型的对象。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)罪(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

定制的解决方案金宝搏官方网站通常涉及许多可调参数,每一个能够改变最终的行为的方式是很难预测的。本节着重介绍一些feature-specific属性及其影响上述计划。然后,建议提供方法调整或增加自定义的逻辑。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。半径=车道Width/2;%在米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

五秒钟的时间越久会导致更加顺畅的驾驶体验。规划师仍然重视长轨迹由于负面的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 maxCurvature minVelocity);gydF4y2Ba%更新预测轨迹的碰撞检查gydF4y2Ba%的演员在现场。gydF4y2Ba为gydF4y2Bai = 1:元素个数(actorPoses) actorPoses(我)。年代t一个te年代=futureTrajectory(i).Trajectory(:,1:3);结束gydF4y2BaupdateObstaclePose (capList actorID actorPoses);gydF4y2Ba%确定评价的顺序。gydF4y2Ba(成本、idx) = (costTS)进行排序;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如果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)罪(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

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