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