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