1 package org.opentrafficsim.road.gtu.lane.tactical.toledo;
2
3 import java.io.Serializable;
4 import java.util.Iterator;
5 import java.util.List;
6 import java.util.Random;
7
8 import org.djunits.unit.AccelerationUnit;
9 import org.djunits.unit.LengthUnit;
10 import org.djunits.unit.LinearDensityUnit;
11 import org.djunits.value.vdouble.scalar.Acceleration;
12 import org.djunits.value.vdouble.scalar.Length;
13 import org.djunits.value.vdouble.scalar.LinearDensity;
14 import org.djunits.value.vdouble.scalar.Speed;
15 import org.djunits.value.vdouble.scalar.Time;
16 import org.opentrafficsim.core.geometry.OTSGeometryException;
17 import org.opentrafficsim.core.gtu.GTU;
18 import org.opentrafficsim.core.gtu.GTUException;
19 import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
20 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
21 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypes;
22 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
23 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
24 import org.opentrafficsim.core.network.LateralDirectionality;
25 import org.opentrafficsim.core.network.NetworkException;
26 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
27 import org.opentrafficsim.road.gtu.lane.perception.CategorialLanePerception;
28 import org.opentrafficsim.road.gtu.lane.perception.InfrastructureLaneChangeInfo;
29 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
30 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
31 import org.opentrafficsim.road.gtu.lane.perception.categories.DirectNeighborsPerception;
32 import org.opentrafficsim.road.gtu.lane.perception.categories.HeadwayGtuType;
33 import org.opentrafficsim.road.gtu.lane.perception.categories.NeighborsPerception;
34 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
35 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneOperationalPlanBuilder;
36 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneOperationalPlanBuilder.LaneChange;
37 import org.opentrafficsim.road.gtu.lane.tactical.AbstractLaneBasedTacticalPlanner;
38 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
39 import org.opentrafficsim.road.gtu.lane.tactical.util.CarFollowingUtil;
40 import org.opentrafficsim.road.network.lane.Lane;
41 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
42 import org.opentrafficsim.road.network.speed.SpeedLimitProspect;
43
44 import nl.tudelft.simulation.language.d3.DirectedPoint;
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60 public class Toledo extends AbstractLaneBasedTacticalPlanner
61 {
62
63
64 private static final long serialVersionUID = 20160711L;
65
66
67 static final Length MAX_LIGHT_VEHICLE_LENGTH = new Length(9.14, LengthUnit.METER);
68
69
70 static final Length TAILGATE_LENGTH = new Length(10, LengthUnit.METER);
71
72
73 static final LinearDensity LOS_DENSITY = new LinearDensity(16, LinearDensityUnit.PER_KILOMETER);
74
75
76 public static final Random RANDOM = new Random();
77
78
79 private final LaneChange laneChange = new LaneChange();
80
81
82
83
84
85
86 public Toledo(final CarFollowingModel carFollowingModel, final LaneBasedGTU gtu)
87 {
88 super(carFollowingModel, gtu, new CategorialLanePerception(gtu));
89 getPerception().addPerceptionCategory(new ToledoPerception(getPerception()));
90 getPerception().addPerceptionCategory(new DirectNeighborsPerception(getPerception(), HeadwayGtuType.WRAP));
91 }
92
93
94 @Override
95 public final OperationalPlan generateOperationalPlan(final Time startTime, final DirectedPoint locationAtStartTime)
96 throws OperationalPlanException, GTUException, NetworkException, ParameterException
97 {
98
99
100 LanePerception perception = getPerception();
101 perception.perceive();
102 SpeedLimitProspect slp =
103 perception.getPerceptionCategory(ToledoPerception.class).getSpeedLimitProspect(RelativeLane.CURRENT);
104 SpeedLimitInfo sli = slp.getSpeedLimitInfo(Length.ZERO);
105 BehavioralCharacteristics bc = getGtu().getBehavioralCharacteristics();
106
107 Acceleration acceleration = null;
108
109
110 LateralDirectionality initiatedLaneChange;
111 if (!this.laneChange.isChangingLane())
112 {
113
114
115
116
117 GapInfo gapFwdL = getGapInfo(bc, perception, Gap.FORWARD, RelativeLane.LEFT);
118 GapInfo gapAdjL = getGapInfo(bc, perception, Gap.ADJACENT, RelativeLane.LEFT);
119 GapInfo gapBckL = getGapInfo(bc, perception, Gap.BACKWARD, RelativeLane.LEFT);
120 GapInfo gapFwdR = getGapInfo(bc, perception, Gap.FORWARD, RelativeLane.RIGHT);
121 GapInfo gapAdjR = getGapInfo(bc, perception, Gap.ADJACENT, RelativeLane.RIGHT);
122 GapInfo gapBckR = getGapInfo(bc, perception, Gap.BACKWARD, RelativeLane.RIGHT);
123 double emuTgL =
124 Math.log(Math.exp(gapFwdL.getUtility()) + Math.exp(gapAdjL.getUtility()) + Math.exp(gapBckL.getUtility()));
125 double emuTgR =
126 Math.log(Math.exp(gapFwdR.getUtility()) + Math.exp(gapAdjR.getUtility()) + Math.exp(gapBckR.getUtility()));
127
128
129
130 double eLead = RANDOM.nextGaussian() * Math.pow(bc.getParameter(ToledoLaneChangeParameters.SIGMA_LEAD), 2);
131 double eLag = RANDOM.nextGaussian() * Math.pow(bc.getParameter(ToledoLaneChangeParameters.SIGMA_LAG), 2);
132 GapAcceptanceInfo gapAcceptL =
133 getGapAcceptanceInfo(getGtu(), bc, perception, emuTgL, eLead, eLag, RelativeLane.LEFT);
134 GapAcceptanceInfo gapAcceptR =
135 getGapAcceptanceInfo(getGtu(), bc, perception, emuTgR, eLead, eLag, RelativeLane.RIGHT);
136
137
138 double vL = laneUtility(getGtu(), bc, perception, gapAcceptL.getEmu(), sli, RelativeLane.LEFT);
139 double vC = laneUtility(getGtu(), bc, perception, 0, sli, RelativeLane.CURRENT);
140 double vR = laneUtility(getGtu(), bc, perception, gapAcceptR.getEmu(), sli, RelativeLane.RIGHT);
141
142
143 if (vL > vR && vL > vC && gapAcceptL.isAcceptable())
144 {
145 initiatedLaneChange = LateralDirectionality.LEFT;
146 }
147 else if (vR > vL && vR > vC && gapAcceptR.isAcceptable())
148 {
149 initiatedLaneChange = LateralDirectionality.RIGHT;
150 }
151 else
152 {
153 initiatedLaneChange = LateralDirectionality.NONE;
154 }
155
156
157 if (initiatedLaneChange.isNone())
158 {
159 if ((vC > vR && vC > vL) || (!perception.getPerceptionCategory(NeighborsPerception.class)
160 .getLeaders(RelativeLane.CURRENT).isEmpty()
161 && perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT).first()
162 .getDistance().lt(getCarFollowingModel().desiredHeadway(bc, getGtu().getSpeed()))))
163 {
164 acceleration = CarFollowingUtil.followLeaders(getCarFollowingModel(), bc, getGtu().getSpeed(), sli,
165 perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT));
166 }
167 else
168 {
169 GapInfo gapAdj;
170 GapInfo gapFwd;
171 GapInfo gapBck;
172 if (vL > vR && vL > vC)
173 {
174 gapAdj = gapAdjL;
175 gapFwd = gapFwdL;
176 gapBck = gapBckL;
177 }
178 else
179 {
180 gapAdj = gapAdjR;
181 gapFwd = gapFwdR;
182 gapBck = gapBckR;
183 }
184 if (gapAdj.getUtility() > gapFwd.getUtility() && gapAdj.getUtility() > gapBck.getUtility())
185 {
186
187 double eadj = Toledo.RANDOM.nextGaussian() * bc.getParameter(ToledoLaneChangeParameters.SIGMA_ADJ)
188 * bc.getParameter(ToledoLaneChangeParameters.SIGMA_ADJ);
189 acceleration = new Acceleration(bc.getParameter(ToledoLaneChangeParameters.C_ADJ_ACC)
190 * (bc.getParameter(ToledoLaneChangeParameters.BETA_DP) * gapAdj.getLength().si
191 - gapAdj.getDistance().si)
192 + eadj, AccelerationUnit.SI);
193 }
194 else if (gapFwd.getUtility() > gapAdj.getUtility() && gapFwd.getUtility() > gapBck.getUtility())
195 {
196
197 Length desiredPosition = new Length(
198 gapFwd.getDistance().si
199 + bc.getParameter(ToledoLaneChangeParameters.BETA_DP) * gapFwd.getLength().si,
200 LengthUnit.SI);
201 double deltaV = gapFwd.getSpeed().si > getGtu().getSpeed().si
202 ? bc.getParameter(ToledoLaneChangeParameters.LAMBDA_FWD_POS)
203 * (gapFwd.getSpeed().si - getGtu().getSpeed().si)
204 : bc.getParameter(ToledoLaneChangeParameters.LAMBDA_FWD_NEG)
205 * (getGtu().getSpeed().si - gapFwd.getSpeed().si);
206 double efwd = Toledo.RANDOM.nextGaussian() * bc.getParameter(ToledoLaneChangeParameters.SIGMA_FWD)
207 * bc.getParameter(ToledoLaneChangeParameters.SIGMA_FWD);
208 acceleration = new Acceleration(bc.getParameter(ToledoLaneChangeParameters.C_FWD_ACC)
209 * Math.pow(desiredPosition.si, bc.getParameter(ToledoLaneChangeParameters.BETA_FWD))
210 * Math.exp(deltaV) + efwd, AccelerationUnit.SI);
211 }
212 else
213 {
214
215 Length desiredPosition = new Length(
216 gapBck.getDistance().si
217 + (1 - bc.getParameter(ToledoLaneChangeParameters.BETA_DP)) * gapBck.getLength().si,
218 LengthUnit.SI);
219 double deltaV = gapBck.getSpeed().si > getGtu().getSpeed().si
220 ? bc.getParameter(ToledoLaneChangeParameters.LAMBDA_BCK_POS)
221 * (gapBck.getSpeed().si - getGtu().getSpeed().si)
222 : bc.getParameter(ToledoLaneChangeParameters.LAMBDA_BCK_NEG)
223 * (getGtu().getSpeed().si - gapBck.getSpeed().si);
224 double ebck = Toledo.RANDOM.nextGaussian() * bc.getParameter(ToledoLaneChangeParameters.SIGMA_BCK)
225 * bc.getParameter(ToledoLaneChangeParameters.SIGMA_BCK);
226 acceleration = new Acceleration(bc.getParameter(ToledoLaneChangeParameters.C_BCK_ACC)
227 * Math.pow(desiredPosition.si, bc.getParameter(ToledoLaneChangeParameters.BETA_BCK))
228 * Math.exp(deltaV) + ebck, AccelerationUnit.SI);
229 }
230 }
231 }
232 }
233 else
234 {
235 initiatedLaneChange = LateralDirectionality.NONE;
236 }
237
238 if (initiatedLaneChange.isLeft())
239 {
240
241 acceleration = CarFollowingUtil.followLeaders(getCarFollowingModel(), bc, getGtu().getSpeed(), sli,
242 perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.LEFT));
243 }
244 else if (initiatedLaneChange.isRight())
245 {
246
247 acceleration = CarFollowingUtil.followLeaders(getCarFollowingModel(), bc, getGtu().getSpeed(), sli,
248 perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.RIGHT));
249 }
250
251 if (acceleration == null)
252 {
253 throw new Error("Acceleration from toledo model is null.");
254 }
255
256
257 Length forwardHeadway = bc.getParameter(ParameterTypes.LOOKAHEAD);
258 List<Lane> lanes = buildLanePathInfo(getGtu(), forwardHeadway).getLanes();
259 if (initiatedLaneChange.isNone())
260 {
261 Length firstLanePosition = getGtu().getReferencePosition().getPosition();
262 try
263 {
264 return LaneOperationalPlanBuilder.buildAccelerationPlan(getGtu(), lanes, firstLanePosition, startTime,
265 getGtu().getSpeed(), acceleration, bc.getParameter(ToledoLaneChangeParameters.DT));
266 }
267 catch (OTSGeometryException exception)
268 {
269 throw new OperationalPlanException(exception);
270 }
271 }
272
273 try
274 {
275 OperationalPlan plan = LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(getGtu(), lanes,
276 initiatedLaneChange, getGtu().getLocation(), startTime, getGtu().getSpeed(), acceleration,
277 bc.getParameter(ToledoLaneChangeParameters.DT), this.laneChange);
278 return plan;
279 }
280 catch (OTSGeometryException exception)
281 {
282 throw new OperationalPlanException(exception);
283 }
284 }
285
286
287
288
289
290
291
292
293
294
295
296 private GapInfo getGapInfo(final BehavioralCharacteristics bc, final LanePerception perception, final Gap gap,
297 final RelativeLane lane) throws ParameterException, OperationalPlanException
298 {
299
300
301 if (gap.equals(Gap.FORWARD) && (perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane) == null
302 || perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).isEmpty()))
303 {
304
305 return new GapInfo(Double.NEGATIVE_INFINITY, null, null, null);
306 }
307 if (gap.equals(Gap.BACKWARD) && (perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane) == null
308 || perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).isEmpty()))
309 {
310
311 return new GapInfo(Double.NEGATIVE_INFINITY, null, null, null);
312 }
313
314 double constant;
315 double alpha;
316 Length distanceToGap;
317 int deltaFrontVehicle;
318 Length putativeLength;
319 Length putativeDistance;
320 Speed putativeSpeed;
321 Length leaderDist;
322 Speed leaderSpeed;
323 Length followerDist;
324 Speed followerSpeed;
325
326 if (gap.equals(Gap.FORWARD))
327 {
328 constant = bc.getParameter(ToledoLaneChangeParameters.C_FWD_TG);
329 alpha = 0;
330 if (perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).size() > 1)
331 {
332
333 Iterator<HeadwayGTU> it =
334 perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).iterator();
335 it.next();
336 HeadwayGTU leader = it.next();
337 leaderDist = leader.getDistance();
338 leaderSpeed = leader.getSpeed();
339 putativeLength = leaderDist
340 .minus(perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first()
341 .getDistance())
342 .minus(perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first()
343 .getLength());
344 }
345 else
346 {
347
348
349 leaderDist = Length.POSITIVE_INFINITY;
350 leaderSpeed = Speed.POSITIVE_INFINITY;
351 putativeLength = leaderDist;
352 }
353
354 followerDist = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first().getDistance()
355 .plus(perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first().getLength());
356 followerSpeed = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first().getSpeed();
357 distanceToGap = followerDist;
358 putativeDistance = distanceToGap;
359 putativeSpeed = followerSpeed;
360 }
361 else if (gap.equals(Gap.ADJACENT))
362 {
363 constant = 0;
364 alpha = bc.getParameter(ToledoLaneChangeParameters.ALPHA_ADJ);
365 distanceToGap = Length.ZERO;
366 if (perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane) != null
367 && !perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).isEmpty())
368 {
369 leaderDist = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first().getDistance();
370 leaderSpeed = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first().getSpeed();
371 }
372 else
373 {
374 leaderDist = Length.POS_MAXVALUE;
375 leaderSpeed = Speed.POS_MAXVALUE;
376 }
377 if (perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane) != null
378 && !perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).isEmpty())
379 {
380
381 followerDist = perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first()
382 .getDistance().plus(getGtu().getLength());
383 followerSpeed =
384 perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first().getSpeed();
385 putativeDistance = followerDist;
386 }
387 else
388 {
389 followerDist = Length.NEG_MAXVALUE;
390 followerSpeed = Speed.NEG_MAXVALUE;
391 putativeDistance = Length.POS_MAXVALUE;
392 }
393 putativeSpeed = null;
394 putativeLength = leaderDist.plus(followerDist).plus(getGtu().getLength());
395 }
396 else
397 {
398 constant = bc.getParameter(ToledoLaneChangeParameters.C_BCK_TG);
399 alpha = bc.getParameter(ToledoLaneChangeParameters.ALPHA_BCK);
400 deltaFrontVehicle = 0;
401 if (perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane) != null
402 && perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).size() > 1)
403 {
404
405 Iterator<HeadwayGTU> it =
406 perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).iterator();
407 it.next();
408 HeadwayGTU follower = it.next();
409 followerDist = follower.getDistance();
410 followerSpeed = follower.getSpeed();
411 putativeLength = followerDist
412 .minus(perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first()
413 .getDistance())
414 .minus(perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first()
415 .getLength());
416 }
417 else
418 {
419
420 followerDist = Length.NEGATIVE_INFINITY;
421 followerSpeed = Speed.NEGATIVE_INFINITY;
422 putativeLength = followerDist;
423 }
424
425 leaderDist = perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first().getDistance()
426 .plus(perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first().getLength());
427 leaderSpeed = perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first().getSpeed();
428 distanceToGap = perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first().getDistance()
429 .plus(getGtu().getLength());
430 putativeDistance = distanceToGap
431 .plus(perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first().getLength());
432 putativeSpeed = leaderSpeed;
433 }
434
435
436 if (!gap.equals(Gap.BACKWARD)
437 && !perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT).isEmpty()
438 && perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT).first()
439 .getDistance().lt(leaderDist))
440 {
441 leaderDist = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT).first()
442 .getDistance();
443 leaderSpeed = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT).first()
444 .getSpeed();
445 deltaFrontVehicle = 1;
446 }
447 else
448 {
449 deltaFrontVehicle = 0;
450 }
451
452
453 Speed dV = followerSpeed.minus(leaderSpeed);
454 Length effectiveGap = leaderDist.minus(followerDist);
455
456
457 double util = constant
458 + bc.getParameter(ToledoLaneChangeParameters.BETA_DTG) * distanceToGap.si
459 + bc.getParameter(ToledoLaneChangeParameters.BETA_EG) * effectiveGap.si
460 + bc.getParameter(ToledoLaneChangeParameters.BETA_FV) * deltaFrontVehicle
461 + bc.getParameter(ToledoLaneChangeParameters.BETA_RGS) * dV.si
462 + alpha * bc.getParameter(ToledoLaneChangeParameters.ERROR_TERM);
463
464 return new GapInfo(util, putativeLength, putativeDistance, putativeSpeed);
465
466 }
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481 private GapAcceptanceInfo getGapAcceptanceInfo(final GTU gtu, final BehavioralCharacteristics bc,
482 final LanePerception perception, final double emuTg, final double eLead, final double eLag, final RelativeLane lane)
483 throws ParameterException, OperationalPlanException
484 {
485
486
487 double vLead;
488 double vLag;
489 boolean acceptLead;
490 boolean acceptLag;
491
492 if (perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane) != null
493 && !perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).isEmpty())
494 {
495 Speed dVLead = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first().getSpeed()
496 .minus(gtu.getSpeed());
497 Length sLead = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).first().getDistance();
498
499 Length gLead =
500 new Length(
501 Math.exp(
502 bc.getParameter(ToledoLaneChangeParameters.C_LEAD)
503 + bc.getParameter(ToledoLaneChangeParameters.BETA_POS_LEAD)
504 * Speed.max(dVLead, Speed.ZERO).si
505 + bc.getParameter(ToledoLaneChangeParameters.BETA_NEG_LEAD)
506 * Speed.min(dVLead, Speed.ZERO).si
507 + bc.getParameter(ToledoLaneChangeParameters.BETA_EMU_LEAD) * emuTg
508 + bc.getParameter(ToledoLaneChangeParameters.ALPHA_TL_LEAD)
509 * bc.getParameter(ToledoLaneChangeParameters.ERROR_TERM)
510 + eLead),
511 LengthUnit.SI);
512 vLead = Math.log(gLead.si) - (bc.getParameter(ToledoLaneChangeParameters.C_LEAD)
513 + bc.getParameter(ToledoLaneChangeParameters.BETA_POS_LEAD) * Speed.max(dVLead, Speed.ZERO).si
514 + bc.getParameter(ToledoLaneChangeParameters.BETA_NEG_LEAD) * Speed.min(dVLead, Speed.ZERO).si
515 + bc.getParameter(ToledoLaneChangeParameters.BETA_EMU_LEAD) * emuTg);
516 acceptLead = gLead.lt(sLead);
517 }
518 else
519 {
520 vLead = 0;
521 acceptLead = false;
522 }
523
524 if (perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane) != null
525 && !perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).isEmpty())
526 {
527 Speed dVLag = perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first().getSpeed()
528 .minus(gtu.getSpeed());
529 Length sLag = perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).first().getDistance();
530
531 Length gLag =
532 new Length(
533 Math.exp(bc.getParameter(ToledoLaneChangeParameters.C_LAG)
534 + bc.getParameter(ToledoLaneChangeParameters.BETA_POS_LAG) * Speed.max(dVLag, Speed.ZERO).si
535 + bc.getParameter(ToledoLaneChangeParameters.BETA_EMU_LAG) * emuTg
536 + bc.getParameter(ToledoLaneChangeParameters.ALPHA_TL_LAG)
537 * bc.getParameter(ToledoLaneChangeParameters.ERROR_TERM)
538 + eLag),
539 LengthUnit.SI);
540 vLag = Math.log(gLag.si) - (bc.getParameter(ToledoLaneChangeParameters.C_LAG)
541 + bc.getParameter(ToledoLaneChangeParameters.BETA_POS_LAG) * Speed.max(dVLag, Speed.ZERO).si
542 + bc.getParameter(ToledoLaneChangeParameters.BETA_EMU_LAG) * emuTg);
543 acceptLag = gLag.lt(sLag);
544 }
545 else
546 {
547 vLag = 0;
548 acceptLag = false;
549 }
550
551
552 double vRatLead = vLead / bc.getParameter(ToledoLaneChangeParameters.SIGMA_LEAD);
553 double vRatLag = vLag / bc.getParameter(ToledoLaneChangeParameters.SIGMA_LAG);
554 double emuGa = vLead * cumNormDist(vRatLead)
555 + bc.getParameter(ToledoLaneChangeParameters.SIGMA_LEAD) * normDist(vRatLead) + vLag * cumNormDist(vRatLag)
556 + bc.getParameter(ToledoLaneChangeParameters.SIGMA_LAG) * normDist(vRatLag);
557
558
559 return new GapAcceptanceInfo(emuGa, acceptLead && acceptLag);
560
561 }
562
563
564
565
566
567
568
569
570
571
572
573
574
575 private double laneUtility(final GTU gtu, final BehavioralCharacteristics bc, final LanePerception perception,
576 final double emuGa, final SpeedLimitInfo sli, final RelativeLane lane)
577 throws ParameterException, OperationalPlanException
578 {
579
580 if (!perception.getLaneStructure().getCrossSection().contains(lane))
581 {
582 return 0.0;
583 }
584
585
586 boolean takeNextOffRamp = false;
587 for (InfrastructureLaneChangeInfoToledo info : perception.getPerceptionCategory(ToledoPerception.class)
588 .getInfrastructureLaneChangeInfo(RelativeLane.CURRENT))
589 {
590 if (info.getSplitNumber() == 1)
591 {
592 takeNextOffRamp = true;
593 }
594 }
595 int deltaNextExit = takeNextOffRamp ? 1 : 0;
596
597 Length dExit;
598 if (!perception.getPerceptionCategory(ToledoPerception.class).getInfrastructureLaneChangeInfo(RelativeLane.CURRENT)
599 .isEmpty())
600 {
601 dExit = perception.getPerceptionCategory(ToledoPerception.class)
602 .getInfrastructureLaneChangeInfo(RelativeLane.CURRENT).first().getRemainingDistance();
603 }
604 else
605 {
606 dExit = Length.POSITIVE_INFINITY;
607 }
608
609 int[] delta = new int[3];
610 int deltaAdd = 0;
611 if (!perception.getPerceptionCategory(ToledoPerception.class).getInfrastructureLaneChangeInfo(lane).isEmpty())
612 {
613 InfrastructureLaneChangeInfo ilciLef =
614 perception.getPerceptionCategory(ToledoPerception.class).getInfrastructureLaneChangeInfo(lane).first();
615 if (ilciLef.getRequiredNumberOfLaneChanges() > 1 && ilciLef.getRequiredNumberOfLaneChanges() < 5)
616 {
617 deltaAdd = ilciLef.getRequiredNumberOfLaneChanges() - 2;
618 delta[deltaAdd] = 1;
619 }
620 }
621
622
623 Length leaderLength = !lane.isCurrent() && !perception.getPerceptionCategory(NeighborsPerception.class)
624 .getFirstLeaders(lane.getLateralDirectionality()).isEmpty()
625 ? perception.getPerceptionCategory(NeighborsPerception.class)
626 .getFirstLeaders(lane.getLateralDirectionality()).first().getLength()
627 : Length.ZERO;
628 Length followerLength = !lane.isCurrent() && !perception.getPerceptionCategory(NeighborsPerception.class)
629 .getFirstFollowers(lane.getLateralDirectionality()).isEmpty()
630 ? perception.getPerceptionCategory(NeighborsPerception.class)
631 .getFirstFollowers(lane.getLateralDirectionality()).first().getDistance()
632 : Length.ZERO;
633 int deltaHeavy = leaderLength.gt(MAX_LIGHT_VEHICLE_LENGTH) || followerLength.gt(MAX_LIGHT_VEHICLE_LENGTH) ? 1 : 0;
634
635
636 LinearDensity d = getDensityInLane(gtu, perception, lane);
637
638
639 int deltaTailgate = 0;
640 if (lane.equals(RelativeLane.CURRENT)
641 && !perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(RelativeLane.CURRENT).isEmpty()
642 && perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(RelativeLane.CURRENT).first()
643 .getDistance().le(TAILGATE_LENGTH))
644 {
645 LinearDensity dL = getDensityInLane(gtu, perception, RelativeLane.LEFT);
646 LinearDensity dR = getDensityInLane(gtu, perception, RelativeLane.RIGHT);
647 LinearDensity dRoad = new LinearDensity((dL.si + d.si + dR.si) / 3, LinearDensityUnit.SI);
648 if (dRoad.le(LOS_DENSITY))
649 {
650 deltaTailgate = 1;
651 }
652 }
653
654
655
656 int deltaRightMost = 0;
657 if (lane.equals(RelativeLane.CURRENT)
658 && !perception.getPerceptionCategory(ToledoPerception.class).getCrossSection().contains(RelativeLane.RIGHT))
659 {
660 deltaRightMost = 1;
661 }
662 else if (lane.equals(RelativeLane.RIGHT)
663 && perception.getPerceptionCategory(ToledoPerception.class).getCrossSection().contains(RelativeLane.RIGHT)
664 && !perception.getPerceptionCategory(ToledoPerception.class).getCrossSection()
665 .contains(RelativeLane.SECOND_RIGHT))
666 {
667 deltaRightMost = 1;
668 }
669
670
671 Speed vFront;
672 Length sFront;
673 if (lane.equals(RelativeLane.CURRENT))
674 {
675 if (!perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT).isEmpty())
676 {
677 vFront = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT).first()
678 .getSpeed();
679 sFront = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT).first()
680 .getDistance();
681 }
682 else
683 {
684 vFront = getCarFollowingModel().desiredSpeed(bc, sli);
685 sFront = getCarFollowingModel().desiredHeadway(bc, gtu.getSpeed());
686 }
687 }
688 else
689 {
690 vFront = Speed.ZERO;
691 sFront = Length.ZERO;
692 }
693
694
695 double constant = 0;
696 double error = 0;
697 if (lane.equals(RelativeLane.CURRENT))
698 {
699 constant = bc.getParameter(ToledoLaneChangeParameters.C_CL);
700 error = bc.getParameter(ToledoLaneChangeParameters.ALPHA_CL)
701 * bc.getParameter(ToledoLaneChangeParameters.ERROR_TERM);
702 }
703 else if (lane.equals(RelativeLane.RIGHT))
704 {
705 constant = bc.getParameter(ToledoLaneChangeParameters.C_RL);
706 error = bc.getParameter(ToledoLaneChangeParameters.ALPHA_RL)
707 * bc.getParameter(ToledoLaneChangeParameters.ERROR_TERM);
708 }
709
710 return constant
711 + bc.getParameter(ToledoLaneChangeParameters.BETA_RIGHT_MOST) * deltaRightMost
712 + bc.getParameter(ToledoLaneChangeParameters.BETA_VFRONT) * vFront.si
713 + bc.getParameter(ToledoLaneChangeParameters.BETA_SFRONT) * sFront.si
714 + bc.getParameter(ToledoLaneChangeParameters.BETA_DENSITY) * d.getInUnit(LinearDensityUnit.PER_KILOMETER)
715 + bc.getParameter(ToledoLaneChangeParameters.BETA_HEAVY_NEIGHBOUR) * deltaHeavy
716 + bc.getParameter(ToledoLaneChangeParameters.BETA_TAILGATE) * deltaTailgate
717 + Math.pow(dExit.getInUnit(LengthUnit.KILOMETER), bc.getParameter(ToledoLaneChangeParameters.THETA_MLC))
718 * (bc.getParameter(ToledoLaneChangeParameters.BETA1) * delta[0]
719 + bc.getParameter(ToledoLaneChangeParameters.BETA2) * delta[1]
720 + bc.getParameter(ToledoLaneChangeParameters.BETA3) * delta[2])
721 + bc.getParameter(ToledoLaneChangeParameters.BETA_NEXT_EXIT) * deltaNextExit
722 + bc.getParameter(ToledoLaneChangeParameters.BETA_ADD) * deltaAdd
723 + bc.getParameter(ToledoLaneChangeParameters.BETA_EMU_GA) * emuGa
724 + error;
725
726 }
727
728
729
730
731
732
733
734
735
736 private LinearDensity getDensityInLane(final GTU gtu, final LanePerception perception, final RelativeLane lane)
737 throws OperationalPlanException
738 {
739 int nVehicles = 0;
740 nVehicles += perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane) == null ? 0
741 : perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).size();
742 nVehicles += perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane) == null ? 0
743 : perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).size();
744 if (nVehicles > 0)
745 {
746 Length d1;
747 if (!perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).isEmpty())
748 {
749 d1 = perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).last().getDistance();
750 d1 = d1.plus(gtu.getLength().divideBy(2.0));
751 d1 = d1.plus(perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(lane).last().getLength()
752 .divideBy(2.0));
753 }
754 else
755 {
756 d1 = Length.ZERO;
757 }
758 Length d2;
759 if (!perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).isEmpty())
760 {
761 d2 = perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).last().getDistance();
762 d2 = d2.plus(gtu.getLength().divideBy(2.0));
763 d2 = d2.plus(perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(lane).last().getLength()
764 .divideBy(2.0));
765 }
766 else
767 {
768 d2 = Length.ZERO;
769 }
770 return new LinearDensity(nVehicles / d1.plus(d2).getInUnit(LengthUnit.KILOMETER), LinearDensityUnit.PER_KILOMETER);
771 }
772 return LinearDensity.ZERO;
773 }
774
775
776
777
778
779
780 private static double cumNormDist(final double x)
781 {
782 return .5 * (1 + erf(x / Math.sqrt(2)));
783 }
784
785
786
787
788
789
790 private static double erf(final double x)
791 {
792 double t = 1.0 / (1.0 + 0.5 * Math.abs(x));
793
794
795 double tau = t * Math.exp(-x * x - 1.26551223 + t * (1.00002368 + t * (0.37409196
796 + t * (0.09678418 + t * (0.18628806 + t * (0.27886807 + t * (-1.13520398
797 + t * (1.48851587 + t * (-0.82215223 + t * (0.17087277))))))))));
798
799 return x >= 0 ? 1 - tau : tau - 1;
800 }
801
802
803
804
805
806
807 private static double normDist(final double x)
808 {
809 return Math.exp(-x * x / 2) / Math.sqrt(2 * Math.PI);
810 }
811
812
813
814
815
816
817
818
819
820
821
822
823
824 private enum Gap
825 {
826
827 FORWARD,
828
829
830 ADJACENT,
831
832
833 BACKWARD;
834 }
835
836
837
838
839
840
841
842
843
844
845
846
847
848 private class GapInfo implements Serializable
849 {
850
851
852 private static final long serialVersionUID = 20160811L;
853
854
855 private final double utility;
856
857
858 private final Length length;
859
860
861 private final Length distance;
862
863
864 private final Speed speed;
865
866
867
868
869
870
871
872 GapInfo(final double utility, final Length length, final Length distance, final Speed speed)
873 {
874 this.utility = utility;
875 this.length = length;
876 this.distance = distance;
877 this.speed = speed;
878 }
879
880
881
882
883 public final double getUtility()
884 {
885 return this.utility;
886 }
887
888
889
890
891 public final Length getLength()
892 {
893 return this.length;
894 }
895
896
897
898
899 public final Length getDistance()
900 {
901 return this.distance;
902 }
903
904
905
906
907 public final Speed getSpeed()
908 {
909 return this.speed;
910 }
911
912
913 @Override
914 public String toString()
915 {
916 return "GapAcceptanceInfo [utility = " + this.utility + ", length = " + this.length + ", distance = "
917 + this.distance + ", speed = " + this.speed + "]";
918 }
919
920 }
921
922
923
924
925
926
927
928
929
930
931
932
933
934 private class GapAcceptanceInfo implements Serializable
935 {
936
937
938 private static final long serialVersionUID = 20160811L;
939
940
941 private final double emu;
942
943
944 private final boolean acceptable;
945
946
947
948
949
950 GapAcceptanceInfo(final double emu, final boolean acceptable)
951 {
952 this.emu = emu;
953 this.acceptable = acceptable;
954 }
955
956
957
958
959 public final double getEmu()
960 {
961 return this.emu;
962 }
963
964
965
966
967 public final boolean isAcceptable()
968 {
969 return this.acceptable;
970 }
971
972
973 @Override
974 public String toString()
975 {
976 return "GapAcceptanceInfo [emu = " + this.emu + ", acceptable = " + this.acceptable + "]";
977 }
978
979 }
980
981
982 @Override
983 public final String toString()
984 {
985 return "Toledo tactical planner.";
986 }
987
988 }