Eclipse SUMO - Simulation of Urban MObility
MSCFModel_ACC.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2020 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
18 // ACC car-following model based on [1], [2].
19 // [1] Milanes, V., and S. E. Shladover. Handling Cut-In Vehicles in Strings
20 // of Cooperative Adaptive Cruise Control Vehicles. Journal of Intelligent
21 // Transportation Systems, Vol. 20, No. 2, 2015, pp. 178-191.
22 // [2] Xiao, L., M. Wang and B. van Arem. Realistic Car-Following Models for
23 // Microscopic Simulation of Adaptive and Cooperative Adaptive Cruise
24 // Control Vehicles. Transportation Research Record: Journal of the
25 // Transportation Research Board, No. 2623, 2017. (DOI: 10.3141/2623-01).
26 /****************************************************************************/
27 #include <config.h>
28 
29 #include <stdio.h>
30 #include <iostream>
31 
32 #include "MSCFModel_ACC.h"
33 #include <microsim/MSVehicle.h>
34 #include <microsim/MSLane.h>
36 #include <utils/common/SUMOTime.h>
38 #include <math.h>
39 #include <microsim/MSNet.h>
40 
41 // ===========================================================================
42 // debug flags
43 // ===========================================================================
44 //#define DEBUG_ACC
45 //#define DEBUG_COND (true)
46 //#define DEBUG_COND (veh->isSelected())
47 
48 
49 // ===========================================================================
50 // defaults
51 // ===========================================================================
52 #define DEFAULT_SC_GAIN -0.4
53 #define DEFAULT_GCC_GAIN_SPEED 0.8
54 #define DEFAULT_GCC_GAIN_SPACE 0.04
55 #define DEFAULT_GC_GAIN_SPEED 0.07
56 #define DEFAULT_GC_GAIN_SPACE 0.23
57 #define DEFAULT_CA_GAIN_SPACE 0.8
58 #define DEFAULT_CA_GAIN_SPEED 0.23
59 
60 // ===========================================================================
61 // thresholds
62 // ===========================================================================
63 #define GAP_THRESHOLD_SPEEDCTRL 120
64 #define GAP_THRESHOLD_GAPCTRL 100
65 
66 
67 
68 
69 // override followSpeed when deemed unsafe by the given margin (the value was selected to reduce the number of necessary interventions)
70 #define DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD 2.0
71 
73 
74 // ===========================================================================
75 // method definitions
76 // ===========================================================================
78  MSCFModel(vtype),
79  mySpeedControlGain(vtype->getParameter().getCFParam(SUMO_ATTR_SC_GAIN, DEFAULT_SC_GAIN)),
80  myGapClosingControlGainSpeed(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_SPEED, DEFAULT_GCC_GAIN_SPEED)),
81  myGapClosingControlGainSpace(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_SPACE, DEFAULT_GCC_GAIN_SPACE)),
82  myGapControlGainSpeed(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_SPEED, DEFAULT_GC_GAIN_SPEED)),
83  myGapControlGainSpace(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_SPACE, DEFAULT_GC_GAIN_SPACE)),
84  myCollisionAvoidanceGainSpeed(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_SPEED, DEFAULT_CA_GAIN_SPEED)),
85  myCollisionAvoidanceGainSpace(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_SPACE, DEFAULT_CA_GAIN_SPACE)) {
86  // ACC does not drive very precise and often violates minGap
88 }
89 
91 
92 
93 double
94 MSCFModel_ACC::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
95  const double desSpeed = MIN2(veh->getLane()->getSpeedLimit(), veh->getMaxSpeed());
96  const double vACC = _v(veh, gap2pred, speed, predSpeed, desSpeed, true);
97  const double vSafe = maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel);
98  if (vSafe + DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD < vACC) {
99  //ACCVehicleVariables* vars = (ACCVehicleVariables*)veh->getCarFollowVariables();
100  //std::cout << SIMTIME << " veh=" << veh->getID() << " v=" << speed << " vL=" << predSpeed << " gap=" << gap2pred << " vACC=" << vACC << " vSafe=" << vSafe << " cm=" << vars->ACC_ControlMode << "\n";
102  }
103  return vACC;
104 }
105 
106 
107 double
108 MSCFModel_ACC::stopSpeed(const MSVehicle* const veh, const double speed, double gap) const {
109  // NOTE: This allows return of smaller values than minNextSpeed().
110  // Only relevant for the ballistic update: We give the argument headway=TS, to assure that
111  // the stopping position is approached with a uniform deceleration also for tau!=TS.
112  return MIN2(maximumSafeStopSpeed(gap, speed, false, veh->getActionStepLengthSecs()), maxNextSpeed(speed, veh));
113 }
114 
115 
116 double
117 MSCFModel_ACC::getSecureGap(const MSVehicle* const /*veh*/, const MSVehicle* const /*pred*/, const double speed, const double leaderSpeed, const double /* leaderMaxDecel */) const {
118  // Accel in gap mode should vanish:
119  // 0 = myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * (g - myHeadwayTime * speed);
120  // <=> myGapControlGainSpace * g = - myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * myHeadwayTime * speed;
121  // <=> g = - myGapControlGainSpeed * (leaderSpeed - speed) / myGapControlGainSpace + myHeadwayTime * speed;
122  return myGapControlGainSpeed * (speed - leaderSpeed) / myGapControlGainSpace + myHeadwayTime * speed;
123 }
124 
125 
126 double
127 MSCFModel_ACC::insertionFollowSpeed(const MSVehicle* const v, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
128 //#ifdef DEBUG_ACC
129 // std::cout << "MSCFModel_ACC::insertionFollowSpeed(), speed="<<speed<< std::endl;
130 //#endif
131  // iterate to find a stationary value for
132  // speed = followSpeed(v, speed, gap2pred, predSpeed, predMaxDecel, nullptr)
133  const int max_iter = 50;
134  int n_iter = 0;
135  const double tol = 0.1;
136  const double damping = 0.1;
137 
138  double res = speed;
139  while (n_iter < max_iter) {
140  // proposed acceleration
141  const double a = SPEED2ACCEL(followSpeed(v, res, gap2pred, predSpeed, predMaxDecel, nullptr) - res);
142  res = res + damping * a;
143 //#ifdef DEBUG_ACC
144 // std::cout << " n_iter=" << n_iter << ", a=" << a << ", res=" << res << std::endl;
145 //#endif
146  if (fabs(a) < tol) {
147  break;
148  } else {
149  n_iter++;
150  }
151  }
152  return res;
153 }
154 
155 
157 double
158 MSCFModel_ACC::interactionGap(const MSVehicle* const /* veh */, double /* vL */) const {
159  /*maximum radar range is ACC is enabled*/
160  return 250;
161 }
162 
163 double MSCFModel_ACC::accelSpeedControl(double vErr) const {
164  // Speed control law
165  return mySpeedControlGain * vErr;
166 }
167 
168 double MSCFModel_ACC::accelGapControl(const MSVehicle* const /* veh */, const double gap2pred, const double speed, const double predSpeed, double vErr) const {
169 
170 #ifdef DEBUG_ACC
171  if (DEBUG_COND) {
172  std::cout << " applying gapControl" << std::endl;
173  }
174 #endif
175 
176 // Gap control law
177  double gclAccel = 0.0;
178  double desSpacing = myHeadwayTime * speed;
179  double spacingErr = gap2pred - desSpacing;
180  double deltaVel = predSpeed - speed;
181 
182 
183  if (fabs(spacingErr) < 0.2 && fabs(vErr) < 0.1) {
184  // gap mode
185  gclAccel = myGapControlGainSpeed * deltaVel + myGapControlGainSpace * spacingErr;
186  } else if (spacingErr < 0) {
187  // collision avoidance mode
188  gclAccel = myCollisionAvoidanceGainSpeed * deltaVel + myCollisionAvoidanceGainSpace * spacingErr;
189  } else {
190  // gap closing mode
191  gclAccel = myGapClosingControlGainSpeed * deltaVel + myGapClosingControlGainSpace * spacingErr;
192  }
193 
194  return gclAccel;
195 }
196 
197 
198 double
199 MSCFModel_ACC::_v(const MSVehicle* const veh, const double gap2pred, const double speed,
200  const double predSpeed, const double desSpeed, const bool /* respectMinGap */) const {
201 
202  double accelACC = 0;
203  double gapLimit_SC = GAP_THRESHOLD_SPEEDCTRL; // lower gap limit in meters to enable speed control law
204  double gapLimit_GC = GAP_THRESHOLD_GAPCTRL; // upper gap limit in meters to enable gap control law
205 
206 #ifdef DEBUG_ACC
207  if (DEBUG_COND) {
208  std::cout << SIMTIME << " MSCFModel_ACC::_v() for veh '" << veh->getID() << "'\n"
209  << " gap=" << gap2pred << " speed=" << speed << " predSpeed=" << predSpeed
210  << " desSpeed=" << desSpeed << std::endl;
211  }
212 #endif
213 
214 
215  /* Velocity error */
216  double vErr = speed - desSpeed;
217  int setControlMode = 0;
221  setControlMode = 1;
222  }
223  if (gap2pred > gapLimit_SC) {
224 
225 #ifdef DEBUG_ACC
226  if (DEBUG_COND) {
227  std::cout << " applying speedControl" << std::endl;
228  }
229 #endif
230  // Find acceleration - Speed control law
231  accelACC = accelSpeedControl(vErr);
232  // Set cl to vehicle parameters
233  if (setControlMode) {
234  vars->ACC_ControlMode = 0;
235  }
236  } else if (gap2pred < gapLimit_GC) {
237  // Find acceleration - Gap control law
238  accelACC = accelGapControl(veh, gap2pred, speed, predSpeed, vErr);
239  // Set cl to vehicle parameters
240  if (setControlMode) {
241  vars->ACC_ControlMode = 1;
242  }
243  } else {
244  // Follow previous applied law
245  int cm = vars->ACC_ControlMode;
246  if (!cm) {
247 
248 #ifdef DEBUG_ACC
249  if (DEBUG_COND) {
250  std::cout << " applying speedControl" << std::endl;
251  }
252 #endif
253  accelACC = accelSpeedControl(vErr);
254  } else {
255  accelACC = accelGapControl(veh, gap2pred, speed, predSpeed, vErr);
256  }
257 
258  }
259 
260  double newSpeed = speed + ACCEL2SPEED(accelACC);
261 
262 #ifdef DEBUG_ACC
263  if (DEBUG_COND) {
264  std::cout << " result: accel=" << accelACC << " newSpeed=" << newSpeed << std::endl;
265  }
266 #endif
267 
268  return MAX2(0., newSpeed);
269 }
270 
271 
272 MSCFModel*
274  return new MSCFModel_ACC(vtype);
275 }
#define DEFAULT_GC_GAIN_SPACE
#define DEFAULT_GCC_GAIN_SPEED
#define DEFAULT_GCC_GAIN_SPACE
#define DEFAULT_CA_GAIN_SPACE
#define DEFAULT_GC_GAIN_SPEED
#define DEFAULT_CA_GAIN_SPEED
#define GAP_THRESHOLD_SPEEDCTRL
#define DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD
#define GAP_THRESHOLD_GAPCTRL
#define DEFAULT_SC_GAIN
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:49
#define SIMTIME
Definition: SUMOTime.h:60
#define SPEED2ACCEL(x)
Definition: SUMOTime.h:51
@ SUMO_ATTR_GCC_GAIN_SPEED
@ SUMO_ATTR_GC_GAIN_SPACE
@ SUMO_ATTR_CA_GAIN_SPACE
@ SUMO_ATTR_GCC_GAIN_SPACE
@ SUMO_ATTR_COLLISION_MINGAP_FACTOR
@ SUMO_ATTR_CA_GAIN_SPEED
@ SUMO_ATTR_GC_GAIN_SPEED
@ SUMO_ATTR_SC_GAIN
T MIN2(T a, T b)
Definition: StdDefs.h:73
T MAX2(T a, T b)
Definition: StdDefs.h:79
double getMaxSpeed() const
Returns the maximum speed.
int ACC_ControlMode
The vehicle's ACC control mode. 0 for speed control and 1 for gap control.
double accelSpeedControl(double vErr) const
double myCollisionAvoidanceGainSpeed
double myCollisionAvoidanceGainSpace
double myGapControlGainSpace
double interactionGap(const MSVehicle *const, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Computes the vehicle's safe speed (no dawdling)
double accelGapControl(const MSVehicle *const veh, const double gap2pred, const double speed, const double predSpeed, double vErr) const
double myGapClosingControlGainSpeed
double insertionFollowSpeed(const MSVehicle *const v, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Computes the vehicle's acceptable speed at insertion.
double myGapControlGainSpeed
double getSecureGap(const MSVehicle *const veh, const MSVehicle *const pred, const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the a gap such that the gap mode acceleration of the follower is zero.
double stopSpeed(const MSVehicle *const veh, const double speed, double gap2pred) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
~MSCFModel_ACC()
Destructor.
MSCFModel_ACC(const MSVehicleType *vtype)
Constructor.
MSCFModel * duplicate(const MSVehicleType *vtype) const
Duplicates the car-following model.
double _v(const MSVehicle *const veh, const double gap2pred, const double mySpeed, const double predSpeed, const double desSpeed, const bool respectMinGap=true) const
double myGapClosingControlGainSpace
double mySpeedControlGain
The car-following model abstraction.
Definition: MSCFModel.h:55
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:237
double maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
Definition: MSCFModel.cpp:855
double maximumSafeStopSpeed(double gap, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
Definition: MSCFModel.cpp:710
double myCollisionMinGapFactor
The factor of minGap that must be maintained to avoid a collision event.
Definition: MSCFModel.h:625
double myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
Definition: MSCFModel.h:628
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
Definition: MSLane.h:531
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:171
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:313
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:502
MSCFModel::VehicleVariables * getCarFollowVariables() const
Returns the vehicle's car following model variables.
Definition: MSVehicle.h:947
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:550
The car-following model and parameter.
Definition: MSVehicleType.h:62
const SUMOVTypeParameter & getParameter() const
const std::string & getID() const
Returns the id.
Definition: Named.h:73
double getCFParam(const SumoXMLAttr attr, const double defaultValue) const
Returns the named value from the map, or the default if it is not contained there.
#define DEBUG_COND