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