城市驾驶最优轨迹生成
这个例子展示了如何在城市场景中使用traometoryoptimalfromet.．
在这个例子中，您将：
探索使用预定义车辆的城市场景。
使用
traometoryoptimalfromet.做导航城市情景的地方规划。
内容
介绍
城市中的自动驾驶需要在全球和本地两个层面进行规划。的全球计划找到开始和目标点之间最可行的路径。的本地规划师根据全局计划和周围信息进行动态重新规划以生成最优轨迹。在本例中，ego车辆（绿色框）遵循全局计划（蓝色虚线）。当试图避开另一辆车（黑色矩形）时，完成局部规划（橙色实线）。
本地规划人员使用障碍物信息来生成最佳的无碰撞轨迹。来自各种板载传感器的障碍物信息，如相机，雷达和激光雷达，融合了入住率地图更新。这张入住地图Egocentric在这里，局部框架以自我为中心。当传感器检测到障碍物并将其放置在地图上时，该地图用于局部规划。
探索当地规划的城市场景
此示例场景具有四个其他车辆（蓝色矩形），其在不同速度的预定义路径中移动。下图说明了在此示例中使用的这种情况和全局计划（实体红线）。下图中的纯度红色点代表了开始和目标位置之间全球计划的航路点。绿色矩形代表自我车辆。
自我车辆使用traometoryoptimalfromet.从位置导航一个定位D在具有三个不同配置参数的三个段中。
第一次(一个到B)，车辆演示自适应巡航控制(ACC)行为。
第二 （B到C)时，车辆转弯跟随全球计划。
第三(C到D)时，车辆执行变道机动。
设置所需的数据和环境变量：
%从urbanScenarioData加载数据。Mat文件，初始化所需的变量[otherVehicles, globalPlanPoints stateValidator] = exampleHelperUrbanSetup;
尤差: [1 x 4]包含字段的结构数组:
位置，偏航，速度,模拟时间，每个车辆在场景中。globalPlanPoints：[18 x 2]矩阵包含由十八航点组成的预先计算的全局计划，每个计划组成，每个方案都表示在场景中的位置。状态验证器：Validatoroccupancimap.对象，该对象基于给定的二维握持地图作为状态验证器。根据障碍物信息和道路边界，更新一个完全以自我为中心的占用地图。还可以根据应用程序使用自定义状态验证器。有关更多信息，请参见导航。StateValidator．
画出场景。
exampleHelperPlotUrbanScenario;
创建本地规划师
指定状态验证器和全局计划以使用traometoryoptimalfromet.．
localplanner = trajectoryoptimalfromet（globalplanpoints，statevalidator）;
探索的性质Localplanner.
的Localplanner.具有各种属性，可以通过调优来实现所需的行为。本节探讨其中一些属性及其默认值。
localPlanner。TerminalStates
纵：[30 45 60 75 90]:定义纵向采样距离，单位为米。该值可以是标量或向量。侧:[-2 -1 0 1 2]：以米为单位定义参考路径的横向偏差（本例中为全局平面）。时间:7：到达轨道终点的时间以秒为单位。速度：10：以米每秒为单位的速度，在轨道的末端达到加速度:0：轨迹末端的加速度，单位是m/s^2。
localPlanner。FeasibilityParameters
MaxCurvature:0.1 :M ^ -1中曲率的最大可行值maxacceleration：2.5:M / S ^ 2中的最大可行性加速度。
localPlanner.TimeResolution：0.1:轨迹离散化间隔以秒为单位
使用traometoryoptimalfromet.演示自适应巡航控制(ACC)行为
在本节中，分配需要配置的属性Localplanner.演示自适应巡航控制(ACC)行为。
为了演示ACC, ego车辆需要保持一个安全距离跟随领头车辆。这一段的前导车是通过使用来获取的otherVehicles (1)．
%从位置A到位置B获取段中的leadVehicleleadVehicle = otherVehicles (1);%定义ACC安全距离ACCSafeDistance = 35;％在米%调整计划者对象的时间分辨率使自我车辆%的旅行顺利timeResolution = 0.01;localPlanner。TimeResolution = TimeResolution;
将自我飞行器设置在位置A，并定义其初始速度和方向(偏航)。
%设置位置A, B, C和Dposita = [5.1，-1.8,0];sitionb = [60，-1.8,0];positc = [74.45，-30.0,0];positiond = [74.45，-135,0];Logpoint = [145.70，-151.8,0];％设置自我车辆的初始状态egoinitpose = positya;egoInitvelocity = [10，-0.3,0];eGoinityaw = -0.165;CurrenteGostate = [eGainitpose（1），eGoInithess（2），Deg2rad（eGinaliant），......0，常态（egoInitvelocity），0];车辆力量= 4.7;％在米%重新规划模拟步骤数量的间隔％（默认50个模拟步骤）ReplanInterval = 50;
可视化模拟结果。
%初始化可视化exampleHelperInitializeVisualization;
ACC行为是通过设置TerminalStates的Localplanner.如下：
保持与前车的安全距离，设置localplanner.terminalstates.longitudinal.to Distance to Lead Vehicle -车辆长度;
保持相对于前车的相对速度，设置localPlanner.TerminalStates.Speed引导车辆速度;
要继续在全球计划中导航，请设置localPlanner.TerminalStates.Lateral到0;
在以下代码片段中，Localplanner.生成轨迹这是执行和可视化的使用exampleHelperUpdateVisualization对于每个模拟步骤。然而，重新规划是在每一个replanInterval模拟步骤中完成的。以下是重新规划过程中的事件顺序:
使用车辆信息更新占用地图
examplehelperupdateoccupancimap.．更新
localPlanner。TerminalStates．轨迹生成使用
计划(localPlanner currentStateInFrenet)．
%模拟自我车辆到达位置Bsimstep = 1;％只检查x，因为y没有变化。而CurrenteGostate（1）<=位置布（1）%在每个“replanInterval”模拟步骤重新规划如果rem（simStep，replantinerval）==0 | | simStep==1%计算重规划时间previousreplantime = simstep * timeresolution;用车辆信息更新占用地图examplehelperupdateoccupancimap（尤差，simstep，currentegostate）;计算到前导车辆的距离和前导车辆的速度distancetoleadvehicle = pdist2（locedhicle.position（simstep，1：2），......currentEgoState (1:2));leadVehicleVelocity = leadVehicle.Velocity (simStep:);％设置localplanner.TerminalStates for ACC行为如果distanceToLeadVehicle <= ACCSafeDistance localplanner . terminalstates .纵向= distanceToLeadVehicle -车辆长度;localPlanner.TerminalStates.Speed =规范(leadVehicleVelocity);localPlanner.TerminalStates.Lateral = 0;desiredTimeBound = localPlanner.TerminalStates.Longitudinal /......localPlanner.TerminalStates.Speed;localPlanner.TerminalStates.Time = desiredTimeBound;localPlanner.FeasibilityParameters.MaxCurvature = 0.5;结束%生成当前参数集的最优轨迹currentStateInFrenet = cart2frenet(localPlanner， [currentEgoState(1:5) 0]); / /当前状态轨迹= plan(localPlanner, currentStateInFrenet);％可视化自我为中心的占用地图显示(egoMap“父”，haxes1）;haxes1.title.string =.“自我为中心二元占用地图”；％可视化占用地图上的自助式车辆Egocenter = Currentegostate（1：2）;Egopolygon = emplationHelperTransformPointTopolygon（Rad2deg（Currentegostate（3）），Egocenter）;持有（Haxes1，“在”)填充(egpolygon (1，:)， egpolygon (2，:)，“g”，“父”hAxes1)％可视化轨迹参考路径和轨迹显示(localPlanner“轨迹”，“最佳”，“父”hAxes1)结束%执行和更新可视化[isGoalReached, currentEgoState] =......exampleHelperExecuteAndVisualize (currentEgoState simStep,......轨迹，普遍存在量）;如果(isGoalReached)打破；结束％更新下次迭代的模拟步骤simstep = simstep + 1;暂停（0.01）;结束
在执行的最后，自我载体在位置B。
接下来,配置traometoryoptimalfromet.在第二段中从位置B转到位置C。
使用traometoryoptimalfromet.到N使顺利转弯
属性的当前设置属性Localplanner.足以谈判顺利转弯。然而，在第二段中，引线车辆从其他侧面取出(2）．
%设置先导车辆与第二段中的车辆相对应从位置B到位置CleadVehicle = otherVehicles (2);%模拟自我车辆到达位置C只检查Y，因为X在C没有零钱而Currentegostate（2）> = PositionC（2）%在每个“replanInterval”模拟步骤重新规划如果REM（SimStep，ReplanInterval）== 0%计算重规划时间previousreplantime = simstep * timeresolution;用车辆信息更新占用地图exampleHelperUpdateOccupancyMap (otherVehicles simStep currentEgoState);计算到前导车辆的距离和前导车辆的速度distancetoleadvehicle = pdist2（locedhicle.position（simstep，1：2），......currentEgoState (1:2));leadVehicleVelocity = leadVehicle.Velocity (simStep:);如果(distanceToLeadVehicle <= ACCSafeDistance) localPlanner.TerminalStates.Longitudinal = distanceToLeadVehicle - vehicleLength;localPlanner.TerminalStates.Speed =规范(leadVehicleVelocity);localPlanner.TerminalStates.Lateral = 0;desiredTimeBound = localPlanner.TerminalStates.Longitudinal /......localPlanner.TerminalStates.Speed;localPlanner.TerminalStates.Time = desiredTimeBound;localPlanner.FeasibilityParameters.MaxCurvature = 0.5;localPlanner.FeasibilityParameters.MaxAcceleration = 5;结束%生成当前参数集的最优轨迹currentStateInFrenet = cart2frenet(localPlanner， [currentEgoState(1:5) 0]); / /当前状态轨迹= plan(localPlanner, currentStateInFrenet);％可视化自我为中心的占用地图显示(egoMap“父”，haxes1）;haxes1.title.string =.“自我为中心二元占用地图”；％可视化占用地图上的自助式车辆Egocenter = Currentegostate（1：2）;Egopolygon = emplationHelperTransformPointTopolygon（Rad2deg（Currentegostate（3）），Egocenter）;持有（Haxes1，“在”)填充(egpolygon (1，:)， egpolygon (2，:)，“g”，“父”hAxes1)％可视化轨迹参考路径和轨迹显示(localPlanner“轨迹”，“最佳”，“父”hAxes1)结束%执行和更新可视化[isGoalReached, currentEgoState] =......exampleHelperExecuteAndVisualize (currentEgoState simStep,......轨迹，普遍存在量）;如果(isGoalReached)打破；结束％更新下次迭代的模拟步骤simstep = simstep + 1;暂停（0.01）;结束
在执行的最后，自我载体在C位置。
接下来,配置traometoryoptimalfromet.用于执行从位置C到位置D的换道操纵。
使用traometoryoptimalfromet.执行变道机动
通过适当配置规划器的横向终端状态，可以执行变道机动。这可以通过设置横向终端状态为车道宽度(在本例中为3.6m)，并假设参考路径与当前自我车道的中心对齐来实现。
%模拟自我车辆到达位置D％设置车道宽度巷宽= 3.6;只检查Y，因为X在D没有变化而currentEgoState (2) > = positionD (2)%在每个“replanInterval”模拟步骤重新规划如果REM（SimStep，ReplanInterval）== 0%计算重规划时间previousreplantime = simstep * timeresolution;用车辆信息更新占用地图examplehelperupdateoccupancimap（尤差，simstep，currentegostate）;％对于谈判变换车道TerminalState设置localPlanner.TerminalStates.Longitudinal = 20:5:40;localPlanner.TerminalStates.Lateral =巷宽;localPlanner.TerminalStates.Speed = 10;desiredTimeBound = localPlanner.TerminalStates.Longitudinal /......((currentEgoState(1、5)+ localPlanner.TerminalStates.Speed) / 2);localPlanner.TerminalStates.Time = desiredTimeBound;localPlanner.FeasibilityParameters.MaxCurvature = 0.5;localPlanner.FeasibilityParameters.MaxAcceleration = 15;%生成当前参数集的最优轨迹CurrentStateinfromet = Cart2Fromet（LocalPlanner，[Currentegostate（1：5）0]）;Trajectory =计划（LocalPlanner，Currentstateinfromet）;％可视化自我为中心的占用地图显示(egoMap“父”，haxes1）;haxes1.title.string =.“自我为中心二元占用地图”；％可视化占用地图上的自助式车辆Egocenter = Currentegostate（1：2）;Egopolygon = emplationHelperTransformPointTopolygon（Rad2deg（Currentegostate（3）），Egocenter）;持有（Haxes1，“在”)填充(egpolygon (1，:)， egpolygon (2，:)，“g”，“父”hAxes1)％可视化轨迹参考路径和轨迹显示(localPlanner“轨迹”，“最佳”，“父”hAxes1)结束%执行和更新可视化[isGoalReached, currentEgoState] =......exampleHelperExecuteAndVisualize (currentEgoState simStep,......轨迹，普遍存在量）;如果(isGoalReached)打破；结束％更新下次迭代的模拟步骤simstep = simstep + 1;暂停（0.01）;结束
模拟自我车辆执行到达目标点
的Localplanner.现在配置为从位置D导航到目标位置。
%模拟自我车辆到达目标位置%只检查X，因为在目标没有改变Y。而currentEgoState (1) < = goalPoint (1)%在每个“replanInterval”模拟步骤重新规划如果REM（SimStep，ReplanInterval）== 0%计算重规划时间previousreplantime = simstep * timeresolution;用车辆信息更新占用地图exampleHelperUpdateOccupancyMap (otherVehicles simStep currentEgoState);localPlanner.TerminalStates.Longitudinal = 20;localPlanner.TerminalStates.Lateral = [-1 0 1];desiredTimeBound = localPlanner.TerminalStates.Longitudinal /......((currentEgoState(1、5)+ localPlanner.TerminalStates.Speed) / 2);localPlanner.TerminalStates.Time = desiredTimeBound: 0.2: desiredTimeBound + 1;%生成当前参数集的最优轨迹currentStateInFrenet = cart2frenet(localPlanner， [currentEgoState(1:5) 0]); / /当前状态轨迹= plan(localPlanner, currentStateInFrenet);％可视化自我为中心的占用地图显示(egoMap“父”，haxes1）;haxes1.title.string =.“自我为中心二元占用地图”；％可视化占用地图上的自助式车辆Egocenter = Currentegostate（1：2）;Egopolygon = emplationHelperTransformPointTopolygon（Rad2deg（Currentegostate（3）），Egocenter）;持有（Haxes1，“在”)填充(egpolygon (1，:)， egpolygon (2，:)，“g”，“父”hAxes1)％可视化轨迹参考路径和轨迹显示(localPlanner“轨迹”，“最佳”，“父”hAxes1)结束%执行和更新可视化[isGoalReached, currentEgoState] =......exampleHelperExecuteAndVisualize (currentEgoState simStep,......轨迹，普遍存在量）;%达到的目标将只在这一部分是正确的。如果(isGoalReached)打破；结束％更新下次迭代的模拟步骤simstep = simstep + 1;暂停（0.01）;结束
记录自我战车的位置开学基础工作区中可用的变量。您可以使用驱动器通用中的车辆路径exampleHelperplaybackinds（例如）．
%清除在示例运行期间创建的工作区变量。％这不包括Epopose，以允许用户在DS中播放模拟examplehelperperurbancleanup;
结论
这个例子解释了如何在城市场景中使用trajectoryOptimalFrenet。特别是，我们学会了如何使用traometoryoptimalfromet.实现以下行为。
自适应巡航控制系统
谈判结果
车道改变策略。
你也可以从以下列表中选择一个网站: