matlab 高速公路,基于Frenet参考路径的高速公路轨迹规划

初始化模拟器

初始化模拟器并创建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.