How to Updated trajectory​(ac,waypoi​nts,speed) during simulation in Automated Driving System Toolbox

Im trying to implement and test a state lattice planner in matlab and want to use the Automated Driving System Toolbox to run simulations on the road. The toolbox has a nice road genereater and i can extract good info about the ego vehicle and the road lines, but i want to be able to run my planner while the simulation is running, and what i know of using the trajectory function could do he job. But i run into trouble when i try to update the trajectory function in the simulation loop.

Right now im just trying to update the trajectory right before i reach my final point in the first trajectory.

close all, clc
%    Loading the paths
load('path.mat')
p = load('path2.mat')
s = createDrivingScenario();
s.SampleTime = 0.02;

%    Predefined waypoints
way = [path.x;path.y;zeros(1,101)]';

egocar = vehicle(s,'Position', [path.x(1) path.y(1) 0],'Yaw',0);

%    Enter the first waypoints
trajectory(egocar,way,0.5);

plot(s)

bep = birdsEyePlot('XLim',[-40 40],'YLim',[-30 30]);
olPlotter = outlinePlotter(bep);
lblPlotter = laneBoundaryPlotter(bep,'Color','r','LineStyle','-');
lbrPlotter = laneBoundaryPlotter(bep,'Color','g','LineStyle','-');
rbsEdgePlotter = laneBoundaryPlotter(bep);
legend('off');




flag = true;
while advance(s)

    rbs = roadBoundaries(egocar);
    [position, yaw, length, width, originOffset, color] = targetOutlines(egocar);
    lb = laneBoundaries(egocar,'XDistance',0:10:10,'LocationType','Center', ...
        'AllBoundaries',false);
   %    Just before the simulation time is up
    if s.SimulationTime>38.3600 && flag
        %    Update the trajectory with w, the second path is countinouse with the first path
        w = [p.path.x;p.path.y;zeros(1,101)]';
        trajectory(egocar,w,0.5);
        disp('Go')
        flag = false;
    end
    plotLaneBoundary(rbsEdgePlotter,rbs)
    plotLaneBoundary(lblPlotter,{lb(1).Coordinates})
    plotLaneBoundary(lbrPlotter,{lb(2).Coordinates})
    plotOutline(olPlotter, position, yaw, length, width, ...
        'OriginOffset', originOffset, 'Color', color)

    s.SimulationTime;
end

The problem is that the simulation does a big jump when i enter the second trajectry and i want the transision to be smooth. Any ideas on how to update the trajectory of the vehicle while in simulation loop?

The paths files are attached.

Thanks

[Luan]

You got it? I have the same problem to update de trajectory

[Victor]

same problem, have you found any solution?

Answer 1

[Altaïr]

Hey @William Miles,

The code snippet shows the trajectory changes when the simulation time exceeds 38.6 seconds. However, the new trajectory w appears to contain only future waypoints rather than the egocar's complete trajectory.

if s.SimulationTime>38.3600 && flag
    %    Update the trajectory with w, the second path is countinouse with the first path
    w = [p.path.x;p.path.y;zeros(1,101)]';

When advance is called, the first waypoint in w is treated as the initial position (simulation time = 0), though it actually corresponds to ~38.4 seconds. This timing mismatch causes unexpected behavior. Here are two solutions:

1. Include full trajectory history: Redefine w to combine existing and new waypoints.

w = [way' [p.path.x;p.path.y;zeros(1,101)]]';

2. Restart the scenario: Reset the simulation after updating the trajectory.

trajectory(egocar,w,0.5);
restart(s);

However, this may not be desirable when there are other moving actors in the scenario.

For better implementations (in R2021a and later), consider smoothTrajectory to generate jerk-limited trajectories:

Helpful links:

• smoothTrajectory - https://www.mathworks.com/help/driving/ref/drivingscenario.smoothtrajectory.html
• advance

    web(fullfile(docroot, 'driving/ref/drivingscenario.advance.html'))

• restart

    web(fullfile(docroot, 'driving/ref/drivingscenario.restart.html'))