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

5 次查看（过去 30 天）
William Miles
William Miles 2019-3-15
回答： Altaïr 2025-6-3
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

请先登录，再回答此问题。

回答（1 个）

Altaïr
Altaïr 2025-6-3
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:
web(fullfile(docroot, 'driving/ref/drivingscenario.advance.html'))
  • restart
web(fullfile(docroot, 'driving/ref/drivingscenario.restart.html'))

类别

在 Help Center 和 File Exchange 中查找有关 Automated Driving Toolbox 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by