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