dynamicCapsuleList: only last step of the capsule list is checked for collisions
1 次查看(过去 30 天)
显示 更早的评论
Hi all,
I am confronted with a problem of the dynamicCapsuleList. I am using the capsule List to detect collisions of future vehicle motions, as done in https://de.mathworks.com/help/nav/ug/highway-trajectory-planning-using-frenet.html .
For some reason during the simulation only the last steps of the capsule List is enabled to be checked for collisions. This is confusing to me, since I simulated the capsule List along the computed frenét trajectories and all of the relevant capsules are displayed in the pop-up figure.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/582826/image.jpeg)
I have written the code as follows:
I start by defining the geometry of the Ego capsules
capList = dynamicCapsuleList;
carLen = 4.7; % in meters
carWidth = 1.8; % in meters
rearAxleRatio = .25;
egoID = egoVehicle.ActorID;
[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
updateEgoGeometry(capList,egoID,egoGeom);
The same is done for the Obstacles
numActors = size(scenario.Actors) -1; %
VT = numActors (1, 2); %
actorID = (2:VT+1)'; %
actorGeom = repelem(egoGeom,VT,1);
updateObstacleGeometry(capList,actorID,actorGeom)
This is followed by a while-loop to simulate the capsules iteratively.
The Capsule for the Obstacles are computed inside the while loop along corresponding frenet trajectories
for i = 1:numel(actorID)
ActorPose = scenario.Actors(1, actorID(i)).Position;
ActorYaw = deg2rad(scenario.Actors(1, actorID(i)).Yaw); %
ActorSpeed = sqrt(scenario.Actors(1, actorID(i)).Velocity(1)^2 + scenario.Actors(1, actorID(i)).Velocity(2)^2); %
ActorAcc = 0;
ActorKappa = scenario.Actors(1, actorID(i)).AngularVelocity(3)/ActorSpeed/180*pi;
% global2frenet
globalActorState = [ActorPose(1:2), ActorYaw, ActorKappa, ActorSpeed, ActorAcc];
frenetActorState = global2frenet(refPathActor1, globalActorState);
[~,ActorTrajGlobal] = connect(connector,frenetActorState,TermStates,2);
% for u = 1:numel(TermStates(:,1))
capList.MaxNumSteps = numel(ActorTrajGlobal(2).Trajectory(:,1));
% CapList States of surrounding vehicles
actorPoses(actorID(i)).States = [ActorTrajGlobal(2).Trajectory(:,1:3)];
updateObstaclePose(capList,actorID(:),actorPoses(actorID(:)));
end
and for the Ego-Vehicle with
for i = 1:numel(idx)
optimalTrajectory = trajGlobal(idx(i)).Trajectory;
egoPoses.States = optimalTrajectory(:,1:3);
updateEgoPose(capList,egoID,egoPoses);
...
then the collision would be checked with
...
isColliding = checkCollision(capList);
end
at every loop the capsules are visualized with
show(capList, 'TimeStep',[1:capList.MaxNumSteps],'FastUpdate',1, 'ShowCollision', true);
The collision checker does not recognize any capsules before the last state. In other words only the last state of every trajectory is checked for collisions. Other steps before that are ignored for some reason.
For more details:
The code is inside the zip file. The Simulation starts by running the "Main" script.
Would be thankful for any help, that I can get. Thanks!
0 个评论
回答(0 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Tracking and Sensor Fusion 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!