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