此示例演示了如何在高速公路驾驶场景中规划局部轨迹。 本示例使用参考路径和障碍物动态列表来生成自我车辆的替代轨迹。 自我车辆从DrivingScenario对象浏览提供的驾驶场景中定义的交通。 车辆会根据成本，可行性和无碰撞运动在自适应巡航控制，车道变更和车辆跟随操纵之间进行切换。
使用工具：matlab2020b
负载驾驶方案
首先加载提供的DrivingScenario对象，该对象定义当前工作空间中的车辆和道路属性。 该场景是使用Driving Scenario Designer应用程序生成的，并已导出到MATLAB®函数DrivingScenarioTrafficExample。 有关更多信息，请参见以编程方式创建行驶方案变体。
scenario = drivingScenarioTrafficExample;
% Default car properties
carLen = 4.7; % in meters
carWidth = 1.8; % in meters
rearAxleRatio = .25;
% Define road dimensions
laneWidth = carWidth*2; % in meters
plot(scenario);
构造参考路径
此示例中的所有本地计划都是相对于由referencePathFrenet（导航工具箱）对象表示的参考路径执行的。 该对象可以沿路径以给定的长度返回曲线的状态，找到沿路径到某个全局xy位置的最近点，并有助于在全局参考系和Frenet参考系之间进行坐标转换。
对于本例，参考路径被视为一条四车道高速公路的中心，路径点与所提供的drivingScenario对象中定义的道路相匹配。
waypoints = [0 50; 150 50; 300 75; 310 75; 400 0; 300 -50; 290 -50; 0 -50]; % in meters
refPath = referencePathFrenet(waypoints);
ax = show(refPath);
axis(ax,'equal');
构造轨迹生成器
对于一个局部规划者来说，其目标通常是在满足当前运动学和动力学条件的情况下，对各种可能的运动进行抽样，以达到最终目标。trajectoryGeneratorFrenet(导航工具箱)对象通过使用4或5阶多项式轨迹将初始状态与一组终端状态连接起来来实现这一点。在Frenet坐标系中定义了初始状态和终止状态，每个多项式解满足横向和纵向位置、速度和加速度边界条件，同时最大限度地减少冲击。
终端状态通常使用自定义行为来计算。 这些行为会利用在本地环境中找到的信息，例如车道信息，速度限制以及自我车辆附近参与者的当前或将来的预测。
使用参考路径构造trajectoryGeneratorFrenet对象。
connector = trajectoryGeneratorFrenet(refPath);
构造动态碰撞检查器
dynamicCapsuleList（导航工具箱）对象是一种数据结构，表示离散时间集上动态环境的状态。 然后，该环境可用于有效验证自我车辆的多个潜在轨迹。 场景中的每个对象都由以下内容表示：
1.唯一的整数值标识符
2.用于有效碰撞检查的胶囊几何体的属性
3.SE2状态序列，其中每个状态代表时间上的离散快照。
在此示例中，trajectoryGeneratorFrenet对象生成的轨迹发生在一定的时间范围内，称为时间范围。 为确保碰撞检查涵盖所有可能的轨迹，dynamicCapsuleList对象应包含跨越最大预期时间范围的所有参与者的预测轨迹。
capList = dynamicCapsuleList;
使用给定的参数为运输工具创建几何结构。
egoID = 1;
[egoID, egoGeom] = egoGeometry(capList,egoID);
egoGeom.Geometry.Length = carLen; % in meters
egoGeom.Geometry.Radius = carWidth/2; % in meters
egoGeom.Geometry.FixedTransform(1,end) = -carLen*rearAxleRatio; % in meters将ego car添加到动态胶囊列表中。
updateEgoGeometry(capList,egoID,egoGeom);
将DrivingScenario actor添加到dynamicCapsuleList对象。 在此处设置几何形状，并在计划循环中定义状态。 您可以看到dynamicCapsuleList现在包含一辆自我车辆和五个障碍物。
actorID = (1:5)';
actorGeom = repelem(egoGeom,5,1);
updateObstacleGeometry(capList,actorID,actorGeom)
通过流量规划自适应路径
规划工具支持一种局部规划策略，即在选择最优轨迹之前，基于当前和可预见的环境状态对一组局部轨迹进行抽样。
在动态环境中进行规划时，通常需要估计环境状态或在不久的将来预测其状态。 为简单起见，本示例将DrivingScenario用作计划参与者在计划范围内的轨迹的地面真实来源。 要测试自定义预测算法，可以使用自定义代码替换或修改exampleHelperRetrieveActorGroundTruth函数。
自动驾驶的一个共同目标是确保计划的轨迹不仅可行，而且自然。 典型的高速公路驾驶涉及保持车道，维持限速，改变车道，使速度适应交通等要素。 每个自定义行为可能需要不同的环境信息。 本示例演示了如何生成实现以下三种行为的终端状态：巡航控制，车道变更和跟随领先车辆。
巡航控制
exampleHelperBasicCruiseControl函数将生成执行巡航控制行为的终端状态。 此功能使用自我车辆的横向速度和时间范围来预测未来自我车辆的预期车道N秒。 计算车道中心并使其成为终端状态的横向偏差，并将横向速度和加速度设置为零。
对于纵向边界条件，将终端速度设置为道路速度极限，并将终端加速度设置为零。 纵向位置不受限制，指定为NaN。 通过删除经度约束，trajectoryGeneratorFrenet可以使用较低的四阶多项式来解决纵向边界值问题，从而在给定的时间范围内使轨迹平滑地匹配车速：
车道变换
exampleHelperBasicLaneChange函数会生成终端状态，这些状态会将车辆从当前车道过渡到任一相邻车道。 该功能首先确定自我车辆的当前车道，然后检查左右车道是否存在。 对于每个现有车道，以与巡航控制行为相同的方式定义终端状态，不同之处在于终端速度设置为当前速度而不是道路的速度限制：
跟随车辆
exampleHelperBasicLeadVehicleFollow函数会生成终端状态，这些状态会尝试追随在自我车辆之前发现的车辆。 该功能首先确定自我车辆的当前车道。 对于每个提供的timeHorizon，该函数将预测每个角色的未来状态，找到占用与自主车辆相同车道的所有角色，并确定哪个是最接近的主导车辆（如果未找到主导车辆，该函数将不返回任何内容 ）。
通过计算领先车辆的位置和速度，将终端的纵向位置缩短一定的安全距离来计算自我车的终端状态。
生成轨迹并检查运动学的可行性
除了以最小的成本获得最终状态外，最优轨迹通常还必须满足与运动学可行性和乘坐舒适性相关的其他约束条件。 轨迹约束是增强所需乘车质量的一种方法，但这样做却以降低驾驶空间为代价。
在此示例中，exampleHelperEvaluateTrajectory函数将验证每个轨迹是否满足以下约束：
1. MaxAcceleration：整个轨迹上的绝对加速度必须低于指定值。 较小的值会降低驾驶积极性并消除运动学上不可行的轨迹。 该限制可以消除否则可能由车辆执行的操纵。
2. MaxCurvature：在整个轨迹中允许的最小转弯半径。 与MaxAcceleration一样，减小此值可带来更流畅的驾驶体验，但可能会消除其他可行的轨迹。
3. MinVelocity（最小速度）：此示例通过设置最小速度将车辆限制为仅向前运动。 在高速公路驾驶场景中需要此限制，并消除适合过度约束或条件差的边界值的轨迹。
检查轨迹是否碰撞并选择最佳轨迹
规划过程的最后一步是选择最佳轨迹，这也将产生无碰撞的路径。 冲突检查通常会推迟到最后，因为这是一项费力的操作，因此通过评估成本并首先分析约束条件，可以将无效轨迹排除在考虑范围之外。 然后可以以最佳顺序检查其余轨迹的碰撞，直到找到无碰撞的路径或评估所有轨迹为止。
定义模拟器和计划参数
本节定义了运行模拟器所需的属性以及计划程序和行为实用程序使用的参数。 诸如scenario.SampleTime和connector.TimeResolution之类的属性将同步，以便地面真人演员轨迹和计划中的自我轨迹中的状态在同一时间步出现。 同样，选择replanRate，timeHorizons和maxHorizon，使它们成为仿真速率的整数倍。
如上一节所述，选择权重和约束条件以在遵循道路规则的同时促进平滑的驾驶轨迹。
最后，定义speedLimit和safetyGap参数，这些参数用于为计划者生成终端状态。
% Synchronize the simulator's update rate to match the trajectory generator's
% discretization interval.
scenario.SampleTime = connector.TimeResolution; % in seconds
% Define planning parameters.
replanRate = 10; % Hz
% Define the time intervals between current and planned states.
timeHorizons = 1:3; % in seconds
maxHorizon = max(timeHorizons); % in seconds
% Define cost parameters.
latDevWeight = 1;
timeWeight = -1;
speedWeight = 1;
% Reject trajectories that violate the following constraints.
maxAcceleration = 15; % in meters/second^2
maxCurvature = 1; % 1/meters, or radians/meter
minVelocity = 0; % in meters/second
% Desired velocity setpoint, used by the cruise control behavior and when
% evaluating the cost of trajectories.
speedLimit = 11; % in meters/second
% Minimum distance the planner should target for following behavior.
safetyGap = 10; % in meters
初始化模拟器
初始化模拟器并创建chasePlot查看器。
[scenarioViewer,futureTrajectory,actorID,actorPoses,egoID,egoPoses,stepPerUpdate,egoState,isRunning,lineHandles] = ...
exampleHelperInitializeSimulator(scenario,capList,refPath,laneWidth,replanRate,carLen); 
tic
while isRunning
% Retrieve the current state of actor vehicles and their trajectory over
% the planning horizon.
[curActorState,futureTrajectory,isRunning] = ...
exampleHelperRetrieveActorGroundTruth(scenario,futureTrajectory,replanRate,maxHorizon);
% Generate cruise control states.
[termStatesCC,timesCC] = exampleHelperBasicCruiseControl(...
refPath,laneWidth,egoState,speedLimit,timeHorizons);
% Generate lane change states.
[termStatesLC,timesLC] = exampleHelperBasicLaneChange(...
refPath,laneWidth,egoState,timeHorizons);
% Generate vehicle following states.
[termStatesF,timesF] = exampleHelperBasicLeadVehicleFollow(...
refPath,laneWidth,safetyGap,egoState,curActorState,timeHorizons);
% Combine the terminal states and times.
allTS = [termStatesCC; termStatesLC; termStatesF];
allDT = [timesCC; timesLC; timesF];
numTS = [numel(timesCC); numel(timesLC); numel(timesF)];
% Evaluate cost of all terminal states.
costTS = exampleHelperEvaluateTSCost(allTS,allDT,laneWidth,speedLimit,...
speedWeight, latDevWeight, timeWeight);
% Generate trajectories.
egoFrenetState = global2frenet(refPath,egoState);
[frenetTraj,globalTraj] = connect(connector,egoFrenetState,allTS,allDT);
% Eliminate trajectories that violate constraints.
isValid = exampleHelperEvaluateTrajectory(globalTraj,maxAcceleration,maxCurvature,minVelocity);
% Display the sampled trajectories.
lineHandles = exampleHelperVisualizeSlineHa
hold on;
show(capList,'TimeStep',1:capList.MaxNumSteps,'FastUpdate',1);
hold off;
if isempty(optimalTrajectory)
% All trajectories either violated a constraint or resulted in collision.
%
% If planning failed immediately, revisit the simulator, planner,
% and behavior properties.
%
% If the planner failed midway through a simulation, additional
% behaviors can be introduced to handle more complicated planning conditions.
error('No valid trajectory has been found.');
else
% Visualize the scene between replanning.
for i = (2+(0:(stepPerUpdate-1)))
% Approximate realtime visualization.
dt = toc;
if scenario.SampleTime-dt > 0
pause(scenario.SampleTime-dt);
end
egoState = optimalTrajectory(i,:);
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).Yaw = egoState(3)*180/pi;
scenarioViewer.Actors(1).AngularVelocity(3) = egoState(4)*egoState(5);
% Update capsule visualization.
hold on;
show(capList,'TimeStep',i:capList.MaxNumSteps,'FastUpdate',1);
hold off;
% Update driving scenario.
advance(scenarioViewer);
ax = gca;
ax.ZLim = [-100 100];
tic;
end
end
end
