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