1 package org.opentrafficsim.road.gtu.lane.tactical.util;
2
3 import java.util.SortedMap;
4 import java.util.SortedSet;
5 import java.util.TreeMap;
6
7 import org.djunits.unit.AccelerationUnit;
8 import org.djunits.unit.SpeedUnit;
9 import org.djunits.value.vdouble.scalar.Acceleration;
10 import org.djunits.value.vdouble.scalar.Length;
11 import org.djunits.value.vdouble.scalar.Speed;
12 import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
13 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
14 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
15 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
16 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
17
18 import nl.tudelft.simulation.language.Throw;
19
20 /**
21 * Static methods regarding car-following for composition in tactical planners.
22 * <p>
23 * Copyright (c) 2013-2017 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
24 * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
25 * <p>
26 * @version $Revision$, $LastChangedDate$, by $Author$, initial version May 23, 2016 <br>
27 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
28 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
29 */
30 public final class CarFollowingUtil
31 {
32
33 /**
34 * Do not instantiate.
35 */
36 private CarFollowingUtil()
37 {
38 //
39 }
40
41 /**
42 * Follow a set of headway GTUs.
43 * @param carFollowingModel car-following model
44 * @param behavioralCharacteristics behavioral characteristics
45 * @param speed current speed
46 * @param speedLimitInfo speed limit info
47 * @param leaders leaders
48 * @return acceleration for following the leader
49 * @throws ParameterException if a parameter is not given or out of bounds
50 */
51 public static Acceleration followLeaders(final CarFollowingModel carFollowingModel,
52 final BehavioralCharacteristics behavioralCharacteristics, final Speed speed, final SpeedLimitInfo speedLimitInfo,
53 final SortedSet<HeadwayGTU> leaders) throws ParameterException
54 {
55 SortedMap<Length, Speed> leaderMap = new TreeMap<>();
56 for (HeadwayGTU headwayGTU : leaders)
57 {
58 leaderMap.put(headwayGTU.getDistance(), headwayGTU.getSpeed());
59 }
60 return carFollowingModel.followingAcceleration(behavioralCharacteristics, speed, speedLimitInfo, leaderMap);
61 }
62
63 /**
64 * Follow a set of headway GTUs.
65 * @param carFollowingModel car-following model
66 * @param behavioralCharacteristics behavioral characteristics
67 * @param speed current speed
68 * @param speedLimitInfo speed limit info
69 * @param distance distance
70 * @param leaderSpeed speed of the leader
71 * @return acceleration for following the leader
72 * @throws ParameterException if a parameter is not given or out of bounds
73 */
74 public static Acceleration followSingleLeader(final CarFollowingModel carFollowingModel,
75 final BehavioralCharacteristics behavioralCharacteristics, final Speed speed, final SpeedLimitInfo speedLimitInfo,
76 final Length distance, final Speed leaderSpeed) throws ParameterException
77 {
78 SortedMap<Length, Speed> leaders = new TreeMap<>();
79 leaders.put(distance, leaderSpeed);
80 return carFollowingModel.followingAcceleration(behavioralCharacteristics, speed, speedLimitInfo, leaders);
81 }
82
83 /**
84 * Stop within given distance.
85 * @param carFollowingModel car-following model
86 * @param behavioralCharacteristics behavioral characteristics
87 * @param speed current speed
88 * @param speedLimitInfo speed limit info
89 * @param distance distance to stop over
90 * @return acceleration to stop over distance
91 * @throws ParameterException if a parameter is not given or out of bounds
92 */
93 public static Acceleration stop(final CarFollowingModel carFollowingModel,
94 final BehavioralCharacteristics behavioralCharacteristics, final Speed speed, final SpeedLimitInfo speedLimitInfo,
95 final Length distance) throws ParameterException
96 {
97 SortedMap<Length, Speed> leaderMap = new TreeMap<>();
98 leaderMap.put(distance, Speed.ZERO);
99 return carFollowingModel.followingAcceleration(behavioralCharacteristics, speed, speedLimitInfo, leaderMap);
100 }
101
102 /**
103 * Return constant acceleration in order to stop in specified distance. The car-following model is used to determine the
104 * stopping distance (i.e. distance remaining at stand still, e.g. 1-3m).
105 * @param carFollowingModel car-following model
106 * @param behavioralCharacteristics behavioral characteristics
107 * @param speed current speed
108 * @param distance distance to stop over
109 * @return constant acceleration in order to stop in specified distance
110 * @throws ParameterException on missing parameter
111 */
112 public static Acceleration constantAccelerationStop(final CarFollowingModel carFollowingModel,
113 final BehavioralCharacteristics behavioralCharacteristics, final Speed speed, final Length distance)
114 throws ParameterException
115 {
116 Length s0 = carFollowingModel.desiredHeadway(behavioralCharacteristics, Speed.ZERO);
117 return new Acceleration(-0.5 * speed.si * speed.si / (distance.si - s0.si), AccelerationUnit.SI);
118 }
119
120 /**
121 * Calculate free acceleration.
122 * @param carFollowingModel car-following model
123 * @param behavioralCharacteristics behavioral characteristics
124 * @param speed current speed
125 * @param speedLimitInfo speed limit info
126 * @return acceleration free acceleration
127 * @throws ParameterException if a parameter is not given or out of bounds
128 */
129 public static Acceleration freeAcceleration(final CarFollowingModel carFollowingModel,
130 final BehavioralCharacteristics behavioralCharacteristics, final Speed speed, final SpeedLimitInfo speedLimitInfo)
131 throws ParameterException
132 {
133 SortedMap<Length, Speed> leaderMap = new TreeMap<>();
134 return carFollowingModel.followingAcceleration(behavioralCharacteristics, speed, speedLimitInfo, leaderMap);
135 }
136
137 /**
138 * Returns an acceleration based on the car-following model in order to adjust the speed to a given value at some location
139 * ahead. This is done by placing a virtual vehicle somewhere near the location. Both the location and speed of this virtual
140 * vehicle are dynamically adjusted to resemble a car-following situation. To explain, first consider the situation where a
141 * virtual vehicle is placed at the target speed and such that the equilibrium headway is in line with the location:
142 *
143 * <pre>
144 *
145 * ___ location of target speed --)| ___
146 * |___|(--------------s--------------) (--h--)|___| ))) vTar
147 * </pre>
148 *
149 * Here, {@code s} is the distance to the target speed, and {@code h} is the desired headway if the vehicle would drive at
150 * the target speed {@code vTar}.<br>
151 * <br>
152 * In this way car-following models will first underestimate the required deceleration, as the virtual vehicle is actually
153 * stationary and does not move with {@code vTar} at all. Because of this underestimation, strong deceleration is required
154 * later. This behavior is not in line with the sensitivity parameters of the car-following model.<br>
155 * <br>
156 * To correct for the fact that the virtual vehicle is actually not moving, the speed difference should be larger, i.e. the
157 * speed of the virtual vehicle {@code vTar'} should be lower. We require:
158 * <ul>
159 * <li>if {@code v = vTar} then {@code vTar' = vTar}, otherwise there is an incentive to accelerate or decelerate for no
160 * good reason</li>
161 * <li>if {@code vTar ~ 0} then {@code vTar' ~ 0}, as car-following models are suitable for stopping and need no additional
162 * incentive to decelerate in such cases</li>
163 * <li>if {@code 0 < vTar < v} then {@code vTar' < vTar}, introducing additional deceleration to compensate for the fact
164 * that the virtual vehicle does not move
165 * </ul>
166 * These requirements are met by {@code vTar' = vTar * (vTar/v) = vTar^2/v}.<br>
167 * <br>
168 * Furthermore, if {@code v < vTar} we get {@code vTar' > vTar} leading to additional acceleration. Acceleration is then
169 * appropriate, and possibly limited by a free term in the car-following model.<br>
170 * <br>
171 * The virtual vehicle is thus placed with speed {@code vTar'} at a distance {@code s + h'} where {@code h'} is the desired
172 * headway if the vehicle would drive at speed {@code vTar'}. Both {@code vTar'} and {@code h'} depend on the current speed
173 * of the vehicle, so the virtual vehicle in this case actually moves, but not with {@code vTar}.<br>
174 * <br>
175 * This approach has been tested with the IDM+ to deliver decelerations in line with the parameters. On a plane with initial
176 * speed ranging from 0 to 33.33m/s and a target speed in 300m also ranging from 0 to 33.33m/s, strongest deceleration is
177 * equal to the car-following model stopping from 33.33m/s to a stand-still vehicle in 300m (+ stopping distance of 3m).
178 * Throughout the plane the maximum deceleration of each scenario is close to this value, unless the initial speed is so
179 * low, and the target speed is so high, that such levels of deceleration are never required.<br>
180 * <br>
181 * @param carFollowingModel car-following model to use
182 * @param behavioralCharacteristics behavioral characteristics
183 * @param speed current speed
184 * @param speedLimitInfo info regarding the desired speed for car-following
185 * @param distance distance to the location of the target speed
186 * @param targetSpeed target speed
187 * @return acceleration acceleration based on the car-following model in order to adjust the speed
188 * @throws ParameterException if parameter exception occurs
189 * @throws NullPointerException if any input is null
190 * @throws IllegalArgumentException if the distance or target speed is not at least 0
191 */
192 public static Acceleration approachTargetSpeed(final CarFollowingModel carFollowingModel,
193 final BehavioralCharacteristics behavioralCharacteristics, final Speed speed, final SpeedLimitInfo speedLimitInfo,
194 final Length distance, final Speed targetSpeed) throws ParameterException
195 {
196 Throw.whenNull(behavioralCharacteristics, "Behavioral characteristics may not be null.");
197 Throw.whenNull(speed, "Speed may not be null.");
198 Throw.whenNull(speedLimitInfo, "Speed limit info may not be null.");
199 Throw.whenNull(distance, "Distance may not be null");
200 Throw.whenNull(targetSpeed, "Target speed may not be null");
201 Throw.when(distance.si < 0, IllegalArgumentException.class, "Distance must be at least 0.");
202 Throw.when(targetSpeed.si < 0, IllegalArgumentException.class, "Target speed must be at least 0.");
203 // adjust speed of virtual vehicle to add deceleration incentive as the virtual vehicle does not move
204 Speed virtualSpeed;
205 if (speed.si > 0)
206 {
207 virtualSpeed = new Speed(targetSpeed.si * targetSpeed.si / speed.si, SpeedUnit.SI);
208 }
209 else
210 {
211 virtualSpeed = new Speed(Double.MAX_VALUE, SpeedUnit.SI);
212 }
213 // set distance in line with equilibrium headway at virtual speed
214 Length virtualDistance = distance.plus(carFollowingModel.desiredHeadway(behavioralCharacteristics, virtualSpeed));
215 // calculate acceleration towards virtual vehicle with car-following model
216 SortedMap<Length, Speed> leaders = new TreeMap<>();
217 leaders.put(virtualDistance, virtualSpeed);
218 return carFollowingModel.followingAcceleration(behavioralCharacteristics, speed, speedLimitInfo, leaders);
219 }
220
221 }