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