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